Simbody  3.8
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 
284 
286 bool isLockedByDefault() const
287 { return getLockByDefaultLevel()!=Motion::NoLevel; }
288 
295  // 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 
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 
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 
747  (const State& state, const MobilizedBody& toBodyA) const {
748  return toBodyA.findStationAtGroundPoint(state,
749  getBodyOriginLocation(state));
750 }
751 
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 
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 
879 
887  (const State& state) const;
888 
898 
907  (const State& state) const;
908 
916  (const State& state, const Vec3& stationOnB) const {
917  return getBodyTransform(state) * stationOnB;
918 }
919 
920 
930  (const State& state, const Vec3& stationOnB, const MobilizedBody& toBodyA)
931  const
932 {
933  return toBodyA.findStationAtGroundPoint(state,
934  findStationLocationInGround(state,stationOnB));
935 }
936 
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 
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 
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 
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 
1060  (const State& state, const MobilizedBody& toBodyA) const {
1061  return findStationLocationInAnotherBody(state,
1062  getBodyMassCenterStation(state),toBodyA);
1063 }
1064 
1072  (const State& state, const Vec3& locationInG) const {
1073  return ~getBodyTransform(state) * locationInG;
1074 }
1075 
1082  (const State& state, const MobilizedBody& fromBodyA,
1083  const Vec3& stationOnA) const {
1084  return fromBodyA.findStationLocationInAnotherBody(state, stationOnA, *this);
1085 }
1086 
1091  (const State& state, const MobilizedBody& fromBodyA) const {
1092  return findStationAtGroundPoint(state,
1093  fromBodyA.getBodyOriginLocation(state));
1094 }
1095 
1100  (const State& state, const MobilizedBody& fromBodyA) const {
1101  return fromBodyA.findStationLocationInAnotherBody(state,
1102  getBodyMassCenterStation(state),*this);
1103 }
1104 
1109  (const State& state, const Transform& frameOnB) const {
1110  return getBodyTransform(state) * frameOnB;
1111 }
1112 
1119  (const State& state, const Transform& frameOnB) const {
1120  return SpatialVec(getBodyAngularVelocity(state),
1121  findStationVelocityInGround(state,frameOnB.p()));
1122 }
1123 
1130  (const State& state, const Transform& frameOnB) const {
1131  return SpatialVec(getBodyAngularAcceleration(state),
1132  findStationAccelerationInGround(state,frameOnB.p()));
1133 }
1134 
1139  (const State& state, const Vec3& vectorInB) const {
1140  return getBodyRotation(state)*vectorInB;
1141 }
1142 
1148  (const State& state, const Vec3& vectorInG) const {
1149  return ~getBodyRotation(state)*vectorInG;
1150 }
1151 
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 
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 
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 
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 
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 
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 
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 
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 
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 
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 
1487 
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 
1521  const DecorativeGeometry& geometry);
1528 
1536  const DecorativeGeometry& geometry);
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 
1574 
1578 bool hasMotion() const;
1579 
1586 const Motion& getMotion() const;
1587 
1600 
1605 const Transform& getDefaultInboardFrame() const; // X_PF
1609 const Transform& getDefaultOutboardFrame() const; // X_BM
1610 
1615 operator MobilizedBodyIndex() const {return getMobilizedBodyIndex();}
1616 
1623 
1628 
1634 
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 
1668 
1673 
1674 
1675  // Utility operators //
1676 
1681  (const State& state, int which, const Vector& qlike) const;
1682 
1688  (const State& state, int which, Vector& qlike) const;
1689 
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 
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;
1832 
1833 class Translation;
1836 
1837 class BendStretch;
1839 
1840 class SphericalCoords;
1841 class LineOrientation;
1842 
1843 class Planar;
1844 class Gimbal;
1845 class Bushing;
1846 
1847 class Ball;
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 
This defines the API for the Body base class and concrete Body types like Body::Rigid that are derive...
#define SimTK_ASSERT_ALWAYS(cond, msg)
Definition: ExceptionMacros.h:349
This defines the Motion class, which is used to specify how the mobilities associated with a particul...
Every Simbody header and source file should include this header before any other Simbody header.
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:1520
The Body class represents a reference frame that can be used to describe mass properties and geometry...
Definition: Body.h:55
This is the client-side interface to an implementation-independent representation of "Decorations" su...
Definition: DecorativeGeometry.h:86
The physical meaning of an inertia is the distribution of a rigid body's mass about a particular poin...
Definition: MassProperties.h:193
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
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B.
Definition: MassProperties.h:1363
const P & getMass() const
Return the mass currently stored in this MassProperties object.
Definition: MassProperties.h:1401
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
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
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
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
Inertia_< P > calcCentralInertia() const
Return the inertia of this MassProperties object, but measured about the mass center rather than abou...
Definition: MassProperties.h:1427
This class represents a small matrix whose size is known at compile time, containing elements of any ...
Definition: Mat.h:97
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
Three mobilities – unrestricted orientation modeled with a quaternion which is never singular.
Definition: MobilizedBody_Ball.h:44
Two mobilities: The z axis of the parent's F frame is used for rotation (and that is always aligned w...
Definition: MobilizedBody_BendStretch.h:42
Six mobilities – arbitrary relative motion modeled as x-y-z translation followed by an x-y-z body-fix...
Definition: MobilizedBody_Bushing.h:86
The handle class MobilizedBody::Custom (dataless) and its companion class MobilizedBody::Custom::Impl...
Definition: MobilizedBody_Custom.h:72
Two mobilities – rotation and translation along the common z axis of the inboard and outboard mobiliz...
Definition: MobilizedBody_Cylinder.h:42
Three mobilities – coordinated rotation and translation along the surface of an ellipsoid fixed to th...
Definition: MobilizedBody_Ellipsoid.h:45
Five mobilities, representing unrestricted motion for a body which is inertialess along its own z axi...
Definition: MobilizedBody_FreeLine.h:58
Unrestricted motion for a rigid body (six mobilities).
Definition: MobilizedBody_Free.h:52
This is a subclass of MobilizedBody::Custom which uses a set of Function objects to define the behavi...
Definition: MobilizedBody_FunctionBased.h:47
Three mobilities – unrestricted orientation modeled as a 1-2-3 body-fixed Euler angle sequence,...
Definition: MobilizedBody_Gimbal.h:69
This is a special type of "mobilized" body generated automatically by Simbody as a placeholder for Gr...
Definition: MobilizedBody_Ground.h:45
Two mobilities, representing unrestricted orientation for a body which is inertialess along its own z...
Definition: MobilizedBody_LineOrientation.h:59
Provides one rotational mobility about the common z axis of the F and M frames of the mobilizer.
Definition: MobilizedBody_Pin.h:46
Three mobilities – z rotation and x,y translation.
Definition: MobilizedBody_Planar.h:38
One mobility – coordinated rotation and translation along the common z axis of the inboard and outboa...
Definition: MobilizedBody_Screw.h:39
One mobility – translation along the common x axis of the F (inboard) and M (outboard) mobilizer fram...
Definition: MobilizedBody_Slider.h:46
Three mobilities – body fixed 3-2 (z-y) rotation followed by translation along body z or body x.
Definition: MobilizedBody_SphericalCoords.h:77
Three translational mobilities describing the Cartesian motion of a point.
Definition: MobilizedBody_Translation.h:38
Two mobilities – rotation about the x axis, followed by a rotation about the new y axis.
Definition: MobilizedBody_Universal.h:40
Zero mobilities.
Definition: MobilizedBody_Weld.h:43
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:169
int getNumU(const State &state) const
Return the number of generalized speeds u currently in use by this mobilizer.
const Rotation & getBodyRotation(const State &state) const
Extract from the state cache the already-calculated spatial orientation R_GB of body B's body frame x...
Definition: MobilizedBody.h:326
int addBodyDecoration(const Transform &X_BD, const DecorativeGeometry &geometry)
Convenience method to add DecorativeGeometry specified relative to the new (outboard) body's referenc...
Definition: MobilizedBody.h:1504
Vec3 findBodyOriginVelocityInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the velocity of body B's origin point in body A's frame, expressed in body A.
Definition: MobilizedBody.h:803
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
const SpatialVec & getMobilizerAcceleration(const State &state) const
TODO: Not implemented yet – any volunteers? At stage Acceleration, return the cross-mobilizer acceler...
Definition: MobilizedBody.h:400
Slider Prismatic
Synonym for Slider mobilizer.
Definition: MobilizedBody.h:1830
int getLevelInMultibodyTree() const
Return this mobilized body's level in the tree of bodies, starting with Ground at 0,...
SpatialVec findMobilizerReactionOnBodyAtOriginInGround(const State &state) const
Return the spatial reaction force (moment and force) applied by the mobilizer to body B but shifted t...
SpatialVec findBodyAccelerationInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the angular and linear acceleration of body B's frame in body A's frame, expressed in body A,...
Definition: MobilizedBody.h:816
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
const Transform & getMobilizerTransform(const State &state) const
At stage Position or higher, return the cross-mobilizer transform X_FM, the body's inboard mobilizer ...
Motion::Method getUDotMotionMethod(const State &state) const
Determine how generalized acceleration udot values are being determined.
const Vec3 & getBodyOriginLocation(const State &state) const
Extract from the state cache the already-calculated spatial location of body B's body frame origin Bo...
Definition: MobilizedBody.h:333
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's frame, expressed in body A.
Definition: MobilizedBody.h:957
const SpatialVec & getBodyAcceleration(const State &state) const
Extract from the state cache the already-calculated spatial acceleration A_GB of this body's referenc...
void applyBodyTorque(const State &state, const Vec3 &torqueInG, Vector_< SpatialVec > &bodyForcesInG) const
This utility adds in the supplied pure torque torqueInG to the appropriate slot of the supplied bodyF...
Vec3 calcBodyMovingPointAccelerationInBody(const State &state, const Vec3 &locationOnBodyB, const Vec3 &velocityOnBodyB, const Vec3 &accelerationOnBodyB, const MobilizedBody &inBodyA) const
TODO: not implemented yet – any volunteers? Return the velocity of a point P moving (and possibly acc...
Definition: MobilizedBody.h:1399
SpatialVec calcBodyMomentumAboutBodyMassCenterInGround(const State &state) const
Calculate body B's momentum (angular, linear) measured and expressed in Ground, but taken about the b...
Definition: MobilizedBody.h:1272
int addBodyDecoration(const DecorativeGeometry &geometry)
Convenience method for use when the geometry is supplied in the body frame.
Definition: MobilizedBody.h:1510
const DecorativeGeometry & getInboardDecoration(int i) const
Return a const reference to the i'th inboard decoration.
Ball Orientation
Synonym for Ball mobilizer.
Definition: MobilizedBody.h:1847
void lockAt(State &state, const Vector &value, Motion::Level level=Motion::Position) const
Lock this mobilizer's q, u, or udot to the given Vector value, depending on level.
const SpatialInertia & getBodySpatialInertiaInGround(const State &state) const
Return a reference to the already-calculated SpatialInertia of this body, taken about the body's orig...
DecorativeGeometry & updInboardDecoration(int i)
Return a writable reference to the i'th inboard decoration.
Vector getLockValueAsVector(const State &state) const
Return the q, u, or udot value at which this mobilizer is locked, depending on the lock level,...
Inertia calcBodyCentralInertia(const State &state, MobilizedBodyIndex objectBodyB) const
Return the central inertia for body B, that is, the inertia taken about body B's mass center Bc,...
Definition: MobilizedBody.h:1228
void applyBodyForce(const State &state, const SpatialVec &spatialForceInG, Vector_< SpatialVec > &bodyForcesInG) const
This utility adds in the supplied spatial force spatialForceInG (consisting of a torque vector,...
SpatialVec getH_FMCol(const State &state, MobilizerUIndex ux) const
Expert use only: obtain a column of the mobilizer-local hinge matrix H_FM which maps generalized spee...
MassProperties expressMassPropertiesInAnotherBodyFrame(const State &state, const MobilizedBody &inBodyA) const
Re-express this body B's mass properties in another body A's frame by applying only a rotation,...
Definition: MobilizedBody.h:1180
Translation Cartesian
Synonym for Translation mobilizer.
Definition: MobilizedBody.h:1833
int addOutboardDecoration(const Transform &X_MD, const DecorativeGeometry &geometry)
Add decorative geometry specified relative to the outboard mobilizer frame M attached to body B,...
Translation CartesianCoords
Synonym for Translation mobilizer.
Definition: MobilizedBody.h:1835
bool isLocked(const State &state) const
Check whether this mobilizer is currently locked in the given state.
Definition: MobilizedBody.h:263
bool isInSubsystem() const
Determine whether the current MobilizedBody object is owned by a matter subsystem.
Real getOneQDot(const State &state, int which) const
Return one of the generalized coordinate derivatives qdot from this mobilizer's partition of the matt...
Real getOneQDotDot(const State &state, int which) const
Return one of the generalized coordinate second derivatives qdotdot from this mobilizer's partition o...
Real calcMovingPointToPointDistance2ndTimeDerivative(const State &state, const Vec3 &locationOnBodyB, const Vec3 &velocityOnBodyB, const Vec3 &accelerationOnBodyB, const MobilizedBody &bodyA, const Vec3 &locationOnBodyA, const Vec3 &velocityOnBodyA, const Vec3 &accelerationOnBodyA) const
TODO: not implemented yet – any volunteers? Calculate the second time derivative of distance from a m...
Definition: MobilizedBody.h:1439
Pin Torsion
Synonym for Pin mobilizer.
Definition: MobilizedBody.h:1822
SpatialVec getHCol(const State &state, MobilizerUIndex ux) const
Expert use only: obtain a column of the hinge matrix H corresponding to one of this mobilizer's mobil...
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
void setInboardFrame(State &state, const Transform &X_PF) const
TODO: not implemented yet.
void setQToFitTransform(State &state, const Transform &X_FM) const
Adjust this mobilizer's q's to best approximate the supplied Transform which requests a particular re...
const SpatialVec & getMobilizerVelocity(const State &state) const
At stage Velocity or higher, return the cross-mobilizer velocity V_FM, the relative velocity of this ...
void findStationLocationVelocityAndAccelerationInGround(const State &state, const Vec3 &locationOnB, Vec3 &locationOnGround, Vec3 &velocityInGround, Vec3 &accelerationInGround) const
It is cheaper to calculate a station's ground location, velocity, and acceleration together than to d...
Definition: MobilizedBody.h:1029
const Vec3 & getBodyMassCenterStation(const State &state) const
Return this body's center of mass station (i.e., the vector fixed in the body, going from body origin...
Definition: MobilizedBody.h:424
const Motion & getMotion() const
If there is a Motion object associated with this MobilizedBody, this returns a const reference to it.
Direction
Constructors can take an argument of this type to indicate that the mobilizer is being defined in the...
Definition: MobilizedBody.h:181
void lock(State &state, Motion::Level level=Motion::Position) const
Lock this mobilizer's position or velocity at its current value, or lock the acceleration to zero,...
const MobilizedBody & getBaseMobilizedBody() const
Return a reference to this MobilizedBody's oldest ancestor other than Ground, or return Ground if thi...
Real calcMovingPointToPointDistanceTimeDerivative(const State &state, const Vec3 &locationOnBodyB, const Vec3 &velocityOnBodyB, const MobilizedBody &bodyA, const Vec3 &locationOnBodyA, const Vec3 &velocityOnBodyA) const
TODO: not implemented yet – any volunteers? Calculate the time rate of change of distance from a movi...
Definition: MobilizedBody.h:1418
Motion::Method getUMotionMethod(const State &state) const
Determine how generalized speed u values are being determined.
Motion::Method getQMotionMethod(const State &state) const
Determine how generalized coordinate q values are being determined.
SpatialVec findMobilizerReactionOnParentAtOriginInGround(const State &state) const
Return the spatial reaction force (moment and force) applied by the mobilizer to the parent (inboard)...
Vec3 findStationAtAnotherBodyStation(const State &state, const MobilizedBody &fromBodyA, const Vec3 &stationOnA) const
Return the station (point) on this body B that is coincident with the given station on another body A...
Definition: MobilizedBody.h:1082
void findStationLocationAndVelocityInGround(const State &state, const Vec3 &locationOnB, Vec3 &locationOnGround, Vec3 &velocityInGround) const
It is cheaper to calculate a station's ground location and velocity together than to do them separate...
Definition: MobilizedBody.h:1011
const UnitInertia & getBodyUnitInertiaAboutBodyOrigin(const State &state) const
Return a reference to this body's unit inertia matrix in the State cache, taken about the body origin...
Definition: MobilizedBody.h:431
Vector getQAsVector(const State &state) const
Return as a Vector of length getNumQ() all the generalized coordinates q currently in use by this mob...
MobilizedBody()
The default constructor provides an empty MobilizedBody handle that can be assigned to reference any ...
Definition: MobilizedBody.h:1465
Rotation findBodyRotationInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return R_AB, the rotation matrix giving this body B's axes in body A's frame.
Definition: MobilizedBody.h:731
Vec3 findBodyOriginAccelerationInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the acceleration of body B's origin point in body A's frame, expressed in body A.
Definition: MobilizedBody.h:863
const Transform & getDefaultInboardFrame() const
Return a reference to this mobilizer's default for the frame F fixed on the parent (inboard) body P,...
void setUToFitAngularVelocity(State &state, const Vec3 &w_FM) const
Adjust this mobilizer's u's (generalized speeds) to best approximate the supplied angular velocity w_...
bool isInSameSubsystem(const MobilizedBody &mobod) const
Determine whether a given MobilizedBody mobod is in the same matter subsystem as the current body.
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's frame,...
Definition: MobilizedBody.h:992
const SimbodyMatterSubsystem & getMatterSubsystem() const
Obtain a reference to the SimbodyMatterSubsystem which contains this MobilizedBody.
Real calcStationToStationDistanceTimeDerivative(const State &state, const Vec3 &locationOnBodyB, const MobilizedBody &bodyA, const Vec3 &locationOnBodyA) const
Calculate the time rate of change of distance from a fixed point PB on body B to a fixed point PA on ...
Definition: MobilizedBody.h:1312
const MassProperties & getBodyMassProperties(const State &state) const
Return a reference to this body's mass properties in the State cache.
Vec3 calcBodyMovingPointVelocityInBody(const State &state, const Vec3 &locationOnBodyB, const Vec3 &velocityOnBodyB, const MobilizedBody &inBodyA) const
TODO: not implemented yet – any volunteers? Return the velocity of a point P moving on body B,...
Definition: MobilizedBody.h:1383
MassProperties expressMassPropertiesInGroundFrame(const State &state) const
Re-express this body B's mass properties in Ground by applying only a rotation, not a shift of refere...
Definition: MobilizedBody.h:1169
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
MobilizedBody & setBody(const Body &)
Replace the Body contained within this MobilizedBody with a new one.
SpatialVec findMobilizerReactionOnBodyAtMInGround(const State &state) const
Return the spatial reaction force (moment and force) applied by the mobilizer to body B at the locati...
int getNumQ(const State &state) const
Return the number of generalized coordinates q currently in use by this mobilizer.
void applyForceToBodyPoint(const State &state, const Vec3 &pointInB, const Vec3 &forceInG, Vector_< SpatialVec > &bodyForcesInG) const
This utility adds in the supplied force forceInG applied at a point pointInB to the appropriate slot ...
BendStretch PolarCoords
Synonym for BendStretch mobilizer.
Definition: MobilizedBody.h:1837
SpatialVec calcBodyMomentumAboutBodyOriginInGround(const State &state) const
Calculate body B's momentum (angular, linear) measured and expressed in Ground, but taken about the b...
Definition: MobilizedBody.h:1263
const DecorativeGeometry & getOutboardDecoration(int i) const
Return a const reference to the i'th outboard decoration.
void lockAt(State &state, Real value, Motion::Level level=Motion::Position) const
Lock this mobilizer's q, u, or udot to the given scalar value, depending on level.
UIndex getFirstUIndex(const State &state) const
Return the global UIndex of the first u for this mobilizer; all the u's range from getFirstUIndex() t...
Motion::Level getLockLevel(const State &state) const
Returns the lock level if the mobilizer is locked in the given state, otherwise Motion::NoLevel.
void convertQForceToUForce(const State &state, const Array_< Real, MobilizerQIndex > &fq, Array_< Real, MobilizerUIndex > &fu) const
Given a generalized force in the q-space of this mobilizer, convert it to the equivalent generalized ...
void adoptMotion(Motion &ownerHandle)
Provide a unique Motion object for this MobilizedBody.
Real getOneU(const State &state, int which) const
Return one of the generalized speeds u from this mobilizer's partition of the matter subsystem's full...
Real getOneUDot(const State &state, int which) const
Return one of the generalized accelerations udot from this mobilizer's partition of the matter subsys...
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
Real & updOneFromQPartition(const State &state, int which, Vector &qlike) const
This utility returns a writable reference to one of the q's (generalized coordinates) associated with...
void setQToFitRotation(State &state, const Rotation &R_FM) const
Adjust this mobilizer's q's to best approximate the supplied Rotation matrix which requests a particu...
Inertia calcBodyInertiaAboutAnotherBodyStation(const State &state, const MobilizedBody &inBodyA, const Vec3 &aboutLocationOnBodyA) const
Return the inertia of this body B, taken about an arbitrary point PA of body A, and expressed in body...
Definition: MobilizedBody.h:1238
void setOneU(State &state, int which, Real v) const
Set one of the generalized speeds u to value v, in this mobilizer's partition of the matter subsystem...
Vec3 findStationAccelerationInGround(const State &state, const Vec3 &stationOnB) const
Given a station fixed on body B, return its inertial (Cartesian) acceleration, that is,...
Definition: MobilizedBody.h:978
Vector getUDotAsVector(const State &state) const
Return as a Vector of length getNumU() all the generalized accelerations udot currently in use by thi...
Vec3 findBodyOriginLocationInAnotherBody(const State &state, const MobilizedBody &toBodyA) const
Return the station on another body A (that is, a point measured and expressed in A) that is currently...
Definition: MobilizedBody.h:747
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
Real getBodyMass(const State &state) const
Return the mass of this body.
Definition: MobilizedBody.h:417
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
const Transform & getInboardFrame(const State &state) const
Return a reference to this mobilizer's frame F fixed on the parent body P, as the fixed Transform fro...
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
bool isLockedByDefault() const
Check whether this mobilizer is to be locked in the default state.
Definition: MobilizedBody.h:286
Motion::Level getLockByDefaultLevel() const
Returns the level at which the mobilizer is locked by default, if it is locked by default,...
Real getOneTau(const State &state, MobilizerUIndex which) const
Return one of the tau forces resulting from prescribed (known) acceleration, corresponding to one of ...
void setUFromVector(State &state, const Vector &v) const
Set all of the generalized speeds u to value v (a Vector of length getNumU()), in this mobilizer's pa...
Body & updBody()
Return a writable reference to the Body contained within this MobilizedBody.
Vector getTauAsVector(const State &state) const
Return the generalized forces tau resulting from prescribed (known) acceleration, corresponding to ea...
Vec3 expressVectorInGroundFrame(const State &state, const Vec3 &vectorInB) const
Re-express a vector expressed in this body B's frame into the same vector in G, by applying only a ro...
Definition: MobilizedBody.h:1139
bool isSameMobilizedBody(const MobilizedBody &mobod) const
Determine whether a given MobilizedBody mobod is the same MobilizedBody as this one.
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
MobilizedBody & setDefaultInboardFrame(const Transform &X_PF)
Change this mobilizer's frame F on the parent body P.
Transform findBodyTransformInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return X_AB, the spatial transform giving this body B's frame in body A's frame.
Definition: MobilizedBody.h:717
SimbodyMatterSubsystem & updMatterSubsystem()
Obtain a writable reference to the SimbodyMatterSubsystem which contains this MobilizedBody.
MobilizedBody Mobod
Mobod is the approved abbreviation for MobilizedBody. Feel free to use it if you get tired of typing ...
Definition: MobilizedBody.h:63
Vec3 findStationAtAnotherBodyOrigin(const State &state, const MobilizedBody &fromBodyA) const
Return the station S of this body that is currently coincident in space with the origin Ao of another...
Definition: MobilizedBody.h:1091
Ball Spherical
Synonym for Ball mobilizer.
Definition: MobilizedBody.h:1849
MobilizedBody & setDefaultOutboardFrame(const Transform &X_BM)
Change this mobilizer's frame M fixed on this (the outboard) body B.
int getNumInboardDecorations() const
Return the count of decorations added with addInboardDecoration().
SpatialVec findFrameVelocityInGround(const State &state, const Transform &frameOnB) const
Return the current Ground-frame spatial velocity V_GF (that is, angular and linear velocity) of a fra...
Definition: MobilizedBody.h:1119
int addInboardDecoration(const Transform &X_FD, const DecorativeGeometry &geometry)
Add decorative geometry specified relative to the inboard mobilizer frame F attached to the parent bo...
void setQFromVector(State &state, const Vector &v) const
Set all of the generalized coordinates q to value v (a Vector of length getNumQ()),...
const Transform & getOutboardFrame(const State &state) const
Return a reference to this MobilizedBody's mobilizer frame M, as the fixed Transform from this body B...
const SpatialVec & getBodyVelocity(const State &state) const
Extract from the state cache the already-calculated spatial velocity V_GB of this body's reference fr...
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
Real calcStationToStationDistance2ndTimeDerivative(const State &state, const Vec3 &locationOnBodyB, const MobilizedBody &bodyA, const Vec3 &locationOnBodyA) const
Calculate the second time derivative of distance from a fixed point PB on body B to a fixed point PA ...
Definition: MobilizedBody.h:1340
MobilizedBodyIndex getMobilizedBodyIndex() const
Return the MobilizedBodyIndex of this MobilizedBody within the owning SimbodyMatterSubsystem.
bool hasMotion() const
Check whether this MobilizedBody has an associated Motion object.
Vec3 findMassCenterLocationInGround(const State &state) const
Return the Cartesian (ground) location of this body B's mass center.
Definition: MobilizedBody.h:1052
DecorativeGeometry & updOutboardDecoration(int i)
Return a writable reference to the i'th outboard decoration.
int getNumOutboardDecorations() const
Return the count of decorations added with addOutboardDecoration().
Vec3 findBodyAngularVelocityInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the angular velocity w_AB of body B's frame in body A's frame, expressed in body A.
Definition: MobilizedBody.h:786
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
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
Vec3 expressVectorInAnotherBodyFrame(const State &state, const Vec3 &vectorInB, const MobilizedBody &inBodyA) const
Re-express a vector expressed in this body B into the same vector expressed in body A,...
Definition: MobilizedBody.h:1158
Real getOneFromQPartition(const State &state, int which, const Vector &qlike) const
This utility selects one of the q's (generalized coordinates) associated with this mobilizer from a s...
MobilizedBody & lockByDefault(Motion::Level level=Motion::Position)
Change whether this mobilizer is initially locked.
Real getOneFromUPartition(const State &state, int which, const Vector &ulike) const
This utility selects one of the u's (generalized speeds) associated with this mobilizer from a suppli...
SpatialVec findFrameAccelerationInGround(const State &state, const Transform &frameOnB) const
Return the current Ground-frame spatial acceleration A_GF (that is, angular and linear acceleration) ...
Definition: MobilizedBody.h:1130
Real getOneQ(const State &state, int which) const
Return one of the generalized coordinates q from this mobilizer's partition of the matter subsystem's...
void lockAt(State &state, const Vec< N > &value, Motion::Level level=Motion::Position) const
Lock this mobilizer's q, u, or udot to the given Vec<N> value, depending on the level.
void setUToFitLinearVelocity(State &state, const Vec3 &v_FM) const
Adjust any of this mobilizer's u's (generalized speeds) to best approximate the supplied linear veloc...
Vector getQDotAsVector(const State &state) const
Return as a Vector of length getNumQ() all the generalized coordinate derivatives qdot currently in u...
Vec3 findMassCenterLocationInAnotherBody(const State &state, const MobilizedBody &toBodyA) const
Return the point of another body A that is currently coincident in space with the mass center CB of t...
Definition: MobilizedBody.h:1060
MobilizedBody(MobilizedBodyImpl *r)
Internal use only.
const MobilizedBody & getParentMobilizedBody() const
Return a reference to the MobilizedBody serving as the parent body of the current MobilizedBody.
void clearMotion()
If there is a Motion object associated with this MobilizedBody it is removed; otherwise,...
void unlock(State &state) const
Unlock this mobilizer, returning it to its normal behavior which may be free motion or may be control...
Vec3 findStationAtAnotherBodyMassCenter(const State &state, const MobilizedBody &fromBodyA) const
Return the station S of this body that is currently coincident in space with the mass center Ac of an...
Definition: MobilizedBody.h:1100
void setOneQ(State &state, int which, Real v) const
Set one of the generalized coordinates q to value v, in this mobilizer's partition of the matter subs...
SpatialVec findBodyVelocityInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the angular and linear velocity of body B's frame in body A's frame, expressed in body A,...
Definition: MobilizedBody.h:758
SpatialVec findMobilizerReactionOnParentAtFInGround(const State &state) const
Return the spatial reaction force (moment and force) applied by the mobilizer to the parent (inboard)...
const Body & getBody() const
Return a const reference to the Body contained within this MobilizedBody.
const MassProperties & getDefaultMassProperties() const
Return the mass properties of the Body stored within this MobilizedBody.
Definition: MobilizedBody.h:1556
Pin Revolute
Synonym for Pin mobilizer.
Definition: MobilizedBody.h:1824
Real & updOneFromUPartition(const State &state, int which, Vector &ulike) const
This utility returns a writable reference to one of the u's (generalized speeds) associated with this...
Vec3 findBodyAngularAccelerationInAnotherBody(const State &state, const MobilizedBody &inBodyA) const
Return the angular acceleration of body B's frame in body A's frame, expressed in body A.
Definition: MobilizedBody.h:836
const Transform & getDefaultOutboardFrame() const
Return a reference to this MobilizedBody's default for mobilizer frame M, as the fixed Transform from...
Vector getQDotDotAsVector(const State &state) const
Return as a Vector of length getNumQ() all the generalized coordinate second derivatives qdotdot curr...
const Transform & getBodyTransform(const State &state) const
Extract from the state cache the already-calculated spatial configuration X_GB of body B's body frame...
Transform findFrameTransformInGround(const State &state, const Transform &frameOnB) const
Return the current Ground-frame pose (position and orientation) of a frame F that is fixed to body B.
Definition: MobilizedBody.h:1109
Vector getUAsVector(const State &state) const
Return as a Vector of length getNumU() all the generalized speeds u currently in use by this mobilize...
Vec3 findStationVelocityInGround(const State &state, const Vec3 &stationOnB) const
Given a station fixed on body B, return its inertial (Cartesian) velocity, that is,...
Definition: MobilizedBody.h:944
void setQToFitTranslation(State &state, const Vec3 &p_FM) const
Adjust this mobilizer's q's to best approximate the supplied position vector which requests a particu...
bool isGround() const
Determine whether this MobilizedBody is Ground, meaning that it is actually body 0 of some matter sub...
MobilizedBody & cloneForNewParent(MobilizedBody &parent) const
Create a new MobilizedBody which is identical to this one, except that it has a different parent (and...
QIndex getFirstQIndex(const State &state) const
Return the global QIndex of the first q for this mobilizer; all the q's range from getFirstQIndex() t...
void setUToFitVelocity(State &state, const SpatialVec &V_FM) const
Adjust this mobilizer's u's (generalized speeds) to best approximate the supplied spatial velocity V_...
void setOutboardFrame(State &state, const Transform &X_BM) const
TODO: not implemented yet.
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
A Motion object belongs to a particular MobilizedBody and prescribes how the associated motion is to ...
Definition: Motion.h:107
Method
There are several ways to specify the motion at this Level, and the selected method also determines l...
Definition: Motion.h:126
Level
What is the highest level of motion that is driven? Lower levels are also driven; higher levels are d...
Definition: Motion.h:112
@ NoLevel
invalid level
Definition: Motion.h:113
@ Position
we know q, u, and udot
Definition: Motion.h:116
Unique integer type for Subsystem-local q indexing.
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body.
Definition: MassProperties.h:993
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
const Vec< 3, P > & p() const
Return a read-only reference to our translation vector p_BF.
Definition: Transform.h:239
const Rotation_< P > & R() const
Return a read-only reference to the contained rotation R_BF.
Definition: Transform.h:215
Unique integer type for Subsystem-local u indexing.
A UnitInertia matrix is a unit-mass inertia matrix; you can convert it to an Inertia by multiplying i...
Definition: MassProperties.h:669
CNT< ScalarNormSq >::TSqrt norm() const
Definition: Vec.h:610
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
Mat< 3, 3 > Mat33
This is the most common 3x3 matrix type: three packed columns of 3 Real values each.
Definition: SmallMatrix.h:142
Vec< 2, Vec3 > SpatialVec
Spatial vectors are used for (rotation,translation) quantities and consist of a pair of Vec3 objects,...
Definition: MassProperties.h:50
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
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
const Real Infinity
This is the IEEE positive infinity constant for this implementation of the default-precision Real typ...
const Real NaN
This is the IEEE "not a number" constant for this implementation of the default-precision Real type; ...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
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:607
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