1 #ifndef SimTK_SIMBODY_MOBILIZED_BODY_H_
2 #define SimTK_SIMBODY_MOBILIZED_BODY_H_
48 class SimbodyMatterSubsystem;
51 class MobilizedBodyImpl;
56 #ifndef SimTK_SIMBODY_DEFINING_MOBILIZED_BODY
57 extern template class PIMPLHandle<MobilizedBody, MobilizedBodyImpl, true>;
327 return getBodyTransform(state).R();
334 return getBodyTransform(state).p();
355 return getBodyVelocity(state)[0];
362 return getBodyVelocity(state)[1];
384 return getBodyAcceleration(state)[0];
392 return getBodyAcceleration(state)[1];
402 "MobilizedBody::getMobilizerAcceleration() not yet implemented -- volunteers?");
418 return getBodyMassProperties(state).getMass();
425 return getBodyMassProperties(state).getMassCenter();
432 return getBodyMassProperties(state).getUnitInertia();
721 const Transform& X_GB = this->getBodyTransform(state);
735 const Rotation& R_GB = this->getBodyRotation(state);
749 getBodyOriginLocation(state));
760 const SpatialVec& V_GB = this->getBodyVelocity(state);
763 const Vec3 w_AB_G = V_GB[0]-V_GA[0];
767 const Transform& X_GB = getBodyTransform(state);
770 const Vec3 p_AB_G = X_GB.
p() - X_GA.
p();
772 const Vec3 p_AB_G_dot = V_GB[1] - V_GA[1];
775 const Vec3 v_AB_G = p_AB_G_dot - V_GA[0] % p_AB_G;
789 const Vec3& w_GB = getBodyAngularVelocity(state);
792 const Vec3 w_AB_G = w_GB-w_GA;
807 return findBodyVelocityInAnotherBody(state,inBodyA)[1];
822 const Transform& X_GB = this->getBodyTransform(state);
823 const SpatialVec& V_GB = this->getBodyVelocity(state);
824 const SpatialVec& A_GB = this->getBodyAcceleration(state);
840 const Vec3& w_GB = this->getBodyAngularVelocity(state);
842 const Vec3& b_GB = this->getBodyAngularAcceleration(state);
845 const Vec3 w_AB_G = w_GB - w_GA;
846 const Vec3 w_AB_G_dot = b_GB - b_GA;
851 const Vec3 b_AB_G = w_AB_G_dot - w_GA % w_AB_G;
854 return ~R_GA * b_AB_G;
868 return findBodyAccelerationInAnotherBody(state,inBodyA)[1];
887 (
const State& state)
const;
907 (
const State& state)
const;
916 (
const State& state,
const Vec3& stationOnB)
const {
917 return getBodyTransform(state) * stationOnB;
934 findStationLocationInGround(state,stationOnB));
944 (
const State& state,
const Vec3& stationOnB)
const {
945 const Vec3& w = getBodyAngularVelocity(state);
946 const Vec3& v = getBodyOriginVelocity(state);
947 const Vec3 r = expressVectorInGroundFrame(state,stationOnB);
958 const Vec3& stationOnBodyB,
962 findBodyVelocityInAnotherBody(state, inBodyA);
965 expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA);
966 return V_AB[1] + (V_AB[0] % p_BS_A);
978 (
const State& state,
const Vec3& stationOnB)
const {
979 const Vec3& w = getBodyAngularVelocity(state);
980 const Vec3& b = getBodyAngularAcceleration(state);
981 const Vec3& a = getBodyOriginAcceleration(state);
983 const Vec3 r = expressVectorInGroundFrame(state,stationOnB);
984 return a + b % r + w % (w % r);
993 const Vec3& stationOnBodyB,
997 findBodyAngularVelocityInAnotherBody(state,inBodyA);
999 findBodyAccelerationInAnotherBody(state,inBodyA);
1002 expressVectorInAnotherBodyFrame(state, stationOnBodyB, inBodyA);
1004 return A_AB[1] + (A_AB[0] % p_BS_A) + w_AB % (w_AB % p_BS_A);
1011 (
const State& state,
const Vec3& locationOnB,
1012 Vec3& locationOnGround,
Vec3& velocityInGround)
const
1014 const Vec3& p_GB = getBodyOriginLocation(state);
1015 const Vec3 p_BS_G = expressVectorInGroundFrame(state,locationOnB);
1016 locationOnGround = p_GB + p_BS_G;
1018 const Vec3& w_GB = getBodyAngularVelocity(state);
1019 const Vec3& v_GB = getBodyOriginVelocity(state);
1020 velocityInGround = v_GB + w_GB % p_BS_G;
1029 (
const State& state,
const Vec3& locationOnB,
1030 Vec3& locationOnGround,
Vec3& velocityInGround,
Vec3& accelerationInGround)
1033 const Rotation& R_GB = getBodyRotation(state);
1034 const Vec3& p_GB = getBodyOriginLocation(state);
1037 const Vec3 r = R_GB*locationOnB;
1038 locationOnGround = p_GB + r;
1040 const Vec3& w = getBodyAngularVelocity(state);
1041 const Vec3& v = getBodyOriginVelocity(state);
1042 const Vec3& b = getBodyAngularAcceleration(state);
1043 const Vec3& a = getBodyOriginAcceleration(state);
1045 const Vec3 wXr = w % r;
1046 velocityInGround = v + wXr;
1047 accelerationInGround = a + b % r + w % wXr;
1053 return findStationLocationInGround(state,getBodyMassCenterStation(state));
1061 return findStationLocationInAnotherBody(state,
1062 getBodyMassCenterStation(state),toBodyA);
1072 (
const State& state,
const Vec3& locationInG)
const {
1073 return ~getBodyTransform(state) * locationInG;
1083 const Vec3& stationOnA)
const {
1092 return findStationAtGroundPoint(state,
1102 getBodyMassCenterStation(state),*
this);
1110 return getBodyTransform(state) * frameOnB;
1120 return SpatialVec(getBodyAngularVelocity(state),
1121 findStationVelocityInGround(state,frameOnB.
p()));
1131 return SpatialVec(getBodyAngularAcceleration(state),
1132 findStationAccelerationInGround(state,frameOnB.
p()));
1139 (
const State& state,
const Vec3& vectorInB)
const {
1140 return getBodyRotation(state)*vectorInB;
1148 (
const State& state,
const Vec3& vectorInG)
const {
1149 return ~getBodyRotation(state)*vectorInG;
1158 (
const State& state,
const Vec3& vectorInB,
1162 expressVectorInGroundFrame(state,vectorInB));
1171 const Rotation& R_GB = getBodyRotation(state);
1182 const Rotation R_AB = findBodyRotationInAnotherBody(state,inBodyA);
1218 const Rotation& R_GB = getBodyRotation(state);
1231 return getBodyMassProperties(state).calcCentralInertia();
1239 const Vec3& aboutLocationOnBodyA)
const
1246 const Vec3 p_Bo_PA =
1247 findStationAtAnotherBodyStation(state, inBodyA, aboutLocationOnBodyA);
1264 const MassProperties M_Bo_G = expressMassPropertiesInGroundFrame(state);
1265 const SpatialVec& V_GB = getBodyVelocity(state);
1272 (
const State& state)
const {
1274 const Rotation& R_GB = getBodyRotation(state);
1281 const Vec3& w_GB = getBodyAngularVelocity(state);
1292 const Vec3& locationOnBodyB,
1294 const Vec3& locationOnBodyA)
const
1296 if (isSameMobilizedBody(bodyA))
1297 return (locationOnBodyA-locationOnBodyB).norm();
1299 const Vec3 r_Go_PB =
1300 this->findStationLocationInGround(state,locationOnBodyB);
1301 const Vec3 r_Go_PA =
1303 return (r_Go_PA - r_Go_PB).
norm();
1312 (
const State& state,
1313 const Vec3& locationOnBodyB,
1315 const Vec3& locationOnBodyA)
const
1317 if (isSameMobilizedBody(bodyA))
1320 Vec3 rB, rA, vB, vA;
1321 this->findStationLocationAndVelocityInGround(state,locationOnBodyB,rB,vB);
1323 const Vec3 r = rA-rB, v = vA-vB;
1329 if (d==0)
return v.
norm();
1330 else return dot(v, r/d);
1340 (
const State& state,
1341 const Vec3& locationOnBodyB,
1343 const Vec3& locationOnBodyA)
const
1345 if (isSameMobilizedBody(bodyA))
1348 Vec3 rB, rA, vB, vA, aB, aA;
1349 this->findStationLocationVelocityAndAccelerationInGround
1350 (state,locationOnBodyB,rB,vB,aB);
1352 (state,locationOnBodyA,rA,vA,aA);
1354 const Vec3 r = rA-rB, v = vA-vB, a = aA-aB;
1369 if (s==0)
return a.
norm();
1370 else return dot(a, v/s);
1375 const Vec3 vp = v -
dot(v,u)*u;
1376 return dot(a,u) +
dot(vp,v)/d;
1383 (
const State& state,
1384 const Vec3& locationOnBodyB,
1385 const Vec3& velocityOnBodyB,
1389 "MobilizedBody::calcBodyMovingPointVelocityInBody()"
1390 " is not yet implemented -- any volunteers?");
1399 (
const State& state,
1400 const Vec3& locationOnBodyB,
1401 const Vec3& velocityOnBodyB,
1402 const Vec3& accelerationOnBodyB,
1406 "MobilizedBody::calcBodyMovingPointAccelerationInBody()"
1407 " is not yet implemented -- any volunteers?");
1418 (
const State& state,
1419 const Vec3& locationOnBodyB,
1420 const Vec3& velocityOnBodyB,
1422 const Vec3& locationOnBodyA,
1423 const Vec3& velocityOnBodyA)
const
1426 "MobilizedBody::calcMovingPointToPointDistanceTimeDerivative()"
1427 " is not yet implemented -- any volunteers?");
1439 (
const State& state,
1440 const Vec3& locationOnBodyB,
1441 const Vec3& velocityOnBodyB,
1442 const Vec3& accelerationOnBodyB,
1444 const Vec3& locationOnBodyA,
1445 const Vec3& velocityOnBodyA,
1446 const Vec3& accelerationOnBodyA)
const
1449 "MobilizedBody::calcMovingPointToPointDistance2ndTimeDerivative()"
1450 " is not yet implemented -- any volunteers?");
1506 return updBody().addDecoration(X_BD, geometry);
1511 return updBody().addDecoration(geometry);
1551 updBody().setDefaultRigidBodyMassProperties(m);
1558 return getBody().getDefaultRigidBodyMassProperties();
1681 (
const State& state,
int which,
const Vector& qlike)
const;
1688 (
const State& state,
int which,
Vector& qlike)
const;
1694 (
const State& state,
int which,
const Vector& ulike)
const;
1708 Vector& mobilityForces)
const
1710 updOneFromUPartition(state,which,mobilityForces) += f;
1781 (
const State& state,
const Vec3& pointInB,
const Vec3& forceInG,
1862 class UniversalImpl;
1864 class BendStretchImpl;
1869 class TranslationImpl;
1870 class SphericalCoordsImpl;
1872 class LineOrientationImpl;
1876 class EllipsoidImpl;
1879 class FunctionBasedImpl;
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
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