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 "functions/auxiliary_functions.h"
00024 
00025 #ifdef HAVE_OPENMBVCPPINTERFACE
00026 #include "mbsim/utils/boost_parameters.h"
00027 #include "mbsim/utils/openmbv_utils.h"
00028 #endif
00029 
00030 namespace MBSim {
00031 
00032   class RigidBody;
00033   class Frame;
00034 
00039   class Constraint : public Object {
00040     private:
00041 
00042     public:
00043       Constraint(const std::string &name);
00044 #ifdef HAVE_OPENMBVCPPINTERFACE
00045       virtual OpenMBV::Group* getOpenMBVGrp() {return 0;}
00046 #endif
00047   };
00048 
00049   struct Transmission {
00050     Transmission(RigidBody *body_, double ratio_) : body(body_), ratio(ratio_) { }
00051     RigidBody *body;
00052     double ratio;
00053   };
00054 
00055   class GearConstraint : public Constraint {
00056 
00057     public:
00058       GearConstraint(const std::string &name="");
00059 
00060       void addTransmission(const Transmission &transmission);
00061 
00062       void init(InitStage stage);
00063 
00064       void setDependentBody(RigidBody* body_) {bd=body_; }
00065 
00066       void updateStateDependentVariables(double t);
00067       void updateJacobians(double t, int j=0);
00068       void setUpInverseKinetics();
00069 
00070       void initializeUsingXML(xercesc::DOMElement * element);
00071 
00072       virtual std::string getType() const { return "GearConstraint"; }
00073     
00074 #ifdef HAVE_OPENMBVCPPINTERFACE
00075 
00076       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))) { 
00077         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00078         FArrow=ombv.createOpenMBV();
00079       }
00080 
00082       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))) { 
00083         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00084         MArrow=ombv.createOpenMBV();
00085       }
00086 #endif
00087 
00088     private:
00089       std::vector<RigidBody*> bi;
00090       RigidBody *bd;
00091       std::vector<double> ratio;
00092 
00093       std::string saved_DependentBody;
00094       std::vector<std::string> saved_IndependentBody;
00095 
00096 #ifdef HAVE_OPENMBVCPPINTERFACE
00097       OpenMBV::Arrow *FArrow, *MArrow;
00098 #endif
00099   };
00100 
00101   class KinematicConstraint : public Constraint {
00102 
00103     public:
00104       KinematicConstraint(const std::string &name="");
00105 
00106       void setDependentBody(RigidBody* body) {bd=body; }
00107       virtual void setUpInverseKinetics() = 0;
00108 
00109       void init(InitStage stage);
00110 
00111       void initializeUsingXML(xercesc::DOMElement * element);
00112 
00113 #ifdef HAVE_OPENMBVCPPINTERFACE
00114 
00115       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))) { 
00116         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00117         FArrow=ombv.createOpenMBV();
00118       }
00119 
00121       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))) { 
00122         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00123         MArrow=ombv.createOpenMBV();
00124       }
00125 #endif
00126 
00127     protected:
00128       RigidBody *bd;
00129 
00130       std::string saved_DependentBody;
00131 
00132 #ifdef HAVE_OPENMBVCPPINTERFACE
00133       OpenMBV::Arrow *FArrow, *MArrow;
00134 #endif
00135   };
00136 
00137   class GeneralizedPositionConstraint : public KinematicConstraint {
00138 
00139     public:
00140       GeneralizedPositionConstraint(const std::string &name="") : KinematicConstraint(name), f(NULL) {}
00141       ~GeneralizedPositionConstraint() { delete f; }
00142 
00143       void init(InitStage stage);
00144 
00145       void setConstraintFunction(fmatvec::Function<fmatvec::VecV(double)>* f_) { f = f_; }
00146 
00147       void setUpInverseKinetics();
00148 
00149       void updateStateDependentVariables(double t);
00150       void updateJacobians(double t, int j=0);
00151 
00152       void initializeUsingXML(xercesc::DOMElement * element);
00153 
00154       virtual std::string getType() const { return "GeneralizedPositionConstraint"; }
00155 
00156     private:
00157       fmatvec::Function<fmatvec::VecV(double)> *f;
00158   };
00159 
00160   class GeneralizedVelocityConstraint : public KinematicConstraint {
00161 
00162     public:
00163       GeneralizedVelocityConstraint(const std::string &name="") : KinematicConstraint(name), f(NULL) {}
00164       ~GeneralizedVelocityConstraint() { delete f; }
00165 
00166       void init(InitStage stage);
00167 
00168       void calcxSize();
00169 
00170       // NOTE: we can not use a overloaded setConstraintFunction here due to restrictions in XML but define them for convinience in c++
00171       void setGeneralConstraintFunction(fmatvec::Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) {
00172         f = f_;
00173       }
00174       void setTimeDependentConstraintFunction(fmatvec::Function<fmatvec::VecV(double)>* f_) {
00175         setGeneralConstraintFunction(new TimeDependentFunction<fmatvec::VecV>(f_));
00176       }
00177       void setStateDependentConstraintFunction(fmatvec::Function<fmatvec::VecV(fmatvec::VecV)>* f_) {
00178         setGeneralConstraintFunction(new StateDependentFunction<fmatvec::VecV>(f_));
00179       }
00180       void setConstraintFunction(fmatvec::Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) { setGeneralConstraintFunction(f_); }
00181       void setConstraintFunction(fmatvec::Function<fmatvec::VecV(double)>* f_) { setTimeDependentConstraintFunction(f_); }
00182       void setConstraintFunction(fmatvec::Function<fmatvec::VecV(fmatvec::VecV)>* f_) { setStateDependentConstraintFunction(f_); }
00183 
00184       virtual void setUpInverseKinetics();
00185 
00186       void updatexd(double t);
00187       void updateStateDependentVariables(double t);
00188       void updateJacobians(double t, int j=0);
00189 
00190       void initializeUsingXML(xercesc::DOMElement * element);
00191 
00192       virtual std::string getType() const { return "GeneralizedVelocityConstraint"; }
00193 
00194     private:
00195       fmatvec::Function<fmatvec::VecV(fmatvec::VecV,double)> *f; 
00196   };
00197 
00198   class GeneralizedAccelerationConstraint : public KinematicConstraint {
00199 
00200     public:
00201       GeneralizedAccelerationConstraint(const std::string &name="") : KinematicConstraint(name) {}
00202       ~GeneralizedAccelerationConstraint() { delete f; }
00203 
00204       void init(InitStage stage);
00205 
00206       void calcxSize();
00207 
00208       // NOTE: we can not use a overloaded setConstraintFunction here due to restrictions in XML but define them for convinience in c++
00209       void setGeneralConstraintFunction(fmatvec::Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) {
00210         f = f_;
00211       }
00212       void setTimeDependentConstraintFunction(fmatvec::Function<fmatvec::VecV(double)>* f_) {
00213         setGeneralConstraintFunction(new TimeDependentFunction<fmatvec::VecV>(f_));
00214       }
00215       void setStateDependentConstraintFunction(fmatvec::Function<fmatvec::VecV(fmatvec::VecV)>* f_) {
00216         setGeneralConstraintFunction( new StateDependentFunction<fmatvec::VecV>(f_));
00217       }
00218       void setConstraintFunction(fmatvec::Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) { setGeneralConstraintFunction(f_); }
00219       void setConstraintFunction(fmatvec::Function<fmatvec::VecV(double)>* f_) { setTimeDependentConstraintFunction(f_); }
00220       void setConstraintFunction(fmatvec::Function<fmatvec::VecV(fmatvec::VecV)>* f_) { setStateDependentConstraintFunction(f_); }
00221 
00222       virtual void setUpInverseKinetics();
00223 
00224       void updatexd(double t);
00225       void updateStateDependentVariables(double t);
00226       void updateJacobians(double t, int j=0);
00227 
00228       void initializeUsingXML(xercesc::DOMElement * element);
00229 
00230       virtual std::string getType() const { return "GeneralizedAccelerationConstraint"; }
00231 
00232     private:
00233       fmatvec::Function<fmatvec::VecV(fmatvec::VecV,double)> *f; 
00234   };
00235 
00241   class JointConstraint : public Constraint {
00242     public:
00243       JointConstraint(const std::string &name="");
00244 
00245       void init(InitStage stage);
00246       void initz();
00247 
00248       void connect(Frame* frame1, Frame* frame2);
00249       void setDependentBodiesFirstSide(std::vector<RigidBody*> bd);
00250       void setDependentBodiesSecondSide(std::vector<RigidBody*> bd);
00251       void setIndependentBody(RigidBody* bi);
00252 
00253       virtual void setUpInverseKinetics();
00254       void setForceDirection(const fmatvec::Mat3xV& d_);
00255       void setMomentDirection(const fmatvec::Mat3xV& d_);
00256 
00260       void setFrameOfReferenceID(int ID) { refFrameID=ID; }
00261 
00262       fmatvec::Vec res(const fmatvec::Vec& q, const double& t);
00263       void updateStateDependentVariables(double t); 
00264       void updateJacobians(double t, int j=0); 
00265       virtual void initializeUsingXML(xercesc::DOMElement *element);
00266       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00267 
00268       virtual std::string getType() const { return "JointConstraint"; }
00269 
00270 #ifdef HAVE_OPENMBVCPPINTERFACE
00271 
00272       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))) { 
00273         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00274         FArrow=ombv.createOpenMBV();
00275       }
00276 
00278       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))) { 
00279         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00280         MArrow=ombv.createOpenMBV();
00281       }
00282 #endif
00283 
00284     private:
00285       class Residuum : public fmatvec::Function<fmatvec::Vec(fmatvec::Vec)> {
00286         private:
00287           std::vector<RigidBody*> body1, body2;
00288           fmatvec::Mat3xV dT, dR;
00289           Frame *frame1, *frame2;
00290           double t;
00291           std::vector<Frame*> i1,i2;
00292         public:
00293           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_);
00294           fmatvec::Vec operator()(const fmatvec::Vec &x);
00295       };
00296       std::vector<RigidBody*> bd1;
00297       std::vector<RigidBody*> bd2;
00298       RigidBody *bi;
00299       std::vector<Frame*> if1;
00300       std::vector<Frame*> if2;
00301 
00302       Frame *frame1,*frame2;
00303 
00307       Frame *refFrame;
00308       int refFrameID;
00309 
00310       fmatvec::Mat3xV dT, dR, forceDir, momentDir;
00311 
00312       std::vector<fmatvec::Index> Iq1, Iq2, Iu1, Iu2, Ih1, Ih2;
00313       int nq, nu, nh;
00314       fmatvec::Vec q, u;
00315       fmatvec::Mat J;
00316       fmatvec::Vec j;
00317       fmatvec::Mat JT, JR;
00318       fmatvec::Vec q0;
00319 
00320       std::string saved_ref1, saved_ref2;
00321       std::vector<std::string> saved_RigidBodyFirstSide, saved_RigidBodySecondSide;
00322       std::string saved_IndependentBody;
00323 #ifdef HAVE_OPENMBVCPPINTERFACE
00324       OpenMBV::Arrow *FArrow, *MArrow;
00325 #endif
00326   };
00327 
00328 }
00329 
00330 #endif