MBSim

Project Page
joint.h
00001 /* Copyright (C) 2004-2014 MBSim Development Team
00002  *
00003  * This library is free software; you can redistribute it and/or 
00004  * modify it under the terms of the GNU Lesser General Public 
00005  * License as published by the Free Software Foundation; either 
00006  * version 2.1 of the License, or (at your option) any later version. 
00007  *  
00008  * This library is distributed in the hope that it will be useful, 
00009  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00010  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00011  * Lesser General Public License for more details. 
00012  *  
00013  * You should have received a copy of the GNU Lesser General Public 
00014  * License along with this library; if not, write to the Free Software 
00015  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00016  *
00017  * Contact: martin.o.foerg@googlemail.com
00018  */
00019 
00020 #ifndef _JOINT_H_
00021 #define _JOINT_H_
00022 
00023 #include "mbsim/link_mechanics.h"
00024 #include "mbsim/frame.h"
00025 
00026 #ifdef HAVE_OPENMBVCPPINTERFACE
00027 #include "mbsim/utils/boost_parameters.h"
00028 #include "mbsim/utils/openmbv_utils.h"
00029 #endif
00030 
00031 namespace MBSim {
00032 
00033   class GeneralizedForceLaw;
00034   class GeneralizedImpactLaw;
00035   class FrictionForceLaw;
00036   class FrictionImpactLaw;
00037   class Body;
00038 
00048   class Joint : public LinkMechanics {
00049     public:
00054       Joint(const std::string &name = "");
00055 
00059       virtual ~Joint();
00060 
00061       /* INHERITED INTERFACE OF LINKINTERFACE */
00062       virtual void updatewb(double t, int i = 0);
00063       virtual void updateW(double t, int i = 0);
00064       virtual void updateh(double t, int i = 0);
00065       virtual void updateg(double t);
00066       virtual void updategd(double t);
00067       virtual void updateJacobians(double t, int j = 0);
00068       /***************************************************/
00069 
00070       /* INHERITED INTERFACE OF EXTRADYNAMICINTERFACE */
00071       virtual void updatexd(double t);
00072       virtual void updatedx(double t, double dt);
00073       virtual void calcxSize();
00074       virtual void init(InitStage stage);
00075       /***************************************************/
00076 
00077       /* INHERITED INTERFACE OF LINK */
00078       virtual void calclaSize(int j);
00079       virtual void calcgSize(int j);
00080       virtual void calcgdSize(int j);
00081       virtual void calcrFactorSize(int j);
00082       virtual void calccorrSize(int j);
00083       virtual bool isSetValued() const;
00084       virtual bool isSingleValued() const;
00085       virtual bool isActive() const {
00086         return true;
00087       }
00088       virtual bool gActiveChanged() {
00089         return false;
00090       }
00091       virtual void solveImpactsFixpointSingle(double dt);
00092       virtual void solveConstraintsFixpointSingle();
00093       virtual void solveImpactsGaussSeidel(double dt);
00094       virtual void solveConstraintsGaussSeidel();
00095       virtual void solveImpactsRootFinding(double dt);
00096       virtual void solveConstraintsRootFinding();
00097       virtual void jacobianConstraints();
00098       virtual void jacobianImpacts();
00099       virtual void updaterFactors();
00100       virtual void checkImpactsForTermination(double dt);
00101       virtual void checkConstraintsForTermination();
00102       /***************************************************/
00103 
00104       /* INHERITED INTERFACE OF ELEMENT */
00105       virtual void plot(double t, double dt = 1);
00106       /***************************************************/
00107 
00108       /* INTERFACE FOR DERIVED CLASSES */
00113       virtual void connect(Frame *frame1, Frame* frame2);
00114       /***************************************************/
00115 
00116       /* GETTER / SETTER */
00117       void setForceLaw(GeneralizedForceLaw * rc);
00118       void setMomentLaw(GeneralizedForceLaw * rc);
00119       /***************************************************/
00120 
00124       void setForceDirection(const fmatvec::Mat3xV& fd);
00125 
00129       void setMomentDirection(const fmatvec::Mat3xV& md);
00130 
00134       void setFrameOfReferenceID(int ID) {
00135         refFrameID = ID;
00136       }
00137 
00138       virtual void initializeUsingXML(xercesc::DOMElement *element);
00139       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00140 
00141       virtual std::string getType() const {
00142         return "Joint";
00143       }
00144 
00145 #ifdef HAVE_OPENMBVCPPINTERFACE
00146 
00147       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVForce, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) {
00148         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00149         setOpenMBVForce(ombv.createOpenMBV());
00150       }
00151       void setOpenMBVForce(const boost::shared_ptr<OpenMBV::Arrow> &arrow) {
00152         std::vector<bool> which; which.resize(2, false);
00153         which[1]=true;
00154         LinkMechanics::setOpenMBVForceArrow(arrow, which);
00155       }
00156 
00158       BOOST_PARAMETER_MEMBER_FUNCTION( (void), enableOpenMBVMoment, tag, (optional (scaleLength,(double),1)(scaleSize,(double),1)(referencePoint,(OpenMBV::Arrow::ReferencePoint),OpenMBV::Arrow::toPoint)(diffuseColor,(const fmatvec::Vec3&),"[-1;1;1]")(transparency,(double),0))) {
00159         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00160         setOpenMBVMoment(ombv.createOpenMBV());
00161       }
00162       void setOpenMBVMoment(const boost::shared_ptr<OpenMBV::Arrow> &arrow) {
00163         std::vector<bool> which; which.resize(2, false);
00164         which[1]=true;
00165         LinkMechanics::setOpenMBVMomentArrow(arrow, which);
00166       }
00167 #endif
00168 
00169     protected:
00173       Frame *refFrame;
00174       int refFrameID;
00175 
00179       fmatvec::Index IT, IR;
00180 
00184       fmatvec::Mat3xV forceDir, momentDir;
00185 
00189       fmatvec::Mat3xV Wf, Wm;
00190 
00194       fmatvec::Mat3xV JT;
00195 
00199       fmatvec::Vec3 WrP0P1, WvP0P1, WomP0P1;
00200 
00204       GeneralizedForceLaw *ffl;
00205 
00209       GeneralizedForceLaw * fml;
00210 
00214       GeneralizedImpactLaw *fifl;
00215 
00219       GeneralizedImpactLaw * fiml;
00220 
00224       fmatvec::Vec gdn, gdd;
00225 
00229       Frame C;
00230 
00231     private:
00232       std::string saved_ref1, saved_ref2;
00233   };
00234 
00235   class InverseKineticsJoint : public Joint {
00236     public:
00237       InverseKineticsJoint(const std::string &name);
00238       virtual void updateb(double t);
00239       void calcbSize();
00240       void setBody(Body* body_) {
00241         body = body_;
00242       }
00243       virtual void init(InitStage stage);
00244       virtual bool isSetValued() const {
00245         return true;
00246       }
00247 
00248     protected:
00249       Body* body;
00250   };
00251 
00252 }
00253 
00254 #endif
00255