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