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(Element::InitStage stage);
00144 
00145       void setConstraintFunction(Function<fmatvec::VecV(double)>* f_) {
00146         f = f_;
00147         f->setParent(this);
00148         f->setName("Constraint");
00149       }
00150 
00151       void setUpInverseKinetics();
00152 
00153       void updateStateDependentVariables(double t);
00154       void updateJacobians(double t, int j=0);
00155 
00156       void initializeUsingXML(xercesc::DOMElement * element);
00157 
00158       virtual std::string getType() const { return "GeneralizedPositionConstraint"; }
00159 
00160     private:
00161       Function<fmatvec::VecV(double)> *f;
00162   };
00163 
00164   class GeneralizedVelocityConstraint : public KinematicConstraint {
00165 
00166     public:
00167       GeneralizedVelocityConstraint(const std::string &name="") : KinematicConstraint(name), f(NULL) {}
00168       ~GeneralizedVelocityConstraint() { delete f; }
00169 
00170       void init(InitStage stage);
00171 
00172       void calcxSize();
00173 
00174       // NOTE: we can not use a overloaded setConstraintFunction here due to restrictions in XML but define them for convinience in c++
00175       void setGeneralConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) {
00176         f = f_;
00177       }
00178       void setTimeDependentConstraintFunction(Function<fmatvec::VecV(double)>* f_) {
00179         setGeneralConstraintFunction(new TimeDependentFunction<fmatvec::VecV>(f_));
00180       }
00181       void setStateDependentConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) {
00182         setGeneralConstraintFunction(new StateDependentFunction<fmatvec::VecV>(f_));
00183       }
00184       void setConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) { setGeneralConstraintFunction(f_); }
00185       void setConstraintFunction(Function<fmatvec::VecV(double)>* f_) { setTimeDependentConstraintFunction(f_); }
00186       void setConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) { setStateDependentConstraintFunction(f_); }
00187 
00188       virtual void setUpInverseKinetics();
00189 
00190       void updatexd(double t);
00191       void updateStateDependentVariables(double t);
00192       void updateJacobians(double t, int j=0);
00193 
00194       void initializeUsingXML(xercesc::DOMElement * element);
00195 
00196       virtual std::string getType() const { return "GeneralizedVelocityConstraint"; }
00197 
00198     private:
00199       Function<fmatvec::VecV(fmatvec::VecV,double)> *f; 
00200   };
00201 
00202   class GeneralizedAccelerationConstraint : public KinematicConstraint {
00203 
00204     public:
00205       GeneralizedAccelerationConstraint(const std::string &name="") : KinematicConstraint(name) {}
00206       ~GeneralizedAccelerationConstraint() { delete f; }
00207 
00208       void init(InitStage stage);
00209 
00210       void calcxSize();
00211 
00212       // NOTE: we can not use a overloaded setConstraintFunction here due to restrictions in XML but define them for convinience in c++
00213       void setGeneralConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) {
00214         f = f_;
00215       }
00216       void setTimeDependentConstraintFunction(Function<fmatvec::VecV(double)>* f_) {
00217         setGeneralConstraintFunction(new TimeDependentFunction<fmatvec::VecV>(f_));
00218       }
00219       void setStateDependentConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) {
00220         setGeneralConstraintFunction( new StateDependentFunction<fmatvec::VecV>(f_));
00221       }
00222       void setConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV,double)>* f_) { setGeneralConstraintFunction(f_); }
00223       void setConstraintFunction(Function<fmatvec::VecV(double)>* f_) { setTimeDependentConstraintFunction(f_); }
00224       void setConstraintFunction(Function<fmatvec::VecV(fmatvec::VecV)>* f_) { setStateDependentConstraintFunction(f_); }
00225 
00226       virtual void setUpInverseKinetics();
00227 
00228       void updatexd(double t);
00229       void updateStateDependentVariables(double t);
00230       void updateJacobians(double t, int j=0);
00231 
00232       void initializeUsingXML(xercesc::DOMElement * element);
00233 
00234       virtual std::string getType() const { return "GeneralizedAccelerationConstraint"; }
00235 
00236     private:
00237       Function<fmatvec::VecV(fmatvec::VecV,double)> *f; 
00238   };
00239 
00245   class JointConstraint : public Constraint {
00246     public:
00247       JointConstraint(const std::string &name="");
00248 
00249       void init(InitStage stage);
00250       void initz();
00251 
00252       void connect(Frame* frame1, Frame* frame2);
00253       void setDependentBodiesFirstSide(std::vector<RigidBody*> bd);
00254       void setDependentBodiesSecondSide(std::vector<RigidBody*> bd);
00255       void setIndependentBody(RigidBody* bi);
00256 
00257       virtual void setUpInverseKinetics();
00258       void setForceDirection(const fmatvec::Mat3xV& d_);
00259       void setMomentDirection(const fmatvec::Mat3xV& d_);
00260 
00264       void setFrameOfReferenceID(int ID) { refFrameID=ID; }
00265 
00266       fmatvec::Vec res(const fmatvec::Vec& q, const double& t);
00267       void updateStateDependentVariables(double t); 
00268       void updateJacobians(double t, int j=0); 
00269       virtual void initializeUsingXML(xercesc::DOMElement *element);
00270       virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00271 
00272       virtual std::string getType() const { return "JointConstraint"; }
00273 
00274 #ifdef HAVE_OPENMBVCPPINTERFACE
00275 
00276       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))) { 
00277         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00278         FArrow=ombv.createOpenMBV();
00279       }
00280 
00282       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))) { 
00283         OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00284         MArrow=ombv.createOpenMBV();
00285       }
00286 #endif
00287 
00288     private:
00289       class Residuum : public Function<fmatvec::Vec(fmatvec::Vec)> {
00290         private:
00291           std::vector<RigidBody*> body1, body2;
00292           fmatvec::Mat3xV dT, dR;
00293           Frame *frame1, *frame2;
00294           double t;
00295           std::vector<Frame*> i1,i2;
00296         public:
00297           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_);
00298           fmatvec::Vec operator()(const fmatvec::Vec &x);
00299       };
00300       std::vector<RigidBody*> bd1;
00301       std::vector<RigidBody*> bd2;
00302       RigidBody *bi;
00303       std::vector<Frame*> if1;
00304       std::vector<Frame*> if2;
00305 
00306       Frame *frame1,*frame2;
00307 
00311       Frame *refFrame;
00312       int refFrameID;
00313 
00314       fmatvec::Mat3xV dT, dR, forceDir, momentDir;
00315 
00316       std::vector<fmatvec::Index> Iq1, Iq2, Iu1, Iu2, Ih1, Ih2;
00317       int nq, nu, nh;
00318       fmatvec::Vec q, u;
00319       fmatvec::Mat J;
00320       fmatvec::Vec j;
00321       fmatvec::Mat JT, JR;
00322       fmatvec::Vec q0;
00323 
00324       std::string saved_ref1, saved_ref2;
00325       std::vector<std::string> saved_RigidBodyFirstSide, saved_RigidBodySecondSide;
00326       std::string saved_IndependentBody;
00327 #ifdef HAVE_OPENMBVCPPINTERFACE
00328       OpenMBV::Arrow *FArrow, *MArrow;
00329 #endif
00330   };
00331 
00332 }
00333 
00334 #endif