1 #ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
2 #define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
35 class SimbodyMatterSubsystemRep;
40 class MultibodySystem;
43 class UnilateralContact;
44 class StateLimitedFriction;
336 bool shouldDisableConstraint)
const;
749 const Vec3& stationPInB,
755 multiplyByStationJacobian(state, bodies, stations, u, JSu);
783 const Vec3& stationPInB,
790 multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
841 const Vec3& stationPInB,
846 calcStationJacobian(state, bodies, stations, JS);
860 const Vec3& stationPInB,
865 calcStationJacobian(state, bodies, stations, JS);
922 const Vec3& stationPInB)
const
927 calcBiasForStationJacobian(state, bodies, stations, JSDotu);
1000 const Vec3& originAoInB,
1006 multiplyByFrameJacobian(state, bodies, stations, u, JFu);
1054 (
const State& state,
1064 const Vec3& originAoInB,
1071 multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
1129 const Vec3& originAoInB,
1134 calcFrameJacobian(state, bodies, stations, JF);
1149 const Vec3& originAoInB,
1154 calcFrameJacobian(state, bodies, stations, JF);
1200 (
const State& state,
1218 const Vec3& originAoInB)
const
1223 calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
1507 calcBiasForMultiplyByG(state, bias);
1508 multiplyByG(state, ulike, bias, Gulike);
1746 Vector& PqXqlike)
const {
1748 calcBiasForMultiplyByPq(state, biasp);
1749 multiplyByPq(state, qlike, biasp, PqXqlike);
1930 calcBiasForMultiplyByPV(state, biaspv);
1931 multiplyByPV(state, ulike, biaspv, PVulike);
2142 (
const State& state,
2143 const Vector& appliedMobilityForces,
2172 (
const State& state,
2173 const Vector& appliedMobilityForces,
2235 (
const State& state,
2236 const Vector& appliedMobilityForces,
2239 Vector& residualMobilityForces)
const;
2305 (
const State& state,
2306 const Vector& appliedMobilityForces,
2309 const Vector& knownLambda,
2310 Vector& residualMobilityForces)
const;
2392 (
const State& state,
2393 const Vector& multipliers,
2395 Vector& mobilityForces)
const;
2480 (
const State& state,
2518 (
const State& state,
2519 Vector& mobilityForces)
const;
2540 (
const State& state,
2542 Vector& mobilityForces)
const;
2602 Vector& mobilityForces)
const;
2634 const Vec3& stationOnB,
2635 const Vec3& forceInG,
2646 const Vec3& torqueInG,
2661 Vector& mobilityForces)
const;
2948 (
const State& state,
3038 int getNumParticles()
const;
3050 return getAllParticleLocations(s)[p];
3053 return getAllParticleVelocities(s)[p];
3059 updAllParticleMasses(s) = masses;
3074 return updAllParticleLocations(s)[p];
3077 return updAllParticleVelocities(s)[p];
3081 updAllParticleLocations(s)[p] = r;
3084 updAllParticleVelocities(s)[p] = v;
3088 updAllParticleLocations(s) = r;
3091 updAllParticleVelocities(s) = v;
3099 return getAllParticleAccelerations(s)[p];
3115 DEPRECATED_14(
"do you really need this? see getTotalCentrifugalForces() instead")
3126 class SubtreeResults;
3130 const SimbodyMatterSubsystemRep& getRep() const;
3131 SimbodyMatterSubsystemRep& updRep();
This defines the MobilizedBody class, which associates a new body (the "child", "outboard",...
#define DEPRECATED_14(MSG)
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:289
#define SimTK_PIMPL_DOWNCAST(Derived, Parent)
Similar to the above but for private implementation abstract classes, that is, abstract class hierarc...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:594
Includes internal headers providing declarations for the basic SimTK Core classes,...
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
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:324
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:1520
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:1235
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
This is the base class for all Constraint classes, which is just a handle for the underlying hidden i...
Definition: Constraint.h:67
The physical meaning of an inertia is the distribution of a rigid body's mass about a particular poin...
Definition: MassProperties.h:193
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B.
Definition: MassProperties.h:1363
This is the matrix class intended to appear in user code for large, variable size matrices.
Definition: Matrix_.h:51
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
This is a special type of "mobilized" body generated automatically by Simbody as a placeholder for Gr...
Definition: MobilizedBody_Ground.h:45
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:169
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
Represents a variable size row vector; much less common than the column vector type Vector_.
Definition: RowVector_.h:52
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
int getNumStateLimitedFrictions() const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void setUseEulerAngles(State &state, bool useEulerAngles) const
For all mobilizers offering unrestricted orientation, decide what method we should use to model their...
void setAllParticleVelocities(State &s, const Vector_< Vec3 > &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3090
void calcMInv(const State &, Matrix &MInv) const
This operator explicitly calculates the inverse of the part of the system mobility-space mass matrix ...
void calcPVTranspose(const State &, Matrix &PVt) const
This O(n*(mp+mv)) operator explicitly calculates the n X (mp+mv) transpose of PV = [P;V],...
void calcP(const State &state, Matrix &P) const
Returns the mp X nu matrix P which is the Jacobian of the first time derivative of the holonomic (pos...
const Array_< UIndex > & getFreeUIndex(const State &state) const
Return a list of the generalized speeds u that are free, that is, not locked or prescribed with a Mot...
const SpatialInertia & getCompositeBodyInertia(const State &state, MobilizedBodyIndex mbx) const
Return the composite body inertia (CBI) R for a particular mobilized body.
MobilizedBody::Ground & updGround()
Return a writable reference to the Ground MobilizedBody within this matter subsystem; you need a writ...
const Vector_< Vec3 > & getAllParticleVelocities(const State &) const
TODO: total number of particles.
Vector_< Vec3 > & updAllParticleLocations(State &) const
TODO: total number of particles.
void calcCompositeBodyInertias(const State &state, Array_< SpatialInertia, MobilizedBodyIndex > &R) const
This operator calculates the composite body inertias R given a State realized to Position stage.
void calcBiasForFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, Vector &JFDotu) const
Alternate signature that returns the bias as a 6*nt-vector of scalars rather than as an nt-vector of ...
void setShowDefaultGeometry(bool show)
Normally the matter subsystem will attempt to generate some decorative geometry as a sketch of the de...
void calcMobilizerReactionForces(const State &state, Vector_< SpatialVec > &forcesAtMInG) const
Calculate the mobilizer reaction force generated at each MobilizedBody, as felt at the mobilizer's ou...
SimbodyMatterSubsystem(MultibodySystem &)
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void calcPqTranspose(const State &state, Matrix &Pqt) const
This O(m*n) operator explicitly calculates the nq X mp transpose of the position-level (holonomic) co...
UnilateralContactIndex adoptUnilateralContact(UnilateralContact *)
(Experimental)
void calcAcceleration(const State &state, const Vector &appliedMobilityForces, const Vector_< SpatialVec > &appliedBodyForces, Vector &udot, Vector_< SpatialVec > &A_GB) const
This is the primary forward dynamics operator.
const Constraint & getConstraint(ConstraintIndex) const
Given a ConstraintIndex, return a read-only (const) reference to the corresponding Constraint within ...
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3052
void invalidatePositionKinematics(const State &state) const
(Advanced) Force invalidation of position kinematics, which otherwise remains valid until an instance...
void multiplyByPq(const State &state, const Vector &qlike, Vector &PqXqlike) const
Calculate in O(n) time the product Pq*qlike where Pq is the mp X nq position (holonomic) constraint J...
Definition: SimbodyMatterSubsystem.h:1744
void calcResidualForceIgnoringConstraints(const State &state, const Vector &appliedMobilityForces, const Vector_< SpatialVec > &appliedBodyForces, const Vector &knownUdot, Vector &residualMobilityForces) const
This is the inverse dynamics operator for the tree system; if there are any constraints or prescribed...
SimbodyMatterSubsystem & operator=(const SimbodyMatterSubsystem &ss)
Copy assignment is not very useful.
Definition: SimbodyMatterSubsystem.h:283
void multiplyBySystemJacobianTranspose(const State &state, const Vector_< SpatialVec > &F_G, Vector &f) const
Calculate the product of the transposed kinematic Jacobian ~J (==J^T) and a vector F_G of spatial for...
const Vector & getConstraintMultipliers(const State &state) const
Return a reference to the constraint multipliers lambda that have already been calculated in the give...
void calcResidualForce(const State &state, const Vector &appliedMobilityForces, const Vector_< SpatialVec > &appliedBodyForces, const Vector &knownUdot, const Vector &knownLambda, Vector &residualMobilityForces) const
This is the inverse dynamics operator for when you know both the accelerations and Lagrange multiplie...
void multiplyByFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, const Vector &u, Vector_< SpatialVec > &JFu) const
Calculate the spatial velocities of a set of nt task frames A={Ai} fixed to nt bodies B={Bi},...
void calcPq(const State &state, Matrix &Pq) const
This O(m*n) operator explicitly calculates the mp X nq position-level (holonomic) constraint Jacobian...
Vector & updAllParticleMasses(State &s) const
TODO: total number of particles.
const Vector & getAllParticleMasses(const State &) const
TODO: total number of particles.
void multiplyByG(const State &state, const Vector &ulike, Vector &Gulike) const
Returns Gulike = G*ulike, the product of the mXn acceleration constraint Jacobian G and a "u-like" (m...
Definition: SimbodyMatterSubsystem.h:1503
const StateLimitedFriction & getStateLimitedFriction(StateLimitedFrictionIndex) const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void multiplyByNInv(const State &s, bool transpose, const Vector &in, Vector &out) const
Calculate out_u = NInv(q)*in_q (like u=NInv*qdot) or out_q = ~NInv*in_u.
void invalidateVelocityKinematics(const State &state) const
(Advanced) Force invalidation of velocity kinematics, which otherwise remains valid until an instance...
void multiplyByMInv(const State &state, const Vector &v, Vector &MinvV) const
This operator calculates in O(n) time the product M^-1*v where M is the system mass matrix and v is a...
void calcBodyAccelerationFromUDot(const State &state, const Vector &knownUDot, Vector_< SpatialVec > &A_GB) const
Given a complete set of n generalized accelerations udot, this kinematic operator calculates in O(n) ...
bool isUsingQuaternion(const State &state, MobilizedBodyIndex mobodIx) const
Check whether a given mobilizer is currently using quaternions, based on the type of mobilizer and th...
void invalidateArticulatedBodyInertias(const State &state) const
(Advanced) Force invalidation of articulated body inertias (ABIs), which otherwise remain valid until...
void calcStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, Matrix &JS) const
Alternate signature that returns a station Jacobian as a 3*nt x n Matrix rather than as a Matrix of V...
Vec3 calcSystemMassCenterLocationInGround(const State &s) const
Return the position vector p_GC of the system mass center C, measured from the Ground origin,...
int getNumBodies() const
The number of bodies includes all mobilized bodies including Ground, which is the first mobilized bod...
void multiplyByStationJacobianTranspose(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, const Vec3 &f_GP, Vector &f) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:781
void calcTreeEquivalentMobilityForces(const State &, const Vector_< SpatialVec > &bodyForces, Vector &mobilityForces) const
Accounts for applied forces and inertial forces produced by non-zero velocities in the State.
void solveForConstraintImpulses(const State &state, const Vector &deltaV, Vector &impulse) const
Given a set of desired constraint-space speed changes, calculate the corresponding constraint-space i...
Real calcMotionPower(const State &state) const
Calculate the power being generated or dissipated by all the Motion objects currently active in this ...
void calcM(const State &, Matrix &M) const
This operator explicitly calculates the n X n mass matrix M.
void calcFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, Matrix &JF) const
Alternate signature that returns a frame Jacobian as a 6*nt X n Matrix rather than as an nt X n Matri...
void setConstraintIsDisabled(State &state, ConstraintIndex constraintIx, bool shouldDisableConstraint) const
Disable or enable the Constraint whose ConstraintIndex is supplied within the supplied state.
SimbodyMatterSubsystem()
Create an orphan matter subsystem containing only the Ground body (mobilized body 0); normally use th...
void multiplyByG(const State &state, const Vector &ulike, const Vector &bias, Vector &Gulike) const
Multiply Gulike=G*ulike using the supplied precalculated bias vector to improve performance (approxim...
void multiplyByM(const State &state, const Vector &a, Vector &Ma) const
This operator calculates in O(n) time the product M*v where M is the system mass matrix and v is a su...
void calcSystemJacobian(const State &state, Matrix_< SpatialVec > &J_G) const
Explicitly calculate and return the nb x nu whole-system kinematic Jacobian J_G, with each element a ...
MobilizedBody & updMobilizedBody(MobilizedBodyIndex)
Given a MobilizedBodyIndex, return a writable reference to the corresponding MobilizedBody within thi...
void invalidateArticulatedBodyVelocity(const State &state) const
(Advanced) Force invalidation of articulated body velocity computations, which otherwise remain valid...
SpatialVec multiplyByFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, const Vector &u) const
Simplified signature for when you just have a single frame task; see the main signature for documenta...
Definition: SimbodyMatterSubsystem.h:998
const SpatialVec & getGyroscopicForce(const State &state, MobilizedBodyIndex mbx) const
This is the rotational velocity-dependent force b on the body due to rotational inertia.
MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent, MobilizedBody &child)
Attach new matter by attaching it to the indicated parent body (not normally called by users – see Mo...
const UnilateralContact & getUnilateralContact(UnilateralContactIndex) const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void realizeCompositeBodyInertias(const State &) const
This method checks whether composite body inertias have already been computed since the last change t...
void calcBiasForMultiplyByG(const State &state, Vector &bias) const
Calculate the bias vector needed for the higher-performance signature of the multiplyByG() method abo...
void calcBiasForMultiplyByPq(const State &state, Vector &biasp) const
Calculate the bias vector needed for the higher-performance signature of the multiplyByPq() method ab...
void calcQDotDot(const State &s, const Vector &udot, Vector &qdotdot) const
Calculate qdotdot = N(q)*udot + Ndot(q,u)*u in O(n) time (very fast).
void multiplyByStationJacobianTranspose(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, const Vector_< Vec3 > &f_GP, Vector &f) const
Calculate the generalized forces resulting from a single force applied to a set of nt station tasks (...
Inertia calcSystemCentralInertiaInGround(const State &s) const
Return the system inertia matrix taken about the system center of mass, expressed in Ground.
void realizeArticulatedBodyVelocity(const State &) const
(Advanced) This method ensures that velocity-dependent computations that also depend on articulated b...
void invalidateCompositeBodyInertias(const State &state) const
(Advanced) This is useful for timing computation time for realizeCompositeBodyInertias(),...
void calcStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, Matrix_< Vec3 > &JS) const
Explicitly calculate and return the 3*nt x n kinematic Jacobian JS for a set of nt station tasks P (a...
bool isVelocityKinematicsRealized(const State &) const
(Advanced) Check whether velocity kinematics has already been realized.
void calcFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, Matrix_< SpatialVec > &JF) const
Explicitly calculate and return the 6*nt x n frame task Jacobian JF for a set of nt frame tasks A={Ai...
void multiplyByPq(const State &state, const Vector &qlike, const Vector &biasp, Vector &PqXqlike) const
Multiply Pq*qlike using the supplied precalculated bias vector to improve performance (approximately ...
void calcConstraintForcesFromMultipliers(const State &state, const Vector &multipliers, Vector_< SpatialVec > &bodyForcesInG, Vector &mobilityForces) const
Treating all Constraints together, given a comprehensive set of m Lagrange multipliers lambda,...
void convertToQuaternions(const State &inputState, State &outputState) const
Given a State which may be modeled using Euler angles, copy it to another State which represents the ...
void calcAccelerationIgnoringConstraints(const State &state, const Vector &appliedMobilityForces, const Vector_< SpatialVec > &appliedBodyForces, Vector &udot, Vector_< SpatialVec > &A_GB) const
This operator is similar to calcAcceleration() but ignores the effects of acceleration constraints al...
void multiplyBySystemJacobian(const State &state, const Vector &u, Vector_< SpatialVec > &Ju) const
Calculate the product of the System kinematic Jacobian J (also known as the partial velocity matrix) ...
void setParticleVelocity(State &s, ParticleIndex p, const Vec3 &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3083
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3076
void multiplyByNDot(const State &s, bool transpose, const Vector &in, Vector &out) const
Calculate out_q = NDot(q,u)*in_u or out_u = ~NDot(q,u)*in_q.
const Vector_< Vec3 > & getAllParticleLocations(const State &) const
TODO: total number of particles.
SpatialVec calcBiasForFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1216
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3073
MobilizedBody::Ground & Ground()
This is a synonym for updGround() that makes for nicer-looking examples.
Definition: SimbodyMatterSubsystem.h:188
UnilateralContact & updUnilateralContact(UnilateralContactIndex)
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void setAllParticleMasses(State &s, const Vector &masses) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3058
Vec3 calcSystemMassCenterVelocityInGround(const State &s) const
Return the velocity v_GC = d/dt p_GC of the system mass center C in the Ground frame G,...
void calcStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, Matrix &JS) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:858
void calcPV(const State &state, Matrix &PV) const
This O((mp+mv)*n) operator explicitly calculates the (mp+mv) X n acceleration-level constraint matrix...
Constraint & updConstraint(ConstraintIndex)
Given a ConstraintIndex, return a writable reference to the corresponding Constraint within this matt...
bool getShowDefaultGeometry() const
Get whether this matter subsystem is set to generate default decorative geometry that can be used to ...
const Array_< UIndex > & getFreeUDotIndex(const State &state) const
Return a list of the generalized speeds whose time derivatives udot are unknown, that is,...
int getNumQuaternionsInUse(const State &state) const
Return the number of quaternions in use by the mobilizers of this system, given the current setting o...
bool isArticulatedBodyInertiasRealized(const State &) const
(Advanced) Check whether articulated body inertias have already been realized.
const Vector & getMotionMultipliers(const State &state) const
Return a reference to the prescribed motion multipliers tau that have already been calculated in the ...
void packFreeU(const State &s, const Vector &allU, Vector &packedFreeU) const
Given a generalized speed (u- or mobility-space) Vector, select only those elements that are free (in...
const Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3049
void multiplyByGTranspose(const State &state, const Vector &lambda, Vector &f) const
Returns f = ~G*lambda, the product of the n X m transpose of the acceleration constraint Jacobian G (...
void calcBiasForSystemJacobian(const State &state, Vector_< SpatialVec > &JDotu) const
Calculate the acceleration bias term for the System Jacobian, that is, the part of the acceleration t...
void calcBiasForStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, Vector &JSDotu) const
Alternate signature that returns the bias as a 3*nt-vector of scalars rather than as an nt-vector of ...
const MobilizedBody::Ground & getGround() const
Return a read-only (const) reference to the Ground MobilizedBody within this matter subsystem.
void multiplyByStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, const Vector &u, Vector_< Vec3 > &JSu) const
Calculate the Cartesian ground-frame velocities of a set of task stations (points fixed on bodies) th...
const SpatialVec & getTotalCoriolisAcceleration(const State &state, MobilizedBodyIndex mbx) const
This is the total Coriolis acceleration of a particular mobilized body, including the effect of the p...
void calcMobilizerReactionForcesUsingFreebodyMethod(const State &state, Vector_< SpatialVec > &forcesAtMInG) const
This is a slower alternative to calcMobilizerReactionForces(), for use in regression testing and Simb...
void unpackFreeQ(const State &s, const Vector &packedFreeQ, Vector &unpackedFreeQ) const
Given a free-q Vector, unpack it into a q-space Vector which must already be allocated to the correct...
SpatialVec calcSystemMomentumAboutGroundOrigin(const State &s) const
Return the momentum of the system as a whole (angular, linear) measured in the Ground frame,...
void normalizeQuaternions(State &state) const
(Advanced) Given a State whose generalized coordinates q have been modified in some manner that doesn...
void calcBiasForStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, Vector_< Vec3 > &JSDotu) const
Calculate the acceleration bias term for a station Jacobian, that is, the part of the station's accel...
void packFreeQ(const State &s, const Vector &allQ, Vector &packedFreeQ) const
Given a generalized coordinate (q-space) Vector, select only those elements that are free (in the sen...
StateLimitedFriction & updStateLimitedFriction(StateLimitedFrictionIndex)
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void calcPt(const State &state, Matrix &Pt) const
Returns the nu X mp matrix ~P - see calcP() for a description.
int getNumUnilateralContacts() const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
Real calcSystemMass(const State &s) const
Calculate the total system mass.
void calcG(const State &state, Matrix &G) const
This O(m*n) operator explicitly calculates the m X n acceleration-level constraint Jacobian G which a...
void multiplyByPV(const State &state, const Vector &ulike, const Vector &biaspv, Vector &PVulike) const
Multiply PVulike=PV*ulike where PV = [P;V] using the supplied precalculated bias vector to improve pe...
void unpackFreeU(const State &s, const Vector &packedFreeU, Vector &unpackedFreeU) const
Given a free-u Vector, unpack it into a u-space Vector which must already be allocated to the correct...
const SpatialVec & getMobilizerCoriolisAcceleration(const State &state, MobilizedBodyIndex mbx) const
This is the cross-mobilizer incremental contribution A to the Coriolis (angular velocity dependent) a...
int getTotalQAlloc() const
The sum of all the q vector allocations for each joint.
void addInStationForce(const State &state, MobilizedBodyIndex bodyB, const Vec3 &stationOnB, const Vec3 &forceInG, Vector_< SpatialVec > &bodyForcesInG) const
Add in to the given body forces vector a force applied to a station (fixed point) S on a body B.
void multiplyByFrameJacobianTranspose(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, const Vector_< SpatialVec > &F_GAo, Vector &f) const
Calculate the n generalized forces f resulting from a set of spatial forces (torque,...
void calcFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, Matrix &JF) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1147
void calcGTranspose(const State &, Matrix &Gt) const
This O(nm) operator explicitly calculates the n X m transpose of the acceleration-level constraint Ja...
void realizeArticulatedBodyInertias(const State &) const
This method ensures that articulated body inertias (ABIs) are up to date with the most recent change ...
Vec3 calcBiasForStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:920
void multiplyByPV(const State &state, const Vector &ulike, Vector &PVulike) const
Returns PVulike = PV*ulike, the product of the (mp+mv)Xn acceleration constraint matrix PV = [P;V] (a...
Definition: SimbodyMatterSubsystem.h:1926
void calcQDot(const State &s, const Vector &u, Vector &qdot) const
Calculate qdot = N(q)*u in O(n) time (very fast).
const Vector_< Vec3 > & getAllParticleAccelerations(const State &) const
TODO: total number of particles.
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3087
int getNumMobilities() const
The sum of all the mobilizer degrees of freedom.
void calcProjectedMInv(const State &s, Matrix &GMInvGt) const
This operator calculates in O(m*n) time the m X m "projected inverse mass matrix" or "constraint com...
SpatialVec calcSystemCentralMomentum(const State &s) const
Return the momentum of the system as a whole (angular, linear) measured in the Ground frame,...
const Vec3 & getParticleAcceleration(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3098
void addInBodyTorque(const State &state, MobilizedBodyIndex mobodIx, const Vec3 &torqueInG, Vector_< SpatialVec > &bodyForcesInG) const
Add in to the given body forces vector a torque applied to a body B.
~SimbodyMatterSubsystem()
The destructor destroys the subsystem implementation object only if this handle is the last reference...
Definition: SimbodyMatterSubsystem.h:161
Real calcKineticEnergy(const State &state) const
Calculate the total kinetic energy of all the mobilized bodies in this matter subsystem,...
void multiplyByFrameJacobianTranspose(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, const SpatialVec &F_GAo, Vector &f) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1062
void calcFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, RowVector_< SpatialVec > &JF) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1127
void calcBiasForFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, Vector_< SpatialVec > &JFDotu) const
Calculate the acceleration bias term for a task frame Jacobian, that is, the parts of the frames' acc...
void calcSystemJacobian(const State &state, Matrix &J_G) const
Alternate signature that returns a system Jacobian as a 6*nb X n Matrix of scalars rather than as an ...
void findConstraintForces(const State &state, Vector_< SpatialVec > &bodyForcesInG, Vector &mobilityForces) const
Find the forces produced by all the active Constraint objects in this system.
void multiplyByN(const State &s, bool transpose, const Vector &in, Vector &out) const
Calculate out_q = N(q)*in_u (like qdot=N*u) or out_u = ~N*in_q.
int getNumConstraints() const
This is the total number of defined constraints, each of which may generate more than one constraint ...
const ArticulatedInertia & getArticulatedBodyInertia(const State &state, MobilizedBodyIndex mbx) const
Return the articulated body inertia (ABI) P for a particular mobilized body.
const SpatialVec & getTotalCentrifugalForces(const State &state, MobilizedBodyIndex mbx) const
This is the total rotational velocity-dependent force acting on this body B, including forces due to ...
void multiplyByPqTranspose(const State &state, const Vector &lambdap, Vector &f) const
Returns f = ~Pq*lambdap, the product of the n X mp transpose of the position (holonomic) constraint J...
const MobilizedBody & getMobilizedBody(MobilizedBodyIndex) const
Given a MobilizedBodyIndex, return a read-only (const) reference to the corresponding MobilizedBody w...
Vec3 multiplyByStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, const Vector &u) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:747
void convertToEulerAngles(const State &inputState, State &outputState) const
Given a State which may be modeled using quaternions, copy it to another State which represents the s...
Vector_< Vec3 > & updAllParticleVelocities(State &) const
TODO: total number of particles.
StateLimitedFrictionIndex adoptStateLimitedFriction(StateLimitedFriction *)
(Experimental)
bool getUseEulerAngles(const State &state) const
Return the current setting of the "use Euler angles" model variable as set in the supplied state.
ConstraintIndex adoptConstraint(Constraint &)
Add a new Constraint object to the matter subsystem (not normally called by users – see Constraint).
void addInMobilityForce(const State &state, MobilizedBodyIndex mobodIx, MobilizerUIndex which, Real f, Vector &mobilityForces) const
Add in to the given mobility forces vector a scalar generalized force, that is a force or torque appl...
const Array_< UIndex > & getKnownUDotIndex(const State &state) const
Return a list of the generalized speeds whose time derivatives udot are known, that is,...
Vec3 calcSystemMassCenterAccelerationInGround(const State &s) const
Return the acceleration a_GC = d/dt p_GC of the system mass center C in the Ground frame G,...
void realizeVelocityKinematics(const State &) const
Velocity kinematics is the first part of the Stage::Velocity realization, mapping generalized speeds ...
bool isArticulatedBodyVelocityRealized(const State &) const
(Advanced) Check whether articulated body velocity computations have already been realized.
void setParticleLocation(State &s, ParticleIndex p, const Vec3 &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3080
QuaternionPoolIndex getQuaternionPoolIndex(const State &state, MobilizedBodyIndex mobodIx) const
If the given mobilizer is currently using a quaternion to represent orientation, return the Quaternio...
void calcBiasForMultiplyByPV(const State &state, Vector &bias) const
Calculate the bias vector needed for the higher-performance signature of the multiplyByPV() method ab...
void calcStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, RowVector_< Vec3 > &JS) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:839
MassProperties calcSystemMassPropertiesInGround(const State &s) const
Return total system mass, mass center location measured from the Ground origin, and system inertia ta...
void findMotionForces(const State &state, Vector &mobilityForces) const
Find the generalized mobility space forces produced by all the Motion objects active in this system.
Vector calcMotionErrors(const State &state, const Stage &stage) const
Calculate the degree to which the supplied state does not satisfy the prescribed motion requirements ...
bool isPositionKinematicsRealized(const State &) const
(Advanced) Check whether position kinematics has already been realized.
void calcConstraintAccelerationErrors(const State &state, const Vector &knownUDot, Vector &pvaerr) const
Given a complete set of nu generalized accelerations udot, this operator computes the constraint acce...
void calcBiasForSystemJacobian(const State &state, Vector &JDotu) const
Alternate signature that returns the bias as a 6*nb-vector of scalars rather than as an nb-vector of ...
SimbodyMatterSubsystem(const SimbodyMatterSubsystem &ss)
Copy constructor is not very useful.
Definition: SimbodyMatterSubsystem.h:281
Real calcConstraintPower(const State &state) const
Return the power begin generated or dissipated by all the Constraint objects currently active in this...
bool isCompositeBodyInertiasRealized(const State &) const
(Advanced) Check whether composite body inertias have already been realized.
void calcBiasForAccelerationConstraints(const State &state, Vector &bias) const
Calculate the acceleration constraint bias vector, that is, the terms in the acceleration constraints...
void realizePositionKinematics(const State &state) const
Position kinematics is the first part of the Stage::Position realization, mapping generalized coordin...
bool isConstraintDisabled(const State &, ConstraintIndex constraint) const
Determine whether a particular Constraint is currently disabled in the given state.
const Array_< QIndex > & getFreeQIndex(const State &state) const
Return a list of the generalized coordinates q that are free, that is, not locked or prescribed with ...
void multiplyByPVTranspose(const State &state, const Vector &lambdapv, Vector &fpv) const
Returns fpv = ~PV*lambdapv, the product of the n X (mp+mv) transpose of PV = [P;V],...
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body.
Definition: MassProperties.h:993
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:66
TODO: not implemented yet.
Definition: ConditionalConstraint.h:289
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
A Subsystem is expected to be part of a larger System and to have interdependencies with other subsys...
Definition: Subsystem.h:55
Subsystem & operator=(const Subsystem &)
Copy assignment deletes the Subsystem::Guts object if there is one and then behaves like the copy con...
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
PhiMatrixTranspose transpose(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:720
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