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 
35 class SimbodyMatterSubsystemRep;
36 
37 namespace SimTK {
38 
39 class MobilizedBody;
40 class MultibodySystem;
41 class Constraint;
42 
43 class UnilateralContact;
44 class StateLimitedFriction;
45 
134 public:
135 
136 //==============================================================================
161 ~SimbodyMatterSubsystem() {} // invokes ~Subsystem()
162 
168 
174 
188 MobilizedBody::Ground& Ground() {return updGround();}
189 
194 
199 
203 void setShowDefaultGeometry(bool show);
207 
225 int getNumBodies() const;
226 
230 int getNumConstraints() const;
231 
234 int getNumMobilities() const;
235 
238 int getTotalQAlloc() const;
239 
253  MobilizedBody& child);
254 
266 
268 UnilateralContactIndex adoptUnilateralContact(UnilateralContact*);
270 const UnilateralContact& getUnilateralContact(UnilateralContactIndex) const;
271 UnilateralContact& updUnilateralContact(UnilateralContactIndex);
276 getStateLimitedFriction(StateLimitedFrictionIndex) const;
278 updStateLimitedFriction(StateLimitedFrictionIndex);
279 
284 { Subsystem::operator=(ss); return *this; }
285 
286 
287 //==============================================================================
302 void setUseEulerAngles(State& state, bool useEulerAngles) const;
303 
306 bool getUseEulerAngles(const State& state) const;
307 
312 int getNumQuaternionsInUse(const State& state) const;
313 
318 bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const;
319 
325 QuaternionPoolIndex getQuaternionPoolIndex(const State& state,
326  MobilizedBodyIndex mobodIx) const;
327 
335  ConstraintIndex constraintIx,
336  bool shouldDisableConstraint) const;
337 
341 bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;
342 
349 void convertToEulerAngles(const State& inputState, State& outputState) const;
350 
357 void convertToQuaternions(const State& inputState, State& outputState) const;
358 
366 void normalizeQuaternions(State& state) const;
367 
371 //==============================================================================
385 Real calcSystemMass(const State& s) const;
386 
392 
398 
404 
410 
416 
424 
433 
438 Real calcKineticEnergy(const State& state) const;
441 //==============================================================================
554 void multiplyBySystemJacobian( const State& state,
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 
683 void calcSystemJacobian(const State& state,
684  Matrix_<SpatialVec>& J_G) const; // nb X nu
685 
690 void 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 
833 void calcStationJacobian(const State& state,
834  const Array_<MobilizedBodyIndex>& onBodyB,
835  const Array_<Vec3>& stationPInB,
836  Matrix_<Vec3>& JS) const;
837 
839 void 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 
852 void calcStationJacobian(const State& state,
853  const Array_<MobilizedBodyIndex>& onBodyB,
854  const Array_<Vec3>& stationPInB,
855  Matrix& JS) const;
856 
858 void 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 
1120 void calcFrameJacobian(const State& state,
1121  const Array_<MobilizedBodyIndex>& onBodyB,
1122  const Array_<Vec3>& originAoInB,
1123  Matrix_<SpatialVec>& JF) const;
1124 
1127 void 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 
1140 void calcFrameJacobian(const State& state,
1141  const Array_<MobilizedBodyIndex>& onBodyB,
1142  const Array_<Vec3>& originAoInB,
1143  Matrix& JF) const;
1144 
1147 void 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 //==============================================================================
1262 void multiplyByM(const State& state, const Vector& a, Vector& Ma) const;
1263 
1343 void multiplyByMInv(const State& state,
1344  const Vector& v,
1345  Vector& MinvV) const;
1346 
1360 void calcM(const State&, Matrix& M) const;
1361 
1379 void calcMInv(const State&, Matrix& MInv) const;
1380 
1428 void calcProjectedMInv(const State& s,
1429  Matrix& GMInvGt) const;
1430 
1476  const Vector& deltaV,
1477  Vector& impulse) const;
1478 
1479 
1503 void 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 
1515 void multiplyByG(const State& state,
1516  const Vector& ulike,
1517  const Vector& bias,
1518  Vector& Gulike) const;
1519 
1545 void calcBiasForMultiplyByG(const State& state,
1546  Vector& bias) const;
1547 
1561 void calcG(const State& state, Matrix& G) const;
1562 
1563 
1601  Vector& bias) const;
1602 
1640  const Vector& knownUDot,
1641  Vector& pvaerr) const;
1642 
1676 void multiplyByGTranspose(const State& state,
1677  const Vector& lambda,
1678  Vector& f) const;
1679 
1691 void calcGTranspose(const State&, Matrix& Gt) const;
1692 
1693 
1744 void 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 
1756 void multiplyByPq(const State& state,
1757  const Vector& qlike,
1758  const Vector& biasp,
1759  Vector& PqXqlike) const;
1760 
1777 void calcBiasForMultiplyByPq(const State& state,
1778  Vector& biasp) const;
1779 
1807 void calcPq(const State& state, Matrix& Pq) const;
1808 
1809 
1842 void multiplyByPqTranspose(const State& state,
1843  const Vector& lambdap,
1844  Vector& f) const;
1845 
1873 void calcPqTranspose(const State& state, Matrix& Pqt) const;
1874 
1891 void calcP(const State& state, Matrix& P) const;
1892 
1896 void calcPt(const State& state, Matrix& Pt) const;
1897 
1898 
1907 void multiplyByN(const State& s, bool transpose,
1908  const Vector& in, Vector& out) const;
1909 
1918 void multiplyByNInv(const State& s, bool transpose,
1919  const Vector& in, Vector& out) const;
1920 
1930 void 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 
2352 const Vector& getMotionMultipliers(const State& state) const;
2353 
2367 Vector calcMotionErrors(const State& state, const Stage& stage) const;
2368 
2378  (const State& state,
2379  Vector& mobilityForces) const;
2380 
2390 const Vector& getConstraintMultipliers(const State& state) const;
2391 
2400  (const State& state,
2401  Vector_<SpatialVec>& bodyForcesInG,
2402  Vector& mobilityForces) const;
2403 
2422 Real calcMotionPower(const State& state) const;
2423 
2453 Real calcConstraintPower(const State& state) const;
2454 
2461  const Vector_<SpatialVec>& bodyForces,
2462  Vector& mobilityForces) const;
2463 
2464 
2469 void calcQDot(const State& s,
2470  const Vector& u,
2471  Vector& qdot) const;
2472 
2478 void calcQDotDot(const State& s,
2479  const Vector& udot,
2480  Vector& qdotdot) const;
2481 
2492 void addInStationForce(const State& state,
2493  MobilizedBodyIndex bodyB,
2494  const Vec3& stationOnB,
2495  const Vec3& forceInG,
2496  Vector_<SpatialVec>& bodyForcesInG) const;
2497 
2504 void addInBodyTorque(const State& state,
2505  MobilizedBodyIndex mobodIx,
2506  const Vec3& torqueInG,
2507  Vector_<SpatialVec>& bodyForcesInG) const;
2508 
2517 void addInMobilityForce(const State& state,
2518  MobilizedBodyIndex mobodIx,
2519  MobilizerUIndex which,
2520  Real f,
2521  Vector& mobilityForces) const;
2526 //==============================================================================
2551 void realizePositionKinematics(const State& state) const;
2552 
2566 
2576 
2591 
2621 
2622 
2623  // INSTANCE STAGE responses and operators //
2624 
2629 const Array_<QIndex>& getFreeQIndex(const State& state) const;
2630 
2635 const Array_<UIndex>& getFreeUIndex(const State& state) const;
2636 
2642 const Array_<UIndex>& getFreeUDotIndex(const State& state) const;
2643 
2649 const 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 
2702 const SpatialInertia&
2704 
2726 const ArticulatedInertia&
2728 
2729 
2730  // VELOCITY STAGE responses //
2731 
2736 const SpatialVec&
2738 
2746 const SpatialVec&
2748  MobilizedBodyIndex mbx) const;
2749 
2758 const SpatialVec&
2760 
2761 
2770 const SpatialVec&
2776 //==============================================================================
2808  (const State& state,
2809  Vector_<SpatialVec>& forcesAtMInG) const;
2810 
2818 void invalidatePositionKinematics(const State& state) const;
2819 
2825 
2834 void invalidateVelocityKinematics(const State& state) const;
2835 
2842 
2846 void invalidateCompositeBodyInertias(const State& state) const;
2847 
2853 
2861 void invalidateArticulatedBodyInertias(const State& state) const;
2862 
2868 
2876 void invalidateArticulatedBodyVelocity(const State& state) const;
2877 
2887 //==============================================================================
2898 int getNumParticles() const;
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 
2909 const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {
2910  return getAllParticleLocations(s)[p];
2911 }
2912 const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {
2913  return getAllParticleVelocities(s)[p];
2914 }
2915 
2917 
2918 void 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 
2933 Vec3& updParticleLocation(State& s, ParticleIndex p) const {
2934  return updAllParticleLocations(s)[p];
2935 }
2936 Vec3& updParticleVelocity(State& s, ParticleIndex p) const {
2937  return updAllParticleVelocities(s)[p];
2938 }
2939 
2940 void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {
2941  updAllParticleLocations(s)[p] = r;
2942 }
2943 void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {
2944  updAllParticleVelocities(s)[p] = v;
2945 }
2946 
2947 void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const {
2948  updAllParticleLocations(s) = r;
2949 }
2951  updAllParticleVelocities(s) = v;
2952 }
2953 
2954 const Vector& getAllParticleMasses(const State&) const;
2955 
2957 
2958 const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {
2959  return getAllParticleAccelerations(s)[p];
2960 }
2963 //==============================================================================
2972 // Internally this is now called getArticulatedBodyCentrifugalForces().
2975 DEPRECATED_14("do you really need this? see getTotalCentrifugalForces() instead")
2976 const SpatialVec&
2977 getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
2981 //==============================================================================
2982 // Bookkeeping methods and internal types -- hide from Doxygen
2984 public:
2985 class Subtree; // used for working with a connected subgraph of the MobilizedBody tree
2986 class SubtreeResults;
2987 
2988 
2990 const SimbodyMatterSubsystemRep& getRep() const;
2991 SimbodyMatterSubsystemRep& updRep();
2994 private:
2995 };
2996 
3000 SimTK_SIMBODY_EXPORT std::ostream&
3001 operator<<(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
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 Array_< UIndex > & getFreeUIndex(const State &state) const
Return a list of the generalized speeds u that are free, that is, not locked or prescribed with a Mot...
const SpatialInertia & getCompositeBodyInertia(const State &state, MobilizedBodyIndex mbx) const
Return the composite body inertia (CBI) R for a particular mobilized body.
MobilizedBody::Ground & updGround()
Return a writable reference to the Ground MobilizedBody within this matter subsystem; you need a writ...
const Vector_< Vec3 > & getAllParticleVelocities(const State &) const
TODO: total number of particles.
Vector_< Vec3 > & updAllParticleLocations(State &) const
TODO: total number of particles.
void calcCompositeBodyInertias(const State &state, Array_< SpatialInertia, MobilizedBodyIndex > &R) const
This operator calculates the composite body inertias R given a State realized to Position stage.
void calcBiasForFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, Vector &JFDotu) const
Alternate signature that returns the bias as a 6*nt-vector of scalars rather than as an nt-vector of ...
void setShowDefaultGeometry(bool show)
Normally the matter subsystem will attempt to generate some decorative geometry as a sketch of the de...
void calcMobilizerReactionForces(const State &state, Vector_< SpatialVec > &forcesAtMInG) const
Calculate the mobilizer reaction force generated at each MobilizedBody, as felt at the mobilizer's ou...
SimbodyMatterSubsystem(MultibodySystem &)
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void calcPqTranspose(const State &state, Matrix &Pqt) const
This O(m*n) operator explicitly calculates the nq X mp transpose of the position-level (holonomic) co...
UnilateralContactIndex adoptUnilateralContact(UnilateralContact *)
(Experimental)
void calcAcceleration(const State &state, const Vector &appliedMobilityForces, const Vector_< SpatialVec > &appliedBodyForces, Vector &udot, Vector_< SpatialVec > &A_GB) const
This is the primary forward dynamics operator.
const Constraint & getConstraint(ConstraintIndex) const
Given a ConstraintIndex, return a read-only (const) reference to the corresponding Constraint within ...
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2912
void invalidatePositionKinematics(const State &state) const
(Advanced) Force invalidation of position kinematics, which otherwise remains valid until an instance...
void multiplyByPq(const State &state, const Vector &qlike, Vector &PqXqlike) const
Calculate in O(n) time the product Pq*qlike where Pq is the mp X nq position (holonomic) constraint J...
Definition: SimbodyMatterSubsystem.h:1744
void calcResidualForceIgnoringConstraints(const State &state, const Vector &appliedMobilityForces, const Vector_< SpatialVec > &appliedBodyForces, const Vector &knownUdot, Vector &residualMobilityForces) const
This is the inverse dynamics operator for the tree system; if there are any constraints or prescribed...
SimbodyMatterSubsystem & operator=(const SimbodyMatterSubsystem &ss)
Copy assignment is not very useful.
Definition: SimbodyMatterSubsystem.h:283
void multiplyBySystemJacobianTranspose(const State &state, const Vector_< SpatialVec > &F_G, Vector &f) const
Calculate the product of the transposed kinematic Jacobian ~J (==J^T) and a vector F_G of spatial for...
const Vector & getConstraintMultipliers(const State &state) const
Return a reference to the constraint multipliers lambda that have already been calculated in the give...
void calcResidualForce(const State &state, const Vector &appliedMobilityForces, const Vector_< SpatialVec > &appliedBodyForces, const Vector &knownUdot, const Vector &knownLambda, Vector &residualMobilityForces) const
This is the inverse dynamics operator for when you know both the accelerations and Lagrange multiplie...
void multiplyByFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, const Vector &u, Vector_< SpatialVec > &JFu) const
Calculate the spatial velocities of a set of nt task frames A={Ai} fixed to nt bodies B={Bi},...
void calcPq(const State &state, Matrix &Pq) const
This O(m*n) operator explicitly calculates the mp X nq position-level (holonomic) constraint Jacobian...
Vector & updAllParticleMasses(State &s) const
TODO: total number of particles.
const Vector & getAllParticleMasses(const State &) const
TODO: total number of particles.
void multiplyByG(const State &state, const Vector &ulike, Vector &Gulike) const
Returns Gulike = G*ulike, the product of the mXn acceleration constraint Jacobian G and a "u-like" (m...
Definition: SimbodyMatterSubsystem.h:1503
const StateLimitedFriction & getStateLimitedFriction(StateLimitedFrictionIndex) const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void multiplyByNInv(const State &s, bool transpose, const Vector &in, Vector &out) const
Calculate out_u = NInv(q)*in_q (like u=NInv*qdot) or out_q = ~NInv*in_u.
void invalidateVelocityKinematics(const State &state) const
(Advanced) Force invalidation of velocity kinematics, which otherwise remains valid until an instance...
void multiplyByMInv(const State &state, const Vector &v, Vector &MinvV) const
This operator calculates in O(n) time the product M^-1*v where M is the system mass matrix and v is a...
void calcBodyAccelerationFromUDot(const State &state, const Vector &knownUDot, Vector_< SpatialVec > &A_GB) const
Given a complete set of n generalized accelerations udot, this kinematic operator calculates in O(n) ...
bool isUsingQuaternion(const State &state, MobilizedBodyIndex mobodIx) const
Check whether a given mobilizer is currently using quaternions, based on the type of mobilizer and th...
void invalidateArticulatedBodyInertias(const State &state) const
(Advanced) Force invalidation of articulated body inertias (ABIs), which otherwise remain valid until...
void calcStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, Matrix &JS) const
Alternate signature that returns a station Jacobian as a 3*nt x n Matrix rather than as a Matrix of V...
Vec3 calcSystemMassCenterLocationInGround(const State &s) const
Return the position vector p_GC of the system mass center C, measured from the Ground origin,...
int getNumBodies() const
The number of bodies includes all mobilized bodies including Ground, which is the first mobilized bod...
void multiplyByStationJacobianTranspose(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, const Vec3 &f_GP, Vector &f) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:781
void calcTreeEquivalentMobilityForces(const State &, const Vector_< SpatialVec > &bodyForces, Vector &mobilityForces) const
Accounts for applied forces and inertial forces produced by non-zero velocities in the State.
void solveForConstraintImpulses(const State &state, const Vector &deltaV, Vector &impulse) const
Given a set of desired constraint-space speed changes, calculate the corresponding constraint-space i...
Real calcMotionPower(const State &state) const
Calculate the power being generated or dissipated by all the Motion objects currently active in this ...
void calcM(const State &, Matrix &M) const
This operator explicitly calculates the n X n mass matrix M.
void calcFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, Matrix &JF) const
Alternate signature that returns a frame Jacobian as a 6*nt X n Matrix rather than as an nt X n Matri...
void setConstraintIsDisabled(State &state, ConstraintIndex constraintIx, bool shouldDisableConstraint) const
Disable or enable the Constraint whose ConstraintIndex is supplied within the supplied state.
SimbodyMatterSubsystem()
Create an orphan matter subsystem containing only the Ground body (mobilized body 0); normally use th...
void multiplyByG(const State &state, const Vector &ulike, const Vector &bias, Vector &Gulike) const
Multiply Gulike=G*ulike using the supplied precalculated bias vector to improve performance (approxim...
void multiplyByM(const State &state, const Vector &a, Vector &Ma) const
This operator calculates in O(n) time the product M*v where M is the system mass matrix and v is a su...
void calcSystemJacobian(const State &state, Matrix_< SpatialVec > &J_G) const
Explicitly calculate and return the nb x nu whole-system kinematic Jacobian J_G, with each element a ...
MobilizedBody & updMobilizedBody(MobilizedBodyIndex)
Given a MobilizedBodyIndex, return a writable reference to the corresponding MobilizedBody within thi...
void invalidateArticulatedBodyVelocity(const State &state) const
(Advanced) Force invalidation of articulated body velocity computations, which otherwise remain valid...
SpatialVec multiplyByFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, const Vector &u) const
Simplified signature for when you just have a single frame task; see the main signature for documenta...
Definition: SimbodyMatterSubsystem.h:998
const SpatialVec & getGyroscopicForce(const State &state, MobilizedBodyIndex mbx) const
This is the rotational velocity-dependent force b on the body due to rotational inertia.
MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent, MobilizedBody &child)
Attach new matter by attaching it to the indicated parent body (not normally called by users – see Mo...
const UnilateralContact & getUnilateralContact(UnilateralContactIndex) const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void realizeCompositeBodyInertias(const State &) const
This method checks whether composite body inertias have already been computed since the last change t...
void calcBiasForMultiplyByG(const State &state, Vector &bias) const
Calculate the bias vector needed for the higher-performance signature of the multiplyByG() method abo...
void calcBiasForMultiplyByPq(const State &state, Vector &biasp) const
Calculate the bias vector needed for the higher-performance signature of the multiplyByPq() method ab...
void calcQDotDot(const State &s, const Vector &udot, Vector &qdotdot) const
Calculate qdotdot = N(q)*udot + Ndot(q,u)*u in O(n) time (very fast).
void multiplyByStationJacobianTranspose(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, const Vector_< Vec3 > &f_GP, Vector &f) const
Calculate the generalized forces resulting from a single force applied to a set of nt station tasks (...
Inertia calcSystemCentralInertiaInGround(const State &s) const
Return the system inertia matrix taken about the system center of mass, expressed in Ground.
void realizeArticulatedBodyVelocity(const State &) const
(Advanced) This method ensures that velocity-dependent computations that also depend on articulated b...
void invalidateCompositeBodyInertias(const State &state) const
(Advanced) This is useful for timing computation time for realizeCompositeBodyInertias(),...
void calcStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, Matrix_< Vec3 > &JS) const
Explicitly calculate and return the 3*nt x n kinematic Jacobian JS for a set of nt station tasks P (a...
bool isVelocityKinematicsRealized(const State &) const
(Advanced) Check whether velocity kinematics has already been realized.
void calcFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, Matrix_< SpatialVec > &JF) const
Explicitly calculate and return the 6*nt x n frame task Jacobian JF for a set of nt frame tasks A={Ai...
void multiplyByPq(const State &state, const Vector &qlike, const Vector &biasp, Vector &PqXqlike) const
Multiply Pq*qlike using the supplied precalculated bias vector to improve performance (approximately ...
void calcConstraintForcesFromMultipliers(const State &state, const Vector &multipliers, Vector_< SpatialVec > &bodyForcesInG, Vector &mobilityForces) const
Treating all Constraints together, given a comprehensive set of m Lagrange multipliers lambda,...
void convertToQuaternions(const State &inputState, State &outputState) const
Given a State which may be modeled using Euler angles, copy it to another State which represents the ...
void calcAccelerationIgnoringConstraints(const State &state, const Vector &appliedMobilityForces, const Vector_< SpatialVec > &appliedBodyForces, Vector &udot, Vector_< SpatialVec > &A_GB) const
This operator is similar to calcAcceleration() but ignores the effects of acceleration constraints al...
void multiplyBySystemJacobian(const State &state, const Vector &u, Vector_< SpatialVec > &Ju) const
Calculate the product of the System kinematic Jacobian J (also known as the partial velocity matrix) ...
void setParticleVelocity(State &s, ParticleIndex p, const Vec3 &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2943
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2936
void multiplyByNDot(const State &s, bool transpose, const Vector &in, Vector &out) const
Calculate out_q = NDot(q,u)*in_u or out_u = ~NDot(q,u)*in_q.
const Vector_< Vec3 > & getAllParticleLocations(const State &) const
TODO: total number of particles.
SpatialVec calcBiasForFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1216
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2933
MobilizedBody::Ground & Ground()
This is a synonym for updGround() that makes for nicer-looking examples.
Definition: SimbodyMatterSubsystem.h:188
UnilateralContact & updUnilateralContact(UnilateralContactIndex)
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void setAllParticleMasses(State &s, const Vector &masses) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h: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
Constraint & updConstraint(ConstraintIndex)
Given a ConstraintIndex, return a writable reference to the corresponding Constraint within this matt...
bool getShowDefaultGeometry() const
Get whether this matter subsystem is set to generate default decorative geometry that can be used to ...
const Array_< UIndex > & getFreeUDotIndex(const State &state) const
Return a list of the generalized speeds whose time derivatives udot are unknown, that is,...
int getNumQuaternionsInUse(const State &state) const
Return the number of quaternions in use by the mobilizers of this system, given the current setting o...
bool isArticulatedBodyInertiasRealized(const State &) const
(Advanced) Check whether articulated body inertias have already been realized.
const Vector & getMotionMultipliers(const State &state) const
Return a reference to the prescribed motion multipliers tau that have already been calculated in the ...
void packFreeU(const State &s, const Vector &allU, Vector &packedFreeU) const
Given a generalized speed (u- or mobility-space) Vector, select only those elements that are free (in...
const Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2909
void multiplyByGTranspose(const State &state, const Vector &lambda, Vector &f) const
Returns f = ~G*lambda, the product of the n X m transpose of the acceleration constraint Jacobian G (...
void calcBiasForSystemJacobian(const State &state, Vector_< SpatialVec > &JDotu) const
Calculate the acceleration bias term for the System Jacobian, that is, the part of the acceleration t...
void calcBiasForStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, Vector &JSDotu) const
Alternate signature that returns the bias as a 3*nt-vector of scalars rather than as an nt-vector of ...
const MobilizedBody::Ground & getGround() const
Return a read-only (const) reference to the Ground MobilizedBody within this matter subsystem.
void multiplyByStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, const Vector &u, Vector_< Vec3 > &JSu) const
Calculate the Cartesian ground-frame velocities of a set of task stations (points fixed on bodies) th...
const SpatialVec & getTotalCoriolisAcceleration(const State &state, MobilizedBodyIndex mbx) const
This is the total Coriolis acceleration of a particular mobilized body, including the effect of the p...
void calcMobilizerReactionForcesUsingFreebodyMethod(const State &state, Vector_< SpatialVec > &forcesAtMInG) const
This is a slower alternative to calcMobilizerReactionForces(), for use in regression testing and Simb...
void unpackFreeQ(const State &s, const Vector &packedFreeQ, Vector &unpackedFreeQ) const
Given a free-q Vector, unpack it into a q-space Vector which must already be allocated to the correct...
SpatialVec calcSystemMomentumAboutGroundOrigin(const State &s) const
Return the momentum of the system as a whole (angular, linear) measured in the Ground frame,...
void normalizeQuaternions(State &state) const
(Advanced) Given a State whose generalized coordinates q have been modified in some manner that doesn...
void calcBiasForStationJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &stationPInB, Vector_< Vec3 > &JSDotu) const
Calculate the acceleration bias term for a station Jacobian, that is, the part of the station's accel...
void packFreeQ(const State &s, const Vector &allQ, Vector &packedFreeQ) const
Given a generalized coordinate (q-space) Vector, select only those elements that are free (in the sen...
StateLimitedFriction & updStateLimitedFriction(StateLimitedFrictionIndex)
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
void calcPt(const State &state, Matrix &Pt) const
Returns the nu X mp matrix ~P - see calcP() for a description.
int getNumUnilateralContacts() const
Create a matter subsystem containing only the Ground body (mobilized body 0), and add the subsystem t...
Real calcSystemMass(const State &s) const
Calculate the total system mass.
void calcG(const State &state, Matrix &G) const
This O(m*n) operator explicitly calculates the m X n acceleration-level constraint Jacobian G which a...
void unpackFreeU(const State &s, const Vector &packedFreeU, Vector &unpackedFreeU) const
Given a free-u Vector, unpack it into a u-space Vector which must already be allocated to the correct...
const SpatialVec & getMobilizerCoriolisAcceleration(const State &state, MobilizedBodyIndex mbx) const
This is the cross-mobilizer incremental contribution A to the Coriolis (angular velocity dependent) a...
int getTotalQAlloc() const
The sum of all the q vector allocations for each joint.
void addInStationForce(const State &state, MobilizedBodyIndex bodyB, const Vec3 &stationOnB, const Vec3 &forceInG, Vector_< SpatialVec > &bodyForcesInG) const
Add in to the given body forces vector a force applied to a station (fixed point) S on a body B.
void multiplyByFrameJacobianTranspose(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, const Vector_< SpatialVec > &F_GAo, Vector &f) const
Calculate the n generalized forces f resulting from a set of spatial forces (torque,...
void calcFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, Matrix &JF) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1147
void calcGTranspose(const State &, Matrix &Gt) const
This O(nm) operator explicitly calculates the n X m transpose of the acceleration-level constraint Ja...
void realizeArticulatedBodyInertias(const State &) const
This method ensures that articulated body inertias (ABIs) are up to date with the most recent change ...
Vec3 calcBiasForStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:920
void calcQDot(const State &s, const Vector &u, Vector &qdot) const
Calculate qdot = N(q)*u in O(n) time (very fast).
const Vector_< Vec3 > & getAllParticleAccelerations(const State &) const
TODO: total number of particles.
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h: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 Vec3 & getParticleAcceleration(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2958
void addInBodyTorque(const State &state, MobilizedBodyIndex mobodIx, const Vec3 &torqueInG, Vector_< SpatialVec > &bodyForcesInG) const
Add in to the given body forces vector a torque applied to a body B.
~SimbodyMatterSubsystem()
The destructor destroys the subsystem implementation object only if this handle is the last reference...
Definition: SimbodyMatterSubsystem.h:161
Real calcKineticEnergy(const State &state) const
Calculate the total kinetic energy of all the mobilized bodies in this matter subsystem,...
void multiplyByFrameJacobianTranspose(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, const SpatialVec &F_GAo, Vector &f) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1062
void calcFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, RowVector_< SpatialVec > &JF) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1127
void calcBiasForFrameJacobian(const State &state, const Array_< MobilizedBodyIndex > &onBodyB, const Array_< Vec3 > &originAoInB, Vector_< SpatialVec > &JFDotu) const
Calculate the acceleration bias term for a task frame Jacobian, that is, the parts of the frames' acc...
void calcSystemJacobian(const State &state, Matrix &J_G) const
Alternate signature that returns a system Jacobian as a 6*nb X n Matrix of scalars rather than as an ...
void findConstraintForces(const State &state, Vector_< SpatialVec > &bodyForcesInG, Vector &mobilityForces) const
Find the forces produced by all the active Constraint objects in this system.
void multiplyByN(const State &s, bool transpose, const Vector &in, Vector &out) const
Calculate out_q = N(q)*in_u (like qdot=N*u) or out_u = ~N*in_q.
int getNumConstraints() const
This is the total number of defined constraints, each of which may generate more than one constraint ...
const ArticulatedInertia & getArticulatedBodyInertia(const State &state, MobilizedBodyIndex mbx) const
Return the articulated body inertia (ABI) P for a particular mobilized body.
const SpatialVec & getTotalCentrifugalForces(const State &state, MobilizedBodyIndex mbx) const
This is the total rotational velocity-dependent force acting on this body B, including forces due to ...
void multiplyByPqTranspose(const State &state, const Vector &lambdap, Vector &f) const
Returns f = ~Pq*lambdap, the product of the n X mp transpose of the position (holonomic) constraint J...
const MobilizedBody & getMobilizedBody(MobilizedBodyIndex) const
Given a MobilizedBodyIndex, return a read-only (const) reference to the corresponding MobilizedBody w...
Vec3 multiplyByStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, const Vector &u) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:747
void convertToEulerAngles(const State &inputState, State &outputState) const
Given a State which may be modeled using quaternions, copy it to another State which represents the s...
Vector_< Vec3 > & updAllParticleVelocities(State &) const
TODO: total number of particles.
StateLimitedFrictionIndex adoptStateLimitedFriction(StateLimitedFriction *)
(Experimental)
bool getUseEulerAngles(const State &state) const
Return the current setting of the "use Euler angles" model variable as set in the supplied state.
ConstraintIndex adoptConstraint(Constraint &)
Add a new Constraint object to the matter subsystem (not normally called by users – see Constraint).
void addInMobilityForce(const State &state, MobilizedBodyIndex mobodIx, MobilizerUIndex which, Real f, Vector &mobilityForces) const
Add in to the given mobility forces vector a scalar generalized force, that is a force or torque appl...
const Array_< UIndex > & getKnownUDotIndex(const State &state) const
Return a list of the generalized speeds whose time derivatives udot are known, that is,...
Vec3 calcSystemMassCenterAccelerationInGround(const State &s) const
Return the acceleration a_GC = d/dt p_GC of the system mass center C in the Ground frame G,...
void realizeVelocityKinematics(const State &) const
Velocity kinematics is the first part of the Stage::Velocity realization, mapping generalized speeds ...
bool isArticulatedBodyVelocityRealized(const State &) const
(Advanced) Check whether articulated body velocity computations have already been realized.
void setParticleLocation(State &s, ParticleIndex p, const Vec3 &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2940
QuaternionPoolIndex getQuaternionPoolIndex(const State &state, MobilizedBodyIndex mobodIx) const
If the given mobilizer is currently using a quaternion to represent orientation, return the Quaternio...
void calcStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, RowVector_< Vec3 > &JS) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:839
MassProperties calcSystemMassPropertiesInGround(const State &s) const
Return total system mass, mass center location measured from the Ground origin, and system inertia ta...
void findMotionForces(const State &state, Vector &mobilityForces) const
Find the generalized mobility space forces produced by all the Motion objects active in this system.
Vector calcMotionErrors(const State &state, const Stage &stage) const
Calculate the degree to which the supplied state does not satisfy the prescribed motion requirements ...
bool isPositionKinematicsRealized(const State &) const
(Advanced) Check whether position kinematics has already been realized.
void calcConstraintAccelerationErrors(const State &state, const Vector &knownUDot, Vector &pvaerr) const
Given a complete set of nu generalized accelerations udot, this operator computes the constraint acce...
void calcBiasForSystemJacobian(const State &state, Vector &JDotu) const
Alternate signature that returns the bias as a 6*nb-vector of scalars rather than as an nb-vector of ...
SimbodyMatterSubsystem(const SimbodyMatterSubsystem &ss)
Copy constructor is not very useful.
Definition: SimbodyMatterSubsystem.h:281
Real calcConstraintPower(const State &state) const
Return the power begin generated or dissipated by all the Constraint objects currently active in this...
bool isCompositeBodyInertiasRealized(const State &) const
(Advanced) Check whether composite body inertias have already been realized.
void calcBiasForAccelerationConstraints(const State &state, Vector &bias) const
Calculate the acceleration constraint bias vector, that is, the terms in the acceleration constraints...
void realizePositionKinematics(const State &state) const
Position kinematics is the first part of the Stage::Position realization, mapping generalized coordin...
bool isConstraintDisabled(const State &, ConstraintIndex constraint) const
Determine whether a particular Constraint is currently disabled in the given state.
const Array_< QIndex > & getFreeQIndex(const State &state) const
Return a list of the generalized coordinates q that are free, that is, not locked or prescribed with ...
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