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