MBSim

Project Page
constraint.h
00001 /* Copyright (C) 2004-2009 MBSim Development Team
00002  * This library is free software; you can redistribute it and/or 
00003  * modify it under the terms of the GNU Lesser General Public 
00004  * License as published by the Free Software Foundation; either 
00005  * version 2.1 of the License, or (at your option) any later version. 
00006  *  
00007  * This library is distributed in the hope that it will be useful, 
00008  * but WITHOUT ANY WARRANTY; without even the implied warranty of 
00009  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU 
00010  * Lesser General Public License for more details. 
00011  *  
00012  * You should have received a copy of the GNU Lesser General Public 
00013  * License along with this library; if not, write to the Free Software 
00014  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301 USA
00015  *
00016  * Contact: martin.o.foerg@googlemail.com
00017  */
00018 
00019 #ifndef _CONSTRAINT_H
00020 #define _CONSTRAINT_H
00021 
00022 #include "object.h"
00023 #include "utils/function.h"
00024 
00025 #ifdef HAVE_OPENMBVCPPINTERFACE
00026 namespace OpenMBV {
00027   class Arrow;
00028 }
00029 #endif
00030 
00031 namespace MBSim {
00032 
00033   class RigidBody;
00034   class Frame;
00035 
00040   class Constraint : public Object {
00041     private:
00042 
00043     public:
00044       Constraint(const std::string &name);
00045 #ifdef HAVE_OPENMBVCPPINTERFACE
00046       virtual OpenMBV::Group* getOpenMBVGrp() {return 0;}
00047 #endif
00048   };
00049 
00050   class GearConstraint : public Constraint {
00051 
00052     public:
00053       GearConstraint(const std::string &name, RigidBody* body);
00054       GearConstraint(const std::string &name="");
00055 
00056       void addDependency(RigidBody* body_, double ratio);
00057 
00058       void init(InitStage stage);
00059 
00060       void setDependentBody(RigidBody* body_) {bd=body_; }
00061 
00062       void updateStateDependentVariables(double t);
00063       void updateJacobians(double t, int j=0);
00064       void setUpInverseKinetics();
00065 
00066       void initializeUsingXML(MBXMLUtils::TiXmlElement * element);
00067 
00068       virtual std::string getType() const { return "GearConstraint"; }
00069     
00070 #ifdef HAVE_OPENMBVCPPINTERFACE
00071 
00072       void setOpenMBVGearForceArrow(OpenMBV::Arrow *arrow) { FArrow = arrow; }
00073 
00075       void setOpenMBVGearMomentArrow(OpenMBV::Arrow *arrow) { MArrow = arrow; }
00076 #endif
00077 
00078     private:
00079       std::vector<RigidBody*> bi;
00080       RigidBody *bd;
00081       std::vector<double> ratio;
00082       
00083       std::string saved_DependentBody;
00084       std::vector<std::string> saved_DependencyBodies;
00085       std::vector<double> saved_ratio;
00086 
00087 #ifdef HAVE_OPENMBVCPPINTERFACE
00088       OpenMBV::Arrow *FArrow, *MArrow;
00089 #endif
00090   };
00091 
00092   class KinematicConstraint : public Constraint {
00093 
00094     public:
00095       KinematicConstraint(const std::string &name, RigidBody* body);
00096       KinematicConstraint(const std::string &name="");
00097 
00098       void setDependentBody(RigidBody* body) {bd=body; }
00099       virtual void setUpInverseKinetics() = 0;
00100 
00101       void init(InitStage stage);
00102 
00103       void initializeUsingXML(MBXMLUtils::TiXmlElement * element);
00104 
00105 #ifdef HAVE_OPENMBVCPPINTERFACE
00106 
00107       void setOpenMBVConstraintForceArrow(OpenMBV::Arrow *arrow) { FArrow = arrow; }
00108 
00110       void setOpenMBVConstraintMomentArrow(OpenMBV::Arrow *arrow) { MArrow = arrow; }
00111 #endif
00112 
00113     protected:
00114       RigidBody *bd;
00115 
00116       std::string saved_DependentBody;
00117 
00118 #ifdef HAVE_OPENMBVCPPINTERFACE
00119       OpenMBV::Arrow *FArrow, *MArrow;
00120 #endif
00121   };
00122 
00123   class TimeDependentKinematicConstraint : public KinematicConstraint {
00124 
00125     public:
00126       TimeDependentKinematicConstraint(const std::string &name, RigidBody* body) : KinematicConstraint(name,body), f(NULL), fd(NULL), fdd(NULL) {}
00127       TimeDependentKinematicConstraint(const std::string &name="") : KinematicConstraint(name), f(NULL), fd(NULL), fdd(NULL) {}
00128 
00129       void init(InitStage stage);
00130 
00131       void calcqSize();
00132 
00133       void setGeneralizedPositionFunction(Function1<fmatvec::VecV,double>* f_) { f = f_;}
00134       void setFirstDerivativeOfGeneralizedPositionFunction(Function1<fmatvec::VecV,double>* fd_) { fd = fd_;}
00135       void setSecondDerivativeOfGeneralizedPositionFunction(Function1<fmatvec::VecV,double>* fdd_) { fdd = fdd_;}
00136 
00137       void setUpInverseKinetics();
00138 
00139       void updateqd(double t);
00140       void updateStateDependentVariables(double t);
00141       void updateJacobians(double t, int j=0);
00142 
00143       void initializeUsingXML(MBXMLUtils::TiXmlElement * element);
00144 
00145       virtual std::string getType() const { return "TimeDependentKinematicConstraint"; }
00146 
00147     private:
00148       Function1<fmatvec::VecV,double> *f, *fd, *fdd;
00149   };
00150 
00151   class StateDependentKinematicConstraint : public KinematicConstraint {
00152 
00153     public:
00154       StateDependentKinematicConstraint(const std::string &name, RigidBody* body) : KinematicConstraint(name,body), f(NULL), fd(NULL) {}
00155       StateDependentKinematicConstraint(const std::string &name="") : KinematicConstraint(name), f(NULL), fd(NULL) {}
00156 
00157       void init(InitStage stage);
00158 
00159       void calcqSize();
00160 
00161       void setGeneralizedVelocityFunction(Function1<fmatvec::VecV,fmatvec::Vec>* f_) { f = f_;}
00162       void setFirstDerivativeOfGeneralizedVelocityFunction(Function2<fmatvec::VecV,fmatvec::Vec,fmatvec::Vec>* fd_) { fd = fd_;}
00163 
00164       virtual void setUpInverseKinetics();
00165 
00166       void updateqd(double t);
00167       void updateStateDependentVariables(double t);
00168       void updateJacobians(double t, int j=0);
00169 
00170       void initializeUsingXML(MBXMLUtils::TiXmlElement * element);
00171 
00172       virtual std::string getType() const { return "StateDependetKinematicConstraint"; }
00173 
00174     private:
00175       Function1<fmatvec::VecV,fmatvec::Vec> *f; 
00176       Function2<fmatvec::VecV,fmatvec::Vec,fmatvec::Vec> *fd; 
00177   };
00178 
00184   class JointConstraint : public Constraint {
00185     public:
00186       JointConstraint(const std::string &name="");
00187 
00188       void init(InitStage stage);
00189       void initz();
00190 
00191       void connect(Frame* frame1, Frame* frame2);
00192       void setDependentBodiesFirstSide(std::vector<RigidBody*> bd);
00193       void setDependentBodiesSecondSide(std::vector<RigidBody*> bd);
00194       void setIndependentBody(RigidBody* bi);
00195 
00196       virtual void setUpInverseKinetics();
00197       void setForceDirection(const fmatvec::Mat3xV& d_) {dT = d_;}
00198       void setMomentDirection(const fmatvec::Mat3xV& d_) {dR = d_;}
00199       void setq0(const fmatvec::Vec& q0_) {q0 = q0_;}
00200 
00201       fmatvec::Vec res(const fmatvec::Vec& q, const double& t);
00202       void updateStateDependentVariables(double t); 
00203       void updateJacobians(double t, int j=0); 
00204       virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00205       virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *element);
00206 
00207       virtual std::string getType() const { return "JointConstraint"; }
00208 
00210       void setOpenMBVJointForceArrow(OpenMBV::Arrow *arrow) {
00211         FArrow = arrow;
00212       }
00213 
00215       void setOpenMBVJointMomentArrow(OpenMBV::Arrow *arrow) {
00216         MArrow = arrow;
00217       }
00218 
00219     private:
00220       class Residuum : public Function1<fmatvec::Vec,fmatvec::Vec> {
00221         private:
00222           std::vector<RigidBody*> body1, body2;
00223           fmatvec::Mat3xV dT, dR;
00224           Frame *frame1, *frame2;
00225           double t;
00226           std::vector<Frame*> i1,i2;
00227         public:
00228           Residuum(std::vector<RigidBody*> body1_, std::vector<RigidBody*> body2_, const fmatvec::Mat3xV &dT_, const fmatvec::Mat3xV &dR_,Frame *frame1_, Frame *frame2_,double t_,std::vector<Frame*> i1_, std::vector<Frame*> i2_);
00229           fmatvec::Vec operator()(const fmatvec::Vec &x, const void * =NULL);
00230       };
00231       std::vector<RigidBody*> bd1;
00232       std::vector<RigidBody*> bd2;
00233       RigidBody *bi;
00234       std::vector<Frame*> if1;
00235       std::vector<Frame*> if2;
00236 
00237       Frame *frame1,*frame2;
00238 
00239       fmatvec::Mat3xV dT;
00240       fmatvec::Mat3xV dR;
00241 
00242       std::vector<fmatvec::Index> Iq1, Iq2, Iu1, Iu2, Ih1, Ih2;
00243       int nq, nu, nh;
00244       fmatvec::Vec q, u;
00245       fmatvec::Mat J;
00246       fmatvec::Vec j;
00247       fmatvec::Mat JT, JR;
00248       fmatvec::Vec q0;
00249 
00250       std::string saved_ref1, saved_ref2;
00251       std::vector<std::string> saved_RigidBodyFirstSide, saved_RigidBodySecondSide;
00252       std::string saved_IndependentBody;
00253 #ifdef HAVE_OPENMBVCPPINTERFACE
00254       OpenMBV::Arrow *FArrow, *MArrow;
00255 #endif
00256   };
00257 
00258 }
00259 
00260 #endif