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;
203 void setShowDefaultGeometry(
bool show);
206 bool getShowDefaultGeometry()
const;
218 int getNumBodies()
const;
223 int getNumConstraints()
const;
227 int getNumMobilities()
const;
231 int getTotalQAlloc()
const;
236 int getTotalMultAlloc()
const;
268 int getNumUnilateralContacts()
const;
273 int getNumStateLimitedFrictions()
const;
275 getStateLimitedFriction(StateLimitedFrictionIndex)
const;
277 updStateLimitedFriction(StateLimitedFrictionIndex);
301 void setUseEulerAngles(
State& state,
bool useEulerAngles)
const;
305 bool getUseEulerAngles (
const State& state)
const;
311 int getNumQuaternionsInUse(
const State& state)
const;
323 QuaternionPoolIndex getQuaternionPoolIndex(
const State& state,
332 void setConstraintIsDisabled(
State& state,
334 bool shouldDisableConstraint)
const;
347 void convertToEulerAngles(
const State& inputState,
State& outputState)
const;
355 void convertToQuaternions(
const State& inputState,
State& outputState)
const;
364 void normalizeQuaternions(
State& state)
const;
383 Real calcSystemMass(
const State& s)
const;
389 Vec3 calcSystemMassCenterLocationInGround(
const State& s)
const;
401 Inertia calcSystemCentralInertiaInGround(
const State& s)
const;
407 Vec3 calcSystemMassCenterVelocityInGround(
const State& s)
const;
413 Vec3 calcSystemMassCenterAccelerationInGround(
const State& s)
const;
436 Real calcKineticEnergy(
const State& state)
const;
552 void multiplyBySystemJacobian(
const State& state,
583 void calcBiasForSystemJacobian(
const State& state,
590 void calcBiasForSystemJacobian(
const State& state,
644 void multiplyBySystemJacobianTranspose(
const State& state,
681 void calcSystemJacobian(
const State& state,
688 void calcSystemJacobian(
const State& state,
737 void multiplyByStationJacobian(
const State& state,
747 const Vec3& stationPInB,
753 multiplyByStationJacobian(state, bodies, stations, u, JSu);
770 void multiplyByStationJacobianTranspose
778 void multiplyByStationJacobianTranspose
781 const Vec3& stationPInB,
788 multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
831 void calcStationJacobian(
const State& state,
839 const Vec3& stationPInB,
844 calcStationJacobian(state, bodies, stations, JS);
850 void calcStationJacobian(
const State& state,
858 const Vec3& stationPInB,
863 calcStationJacobian(state, bodies, stations, JS);
903 void calcBiasForStationJacobian(
const State& state,
911 void calcBiasForStationJacobian(
const State& state,
920 const Vec3& stationPInB)
const 925 calcBiasForStationJacobian(state, bodies, stations, JSDotu);
986 void multiplyByFrameJacobian
998 const Vec3& originAoInB,
1004 multiplyByFrameJacobian(state, bodies, stations, u, JFu);
1051 void multiplyByFrameJacobianTranspose
1052 (
const State& state,
1062 const Vec3& originAoInB,
1069 multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
1118 void calcFrameJacobian(
const State& state,
1127 const Vec3& originAoInB,
1132 calcFrameJacobian(state, bodies, stations, JF);
1138 void calcFrameJacobian(
const State& state,
1147 const Vec3& originAoInB,
1152 calcFrameJacobian(state, bodies, stations, JF);
1197 void calcBiasForFrameJacobian
1198 (
const State& state,
1206 void calcBiasForFrameJacobian(
const State& state,
1216 const Vec3& originAoInB)
const 1221 calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
1341 void multiplyByMInv(
const State& state,
1415 void calcProjectedMInv(
const State& s,
1462 void solveForConstraintImpulses(
const State& state,
1494 calcBiasForMultiplyByG(state, bias);
1495 multiplyByG(state, ulike, bias, Gulike);
1502 void multiplyByG(
const State& state,
1532 void calcBiasForMultiplyByG(
const State& state,
1585 void calcBiasForAccelerationConstraints(
const State& state,
1622 void multiplyByGTranspose(
const State& state,
1637 void calcGTranspose(
const State&,
Matrix& Gt)
const;
1692 Vector& PqXqlike)
const {
1694 calcBiasForMultiplyByPq(state, biasp);
1695 multiplyByPq(state, qlike, biasp, PqXqlike);
1702 void multiplyByPq(
const State& state,
1723 void calcBiasForMultiplyByPq(
const State& state,
1753 void calcPq(
const State& state,
Matrix& Pq)
const;
1788 void multiplyByPqTranspose(
const State& state,
1819 void calcPqTranspose(
const State& state,
Matrix& Pqt)
const;
1842 void calcPt(
const State& state,
Matrix& Pt)
const;
1864 void multiplyByNInv(
const State& s,
bool transpose,
1876 void multiplyByNDot(
const State& s,
bool transpose,
1943 void calcAcceleration
1944 (
const State& state,
1945 const Vector& appliedMobilityForces,
1973 void calcAccelerationIgnoringConstraints
1974 (
const State& state,
1975 const Vector& appliedMobilityForces,
2036 void calcResidualForceIgnoringConstraints
2037 (
const State& state,
2038 const Vector& appliedMobilityForces,
2041 Vector& residualMobilityForces)
const;
2106 void calcResidualForce
2107 (
const State& state,
2108 const Vector& appliedMobilityForces,
2111 const Vector& knownLambda,
2112 Vector& residualMobilityForces)
const;
2125 void calcCompositeBodyInertias(
const State& state,
2167 void calcBodyAccelerationFromUDot(
const State& state,
2194 void calcConstraintForcesFromMultipliers
2195 (
const State& state,
2196 const Vector& multipliers,
2198 Vector& mobilityForces)
const;
2282 void calcMobilizerReactionForces
2283 (
const State& state,
2292 const Vector& getMotionMultipliers(
const State& state)
const;
2314 void findMotionForces
2315 (
const State& state,
2316 Vector& mobilityForces)
const;
2324 const Vector& getConstraintMultipliers(
const State& state)
const;
2330 void findConstraintForces
2331 (
const State& state,
2333 Vector& mobilityForces)
const;
2350 Real calcMotionPower(
const State& state)
const;
2378 Real calcConstraintPower(
const State& state)
const;
2385 void calcTreeEquivalentMobilityForces(
const State&,
2387 Vector& mobilityForces)
const;
2394 void calcQDot(
const State& s,
2403 void calcQDotDot(
const State& s,
2417 void addInStationForce(
const State& state,
2419 const Vec3& stationOnB,
2420 const Vec3& forceInG,
2429 void addInBodyTorque(
const State& state,
2431 const Vec3& torqueInG,
2442 void addInMobilityForce(
const State& state,
2446 Vector& mobilityForces)
const;
2476 void realizeCompositeBodyInertias(
const State&)
const;
2487 void realizeArticulatedBodyInertias(
const State&)
const;
2549 getMobilizerCoriolisAcceleration(
const State& state,
2620 void calcMobilizerReactionForcesUsingFreebodyMethod
2621 (
const State& state,
2627 void invalidateCompositeBodyInertias(
const State& state)
const;
2634 void invalidateArticulatedBodyInertias(
const State& state)
const;
2649 int getNumParticles()
const;
2661 return getAllParticleLocations(s)[p];
2664 return getAllParticleVelocities(s)[p];
2670 updAllParticleMasses(s) = masses;
2685 return updAllParticleLocations(s)[p];
2688 return updAllParticleVelocities(s)[p];
2692 updAllParticleLocations(s)[p] = r;
2695 updAllParticleVelocities(s)[p] = v;
2699 updAllParticleLocations(s) = r;
2702 updAllParticleVelocities(s) = v;
2705 const Vector& getAllParticleMasses(
const State&)
const;
2710 return getAllParticleAccelerations(s)[p];
2726 void calcSpatialKinematicsFromInternal(
const State& state,
2729 { multiplyBySystemJacobian(state,u,Ju); }
2733 void calcInternalGradientFromSpatial(
const State& state,
2736 { multiplyBySystemJacobianTranspose(state, F_G, f); }
2741 { multiplyByM(state,v,MV); }
2745 void calcMInverseV(
const State& state,
2748 { multiplyByMInv(state,v,MinvV); }
2752 void calcPNInv(
const State& state,
Matrix& PNInv)
const;
2767 class SubtreeResults;
2771 const SimbodyMatterSubsystemRep& getRep()
const;
2772 SimbodyMatterSubsystemRep& updRep();
2787 #endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_ PhiMatrixTranspose transpose(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:720
A Subsystem is expected to be part of a larger System and to have interdependencies with other subsys...
Definition: Subsystem.h:55
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2684
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:2669
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:856
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:2687
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:918
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:2694
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:1060
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85
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:745
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:593
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
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:2701
TODO: not implemented yet.
Definition: ConditionalConstraint.h:289
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
The SimTK::Array_<T> container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
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:996
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:1690
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
SimbodyMatterSubsystem & operator=(const SimbodyMatterSubsystem &ss)
Copy assignment is not very useful.
Definition: SimbodyMatterSubsystem.h:282
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2663
This is the matrix class intended to appear in user code for large, variable size matrices...
Definition: BigMatrix.h:168
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:837
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:1214
The physical meaning of an inertia is the distribution of a rigid body's mass about a particular poin...
Definition: MassProperties.h:82
MobilizedBody::Ground & Ground()
This is a synonym for updGround() that makes for nicer-looking examples.
Definition: SimbodyMatterSubsystem.h:188
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.
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:48
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2698
#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:280
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:167
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:161
Represents a variable size row vector; much less common than the column vector type Vector_...
Definition: BigMatrix.h:174
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83
const Vec3 & getParticleAcceleration(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2709
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:84
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:1145
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
void setParticleLocation(State &s, ParticleIndex p, const Vec3 &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2691
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:1125
const Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2660
#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:580
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:1490
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
Subsystem & operator=(const Subsystem &)
Copy assignment deletes the Subsystem::Guts object if there is one and then behaves like the copy con...