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 "utils/function.h"
00024
00025 #ifdef HAVE_OPENMBVCPPINTERFACE
00026 namespace OpenMBV {
00027 class Arrow;
00028 }
00029 #endif
00030
00031 namespace MBSim {
00032
00033 class RigidBody;
00034 class Frame;
00035
00040 class Constraint : public Object {
00041 private:
00042
00043 public:
00044 Constraint(const std::string &name);
00045 #ifdef HAVE_OPENMBVCPPINTERFACE
00046 virtual OpenMBV::Group* getOpenMBVGrp() {return 0;}
00047 #endif
00048 };
00049
00050 class GearConstraint : public Constraint {
00051
00052 public:
00053 GearConstraint(const std::string &name, RigidBody* body);
00054 GearConstraint(const std::string &name="");
00055
00056 void addDependency(RigidBody* body_, double ratio);
00057
00058 void init(InitStage stage);
00059
00060 void setDependentBody(RigidBody* body_) {bd=body_; }
00061
00062 void updateStateDependentVariables(double t);
00063 void updateJacobians(double t, int j=0);
00064 void setUpInverseKinetics();
00065
00066 void initializeUsingXML(MBXMLUtils::TiXmlElement * element);
00067
00068 virtual std::string getType() const { return "GearConstraint"; }
00069
00070 #ifdef HAVE_OPENMBVCPPINTERFACE
00071
00072 void setOpenMBVGearForceArrow(OpenMBV::Arrow *arrow) { FArrow = arrow; }
00073
00075 void setOpenMBVGearMomentArrow(OpenMBV::Arrow *arrow) { MArrow = arrow; }
00076 #endif
00077
00078 private:
00079 std::vector<RigidBody*> bi;
00080 RigidBody *bd;
00081 std::vector<double> ratio;
00082
00083 std::string saved_DependentBody;
00084 std::vector<std::string> saved_DependencyBodies;
00085 std::vector<double> saved_ratio;
00086
00087 #ifdef HAVE_OPENMBVCPPINTERFACE
00088 OpenMBV::Arrow *FArrow, *MArrow;
00089 #endif
00090 };
00091
00092 class KinematicConstraint : public Constraint {
00093
00094 public:
00095 KinematicConstraint(const std::string &name, RigidBody* body);
00096 KinematicConstraint(const std::string &name="");
00097
00098 void setDependentBody(RigidBody* body) {bd=body; }
00099 virtual void setUpInverseKinetics() = 0;
00100
00101 void init(InitStage stage);
00102
00103 void initializeUsingXML(MBXMLUtils::TiXmlElement * element);
00104
00105 #ifdef HAVE_OPENMBVCPPINTERFACE
00106
00107 void setOpenMBVConstraintForceArrow(OpenMBV::Arrow *arrow) { FArrow = arrow; }
00108
00110 void setOpenMBVConstraintMomentArrow(OpenMBV::Arrow *arrow) { MArrow = arrow; }
00111 #endif
00112
00113 protected:
00114 RigidBody *bd;
00115
00116 std::string saved_DependentBody;
00117
00118 #ifdef HAVE_OPENMBVCPPINTERFACE
00119 OpenMBV::Arrow *FArrow, *MArrow;
00120 #endif
00121 };
00122
00123 class TimeDependentKinematicConstraint : public KinematicConstraint {
00124
00125 public:
00126 TimeDependentKinematicConstraint(const std::string &name, RigidBody* body) : KinematicConstraint(name,body), f(NULL), fd(NULL), fdd(NULL) {}
00127 TimeDependentKinematicConstraint(const std::string &name="") : KinematicConstraint(name), f(NULL), fd(NULL), fdd(NULL) {}
00128
00129 void init(InitStage stage);
00130
00131 void calcqSize();
00132
00133 void setGeneralizedPositionFunction(Function1<fmatvec::VecV,double>* f_) { f = f_;}
00134 void setFirstDerivativeOfGeneralizedPositionFunction(Function1<fmatvec::VecV,double>* fd_) { fd = fd_;}
00135 void setSecondDerivativeOfGeneralizedPositionFunction(Function1<fmatvec::VecV,double>* fdd_) { fdd = fdd_;}
00136
00137 void setUpInverseKinetics();
00138
00139 void updateqd(double t);
00140 void updateStateDependentVariables(double t);
00141 void updateJacobians(double t, int j=0);
00142
00143 void initializeUsingXML(MBXMLUtils::TiXmlElement * element);
00144
00145 virtual std::string getType() const { return "TimeDependentKinematicConstraint"; }
00146
00147 private:
00148 Function1<fmatvec::VecV,double> *f, *fd, *fdd;
00149 };
00150
00151 class StateDependentKinematicConstraint : public KinematicConstraint {
00152
00153 public:
00154 StateDependentKinematicConstraint(const std::string &name, RigidBody* body) : KinematicConstraint(name,body), f(NULL), fd(NULL) {}
00155 StateDependentKinematicConstraint(const std::string &name="") : KinematicConstraint(name), f(NULL), fd(NULL) {}
00156
00157 void init(InitStage stage);
00158
00159 void calcqSize();
00160
00161 void setGeneralizedVelocityFunction(Function1<fmatvec::VecV,fmatvec::Vec>* f_) { f = f_;}
00162 void setFirstDerivativeOfGeneralizedVelocityFunction(Function2<fmatvec::VecV,fmatvec::Vec,fmatvec::Vec>* fd_) { fd = fd_;}
00163
00164 virtual void setUpInverseKinetics();
00165
00166 void updateqd(double t);
00167 void updateStateDependentVariables(double t);
00168 void updateJacobians(double t, int j=0);
00169
00170 void initializeUsingXML(MBXMLUtils::TiXmlElement * element);
00171
00172 virtual std::string getType() const { return "StateDependetKinematicConstraint"; }
00173
00174 private:
00175 Function1<fmatvec::VecV,fmatvec::Vec> *f;
00176 Function2<fmatvec::VecV,fmatvec::Vec,fmatvec::Vec> *fd;
00177 };
00178
00184 class JointConstraint : public Constraint {
00185 public:
00186 JointConstraint(const std::string &name="");
00187
00188 void init(InitStage stage);
00189 void initz();
00190
00191 void connect(Frame* frame1, Frame* frame2);
00192 void setDependentBodiesFirstSide(std::vector<RigidBody*> bd);
00193 void setDependentBodiesSecondSide(std::vector<RigidBody*> bd);
00194 void setIndependentBody(RigidBody* bi);
00195
00196 virtual void setUpInverseKinetics();
00197 void setForceDirection(const fmatvec::Mat3xV& d_) {dT = d_;}
00198 void setMomentDirection(const fmatvec::Mat3xV& d_) {dR = d_;}
00199 void setq0(const fmatvec::Vec& q0_) {q0 = q0_;}
00200
00201 fmatvec::Vec res(const fmatvec::Vec& q, const double& t);
00202 void updateStateDependentVariables(double t);
00203 void updateJacobians(double t, int j=0);
00204 virtual void initializeUsingXML(MBXMLUtils::TiXmlElement *element);
00205 virtual MBXMLUtils::TiXmlElement* writeXMLFile(MBXMLUtils::TiXmlNode *element);
00206
00207 virtual std::string getType() const { return "JointConstraint"; }
00208
00210 void setOpenMBVJointForceArrow(OpenMBV::Arrow *arrow) {
00211 FArrow = arrow;
00212 }
00213
00215 void setOpenMBVJointMomentArrow(OpenMBV::Arrow *arrow) {
00216 MArrow = arrow;
00217 }
00218
00219 private:
00220 class Residuum : public Function1<fmatvec::Vec,fmatvec::Vec> {
00221 private:
00222 std::vector<RigidBody*> body1, body2;
00223 fmatvec::Mat3xV dT, dR;
00224 Frame *frame1, *frame2;
00225 double t;
00226 std::vector<Frame*> i1,i2;
00227 public:
00228 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_);
00229 fmatvec::Vec operator()(const fmatvec::Vec &x, const void * =NULL);
00230 };
00231 std::vector<RigidBody*> bd1;
00232 std::vector<RigidBody*> bd2;
00233 RigidBody *bi;
00234 std::vector<Frame*> if1;
00235 std::vector<Frame*> if2;
00236
00237 Frame *frame1,*frame2;
00238
00239 fmatvec::Mat3xV dT;
00240 fmatvec::Mat3xV dR;
00241
00242 std::vector<fmatvec::Index> Iq1, Iq2, Iu1, Iu2, Ih1, Ih2;
00243 int nq, nu, nh;
00244 fmatvec::Vec q, u;
00245 fmatvec::Mat J;
00246 fmatvec::Vec j;
00247 fmatvec::Mat JT, JR;
00248 fmatvec::Vec q0;
00249
00250 std::string saved_ref1, saved_ref2;
00251 std::vector<std::string> saved_RigidBodyFirstSide, saved_RigidBodySecondSide;
00252 std::string saved_IndependentBody;
00253 #ifdef HAVE_OPENMBVCPPINTERFACE
00254 OpenMBV::Arrow *FArrow, *MArrow;
00255 #endif
00256 };
00257
00258 }
00259
00260 #endif