MBSim

Project Page
joint.h
00001 /* Copyright (C) 2004-2009 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 RigidBody;
00038 
00047   class Joint : public LinkMechanics {
00048     public:
00053       Joint(const std::string &name = "");
00054 
00058       virtual ~Joint();
00059 
00060       /* INHERITED INTERFACE OF LINKINTERFACE */
00061       virtual void updatewb(double t, int i = 0);
00062       virtual void updateW(double t, int i = 0);
00063       virtual void updateh(double t, int i = 0);
00064       virtual void updateg(double t);
00065       virtual void updategd(double t);
00066       virtual void updateJacobians(double t, int j = 0);
00067       /***************************************************/
00068 
00069       /* INHERITED INTERFACE OF EXTRADYNAMICINTERFACE */
00070       virtual void updatexd(double t);
00071       virtual void updatedx(double t, double dt);
00072       virtual void calcxSize();
00073       virtual void init(InitStage stage);
00074       /***************************************************/
00075 
00076       /* INHERITED INTERFACE OF LINK */
00077       virtual void calclaSize(int j);
00078       virtual void calcgSize(int j);
00079       virtual void calcgdSize(int j);
00080       virtual void calcrFactorSize(int j);
00081       virtual bool isSetValued() const;
00082       virtual bool isSingleValued() const;
00083       virtual bool isActive() const {
00084         return true;
00085       }
00086       virtual bool gActiveChanged() {
00087         return false;
00088       }
00089       virtual void solveImpactsFixpointSingle(double dt);
00090       virtual void solveConstraintsFixpointSingle();
00091       virtual void solveImpactsGaussSeidel(double dt);
00092       virtual void solveConstraintsGaussSeidel();
00093       virtual void solveImpactsRootFinding(double dt);
00094       virtual void solveConstraintsRootFinding();
00095       virtual void jacobianConstraints();
00096       virtual void jacobianImpacts();
00097       virtual void updaterFactors();
00098       virtual void checkImpactsForTermination(double dt);
00099       virtual void checkConstraintsForTermination();
00100       /***************************************************/
00101 
00102       /* INHERITED INTERFACE OF ELEMENT */
00103       virtual void plot(double t, double dt = 1);
00104       /***************************************************/
00105 
00106       /* INTERFACE FOR DERIVED CLASSES */
00111       virtual void connect(Frame *frame1, Frame* frame2);
00112       /***************************************************/
00113 
00114       /* GETTER / SETTER */
00115       void setForceLaw(GeneralizedForceLaw * rc) {
00116         ffl = rc;
00117       }
00118       void setMomentLaw(GeneralizedForceLaw * rc) {
00119         fml = rc;
00120       }
00121       /***************************************************/
00122 
00126       void setForceDirection(const fmatvec::Mat3xV& fd);
00127 
00131       void setMomentDirection(const fmatvec::Mat3xV& md);
00132 
00136       void setFrameOfReferenceID(int ID) {
00137         refFrameID = ID;
00138       }
00139 
00140       virtual void initializeUsingXML(xercesc::DOMElement *element);
00141       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00142 
00143       virtual std::string getType() const {
00144         return "Joint";
00145       }
00146 
00147 #ifdef HAVE_OPENMBVCPPINTERFACE
00148 
00149       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))) {
00150         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00151         setOpenMBVForce(ombv.createOpenMBV());
00152       }
00153       void setOpenMBVForce(OpenMBV::Arrow *arrow) {
00154         std::vector<bool> which; which.resize(2, false);
00155         which[1]=true;
00156         LinkMechanics::setOpenMBVForceArrow(arrow, which);
00157       }
00158 
00160       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))) {
00161         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00162         setOpenMBVMoment(ombv.createOpenMBV());
00163       }
00164       void setOpenMBVMoment(OpenMBV::Arrow *arrow) {
00165         std::vector<bool> which; which.resize(2, false);
00166         which[1]=true;
00167         LinkMechanics::setOpenMBVMomentArrow(arrow, which);
00168       }
00169 #endif
00170 
00171     protected:
00175       Frame *refFrame;
00176       int refFrameID;
00177 
00181       fmatvec::Index IT, IR;
00182 
00186       fmatvec::Mat3xV forceDir, momentDir;
00187 
00191       fmatvec::Mat3xV Wf, Wm;
00192 
00196       fmatvec::Mat3xV JT;
00197 
00201       fmatvec::Vec3 WrP0P1, WvP0P1, WomP0P1;
00202 
00206       GeneralizedForceLaw *ffl;
00207 
00211       GeneralizedForceLaw * fml;
00212 
00216       GeneralizedImpactLaw *fifl;
00217 
00221       GeneralizedImpactLaw * fiml;
00222 
00226       fmatvec::Vec gdn, gdd;
00227 
00231       Frame C;
00232 
00233     private:
00234       std::string saved_ref1, saved_ref2;
00235   };
00236 
00237   class InverseKineticsJoint : public Joint {
00238     public:
00239       InverseKineticsJoint(const std::string &name);
00240       virtual void updateb(double t);
00241       void calcbSize();
00242       void setBody(RigidBody* body_) {
00243         body = body_;
00244       }
00245       virtual void init(InitStage stage);
00246       virtual bool isSetValued() const {
00247         return true;
00248       }
00249 
00250     protected:
00251       RigidBody* body;
00252   };
00253 
00254 }
00255 
00256 #endif
00257