Simbody 3.7
SimbodyMatterSubsystem.h
Go to the documentation of this file.
1#ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
2#define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
3
4/* -------------------------------------------------------------------------- *
5 * Simbody(tm) *
6 * -------------------------------------------------------------------------- *
7 * This is part of the SimTK biosimulation toolkit originating from *
8 * Simbios, the NIH National Center for Physics-Based Simulation of *
9 * Biological Structures at Stanford, funded under the NIH Roadmap for *
10 * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11 * *
12 * Portions copyright (c) 2006-15 Stanford University and the Authors. *
13 * Authors: Michael Sherman *
14 * Contributors: Paul Mitiguy *
15 * *
16 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17 * not use this file except in compliance with the License. You may obtain a *
18 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19 * *
20 * Unless required by applicable law or agreed to in writing, software *
21 * distributed under the License is distributed on an "AS IS" BASIS, *
22 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23 * See the License for the specific language governing permissions and *
24 * limitations under the License. *
25 * -------------------------------------------------------------------------- */
26
27#include "SimTKcommon.h"
30
31#include <cassert>
32#include <vector>
33#include <iostream>
34
35class SimbodyMatterSubsystemRep;
36
37namespace SimTK {
38
39class MobilizedBody;
40class MultibodySystem;
41class Constraint;
42
43class UnilateralContact;
44class StateLimitedFriction;
45
134public:
135
136//==============================================================================
161~SimbodyMatterSubsystem() {} // invokes ~Subsystem()
162
168
174
188MobilizedBody::Ground& Ground() {return updGround();}
189
194
199
207
225int getNumBodies() const;
226
231
235
238int getTotalQAlloc() const;
239
253 MobilizedBody& child);
254
266
270const UnilateralContact& getUnilateralContact(UnilateralContactIndex) const;
276getStateLimitedFriction(StateLimitedFrictionIndex) const;
278updStateLimitedFriction(StateLimitedFrictionIndex);
279
284{ Subsystem::operator=(ss); return *this; }
285
286
287//==============================================================================
302void setUseEulerAngles(State& state, bool useEulerAngles) const;
303
306bool getUseEulerAngles(const State& state) const;
307
312int getNumQuaternionsInUse(const State& state) const;
313
318bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const;
319
325QuaternionPoolIndex getQuaternionPoolIndex(const State& state,
326 MobilizedBodyIndex mobodIx) const;
327
335 ConstraintIndex constraintIx,
336 bool shouldDisableConstraint) const;
337
341bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;
342
349void convertToEulerAngles(const State& inputState, State& outputState) const;
350
357void convertToQuaternions(const State& inputState, State& outputState) const;
358
366void normalizeQuaternions(State& state) const;
367
371//==============================================================================
385Real calcSystemMass(const State& s) const;
386
392
398
404
410
416
424
433
438Real calcKineticEnergy(const State& state) const;
441//==============================================================================
555 const Vector& u,
556 Vector_<SpatialVec>& Ju) const;
557
586 Vector_<SpatialVec>& JDotu) const;
587
588
593 Vector& JDotu) const;
594
647 const Vector_<SpatialVec>& F_G,
648 Vector& f) const;
649
650
683void calcSystemJacobian(const State& state,
684 Matrix_<SpatialVec>& J_G) const; // nb X nu
685
690void calcSystemJacobian(const State& state,
691 Matrix& J_G) const; // 6 nb X nu
692
693
740 const Array_<MobilizedBodyIndex>& onBodyB,
741 const Array_<Vec3>& stationPInB,
742 const Vector& u,
743 Vector_<Vec3>& JSu) const;
744
748 MobilizedBodyIndex onBodyB,
749 const Vec3& stationPInB,
750 const Vector& u) const
751{
752 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
753 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
754 Vector_<Vec3> JSu(1);
755 multiplyByStationJacobian(state, bodies, stations, u, JSu);
756 return JSu[0];
757}
758
759
773 (const State& state,
774 const Array_<MobilizedBodyIndex>& onBodyB,
775 const Array_<Vec3>& stationPInB,
776 const Vector_<Vec3>& f_GP,
777 Vector& f) const;
778
781 (const State& state,
782 MobilizedBodyIndex onBodyB,
783 const Vec3& stationPInB,
784 const Vec3& f_GP,
785 Vector& f) const
786{
787 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
788 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
789 Vector_<Vec3> forces(1, f_GP);
790 multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
791}
792
833void calcStationJacobian(const State& state,
834 const Array_<MobilizedBodyIndex>& onBodyB,
835 const Array_<Vec3>& stationPInB,
836 Matrix_<Vec3>& JS) const;
837
839void calcStationJacobian(const State& state,
840 MobilizedBodyIndex onBodyB,
841 const Vec3& stationPInB,
842 RowVector_<Vec3>& JS) const
843{
844 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
845 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
846 calcStationJacobian(state, bodies, stations, JS);
847}
848
852void calcStationJacobian(const State& state,
853 const Array_<MobilizedBodyIndex>& onBodyB,
854 const Array_<Vec3>& stationPInB,
855 Matrix& JS) const;
856
858void calcStationJacobian(const State& state,
859 MobilizedBodyIndex onBodyB,
860 const Vec3& stationPInB,
861 Matrix& JS) const
862{
863 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
864 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
865 calcStationJacobian(state, bodies, stations, JS);
866}
867
868
906 const Array_<MobilizedBodyIndex>& onBodyB,
907 const Array_<Vec3>& stationPInB,
908 Vector_<Vec3>& JSDotu) const;
909
914 const Array_<MobilizedBodyIndex>& onBodyB,
915 const Array_<Vec3>& stationPInB,
916 Vector& JSDotu) const;
917
921 MobilizedBodyIndex onBodyB,
922 const Vec3& stationPInB) const
923{
924 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
925 ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
926 Vector_<Vec3> JSDotu(1);
927 calcBiasForStationJacobian(state, bodies, stations, JSDotu);
928 return JSDotu[0];
929}
930
931
989 (const State& state,
990 const Array_<MobilizedBodyIndex>& onBodyB,
991 const Array_<Vec3>& originAoInB,
992 const Vector& u,
993 Vector_<SpatialVec>& JFu) const;
994
999 MobilizedBodyIndex onBodyB,
1000 const Vec3& originAoInB,
1001 const Vector& u) const
1002{
1003 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1004 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1005 Vector_<SpatialVec> JFu(1);
1006 multiplyByFrameJacobian(state, bodies, stations, u, JFu);
1007 return JFu[0];
1008}
1009
1010
1054 (const State& state,
1055 const Array_<MobilizedBodyIndex>& onBodyB,
1056 const Array_<Vec3>& originAoInB,
1057 const Vector_<SpatialVec>& F_GAo,
1058 Vector& f) const;
1059
1063 MobilizedBodyIndex onBodyB,
1064 const Vec3& originAoInB,
1065 const SpatialVec& F_GAo,
1066 Vector& f) const
1067{
1068 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1069 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1070 const Vector_<SpatialVec> forces(1, F_GAo);
1071 multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
1072}
1073
1074
1075
1120void calcFrameJacobian(const State& state,
1121 const Array_<MobilizedBodyIndex>& onBodyB,
1122 const Array_<Vec3>& originAoInB,
1123 Matrix_<SpatialVec>& JF) const;
1124
1127void calcFrameJacobian(const State& state,
1128 MobilizedBodyIndex onBodyB,
1129 const Vec3& originAoInB,
1130 RowVector_<SpatialVec>& JF) const
1131{
1132 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1133 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1134 calcFrameJacobian(state, bodies, stations, JF);
1135}
1136
1140void calcFrameJacobian(const State& state,
1141 const Array_<MobilizedBodyIndex>& onBodyB,
1142 const Array_<Vec3>& originAoInB,
1143 Matrix& JF) const;
1144
1147void calcFrameJacobian(const State& state,
1148 MobilizedBodyIndex onBodyB,
1149 const Vec3& originAoInB,
1150 Matrix& JF) const
1151{
1152 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1153 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1154 calcFrameJacobian(state, bodies, stations, JF);
1155}
1156
1200 (const State& state,
1201 const Array_<MobilizedBodyIndex>& onBodyB,
1202 const Array_<Vec3>& originAoInB,
1203 Vector_<SpatialVec>& JFDotu) const;
1204
1209 const Array_<MobilizedBodyIndex>& onBodyB,
1210 const Array_<Vec3>& originAoInB,
1211 Vector& JFDotu) const;
1212
1217 MobilizedBodyIndex onBodyB,
1218 const Vec3& originAoInB) const
1219{
1220 ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1221 ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1222 Vector_<SpatialVec> JFDotu(1);
1223 calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
1224 return JFDotu[0];
1225}
1226
1229//==============================================================================
1262void multiplyByM(const State& state, const Vector& a, Vector& Ma) const;
1263
1343void multiplyByMInv(const State& state,
1344 const Vector& v,
1345 Vector& MinvV) const;
1346
1360void calcM(const State&, Matrix& M) const;
1361
1379void calcMInv(const State&, Matrix& MInv) const;
1380
1429 Matrix& GMInvGt) const;
1430
1476 const Vector& deltaV,
1477 Vector& impulse) const;
1478
1479
1503void multiplyByG(const State& state,
1504 const Vector& ulike,
1505 Vector& Gulike) const {
1506 Vector bias;
1507 calcBiasForMultiplyByG(state, bias);
1508 multiplyByG(state, ulike, bias, Gulike);
1509}
1510
1511
1515void multiplyByG(const State& state,
1516 const Vector& ulike,
1517 const Vector& bias,
1518 Vector& Gulike) const;
1519
1546 Vector& bias) const;
1547
1561void calcG(const State& state, Matrix& G) const;
1562
1563
1601 Vector& bias) const;
1602
1640 const Vector& knownUDot,
1641 Vector& pvaerr) const;
1642
1676void multiplyByGTranspose(const State& state,
1677 const Vector& lambda,
1678 Vector& f) const;
1679
1691void calcGTranspose(const State&, Matrix& Gt) const;
1692
1693
1744void multiplyByPq(const State& state,
1745 const Vector& qlike,
1746 Vector& PqXqlike) const {
1747 Vector biasp;
1748 calcBiasForMultiplyByPq(state, biasp);
1749 multiplyByPq(state, qlike, biasp, PqXqlike);
1750}
1751
1752
1756void multiplyByPq(const State& state,
1757 const Vector& qlike,
1758 const Vector& biasp,
1759 Vector& PqXqlike) const;
1760
1778 Vector& biasp) const;
1779
1807void calcPq(const State& state, Matrix& Pq) const;
1808
1809
1843 const Vector& lambdap,
1844 Vector& f) const;
1845
1873void calcPqTranspose(const State& state, Matrix& Pqt) const;
1874
1891void calcP(const State& state, Matrix& P) const;
1892
1896void calcPt(const State& state, Matrix& Pt) const;
1897
1898
1907void multiplyByN(const State& s, bool transpose,
1908 const Vector& in, Vector& out) const;
1909
1918void multiplyByNInv(const State& s, bool transpose,
1919 const Vector& in, Vector& out) const;
1920
1930void multiplyByNDot(const State& s, bool transpose,
1931 const Vector& in, Vector& out) const;
1932
1936//==============================================================================
2002 (const State& state,
2003 const Vector& appliedMobilityForces,
2004 const Vector_<SpatialVec>& appliedBodyForces,
2005 Vector& udot, // output only; no prescribed motions
2006 Vector_<SpatialVec>& A_GB) const;
2007
2032 (const State& state,
2033 const Vector& appliedMobilityForces,
2034 const Vector_<SpatialVec>& appliedBodyForces,
2035 Vector& udot,
2036 Vector_<SpatialVec>& A_GB) const;
2037
2038
2039
2095 (const State& state,
2096 const Vector& appliedMobilityForces,
2097 const Vector_<SpatialVec>& appliedBodyForces,
2098 const Vector& knownUdot,
2099 Vector& residualMobilityForces) const;
2100
2101
2165 (const State& state,
2166 const Vector& appliedMobilityForces,
2167 const Vector_<SpatialVec>& appliedBodyForces,
2168 const Vector& knownUdot,
2169 const Vector& knownLambda,
2170 Vector& residualMobilityForces) const;
2171
2172
2185
2186
2187
2226 const Vector& knownUDot,
2227 Vector_<SpatialVec>& A_GB) const;
2228
2252 (const State& state,
2253 const Vector& multipliers,
2254 Vector_<SpatialVec>& bodyForcesInG,
2255 Vector& mobilityForces) const;
2256
2340 (const State& state,
2341 Vector_<SpatialVec>& forcesAtMInG) const;
2342
2352const Vector& getMotionMultipliers(const State& state) const;
2353
2367Vector calcMotionErrors(const State& state, const Stage& stage) const;
2368
2378 (const State& state,
2379 Vector& mobilityForces) const;
2380
2390const Vector& getConstraintMultipliers(const State& state) const;
2391
2400 (const State& state,
2401 Vector_<SpatialVec>& bodyForcesInG,
2402 Vector& mobilityForces) const;
2403
2422Real calcMotionPower(const State& state) const;
2423
2453Real calcConstraintPower(const State& state) const;
2454
2461 const Vector_<SpatialVec>& bodyForces,
2462 Vector& mobilityForces) const;
2463
2464
2469void calcQDot(const State& s,
2470 const Vector& u,
2471 Vector& qdot) const;
2472
2478void calcQDotDot(const State& s,
2479 const Vector& udot,
2480 Vector& qdotdot) const;
2481
2492void addInStationForce(const State& state,
2493 MobilizedBodyIndex bodyB,
2494 const Vec3& stationOnB,
2495 const Vec3& forceInG,
2496 Vector_<SpatialVec>& bodyForcesInG) const;
2497
2504void addInBodyTorque(const State& state,
2505 MobilizedBodyIndex mobodIx,
2506 const Vec3& torqueInG,
2507 Vector_<SpatialVec>& bodyForcesInG) const;
2508
2517void addInMobilityForce(const State& state,
2518 MobilizedBodyIndex mobodIx,
2519 MobilizerUIndex which,
2520 Real f,
2521 Vector& mobilityForces) const;
2526//==============================================================================
2551void realizePositionKinematics(const State& state) const;
2552
2566
2576
2591
2621
2622
2623 // INSTANCE STAGE responses and operators //
2624
2629const Array_<QIndex>& getFreeQIndex(const State& state) const;
2630
2635const Array_<UIndex>& getFreeUIndex(const State& state) const;
2636
2642const Array_<UIndex>& getFreeUDotIndex(const State& state) const;
2643
2649const Array_<UIndex>& getKnownUDotIndex(const State& state) const;
2650
2658 (const State& s, const Vector& allQ, Vector& packedFreeQ) const;
2659
2666 (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const;
2667
2675 (const State& s, const Vector& allU, Vector& packedFreeU) const;
2676
2683 (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const;
2684
2685
2686 // POSITION STAGE responses //
2687
2702const SpatialInertia&
2704
2726const ArticulatedInertia&
2728
2729
2730 // VELOCITY STAGE responses //
2731
2736const SpatialVec&
2738
2746const SpatialVec&
2748 MobilizedBodyIndex mbx) const;
2749
2758const SpatialVec&
2760
2761
2770const SpatialVec&
2776//==============================================================================
2808 (const State& state,
2809 Vector_<SpatialVec>& forcesAtMInG) const;
2810
2818void invalidatePositionKinematics(const State& state) const;
2819
2825
2834void invalidateVelocityKinematics(const State& state) const;
2835
2842
2847
2853
2862
2868
2877
2887//==============================================================================
2898
2900
2901// The generalized coordinates for a particle are always the three measure numbers
2902// (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized
2903// speeds are always the three corresponding measure numbers of the particle's
2904// Ground-relative Cartesian velocity. The generalized applied forces are
2905// always the three measure numbers of a Ground-relative force vector.
2908
2909const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {
2910 return getAllParticleLocations(s)[p];
2911}
2912const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {
2913 return getAllParticleVelocities(s)[p];
2914}
2915
2917
2918void setAllParticleMasses(State& s, const Vector& masses) const {
2919 updAllParticleMasses(s) = masses;
2920}
2921
2922// Note that particle generalized coordinates, speeds, and applied forces
2923// are defined to be the particle Cartesian locations, velocities, and
2924// applied force vectors, so can be set directly at Stage::Model or higher.
2925
2926// These are the only routines that must be provided by the concrete MatterSubsystem.
2929
2930// The following inline routines are provided by the generic MatterSubsystem
2931// class for convenience.
2932
2933Vec3& updParticleLocation(State& s, ParticleIndex p) const {
2934 return updAllParticleLocations(s)[p];
2935}
2936Vec3& updParticleVelocity(State& s, ParticleIndex p) const {
2937 return updAllParticleVelocities(s)[p];
2938}
2939
2940void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {
2941 updAllParticleLocations(s)[p] = r;
2942}
2943void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {
2944 updAllParticleVelocities(s)[p] = v;
2945}
2946
2948 updAllParticleLocations(s) = r;
2949}
2951 updAllParticleVelocities(s) = v;
2952}
2953
2954const Vector& getAllParticleMasses(const State&) const;
2955
2957
2958const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {
2959 return getAllParticleAccelerations(s)[p];
2960}
2963//==============================================================================
2972// Internally this is now called getArticulatedBodyCentrifugalForces().
2975DEPRECATED_14("do you really need this? see getTotalCentrifugalForces() instead")
2976const SpatialVec&
2977getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
2981//==============================================================================
2982// Bookkeeping methods and internal types -- hide from Doxygen
2984public:
2985class Subtree; // used for working with a connected subgraph of the MobilizedBody tree
2986class SubtreeResults;
2987
2988
2990const SimbodyMatterSubsystemRep& getRep() const;
2991SimbodyMatterSubsystemRep& updRep();
2994private:
2995};
2996
3001operator<<(std::ostream&, const SimbodyMatterSubsystem&);
3002
3003
3004} // namespace SimTK
3005
3006#endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
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:593
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:2950
const StateLimitedFriction & getStateLimitedFriction(StateLimitedFrictionIndex) const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void calcMInv(const State &, Matrix &MInv) const
This operator explicitly calculates the inverse of the part of the system mobility-space mass matrix ...
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 Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2909
MobilizedBody & updMobilizedBody(MobilizedBodyIndex)
Given a MobilizedBodyIndex, return a writable reference to the corresponding MobilizedBody within thi...
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.
const MobilizedBody & getMobilizedBody(MobilizedBodyIndex) const
Given a MobilizedBodyIndex, return a read-only (const) reference to the corresponding MobilizedBody w...
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 Vector_< Vec3 > & getAllParticleVelocities(const State &) const
TODO: total number of particles.
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
const Vector & getMotionMultipliers(const State &state) const
Return a reference to the prescribed motion multipliers tau that have already been calculated in the ...
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...
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...
Vector & updAllParticleMasses(State &s) const
TODO: total number of particles.
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...
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 Constraint & getConstraint(ConstraintIndex) const
Given a ConstraintIndex, return a read-only (const) reference to the corresponding Constraint within ...
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...
MobilizedBody::Ground & Ground()
This is a synonym for updGround() that makes for nicer-looking examples.
Definition: SimbodyMatterSubsystem.h:188
StateLimitedFriction & updStateLimitedFriction(StateLimitedFrictionIndex)
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
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...
const MobilizedBody::Ground & getGround() const
Return a read-only (const) reference to the Ground MobilizedBody within this matter subsystem.
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 ...
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2933
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 ...
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...
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
MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent, MobilizedBody &child)
Attach new matter by attaching it to the indicated parent body (not normally called by users – see Mo...
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...
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2912
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.
Vector_< Vec3 > & updAllParticleLocations(State &) const
TODO: total number of particles.
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,...
UnilateralContact & updUnilateralContact(UnilateralContactIndex)
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
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:2943
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2936
int getNumParticles() const
TODO: total number of particles.
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.
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
Constraint & updConstraint(ConstraintIndex)
Given a ConstraintIndex, return a writable reference to the corresponding Constraint within this matt...
void setAllParticleMasses(State &s, const Vector &masses) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2918
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
MobilizedBody::Ground & updGround()
Return a writable reference to the Ground MobilizedBody within this matter subsystem; you need a writ...
const Vector_< Vec3 > & getAllParticleLocations(const State &) const
TODO: total number of particles.
const Vec3 & getParticleAcceleration(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2958
bool getShowDefaultGeometry() const
Get whether this matter subsystem is set to generate default decorative geometry that can be used to ...
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.
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...
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...
const ArticulatedInertia & getArticulatedBodyInertia(const State &state, MobilizedBodyIndex mbx) const
Return the articulated body inertia (ABI) P for a particular mobilized body.
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 ...
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 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 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...
void calcPt(const State &state, Matrix &Pt) const
Returns the nu X mp matrix ~P - see calcP() for a description.
const Vector & getConstraintMultipliers(const State &state) const
Return a reference to the constraint multipliers lambda that have already been calculated in the give...
int getNumUnilateralContacts() const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
const Vector_< Vec3 > & getAllParticleAccelerations(const State &) const
TODO: total number of particles.
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 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...
int getTotalQAlloc() const
The sum of all the q vector allocations for each joint.
const SpatialVec & getMobilizerCoriolisAcceleration(const State &state, MobilizedBodyIndex mbx) const
This is the cross-mobilizer incremental contribution A to the Coriolis (angular velocity dependent) a...
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
const Array_< UIndex > & getFreeUDotIndex(const State &state) const
Return a list of the generalized speeds whose time derivatives udot are unknown, that is,...
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 calcQDot(const State &s, const Vector &u, Vector &qdot) const
Calculate qdot = N(q)*u in O(n) time (very fast).
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2947
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 Array_< UIndex > & getKnownUDotIndex(const State &state) const
Return a list of the generalized speeds whose time derivatives udot are known, that is,...
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
const SpatialInertia & getCompositeBodyInertia(const State &state, MobilizedBodyIndex mbx) const
Return the composite body inertia (CBI) R for a particular mobilized body.
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 ...
SimbodyMatterSubsystem & operator=(const SimbodyMatterSubsystem &ss)
Copy assignment is not very useful.
Definition: SimbodyMatterSubsystem.h:283
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...
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...
const SpatialVec & getGyroscopicForce(const State &state, MobilizedBodyIndex mbx) const
This is the rotational velocity-dependent force b on the body due to rotational inertia.
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).
Vector_< Vec3 > & updAllParticleVelocities(State &) const
TODO: total number of particles.
const UnilateralContact & getUnilateralContact(UnilateralContactIndex) const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
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...
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:2940
QuaternionPoolIndex getQuaternionPoolIndex(const State &state, MobilizedBodyIndex mobodIx) const
If the given mobilizer is currently using a quaternion to represent orientation, return the Quaternio...
const Vector & getAllParticleMasses(const State &) const
TODO: total number of particles.
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 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.
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...
(Experimental – API will change – use at your own risk) A unilateral contact constraint uses a single...
Definition: ConditionalConstraint.h:120
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:606