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 #include "mbsim/kinematics.h"
00026 
00027 namespace MBSim {
00028 
00029   class GeneralizedForceLaw;
00030   class GeneralizedImpactLaw;
00031   class FrictionForceLaw;
00032   class FrictionImpactLaw;
00033   class RigidBody;
00034 
00043   class Joint: public LinkMechanics {
00044     public: 
00049       Joint(const std::string &name="");
00050 
00054       virtual ~Joint();
00055 
00056       /* INHERITED INTERFACE OF LINKINTERFACE */
00057       virtual void updatewb(double t, int i=0);
00058       virtual void updateW(double t, int i=0);
00059       virtual void updateh(double t, int i=0);
00060       virtual void updateg(double t);
00061       virtual void updategd(double t);
00062       virtual void updateJacobians(double t, int j=0);
00063       /***************************************************/
00064 
00065       /* INHERITED INTERFACE OF EXTRADYNAMICINTERFACE */
00066       virtual void updatexd(double t);
00067       virtual void updatedx(double t, double dt);
00068       virtual void calcxSize();
00069       virtual void init(InitStage stage);
00070       /***************************************************/
00071 
00072       /* INHERITED INTERFACE OF LINK */
00073       virtual void calclaSize(int j);
00074       virtual void calcgSize(int j);
00075       virtual void calcgdSize(int j);
00076       virtual void calcrFactorSize(int j);
00077       virtual bool isSetValued() const;
00078       virtual bool isSingleValued() const;
00079       virtual bool isActive() const { return true; }
00080       virtual bool gActiveChanged() { return false; }
00081       virtual void solveImpactsFixpointSingle(double dt);
00082       virtual void solveConstraintsFixpointSingle();
00083       virtual void solveImpactsGaussSeidel(double dt);
00084       virtual void solveConstraintsGaussSeidel();
00085       virtual void solveImpactsRootFinding(double dt);
00086       virtual void solveConstraintsRootFinding();
00087       virtual void jacobianConstraints();
00088       virtual void jacobianImpacts();
00089       virtual void updaterFactors();
00090       virtual void checkImpactsForTermination(double dt);
00091       virtual void checkConstraintsForTermination();
00092       /***************************************************/
00093 
00094       /* INHERITED INTERFACE OF ELEMENT */
00095       virtual void plot(double t, double dt = 1);
00096       /***************************************************/
00097 
00098       /* INTERFACE FOR DERIVED CLASSES */
00103       virtual void connect(Frame *frame1, Frame* frame2);
00104       /***************************************************/
00105 
00106       /* GETTER / SETTER */
00107       void setForceLaw(GeneralizedForceLaw * rc) { ffl = rc; }
00108       void setMomentLaw(GeneralizedForceLaw * rc) { fml = rc; }
00109       void setImpactForceLaw(GeneralizedImpactLaw * rc) { fifl = rc; }
00110       void setImpactMomentLaw(GeneralizedImpactLaw * rc) { fiml = rc; }
00111       /***************************************************/
00112 
00116       void setForceDirection(const fmatvec::Mat3xV& fd);
00117 
00121       void setMomentDirection(const fmatvec::Mat3xV& md);
00122 
00123       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00124       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *element);
00125 
00126       virtual std::string getType() const { return "Joint"; }
00127 
00128 #ifdef HAVE_OPENMBVCPPINTERFACE
00129 
00130       void setOpenMBVForceArrow(OpenMBV::Arrow *arrow) {
00131         std::vector<bool> which; which.resize(2, false);
00132         which[1]=true;
00133         LinkMechanics::setOpenMBVForceArrow(arrow, which);
00134       }
00135 
00137       void setOpenMBVMomentArrow(OpenMBV::Arrow *arrow) {
00138         std::vector<bool> which; which.resize(2, false);
00139         which[1]=true;
00140         LinkMechanics::setOpenMBVMomentArrow(arrow, which);
00141       }
00142 #endif
00143 
00144     protected:
00148       fmatvec::Index IT, IR;
00149 
00153       fmatvec::Mat3xV forceDir, momentDir;
00154 
00158       fmatvec::Mat3xV Wf, Wm;
00159 
00163       fmatvec::Mat3xV JT;
00164 
00168       fmatvec::Vec3 WrP0P1, WvP0P1, WomP0P1;
00169 
00173       GeneralizedForceLaw *ffl, *fml;
00174       GeneralizedImpactLaw *fifl, *fiml;
00175 
00179       fmatvec::Vec gdn, gdd;
00180 
00184       Frame C;
00185 
00186     private:
00187       std::string saved_ref1, saved_ref2;
00188   };
00189 
00190   class InverseKineticsJoint: public Joint {
00191     public: 
00192       InverseKineticsJoint(const std::string &name);
00193       virtual void updateb(double t);
00194       void calcbSize();
00195       void setBody(RigidBody* body_)    { body = body_; }
00196       virtual void init(InitStage stage);
00197       virtual bool isSetValued() const {return true;}
00198 
00199     protected:
00200       RigidBody* body;
00201   };
00202 
00203 }
00204 
00205 #endif
00206