Simbody  3.4
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
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-12 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 
131 public:
132 
133 //==============================================================================
158 ~SimbodyMatterSubsystem() {} // invokes ~Subsystem()
159 
164 const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;
165 
170 MobilizedBody& updMobilizedBody(MobilizedBodyIndex);
171 
174 const MobilizedBody::Ground& getGround() const;
178 MobilizedBody::Ground& updGround();
185 MobilizedBody::Ground& Ground() {return updGround();}
186 
187 
191 const Constraint& getConstraint(ConstraintIndex) const;
192 
196 Constraint& updConstraint(ConstraintIndex);
197 
201 void setShowDefaultGeometry(bool show);
204 bool getShowDefaultGeometry() const;
205 
216 int getNumBodies() const;
217 
221 int getNumConstraints() const;
222 
225 int getNumMobilities() const;
226 
229 int getTotalQAlloc() const;
230 
234 int getTotalMultAlloc() const;
235 
236 
249 MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent,
250  MobilizedBody& child);
251 
262 ConstraintIndex adoptConstraint(Constraint&);
263 
268 { Subsystem::operator=(ss); return *this; }
269 
270 
271 //==============================================================================
286 void setUseEulerAngles(State& state, bool useEulerAngles) const;
287 
290 bool getUseEulerAngles (const State& state) const;
291 
296 int getNumQuaternionsInUse(const State& state) const;
297 
302 bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const;
308 QuaternionPoolIndex getQuaternionPoolIndex(const State& state,
309  MobilizedBodyIndex mobodIx) const;
310 
317 void setConstraintIsDisabled(State& state,
318  ConstraintIndex constraintIx,
319  bool shouldDisableConstraint) const;
320 
324 bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;
325 
332 void convertToEulerAngles(const State& inputState, State& outputState) const;
333 
340 void convertToQuaternions(const State& inputState, State& outputState) const;
341 
349 void normalizeQuaternions(State& state) const;
350 
354 //==============================================================================
368 Real calcSystemMass(const State& s) const;
369 
374 Vec3 calcSystemMassCenterLocationInGround(const State& s) const;
375 
380 MassProperties calcSystemMassPropertiesInGround(const State& s) const;
381 
386 Inertia calcSystemCentralInertiaInGround(const State& s) const;
387 
392 Vec3 calcSystemMassCenterVelocityInGround(const State& s) const;
393 
398 Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const;
399 
406 SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const;
407 
415 SpatialVec calcSystemCentralMomentum(const State& s) const;
416 
421 Real calcKineticEnergy(const State& state) const;
424 //==============================================================================
537 void multiplyBySystemJacobian( const State& state,
538  const Vector& u,
539  Vector_<SpatialVec>& Ju) const;
540 
568 void calcBiasForSystemJacobian(const State& state,
569  Vector_<SpatialVec>& JDotu) const;
570 
571 
575 void calcBiasForSystemJacobian(const State& state,
576  Vector& JDotu) const;
577 
629 void multiplyBySystemJacobianTranspose( const State& state,
630  const Vector_<SpatialVec>& F_G,
631  Vector& f) const;
632 
633 
666 void calcSystemJacobian(const State& state,
667  Matrix_<SpatialVec>& J_G) const; // nb X nu
668 
673 void calcSystemJacobian(const State& state,
674  Matrix& J_G) const; // 6 nb X nu
675 
676 
722 void multiplyByStationJacobian(const State& state,
723  const Array_<MobilizedBodyIndex>& onBodyB,
724  const Array_<Vec3>& stationPInB,
725  const Vector& u,
726  Vector_<Vec3>& JSu) const;
727 
731  MobilizedBodyIndex onBodyB,
732  const Vec3& stationPInB,
733  const Vector& u) const
734 {
735  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
736  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
737  Vector_<Vec3> JSu(1);
738  multiplyByStationJacobian(state, bodies, stations, u, JSu);
739  return JSu[0];
740 }
741 
742 
755 void multiplyByStationJacobianTranspose
756  (const State& state,
757  const Array_<MobilizedBodyIndex>& onBodyB,
758  const Array_<Vec3>& stationPInB,
759  const Vector_<Vec3>& f_GP,
760  Vector& f) const;
761 
763 void multiplyByStationJacobianTranspose
764  (const State& state,
765  MobilizedBodyIndex onBodyB,
766  const Vec3& stationPInB,
767  const Vec3& f_GP,
768  Vector& f) const
769 {
770  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
771  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
772  Vector_<Vec3> forces(1, f_GP);
773  multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
774 }
775 
816 void calcStationJacobian(const State& state,
817  const Array_<MobilizedBodyIndex>& onBodyB,
818  const Array_<Vec3>& stationPInB,
819  Matrix_<Vec3>& JS) const;
820 
822 void calcStationJacobian(const State& state,
823  MobilizedBodyIndex onBodyB,
824  const Vec3& stationPInB,
825  RowVector_<Vec3>& JS) const
826 {
827  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
828  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
829  calcStationJacobian(state, bodies, stations, JS);
830 }
831 
835 void calcStationJacobian(const State& state,
836  const Array_<MobilizedBodyIndex>& onBodyB,
837  const Array_<Vec3>& stationPInB,
838  Matrix& JS) const;
839 
841 void calcStationJacobian(const State& state,
842  MobilizedBodyIndex onBodyB,
843  const Vec3& stationPInB,
844  Matrix& JS) const
845 {
846  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
847  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
848  calcStationJacobian(state, bodies, stations, JS);
849 }
850 
851 
888 void calcBiasForStationJacobian(const State& state,
889  const Array_<MobilizedBodyIndex>& onBodyB,
890  const Array_<Vec3>& stationPInB,
891  Vector_<Vec3>& JSDotu) const;
892 
896 void calcBiasForStationJacobian(const State& state,
897  const Array_<MobilizedBodyIndex>& onBodyB,
898  const Array_<Vec3>& stationPInB,
899  Vector& JSDotu) const;
900 
904  MobilizedBodyIndex onBodyB,
905  const Vec3& stationPInB) const
906 {
907  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
908  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
909  Vector_<Vec3> JSDotu(1);
910  calcBiasForStationJacobian(state, bodies, stations, JSDotu);
911  return JSDotu[0];
912 }
913 
914 
971 void multiplyByFrameJacobian
972  (const State& state,
973  const Array_<MobilizedBodyIndex>& onBodyB,
974  const Array_<Vec3>& originAoInB,
975  const Vector& u,
976  Vector_<SpatialVec>& JFu) const;
977 
982  MobilizedBodyIndex onBodyB,
983  const Vec3& originAoInB,
984  const Vector& u) const
985 {
986  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
987  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
988  Vector_<SpatialVec> JFu(1);
989  multiplyByFrameJacobian(state, bodies, stations, u, JFu);
990  return JFu[0];
991 }
992 
993 
1036 void multiplyByFrameJacobianTranspose
1037  (const State& state,
1038  const Array_<MobilizedBodyIndex>& onBodyB,
1039  const Array_<Vec3>& originAoInB,
1040  const Vector_<SpatialVec>& F_GAo,
1041  Vector& f) const;
1042 
1046  MobilizedBodyIndex onBodyB,
1047  const Vec3& originAoInB,
1048  const SpatialVec& F_GAo,
1049  Vector& f) const
1050 {
1051  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1052  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1053  const Vector_<SpatialVec> forces(1, F_GAo);
1054  multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
1055 }
1056 
1057 
1058 
1103 void calcFrameJacobian(const State& state,
1104  const Array_<MobilizedBodyIndex>& onBodyB,
1105  const Array_<Vec3>& originAoInB,
1106  Matrix_<SpatialVec>& JF) const;
1107 
1110 void calcFrameJacobian(const State& state,
1111  MobilizedBodyIndex onBodyB,
1112  const Vec3& originAoInB,
1113  RowVector_<SpatialVec>& JF) const
1114 {
1115  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1116  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1117  calcFrameJacobian(state, bodies, stations, JF);
1118 }
1119 
1123 void calcFrameJacobian(const State& state,
1124  const Array_<MobilizedBodyIndex>& onBodyB,
1125  const Array_<Vec3>& originAoInB,
1126  Matrix& JF) const;
1127 
1130 void calcFrameJacobian(const State& state,
1131  MobilizedBodyIndex onBodyB,
1132  const Vec3& originAoInB,
1133  Matrix& JF) const
1134 {
1135  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1136  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1137  calcFrameJacobian(state, bodies, stations, JF);
1138 }
1139 
1182 void calcBiasForFrameJacobian
1183  (const State& state,
1184  const Array_<MobilizedBodyIndex>& onBodyB,
1185  const Array_<Vec3>& originAoInB,
1186  Vector_<SpatialVec>& JFDotu) const;
1187 
1191 void calcBiasForFrameJacobian(const State& state,
1192  const Array_<MobilizedBodyIndex>& onBodyB,
1193  const Array_<Vec3>& originAoInB,
1194  Vector& JFDotu) const;
1195 
1200  MobilizedBodyIndex onBodyB,
1201  const Vec3& originAoInB) const
1202 {
1203  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1204  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1205  Vector_<SpatialVec> JFDotu(1);
1206  calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
1207  return JFDotu[0];
1208 }
1209 
1212 //==============================================================================
1245 void multiplyByM(const State& state, const Vector& a, Vector& Ma) const;
1246 
1326 void multiplyByMInv(const State& state,
1327  const Vector& v,
1328  Vector& MinvV) const;
1329 
1339 void calcM(const State&, Matrix& M) const;
1340 
1354 void calcMInv(const State&, Matrix& MInv) const;
1355 
1400 void calcProjectedMInv(const State& s,
1401  Matrix& GMInvGt) const;
1402 
1447 void solveForConstraintImpulses(const State& state,
1448  const Vector& deltaV,
1449  Vector& impulse) const;
1450 
1451 
1475 void multiplyByG(const State& state,
1476  const Vector& ulike,
1477  Vector& Gulike) const {
1478  Vector bias;
1479  calcBiasForMultiplyByG(state, bias);
1480  multiplyByG(state, ulike, bias, Gulike);
1481 }
1482 
1483 
1487 void multiplyByG(const State& state,
1488  const Vector& ulike,
1489  const Vector& bias,
1490  Vector& Gulike) const;
1491 
1517 void calcBiasForMultiplyByG(const State& state,
1518  Vector& bias) const;
1519 
1533 void calcG(const State& state, Matrix& G) const;
1534 
1535 
1570 void calcBiasForAccelerationConstraints(const State& state,
1571  Vector& bias) const;
1572 
1573 
1607 void multiplyByGTranspose(const State& state,
1608  const Vector& lambda,
1609  Vector& f) const;
1610 
1622 void calcGTranspose(const State&, Matrix& Gt) const;
1623 
1624 
1675 void multiplyByPq(const State& state,
1676  const Vector& qlike,
1677  Vector& PqXqlike) const {
1678  Vector biasp;
1679  calcBiasForMultiplyByPq(state, biasp);
1680  multiplyByPq(state, qlike, biasp, PqXqlike);
1681 }
1682 
1683 
1687 void multiplyByPq(const State& state,
1688  const Vector& qlike,
1689  const Vector& biasp,
1690  Vector& PqXqlike) const;
1691 
1708 void calcBiasForMultiplyByPq(const State& state,
1709  Vector& biasp) const;
1710 
1738 void calcPq(const State& state, Matrix& Pq) const;
1739 
1740 
1773 void multiplyByPqTranspose(const State& state,
1774  const Vector& lambdap,
1775  Vector& f) const;
1776 
1804 void calcPqTranspose(const State& state, Matrix& Pqt) const;
1805 
1822 void calcP(const State& state, Matrix& P) const;
1823 
1827 void calcPt(const State& state, Matrix& Pt) const;
1828 
1829 
1838 void multiplyByN(const State& s, bool transpose,
1839  const Vector& in, Vector& out) const;
1840 
1849 void multiplyByNInv(const State& s, bool transpose,
1850  const Vector& in, Vector& out) const;
1851 
1861 void multiplyByNDot(const State& s, bool transpose,
1862  const Vector& in, Vector& out) const;
1863 
1867 //==============================================================================
1928 void calcAcceleration
1929  (const State& state,
1930  const Vector& appliedMobilityForces,
1931  const Vector_<SpatialVec>& appliedBodyForces,
1932  Vector& udot, // output only; no prescribed motions
1933  Vector_<SpatialVec>& A_GB) const;
1934 
1958 void calcAccelerationIgnoringConstraints
1959  (const State& state,
1960  const Vector& appliedMobilityForces,
1961  const Vector_<SpatialVec>& appliedBodyForces,
1962  Vector& udot,
1963  Vector_<SpatialVec>& A_GB) const;
1964 
1965 
1966 
2021 void calcResidualForceIgnoringConstraints
2022  (const State& state,
2023  const Vector& appliedMobilityForces,
2024  const Vector_<SpatialVec>& appliedBodyForces,
2025  const Vector& knownUdot,
2026  Vector& residualMobilityForces) const;
2027 
2028 
2091 void calcResidualForce
2092  (const State& state,
2093  const Vector& appliedMobilityForces,
2094  const Vector_<SpatialVec>& appliedBodyForces,
2095  const Vector& knownUdot,
2096  const Vector& knownLambda,
2097  Vector& residualMobilityForces) const;
2098 
2099 
2110 void calcCompositeBodyInertias(const State& state,
2112 
2113 
2114 
2152 void calcBodyAccelerationFromUDot(const State& state,
2153  const Vector& knownUDot,
2154  Vector_<SpatialVec>& A_GB) const;
2155 
2156 
2179 void calcConstraintForcesFromMultipliers
2180  (const State& state,
2181  const Vector& multipliers,
2182  Vector_<SpatialVec>& bodyForcesInG,
2183  Vector& mobilityForces) const;
2184 
2267 void calcMobilizerReactionForces
2268  (const State& state,
2269  Vector_<SpatialVec>& forcesAtMInG) const;
2270 
2277 const Vector& getMotionMultipliers(const State& state) const;
2278 
2292 Vector calcMotionErrors(const State& state, const Stage& stage) const;
2293 
2299 void findMotionForces
2300  (const State& state,
2301  Vector& mobilityForces) const;
2302 
2309 const Vector& getConstraintMultipliers(const State& state) const;
2310 
2315 void findConstraintForces
2316  (const State& state,
2317  Vector_<SpatialVec>& bodyForcesInG,
2318  Vector& mobilityForces) const;
2319 
2335 Real calcMotionPower(const State& state) const;
2336 
2363 Real calcConstraintPower(const State& state) const;
2364 
2370 void calcTreeEquivalentMobilityForces(const State&,
2371  const Vector_<SpatialVec>& bodyForces,
2372  Vector& mobilityForces) const;
2373 
2374 
2379 void calcQDot(const State& s,
2380  const Vector& u,
2381  Vector& qdot) const;
2382 
2388 void calcQDotDot(const State& s,
2389  const Vector& udot,
2390  Vector& qdotdot) const;
2391 
2402 void addInStationForce(const State& state,
2403  MobilizedBodyIndex bodyB,
2404  const Vec3& stationOnB,
2405  const Vec3& forceInG,
2406  Vector_<SpatialVec>& bodyForcesInG) const;
2407 
2414 void addInBodyTorque(const State& state,
2415  MobilizedBodyIndex mobodIx,
2416  const Vec3& torqueInG,
2417  Vector_<SpatialVec>& bodyForcesInG) const;
2418 
2427 void addInMobilityForce(const State& state,
2428  MobilizedBodyIndex mobodIx,
2429  MobilizerUIndex which,
2430  Real f,
2431  Vector& mobilityForces) const;
2436 //==============================================================================
2451  // POSITION STAGE realizations //
2452 
2461 void realizeCompositeBodyInertias(const State&) const;
2462 
2472 void realizeArticulatedBodyInertias(const State&) const;
2473 
2474 
2475  // INSTANCE STAGE responses //
2476 
2477 const Array_<QIndex>& getFreeQIndex(const State& state) const;
2478 const Array_<UIndex>& getFreeUIndex(const State& state) const;
2479 const Array_<UIndex>& getFreeUDotIndex(const State& state) const;
2480 const Array_<UIndex>& getKnownUDotIndex(const State& state) const;
2481 void packFreeQ
2482  (const State& s, const Vector& allQ, Vector& packedFreeQ) const;
2483 void unpackFreeQ
2484  (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const;
2485 void packFreeU
2486  (const State& s, const Vector& allU, Vector& packedFreeU) const;
2487 void unpackFreeU
2488  (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const;
2489 
2490 
2491  // POSITION STAGE responses //
2492 
2502 const SpatialInertia&
2503 getCompositeBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
2504 
2515 const ArticulatedInertia&
2516 getArticulatedBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
2517 
2518 
2519  // VELOCITY STAGE responses //
2520 
2525 const SpatialVec&
2526 getGyroscopicForce(const State& state, MobilizedBodyIndex mbx) const;
2527 
2533 const SpatialVec&
2534 getMobilizerCoriolisAcceleration(const State& state,
2535  MobilizedBodyIndex mbx) const;
2536 
2545 const SpatialVec&
2546 getTotalCoriolisAcceleration(const State& state, MobilizedBodyIndex mbx) const;
2547 
2548 
2549  // DYNAMICS STAGE responses //
2550 
2557 const SpatialVec&
2558 getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
2559 
2568 const SpatialVec&
2569 getTotalCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
2574 //==============================================================================
2605 void calcMobilizerReactionForcesUsingFreebodyMethod
2606  (const State& state,
2607  Vector_<SpatialVec>& forcesAtMInG) const;
2608 
2612 void invalidateCompositeBodyInertias(const State& state) const;
2613 
2619 void invalidateArticulatedBodyInertias(const State& state) const;
2623 //==============================================================================
2634 int getNumParticles() const;
2636 
2637 // The generalized coordinates for a particle are always the three measure numbers
2638 // (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized
2639 // speeds are always the three corresponding measure numbers of the particle's
2640 // Ground-relative Cartesian velocity. The generalized applied forces are
2641 // always the three measure numbers of a Ground-relative force vector.
2642 const Vector_<Vec3>& getAllParticleLocations (const State&) const;
2643 const Vector_<Vec3>& getAllParticleVelocities (const State&) const;
2644 
2645 const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {
2646  return getAllParticleLocations(s)[p];
2647 }
2648 const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {
2649  return getAllParticleVelocities(s)[p];
2650 }
2651 
2652 Vector& updAllParticleMasses(State& s) const;
2653 
2654 void setAllParticleMasses(State& s, const Vector& masses) const {
2655  updAllParticleMasses(s) = masses;
2656 }
2657 
2658 // Note that particle generalized coordinates, speeds, and applied forces
2659 // are defined to be the particle Cartesian locations, velocities, and
2660 // applied force vectors, so can be set directly at Stage::Model or higher.
2661 
2662 // These are the only routines that must be provided by the concrete MatterSubsystem.
2663 Vector_<Vec3>& updAllParticleLocations(State&) const;
2664 Vector_<Vec3>& updAllParticleVelocities(State&) const;
2665 
2666 // The following inline routines are provided by the generic MatterSubsystem
2667 // class for convenience.
2668 
2669 Vec3& updParticleLocation(State& s, ParticleIndex p) const {
2670  return updAllParticleLocations(s)[p];
2671 }
2672 Vec3& updParticleVelocity(State& s, ParticleIndex p) const {
2673  return updAllParticleVelocities(s)[p];
2674 }
2675 
2676 void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {
2677  updAllParticleLocations(s)[p] = r;
2678 }
2679 void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {
2680  updAllParticleVelocities(s)[p] = v;
2681 }
2682 
2683 void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const {
2684  updAllParticleLocations(s) = r;
2685 }
2687  updAllParticleVelocities(s) = v;
2688 }
2689 
2690 const Vector& getAllParticleMasses(const State&) const;
2691 
2692 const Vector_<Vec3>& getAllParticleAccelerations(const State&) const;
2693 
2694 const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {
2695  return getAllParticleAccelerations(s)[p];
2696 }
2699 //==============================================================================
2708 private:
2711 void calcSpatialKinematicsFromInternal(const State& state,
2712  const Vector& u,
2713  Vector_<SpatialVec>& Ju) const
2714 { multiplyBySystemJacobian(state,u,Ju); }
2715 
2718 void calcInternalGradientFromSpatial(const State& state,
2719  const Vector_<SpatialVec>& F_G,
2720  Vector& f) const
2721 { multiplyBySystemJacobianTranspose(state, F_G, f); }
2722 
2725 void calcMV(const State& state, const Vector& v, Vector& MV) const
2726 { multiplyByM(state,v,MV); }
2727 
2730 void calcMInverseV(const State& state,
2731  const Vector& v,
2732  Vector& MinvV) const
2733 { multiplyByMInv(state,v,MinvV); }
2734 
2737 void calcPNInv(const State& state, Matrix& PNInv) const;
2738 
2741 void calcGt(const State&, Matrix& Gt) const;
2742 
2743 
2747 //==============================================================================
2748 // Bookkeeping methods and internal types -- hide from Doxygen
2750 public:
2751 class Subtree; // used for working with a connected subgraph of the MobilizedBody tree
2752 class SubtreeResults;
2753 
2754 
2755 SimTK_PIMPL_DOWNCAST(SimbodyMatterSubsystem, Subsystem);
2756 const SimbodyMatterSubsystemRep& getRep() const;
2757 SimbodyMatterSubsystemRep& updRep();
2760 private:
2761 };
2762 
2766 SimTK_SIMBODY_EXPORT std::ostream&
2767 operator<<(std::ostream&, const SimbodyMatterSubsystem&);
2768 
2769 
2770 } // namespace SimTK
2771 
2772 #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:552
PhiMatrixTranspose transpose(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:720
The abstract parent of all Subsystems.
Definition: Subsystem.h:61
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2669
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...
void setAllParticleMasses(State &s, const Vector &masses) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2654
This is a special type of "mobilized" body generated automatically by Simbody as a placeholder for ...
Definition: MobilizedBody_Ground.h:45
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:841
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:50
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2672
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:903
Every Simbody header and source file should include this header before any other Simbody header...
void setParticleVelocity(State &s, ParticleIndex p, const Vec3 &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2679
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:1045
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85
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:730
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:565
This is the handle class for the hidden State implementation.
Definition: State.h:264
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
void setAllParticleVelocities(State &s, const Vector_< Vec3 > &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2686
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
The SimTK::Array_<T> container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50
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:981
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:1675
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:267
Matrix_< Real > Matrix
Variable-size 2D matrix of Real elements; abbreviation for Matrix_<Real>.
Definition: BigMatrix.h:1476
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2648
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 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:822
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:1199
The physical meaning of an inertia is the distribution of a rigid body's mass about a particular poin...
Definition: MassProperties.h:82
MobilizedBody::Ground & Ground()
This is a synonym for updGround() that makes for nicer-looking examples.
Definition: SimbodyMatterSubsystem.h:185
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:48
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2683
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72
SimbodyMatterSubsystem(const SimbodyMatterSubsystem &ss)
Copy constructor is not very useful.
Definition: SimbodyMatterSubsystem.h:265
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:167
Vector_< Real > Vector
Variable-size column vector of Real elements; abbreviation for Vector_<Real>.
Definition: BigMatrix.h:1471
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:158
Represents a variable size row vector; much less common than the column vector type Vector_...
Definition: BigMatrix.h:174
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83
const Vec3 & getParticleAcceleration(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2694
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:84
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:1130
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:130
void setParticleLocation(State &s, ParticleIndex p, const Vec3 &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2676
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:1110
const Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2645
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:1475
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...
Subsystem & operator=(const Subsystem &)