Simbody 3.7
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"
43
44#include <cassert>
45
46namespace SimTK {
47
48class SimbodyMatterSubsystem;
49class Motion;
50class MobilizedBody;
51class 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//==============================================================================
170public:
171
182 Forward = 0,
183 Reverse = 1
185
186//------------------------------------------------------------------------------
220void lock(State& state, Motion::Level level=Motion::Position) const;
221
230void lockAt(State& state, Real value,
231 Motion::Level level=Motion::Position) const;
232
241void lockAt(State& state, const Vector& value,
242 Motion::Level level=Motion::Position) const;
243
253template <int N> SimTK_SIMBODY_EXPORT // instantiated in library
254void lockAt(State& state, const Vec<N>& value,
255 Motion::Level level=Motion::Position) const;
256
260void unlock(State& state) const;
261
263bool isLocked(const State& state) const
264{ return getLockLevel(state)!=Motion::NoLevel; }
265
268Motion::Level getLockLevel(const State& state) const;
269
274
284
286bool isLockedByDefault() const
287{ return getLockByDefaultLevel()!=Motion::NoLevel; }
288
295 // STATE ACCESS METHODS //
297
298//------------------------------------------------------------------------------
316const Transform& getBodyTransform(const State& state) const; // X_GB
317
326const Rotation& getBodyRotation(const State& state) const {
327 return getBodyTransform(state).R();
328}
333const Vec3& getBodyOriginLocation(const State& state) const {
334 return getBodyTransform(state).p();
335}
336
340const Transform& getMobilizerTransform(const State& state) const; // X_FM
341
348const SpatialVec& getBodyVelocity(const State& state) const; // V_GB
349
354const Vec3& getBodyAngularVelocity(const State& state) const { // w_GB
355 return getBodyVelocity(state)[0];
356}
361const Vec3& getBodyOriginVelocity(const State& state) const { // v_GB
362 return getBodyVelocity(state)[1];
363}
364
369const SpatialVec& getMobilizerVelocity(const State& state) const; // V_FM
370
377const SpatialVec& getBodyAcceleration(const State& state) const; // A_GB
378
383const Vec3& getBodyAngularAcceleration(const State& state) const { // b_GB
384 return getBodyAcceleration(state)[0];
385}
386
391const Vec3& getBodyOriginAcceleration(const State& state) const { // a_GB
392 return getBodyAcceleration(state)[1];
393}
394
400const 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
408const MassProperties& getBodyMassProperties(const State& state) const;
409
414
417Real getBodyMass(const State& state) const {
418 return getBodyMassProperties(state).getMass();
419}
420
424const Vec3& getBodyMassCenterStation(const State& state) const {
425 return getBodyMassProperties(state).getMassCenter();
426}
427
432 return getBodyMassProperties(state).getUnitInertia();
433}
434
440const Transform& getInboardFrame (const State& state) const; // X_PF
446const Transform& getOutboardFrame(const State& state) const; // X_BM
447
451void setInboardFrame (State& state, const Transform& X_PF) const;
455void setOutboardFrame(State& state, const Transform& X_BM) const;
456
457// End of State Access - Bodies
461//------------------------------------------------------------------------------
468int getNumQ(const State& state) const;
471int getNumU(const State& state) const;
472
475QIndex getFirstQIndex(const State& state) const;
476
479UIndex getFirstUIndex(const State& state) const;
480
490
495Real getOneQ(const State& state, int which) const;
496
500Real getOneU(const State& state, int which) const;
501
505Vector getQAsVector(const State& state) const;
509Vector getUAsVector(const State& state) const;
510
515Real getOneQDot (const State& state, int which) const;
519Vector getQDotAsVector(const State& state) const;
520
525Real getOneUDot(const State& state, int which) const;
530Real getOneQDotDot(const State& state, int which) const;
534Vector getUDotAsVector(const State& state) const;
539Vector getQDotDotAsVector(const State& state) const;
540
550Vector getTauAsVector(const State& state) const;
551
556Real getOneTau(const State& state, MobilizerUIndex which) const;
557
562void setOneQ(State& state, int which, Real v) const;
567void setOneU(State& state, int which, Real v) const;
568
572void setQFromVector(State& state, const Vector& v) const;
576void setUFromVector(State& state, const Vector& v) const;
577
611void setQToFitTransform(State& state, const Transform& X_FM) const;
612
617void setQToFitRotation(State& state, const Rotation& R_FM) const;
618
624void setQToFitTranslation(State& state, const Vec3& p_FM) const;
625
632void setUToFitVelocity(State& state, const SpatialVec& V_FM) const;
633
638void setUToFitAngularVelocity(State& state, const Vec3& w_FM) const;
639
645void setUToFitLinearVelocity(State& state, const Vec3& v_FM) const;
646
656SpatialVec getHCol(const State& state, MobilizerUIndex ux) const;
657
665
666// End of State Access Methods.
669
670 // 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
1460 // CONSTRUCTION METHODS //
1462
1466
1468explicit MobilizedBody(MobilizedBodyImpl* r);
1469
1470//------------------------------------------------------------------------------
1480const Body& getBody() const;
1481
1487
1494
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
1568void adoptMotion(Motion& ownerHandle);
1569
1574
1578bool hasMotion() const;
1579
1586const Motion& getMotion() const;
1587
1600
1605const Transform& getDefaultInboardFrame() const; // X_PF
1609const Transform& getDefaultOutboardFrame() const; // X_BM
1610
1615operator MobilizedBodyIndex() const {return getMobilizedBodyIndex();}
1616
1623
1628
1634
1643
1646bool isInSubsystem() const;
1647
1651bool isInSameSubsystem(const MobilizedBody& mobod) const;
1652
1657bool isSameMobilizedBody(const MobilizedBody& mobod) const;
1658
1661bool 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
1700Real& updOneFromUPartition(const State& state, int which, Vector& ulike) const;
1701
1707void applyOneMobilityForce(const State& state, int which, Real f,
1708 Vector& mobilityForces) const
1709{
1710 updOneFromUPartition(state,which,mobilityForces) += f;
1711}
1712
1752
1759void applyBodyForce(const State& state, const SpatialVec& spatialForceInG,
1760 Vector_<SpatialVec>& bodyForcesInG) const;
1761
1767void 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
1788 // 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
1822class Pin;
1823typedef Pin Torsion;
1824typedef Pin Revolute;
1825
1826class Universal;
1827class Cylinder;
1828class Weld;
1829
1830class Slider;
1832
1833class Translation;
1836
1837class BendStretch;
1839
1840class SphericalCoords;
1841class LineOrientation;
1842
1843class Planar;
1844class Gimbal;
1845class Bushing;
1846
1847class Ball;
1850
1851class Free;
1852class FreeLine;
1853class Screw;
1854class Ellipsoid;
1855class Custom;
1856class Ground;
1857class FunctionBased;
1858
1859// Internal use only.
1860class PinImpl;
1861class SliderImpl;
1862class UniversalImpl;
1863class CylinderImpl;
1864class BendStretchImpl;
1865class PlanarImpl;
1866class GimbalImpl;
1867class BushingImpl;
1868class BallImpl;
1869class TranslationImpl;
1870class SphericalCoordsImpl;
1871class FreeImpl;
1872class LineOrientationImpl;
1873class FreeLineImpl;
1874class WeldImpl;
1875class ScrewImpl;
1876class EllipsoidImpl;
1877class CustomImpl;
1878class GroundImpl;
1879class 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
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
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
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 P & getMass() const
Return the mass currently stored in this MassProperties object.
Definition: MassProperties.h:1401
Inertia_< P > calcCentralInertia() const
Return the inertia of this MassProperties object, but measured about the mass center rather than abou...
Definition: MassProperties.h:1427
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
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.
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
const SpatialInertia & getBodySpatialInertiaInGround(const State &state) const
Return a reference to the already-calculated SpatialInertia of this body, taken about the body's orig...
SimbodyMatterSubsystem & updMatterSubsystem()
Obtain a writable reference to the SimbodyMatterSubsystem which contains this MobilizedBody.
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 MobilizedBody & getBaseMobilizedBody() const
Return a reference to this MobilizedBody's oldest ancestor other than Ground, or return Ground if thi...
MobilizedBody & cloneForNewParent(MobilizedBody &parent) const
Create a new MobilizedBody which is identical to this one, except that it has a different parent (and...
Slider Prismatic
Synonym for Slider mobilizer.
Definition: MobilizedBody.h:1831
const DecorativeGeometry & getOutboardDecoration(int i) const
Return a const reference to the i'th outboard decoration.
int getLevelInMultibodyTree() const
Return this mobilized body's level in the tree of bodies, starting with Ground at 0,...
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...
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 & getDefaultOutboardFrame() const
Return a reference to this MobilizedBody's default for mobilizer frame M, as the fixed Transform from...
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...
Motion::Method getUDotMotionMethod(const State &state) const
Determine how generalized acceleration udot values are being determined.
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
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
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 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
Ball Orientation
Synonym for Ball mobilizer.
Definition: MobilizedBody.h:1848
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.
Vector getLockValueAsVector(const State &state) const
Return the q, u, or udot value at which this mobilizer is locked, depending on the lock level,...
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...
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,...
DecorativeGeometry & updInboardDecoration(int i)
Return a writable reference to the i'th inboard decoration.
MobilizedBody & setDefaultOutboardFrame(const Transform &X_BM)
Change this mobilizer's frame M fixed on this (the outboard) body B.
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:1834
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
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...
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
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
Pin Torsion
Synonym for Pin mobilizer.
Definition: MobilizedBody.h:1823
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...
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 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
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,...
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
Vector getQAsVector(const State &state) const
Return as a Vector of length getNumQ() all the generalized coordinates q currently in use by this mob...
const DecorativeGeometry & getInboardDecoration(int i) const
Return a const reference to the i'th inboard decoration.
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
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
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
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_...
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...
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
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
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
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
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
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 SpatialVec & getBodyAcceleration(const State &state) const
Extract from the state cache the already-calculated spatial acceleration A_GB of this body's referenc...
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 ...
const SpatialVec & getMobilizerVelocity(const State &state) const
At stage Velocity or higher, return the cross-mobilizer velocity V_FM, the relative velocity of this ...
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
BendStretch PolarCoords
Synonym for BendStretch mobilizer.
Definition: MobilizedBody.h:1838
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
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.
const Transform & getDefaultInboardFrame() const
Return a reference to this mobilizer's default for the frame F fixed on the parent (inboard) body P,...
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...
const MobilizedBody & getParentMobilizedBody() const
Return a reference to the MobilizedBody serving as the parent body of the current MobilizedBody.
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
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...
DecorativeGeometry & updOutboardDecoration(int i)
Return a writable reference to the i'th outboard decoration.
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
const MassProperties & getDefaultMassProperties() const
Return the mass properties of the Body stored within this MobilizedBody.
Definition: MobilizedBody.h:1556
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
Real getBodyMass(const State &state) const
Return the mass of this body.
Definition: MobilizedBody.h:417
const Body & getBody() const
Return a const reference to the Body contained within this MobilizedBody.
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
Body & updBody()
Return a writable reference to the Body contained within this MobilizedBody.
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...
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
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
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
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()),...
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
MobilizedBody & setDefaultInboardFrame(const Transform &X_PF)
Change this mobilizer's frame F on the parent body P.
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
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...
const SimbodyMatterSubsystem & getMatterSubsystem() const
Obtain a reference to the SimbodyMatterSubsystem which contains this MobilizedBody.
const Motion & getMotion() const
If there is a Motion object associated with this MobilizedBody, this returns a const reference to it.
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.
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...
MobilizedBody & lockByDefault(Motion::Level level=Motion::Position)
Change whether this mobilizer is initially locked.
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
const MassProperties & getBodyMassProperties(const State &state) const
Return a reference to this body's mass properties in the State cache.
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)...
Pin Revolute
Synonym for Pin mobilizer.
Definition: MobilizedBody.h:1824
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
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
Vector getQDotDotAsVector(const State &state) const
Return as a Vector of length getNumQ() all the generalized coordinate second derivatives qdotdot curr...
MobilizedBody & setBody(const Body &)
Replace the Body contained within this MobilizedBody with a new one.
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
const Transform & getMobilizerTransform(const State &state) const
At stage Position or higher, return the cross-mobilizer transform X_FM, the body's inboard mobilizer ...
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...
SpatialVec calcBodyMomentumAboutBodyOriginInGround(const State &state)
Calculate body B's momentum (angular, linear) measured and expressed in Ground, but taken about the b...
Definition: MobilizedBody.h:1263
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
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
CNT< typenameCNT< E1 >::THerm >::template Result< E2 >::Mul dot(const Vec< M, E1, S1 > &r, const Vec< M, E2, S2 > &v)
Definition: SmallMatrixMixed.h:86
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