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 RigidBody;
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 bool isSetValued() const;
00083       virtual bool isSingleValued() const;
00084       virtual bool isActive() const {
00085         return true;
00086       }
00087       virtual bool gActiveChanged() {
00088         return false;
00089       }
00090       virtual void solveImpactsFixpointSingle(double dt);
00091       virtual void solveConstraintsFixpointSingle();
00092       virtual void solveImpactsGaussSeidel(double dt);
00093       virtual void solveConstraintsGaussSeidel();
00094       virtual void solveImpactsRootFinding(double dt);
00095       virtual void solveConstraintsRootFinding();
00096       virtual void jacobianConstraints();
00097       virtual void jacobianImpacts();
00098       virtual void updaterFactors();
00099       virtual void checkImpactsForTermination(double dt);
00100       virtual void checkConstraintsForTermination();
00101       /***************************************************/
00102 
00103       /* INHERITED INTERFACE OF ELEMENT */
00104       virtual void plot(double t, double dt = 1);
00105       /***************************************************/
00106 
00107       /* INTERFACE FOR DERIVED CLASSES */
00112       virtual void connect(Frame *frame1, Frame* frame2);
00113       /***************************************************/
00114 
00115       /* GETTER / SETTER */
00116       void setForceLaw(GeneralizedForceLaw * rc) {
00117         ffl = rc;
00118       }
00119       void setMomentLaw(GeneralizedForceLaw * rc) {
00120         fml = rc;
00121       }
00122       /***************************************************/
00123 
00127       void setForceDirection(const fmatvec::Mat3xV& fd);
00128 
00132       void setMomentDirection(const fmatvec::Mat3xV& md);
00133 
00137       void setFrameOfReferenceID(int ID) {
00138         refFrameID = ID;
00139       }
00140 
00141       virtual void initializeUsingXML(xercesc::DOMElement *element);
00142       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00143 
00144       virtual std::string getType() const {
00145         return "Joint";
00146       }
00147 
00148 #ifdef HAVE_OPENMBVCPPINTERFACE
00149 
00150       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))) {
00151         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00152         setOpenMBVForce(ombv.createOpenMBV());
00153       }
00154       void setOpenMBVForce(OpenMBV::Arrow *arrow) {
00155         std::vector<bool> which; which.resize(2, false);
00156         which[1]=true;
00157         LinkMechanics::setOpenMBVForceArrow(arrow, which);
00158       }
00159 
00161       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))) {
00162         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00163         setOpenMBVMoment(ombv.createOpenMBV());
00164       }
00165       void setOpenMBVMoment(OpenMBV::Arrow *arrow) {
00166         std::vector<bool> which; which.resize(2, false);
00167         which[1]=true;
00168         LinkMechanics::setOpenMBVMomentArrow(arrow, which);
00169       }
00170 #endif
00171 
00172     protected:
00176       Frame *refFrame;
00177       int refFrameID;
00178 
00182       fmatvec::Index IT, IR;
00183 
00187       fmatvec::Mat3xV forceDir, momentDir;
00188 
00192       fmatvec::Mat3xV Wf, Wm;
00193 
00197       fmatvec::Mat3xV JT;
00198 
00202       fmatvec::Vec3 WrP0P1, WvP0P1, WomP0P1;
00203 
00207       GeneralizedForceLaw *ffl;
00208 
00212       GeneralizedForceLaw * fml;
00213 
00217       GeneralizedImpactLaw *fifl;
00218 
00222       GeneralizedImpactLaw * fiml;
00223 
00227       fmatvec::Vec gdn, gdd;
00228 
00232       Frame C;
00233 
00234     private:
00235       std::string saved_ref1, saved_ref2;
00236   };
00237 
00238   class InverseKineticsJoint : public Joint {
00239     public:
00240       InverseKineticsJoint(const std::string &name);
00241       virtual void updateb(double t);
00242       void calcbSize();
00243       void setBody(RigidBody* body_) {
00244         body = body_;
00245       }
00246       virtual void init(InitStage stage);
00247       virtual bool isSetValued() const {
00248         return true;
00249       }
00250 
00251     protected:
00252       RigidBody* body;
00253   };
00254 
00255 }
00256 
00257 #endif
00258