Simbody  3.6
MobilizedBody.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMBODY_MOBILIZED_BODY_H_
2 #define SimTK_SIMBODY_MOBILIZED_BODY_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm) *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2007-13 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: Paul Mitiguy, Peter Eastman *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
39 #include "SimTKmath.h"
41 #include "simbody/internal/Body.h"
43 
44 #include <cassert>
45 
46 namespace SimTK {
47 
48 class SimbodyMatterSubsystem;
49 class Motion;
50 class MobilizedBody;
51 class MobilizedBodyImpl;
52 
53 // We only want the template instantiation to occur once. This symbol is
54 // defined in the SimTK core compilation unit that instantiates the mobilized
55 // body class but should not be defined any other time.
56 #ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
57  extern template class PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true>;
58 #endif
59 
64 
65 
66 
67 //==============================================================================
68 // MOBILIZED BODY
69 //==============================================================================
170 public:
171 
181 enum Direction {
182  Forward = 0,
183  Reverse = 1
184 };
185 
186 //------------------------------------------------------------------------------
220 void lock(State& state, Motion::Level level=Motion::Position) const;
221 
230 void lockAt(State& state, Real value,
231  Motion::Level level=Motion::Position) const;
232 
241 void lockAt(State& state, const Vector& value,
242  Motion::Level level=Motion::Position) const;
243 
253 template <int N> SimTK_SIMBODY_EXPORT // instantiated in library
254 void lockAt(State& state, const Vec<N>& value,
255  Motion::Level level=Motion::Position) const;
256 
260 void unlock(State& state) const;
261 
263 bool isLocked(const State& state) const
264 { return getLockLevel(state)!=Motion::NoLevel; }
265 
268 Motion::Level getLockLevel(const State& state) const;
269 
273 Vector getLockValueAsVector(const State& state) const;
274 
283 MobilizedBody& lockByDefault(Motion::Level level=Motion::Position);
284 
286 bool isLockedByDefault() const
287 { return getLockByDefaultLevel()!=Motion::NoLevel; }
288 
291 Motion::Level getLockByDefaultLevel() const;
294  // STATE ACCESS METHODS //
297 
298 //------------------------------------------------------------------------------
316 const Transform& getBodyTransform(const State& state) const; // X_GB
317 
326 const Rotation& getBodyRotation(const State& state) const {
327  return getBodyTransform(state).R();
328 }
333 const Vec3& getBodyOriginLocation(const State& state) const {
334  return getBodyTransform(state).p();
335 }
336 
340 const Transform& getMobilizerTransform(const State& state) const; // X_FM
341 
348 const SpatialVec& getBodyVelocity(const State& state) const; // V_GB
349 
354 const Vec3& getBodyAngularVelocity(const State& state) const { // w_GB
355  return getBodyVelocity(state)[0];
356 }
361 const Vec3& getBodyOriginVelocity(const State& state) const { // v_GB
362  return getBodyVelocity(state)[1];
363 }
364 
369 const SpatialVec& getMobilizerVelocity(const State& state) const; // V_FM
370 
377 const SpatialVec& getBodyAcceleration(const State& state) const; // A_GB
378 
383 const Vec3& getBodyAngularAcceleration(const State& state) const { // b_GB
384  return getBodyAcceleration(state)[0];
385 }
386 
391 const Vec3& getBodyOriginAcceleration(const State& state) const { // a_GB
392  return getBodyAcceleration(state)[1];
393 }
394 
400 const SpatialVec& getMobilizerAcceleration(const State& state) const { // A_FM
401  SimTK_ASSERT_ALWAYS(!"unimplemented method",
402 "MobilizedBody::getMobilizerAcceleration() not yet implemented -- volunteers?");
403  return *(new SpatialVec());
404 }
405 
408 const MassProperties& getBodyMassProperties(const State& state) const;
409 
413 const SpatialInertia& getBodySpatialInertiaInGround(const State& state) const;
414 
417 Real getBodyMass(const State& state) const {
418  return getBodyMassProperties(state).getMass();
419 }
420 
424 const Vec3& getBodyMassCenterStation(const State& state) const {
425  return getBodyMassProperties(state).getMassCenter();
426 }
427 
432  return getBodyMassProperties(state).getUnitInertia();
433 }
434 
440 const Transform& getInboardFrame (const State& state) const; // X_PF
446 const Transform& getOutboardFrame(const State& state) const; // X_BM
447 
451 void setInboardFrame (State& state, const Transform& X_PF) const;
455 void setOutboardFrame(State& state, const Transform& X_BM) const;
456 
457 // End of State Access - Bodies
461 //------------------------------------------------------------------------------
468 int getNumQ(const State& state) const;
471 int getNumU(const State& state) const;
472 
475 QIndex getFirstQIndex(const State& state) const;
476 
479 UIndex getFirstUIndex(const State& state) const;
480 
483 Motion::Method getQMotionMethod(const State& state) const;
486 Motion::Method getUMotionMethod(const State& state) const;
489 Motion::Method getUDotMotionMethod(const State& state) const;
490 
495 Real getOneQ(const State& state, int which) const;
496 
500 Real getOneU(const State& state, int which) const;
501 
505 Vector getQAsVector(const State& state) const;
509 Vector getUAsVector(const State& state) const;
510 
515 Real getOneQDot (const State& state, int which) const;
519 Vector getQDotAsVector(const State& state) const;
520 
525 Real getOneUDot(const State& state, int which) const;
530 Real getOneQDotDot(const State& state, int which) const;
534 Vector getUDotAsVector(const State& state) const;
539 Vector getQDotDotAsVector(const State& state) const;
540 
550 Vector getTauAsVector(const State& state) const;
551 
556 Real getOneTau(const State& state, MobilizerUIndex which) const;
557 
562 void setOneQ(State& state, int which, Real v) const;
567 void setOneU(State& state, int which, Real v) const;
568 
572 void setQFromVector(State& state, const Vector& v) const;
576 void setUFromVector(State& state, const Vector& v) const;
577 
611 void setQToFitTransform(State& state, const Transform& X_FM) const;
612 
617 void setQToFitRotation(State& state, const Rotation& R_FM) const;
618 
624 void setQToFitTranslation(State& state, const Vec3& p_FM) const;
625 
632 void setUToFitVelocity(State& state, const SpatialVec& V_FM) const;
633 
638 void setUToFitAngularVelocity(State& state, const Vec3& w_FM) const;
639 
645 void setUToFitLinearVelocity(State& state, const Vec3& v_FM) const;
646 
656 SpatialVec getHCol(const State& state, MobilizerUIndex ux) const;
657 
664 SpatialVec getH_FMCol(const State& state, MobilizerUIndex ux) const;
665 
666 // End of State Access Methods.
669  // BASIC OPERATORS //
672 
673 //------------------------------------------------------------------------------
718  const MobilizedBody& inBodyA) const
719 {
720  const Transform& X_GA = inBodyA.getBodyTransform(state);
721  const Transform& X_GB = this->getBodyTransform(state);
722 
723  return ~X_GA*X_GB; // X_AB=X_AG*X_GB
724 }
725 
732  const MobilizedBody& inBodyA) const
733 {
734  const Rotation& R_GA = inBodyA.getBodyRotation(state);
735  const Rotation& R_GB = this->getBodyRotation(state);
736 
737  return ~R_GA*R_GB; // R_AB=R_AG*R_GB
738 }
739 
746 Vec3 findBodyOriginLocationInAnotherBody
747  (const State& state, const MobilizedBody& toBodyA) const {
748  return toBodyA.findStationAtGroundPoint(state,
749  getBodyOriginLocation(state));
750 }
751 
757 SpatialVec findBodyVelocityInAnotherBody
758  (const State& state, const MobilizedBody& inBodyA) const
759 {
760  const SpatialVec& V_GB = this->getBodyVelocity(state);
761  const SpatialVec& V_GA = inBodyA.getBodyVelocity(state);
762  // angular velocity of B in A, exp in G
763  const Vec3 w_AB_G = V_GB[0]-V_GA[0]; // 3 flops
764 
765  // Angular vel. was easy, but for linear vel. we have to add in an wXr term.
766 
767  const Transform& X_GB = getBodyTransform(state);
768  const Transform& X_GA = inBodyA.getBodyTransform(state);
769  // vector from Ao to Bo, exp in G
770  const Vec3 p_AB_G = X_GB.p() - X_GA.p(); // 3 flops
771  // d/dt p taken in G
772  const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1]; // 3 flops
773 
774  // d/dt p taken in A, exp in G
775  const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G; // 12 flops
776 
777  // We're done, but answer is expressed in Ground. Reexpress in A and return.
778  return ~X_GA.R()*SpatialVec(w_AB_G, v_AB_G); // 30 flops
779 }
780 
787  const MobilizedBody& inBodyA) const
788 {
789  const Vec3& w_GB = getBodyAngularVelocity(state);
790  const Vec3& w_GA = inBodyA.getBodyAngularVelocity(state);
791  // angular velocity of B in A, exp in G
792  const Vec3 w_AB_G = w_GB-w_GA; // 3 flops
793 
794  // Now reexpress in A.
795  return inBodyA.expressGroundVectorInBodyFrame(state, w_AB_G); // 15 flops
796 }
797 
804  const MobilizedBody& inBodyA) const
805 {
806  // Doesn't save much to special case this one.
807  return findBodyVelocityInAnotherBody(state,inBodyA)[1];
808 }
809 
817  const MobilizedBody& inBodyA) const
818 {
819  const Transform& X_GA = inBodyA.getBodyTransform(state);
820  const SpatialVec& V_GA = inBodyA.getBodyVelocity(state);
821  const SpatialVec& A_GA = inBodyA.getBodyAcceleration(state);
822  const Transform& X_GB = this->getBodyTransform(state);
823  const SpatialVec& V_GB = this->getBodyVelocity(state);
824  const SpatialVec& A_GB = this->getBodyAcceleration(state);
825 
826  return findRelativeAcceleration(X_GA, V_GA, A_GA,
827  X_GB, V_GB, A_GB);
828 }
829 
835 Vec3 findBodyAngularAccelerationInAnotherBody
836  (const State& state, const MobilizedBody& inBodyA) const
837 {
838  const Rotation& R_GA = inBodyA.getBodyRotation(state);
839  const Vec3& w_GA = inBodyA.getBodyAngularVelocity(state);
840  const Vec3& w_GB = this->getBodyAngularVelocity(state);
841  const Vec3& b_GA = inBodyA.getBodyAngularAcceleration(state);
842  const Vec3& b_GB = this->getBodyAngularAcceleration(state);
843 
844  // relative ang. vel. of B in A, exp. in G
845  const Vec3 w_AB_G = w_GB - w_GA; // 3 flops
846  const Vec3 w_AB_G_dot = b_GB - b_GA; // d/dt of w_AB_G taken in G; 3 flops
847 
848  // We have the derivative in G; change it to derivative in A by adding
849  // in contribution caused by motion of G in A, that is w_AG X w_AB_G.
850  // (Note that w_AG=-w_GA.)
851  const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G; // ang. accel. of B in A
852  // 12 flops
853 
854  return ~R_GA * b_AB_G; // taken in A, expressed in A; 15 flops
855 }
856 
864  const MobilizedBody& inBodyA) const
865 {
866  // Not much to be saved by trying to optimize this since the linear part
867  // is the most expensive to calculate.
868  return findBodyAccelerationInAnotherBody(state,inBodyA)[1];
869 }
870 
878 SpatialVec findMobilizerReactionOnBodyAtMInGround(const State& state) const;
879 
886 SpatialVec findMobilizerReactionOnBodyAtOriginInGround
887  (const State& state) const;
888 
897 SpatialVec findMobilizerReactionOnParentAtFInGround(const State& state) const;
898 
906 SpatialVec findMobilizerReactionOnParentAtOriginInGround
907  (const State& state) const;
908 
915 Vec3 findStationLocationInGround
916  (const State& state, const Vec3& stationOnB) const {
917  return getBodyTransform(state) * stationOnB;
918 }
919 
920 
929 Vec3 findStationLocationInAnotherBody
930  (const State& state, const Vec3& stationOnB, const MobilizedBody& toBodyA)
931  const
932 {
933  return toBodyA.findStationAtGroundPoint(state,
934  findStationLocationInGround(state,stationOnB));
935 }
936 
943 Vec3 findStationVelocityInGround
944  (const State& state, const Vec3& stationOnB) const {
945  const Vec3& w = getBodyAngularVelocity(state); // in G
946  const Vec3& v = getBodyOriginVelocity(state); // in G
947  const Vec3 r = expressVectorInGroundFrame(state,stationOnB); // 15 flops
948  return v + w % r; // 12 flops
949 }
950 
951 
958  const Vec3& stationOnBodyB,//p_BS
959  const MobilizedBody& inBodyA) const
960 {
961  const SpatialVec V_AB =
962  findBodyVelocityInAnotherBody(state, inBodyA); //51 flops
963  // Bo->S rexpressed in A but not shifted to Ao
964  const Vec3 p_BS_A =
965  expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA); //30
966  return V_AB[1] + (V_AB[0] % p_BS_A); //12 flops
967 }
968 
969 
977 Vec3 findStationAccelerationInGround
978  (const State& state, const Vec3& stationOnB) const {
979  const Vec3& w = getBodyAngularVelocity(state); // in G
980  const Vec3& b = getBodyAngularAcceleration(state); // in G
981  const Vec3& a = getBodyOriginAcceleration(state); // in G
982 
983  const Vec3 r = expressVectorInGroundFrame(state,stationOnB); // 15 flops
984  return a + b % r + w % (w % r); // 33 flops
985 }
986 
993  const Vec3& stationOnBodyB,
994  const MobilizedBody& inBodyA) const
995 {
996  const Vec3 w_AB =
997  findBodyAngularVelocityInAnotherBody(state,inBodyA); // 18 flops
998  const SpatialVec A_AB =
999  findBodyAccelerationInAnotherBody(state,inBodyA); // 105 flops
1000  // Bo->S rexpressed in A but not shifted to Ao
1001  const Vec3 p_BS_A =
1002  expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA); // 30
1003 
1004  return A_AB[1] + (A_AB[0] % p_BS_A) + w_AB % (w_AB % p_BS_A); // 33 flops
1005 }
1006 
1010 void findStationLocationAndVelocityInGround
1011  (const State& state, const Vec3& locationOnB,
1012  Vec3& locationOnGround, Vec3& velocityInGround) const
1013 {
1014  const Vec3& p_GB = getBodyOriginLocation(state);
1015  const Vec3 p_BS_G = expressVectorInGroundFrame(state,locationOnB);//15flops
1016  locationOnGround = p_GB + p_BS_G; // 3flops
1017 
1018  const Vec3& w_GB = getBodyAngularVelocity(state);
1019  const Vec3& v_GB = getBodyOriginVelocity(state);
1020  velocityInGround = v_GB + w_GB % p_BS_G; // 12 flops
1021 }
1022 
1023 
1028 void findStationLocationVelocityAndAccelerationInGround
1029  (const State& state, const Vec3& locationOnB,
1030  Vec3& locationOnGround, Vec3& velocityInGround, Vec3& accelerationInGround)
1031  const
1032 {
1033  const Rotation& R_GB = getBodyRotation(state);
1034  const Vec3& p_GB = getBodyOriginLocation(state);
1035 
1036  // re-express station vector p_BS in G
1037  const Vec3 r = R_GB*locationOnB; // 15 flops
1038  locationOnGround = p_GB + r; // 3 flops
1039 
1040  const Vec3& w = getBodyAngularVelocity(state); // in G
1041  const Vec3& v = getBodyOriginVelocity(state); // in G
1042  const Vec3& b = getBodyAngularAcceleration(state); // in G
1043  const Vec3& a = getBodyOriginAcceleration(state); // in G
1044 
1045  const Vec3 wXr = w % r; // "whipping" velocity w X r due to ang vel; 9 flops
1046  velocityInGround = v + wXr; // v + w X r (3 flops)
1047  accelerationInGround = a + b % r + w % wXr; // 24 flops
1048 }
1049 
1053  return findStationLocationInGround(state,getBodyMassCenterStation(state));
1054 }
1055 
1059 Vec3 findMassCenterLocationInAnotherBody
1060  (const State& state, const MobilizedBody& toBodyA) const {
1061  return findStationLocationInAnotherBody(state,
1062  getBodyMassCenterStation(state),toBodyA);
1063 }
1064 
1071 Vec3 findStationAtGroundPoint
1072  (const State& state, const Vec3& locationInG) const {
1073  return ~getBodyTransform(state) * locationInG;
1074 }
1075 
1081 Vec3 findStationAtAnotherBodyStation
1082  (const State& state, const MobilizedBody& fromBodyA,
1083  const Vec3& stationOnA) const {
1084  return fromBodyA.findStationLocationInAnotherBody(state, stationOnA, *this);
1085 }
1086 
1090 Vec3 findStationAtAnotherBodyOrigin
1091  (const State& state, const MobilizedBody& fromBodyA) const {
1092  return findStationAtGroundPoint(state,
1093  fromBodyA.getBodyOriginLocation(state));
1094 }
1095 
1099 Vec3 findStationAtAnotherBodyMassCenter
1100  (const State& state, const MobilizedBody& fromBodyA) const {
1101  return fromBodyA.findStationLocationInAnotherBody(state,
1102  getBodyMassCenterStation(state),*this);
1103 }
1104 
1108 Transform findFrameTransformInGround
1109  (const State& state, const Transform& frameOnB) const {
1110  return getBodyTransform(state) * frameOnB;
1111 }
1112 
1118 SpatialVec findFrameVelocityInGround
1119  (const State& state, const Transform& frameOnB) const {
1120  return SpatialVec(getBodyAngularVelocity(state),
1121  findStationVelocityInGround(state,frameOnB.p()));
1122 }
1123 
1129 SpatialVec findFrameAccelerationInGround
1130  (const State& state, const Transform& frameOnB) const {
1131  return SpatialVec(getBodyAngularAcceleration(state),
1132  findStationAccelerationInGround(state,frameOnB.p()));
1133 }
1134 
1138 Vec3 expressVectorInGroundFrame
1139  (const State& state, const Vec3& vectorInB) const {
1140  return getBodyRotation(state)*vectorInB;
1141 }
1142 
1147 Vec3 expressGroundVectorInBodyFrame
1148  (const State& state, const Vec3& vectorInG) const {
1149  return ~getBodyRotation(state)*vectorInG;
1150 }
1151 
1157 Vec3 expressVectorInAnotherBodyFrame
1158  (const State& state, const Vec3& vectorInB,
1159  const MobilizedBody& inBodyA) const
1160 {
1161  return inBodyA.expressGroundVectorInBodyFrame(state,
1162  expressVectorInGroundFrame(state,vectorInB));
1163 }
1164 
1170  const MassProperties& M_Bo_B = getBodyMassProperties(state);
1171  const Rotation& R_GB = getBodyRotation(state);
1172  return M_Bo_B.reexpress(~R_GB);
1173 }
1174 
1179 MassProperties expressMassPropertiesInAnotherBodyFrame
1180  (const State& state, const MobilizedBody& inBodyA) const {
1181  const MassProperties& M_Bo_B = getBodyMassProperties(state);
1182  const Rotation R_AB = findBodyRotationInAnotherBody(state,inBodyA);
1183  return M_Bo_B.reexpress(~R_AB);
1184 }
1185 
1186 // End of Basic Operators.
1189 //------------------------------------------------------------------------------
1213 {
1214  if (isGround())
1215  return SpatialMat(Mat33(Infinity)); // sets diagonals to Inf
1216 
1217  const MassProperties& mp = getBodyMassProperties(state);
1218  const Rotation& R_GB = getBodyRotation(state);
1219  // re-express in Ground without shifting, convert to spatial mat.
1220  return mp.reexpress(~R_GB).toSpatialMat();
1221 }
1222 
1226 
1229  MobilizedBodyIndex objectBodyB) const
1230 {
1231  return getBodyMassProperties(state).calcCentralInertia();
1232 }
1233 
1237 Inertia calcBodyInertiaAboutAnotherBodyStation
1238  (const State& state, const MobilizedBody& inBodyA,
1239  const Vec3& aboutLocationOnBodyA) const
1240 {
1241  // get B's mass props MB, measured about Bo, exp. in B
1242  const MassProperties& MB_Bo_B = getBodyMassProperties(state);
1243 
1244  // Calculate the vector from the body B origin (current "about" point) to
1245  // the new "about" point PA, expressed in B.
1246  const Vec3 p_Bo_PA =
1247  findStationAtAnotherBodyStation(state, inBodyA, aboutLocationOnBodyA);
1248 
1249  // Now shift the "about" point for body B's inertia IB to PA, but still
1250  // expressed in B.
1251  const Inertia IB_PA_B = MB_Bo_B.calcShiftedInertia(p_Bo_PA);
1252 
1253  // Finally reexpress the inertia in the A frame.
1254  const Rotation R_BA =
1255  inBodyA.findBodyRotationInAnotherBody(state, *this);
1256  const Inertia IB_PA_A = IB_PA_B.reexpress(R_BA);
1257  return IB_PA_A;
1258 }
1259 
1260 
1264  const MassProperties M_Bo_G = expressMassPropertiesInGroundFrame(state);
1265  const SpatialVec& V_GB = getBodyVelocity(state);
1266  return M_Bo_G.toSpatialMat() * V_GB;
1267 }
1268 
1271 SpatialVec calcBodyMomentumAboutBodyMassCenterInGround
1272  (const State& state) const {
1273  const MassProperties& M_Bo_B = getBodyMassProperties(state);
1274  const Rotation& R_GB = getBodyRotation(state);
1275 
1276  // Given a central inertia matrix I, angular velocity w, and mass center
1277  // velocity v, the central angular momentum is Iw and linear momentum is mv.
1278  const Inertia I_Bc_B = M_Bo_B.calcCentralInertia();
1279  const Inertia I_Bc_G = I_Bc_B.reexpress(~R_GB);
1280  const Real mb = M_Bo_B.getMass();
1281  const Vec3& w_GB = getBodyAngularVelocity(state);
1282  Vec3 v_GBc =
1283  findStationVelocityInGround(state, M_Bo_B.getMassCenter());
1284 
1285  return SpatialVec( I_Bc_G*w_GB, mb*v_GBc );
1286 }
1287 
1292  const Vec3& locationOnBodyB,
1293  const MobilizedBody& bodyA,
1294  const Vec3& locationOnBodyA) const
1295 {
1296  if (isSameMobilizedBody(bodyA))
1297  return (locationOnBodyA-locationOnBodyB).norm();
1298 
1299  const Vec3 r_Go_PB =
1300  this->findStationLocationInGround(state,locationOnBodyB);
1301  const Vec3 r_Go_PA =
1302  bodyA.findStationLocationInGround(state,locationOnBodyA);
1303  return (r_Go_PA - r_Go_PB).norm();
1304 }
1305 
1311 Real calcStationToStationDistanceTimeDerivative
1312  (const State& state,
1313  const Vec3& locationOnBodyB,
1314  const MobilizedBody& bodyA,
1315  const Vec3& locationOnBodyA) const
1316 {
1317  if (isSameMobilizedBody(bodyA))
1318  return 0;
1319 
1320  Vec3 rB, rA, vB, vA;
1321  this->findStationLocationAndVelocityInGround(state,locationOnBodyB,rB,vB);
1322  bodyA.findStationLocationAndVelocityInGround(state,locationOnBodyA,rA,vA);
1323  const Vec3 r = rA-rB, v = vA-vB;
1324  const Real d = r.norm();
1325 
1326  // When the points are coincident, the rate of change of distance is just
1327  // their relative speed. Otherwise, it is the speed along the direction of
1328  // separation.
1329  if (d==0) return v.norm();
1330  else return dot(v, r/d);
1331 }
1332 
1333 
1339 Real calcStationToStationDistance2ndTimeDerivative
1340  (const State& state,
1341  const Vec3& locationOnBodyB,
1342  const MobilizedBody& bodyA,
1343  const Vec3& locationOnBodyA) const
1344 {
1345  if (isSameMobilizedBody(bodyA))
1346  return 0;
1347 
1348  Vec3 rB, rA, vB, vA, aB, aA;
1349  this->findStationLocationVelocityAndAccelerationInGround
1350  (state,locationOnBodyB,rB,vB,aB);
1352  (state,locationOnBodyA,rA,vA,aA);
1353 
1354  const Vec3 r = rA-rB, v = vA-vB, a = aA-aB;
1355  const Real d = r.norm();
1356 
1357  // This method is the time derivative of
1358  // calcFixedPointToPointDistanceTimeDerivative(), so it must follow the same
1359  // two cases. That is, when the points are coincident the change in
1360  // separation rate is the time derivative of the speed |v|, otherwise it is
1361  // the time derivative of the speed along the separation vector.
1362 
1363  if (d==0) {
1364  // Return d/dt |v|. This has two cases: if |v| is zero, the rate of
1365  // change of speed is just the points' relative acceleration magnitude.
1366  // Otherwise, it is the acceleration in the direction of the current
1367  // relative velocity vector.
1368  const Real s = v.norm(); // speed
1369  if (s==0) return a.norm();
1370  else return dot(a, v/s);
1371  }
1372 
1373  // Points are separated.
1374  const Vec3 u = r/d; // u is separation direction (a unit vector from B to A)
1375  const Vec3 vp = v - dot(v,u)*u; // velocity perp. to separation direction
1376  return dot(a,u) + dot(vp,v)/d;
1377 }
1378 
1379 
1382 Vec3 calcBodyMovingPointVelocityInBody
1383  (const State& state,
1384  const Vec3& locationOnBodyB,
1385  const Vec3& velocityOnBodyB,
1386  const MobilizedBody& inBodyA) const
1387 {
1388  SimTK_ASSERT_ALWAYS(!"unimplemented method",
1389  "MobilizedBody::calcBodyMovingPointVelocityInBody()"
1390  " is not yet implemented -- any volunteers?");
1391  return Vec3::getNaN();
1392 }
1393 
1394 
1398 Vec3 calcBodyMovingPointAccelerationInBody
1399  (const State& state,
1400  const Vec3& locationOnBodyB,
1401  const Vec3& velocityOnBodyB,
1402  const Vec3& accelerationOnBodyB,
1403  const MobilizedBody& inBodyA) const
1404 {
1405  SimTK_ASSERT_ALWAYS(!"unimplemented method",
1406  "MobilizedBody::calcBodyMovingPointAccelerationInBody()"
1407  " is not yet implemented -- any volunteers?");
1408  return Vec3::getNaN();
1409 }
1410 
1417 Real calcMovingPointToPointDistanceTimeDerivative
1418  (const State& state,
1419  const Vec3& locationOnBodyB,
1420  const Vec3& velocityOnBodyB,
1421  const MobilizedBody& bodyA,
1422  const Vec3& locationOnBodyA,
1423  const Vec3& velocityOnBodyA) const
1424 {
1425  SimTK_ASSERT_ALWAYS(!"unimplemented method",
1426  "MobilizedBody::calcMovingPointToPointDistanceTimeDerivative()"
1427  " is not yet implemented -- any volunteers?");
1428  return NaN;
1429 }
1430 
1438 Real calcMovingPointToPointDistance2ndTimeDerivative
1439  (const State& state,
1440  const Vec3& locationOnBodyB,
1441  const Vec3& velocityOnBodyB,
1442  const Vec3& accelerationOnBodyB,
1443  const MobilizedBody& bodyA,
1444  const Vec3& locationOnBodyA,
1445  const Vec3& velocityOnBodyA,
1446  const Vec3& accelerationOnBodyA) const
1447 {
1448  SimTK_ASSERT_ALWAYS(!"unimplemented method",
1449  "MobilizedBody::calcMovingPointToPointDistance2ndTimeDerivative()"
1450  " is not yet implemented -- any volunteers?");
1451  return NaN;
1452 }
1453 
1454 
1455 // End of High Level Operators.
1459  // CONSTRUCTION METHODS //
1462 
1466 
1468 explicit MobilizedBody(MobilizedBodyImpl* r);
1469 
1470 //------------------------------------------------------------------------------
1480 const Body& getBody() const;
1481 
1486 Body& updBody();
1487 
1493 MobilizedBody& setBody(const Body&);
1494 
1504 int addBodyDecoration(const Transform& X_BD,
1505  const DecorativeGeometry& geometry) {
1506  return updBody().addDecoration(X_BD, geometry);
1507 }
1511  return updBody().addDecoration(geometry);
1512 }
1513 
1520 int addOutboardDecoration(const Transform& X_MD,
1521  const DecorativeGeometry& geometry);
1523 int getNumOutboardDecorations() const;
1525 const DecorativeGeometry& getOutboardDecoration(int i) const;
1527 DecorativeGeometry& updOutboardDecoration(int i);
1528 
1535 int addInboardDecoration(const Transform& X_FD,
1536  const DecorativeGeometry& geometry);
1538 int getNumInboardDecorations() const;
1540 const DecorativeGeometry& getInboardDecoration(int i) const;
1542 DecorativeGeometry& updInboardDecoration(int i);
1543 
1551  updBody().setDefaultRigidBodyMassProperties(m); // might not be allowed
1552  return *this;
1553 }
1554 
1557  // every body type can do this
1558  return getBody().getDefaultRigidBodyMassProperties();
1559 }
1560 
1568 void adoptMotion(Motion& ownerHandle);
1569 
1573 void clearMotion();
1574 
1578 bool hasMotion() const;
1579 
1586 const Motion& getMotion() const;
1587 
1593 MobilizedBody& setDefaultInboardFrame (const Transform& X_PF);
1599 MobilizedBody& setDefaultOutboardFrame(const Transform& X_BM);
1600 
1605 const Transform& getDefaultInboardFrame() const; // X_PF
1609 const Transform& getDefaultOutboardFrame() const; // X_BM
1610 
1615 operator MobilizedBodyIndex() const {return getMobilizedBodyIndex();}
1616 
1622 MobilizedBodyIndex getMobilizedBodyIndex() const;
1623 
1627 const MobilizedBody& getParentMobilizedBody() const;
1628 
1633 const MobilizedBody& getBaseMobilizedBody() const;
1634 
1638 const SimbodyMatterSubsystem& getMatterSubsystem() const;
1642 SimbodyMatterSubsystem& updMatterSubsystem();
1643 
1646 bool isInSubsystem() const;
1647 
1651 bool isInSameSubsystem(const MobilizedBody& mobod) const;
1652 
1657 bool isSameMobilizedBody(const MobilizedBody& mobod) const;
1658 
1661 bool isGround() const;
1662 
1667 int getLevelInMultibodyTree() const;
1668 
1672 MobilizedBody& cloneForNewParent(MobilizedBody& parent) const;
1673 
1674 
1675  // Utility operators //
1676 
1680 Real getOneFromQPartition
1681  (const State& state, int which, const Vector& qlike) const;
1682 
1687 Real& updOneFromQPartition
1688  (const State& state, int which, Vector& qlike) const;
1689 
1693 Real getOneFromUPartition
1694  (const State& state, int which, const Vector& ulike) const;
1695 
1700 Real& updOneFromUPartition(const State& state, int which, Vector& ulike) const;
1701 
1707 void applyOneMobilityForce(const State& state, int which, Real f,
1708  Vector& mobilityForces) const
1709 {
1710  updOneFromUPartition(state,which,mobilityForces) += f;
1711 }
1712 
1749 void convertQForceToUForce(const State& state,
1750  const Array_<Real,MobilizerQIndex>& fq,
1751  Array_<Real,MobilizerUIndex>& fu) const;
1752 
1759 void applyBodyForce(const State& state, const SpatialVec& spatialForceInG,
1760  Vector_<SpatialVec>& bodyForcesInG) const;
1761 
1767 void applyBodyTorque(const State& state, const Vec3& torqueInG,
1768  Vector_<SpatialVec>& bodyForcesInG) const;
1769 
1780 void applyForceToBodyPoint
1781  (const State& state, const Vec3& pointInB, const Vec3& forceInG,
1782  Vector_<SpatialVec>& bodyForcesInG) const;
1783 
1784 // End of Construction and Misc Methods.
1787  // BUILT IN MOBILIZER DECLARATIONS //
1790 
1791 //------------------------------------------------------------------------------
1792 // These are the built-in MobilizedBody types. Each of these has a known
1793 // number of coordinates and speeds (at least a default number) so
1794 // can define routines which return and accept specific-size arguments, e.g.
1795 // Real (for 1-dof mobilizer) and Vec5 (for 5-dof mobilizer). Here is the
1796 // conventional interface that each built-in should provide. The base type
1797 // provides similar routines but using variable-sized or "one at a time"
1798 // arguments. (Vec<1> here will actually be a Real; assume the built-in
1799 // MobilizedBody class is "BuiltIn")
1800 //
1801 // BuiltIn& setDefaultQ(const Vec<nq>&);
1802 // const Vec<nq>& getDefaultQ() const;
1803 //
1804 // const Vec<nq>& getQ[Dot[Dot]](const State&) const;
1805 // const Vec<nu>& getU[Dot](const State&) const;
1806 //
1807 // void setQ(State&, const Vec<nq>&) const;
1808 // void setU(State&, const Vec<nu>&) const;
1809 //
1810 // const Vec<nq>& getMyPartQ(const State&, const Vector& qlike) const;
1811 // const Vec<nu>& getMyPartU(const State&, const Vector& ulike) const;
1812 //
1813 // Vec<nq>& updMyPartQ(const State&, Vector& qlike) const;
1814 // Vec<nu>& updMyPartU(const State&, Vector& ulike) const;
1815 //
1816 // Each built in mobilized body type is declared in its own header
1817 // file using naming convention MobilizedBody_Pin.h, for example. All the
1818 // built-in headers are collected in MobilizedBody_BuiltIns.h; you should
1819 // include new ones there also.
1820 
1821 
1822 class Pin;
1823 typedef Pin Torsion;
1824 typedef Pin Revolute;
1825 
1826 class Universal;
1827 class Cylinder;
1828 class Weld;
1829 
1830 class Slider;
1831 typedef Slider Prismatic;
1832 
1834 typedef Translation Cartesian;
1836 
1838 typedef BendStretch PolarCoords;
1839 
1840 class SphericalCoords;
1841 class LineOrientation;
1842 
1843 class Planar;
1844 class Gimbal;
1845 class Bushing;
1846 
1847 class Ball;
1848 typedef Ball Orientation;
1849 typedef Ball Spherical;
1850 
1851 class Free;
1852 class FreeLine;
1853 class Screw;
1854 class Ellipsoid;
1855 class Custom;
1856 class Ground;
1857 class FunctionBased;
1858 
1859 // Internal use only.
1860 class PinImpl;
1861 class SliderImpl;
1862 class UniversalImpl;
1863 class CylinderImpl;
1864 class BendStretchImpl;
1865 class PlanarImpl;
1866 class GimbalImpl;
1867 class BushingImpl;
1868 class BallImpl;
1869 class TranslationImpl;
1870 class SphericalCoordsImpl;
1871 class FreeImpl;
1872 class LineOrientationImpl;
1873 class FreeLineImpl;
1874 class WeldImpl;
1875 class ScrewImpl;
1876 class EllipsoidImpl;
1877 class CustomImpl;
1878 class GroundImpl;
1879 class FunctionBasedImpl;
1880 };
1881 
1882 } // namespace SimTK
1883 
1884 #endif // SimTK_SIMBODY_MOBILIZED_BODY_H_
1885 
1886 
1887 
SpatialMat calcBodySpatialInertiaMatrixInGround(const State &state) const
Return the mass properties of body B, measured from and about the B origin Bo, but expressed in Groun...
Definition: MobilizedBody.h:1212
Three translational mobilities describing the Cartesian motion of a point.
Definition: MobilizedBody_Translation.h:38
const Vec3 & getBodyAngularAcceleration(const State &state) const
Extract from the state cache the already-calculated inertial angular acceleration vector b_GB of this...
Definition: MobilizedBody.h:383
Vec< 2, Vec3 > SpatialVec
Spatial vectors are used for (rotation,translation) quantities and consist of a pair of Vec3 objects...
Definition: MassProperties.h:50
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
static Vec< M, ELT, 1 > getNaN()
Return a Vec of the same length and element type as this one but with all elements set to NaN...
Definition: Vec.h:915
const Vec< 3, P > & getMassCenter() const
Return the mass center currently stored in this MassProperties object; this is expressed in an implic...
Definition: MassProperties.h:1406
Vec3 findStationVelocityInAnotherBody(const State &state, const Vec3 &stationOnBodyB, const MobilizedBody &inBodyA) const
Return the velocity of a station S fixed on body B, in body A&#39;s frame, expressed in body A...
Definition: MobilizedBody.h:957
SpatialVec findBodyAccelerationInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the angular and linear acceleration of body B&#39;s frame in body A&#39;s frame, expressed in body A...
Definition: MobilizedBody.h:816
Three mobilities – z rotation and x,y translation.
Definition: MobilizedBody_Planar.h:38
This is a special type of "mobilized" body generated automatically by Simbody as a placeholder for Gr...
Definition: MobilizedBody_Ground.h:45
we know q, u, and udot
Definition: Motion.h:116
MobilizedBody Mobod
Mobod is the approved abbreviation for MobilizedBody. Feel free to use it if you get tired of typing ...
Definition: MobilizedBody.h:63
Five mobilities, representing unrestricted motion for a body which is inertialess along its own z axi...
Definition: MobilizedBody_FreeLine.h:58
const Transform & getBodyTransform(const State &state) const
Extract from the state cache the already-calculated spatial configuration X_GB of body B&#39;s body frame...
The Body class represents a reference frame that can be used to describe mass properties and geometry...
Definition: Body.h:55
Unique integer type for Subsystem-local u indexing.
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
One mobility – coordinated rotation and translation along the common z axis of the inboard and outbo...
Definition: MobilizedBody_Screw.h:39
Two mobilities, representing unrestricted orientation for a body which is inertialess along its own z...
Definition: MobilizedBody_LineOrientation.h:59
Inertia_< P > calcCentralInertia() const
Return the inertia of this MassProperties object, but measured about the mass center rather than abou...
Definition: MassProperties.h:1427
MassProperties_ reexpress(const Rotation_< P > &R_BC) const
Re-express these mass properties from the current frame "B" to a new frame "C", given the orientation...
Definition: MassProperties.h:1475
const Vec3 & getBodyMassCenterStation(const State &state) const
Return this body&#39;s center of mass station (i.e., the vector fixed in the body, going from body origin...
Definition: MobilizedBody.h:424
#define SimTK_ASSERT_ALWAYS(cond, msg)
Definition: ExceptionMacros.h:349
int addDecoration(const Transform &X_BD, const DecorativeGeometry &geometry)
Add a piece of decorative geometry fixed at some pose on this Body.
This defines the API for the Body base class and concrete Body types like Body::Rigid that are derive...
A Motion object belongs to a particular MobilizedBody and prescribes how the associated motion is to ...
Definition: Motion.h:107
Slider Prismatic
Synonym for Slider mobilizer.
Definition: MobilizedBody.h:1830
Two mobilities – rotation and translation along the common z axis of the inboard and outboard mobili...
Definition: MobilizedBody_Cylinder.h:42
Real calcStationToStationDistance(const State &state, const Vec3 &locationOnBodyB, const MobilizedBody &bodyA, const Vec3 &locationOnBodyA) const
Calculate the distance from a station PB on body B to a station PA on body A.
Definition: MobilizedBody.h:1291
const Vec3 & getBodyOriginVelocity(const State &state) const
Extract from the state cache the already-calculated inertial linear velocity vector v_GB (more explic...
Definition: MobilizedBody.h:361
const Rotation_< P > & R() const
Return a read-only reference to the contained rotation R_BF.
Definition: Transform.h:215
Three mobilities – coordinated rotation and translation along the surface of an ellipsoid fixed to t...
Definition: MobilizedBody_Ellipsoid.h:45
Translation Cartesian
Synonym for Translation mobilizer.
Definition: MobilizedBody.h:1833
Vec3 findStationLocationInGround(const State &state, const Vec3 &stationOnB) const
Return the Cartesian (ground) location that is currently coincident with a station (point) S fixed on...
Definition: MobilizedBody.h:916
const Vec3 & getBodyAngularVelocity(const State &state) const
Extract from the state cache the already-calculated inertial angular velocity vector w_GB of this bod...
Definition: MobilizedBody.h:354
Every Simbody header and source file should include this header before any other Simbody header...
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
Ball Spherical
Synonym for Ball mobilizer.
Definition: MobilizedBody.h:1849
Inertia_ reexpress(const Rotation_< P > &R_FB) const
Return a new inertia matrix like this one but re-expressed in another frame (leaving the origin point...
Definition: MassProperties.h:371
const P & getMass() const
Return the mass currently stored in this MassProperties object.
Definition: MassProperties.h:1401
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:610
MassProperties expressMassPropertiesInGroundFrame(const State &state) const
Re-express this body B&#39;s mass properties in Ground by applying only a rotation, not a shift of refere...
Definition: MobilizedBody.h:1169
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
const SpatialVec & getBodyAcceleration(const State &state) const
Extract from the state cache the already-calculated spatial acceleration A_GB of this body&#39;s referenc...
Vec3 findStationAtGroundPoint(const State &state, const Vec3 &locationInG) const
Return the station (point) S of this body B that is coincident with the given Ground location...
Definition: MobilizedBody.h:1072
Vec3 findMassCenterLocationInGround(const State &state) const
Return the Cartesian (ground) location of this body B&#39;s mass center.
Definition: MobilizedBody.h:1052
Vec3 expressGroundVectorInBodyFrame(const State &state, const Vec3 &vectorInG) const
Re-express a vector expressed in Ground into the same vector expressed in this body B...
Definition: MobilizedBody.h:1148
int addBodyDecoration(const Transform &X_BD, const DecorativeGeometry &geometry)
Convenience method to add DecorativeGeometry specified relative to the new (outboard) body&#39;s referenc...
Definition: MobilizedBody.h:1504
Mat< 2, 2, Mat< 3, 3, P > > toSpatialMat() const
Convert this MassProperties object to a spatial inertia matrix and return it as a SpatialMat...
Definition: MassProperties.h:1526
Three mobilities – unrestricted orientation modeled as a 1-2-3 body-fixed Euler angle sequence...
Definition: MobilizedBody_Gimbal.h:69
Vec3 findStationLocationInAnotherBody(const State &state, const Vec3 &stationOnB, const MobilizedBody &toBodyA) const
Given a station S on this body B, return the location on another body A which is at the same location...
Definition: MobilizedBody.h:930
void applyOneMobilityForce(const State &state, int which, Real f, Vector &mobilityForces) const
This utility adds in the supplied generalized force f (a scalar) to the appropriate slot of the suppl...
Definition: MobilizedBody.h:1707
const UnitInertia_< P > & getUnitInertia() const
Return the unit inertia currently stored in this MassProperties object; this is expressed in an impli...
Definition: MassProperties.h:1411
Level
What is the highest level of motion that is driven? Lower levels are also driven; higher levels are d...
Definition: Motion.h:112
const MassProperties & getDefaultMassProperties() const
Return the mass properties of the Body stored within this MobilizedBody.
Definition: MobilizedBody.h:1556
Vec3 findBodyOriginAccelerationInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the acceleration of body B&#39;s origin point in body A&#39;s frame, expressed in body A...
Definition: MobilizedBody.h:863
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:53
Pin Torsion
Synonym for Pin mobilizer.
Definition: MobilizedBody.h:1822
SpatialVec calcBodyMomentumAboutBodyOriginInGround(const State &state)
Calculate body B&#39;s momentum (angular, linear) measured and expressed in Ground, but taken about the b...
Definition: MobilizedBody.h:1263
Inertia calcBodyCentralInertia(const State &state, MobilizedBodyIndex objectBodyB) const
Return the central inertia for body B, that is, the inertia taken about body B&#39;s mass center Bc...
Definition: MobilizedBody.h:1228
Real getBodyMass(const State &state) const
Return the mass of this body.
Definition: MobilizedBody.h:417
const UnitInertia & getBodyUnitInertiaAboutBodyOrigin(const State &state) const
Return a reference to this body&#39;s unit inertia matrix in the State cache, taken about the body origin...
Definition: MobilizedBody.h:431
Three mobilities – unrestricted orientation modeled with a quaternion which is never singular...
Definition: MobilizedBody_Ball.h:44
Mat< 3, 3 > Mat33
This is the most common 3x3 matrix type: three packed columns of 3 Real values each.
Definition: SmallMatrix.h:142
int addBodyDecoration(const DecorativeGeometry &geometry)
Convenience method for use when the geometry is supplied in the body frame.
Definition: MobilizedBody.h:1510
MobilizedBody()
The default constructor provides an empty MobilizedBody handle that can be assigned to reference any ...
Definition: MobilizedBody.h:1465
Unique integer type for Subsystem-local q indexing.
Pin Revolute
Synonym for Pin mobilizer.
Definition: MobilizedBody.h:1824
Provides one rotational mobility about the common z axis of the F and M frames of the mobilizer...
Definition: MobilizedBody_Pin.h:46
void findStationLocationAndVelocityInGround(const State &state, const Vec3 &locationOnB, Vec3 &locationOnGround, Vec3 &velocityInGround) const
It is cheaper to calculate a station&#39;s ground location and velocity together than to do them separate...
Definition: MobilizedBody.h:1011
Body & setDefaultRigidBodyMassProperties(const MassProperties &)
Every type of Body should provide an initial set of rigid body mass properties defined at Topology st...
invalid level
Definition: Motion.h:113
One mobility – translation along the common x axis of the F (inboard) and M (outboard) mobilizer fra...
Definition: MobilizedBody_Slider.h:46
Direction
Constructors can take an argument of this type to indicate that the mobilizer is being defined in the...
Definition: MobilizedBody.h:181
This is a subclass of MobilizedBody::Custom which uses a set of Function objects to define the behavi...
Definition: MobilizedBody_FunctionBased.h:46
Mat< 2, 2, Mat33 > SpatialMat
Spatial matrices are used to hold 6x6 matrices that are best viewed as 2x2 matrices of 3x3 matrices; ...
Definition: MassProperties.h:72
This defines the Motion class, which is used to specify how the mobilities associated with a particul...
const Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
The physical meaning of an inertia is the distribution of a rigid body&#39;s mass about a particular poin...
Definition: MassProperties.h:82
This is the client-side interface to an implementation-independent representation of "Decorations" su...
Definition: DecorativeGeometry.h:86
CNT< typename CNT< E1 >::THerm >::template Result< E2 >::Mul dot(const Vec< M, E1, S1 > &r, const Vec< M, E2, S2 > &v)
Definition: SmallMatrixMixed.h:86
bool isLockedByDefault() const
Check whether this mobilizer is to be locked in the default state.
Definition: MobilizedBody.h:286
A UnitInertia matrix is a unit-mass inertia matrix; you can convert it to an Inertia by multiplying i...
Definition: MassProperties.h:81
bool isLocked(const State &state) const
Check whether this mobilizer is currently locked in the given state.
Definition: MobilizedBody.h:263
Rotation findBodyRotationInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return R_AB, the rotation matrix giving this body B&#39;s axes in body A&#39;s frame.
Definition: MobilizedBody.h:731
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
Two mobilities – rotation about the x axis, followed by a rotation about the new y axis...
Definition: MobilizedBody_Universal.h:40
A MobilizedBody is Simbody&#39;s fundamental body-and-joint object used to parameterize a system&#39;s motion...
Definition: MobilizedBody.h:168
Transform findBodyTransformInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return X_AB, the spatial transform giving this body B&#39;s frame in body A&#39;s frame.
Definition: MobilizedBody.h:717
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83
Vec3 findBodyOriginVelocityInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the velocity of body B&#39;s origin point in body A&#39;s frame, expressed in body A...
Definition: MobilizedBody.h:803
Ball Orientation
Synonym for Ball mobilizer.
Definition: MobilizedBody.h:1847
Vec3 findBodyAngularVelocityInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the angular velocity w_AB of body B&#39;s frame in body A&#39;s frame, expressed in body A...
Definition: MobilizedBody.h:786
The handle class MobilizedBody::Custom (dataless) and its companion class MobilizedBody::Custom::Impl...
Definition: MobilizedBody_Custom.h:72
Unrestricted motion for a rigid body (six mobilities).
Definition: MobilizedBody_Free.h:52
const Vec3 & getBodyOriginAcceleration(const State &state) const
Extract from the state cache the already-calculated inertial linear acceleration vector a_GB (more ex...
Definition: MobilizedBody.h:391
const SpatialVec & getBodyVelocity(const State &state) const
Extract from the state cache the already-calculated spatial velocity V_GB of this body&#39;s reference fr...
const MassProperties & getDefaultRigidBodyMassProperties() const
Get the default (that is, Topology stage) mass properties for this Body.
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:620
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
SpatialVec findRelativeAcceleration(const Transform &X_FA, const SpatialVec &V_FA, const SpatialVec &A_FA, const Transform &X_FB, const SpatialVec &V_FB, const SpatialVec &A_FB)
Find the relative spatial acceleration between two frames A and B whose individual spatial accelerati...
Definition: SpatialAlgebra.h:245
Zero mobilities.
Definition: MobilizedBody_Weld.h:43
void findStationLocationVelocityAndAccelerationInGround(const State &state, const Vec3 &locationOnB, Vec3 &locationOnGround, Vec3 &velocityInGround, Vec3 &accelerationInGround) const
It is cheaper to calculate a station&#39;s ground location, velocity, and acceleration together than to d...
Definition: MobilizedBody.h:1029
Method
There are several ways to specify the motion at this Level, and the selected method also determines l...
Definition: Motion.h:126
const Rotation & getBodyRotation(const State &state) const
Extract from the state cache the already-calculated spatial orientation R_GB of body B&#39;s body frame x...
Definition: MobilizedBody.h:326
BendStretch PolarCoords
Synonym for BendStretch mobilizer.
Definition: MobilizedBody.h:1837
Inertia_< P > calcShiftedInertia(const Vec< 3, P > &newOriginB) const
Return the inertia of this MassProperties object, but with the "measured about" point shifted from th...
Definition: MassProperties.h:1434
const Vec< 3, P > & p() const
Return a read-only reference to our translation vector p_BF.
Definition: Transform.h:239
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
Three mobilities – body fixed 3-2 (z-y) rotation followed by translation along body z or body x...
Definition: MobilizedBody_SphericalCoords.h:77
Vec3 findStationAccelerationInAnotherBody(const State &state, const Vec3 &stationOnBodyB, const MobilizedBody &inBodyA) const
Return the acceleration of a station S fixed on body B, in another body A&#39;s frame, expressed in body A.
Definition: MobilizedBody.h:992
Two mobilities: The z axis of the parent&#39;s F frame is used for rotation (and that is always aligned w...
Definition: MobilizedBody_BendStretch.h:42
const Vec3 & getBodyOriginLocation(const State &state) const
Extract from the state cache the already-calculated spatial location of body B&#39;s body frame origin Bo...
Definition: MobilizedBody.h:333
const SpatialVec & getMobilizerAcceleration(const State &state) const
TODO: Not implemented yet – any volunteers? At stage Acceleration, return the cross-mobilizer accele...
Definition: MobilizedBody.h:400
MobilizedBody & setDefaultMassProperties(const MassProperties &m)
If the contained Body can have its mass properties set to the supplied value m its mass properties ar...
Definition: MobilizedBody.h:1550
Translation CartesianCoords
Synonym for Translation mobilizer.
Definition: MobilizedBody.h:1835
Six mobilities – arbitrary relative motion modeled as x-y-z translation followed by an x-y-z body-fi...
Definition: MobilizedBody_Bushing.h:86