00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020 #ifndef _JOINT_H_
00021 #define _JOINT_H_
00022
00023 #include "mbsim/link_mechanics.h"
00024 #include "mbsim/frame.h"
00025
00026 #ifdef HAVE_OPENMBVCPPINTERFACE
00027 #include "mbsim/utils/boost_parameters.h"
00028 #include "mbsim/utils/openmbv_utils.h"
00029 #endif
00030
00031 namespace MBSim {
00032
00033 class GeneralizedForceLaw;
00034 class GeneralizedImpactLaw;
00035 class FrictionForceLaw;
00036 class FrictionImpactLaw;
00037 class RigidBody;
00038
00048 class Joint : public LinkMechanics {
00049 public:
00054 Joint(const std::string &name = "");
00055
00059 virtual ~Joint();
00060
00061
00062 virtual void updatewb(double t, int i = 0);
00063 virtual void updateW(double t, int i = 0);
00064 virtual void updateh(double t, int i = 0);
00065 virtual void updateg(double t);
00066 virtual void updategd(double t);
00067 virtual void updateJacobians(double t, int j = 0);
00068
00069
00070
00071 virtual void updatexd(double t);
00072 virtual void updatedx(double t, double dt);
00073 virtual void calcxSize();
00074 virtual void init(InitStage stage);
00075
00076
00077
00078 virtual void calclaSize(int j);
00079 virtual void calcgSize(int j);
00080 virtual void calcgdSize(int j);
00081 virtual void calcrFactorSize(int j);
00082 virtual bool isSetValued() const;
00083 virtual bool isSingleValued() const;
00084 virtual bool isActive() const {
00085 return true;
00086 }
00087 virtual bool gActiveChanged() {
00088 return false;
00089 }
00090 virtual void solveImpactsFixpointSingle(double dt);
00091 virtual void solveConstraintsFixpointSingle();
00092 virtual void solveImpactsGaussSeidel(double dt);
00093 virtual void solveConstraintsGaussSeidel();
00094 virtual void solveImpactsRootFinding(double dt);
00095 virtual void solveConstraintsRootFinding();
00096 virtual void jacobianConstraints();
00097 virtual void jacobianImpacts();
00098 virtual void updaterFactors();
00099 virtual void checkImpactsForTermination(double dt);
00100 virtual void checkConstraintsForTermination();
00101
00102
00103
00104 virtual void plot(double t, double dt = 1);
00105
00106
00107
00112 virtual void connect(Frame *frame1, Frame* frame2);
00113
00114
00115
00116 void setForceLaw(GeneralizedForceLaw * rc) {
00117 ffl = rc;
00118 }
00119 void setMomentLaw(GeneralizedForceLaw * rc) {
00120 fml = rc;
00121 }
00122
00123
00127 void setForceDirection(const fmatvec::Mat3xV& fd);
00128
00132 void setMomentDirection(const fmatvec::Mat3xV& md);
00133
00137 void setFrameOfReferenceID(int ID) {
00138 refFrameID = ID;
00139 }
00140
00141 virtual void initializeUsingXML(xercesc::DOMElement *element);
00142 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00143
00144 virtual std::string getType() const {
00145 return "Joint";
00146 }
00147
00148 #ifdef HAVE_OPENMBVCPPINTERFACE
00149
00150 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))) {
00151 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00152 setOpenMBVForce(ombv.createOpenMBV());
00153 }
00154 void setOpenMBVForce(OpenMBV::Arrow *arrow) {
00155 std::vector<bool> which; which.resize(2, false);
00156 which[1]=true;
00157 LinkMechanics::setOpenMBVForceArrow(arrow, which);
00158 }
00159
00161 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))) {
00162 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00163 setOpenMBVMoment(ombv.createOpenMBV());
00164 }
00165 void setOpenMBVMoment(OpenMBV::Arrow *arrow) {
00166 std::vector<bool> which; which.resize(2, false);
00167 which[1]=true;
00168 LinkMechanics::setOpenMBVMomentArrow(arrow, which);
00169 }
00170 #endif
00171
00172 protected:
00176 Frame *refFrame;
00177 int refFrameID;
00178
00182 fmatvec::Index IT, IR;
00183
00187 fmatvec::Mat3xV forceDir, momentDir;
00188
00192 fmatvec::Mat3xV Wf, Wm;
00193
00197 fmatvec::Mat3xV JT;
00198
00202 fmatvec::Vec3 WrP0P1, WvP0P1, WomP0P1;
00203
00207 GeneralizedForceLaw *ffl;
00208
00212 GeneralizedForceLaw * fml;
00213
00217 GeneralizedImpactLaw *fifl;
00218
00222 GeneralizedImpactLaw * fiml;
00223
00227 fmatvec::Vec gdn, gdd;
00228
00232 Frame C;
00233
00234 private:
00235 std::string saved_ref1, saved_ref2;
00236 };
00237
00238 class InverseKineticsJoint : public Joint {
00239 public:
00240 InverseKineticsJoint(const std::string &name);
00241 virtual void updateb(double t);
00242 void calcbSize();
00243 void setBody(RigidBody* body_) {
00244 body = body_;
00245 }
00246 virtual void init(InitStage stage);
00247 virtual bool isSetValued() const {
00248 return true;
00249 }
00250
00251 protected:
00252 RigidBody* body;
00253 };
00254
00255 }
00256
00257 #endif
00258