Simbody  3.6
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 
167 const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;
168 
173 MobilizedBody& updMobilizedBody(MobilizedBodyIndex);
174 
177 const MobilizedBody::Ground& getGround() const;
181 MobilizedBody::Ground& updGround();
188 MobilizedBody::Ground& Ground() {return updGround();}
189 
193 const Constraint& getConstraint(ConstraintIndex) const;
194 
198 Constraint& updConstraint(ConstraintIndex);
199 
203 void setShowDefaultGeometry(bool show);
206 bool getShowDefaultGeometry() const;
207 
225 int getNumBodies() const;
226 
230 int getNumConstraints() const;
231 
234 int getNumMobilities() const;
235 
238 int getTotalQAlloc() const;
239 
252 MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent,
253  MobilizedBody& child);
254 
265 ConstraintIndex adoptConstraint(Constraint&);
266 
268 UnilateralContactIndex adoptUnilateralContact(UnilateralContact*);
269 int getNumUnilateralContacts() const;
270 const UnilateralContact& getUnilateralContact(UnilateralContactIndex) const;
271 UnilateralContact& updUnilateralContact(UnilateralContactIndex);
273 StateLimitedFrictionIndex adoptStateLimitedFriction(StateLimitedFriction*);
274 int getNumStateLimitedFrictions() const;
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 
334 void setConstraintIsDisabled(State& state,
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 
391 Vec3 calcSystemMassCenterLocationInGround(const State& s) const;
392 
397 MassProperties calcSystemMassPropertiesInGround(const State& s) const;
398 
403 Inertia calcSystemCentralInertiaInGround(const State& s) const;
404 
409 Vec3 calcSystemMassCenterVelocityInGround(const State& s) const;
410 
415 Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const;
416 
423 SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const;
424 
432 SpatialVec calcSystemCentralMomentum(const State& s) const;
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 
585 void calcBiasForSystemJacobian(const State& state,
586  Vector_<SpatialVec>& JDotu) const;
587 
588 
592 void calcBiasForSystemJacobian(const State& state,
593  Vector& JDotu) const;
594 
646 void multiplyBySystemJacobianTranspose( const State& state,
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 
739 void multiplyByStationJacobian(const State& state,
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 
772 void multiplyByStationJacobianTranspose
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 
780 void multiplyByStationJacobianTranspose
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 
905 void calcBiasForStationJacobian(const State& state,
906  const Array_<MobilizedBodyIndex>& onBodyB,
907  const Array_<Vec3>& stationPInB,
908  Vector_<Vec3>& JSDotu) const;
909 
913 void calcBiasForStationJacobian(const State& state,
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 
988 void multiplyByFrameJacobian
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 
1053 void multiplyByFrameJacobianTranspose
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 
1199 void calcBiasForFrameJacobian
1200  (const State& state,
1201  const Array_<MobilizedBodyIndex>& onBodyB,
1202  const Array_<Vec3>& originAoInB,
1203  Vector_<SpatialVec>& JFDotu) const;
1204 
1208 void calcBiasForFrameJacobian(const State& state,
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 
1475 void solveForConstraintImpulses(const State& state,
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 
1600 void calcBiasForAccelerationConstraints(const State& state,
1601  Vector& bias) const;
1602 
1639 void calcConstraintAccelerationErrors(const State& state,
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 //==============================================================================
2001 void calcAcceleration
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 
2031 void calcAccelerationIgnoringConstraints
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 
2094 void calcResidualForceIgnoringConstraints
2095  (const State& state,
2096  const Vector& appliedMobilityForces,
2097  const Vector_<SpatialVec>& appliedBodyForces,
2098  const Vector& knownUdot,
2099  Vector& residualMobilityForces) const;
2100 
2101 
2164 void calcResidualForce
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 
2183 void calcCompositeBodyInertias(const State& state,
2185 
2186 
2187 
2225 void calcBodyAccelerationFromUDot(const State& state,
2226  const Vector& knownUDot,
2227  Vector_<SpatialVec>& A_GB) const;
2228 
2251 void calcConstraintForcesFromMultipliers
2252  (const State& state,
2253  const Vector& multipliers,
2254  Vector_<SpatialVec>& bodyForcesInG,
2255  Vector& mobilityForces) const;
2256 
2339 void calcMobilizerReactionForces
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 
2377 void findMotionForces
2378  (const State& state,
2379  Vector& mobilityForces) const;
2380 
2390 const Vector& getConstraintMultipliers(const State& state) const;
2391 
2399 void findConstraintForces
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 
2460 void calcTreeEquivalentMobilityForces(const State&,
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 
2565 void realizeVelocityKinematics(const State&) const;
2566 
2575 void realizeCompositeBodyInertias(const State&) const;
2576 
2590 void realizeArticulatedBodyInertias(const State&) const;
2591 
2620 void realizeArticulatedBodyVelocity(const State&) const;
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 
2657 void packFreeQ
2658  (const State& s, const Vector& allQ, Vector& packedFreeQ) const;
2659 
2665 void unpackFreeQ
2666  (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const;
2667 
2674 void packFreeU
2675  (const State& s, const Vector& allU, Vector& packedFreeU) const;
2676 
2682 void unpackFreeU
2683  (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const;
2684 
2685 
2686  // POSITION STAGE responses //
2687 
2702 const SpatialInertia&
2703 getCompositeBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
2704 
2726 const ArticulatedInertia&
2727 getArticulatedBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
2728 
2729 
2730  // VELOCITY STAGE responses //
2731 
2736 const SpatialVec&
2737 getGyroscopicForce(const State& state, MobilizedBodyIndex mbx) const;
2738 
2746 const SpatialVec&
2747 getMobilizerCoriolisAcceleration(const State& state,
2748  MobilizedBodyIndex mbx) const;
2749 
2758 const SpatialVec&
2759 getTotalCoriolisAcceleration(const State& state, MobilizedBodyIndex mbx) const;
2760 
2761 
2770 const SpatialVec&
2771 getTotalCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
2776 //==============================================================================
2807 void calcMobilizerReactionForcesUsingFreebodyMethod
2808  (const State& state,
2809  Vector_<SpatialVec>& forcesAtMInG) const;
2810 
2818 void invalidatePositionKinematics(const State& state) const;
2819 
2824 bool isPositionKinematicsRealized(const State&) const;
2825 
2834 void invalidateVelocityKinematics(const State& state) const;
2835 
2841 bool isVelocityKinematicsRealized(const State&) const;
2842 
2846 void invalidateCompositeBodyInertias(const State& state) const;
2847 
2852 bool isCompositeBodyInertiasRealized(const State&) const;
2853 
2861 void invalidateArticulatedBodyInertias(const State& state) const;
2862 
2867 bool isArticulatedBodyInertiasRealized(const State&) const;
2868 
2876 void invalidateArticulatedBodyVelocity(const State& state) const;
2877 
2883 bool isArticulatedBodyVelocityRealized(const State&) const;
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.
2906 const Vector_<Vec3>& getAllParticleLocations (const State&) const;
2907 const Vector_<Vec3>& getAllParticleVelocities (const State&) const;
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 
2916 Vector& updAllParticleMasses(State& s) const;
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.
2927 Vector_<Vec3>& updAllParticleLocations(State&) const;
2928 Vector_<Vec3>& updAllParticleVelocities(State&) const;
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 
2956 const Vector_<Vec3>& getAllParticleAccelerations(const State&) const;
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_
#define SimTK_PIMPL_DOWNCAST(Derived, Parent)
Similar to the above but for private implementation abstract classes, that is, abstract class hierarc...
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:593
PhiMatrixTranspose transpose(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:720
A Subsystem is expected to be part of a larger System and to have interdependencies with other subsys...
Definition: Subsystem.h:55
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2912
void setAllParticleMasses(State &s, const Vector &masses) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2918
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
void multiplyByG(const State &state, const Vector &ulike, Vector &Gulike) const
Returns Gulike = G*ulike, the product of the mXn acceleration constraint Jacobian G and a "u-like" (m...
Definition: SimbodyMatterSubsystem.h:1503
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2936
This is a special type of "mobilized" body generated automatically by Simbody as a placeholder for Gr...
Definition: MobilizedBody_Ground.h:45
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:66
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2947
Every Simbody header and source file should include this header before any other Simbody header...
Vec3 calcBiasForStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:920
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85
SimTK_Real Real
This is the default compiled-in floating point type for SimTK, either float or double.
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:606
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:280
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
void calcStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, RowVector_< Vec3 > &JS) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:839
#define DEPRECATED_14(MSG)
Definition: SimTKcommon/include/SimTKcommon/internal/common.h:289
(Experimental – API will change – use at your own risk) A unilateral contact constraint uses a sing...
Definition: ConditionalConstraint.h:120
TODO: not implemented yet.
Definition: ConditionalConstraint.h:289
void multiplyByFrameJacobianTranspose(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, const SpatialVec &F_GAo, Vector &f) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1062
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
void multiplyByPq(const State &state, const Vector &qlike, Vector &PqXqlike) const
Calculate in O(n) time the product Pq*qlike where Pq is the mp X nq position (holonomic) constraint J...
Definition: SimbodyMatterSubsystem.h:1744
void setParticleVelocity(State &s, ParticleIndex p, const Vec3 &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2943
The Array_<T> container class is a plug-compatible replacement for the C++ standard template library ...
Definition: Array.h:53
const Vec3 & getParticleAcceleration(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2958
SpatialVec multiplyByFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, const Vector &u) const
Simplified signature for when you just have a single frame task; see the main signature for documenta...
Definition: SimbodyMatterSubsystem.h:998
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48
SimbodyMatterSubsystem & operator=(const SimbodyMatterSubsystem &ss)
Copy assignment is not very useful.
Definition: SimbodyMatterSubsystem.h:283
void calcStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, Matrix &JS) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:858
const Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2909
This is the matrix class intended to appear in user code for large, variable size matrices...
Definition: BigMatrix.h:168
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition: CompliantContactSubsystem.h:387
void calcFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, RowVector_< SpatialVec > &JF) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1127
void setParticleLocation(State &s, ParticleIndex p, const Vec3 &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2940
The physical meaning of an inertia is the distribution of a rigid body&#39;s mass about a particular poin...
Definition: MassProperties.h:82
MobilizedBody::Ground & Ground()
This is a synonym for updGround() that makes for nicer-looking examples.
Definition: SimbodyMatterSubsystem.h:188
This defines the MobilizedBody class, which associates a new body (the "child", "outboard", or "successor" body) with a mobilizer and a reference frame on an existing body (the "parent", "inboard", or "predecessor" body) that is already part of a SimbodyMatterSubsystem.
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:51
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:68
SimbodyMatterSubsystem(const SimbodyMatterSubsystem &ss)
Copy constructor is not very useful.
Definition: SimbodyMatterSubsystem.h:281
A MobilizedBody is Simbody&#39;s fundamental body-and-joint object used to parameterize a system&#39;s motion...
Definition: MobilizedBody.h:168
This is the base class for all Constraint classes, which is just a handle for the underlying hidden i...
Definition: Constraint.h:66
~SimbodyMatterSubsystem()
The destructor destroys the subsystem implementation object only if this handle is the last reference...
Definition: SimbodyMatterSubsystem.h:161
Represents a variable size row vector; much less common than the column vector type Vector_...
Definition: BigMatrix.h:174
SpatialVec calcBiasForFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1216
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83
void setAllParticleVelocities(State &s, const Vector_< Vec3 > &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2950
void calcFrameJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &originAoInB, Matrix &JF) const
Simplified signature for when you just have a single frame task.
Definition: SimbodyMatterSubsystem.h:1147
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:84
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
Subsystem & operator=(const Subsystem &)
Copy assignment deletes the Subsystem::Guts object if there is one and then behaves like the copy con...
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2933
Vec3 multiplyByStationJacobian(const State &state, MobilizedBodyIndex onBodyB, const Vec3 &stationPInB, const Vector &u) const
Alternate signature for when you just have a single station task.
Definition: SimbodyMatterSubsystem.h:747