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;
225 int getNumBodies()
const;
230 int getNumConstraints()
const;
234 int getNumMobilities()
const;
238 int getTotalQAlloc()
const;
269 int getNumUnilateralContacts()
const;
274 int getNumStateLimitedFrictions()
const;
276 getStateLimitedFriction(StateLimitedFrictionIndex)
const;
278 updStateLimitedFriction(StateLimitedFrictionIndex);
302 void setUseEulerAngles(
State& state,
bool useEulerAngles)
const;
306 bool getUseEulerAngles(
const State& state)
const;
312 int getNumQuaternionsInUse(
const State& state)
const;
325 QuaternionPoolIndex getQuaternionPoolIndex(
const State& state,
334 void setConstraintIsDisabled(
State& state,
336 bool shouldDisableConstraint)
const;
349 void convertToEulerAngles(
const State& inputState,
State& outputState)
const;
357 void convertToQuaternions(
const State& inputState,
State& outputState)
const;
366 void normalizeQuaternions(
State& state)
const;
385 Real calcSystemMass(
const State& s)
const;
391 Vec3 calcSystemMassCenterLocationInGround(
const State& s)
const;
403 Inertia calcSystemCentralInertiaInGround(
const State& s)
const;
409 Vec3 calcSystemMassCenterVelocityInGround(
const State& s)
const;
415 Vec3 calcSystemMassCenterAccelerationInGround(
const State& s)
const;
438 Real calcKineticEnergy(
const State& state)
const;
554 void multiplyBySystemJacobian(
const State& state,
585 void calcBiasForSystemJacobian(
const State& state,
592 void calcBiasForSystemJacobian(
const State& state,
646 void multiplyBySystemJacobianTranspose(
const State& state,
683 void calcSystemJacobian(
const State& state,
690 void calcSystemJacobian(
const State& state,
739 void multiplyByStationJacobian(
const State& state,
749 const Vec3& stationPInB,
755 multiplyByStationJacobian(state, bodies, stations, u, JSu);
772 void multiplyByStationJacobianTranspose
780 void multiplyByStationJacobianTranspose
783 const Vec3& stationPInB,
790 multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
833 void calcStationJacobian(
const State& state,
841 const Vec3& stationPInB,
846 calcStationJacobian(state, bodies, stations, JS);
852 void calcStationJacobian(
const State& state,
860 const Vec3& stationPInB,
865 calcStationJacobian(state, bodies, stations, JS);
905 void calcBiasForStationJacobian(
const State& state,
913 void calcBiasForStationJacobian(
const State& state,
922 const Vec3& stationPInB)
const 927 calcBiasForStationJacobian(state, bodies, stations, JSDotu);
988 void multiplyByFrameJacobian
1000 const Vec3& originAoInB,
1006 multiplyByFrameJacobian(state, bodies, stations, u, JFu);
1053 void multiplyByFrameJacobianTranspose
1054 (
const State& state,
1064 const Vec3& originAoInB,
1071 multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
1120 void calcFrameJacobian(
const State& state,
1129 const Vec3& originAoInB,
1134 calcFrameJacobian(state, bodies, stations, JF);
1140 void calcFrameJacobian(
const State& state,
1149 const Vec3& originAoInB,
1154 calcFrameJacobian(state, bodies, stations, JF);
1199 void calcBiasForFrameJacobian
1200 (
const State& state,
1208 void calcBiasForFrameJacobian(
const State& state,
1218 const Vec3& originAoInB)
const 1223 calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
1343 void multiplyByMInv(
const State& state,
1428 void calcProjectedMInv(
const State& s,
1475 void solveForConstraintImpulses(
const State& state,
1507 calcBiasForMultiplyByG(state, bias);
1508 multiplyByG(state, ulike, bias, Gulike);
1515 void multiplyByG(
const State& state,
1545 void calcBiasForMultiplyByG(
const State& state,
1600 void calcBiasForAccelerationConstraints(
const State& state,
1639 void calcConstraintAccelerationErrors(
const State& state,
1676 void multiplyByGTranspose(
const State& state,
1691 void calcGTranspose(
const State&,
Matrix& Gt)
const;
1746 Vector& PqXqlike)
const {
1748 calcBiasForMultiplyByPq(state, biasp);
1749 multiplyByPq(state, qlike, biasp, PqXqlike);
1756 void multiplyByPq(
const State& state,
1777 void calcBiasForMultiplyByPq(
const State& state,
1807 void calcPq(
const State& state,
Matrix& Pq)
const;
1842 void multiplyByPqTranspose(
const State& state,
1873 void calcPqTranspose(
const State& state,
Matrix& Pqt)
const;
1896 void calcPt(
const State& state,
Matrix& Pt)
const;
1918 void multiplyByNInv(
const State& s,
bool transpose,
1930 void multiplyByNDot(
const State& s,
bool transpose,
2001 void calcAcceleration
2002 (
const State& state,
2003 const Vector& appliedMobilityForces,
2031 void calcAccelerationIgnoringConstraints
2032 (
const State& state,
2033 const Vector& appliedMobilityForces,
2094 void calcResidualForceIgnoringConstraints
2095 (
const State& state,
2096 const Vector& appliedMobilityForces,
2099 Vector& residualMobilityForces)
const;
2164 void calcResidualForce
2165 (
const State& state,
2166 const Vector& appliedMobilityForces,
2169 const Vector& knownLambda,
2170 Vector& residualMobilityForces)
const;
2183 void calcCompositeBodyInertias(
const State& state,
2225 void calcBodyAccelerationFromUDot(
const State& state,
2251 void calcConstraintForcesFromMultipliers
2252 (
const State& state,
2253 const Vector& multipliers,
2255 Vector& mobilityForces)
const;
2339 void calcMobilizerReactionForces
2340 (
const State& state,
2352 const Vector& getMotionMultipliers(
const State& state)
const;
2377 void findMotionForces
2378 (
const State& state,
2379 Vector& mobilityForces)
const;
2390 const Vector& getConstraintMultipliers(
const State& state)
const;
2399 void findConstraintForces
2400 (
const State& state,
2402 Vector& mobilityForces)
const;
2422 Real calcMotionPower(
const State& state)
const;
2453 Real calcConstraintPower(
const State& state)
const;
2460 void calcTreeEquivalentMobilityForces(
const State&,
2462 Vector& mobilityForces)
const;
2469 void calcQDot(
const State& s,
2478 void calcQDotDot(
const State& s,
2492 void addInStationForce(
const State& state,
2494 const Vec3& stationOnB,
2495 const Vec3& forceInG,
2504 void addInBodyTorque(
const State& state,
2506 const Vec3& torqueInG,
2517 void addInMobilityForce(
const State& state,
2521 Vector& mobilityForces)
const;
2551 void realizePositionKinematics(
const State& state)
const;
2565 void realizeVelocityKinematics(
const State&)
const;
2575 void realizeCompositeBodyInertias(
const State&)
const;
2590 void realizeArticulatedBodyInertias(
const State&)
const;
2620 void realizeArticulatedBodyVelocity(
const State&)
const;
2747 getMobilizerCoriolisAcceleration(
const State& state,
2807 void calcMobilizerReactionForcesUsingFreebodyMethod
2808 (
const State& state,
2818 void invalidatePositionKinematics(
const State& state)
const;
2824 bool isPositionKinematicsRealized(
const State&)
const;
2834 void invalidateVelocityKinematics(
const State& state)
const;
2841 bool isVelocityKinematicsRealized(
const State&)
const;
2846 void invalidateCompositeBodyInertias(
const State& state)
const;
2852 bool isCompositeBodyInertiasRealized(
const State&)
const;
2861 void invalidateArticulatedBodyInertias(
const State& state)
const;
2867 bool isArticulatedBodyInertiasRealized(
const State&)
const;
2876 void invalidateArticulatedBodyVelocity(
const State& state)
const;
2883 bool isArticulatedBodyVelocityRealized(
const State&)
const;
2898 int getNumParticles()
const;
2910 return getAllParticleLocations(s)[p];
2913 return getAllParticleVelocities(s)[p];
2919 updAllParticleMasses(s) = masses;
2934 return updAllParticleLocations(s)[p];
2937 return updAllParticleVelocities(s)[p];
2941 updAllParticleLocations(s)[p] = r;
2944 updAllParticleVelocities(s)[p] = v;
2948 updAllParticleLocations(s) = r;
2951 updAllParticleVelocities(s) = v;
2954 const Vector& getAllParticleMasses(
const State&)
const;
2959 return getAllParticleAccelerations(s)[p];
2975 DEPRECATED_14(
"do you really need this? see getTotalCentrifugalForces() instead")
2986 class SubtreeResults;
2990 const SimbodyMatterSubsystemRep& getRep()
const;
2991 SimbodyMatterSubsystemRep& updRep();
3006 #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:593
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
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2912
void setAllParticleMasses(State &s, const Vector &masses) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2918
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
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
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2936
This is a special type of "mobilized" body generated automatically by Simbody as a placeholder for Gr...
Definition: MobilizedBody_Ground.h:45
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:66
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2947
Every Simbody header and source file should include this header before any other Simbody header...
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
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
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
#define DEPRECATED_14(MSG)
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:289
TODO: not implemented yet.
Definition: ConditionalConstraint.h:289
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
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
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 setParticleVelocity(State &s, ParticleIndex p, const Vec3 &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2943
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:53
const Vec3 & getParticleAcceleration(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2958
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
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:283
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
const Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2909
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 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 setParticleLocation(State &s, ParticleIndex p, const Vec3 &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2940
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:51
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
SimbodyMatterSubsystem(const SimbodyMatterSubsystem &ss)
Copy constructor is not very useful.
Definition: SimbodyMatterSubsystem.h:281
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:168
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
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
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83
void setAllParticleVelocities(State &s, const Vector_< Vec3 > &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2950
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
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:84
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
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...
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2933
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