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
00047 class Joint : public LinkMechanics {
00048 public:
00053 Joint(const std::string &name = "");
00054
00058 virtual ~Joint();
00059
00060
00061 virtual void updatewb(double t, int i = 0);
00062 virtual void updateW(double t, int i = 0);
00063 virtual void updateh(double t, int i = 0);
00064 virtual void updateg(double t);
00065 virtual void updategd(double t);
00066 virtual void updateJacobians(double t, int j = 0);
00067
00068
00069
00070 virtual void updatexd(double t);
00071 virtual void updatedx(double t, double dt);
00072 virtual void calcxSize();
00073 virtual void init(InitStage stage);
00074
00075
00076
00077 virtual void calclaSize(int j);
00078 virtual void calcgSize(int j);
00079 virtual void calcgdSize(int j);
00080 virtual void calcrFactorSize(int j);
00081 virtual bool isSetValued() const;
00082 virtual bool isSingleValued() const;
00083 virtual bool isActive() const {
00084 return true;
00085 }
00086 virtual bool gActiveChanged() {
00087 return false;
00088 }
00089 virtual void solveImpactsFixpointSingle(double dt);
00090 virtual void solveConstraintsFixpointSingle();
00091 virtual void solveImpactsGaussSeidel(double dt);
00092 virtual void solveConstraintsGaussSeidel();
00093 virtual void solveImpactsRootFinding(double dt);
00094 virtual void solveConstraintsRootFinding();
00095 virtual void jacobianConstraints();
00096 virtual void jacobianImpacts();
00097 virtual void updaterFactors();
00098 virtual void checkImpactsForTermination(double dt);
00099 virtual void checkConstraintsForTermination();
00100
00101
00102
00103 virtual void plot(double t, double dt = 1);
00104
00105
00106
00111 virtual void connect(Frame *frame1, Frame* frame2);
00112
00113
00114
00115 void setForceLaw(GeneralizedForceLaw * rc) {
00116 ffl = rc;
00117 }
00118 void setMomentLaw(GeneralizedForceLaw * rc) {
00119 fml = rc;
00120 }
00121
00122
00126 void setForceDirection(const fmatvec::Mat3xV& fd);
00127
00131 void setMomentDirection(const fmatvec::Mat3xV& md);
00132
00136 void setFrameOfReferenceID(int ID) {
00137 refFrameID = ID;
00138 }
00139
00140 virtual void initializeUsingXML(xercesc::DOMElement *element);
00141 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00142
00143 virtual std::string getType() const {
00144 return "Joint";
00145 }
00146
00147 #ifdef HAVE_OPENMBVCPPINTERFACE
00148
00149 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))) {
00150 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00151 setOpenMBVForce(ombv.createOpenMBV());
00152 }
00153 void setOpenMBVForce(OpenMBV::Arrow *arrow) {
00154 std::vector<bool> which; which.resize(2, false);
00155 which[1]=true;
00156 LinkMechanics::setOpenMBVForceArrow(arrow, which);
00157 }
00158
00160 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))) {
00161 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00162 setOpenMBVMoment(ombv.createOpenMBV());
00163 }
00164 void setOpenMBVMoment(OpenMBV::Arrow *arrow) {
00165 std::vector<bool> which; which.resize(2, false);
00166 which[1]=true;
00167 LinkMechanics::setOpenMBVMomentArrow(arrow, which);
00168 }
00169 #endif
00170
00171 protected:
00175 Frame *refFrame;
00176 int refFrameID;
00177
00181 fmatvec::Index IT, IR;
00182
00186 fmatvec::Mat3xV forceDir, momentDir;
00187
00191 fmatvec::Mat3xV Wf, Wm;
00192
00196 fmatvec::Mat3xV JT;
00197
00201 fmatvec::Vec3 WrP0P1, WvP0P1, WomP0P1;
00202
00206 GeneralizedForceLaw *ffl;
00207
00211 GeneralizedForceLaw * fml;
00212
00216 GeneralizedImpactLaw *fifl;
00217
00221 GeneralizedImpactLaw * fiml;
00222
00226 fmatvec::Vec gdn, gdd;
00227
00231 Frame C;
00232
00233 private:
00234 std::string saved_ref1, saved_ref2;
00235 };
00236
00237 class InverseKineticsJoint : public Joint {
00238 public:
00239 InverseKineticsJoint(const std::string &name);
00240 virtual void updateb(double t);
00241 void calcbSize();
00242 void setBody(RigidBody* body_) {
00243 body = body_;
00244 }
00245 virtual void init(InitStage stage);
00246 virtual bool isSetValued() const {
00247 return true;
00248 }
00249
00250 protected:
00251 RigidBody* body;
00252 };
00253
00254 }
00255
00256 #endif
00257