00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
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
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
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