Simbody  3.8
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 
1926 void multiplyByPV(const State& state,
1927  const Vector& ulike,
1928  Vector& PVulike) const {
1929  Vector biaspv;
1930  calcBiasForMultiplyByPV(state, biaspv);
1931  multiplyByPV(state, ulike, biaspv, PVulike);
1932 }
1933 
1937 void multiplyByPV(const State& state,
1938  const Vector& ulike,
1939  const Vector& biaspv,
1940  Vector& PVulike) const;
1941 
1968 void calcBiasForMultiplyByPV(const State& state,
1969  Vector& bias) const;
1970 
1984 void calcPV(const State& state, Matrix& PV) const;
1985 
2021 void multiplyByPVTranspose(const State& state,
2022  const Vector& lambdapv,
2023  Vector& fpv) const;
2024 
2037 void calcPVTranspose(const State&, Matrix& PVt) const;
2038 
2047 void multiplyByN(const State& s, bool transpose,
2048  const Vector& in, Vector& out) const;
2049 
2058 void multiplyByNInv(const State& s, bool transpose,
2059  const Vector& in, Vector& out) const;
2060 
2070 void multiplyByNDot(const State& s, bool transpose,
2071  const Vector& in, Vector& out) const;
2072 
2076 //==============================================================================
2142  (const State& state,
2143  const Vector& appliedMobilityForces,
2144  const Vector_<SpatialVec>& appliedBodyForces,
2145  Vector& udot, // output only; no prescribed motions
2146  Vector_<SpatialVec>& A_GB) const;
2147 
2172  (const State& state,
2173  const Vector& appliedMobilityForces,
2174  const Vector_<SpatialVec>& appliedBodyForces,
2175  Vector& udot,
2176  Vector_<SpatialVec>& A_GB) const;
2177 
2178 
2179 
2235  (const State& state,
2236  const Vector& appliedMobilityForces,
2237  const Vector_<SpatialVec>& appliedBodyForces,
2238  const Vector& knownUdot,
2239  Vector& residualMobilityForces) const;
2240 
2241 
2305  (const State& state,
2306  const Vector& appliedMobilityForces,
2307  const Vector_<SpatialVec>& appliedBodyForces,
2308  const Vector& knownUdot,
2309  const Vector& knownLambda,
2310  Vector& residualMobilityForces) const;
2311 
2312 
2325 
2326 
2327 
2366  const Vector& knownUDot,
2367  Vector_<SpatialVec>& A_GB) const;
2368 
2392  (const State& state,
2393  const Vector& multipliers,
2394  Vector_<SpatialVec>& bodyForcesInG,
2395  Vector& mobilityForces) const;
2396 
2480  (const State& state,
2481  Vector_<SpatialVec>& forcesAtMInG) const;
2482 
2492 const Vector& getMotionMultipliers(const State& state) const;
2493 
2507 Vector calcMotionErrors(const State& state, const Stage& stage) const;
2508 
2518  (const State& state,
2519  Vector& mobilityForces) const;
2520 
2530 const Vector& getConstraintMultipliers(const State& state) const;
2531 
2540  (const State& state,
2541  Vector_<SpatialVec>& bodyForcesInG,
2542  Vector& mobilityForces) const;
2543 
2562 Real calcMotionPower(const State& state) const;
2563 
2593 Real calcConstraintPower(const State& state) const;
2594 
2601  const Vector_<SpatialVec>& bodyForces,
2602  Vector& mobilityForces) const;
2603 
2604 
2609 void calcQDot(const State& s,
2610  const Vector& u,
2611  Vector& qdot) const;
2612 
2618 void calcQDotDot(const State& s,
2619  const Vector& udot,
2620  Vector& qdotdot) const;
2621 
2632 void addInStationForce(const State& state,
2633  MobilizedBodyIndex bodyB,
2634  const Vec3& stationOnB,
2635  const Vec3& forceInG,
2636  Vector_<SpatialVec>& bodyForcesInG) const;
2637 
2644 void addInBodyTorque(const State& state,
2645  MobilizedBodyIndex mobodIx,
2646  const Vec3& torqueInG,
2647  Vector_<SpatialVec>& bodyForcesInG) const;
2648 
2657 void addInMobilityForce(const State& state,
2658  MobilizedBodyIndex mobodIx,
2659  MobilizerUIndex which,
2660  Real f,
2661  Vector& mobilityForces) const;
2666 //==============================================================================
2691 void realizePositionKinematics(const State& state) const;
2692 
2706 
2716 
2731 
2761 
2762 
2763  // INSTANCE STAGE responses and operators //
2764 
2769 const Array_<QIndex>& getFreeQIndex(const State& state) const;
2770 
2775 const Array_<UIndex>& getFreeUIndex(const State& state) const;
2776 
2782 const Array_<UIndex>& getFreeUDotIndex(const State& state) const;
2783 
2789 const Array_<UIndex>& getKnownUDotIndex(const State& state) const;
2790 
2798  (const State& s, const Vector& allQ, Vector& packedFreeQ) const;
2799 
2806  (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const;
2807 
2815  (const State& s, const Vector& allU, Vector& packedFreeU) const;
2816 
2823  (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const;
2824 
2825 
2826  // POSITION STAGE responses //
2827 
2842 const SpatialInertia&
2844 
2866 const ArticulatedInertia&
2868 
2869 
2870  // VELOCITY STAGE responses //
2871 
2876 const SpatialVec&
2878 
2886 const SpatialVec&
2888  MobilizedBodyIndex mbx) const;
2889 
2898 const SpatialVec&
2900 
2901 
2910 const SpatialVec&
2916 //==============================================================================
2948  (const State& state,
2949  Vector_<SpatialVec>& forcesAtMInG) const;
2950 
2958 void invalidatePositionKinematics(const State& state) const;
2959 
2965 
2974 void invalidateVelocityKinematics(const State& state) const;
2975 
2982 
2986 void invalidateCompositeBodyInertias(const State& state) const;
2987 
2993 
3001 void invalidateArticulatedBodyInertias(const State& state) const;
3002 
3008 
3016 void invalidateArticulatedBodyVelocity(const State& state) const;
3017 
3027 //==============================================================================
3038 int getNumParticles() const;
3040 
3041 // The generalized coordinates for a particle are always the three measure numbers
3042 // (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized
3043 // speeds are always the three corresponding measure numbers of the particle's
3044 // Ground-relative Cartesian velocity. The generalized applied forces are
3045 // always the three measure numbers of a Ground-relative force vector.
3048 
3049 const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {
3050  return getAllParticleLocations(s)[p];
3051 }
3052 const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {
3053  return getAllParticleVelocities(s)[p];
3054 }
3055 
3057 
3058 void setAllParticleMasses(State& s, const Vector& masses) const {
3059  updAllParticleMasses(s) = masses;
3060 }
3061 
3062 // Note that particle generalized coordinates, speeds, and applied forces
3063 // are defined to be the particle Cartesian locations, velocities, and
3064 // applied force vectors, so can be set directly at Stage::Model or higher.
3065 
3066 // These are the only routines that must be provided by the concrete MatterSubsystem.
3069 
3070 // The following inline routines are provided by the generic MatterSubsystem
3071 // class for convenience.
3072 
3073 Vec3& updParticleLocation(State& s, ParticleIndex p) const {
3074  return updAllParticleLocations(s)[p];
3075 }
3076 Vec3& updParticleVelocity(State& s, ParticleIndex p) const {
3077  return updAllParticleVelocities(s)[p];
3078 }
3079 
3080 void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {
3081  updAllParticleLocations(s)[p] = r;
3082 }
3083 void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {
3084  updAllParticleVelocities(s)[p] = v;
3085 }
3086 
3087 void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const {
3088  updAllParticleLocations(s) = r;
3089 }
3091  updAllParticleVelocities(s) = v;
3092 }
3093 
3094 const Vector& getAllParticleMasses(const State&) const;
3095 
3097 
3098 const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {
3099  return getAllParticleAccelerations(s)[p];
3100 }
3103 //==============================================================================
3112 // Internally this is now called getArticulatedBodyCentrifugalForces().
3115 DEPRECATED_14("do you really need this? see getTotalCentrifugalForces() instead")
3116 const SpatialVec&
3117 getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
3121 //==============================================================================
3122 // Bookkeeping methods and internal types -- hide from Doxygen
3124 public:
3125 class Subtree; // used for working with a connected subgraph of the MobilizedBody tree
3126 class SubtreeResults;
3127 
3128 
3130 const SimbodyMatterSubsystemRep& getRep() const;
3131 SimbodyMatterSubsystemRep& updRep();
3134 private:
3135 };
3136 
3140 SimTK_SIMBODY_EXPORT std::ostream&
3141 operator<<(std::ostream&, const SimbodyMatterSubsystem&);
3142 
3143 
3144 } // namespace SimTK
3145 
3146 #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:594
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:3090
void calcMInv(const State &, Matrix &MInv) const
This operator explicitly calculates the inverse of the part of the system mobility-space mass matrix ...
void calcPVTranspose(const State &, Matrix &PVt) const
This O(n*(mp+mv)) operator explicitly calculates the n X (mp+mv) transpose of PV = [P;V],...
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:3052
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:3083
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:3076
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:3073
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:3058
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
void calcPV(const State &state, Matrix &PV) const
This O((mp+mv)*n) operator explicitly calculates the (mp+mv) X n acceleration-level constraint matrix...
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:3049
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 multiplyByPV(const State &state, const Vector &ulike, const Vector &biaspv, Vector &PVulike) const
Multiply PVulike=PV*ulike where PV = [P;V] using the supplied precalculated bias vector to improve pe...
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 multiplyByPV(const State &state, const Vector &ulike, Vector &PVulike) const
Returns PVulike = PV*ulike, the product of the (mp+mv)Xn acceleration constraint matrix PV = [P;V] (a...
Definition: SimbodyMatterSubsystem.h:1926
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:3087
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:3098
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:3080
QuaternionPoolIndex getQuaternionPoolIndex(const State &state, MobilizedBodyIndex mobodIx) const
If the given mobilizer is currently using a quaternion to represent orientation, return the Quaternio...
void calcBiasForMultiplyByPV(const State &state, Vector &bias) const
Calculate the bias vector needed for the higher-performance signature of the multiplyByPV() method ab...
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 ...
void multiplyByPVTranspose(const State &state, const Vector &lambdapv, Vector &fpv) const
Returns fpv = ~PV*lambdapv, the product of the n X (mp+mv) transpose of PV = [P;V],...
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:607