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(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
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
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