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 Body;
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 void calccorrSize(int j);
00083 virtual bool isSetValued() const;
00084 virtual bool isSingleValued() const;
00085 virtual bool isActive() const {
00086 return true;
00087 }
00088 virtual bool gActiveChanged() {
00089 return false;
00090 }
00091 virtual void solveImpactsFixpointSingle(double dt);
00092 virtual void solveConstraintsFixpointSingle();
00093 virtual void solveImpactsGaussSeidel(double dt);
00094 virtual void solveConstraintsGaussSeidel();
00095 virtual void solveImpactsRootFinding(double dt);
00096 virtual void solveConstraintsRootFinding();
00097 virtual void jacobianConstraints();
00098 virtual void jacobianImpacts();
00099 virtual void updaterFactors();
00100 virtual void checkImpactsForTermination(double dt);
00101 virtual void checkConstraintsForTermination();
00102
00103
00104
00105 virtual void plot(double t, double dt = 1);
00106
00107
00108
00113 virtual void connect(Frame *frame1, Frame* frame2);
00114
00115
00116
00117 void setForceLaw(GeneralizedForceLaw * rc);
00118 void setMomentLaw(GeneralizedForceLaw * rc);
00119
00120
00124 void setForceDirection(const fmatvec::Mat3xV& fd);
00125
00129 void setMomentDirection(const fmatvec::Mat3xV& md);
00130
00134 void setFrameOfReferenceID(int ID) {
00135 refFrameID = ID;
00136 }
00137
00138 virtual void initializeUsingXML(xercesc::DOMElement *element);
00139 virtual xercesc::DOMElement* writeXMLFile(xercesc::DOMNode *element);
00140
00141 virtual std::string getType() const {
00142 return "Joint";
00143 }
00144
00145 #ifdef HAVE_OPENMBVCPPINTERFACE
00146
00147 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))) {
00148 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toHead,referencePoint,scaleLength,scaleSize);
00149 setOpenMBVForce(ombv.createOpenMBV());
00150 }
00151 void setOpenMBVForce(const boost::shared_ptr<OpenMBV::Arrow> &arrow) {
00152 std::vector<bool> which; which.resize(2, false);
00153 which[1]=true;
00154 LinkMechanics::setOpenMBVForceArrow(arrow, which);
00155 }
00156
00158 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))) {
00159 OpenMBVArrow ombv(diffuseColor,transparency,OpenMBV::Arrow::toDoubleHead,referencePoint,scaleLength,scaleSize);
00160 setOpenMBVMoment(ombv.createOpenMBV());
00161 }
00162 void setOpenMBVMoment(const boost::shared_ptr<OpenMBV::Arrow> &arrow) {
00163 std::vector<bool> which; which.resize(2, false);
00164 which[1]=true;
00165 LinkMechanics::setOpenMBVMomentArrow(arrow, which);
00166 }
00167 #endif
00168
00169 protected:
00173 Frame *refFrame;
00174 int refFrameID;
00175
00179 fmatvec::Index IT, IR;
00180
00184 fmatvec::Mat3xV forceDir, momentDir;
00185
00189 fmatvec::Mat3xV Wf, Wm;
00190
00194 fmatvec::Mat3xV JT;
00195
00199 fmatvec::Vec3 WrP0P1, WvP0P1, WomP0P1;
00200
00204 GeneralizedForceLaw *ffl;
00205
00209 GeneralizedForceLaw * fml;
00210
00214 GeneralizedImpactLaw *fifl;
00215
00219 GeneralizedImpactLaw * fiml;
00220
00224 fmatvec::Vec gdn, gdd;
00225
00229 Frame C;
00230
00231 private:
00232 std::string saved_ref1, saved_ref2;
00233 };
00234
00235 class InverseKineticsJoint : public Joint {
00236 public:
00237 InverseKineticsJoint(const std::string &name);
00238 virtual void updateb(double t);
00239 void calcbSize();
00240 void setBody(Body* body_) {
00241 body = body_;
00242 }
00243 virtual void init(InitStage stage);
00244 virtual bool isSetValued() const {
00245 return true;
00246 }
00247
00248 protected:
00249 Body* body;
00250 };
00251
00252 }
00253
00254 #endif
00255