00001 #ifndef SimTK_SIMBODY_MOBILIZED_BODY_H_
00002 #define SimTK_SIMBODY_MOBILIZED_BODY_H_
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00046 #include "SimTKmath.h"
00047 #include "simbody/internal/common.h"
00048 #include "simbody/internal/Body.h"
00049
00050 #include <cassert>
00051 #include <vector>
00052
00053 namespace SimTK {
00054
00055 class SimbodyMatterSubsystem;
00056 class MobilizedBody;
00057 class MobilizedBodyImpl;
00058
00059
00060
00061 #ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
00062 extern template class PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true>;
00063 #endif
00064
00112 class SimTK_SIMBODY_EXPORT MobilizedBody : public PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true> {
00113 public:
00114
00120 enum Direction {
00121 Forward = 0,
00122 Reverse = 1
00123 };
00124
00126
00128
00132
00133
00146 const Transform& getBodyTransform(const State&) const;
00147
00156 const Rotation& getBodyRotation(const State& s) const {
00157 return getBodyTransform(s).R();
00158 }
00163 const Vec3& getBodyOriginLocation(const State& s) const {
00164 return getBodyTransform(s).p();
00165 }
00166
00170 const Transform& getMobilizerTransform(const State&) const;
00171
00177 const SpatialVec& getBodyVelocity(const State&) const;
00178
00182 const Vec3& getBodyAngularVelocity(const State& s) const {
00183 return getBodyVelocity(s)[0];
00184 }
00190 const Vec3& getBodyOriginVelocity(const State& s) const {
00191 return getBodyVelocity(s)[1];
00192 }
00193
00199 const SpatialVec& getMobilizerVelocity(const State&) const;
00200
00206 const SpatialVec& getBodyAcceleration(const State& s) const;
00207
00211 const Vec3& getBodyAngularAcceleration(const State& s) const {
00212 return getBodyAcceleration(s)[0];
00213 }
00214
00219 const Vec3& getBodyOriginAcceleration(const State& s) const {
00220 return getBodyAcceleration(s)[1];
00221 }
00222
00228 const SpatialVec& getMobilizerAcceleration(const State&) const {
00229 SimTK_ASSERT_ALWAYS(!"unimplemented method",
00230 "MobilizedBody::getMobilizerAcceleration() is not yet implemented -- any volunteers?");
00231 return *(new SpatialVec());
00232 }
00233
00236 const MassProperties& getBodyMassProperties(const State&) const;
00237
00239 Real getBodyMass(const State& s) const {
00240 return getBodyMassProperties(s).getMass();
00241 }
00242
00246 const Vec3& getBodyMassCenterStation(const State& s) const {
00247 return getBodyMassProperties(s).getMassCenter();
00248 }
00249
00253 const Inertia& getBodyInertiaAboutBodyOrigin(const State& s) const {
00254 return getBodyMassProperties(s).getInertia();
00255 }
00256
00261 const Transform& getInboardFrame (const State&) const;
00266 const Transform& getOutboardFrame(const State&) const;
00267
00270 void setInboardFrame (State&, const Transform& X_PF) const;
00273 void setOutboardFrame(State&, const Transform& X_BM) const;
00274
00275
00277
00281
00282
00283
00284 int getNumQ(const State&) const;
00287 int getNumU(const State&) const;
00288
00292 Real getOneQ(const State&, int which) const;
00293
00297 Real getOneU(const State&, int which) const;
00298
00302 Vector getQAsVector(const State&) const;
00306 Vector getUAsVector(const State&) const;
00307
00311 Real getOneQDot (const State&, int which) const;
00315 Vector getQDotAsVector(const State&) const;
00316
00320 Real getOneUDot (const State&, int which) const;
00324 Real getOneQDotDot(const State&, int which) const;
00328 Vector getUDotAsVector (const State&) const;
00332 Vector getQDotDotAsVector(const State&) const;
00333
00337 void setOneQ(State&, int which, Real v) const;
00341 void setOneU(State&, int which, Real v) const;
00342
00345 void setQFromVector(State& s, const Vector& v) const;
00348 void setUFromVector(State& s, const Vector& v) const;
00349
00385
00386 void setQToFitTransform (State&, const Transform& X_FM) const;
00391 void setQToFitRotation (State&, const Rotation& R_FM) const;
00397 void setQToFitTranslation (State&, const Vec3& p_FM) const;
00398
00405 void setUToFitVelocity (State&, const SpatialVec& V_FM) const;
00412 void setUToFitAngularVelocity (State&, const Vec3& w_FM) const;
00419 void setUToFitLinearVelocity (State&, const Vec3& v_FM) const;
00420
00423 SpatialVec getHCol(const State& s, UIndex ux) const;
00424
00425
00427
00429
00431
00433
00456
00458
00464 Transform findBodyTransformInAnotherBody(const State& s,
00465 const MobilizedBody& inBodyA) const
00466 {
00467 const Transform& X_GA = inBodyA.getBodyTransform(s);
00468 const Transform& X_GB = this->getBodyTransform(s);
00469
00470 return ~X_GA*X_GB;
00471 }
00472
00478 Rotation findBodyRotationInAnotherBody(const State& s,
00479 const MobilizedBody& inBodyA) const
00480 {
00481 const Rotation& R_GA = inBodyA.getBodyRotation(s);
00482 const Rotation& R_GB = this->getBodyRotation(s);
00483
00484 return ~R_GA*R_GB;
00485 }
00486
00492 Vec3 findBodyOriginLocationInAnotherBody(const State& s, const MobilizedBody& toBodyA) const {
00493 return toBodyA.findStationAtGroundPoint(s,getBodyOriginLocation(s));
00494 }
00495
00501 SpatialVec findBodyVelocityInAnotherBody(const State& s,
00502 const MobilizedBody& inBodyA) const
00503 {
00504 const SpatialVec& V_GB = this->getBodyVelocity(s);
00505 const SpatialVec& V_GA = inBodyA.getBodyVelocity(s);
00506 const Vec3 w_AB_G = V_GB[0]-V_GA[0];
00507
00508
00509 const Transform& X_GB = getBodyTransform(s);
00510 const Transform& X_GA = inBodyA.getBodyTransform(s);
00511 const Vec3 p_AB_G = X_GB.p() - X_GA.p();
00512 const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1];
00513
00514 const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G;
00515
00516
00517 return ~X_GA.R()*SpatialVec(w_AB_G, v_AB_G);
00518 }
00519
00524 Vec3 findBodyAngularVelocityInAnotherBody(const State& s,
00525 const MobilizedBody& inBodyA) const
00526 {
00527 const Vec3& w_GB = getBodyAngularVelocity(s);
00528 const Vec3& w_GA = inBodyA.getBodyAngularVelocity(s);
00529 const Vec3 w_AB_G = w_GB-w_GA;
00530
00531
00532 return inBodyA.expressGroundVectorInBodyFrame(s, w_AB_G);
00533 }
00534
00539 Vec3 findBodyOriginVelocityInAnotherBody(const State& s,
00540 const MobilizedBody& inBodyA) const
00541 {
00542
00543 return findBodyVelocityInAnotherBody(s,inBodyA)[1];
00544 }
00545
00550 SpatialVec findBodyAccelerationInAnotherBody(const State& s,
00551 const MobilizedBody& inBodyA) const
00552 {
00553 const Vec3& p_GB = this->getBodyOriginLocation(s);
00554 const Transform& X_GA = inBodyA.getBodyTransform(s);
00555 const SpatialVec& V_GB = this->getBodyVelocity(s);
00556 const SpatialVec& V_GA = inBodyA.getBodyVelocity(s);
00557 const SpatialVec& A_GB = this->getBodyAcceleration(s);
00558 const SpatialVec& A_GA = inBodyA.getBodyAcceleration(s);
00559 const Vec3& p_GA = X_GA.p();
00560 const Vec3& w_GA = V_GA[0];
00561 const Vec3& w_GB = V_GB[0];
00562 const Vec3& b_GA = A_GA[0];
00563 const Vec3& b_GB = A_GB[0];
00564
00565 const Vec3 p_AB_G = p_GB - p_GA;
00566 const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1];
00567 const Vec3 p_AB_G_dotdot = A_GB[1] - A_GA[1];
00568
00569 const Vec3 w_AB_G = w_GB - w_GA;
00570 const Vec3 v_AB_G = p_AB_G_dot - w_GA % p_AB_G;
00571
00572 const Vec3 w_AB_G_dot = b_GB - b_GA;
00573 const Vec3 v_AB_G_dot = p_AB_G_dotdot - (b_GA % p_AB_G + w_GA % p_AB_G_dot);
00574
00575
00576
00577
00578 const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G;
00579 const Vec3 a_AB_G = v_AB_G_dot - w_GA % v_AB_G;
00580
00581 return ~X_GA.R() * SpatialVec(b_AB_G, a_AB_G);
00582 }
00583
00588 Vec3 findBodyAngularAccelerationInAnotherBody(const State& s,
00589 const MobilizedBody& inBodyA) const
00590 {
00591 const Rotation& R_GA = inBodyA.getBodyRotation(s);
00592 const Vec3& w_GA = inBodyA.getBodyAngularVelocity(s);
00593 const Vec3& w_GB = this->getBodyAngularVelocity(s);
00594 const Vec3& b_GA = inBodyA.getBodyAngularAcceleration(s);
00595 const Vec3& b_GB = this->getBodyAngularAcceleration(s);
00596
00597 const Vec3 w_AB_G = w_GB - w_GA;
00598 const Vec3 w_AB_G_dot = b_GB - b_GA;
00599
00600
00601
00602 const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G;
00603
00604 return ~R_GA * b_AB_G;
00605 }
00606
00611 Vec3 findBodyOriginAccelerationInAnotherBody(const State& s,
00612 const MobilizedBody& inBodyA) const
00613 {
00614
00615
00616 return findBodyAccelerationInAnotherBody(s,inBodyA)[1];
00617 }
00618
00624 Vec3 findStationLocationInGround(const State& s, const Vec3& stationOnB) const {
00625 return getBodyTransform(s) * stationOnB;
00626 }
00627
00628
00637 Vec3 findStationLocationInAnotherBody(const State& s, const Vec3& stationOnB,
00638 const MobilizedBody& toBodyA) const
00639 {
00640 return toBodyA.findStationAtGroundPoint(s, findStationLocationInGround(s,stationOnB));
00641 }
00642
00646 Vec3 findStationVelocityInGround(const State& s, const Vec3& stationOnB) const {
00647 const Vec3& w = getBodyAngularVelocity(s);
00648 const Vec3& v = getBodyOriginVelocity(s);
00649 const Vec3 r = expressVectorInGroundFrame(s,stationOnB);
00650 return v + w % r;
00651 }
00652
00653
00658 Vec3 findStationVelocityInAnotherBody(const State& s,
00659 const Vec3& stationOnBodyB,
00660 const MobilizedBody& inBodyA) const
00661 {
00662 const SpatialVec V_AB = findBodyVelocityInAnotherBody(s,inBodyA);
00663
00664 const Vec3 p_BS_A = expressVectorInAnotherBodyFrame(s, stationOnBodyB, inBodyA);
00665
00666 return V_AB[1] + (V_AB[0] % p_BS_A);
00667 }
00668
00669
00673 Vec3 findStationAccelerationInGround(const State& s, const Vec3& stationOnB) const {
00674 const Vec3& w = getBodyAngularVelocity(s);
00675 const Vec3& b = getBodyAngularAcceleration(s);
00676 const Vec3& a = getBodyOriginAcceleration(s);
00677
00678 const Vec3 r = expressVectorInGroundFrame(s,stationOnB);
00679 return a + b % r + w % (w % r);
00680 }
00681
00686 Vec3 findStationAccelerationInAnotherBody(const State& s,
00687 const Vec3& stationOnBodyB,
00688 const MobilizedBody& inBodyA) const
00689 {
00690 const Vec3 w_AB = findBodyAngularVelocityInAnotherBody(s,inBodyA);
00691 const SpatialVec A_AB = findBodyAccelerationInAnotherBody(s,inBodyA);
00692
00693 const Vec3 p_BS_A = expressVectorInAnotherBodyFrame(s, stationOnBodyB, inBodyA);
00694
00695
00696 return A_AB[1] + (A_AB[0] % p_BS_A) + w_AB % (w_AB % p_BS_A);
00697 }
00698
00702 void findStationLocationAndVelocityInGround(const State& s, const Vec3& locationOnB,
00703 Vec3& locationOnGround, Vec3& velocityInGround) const
00704 {
00705 const Vec3& p_GB = getBodyOriginLocation(s);
00706 const Vec3 p_BS_G = expressVectorInGroundFrame(s,locationOnB);
00707 locationOnGround = p_GB + p_BS_G;
00708
00709 const Vec3& w_GB = getBodyAngularVelocity(s);
00710 const Vec3& v_GB = getBodyOriginVelocity(s);
00711 velocityInGround = v_GB + w_GB % p_BS_G;
00712 }
00713
00714
00718 void findStationLocationVelocityAndAccelerationInGround
00719 (const State& s, const Vec3& locationOnB,
00720 Vec3& locationOnGround, Vec3& velocityInGround, Vec3& accelerationInGround) const
00721 {
00722 const Rotation& R_GB = getBodyRotation(s);
00723 const Vec3& p_GB = getBodyOriginLocation(s);
00724
00725 const Vec3 r = R_GB*locationOnB;
00726 locationOnGround = p_GB + r;
00727
00728 const Vec3& w = getBodyAngularVelocity(s);
00729 const Vec3& v = getBodyOriginVelocity(s);
00730 const Vec3& b = getBodyAngularAcceleration(s);
00731 const Vec3& a = getBodyOriginAcceleration(s);
00732
00733 const Vec3 wXr = w % r;
00734 velocityInGround = v + wXr;
00735 accelerationInGround = a + b % r + w % wXr;
00736 }
00737
00740 Vec3 findMassCenterLocationInGround(const State& s) const {
00741 return findStationLocationInGround(s,getBodyMassCenterStation(s));
00742 }
00743
00747 Vec3 findMassCenterLocationInAnotherBody(const State& s, const MobilizedBody& toBodyA) const {
00748 return findStationLocationInAnotherBody(s,getBodyMassCenterStation(s),toBodyA);
00749 }
00750
00756 Vec3 findStationAtGroundPoint(const State& s, const Vec3& locationInG) const {
00757 return ~getBodyTransform(s) * locationInG;
00758 }
00759
00765 Vec3 findStationAtAnotherBodyStation(const State& s, const MobilizedBody& fromBodyA,
00766 const Vec3& stationOnA) const {
00767 return fromBodyA.findStationLocationInAnotherBody(s, stationOnA, *this);
00768 }
00769
00773 Vec3 findStationAtAnotherBodyOrigin(const State& s, const MobilizedBody& fromBodyA) const {
00774 return findStationAtGroundPoint(s,fromBodyA.getBodyOriginLocation(s));
00775 }
00776
00780 Vec3 findStationAtAnotherBodyMassCenter(const State& s, const MobilizedBody& fromBodyA) const {
00781 return fromBodyA.findStationLocationInAnotherBody(s,getBodyMassCenterStation(s),*this);
00782 }
00783
00787 Vec3 expressVectorInGroundFrame(const State& s, const Vec3& vectorInB) const {
00788 return getBodyRotation(s)*vectorInB;
00789 }
00790
00794 Vec3 expressGroundVectorInBodyFrame(const State& s, const Vec3& vectorInG) const {
00795 return ~getBodyRotation(s)*vectorInG;
00796 }
00797
00803 Vec3 expressVectorInAnotherBodyFrame(const State& s, const Vec3& vectorInB,
00804 const MobilizedBody& inBodyA) const
00805 {
00806 return inBodyA.expressGroundVectorInBodyFrame(s, expressVectorInGroundFrame(s,vectorInB));
00807 }
00808
00812 MassProperties expressMassPropertiesInGroundFrame(const State& s) {
00813 const MassProperties& M_OB_B = getBodyMassProperties(s);
00814 const Rotation& R_GB = getBodyRotation(s);
00815 return M_OB_B.reexpress(~R_GB);
00816 }
00817
00821 MassProperties expressMassPropertiesInAnotherBodyFrame(const State& s, const MobilizedBody& inBodyA) {
00822 const MassProperties& M_OB_B = getBodyMassProperties(s);
00823 const Rotation R_AB = findBodyRotationInAnotherBody(s,inBodyA);
00824 return M_OB_B.reexpress(~R_AB);
00825 }
00826
00827
00829
00834
00835
00837
00857 SpatialMat calcBodySpatialInertiaMatrixInGround(const State& s) const
00858 {
00859 if (isGround())
00860 return SpatialMat(Mat33(Infinity));
00861
00862 const MassProperties& mp = getBodyMassProperties(s);
00863 const Rotation& R_GB = getBodyRotation(s);
00864
00865 return mp.reexpress(~R_GB).toSpatialMat();
00866 }
00867
00873 Inertia calcBodyCentralInertia(const State& s,
00874 MobilizedBodyIndex objectBodyB) const
00875 {
00876 return getBodyMassProperties(s).calcCentralInertia();
00877 }
00878
00882 Inertia calcBodyInertiaAboutAnotherBodyStation(const State& s,
00883 const MobilizedBody& inBodyA,
00884 const Vec3& aboutLocationOnBodyA) const
00885 {
00886
00887 const MassProperties& MB_OB_B = getBodyMassProperties(s);
00888
00889
00890
00891 const Vec3 r_OB_PA = findStationAtAnotherBodyStation(s, inBodyA, aboutLocationOnBodyA);
00892
00893
00894 const Inertia IB_PA_B = MB_OB_B.calcShiftedInertia(r_OB_PA);
00895
00896
00897 const Rotation R_BA = inBodyA.findBodyRotationInAnotherBody(s, *this);
00898 const Inertia IB_PA_A = IB_PA_B.reexpress(R_BA);
00899 return IB_PA_A;
00900 }
00901
00902
00905 SpatialVec calcBodyMomentumAboutBodyOriginInGround(const State& s) {
00906 const MassProperties M_OB_G = expressMassPropertiesInGroundFrame(s);
00907 const SpatialVec& V_GB = getBodyVelocity(s);
00908 return M_OB_G.toSpatialMat() * V_GB;
00909 }
00910
00913 SpatialVec calcBodyMomentumAboutBodyMassCenterInGround(const State& s) const {
00914 const MassProperties& M_OB_B = getBodyMassProperties(s);
00915 const Rotation& R_GB = getBodyRotation(s);
00916
00917
00918
00919 const Inertia I_CB_B = M_OB_B.calcCentralInertia();
00920 const Inertia I_CB_G = I_CB_B.reexpress(~R_GB);
00921 const Real mb = M_OB_B.getMass();
00922 const Vec3& w_GB = getBodyAngularVelocity(s);
00923 Vec3 v_G_CB = findStationVelocityInGround(s, M_OB_B.getMassCenter());
00924
00925 return SpatialVec( I_CB_G*w_GB, mb*v_G_CB );
00926 }
00927
00931 Real calcStationToStationDistance(const State& s,
00932 const Vec3& locationOnBodyB,
00933 const MobilizedBody& bodyA,
00934 const Vec3& locationOnBodyA) const
00935 {
00936 if (isSameMobilizedBody(bodyA))
00937 return (locationOnBodyA-locationOnBodyB).norm();
00938
00939 const Vec3 r_OG_PB = this->findStationLocationInGround(s,locationOnBodyB);
00940 const Vec3 r_OG_PA = bodyA.findStationLocationInGround(s,locationOnBodyA);
00941 return (r_OG_PA - r_OG_PB).norm();
00942 }
00943
00948 Real calcStationToStationDistanceTimeDerivative(const State& s,
00949 const Vec3& locationOnBodyB,
00950 const MobilizedBody& bodyA,
00951 const Vec3& locationOnBodyA) const
00952 {
00953 if (isSameMobilizedBody(bodyA))
00954 return 0;
00955
00956 Vec3 rB, rA, vB, vA;
00957 this->findStationLocationAndVelocityInGround(s,locationOnBodyB,rB,vB);
00958 bodyA.findStationLocationAndVelocityInGround(s,locationOnBodyA,rA,vA);
00959 const Vec3 r = rA-rB, v = vA-vB;
00960 const Real d = r.norm();
00961
00962
00963
00964 if (d==0) return v.norm();
00965 else return dot(v, r/d);
00966 }
00967
00968
00973 Real calcStationToStationDistance2ndTimeDerivative(const State& s,
00974 const Vec3& locationOnBodyB,
00975 const MobilizedBody& bodyA,
00976 const Vec3& locationOnBodyA) const
00977 {
00978 if (isSameMobilizedBody(bodyA))
00979 return 0;
00980
00981 Vec3 rB, rA, vB, vA, aB, aA;
00982 this->findStationLocationVelocityAndAccelerationInGround(s,locationOnBodyB,rB,vB,aB);
00983 bodyA.findStationLocationVelocityAndAccelerationInGround(s,locationOnBodyA,rA,vA,aA);
00984
00985 const Vec3 r = rA-rB, v = vA-vB, a = aA-aB;
00986 const Real d = r.norm();
00987
00988
00989
00990
00991
00992
00993 if (d==0) {
00994
00995
00996
00997 const Real s = v.norm();
00998 if (s==0) return a.norm();
00999 else return dot(a, v/s);
01000 }
01001
01002
01003 const Vec3 u = r/d;
01004 const Vec3 vp = v - dot(v,u)*u;
01005 return dot(a,u) + dot(vp,v)/d;
01006 }
01007
01008
01011 Vec3 calcBodyMovingPointVelocityInBody(const State& s,
01012 const Vec3& locationOnBodyB,
01013 const Vec3& velocityOnBodyB,
01014 const MobilizedBody& inBodyA) const
01015 {
01016 SimTK_ASSERT_ALWAYS(!"unimplemented method",
01017 "MobilizedBody::calcBodyMovingPointVelocityInBody() is not yet implemented -- any volunteers?");
01018 return Vec3::getNaN();
01019 }
01020
01021
01024 Vec3 calcBodyMovingPointAccelerationInBody(const State& s,
01025 const Vec3& locationOnBodyB,
01026 const Vec3& velocityOnBodyB,
01027 const Vec3& accelerationOnBodyB,
01028 const MobilizedBody& inBodyA) const
01029 {
01030 SimTK_ASSERT_ALWAYS(!"unimplemented method",
01031 "MobilizedBody::calcBodyMovingPointAccelerationInBody() is not yet implemented -- any volunteers?");
01032 return Vec3::getNaN();
01033 }
01034
01041 Real calcMovingPointToPointDistanceTimeDerivative(const State& s,
01042 const Vec3& locationOnBodyB,
01043 const Vec3& velocityOnBodyB,
01044 const MobilizedBody& bodyA,
01045 const Vec3& locationOnBodyA,
01046 const Vec3& velocityOnBodyA) const
01047 {
01048 SimTK_ASSERT_ALWAYS(!"unimplemented method",
01049 "MobilizedBody::calcMovingPointToPointDistanceTimeDerivative() is not yet implemented -- any volunteers?");
01050 return NaN;
01051 }
01052
01059 Real calcMovingPointToPointDistance2ndTimeDerivative(const State& s,
01060 const Vec3& locationOnBodyB,
01061 const Vec3& velocityOnBodyB,
01062 const Vec3& accelerationOnBodyB,
01063 const MobilizedBody& bodyA,
01064 const Vec3& locationOnBodyA,
01065 const Vec3& velocityOnBodyA,
01066 const Vec3& accelerationOnBodyA) const
01067 {
01068 SimTK_ASSERT_ALWAYS(!"unimplemented method",
01069 "MobilizedBody::calcMovingPointToPointDistance2ndTimeDerivative() is not yet implemented -- any volunteers?");
01070 return NaN;
01071 }
01072
01073
01074
01076
01077
01079
01081
01085 MobilizedBody();
01086
01088 explicit MobilizedBody(MobilizedBodyImpl* r);
01089
01094
01095
01101 MobilizedBody& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01102 (void)updBody().addDecoration(X_BD,g);
01103 return *this;
01104 }
01105
01109 MobilizedBody& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry&);
01110
01114 MobilizedBody& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry&);
01115
01117 const Body& getBody() const;
01121 Body& updBody();
01122
01128 MobilizedBody& setBody(const Body&);
01129
01136 MobilizedBody& setDefaultMassProperties(const MassProperties& m) {
01137 updBody().setDefaultRigidBodyMassProperties(m);
01138 return *this;
01139 }
01140
01142 const MassProperties& getDefaultMassProperties() const {
01143 return getBody().getDefaultRigidBodyMassProperties();
01144 }
01145
01151 MobilizedBody& setDefaultInboardFrame (const Transform& X_PF);
01157 MobilizedBody& setDefaultOutboardFrame(const Transform& X_BM);
01158
01163 const Transform& getDefaultInboardFrame() const;
01167 const Transform& getDefaultOutboardFrame() const;
01168
01171 operator MobilizedBodyIndex() const {return getMobilizedBodyIndex();}
01172
01175 MobilizedBodyIndex getMobilizedBodyIndex() const;
01176
01179 const MobilizedBody& getParentMobilizedBody() const;
01180
01184 const MobilizedBody& getBaseMobilizedBody() const;
01185
01188 const SimbodyMatterSubsystem& getMatterSubsystem() const;
01191 SimbodyMatterSubsystem& updMatterSubsystem();
01192
01194 bool isInSubsystem() const;
01195
01199 bool isInSameSubsystem(const MobilizedBody&) const;
01200
01205 bool isSameMobilizedBody(const MobilizedBody& mBody) const;
01206
01209 bool isGround() const;
01210
01215 int getLevelInMultibodyTree() const;
01216
01219 MobilizedBody& cloneForNewParent(MobilizedBody& parent) const;
01220
01221
01222
01226 Real getOneFromQPartition(const State&, int which, const Vector& qlike) const;
01227
01232 Real& updOneFromQPartition(const State&, int which, Vector& qlike) const;
01233
01237 Real getOneFromUPartition(const State&, int which, const Vector& ulike) const;
01238
01243 Real& updOneFromUPartition(const State&, int which, Vector& ulike) const;
01244
01250 void applyOneMobilityForce(const State& s, int which, Real f,
01251 Vector& mobilityForces) const
01252 {
01253 updOneFromUPartition(s,which,mobilityForces) += f;
01254 }
01255
01262 void applyBodyForce(const State& s, const SpatialVec& spatialForceInG,
01263 Vector_<SpatialVec>& bodyForcesInG) const;
01264
01270 void applyBodyTorque(const State& s, const Vec3& torqueInG,
01271 Vector_<SpatialVec>& bodyForcesInG) const;
01272
01282 void applyForceToBodyPoint(const State& s, const Vec3& pointInB, const Vec3& forceInG,
01283 Vector_<SpatialVec>& bodyForcesInG) const;
01284
01285
01287
01289
01291
01292
01293
01294
01295
01296
01297
01298
01299
01300
01301
01302
01303
01304
01305
01306
01307
01308
01309
01310
01311
01312
01313
01314
01315
01316
01317
01318
01319 class Pin; typedef Pin Torsion;
01320 class Universal;
01321 class Cylinder;
01322
01323 class Weld;
01324 class Slider; typedef Slider Prismatic;
01325 class Translation2D; typedef Translation2D Cartesian2D, CartesianCoords2D;
01326 class Translation; typedef Translation Cartesian, CartesianCoords;
01327 class BendStretch; typedef BendStretch PolarCoords;
01328 class TorsionStretch; typedef TorsionStretch ConicalCoords2D;
01329 class SphericalCoords;
01330 class CylindricalCoords;
01331 class LineOrientation;
01332
01333
01334 class Planar;
01335 class Gimbal;
01336 class Ball; typedef Ball Orientation, Spherical;
01337 class Free;
01338 class FreeLine;
01339 class Screw;
01340 class Ellipsoid;
01341 class Custom;
01342 class Ground;
01343 class FunctionBased;
01344
01345 class PinImpl;
01346 class SliderImpl;
01347 class UniversalImpl;
01348 class CylinderImpl;
01349 class BendStretchImpl;
01350 class TorsionStretchImpl;
01351 class PlanarImpl;
01352 class GimbalImpl;
01353 class BallImpl;
01354 class TranslationImpl;
01355 class SphericalCoordsImpl;
01356 class FreeImpl;
01357 class LineOrientationImpl;
01358 class FreeLineImpl;
01359 class WeldImpl;
01360 class ScrewImpl;
01361 class EllipsoidImpl;
01362 class CustomImpl;
01363 class GroundImpl;
01364 class FunctionBasedImpl;
01365
01366 };
01367
01371 class SimTK_SIMBODY_EXPORT MobilizedBody::Pin : public MobilizedBody {
01372 public:
01373
01374
01375
01376 Pin& setDefaultAngle(Real angleInRadians) {return setDefaultQ(angleInRadians);}
01377 Real getDefaultAngle() const {return getDefaultQ();}
01378
01379
01380
01381 void setAngle(State& s, Real angleInRadians) {setQ(s, angleInRadians);}
01382 Real getAngle(const State& s) const {return getQ(s);}
01383
01384 void setRate(State& s, Real rateInRadiansPerTime) {setU(s, rateInRadiansPerTime);}
01385 Real getRate(const State& s) const {return getU(s);}
01386
01387
01388 Real getAppliedPinTorque(const State& s, const Vector& mobilityForces) const {
01389 return getMyPartU(s,mobilityForces);
01390 }
01391 void applyPinTorque(const State& s, Real torque, Vector& mobilityForces) const {
01392 updMyPartU(s,mobilityForces) += torque;
01393 }
01394
01395
01396
01397
01398 explicit Pin(Direction=Forward);
01399 Pin(MobilizedBody& parent, const Body&, Direction=Forward);
01400 Pin(MobilizedBody& parent, const Transform& inbFrame,
01401 const Body&, const Transform& outbFrame,
01402 Direction=Forward);
01403
01404
01405 Pin& setDefaultQ(Real);
01406 Real getDefaultQ() const;
01407
01408 Real getQ(const State&) const;
01409 Real getQDot(const State&) const;
01410 Real getQDotDot(const State&) const;
01411 Real getU(const State&) const;
01412 Real getUDot(const State&) const;
01413
01414 void setQ(State&, Real) const;
01415 void setU(State&, Real) const;
01416
01417 Real getMyPartQ(const State&, const Vector& qlike) const;
01418 Real getMyPartU(const State&, const Vector& ulike) const;
01419
01420 Real& updMyPartQ(const State&, Vector& qlike) const;
01421 Real& updMyPartU(const State&, Vector& ulike) const;
01422
01423
01424 Pin& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
01425 { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
01426 Pin& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g)
01427 { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
01428 Pin& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
01429 { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
01430 Pin& setDefaultInboardFrame(const Transform& X_PF)
01431 { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
01432 Pin& setDefaultOutboardFrame(const Transform& X_BM)
01433 { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
01434 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Pin, PinImpl, MobilizedBody);
01435 };
01436
01440 class SimTK_SIMBODY_EXPORT MobilizedBody::Slider : public MobilizedBody {
01441 public:
01442
01443
01444
01445 Slider& setDefaultLength(Real length) {return setDefaultQ(length);}
01446 Real getDefaultLength() const {return getDefaultQ();}
01447
01448
01449
01450 void setLength(State& s, Real length) {setQ(s, length);}
01451 Real getLength(const State& s) const {return getQ(s);}
01452
01453 void setRate(State& s, Real rateInLengthPerTime) {setU(s, rateInLengthPerTime);}
01454 Real getRate(const State& s) const {return getU(s);}
01455
01456
01457 Real getAppliedForce(const State& s, const Vector& mobilityForces) const {
01458 return getMyPartU(s,mobilityForces);
01459 }
01460 void applyForce(const State& s, Real force, Vector& mobilityForces) const {
01461 updMyPartU(s,mobilityForces) += force;
01462 }
01463
01464
01465
01466
01467 explicit Slider(Direction=Forward);
01468 Slider(MobilizedBody& parent, const Body&, Direction=Forward);
01469 Slider(MobilizedBody& parent, const Transform& inbFrame,
01470 const Body&, const Transform& outbFrame, Direction=Forward);
01471
01472
01473 Slider& setDefaultQ(Real);
01474 Real getDefaultQ() const;
01475
01476 Real getQ(const State&) const;
01477 Real getQDot(const State&) const;
01478 Real getQDotDot(const State&) const;
01479 Real getU(const State&) const;
01480 Real getUDot(const State&) const;
01481
01482 void setQ(State&, Real) const;
01483 void setU(State&, Real) const;
01484
01485 Real getMyPartQ(const State&, const Vector& qlike) const;
01486 Real getMyPartU(const State&, const Vector& ulike) const;
01487
01488 Real& updMyPartQ(const State&, Vector& qlike) const;
01489 Real& updMyPartU(const State&, Vector& ulike) const;
01490
01491
01492 Slider& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g)
01493 { (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this; }
01494 Slider& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g)
01495 { (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this; }
01496 Slider& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g)
01497 { (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this; }
01498 Slider& setDefaultInboardFrame(const Transform& X_PF)
01499 { (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this; }
01500 Slider& setDefaultOutboardFrame(const Transform& X_BM)
01501 { (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this; }
01502 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Slider, SliderImpl, MobilizedBody);
01503 };
01504
01510 class SimTK_SIMBODY_EXPORT MobilizedBody::Screw : public MobilizedBody {
01511 public:
01512 explicit Screw(Real pitch, Direction=Forward);
01513
01516 Screw(MobilizedBody& parent, const Body&, Real pitch, Direction=Forward);
01517
01520 Screw(MobilizedBody& parent, const Transform& inbFrame,
01521 const Body&, const Transform& outbFrame,
01522 Real pitch, Direction=Forward);
01523
01524 Screw& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01525 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01526 }
01527 Screw& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
01528 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01529 }
01530 Screw& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01531 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01532 }
01533
01534 Screw& setDefaultInboardFrame(const Transform& X_PF) {
01535 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01536 }
01537
01538 Screw& setDefaultOutboardFrame(const Transform& X_BM) {
01539 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01540 }
01541
01542 Screw& setDefaultPitch(Real pitch);
01543 Real getDefaultPitch() const;
01544
01545 Screw& setDefaultQ(Real);
01546 Real getDefaultQ() const;
01547
01548 Real getQ(const State&) const;
01549 Real getQDot(const State&) const;
01550 Real getQDotDot(const State&) const;
01551 Real getU(const State&) const;
01552 Real getUDot(const State&) const;
01553
01554 void setQ(State&, Real) const;
01555 void setU(State&, Real) const;
01556
01557 Real getMyPartQ(const State&, const Vector& qlike) const;
01558 Real getMyPartU(const State&, const Vector& ulike) const;
01559
01560 Real& updMyPartQ(const State&, Vector& qlike) const;
01561 Real& updMyPartU(const State&, Vector& ulike) const;
01562 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Screw, ScrewImpl, MobilizedBody);
01563 };
01564
01568 class SimTK_SIMBODY_EXPORT MobilizedBody::Universal : public MobilizedBody {
01569 public:
01570 explicit Universal(Direction=Forward);
01571
01574 Universal(MobilizedBody& parent, const Body&, Direction=Forward);
01575
01578 Universal(MobilizedBody& parent, const Transform& inbFrame,
01579 const Body&, const Transform& outbFrame, Direction=Forward);
01580
01581 Universal& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01582 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01583 }
01584 Universal& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
01585 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01586 }
01587 Universal& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01588 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01589 }
01590
01591 Universal& setDefaultInboardFrame(const Transform& X_PF) {
01592 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01593 }
01594
01595 Universal& setDefaultOutboardFrame(const Transform& X_BM) {
01596 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01597 }
01598 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Universal, UniversalImpl, MobilizedBody);
01599 };
01600
01603 class SimTK_SIMBODY_EXPORT MobilizedBody::Cylinder : public MobilizedBody {
01604 public:
01605 explicit Cylinder(Direction=Forward);
01606
01609 Cylinder(MobilizedBody& parent, const Body&, Direction=Forward);
01610
01613 Cylinder(MobilizedBody& parent, const Transform& inbFrame,
01614 const Body&, const Transform& outbFrame, Direction=Forward);
01615
01616 Cylinder& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01617 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01618 }
01619 Cylinder& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
01620 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01621 }
01622 Cylinder& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01623 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01624 }
01625
01626 Cylinder& setDefaultInboardFrame(const Transform& X_PF) {
01627 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01628 }
01629
01630 Cylinder& setDefaultOutboardFrame(const Transform& X_BM) {
01631 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01632 }
01633 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Cylinder, CylinderImpl, MobilizedBody);
01634 };
01635
01644 class SimTK_SIMBODY_EXPORT MobilizedBody::BendStretch : public MobilizedBody {
01645 public:
01646 explicit BendStretch(Direction=Forward);
01647
01650 BendStretch(MobilizedBody& parent, const Body&, Direction=Forward);
01651
01654 BendStretch(MobilizedBody& parent, const Transform& inbFrame,
01655 const Body&, const Transform& outbFrame, Direction=Forward);
01656
01657 BendStretch& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01658 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01659 }
01660 BendStretch& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
01661 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01662 }
01663 BendStretch& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01664 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01665 }
01666
01667 BendStretch& setDefaultInboardFrame(const Transform& X_PF) {
01668 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01669 }
01670
01671 BendStretch& setDefaultOutboardFrame(const Transform& X_BM) {
01672 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01673 }
01674 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(BendStretch, BendStretchImpl, MobilizedBody);
01675 };
01676
01681 class SimTK_SIMBODY_EXPORT MobilizedBody::Planar : public MobilizedBody {
01682 public:
01683 explicit Planar(Direction=Forward);
01684
01687 Planar(MobilizedBody& parent, const Body&, Direction=Forward);
01688
01691 Planar(MobilizedBody& parent, const Transform& inbFrame,
01692 const Body&, const Transform& outbFrame,
01693 Direction=Forward);
01694
01695 Planar& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01696 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01697 }
01698 Planar& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
01699 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01700 }
01701 Planar& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01702 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01703 }
01704
01705 Planar& setDefaultInboardFrame(const Transform& X_PF) {
01706 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01707 }
01708
01709 Planar& setDefaultOutboardFrame(const Transform& X_BM) {
01710 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01711 }
01712
01713
01714 Planar& setDefaultAngle(Real a) {
01715 Vec3 q = getDefaultQ(); q[0] = a; setDefaultQ(q);
01716 return *this;
01717 }
01718 Planar& setDefaultTranslation(const Vec2& r) {
01719 Vec3 q = getDefaultQ(); q.updSubVec<2>(1) = r; setDefaultQ(q);
01720 return *this;
01721 }
01722
01723 Real getDefaultAngle() const {return getDefaultQ()[0];}
01724 const Vec2& getDefaultTranslation() const {return getDefaultQ().getSubVec<2>(1);}
01725
01726 void setAngle (State& s, Real a) {setOneQ(s,0,a);}
01727 void setTranslation(State& s, const Vec2& r) {setOneQ(s,1,r[0]); setOneQ(s,2,r[1]);}
01728
01729 Real getAngle(const State& s) const {return getQ(s)[0];}
01730 const Vec2& getTranslation(const State& s) const {return getQ(s).getSubVec<2>(1);}
01731
01732
01733 const Vec3& getDefaultQ() const;
01734 Planar& setDefaultQ(const Vec3& q);
01735
01736 const Vec3& getQ(const State&) const;
01737 const Vec3& getQDot(const State&) const;
01738 const Vec3& getQDotDot(const State&) const;
01739 const Vec3& getU(const State&) const;
01740 const Vec3& getUDot(const State&) const;
01741
01742 void setQ(State&, const Vec3&) const;
01743 void setU(State&, const Vec3&) const;
01744
01745 const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
01746 const Vec3& getMyPartU(const State&, const Vector& ulike) const;
01747
01748 Vec3& updMyPartQ(const State&, Vector& qlike) const;
01749 Vec3& updMyPartU(const State&, Vector& ulike) const;
01750 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Planar, PlanarImpl, MobilizedBody);
01751 };
01752
01777 class SimTK_SIMBODY_EXPORT MobilizedBody::SphericalCoords : public MobilizedBody {
01778 public:
01779 explicit SphericalCoords(Direction=Forward);
01780
01783 SphericalCoords(MobilizedBody& parent, const Body&, Direction=Forward);
01784
01789 SphericalCoords(MobilizedBody& parent, const Transform& inbFrame,
01790 const Body&, const Transform& outbFrame,
01791 Direction=Forward);
01792
01794 SphericalCoords(MobilizedBody& parent, const Transform& inbFrame,
01795 const Body&, const Transform& outbFrame,
01796 Real azimuthOffset, bool azimuthNegated,
01797 Real zenithOffset, bool zenithNegated,
01798 CoordinateAxis radialAxis, bool radialNegated,
01799 Direction=Forward);
01800
01801 SphericalCoords& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01802 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01803 }
01804 SphericalCoords& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
01805 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01806 }
01807 SphericalCoords& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01808 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01809 }
01810
01811 SphericalCoords& setDefaultInboardFrame(const Transform& X_PF) {
01812 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01813 }
01814
01815 SphericalCoords& setDefaultOutboardFrame(const Transform& X_BM) {
01816 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01817 }
01818
01819
01820 SphericalCoords& setDefaultAngles(const Vec2& a) {
01821 Vec3 q = getDefaultQ(); q.updSubVec<2>(0) = a; setDefaultQ(q);
01822 return *this;
01823 }
01824 SphericalCoords& setDefaultRadius(Real r) {
01825 Vec3 q = getDefaultQ(); q[2] = r; setDefaultQ(q);
01826 return *this;
01827 }
01828 SphericalCoords& setRadialAxis(CoordinateAxis);
01829 SphericalCoords& setNegateAzimuth(bool);
01830 SphericalCoords& setNegateZenith(bool);
01831 SphericalCoords& setNegateRadial(bool);
01832
01833 const Vec2& getDefaultAngles() const {return getDefaultQ().getSubVec<2>(0);}
01834 Real getDefaultTranslation() const {return getDefaultQ()[2];}
01835
01836 CoordinateAxis getRadialAxis() const;
01837 bool isAzimuthNegated() const;
01838 bool isZenithNegated() const;
01839 bool isRadialNegated() const;
01840
01841 void setAngles(State& s, const Vec2& a) {setOneQ(s,0,a[0]); setOneQ(s,1,a[1]);}
01842 void setRadius(State& s, Real r) {setOneQ(s,2,r);}
01843
01844 const Vec2& getAngles(const State& s) const {return getQ(s).getSubVec<2>(0);}
01845 Real getRadius(const State& s) const {return getQ(s)[2];}
01846
01847
01848 const Vec3& getDefaultQ() const;
01849 SphericalCoords& setDefaultQ(const Vec3& q);
01850
01851 const Vec3& getQ(const State&) const;
01852 const Vec3& getQDot(const State&) const;
01853 const Vec3& getQDotDot(const State&) const;
01854 const Vec3& getU(const State&) const;
01855 const Vec3& getUDot(const State&) const;
01856
01857 void setQ(State&, const Vec3&) const;
01858 void setU(State&, const Vec3&) const;
01859
01860 const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
01861 const Vec3& getMyPartU(const State&, const Vector& ulike) const;
01862
01863 Vec3& updMyPartQ(const State&, Vector& qlike) const;
01864 Vec3& updMyPartU(const State&, Vector& ulike) const;
01865
01866 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(SphericalCoords, SphericalCoordsImpl, MobilizedBody);
01867 };
01868
01872 class SimTK_SIMBODY_EXPORT MobilizedBody::Gimbal : public MobilizedBody {
01873 public:
01874 explicit Gimbal(Direction=Forward);
01875
01878 Gimbal(MobilizedBody& parent, const Body&, Direction=Forward);
01879
01882 Gimbal(MobilizedBody& parent, const Transform& inbFrame,
01883 const Body&, const Transform& outbFrame, Direction=Forward);
01884
01885 Gimbal& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01886 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01887 }
01888 Gimbal& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
01889 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01890 }
01891 Gimbal& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01892 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01893 }
01894
01895 Gimbal& setDefaultInboardFrame(const Transform& X_PF) {
01896 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01897 }
01898
01899 Gimbal& setDefaultOutboardFrame(const Transform& X_BM) {
01900 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01901 }
01902
01903
01904 Gimbal& setDefaultRotation(const Rotation& R_FM) {
01905 return setDefaultQ(R_FM.convertRotationToBodyFixedXYZ());
01906 }
01907 Rotation getDefaultRotation() const {
01908 const Vec3& q = getDefaultQ();
01909 return Rotation(BodyRotationSequence,
01910 q[0], XAxis, q[1], YAxis, q[2], ZAxis);
01911 }
01912
01913
01914 Gimbal& setDefaultRadius(Real r);
01915 Real getDefaultRadius() const;
01916
01917
01918 const Vec3& getDefaultQ() const;
01919 Gimbal& setDefaultQ(const Vec3& q);
01920
01921 const Vec3& getQ(const State&) const;
01922 const Vec3& getQDot(const State&) const;
01923 const Vec3& getQDotDot(const State&) const;
01924 const Vec3& getU(const State&) const;
01925 const Vec3& getUDot(const State&) const;
01926
01927 void setQ(State&, const Vec3&) const;
01928 void setU(State&, const Vec3&) const;
01929
01930 const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
01931 const Vec3& getMyPartU(const State&, const Vector& ulike) const;
01932
01933 Vec3& updMyPartQ(const State&, Vector& qlike) const;
01934 Vec3& updMyPartU(const State&, Vector& ulike) const;
01935 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Gimbal, GimbalImpl, MobilizedBody);
01936 };
01937
01942 class SimTK_SIMBODY_EXPORT MobilizedBody::Ball : public MobilizedBody {
01943 public:
01944 explicit Ball(Direction=Forward);
01945
01948 Ball(MobilizedBody& parent, const Body&, Direction=Forward);
01949
01952 Ball(MobilizedBody& parent, const Transform& inbFrame,
01953 const Body&, const Transform& outbFrame, Direction=Forward);
01954
01955 Ball& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
01956 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
01957 }
01958 Ball& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
01959 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
01960 }
01961 Ball& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
01962 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
01963 }
01964
01965 Ball& setDefaultInboardFrame(const Transform& X_PF) {
01966 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
01967 }
01968
01969 Ball& setDefaultOutboardFrame(const Transform& X_BM) {
01970 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
01971 }
01972
01973
01974 Ball& setDefaultRotation(const Rotation& R_FM) {
01975 return setDefaultQ(R_FM.convertRotationToQuaternion());
01976 }
01977 Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}
01978
01979
01980 Ball& setDefaultRadius(Real r);
01981 Real getDefaultRadius() const;
01982
01983
01984 const Quaternion& getDefaultQ() const;
01985 Ball& setDefaultQ(const Quaternion& q);
01986
01987 const Vec4& getQ(const State&) const;
01988 const Vec4& getQDot(const State&) const;
01989 const Vec4& getQDotDot(const State&) const;
01990 const Vec3& getU(const State&) const;
01991 const Vec3& getUDot(const State&) const;
01992
01993 void setQ(State&, const Vec4&) const;
01994 void setU(State&, const Vec3&) const;
01995
01996 const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
01997 const Vec3& getMyPartU(const State&, const Vector& ulike) const;
01998
01999 Vec4& updMyPartQ(const State&, Vector& qlike) const;
02000 Vec3& updMyPartU(const State&, Vector& ulike) const;
02001 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ball, BallImpl, MobilizedBody);
02002 };
02003
02008 class SimTK_SIMBODY_EXPORT MobilizedBody::Ellipsoid : public MobilizedBody {
02009 public:
02012 explicit Ellipsoid(Direction=Forward);
02013
02016 Ellipsoid(MobilizedBody& parent, const Body&, Direction=Forward);
02017
02020 Ellipsoid(MobilizedBody& parent, const Transform& inbFrame,
02021 const Body&, const Transform& outbFrame,
02022 Direction=Forward);
02023
02027 Ellipsoid(MobilizedBody& parent, const Transform& inbFrame,
02028 const Body&, const Transform& outbFrame,
02029 const Vec3& radii,
02030 Direction=Forward);
02031
02032 Ellipsoid& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02033 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02034 }
02035 Ellipsoid& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
02036 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02037 }
02038 Ellipsoid& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02039 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02040 }
02041
02042 Ellipsoid& setDefaultInboardFrame(const Transform& X_PF) {
02043 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02044 }
02045
02046 Ellipsoid& setDefaultOutboardFrame(const Transform& X_BM) {
02047 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02048 }
02049
02050
02051 Ellipsoid& setDefaultRotation(const Rotation& R_FM) {
02052 return setDefaultQ(R_FM.convertRotationToQuaternion());
02053 }
02054 Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}
02055
02056 Ellipsoid& setDefaultRadii(const Vec3& r);
02057 const Vec3& getDefaultRadii() const;
02058
02059
02060 const Quaternion& getDefaultQ() const;
02061 Quaternion& updDefaultQ();
02062 Ellipsoid& setDefaultQ(const Quaternion& q) {updDefaultQ()=q; return *this;}
02063
02064 const Vec4& getQ(const State&) const;
02065 const Vec4& getQDot(const State&) const;
02066 const Vec4& getQDotDot(const State&) const;
02067 const Vec3& getU(const State&) const;
02068 const Vec3& getUDot(const State&) const;
02069
02070 void setQ(State&, const Vec4&) const;
02071 void setU(State&, const Vec3&) const;
02072
02073 const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
02074 const Vec3& getMyPartU(const State&, const Vector& ulike) const;
02075
02076 Vec4& updMyPartQ(const State&, Vector& qlike) const;
02077 Vec3& updMyPartU(const State&, Vector& ulike) const;
02078
02079 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ellipsoid, EllipsoidImpl, MobilizedBody);
02080 };
02081
02084 class SimTK_SIMBODY_EXPORT MobilizedBody::Translation : public MobilizedBody {
02085 public:
02086 explicit Translation(Direction=Forward);
02087
02090 Translation(MobilizedBody& parent, const Body&, Direction=Forward);
02091
02094 Translation(MobilizedBody& parent, const Transform& inbFrame,
02095 const Body&, const Transform& outbFrame, Direction=Forward);
02096
02097 Translation& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02098 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02099 }
02100 Translation& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
02101 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02102 }
02103 Translation& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02104 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02105 }
02106
02107 Translation& setDefaultInboardFrame(const Transform& X_PF) {
02108 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02109 }
02110
02111 Translation& setDefaultOutboardFrame(const Transform& X_BM) {
02112 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02113 }
02114
02115
02116
02117
02118
02119
02120 Translation& setDefaultTranslation(const Vec3& p_FM) {
02121 return setDefaultQ(p_FM);
02122 }
02123
02124
02125 const Vec3& getDefaultTranslation() const {
02126 return getDefaultQ();
02127 }
02128
02129
02130
02131 void setMobilizerTranslation(State& s, const Vec3& p_FM) const {
02132 setQ(s,p_FM);
02133 }
02134
02135
02136 const Vec3& getMobilizerTranslation(const State& s) const {
02137 return getQ(s);
02138 }
02139
02140
02141
02142
02143 void setMobilizerVelocity(State& s, const Vec3& v_FM) const {
02144 setU(s,v_FM);
02145 }
02146
02147
02148 const Vec3& getMobilizerVelocity(const State& s) const {
02149 return getU(s);
02150 }
02151
02152
02153 const Vec3& getMobilizerAcceleration(const State& s) const {
02154 return getUDot(s);
02155 }
02156
02157
02158 const Vec3& getDefaultQ() const;
02159 Translation& setDefaultQ(const Vec3& q);
02160
02161 const Vec3& getQ(const State&) const;
02162 const Vec3& getQDot(const State&) const;
02163 const Vec3& getQDotDot(const State&) const;
02164 const Vec3& getU(const State&) const;
02165 const Vec3& getUDot(const State&) const;
02166
02167 void setQ(State&, const Vec3&) const;
02168 void setU(State&, const Vec3&) const;
02169
02170 const Vec3& getMyPartQ(const State&, const Vector& qlike) const;
02171 const Vec3& getMyPartU(const State&, const Vector& ulike) const;
02172
02173 Vec3& updMyPartQ(const State&, Vector& qlike) const;
02174 Vec3& updMyPartU(const State&, Vector& ulike) const;
02175 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Translation, TranslationImpl, MobilizedBody);
02176 };
02177
02184 class SimTK_SIMBODY_EXPORT MobilizedBody::Free : public MobilizedBody {
02185 public:
02186 explicit Free(Direction=Forward);
02187
02190 Free(MobilizedBody& parent, const Body&, Direction=Forward);
02191
02194 Free(MobilizedBody& parent, const Transform& inbFrame,
02195 const Body&, const Transform& outbFrame,
02196 Direction=Forward);
02197
02198 Free& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02199 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02200 }
02201 Free& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
02202 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02203 }
02204 Free& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02205 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02206 }
02207
02208 Free& setDefaultInboardFrame(const Transform& X_PF) {
02209 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02210 }
02211
02212 Free& setDefaultOutboardFrame(const Transform& X_BM) {
02213 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02214 }
02215
02216
02217 Free& setDefaultTranslation(const Vec3&);
02218
02219
02220
02221
02222 Free& setDefaultQuaternion(const Quaternion&);
02223
02224
02225
02226 Free& setDefaultRotation(const Rotation&);
02227
02228
02229 Free& setDefaultTransform(const Transform&);
02230
02231
02232 const Vec3& getDefaultTranslation() const;
02233 const Quaternion& getDefaultQuaternion() const;
02234
02235
02236 Rotation getDefaultRotation() const {
02237 return Rotation(getDefaultQuaternion());
02238 }
02239 Transform getDefaultTransform() const {
02240 return Transform(Rotation(getDefaultQuaternion()), getDefaultTranslation());
02241 }
02242
02243
02244
02245
02246 const Vec7& getDefaultQ() const;
02247
02248
02249
02250
02251
02252 Free& setDefaultQ(const Vec7& q);
02253
02254
02255 const Vec7& getQ(const State&) const;
02256 const Vec7& getQDot(const State&) const;
02257 const Vec7& getQDotDot(const State&) const;
02258
02259 const Vec6& getU(const State&) const;
02260 const Vec6& getUDot(const State&) const;
02261
02262
02263 void setQ(State&, const Vec7&) const;
02264 void setU(State&, const Vec6&) const;
02265
02266 const Vec7& getMyPartQ(const State&, const Vector& qlike) const;
02267 const Vec6& getMyPartU(const State&, const Vector& ulike) const;
02268
02269 Vec7& updMyPartQ(const State&, Vector& qlike) const;
02270 Vec6& updMyPartU(const State&, Vector& ulike) const;
02271 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Free, FreeImpl, MobilizedBody);
02272 };
02273
02274
02275
02276
02277
02278
02279
02280
02281
02282
02283
02284
02285
02286
02287
02288
02289
02290
02296 class SimTK_SIMBODY_EXPORT MobilizedBody::LineOrientation : public MobilizedBody {
02297 public:
02298 explicit LineOrientation(Direction=Forward);
02299
02302 LineOrientation(MobilizedBody& parent, const Body&, Direction=Forward);
02303
02306 LineOrientation(MobilizedBody& parent, const Transform& inbFrame,
02307 const Body&, const Transform& outbFrame, Direction=Forward);
02308
02309 LineOrientation& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02310 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02311 }
02312 LineOrientation& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
02313 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02314 }
02315 LineOrientation& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02316 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02317 }
02318
02319 LineOrientation& setDefaultInboardFrame(const Transform& X_PF) {
02320 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02321 }
02322
02323 LineOrientation& setDefaultOutboardFrame(const Transform& X_BM) {
02324 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02325 }
02326
02327
02328 LineOrientation& setDefaultRotation(const Rotation& R_FM) {
02329 return setDefaultQ(R_FM.convertRotationToQuaternion());
02330 }
02331 Rotation getDefaultRotation() const {return Rotation(getDefaultQ());}
02332
02333
02334 const Quaternion& getDefaultQ() const;
02335 LineOrientation& setDefaultQ(const Quaternion& q);
02336
02337 const Vec4& getQ(const State&) const;
02338 const Vec4& getQDot(const State&) const;
02339 const Vec4& getQDotDot(const State&) const;
02340 const Vec2& getU(const State&) const;
02341 const Vec2& getUDot(const State&) const;
02342
02343 void setQ(State&, const Vec4&) const;
02344 void setU(State&, const Vec2&) const;
02345
02346 const Vec4& getMyPartQ(const State&, const Vector& qlike) const;
02347 const Vec2& getMyPartU(const State&, const Vector& ulike) const;
02348
02349 Vec4& updMyPartQ(const State&, Vector& qlike) const;
02350 Vec2& updMyPartU(const State&, Vector& ulike) const;
02351 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(LineOrientation, LineOrientationImpl, MobilizedBody);
02352 };
02353
02358 class SimTK_SIMBODY_EXPORT MobilizedBody::FreeLine : public MobilizedBody {
02359 public:
02360 explicit FreeLine(Direction=Forward);
02361
02364 FreeLine(MobilizedBody& parent, const Body&, Direction=Forward);
02365
02368 FreeLine(MobilizedBody& parent, const Transform& inbFrame,
02369 const Body&, const Transform& outbFrame, Direction=Forward);
02370
02371 FreeLine& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02372 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02373 }
02374 FreeLine& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
02375 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02376 }
02377 FreeLine& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02378 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02379 }
02380
02381 FreeLine& setDefaultInboardFrame(const Transform& X_PF) {
02382 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02383 }
02384
02385 FreeLine& setDefaultOutboardFrame(const Transform& X_BM) {
02386 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02387 }
02388 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(FreeLine, FreeLineImpl, MobilizedBody);
02389 };
02390
02393 class SimTK_SIMBODY_EXPORT MobilizedBody::Weld : public MobilizedBody {
02394 public:
02397 Weld();
02398
02401 Weld(MobilizedBody& parent, const Body&);
02402
02405 Weld(MobilizedBody& parent, const Transform& inbFrame,
02406 const Body&, const Transform& outbFrame);
02407
02408 Weld& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02409 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02410 }
02411 Weld& addOutboardDecoration(const Transform& X_MD, const DecorativeGeometry& g) {
02412 (void)MobilizedBody::addOutboardDecoration(X_MD,g); return *this;
02413 }
02414 Weld& addInboardDecoration (const Transform& X_FD, const DecorativeGeometry& g) {
02415 (void)MobilizedBody::addInboardDecoration(X_FD,g); return *this;
02416 }
02417
02418 Weld& setDefaultInboardFrame(const Transform& X_PF) {
02419 (void)MobilizedBody::setDefaultInboardFrame(X_PF); return *this;
02420 }
02421
02422 Weld& setDefaultOutboardFrame(const Transform& X_BM) {
02423 (void)MobilizedBody::setDefaultOutboardFrame(X_BM); return *this;
02424 }
02425 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Weld, WeldImpl, MobilizedBody);
02426 };
02427
02428
02432 class SimTK_SIMBODY_EXPORT MobilizedBody::Ground : public MobilizedBody {
02433 public:
02435 Ground();
02436 Ground& addBodyDecoration(const Transform& X_BD, const DecorativeGeometry& g) {
02437 (void)MobilizedBody::addBodyDecoration(X_BD,g); return *this;
02438 }
02439 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Ground, GroundImpl, MobilizedBody);
02440 };
02441
02442
02474 class SimTK_SIMBODY_EXPORT MobilizedBody::Custom : public MobilizedBody {
02475 public:
02476 class Implementation;
02477 class ImplementationImpl;
02478
02479
02480
02481
02482
02483
02484
02485
02486
02487
02488 Custom(MobilizedBody& parent, Implementation* implementation, const Body& body, Direction direction=Forward);
02489
02490
02491
02492
02493
02494
02495
02496
02497
02498
02499
02500 Custom(MobilizedBody& parent, Implementation* implementation,
02501 const Transform& inbFrame, const Body& body, const Transform& outbFrame,
02502 Direction direction=Forward);
02503 SimTK_INSERT_DERIVED_HANDLE_DECLARATIONS(Custom, CustomImpl, MobilizedBody);
02504 protected:
02505 const Implementation& getImplementation() const;
02506 Implementation& updImplementation();
02507 };
02508
02509
02510
02511
02512 #ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
02513 extern template class PIMPLHandle<MobilizedBody::Custom::Implementation, MobilizedBody::Custom::ImplementationImpl>;
02514 #endif
02515
02516
02517 class SimTK_SIMBODY_EXPORT MobilizedBody::Custom::Implementation
02518 : public PIMPLHandle<Implementation,ImplementationImpl>
02519 {
02520 public:
02521
02522
02523
02525 virtual ~Implementation();
02526
02531 virtual Implementation* clone() const = 0;
02532
02556 Implementation(SimbodyMatterSubsystem&, int nu, int nq, int nAngles=0);
02557
02561 Vector getQ(const State& s) const;
02562
02564 Vector getU(const State& s) const;
02565
02569 Vector getQDot(const State& s) const;
02570
02572 Vector getUDot(const State& s) const;
02573
02577 Vector getQDotDot(const State& s) const;
02578
02583 Transform getMobilizerTransform(const State& s) const;
02584
02591 SpatialVec getMobilizerVelocity(const State& s) const;
02592
02601 bool getUseEulerAngles(const State& s) const;
02602
02609 void invalidateTopologyCache() const;
02610
02615
02616
02621 virtual Transform calcMobilizerTransformFromQ(const State& s, int nq, const Real* q) const = 0;
02622
02623
02640 virtual SpatialVec multiplyByHMatrix(const State& s, int nu, const Real* u) const = 0;
02641
02650 virtual void multiplyByHTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const = 0;
02651
02661 virtual SpatialVec multiplyByHDotMatrix(const State& s, int nu, const Real* u) const = 0;
02662
02671 virtual void multiplyByHDotTranspose(const State& s, const SpatialVec& F, int nu, Real* f) const = 0;
02672
02683 virtual void multiplyByN(const State& s, bool transposeMatrix,
02684 int nIn, const Real* in, int nOut, Real* out) const;
02685
02696 virtual void multiplyByNInv(const State& s, bool transposeMatrix,
02697 int nIn, const Real* in, int nOut, Real* out) const;
02698
02709 virtual void multiplyByNDot(const State& s, bool transposeMatrix,
02710 int nIn, const Real* in, int nOut, Real* out) const;
02711
02712
02713
02714
02715
02716
02717
02718
02719
02720
02721
02730 virtual void setQToFitTransform(const State&, const Transform& X_FM, int nq, Real* q) const;
02731
02743 virtual void setUToFitVelocity(const State&, const SpatialVec& V_FM, int nu, Real* u) const;
02744
02751 virtual void calcDecorativeGeometryAndAppend
02752 (const State& s, Stage stage, std::vector<DecorativeGeometry>& geom) const
02753 {
02754 }
02756
02757
02766
02768
02769
02770
02771
02772
02773
02774
02775
02776 virtual void realizeTopology(State&) const { }
02777
02786 virtual void realizeModel(State&) const { }
02787
02791 virtual void realizeInstance(const State&) const { }
02792
02797 virtual void realizeTime(const State&) const { }
02798
02805 virtual void realizePosition(const State&) const { }
02806
02813 virtual void realizeVelocity(const State&) const { }
02814
02821 virtual void realizeDynamics(const State&) const { }
02822
02829 virtual void realizeAcceleration(const State&) const { }
02830
02836 virtual void realizeReport(const State&) const { }
02838
02839 friend class MobilizedBody::CustomImpl;
02840 };
02841
02853 class SimTK_SIMBODY_EXPORT MobilizedBody::FunctionBased : public MobilizedBody::Custom {
02854 public:
02855
02856
02857
02858
02859
02860
02861
02862
02863
02864
02865
02866
02867
02868 FunctionBased(MobilizedBody& parent, const Body& body,
02869 int nmobilities, const std::vector<const Function*>& functions,
02870 const std::vector<std::vector<int> >& coordIndices,
02871 Direction direction=Forward);
02872
02873
02874
02875
02876
02877
02878
02879
02880
02881
02882
02883
02884
02885
02886
02887 FunctionBased(MobilizedBody& parent, const Transform& inbFrame,
02888 const Body& body, const Transform& outbFrame,
02889 int nmobilities, const std::vector<const Function*>& functions,
02890 const std::vector<std::vector<int> >& coordIndices,
02891 Direction direction=Forward);
02892
02893
02894
02895
02896
02897
02898
02899
02900
02901
02902
02903
02904
02905
02906
02907 FunctionBased(MobilizedBody& parent, const Body& body,
02908 int nmobilities, const std::vector<const Function*>& functions,
02909 const std::vector<std::vector<int> >& coordIndices, const std::vector<Vec3>& axes,
02910 Direction direction=Forward);
02911
02912
02913
02914
02915
02916
02917
02918
02919
02920
02921
02922
02923
02924
02925
02926
02927
02928 FunctionBased(MobilizedBody& parent, const Transform& inbFrame,
02929 const Body& body, const Transform& outbFrame,
02930 int nmobilities, const std::vector<const Function*>& functions,
02931 const std::vector<std::vector<int> >& coordIndices, const std::vector<Vec3>& axes,
02932 Direction direction=Forward);
02933 };
02934
02935 }
02936
02937 #endif // SimTK_SIMBODY_MOBILIZED_BODY_H_
02938
02939
02940