1 #ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
2 #define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
35 class SimbodyMatterSubsystemRep;
40 class MultibodySystem;
201 void setShowDefaultGeometry(
bool show);
204 bool getShowDefaultGeometry()
const;
216 int getNumBodies()
const;
221 int getNumConstraints()
const;
225 int getNumMobilities()
const;
229 int getTotalQAlloc()
const;
234 int getTotalMultAlloc()
const;
286 void setUseEulerAngles(
State& state,
bool useEulerAngles)
const;
290 bool getUseEulerAngles (
const State& state)
const;
296 int getNumQuaternionsInUse(
const State& state)
const;
308 QuaternionPoolIndex getQuaternionPoolIndex(
const State& state,
317 void setConstraintIsDisabled(
State& state,
319 bool shouldDisableConstraint)
const;
332 void convertToEulerAngles(
const State& inputState,
State& outputState)
const;
340 void convertToQuaternions(
const State& inputState,
State& outputState)
const;
349 void normalizeQuaternions(
State& state)
const;
368 Real calcSystemMass(
const State& s)
const;
374 Vec3 calcSystemMassCenterLocationInGround(
const State& s)
const;
386 Inertia calcSystemCentralInertiaInGround(
const State& s)
const;
392 Vec3 calcSystemMassCenterVelocityInGround(
const State& s)
const;
398 Vec3 calcSystemMassCenterAccelerationInGround(
const State& s)
const;
421 Real calcKineticEnergy(
const State& state)
const;
537 void multiplyBySystemJacobian(
const State& state,
568 void calcBiasForSystemJacobian(
const State& state,
575 void calcBiasForSystemJacobian(
const State& state,
629 void multiplyBySystemJacobianTranspose(
const State& state,
666 void calcSystemJacobian(
const State& state,
673 void calcSystemJacobian(
const State& state,
722 void multiplyByStationJacobian(
const State& state,
732 const Vec3& stationPInB,
738 multiplyByStationJacobian(state, bodies, stations, u, JSu);
755 void multiplyByStationJacobianTranspose
763 void multiplyByStationJacobianTranspose
766 const Vec3& stationPInB,
773 multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
816 void calcStationJacobian(
const State& state,
824 const Vec3& stationPInB,
829 calcStationJacobian(state, bodies, stations, JS);
835 void calcStationJacobian(
const State& state,
843 const Vec3& stationPInB,
848 calcStationJacobian(state, bodies, stations, JS);
888 void calcBiasForStationJacobian(
const State& state,
896 void calcBiasForStationJacobian(
const State& state,
905 const Vec3& stationPInB)
const
910 calcBiasForStationJacobian(state, bodies, stations, JSDotu);
971 void multiplyByFrameJacobian
983 const Vec3& originAoInB,
989 multiplyByFrameJacobian(state, bodies, stations, u, JFu);
1036 void multiplyByFrameJacobianTranspose
1037 (
const State& state,
1047 const Vec3& originAoInB,
1054 multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
1103 void calcFrameJacobian(
const State& state,
1112 const Vec3& originAoInB,
1117 calcFrameJacobian(state, bodies, stations, JF);
1123 void calcFrameJacobian(
const State& state,
1132 const Vec3& originAoInB,
1137 calcFrameJacobian(state, bodies, stations, JF);
1182 void calcBiasForFrameJacobian
1183 (
const State& state,
1191 void calcBiasForFrameJacobian(
const State& state,
1201 const Vec3& originAoInB)
const
1206 calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
1326 void multiplyByMInv(
const State& state,
1400 void calcProjectedMInv(
const State& s,
1447 void solveForConstraintImpulses(
const State& state,
1479 calcBiasForMultiplyByG(state, bias);
1480 multiplyByG(state, ulike, bias, Gulike);
1487 void multiplyByG(
const State& state,
1517 void calcBiasForMultiplyByG(
const State& state,
1570 void calcBiasForAccelerationConstraints(
const State& state,
1607 void multiplyByGTranspose(
const State& state,
1622 void calcGTranspose(
const State&,
Matrix& Gt)
const;
1677 Vector& PqXqlike)
const {
1679 calcBiasForMultiplyByPq(state, biasp);
1680 multiplyByPq(state, qlike, biasp, PqXqlike);
1687 void multiplyByPq(
const State& state,
1708 void calcBiasForMultiplyByPq(
const State& state,
1738 void calcPq(
const State& state,
Matrix& Pq)
const;
1773 void multiplyByPqTranspose(
const State& state,
1804 void calcPqTranspose(
const State& state,
Matrix& Pqt)
const;
1827 void calcPt(
const State& state,
Matrix& Pt)
const;
1928 void calcAcceleration
1929 (
const State& state,
1930 const Vector& appliedMobilityForces,
1958 void calcAccelerationIgnoringConstraints
1959 (
const State& state,
1960 const Vector& appliedMobilityForces,
2021 void calcResidualForceIgnoringConstraints
2022 (
const State& state,
2023 const Vector& appliedMobilityForces,
2026 Vector& residualMobilityForces)
const;
2091 void calcResidualForce
2092 (
const State& state,
2093 const Vector& appliedMobilityForces,
2096 const Vector& knownLambda,
2097 Vector& residualMobilityForces)
const;
2110 void calcCompositeBodyInertias(
const State& state,
2152 void calcBodyAccelerationFromUDot(
const State& state,
2179 void calcConstraintForcesFromMultipliers
2180 (
const State& state,
2181 const Vector& multipliers,
2183 Vector& mobilityForces)
const;
2267 void calcMobilizerReactionForces
2268 (
const State& state,
2277 const Vector& getMotionMultipliers(
const State& state)
const;
2299 void findMotionForces
2300 (
const State& state,
2301 Vector& mobilityForces)
const;
2309 const Vector& getConstraintMultipliers(
const State& state)
const;
2315 void findConstraintForces
2316 (
const State& state,
2318 Vector& mobilityForces)
const;
2335 Real calcMotionPower(
const State& state)
const;
2363 Real calcConstraintPower(
const State& state)
const;
2370 void calcTreeEquivalentMobilityForces(
const State&,
2372 Vector& mobilityForces)
const;
2379 void calcQDot(
const State& s,
2388 void calcQDotDot(
const State& s,
2402 void addInStationForce(
const State& state,
2404 const Vec3& stationOnB,
2405 const Vec3& forceInG,
2414 void addInBodyTorque(
const State& state,
2416 const Vec3& torqueInG,
2427 void addInMobilityForce(
const State& state,
2431 Vector& mobilityForces)
const;
2461 void realizeCompositeBodyInertias(
const State&)
const;
2472 void realizeArticulatedBodyInertias(
const State&)
const;
2534 getMobilizerCoriolisAcceleration(
const State& state,
2605 void calcMobilizerReactionForcesUsingFreebodyMethod
2606 (
const State& state,
2612 void invalidateCompositeBodyInertias(
const State& state)
const;
2619 void invalidateArticulatedBodyInertias(
const State& state)
const;
2634 int getNumParticles()
const;
2646 return getAllParticleLocations(s)[p];
2649 return getAllParticleVelocities(s)[p];
2655 updAllParticleMasses(s) = masses;
2670 return updAllParticleLocations(s)[p];
2673 return updAllParticleVelocities(s)[p];
2677 updAllParticleLocations(s)[p] = r;
2680 updAllParticleVelocities(s)[p] = v;
2684 updAllParticleLocations(s) = r;
2687 updAllParticleVelocities(s) = v;
2690 const Vector& getAllParticleMasses(
const State&)
const;
2695 return getAllParticleAccelerations(s)[p];
2711 void calcSpatialKinematicsFromInternal(
const State& state,
2714 { multiplyBySystemJacobian(state,u,Ju); }
2718 void calcInternalGradientFromSpatial(
const State& state,
2719 const Vector_<SpatialVec>& F_G,
2721 { multiplyBySystemJacobianTranspose(state, F_G, f); }
2725 void calcMV(
const State& state,
const Vector& v,
Vector& MV)
const
2726 { multiplyByM(state,v,MV); }
2730 void calcMInverseV(
const State& state,
2733 { multiplyByMInv(state,v,MinvV); }
2737 void calcPNInv(
const State& state,
Matrix& PNInv)
const;
2741 void calcGt(
const State&,
Matrix& Gt)
const;
2752 class SubtreeResults;
2756 const SimbodyMatterSubsystemRep& getRep()
const;
2757 SimbodyMatterSubsystemRep& updRep();
2767 operator<<(std::ostream&,
const SimbodyMatterSubsystem&);
2772 #endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
#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:552
PhiMatrixTranspose transpose(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:720
The abstract parent of all Subsystems.
Definition: Subsystem.h:61
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2669
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:48
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
void setAllParticleMasses(State &s, const Vector &masses) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2654
This is a special type of "mobilized" body generated automatically by Simbody as a placeholder for Gr...
Definition: MobilizedBody_Ground.h:45
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:841
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:50
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2672
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:903
Every Simbody header and source file should include this header before any other Simbody header...
void setParticleVelocity(State &s, ParticleIndex p, const Vec3 &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2679
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:1045
The SimTK::Array_ container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
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:730
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:565
This is the handle class for the hidden State implementation.
Definition: State.h:264
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
void setAllParticleVelocities(State &s, const Vector_< Vec3 > &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2686
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
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:981
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:1675
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85
SimbodyMatterSubsystem & operator=(const SimbodyMatterSubsystem &ss)
Copy assignment is not very useful.
Definition: SimbodyMatterSubsystem.h:267
Matrix_< Real > Matrix
Variable-size 2D matrix of Real elements; abbreviation for Matrix_.
Definition: BigMatrix.h:1476
Represents a variable size row vector; much less common than the column vector type Vector_...
Definition: BigMatrix.h:174
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2648
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition: CompliantContactSubsystem.h:387
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:822
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:1199
MobilizedBody::Ground & Ground()
This is a synonym for updGround() that makes for nicer-looking examples.
Definition: SimbodyMatterSubsystem.h:185
This defines the MobilizedBody class, which associates a new body (the "child", "outboard", or "successor" body) with a mobilizer and a reference frame on an existing body (the "parent", "inboard", or "predecessor" body) that is already part of a SimbodyMatterSubsystem.
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2683
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
SimbodyMatterSubsystem(const SimbodyMatterSubsystem &ss)
Copy constructor is not very useful.
Definition: SimbodyMatterSubsystem.h:265
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:167
Vector_< Real > Vector
Variable-size column vector of Real elements; abbreviation for Vector_.
Definition: BigMatrix.h:1471
This is the base class for all Constraint classes, which is just a handle for the underlying hidden i...
Definition: Constraint.h:66
~SimbodyMatterSubsystem()
The destructor destroys the subsystem implementation object only if this handle is the last reference...
Definition: SimbodyMatterSubsystem.h:158
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:84
const Vec3 & getParticleAcceleration(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2694
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:1130
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:130
void setParticleLocation(State &s, ParticleIndex p, const Vec3 &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2676
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:1110
const Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2645
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:1475
This is the matrix class intended to appear in user code for large, variable size matrices...
Definition: BigMatrix.h:168
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
Subsystem & operator=(const Subsystem &)
The physical meaning of an inertia is the distribution of a rigid body's mass about a particular poin...
Definition: MassProperties.h:82