Simbody  3.5
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 
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 
218 int getNumBodies() const;
219 
223 int getNumConstraints() const;
224 
227 int getNumMobilities() const;
228 
231 int getTotalQAlloc() const;
232 
236 int getTotalMultAlloc() const;
237 
238 
251 MobilizedBodyIndex adoptMobilizedBody(MobilizedBodyIndex parent,
252  MobilizedBody& child);
253 
264 ConstraintIndex adoptConstraint(Constraint&);
265 
267 UnilateralContactIndex adoptUnilateralContact(UnilateralContact*);
268 int getNumUnilateralContacts() const;
269 const UnilateralContact& getUnilateralContact(UnilateralContactIndex) const;
270 UnilateralContact& updUnilateralContact(UnilateralContactIndex);
272 StateLimitedFrictionIndex adoptStateLimitedFriction(StateLimitedFriction*);
273 int getNumStateLimitedFrictions() const;
275 getStateLimitedFriction(StateLimitedFrictionIndex) const;
277 updStateLimitedFriction(StateLimitedFrictionIndex);
278 
283 { Subsystem::operator=(ss); return *this; }
284 
285 
286 //==============================================================================
301 void setUseEulerAngles(State& state, bool useEulerAngles) const;
302 
305 bool getUseEulerAngles (const State& state) const;
306 
311 int getNumQuaternionsInUse(const State& state) const;
312 
317 bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const;
323 QuaternionPoolIndex getQuaternionPoolIndex(const State& state,
324  MobilizedBodyIndex mobodIx) const;
325 
332 void setConstraintIsDisabled(State& state,
333  ConstraintIndex constraintIx,
334  bool shouldDisableConstraint) const;
335 
339 bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;
340 
347 void convertToEulerAngles(const State& inputState, State& outputState) const;
348 
355 void convertToQuaternions(const State& inputState, State& outputState) const;
356 
364 void normalizeQuaternions(State& state) const;
365 
369 //==============================================================================
383 Real calcSystemMass(const State& s) const;
384 
389 Vec3 calcSystemMassCenterLocationInGround(const State& s) const;
390 
395 MassProperties calcSystemMassPropertiesInGround(const State& s) const;
396 
401 Inertia calcSystemCentralInertiaInGround(const State& s) const;
402 
407 Vec3 calcSystemMassCenterVelocityInGround(const State& s) const;
408 
413 Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const;
414 
421 SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const;
422 
430 SpatialVec calcSystemCentralMomentum(const State& s) const;
431 
436 Real calcKineticEnergy(const State& state) const;
439 //==============================================================================
552 void multiplyBySystemJacobian( const State& state,
553  const Vector& u,
554  Vector_<SpatialVec>& Ju) const;
555 
583 void calcBiasForSystemJacobian(const State& state,
584  Vector_<SpatialVec>& JDotu) const;
585 
586 
590 void calcBiasForSystemJacobian(const State& state,
591  Vector& JDotu) const;
592 
644 void multiplyBySystemJacobianTranspose( const State& state,
645  const Vector_<SpatialVec>& F_G,
646  Vector& f) const;
647 
648 
681 void calcSystemJacobian(const State& state,
682  Matrix_<SpatialVec>& J_G) const; // nb X nu
683 
688 void calcSystemJacobian(const State& state,
689  Matrix& J_G) const; // 6 nb X nu
690 
691 
737 void multiplyByStationJacobian(const State& state,
738  const Array_<MobilizedBodyIndex>& onBodyB,
739  const Array_<Vec3>& stationPInB,
740  const Vector& u,
741  Vector_<Vec3>& JSu) const;
742 
746  MobilizedBodyIndex onBodyB,
747  const Vec3& stationPInB,
748  const Vector& u) const
749 {
750  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
751  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
752  Vector_<Vec3> JSu(1);
753  multiplyByStationJacobian(state, bodies, stations, u, JSu);
754  return JSu[0];
755 }
756 
757 
770 void multiplyByStationJacobianTranspose
771  (const State& state,
772  const Array_<MobilizedBodyIndex>& onBodyB,
773  const Array_<Vec3>& stationPInB,
774  const Vector_<Vec3>& f_GP,
775  Vector& f) const;
776 
778 void multiplyByStationJacobianTranspose
779  (const State& state,
780  MobilizedBodyIndex onBodyB,
781  const Vec3& stationPInB,
782  const Vec3& f_GP,
783  Vector& f) const
784 {
785  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
786  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
787  Vector_<Vec3> forces(1, f_GP);
788  multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
789 }
790 
831 void calcStationJacobian(const State& state,
832  const Array_<MobilizedBodyIndex>& onBodyB,
833  const Array_<Vec3>& stationPInB,
834  Matrix_<Vec3>& JS) const;
835 
837 void calcStationJacobian(const State& state,
838  MobilizedBodyIndex onBodyB,
839  const Vec3& stationPInB,
840  RowVector_<Vec3>& JS) const
841 {
842  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
843  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
844  calcStationJacobian(state, bodies, stations, JS);
845 }
846 
850 void calcStationJacobian(const State& state,
851  const Array_<MobilizedBodyIndex>& onBodyB,
852  const Array_<Vec3>& stationPInB,
853  Matrix& JS) const;
854 
856 void calcStationJacobian(const State& state,
857  MobilizedBodyIndex onBodyB,
858  const Vec3& stationPInB,
859  Matrix& JS) const
860 {
861  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
862  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
863  calcStationJacobian(state, bodies, stations, JS);
864 }
865 
866 
903 void calcBiasForStationJacobian(const State& state,
904  const Array_<MobilizedBodyIndex>& onBodyB,
905  const Array_<Vec3>& stationPInB,
906  Vector_<Vec3>& JSDotu) const;
907 
911 void calcBiasForStationJacobian(const State& state,
912  const Array_<MobilizedBodyIndex>& onBodyB,
913  const Array_<Vec3>& stationPInB,
914  Vector& JSDotu) const;
915 
919  MobilizedBodyIndex onBodyB,
920  const Vec3& stationPInB) const
921 {
922  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
923  ArrayViewConst_<Vec3> stations(&stationPInB, &stationPInB+1);
924  Vector_<Vec3> JSDotu(1);
925  calcBiasForStationJacobian(state, bodies, stations, JSDotu);
926  return JSDotu[0];
927 }
928 
929 
986 void multiplyByFrameJacobian
987  (const State& state,
988  const Array_<MobilizedBodyIndex>& onBodyB,
989  const Array_<Vec3>& originAoInB,
990  const Vector& u,
991  Vector_<SpatialVec>& JFu) const;
992 
997  MobilizedBodyIndex onBodyB,
998  const Vec3& originAoInB,
999  const Vector& u) const
1000 {
1001  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1002  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1003  Vector_<SpatialVec> JFu(1);
1004  multiplyByFrameJacobian(state, bodies, stations, u, JFu);
1005  return JFu[0];
1006 }
1007 
1008 
1051 void multiplyByFrameJacobianTranspose
1052  (const State& state,
1053  const Array_<MobilizedBodyIndex>& onBodyB,
1054  const Array_<Vec3>& originAoInB,
1055  const Vector_<SpatialVec>& F_GAo,
1056  Vector& f) const;
1057 
1061  MobilizedBodyIndex onBodyB,
1062  const Vec3& originAoInB,
1063  const SpatialVec& F_GAo,
1064  Vector& f) const
1065 {
1066  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1067  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1068  const Vector_<SpatialVec> forces(1, F_GAo);
1069  multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
1070 }
1071 
1072 
1073 
1118 void calcFrameJacobian(const State& state,
1119  const Array_<MobilizedBodyIndex>& onBodyB,
1120  const Array_<Vec3>& originAoInB,
1121  Matrix_<SpatialVec>& JF) const;
1122 
1125 void calcFrameJacobian(const State& state,
1126  MobilizedBodyIndex onBodyB,
1127  const Vec3& originAoInB,
1128  RowVector_<SpatialVec>& JF) const
1129 {
1130  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1131  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1132  calcFrameJacobian(state, bodies, stations, JF);
1133 }
1134 
1138 void calcFrameJacobian(const State& state,
1139  const Array_<MobilizedBodyIndex>& onBodyB,
1140  const Array_<Vec3>& originAoInB,
1141  Matrix& JF) const;
1142 
1145 void calcFrameJacobian(const State& state,
1146  MobilizedBodyIndex onBodyB,
1147  const Vec3& originAoInB,
1148  Matrix& JF) const
1149 {
1150  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1151  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1152  calcFrameJacobian(state, bodies, stations, JF);
1153 }
1154 
1197 void calcBiasForFrameJacobian
1198  (const State& state,
1199  const Array_<MobilizedBodyIndex>& onBodyB,
1200  const Array_<Vec3>& originAoInB,
1201  Vector_<SpatialVec>& JFDotu) const;
1202 
1206 void calcBiasForFrameJacobian(const State& state,
1207  const Array_<MobilizedBodyIndex>& onBodyB,
1208  const Array_<Vec3>& originAoInB,
1209  Vector& JFDotu) const;
1210 
1215  MobilizedBodyIndex onBodyB,
1216  const Vec3& originAoInB) const
1217 {
1218  ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
1219  ArrayViewConst_<Vec3> stations(&originAoInB, &originAoInB+1);
1220  Vector_<SpatialVec> JFDotu(1);
1221  calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
1222  return JFDotu[0];
1223 }
1224 
1227 //==============================================================================
1260 void multiplyByM(const State& state, const Vector& a, Vector& Ma) const;
1261 
1341 void multiplyByMInv(const State& state,
1342  const Vector& v,
1343  Vector& MinvV) const;
1344 
1354 void calcM(const State&, Matrix& M) const;
1355 
1369 void calcMInv(const State&, Matrix& MInv) const;
1370 
1415 void calcProjectedMInv(const State& s,
1416  Matrix& GMInvGt) const;
1417 
1462 void solveForConstraintImpulses(const State& state,
1463  const Vector& deltaV,
1464  Vector& impulse) const;
1465 
1466 
1490 void multiplyByG(const State& state,
1491  const Vector& ulike,
1492  Vector& Gulike) const {
1493  Vector bias;
1494  calcBiasForMultiplyByG(state, bias);
1495  multiplyByG(state, ulike, bias, Gulike);
1496 }
1497 
1498 
1502 void multiplyByG(const State& state,
1503  const Vector& ulike,
1504  const Vector& bias,
1505  Vector& Gulike) const;
1506 
1532 void calcBiasForMultiplyByG(const State& state,
1533  Vector& bias) const;
1534 
1548 void calcG(const State& state, Matrix& G) const;
1549 
1550 
1585 void calcBiasForAccelerationConstraints(const State& state,
1586  Vector& bias) const;
1587 
1588 
1622 void multiplyByGTranspose(const State& state,
1623  const Vector& lambda,
1624  Vector& f) const;
1625 
1637 void calcGTranspose(const State&, Matrix& Gt) const;
1638 
1639 
1690 void multiplyByPq(const State& state,
1691  const Vector& qlike,
1692  Vector& PqXqlike) const {
1693  Vector biasp;
1694  calcBiasForMultiplyByPq(state, biasp);
1695  multiplyByPq(state, qlike, biasp, PqXqlike);
1696 }
1697 
1698 
1702 void multiplyByPq(const State& state,
1703  const Vector& qlike,
1704  const Vector& biasp,
1705  Vector& PqXqlike) const;
1706 
1723 void calcBiasForMultiplyByPq(const State& state,
1724  Vector& biasp) const;
1725 
1753 void calcPq(const State& state, Matrix& Pq) const;
1754 
1755 
1788 void multiplyByPqTranspose(const State& state,
1789  const Vector& lambdap,
1790  Vector& f) const;
1791 
1819 void calcPqTranspose(const State& state, Matrix& Pqt) const;
1820 
1837 void calcP(const State& state, Matrix& P) const;
1838 
1842 void calcPt(const State& state, Matrix& Pt) const;
1843 
1844 
1853 void multiplyByN(const State& s, bool transpose,
1854  const Vector& in, Vector& out) const;
1855 
1864 void multiplyByNInv(const State& s, bool transpose,
1865  const Vector& in, Vector& out) const;
1866 
1876 void multiplyByNDot(const State& s, bool transpose,
1877  const Vector& in, Vector& out) const;
1878 
1882 //==============================================================================
1943 void calcAcceleration
1944  (const State& state,
1945  const Vector& appliedMobilityForces,
1946  const Vector_<SpatialVec>& appliedBodyForces,
1947  Vector& udot, // output only; no prescribed motions
1948  Vector_<SpatialVec>& A_GB) const;
1949 
1973 void calcAccelerationIgnoringConstraints
1974  (const State& state,
1975  const Vector& appliedMobilityForces,
1976  const Vector_<SpatialVec>& appliedBodyForces,
1977  Vector& udot,
1978  Vector_<SpatialVec>& A_GB) const;
1979 
1980 
1981 
2036 void calcResidualForceIgnoringConstraints
2037  (const State& state,
2038  const Vector& appliedMobilityForces,
2039  const Vector_<SpatialVec>& appliedBodyForces,
2040  const Vector& knownUdot,
2041  Vector& residualMobilityForces) const;
2042 
2043 
2106 void calcResidualForce
2107  (const State& state,
2108  const Vector& appliedMobilityForces,
2109  const Vector_<SpatialVec>& appliedBodyForces,
2110  const Vector& knownUdot,
2111  const Vector& knownLambda,
2112  Vector& residualMobilityForces) const;
2113 
2114 
2125 void calcCompositeBodyInertias(const State& state,
2127 
2128 
2129 
2167 void calcBodyAccelerationFromUDot(const State& state,
2168  const Vector& knownUDot,
2169  Vector_<SpatialVec>& A_GB) const;
2170 
2171 
2194 void calcConstraintForcesFromMultipliers
2195  (const State& state,
2196  const Vector& multipliers,
2197  Vector_<SpatialVec>& bodyForcesInG,
2198  Vector& mobilityForces) const;
2199 
2282 void calcMobilizerReactionForces
2283  (const State& state,
2284  Vector_<SpatialVec>& forcesAtMInG) const;
2285 
2292 const Vector& getMotionMultipliers(const State& state) const;
2293 
2307 Vector calcMotionErrors(const State& state, const Stage& stage) const;
2308 
2314 void findMotionForces
2315  (const State& state,
2316  Vector& mobilityForces) const;
2317 
2324 const Vector& getConstraintMultipliers(const State& state) const;
2325 
2330 void findConstraintForces
2331  (const State& state,
2332  Vector_<SpatialVec>& bodyForcesInG,
2333  Vector& mobilityForces) const;
2334 
2350 Real calcMotionPower(const State& state) const;
2351 
2378 Real calcConstraintPower(const State& state) const;
2379 
2385 void calcTreeEquivalentMobilityForces(const State&,
2386  const Vector_<SpatialVec>& bodyForces,
2387  Vector& mobilityForces) const;
2388 
2389 
2394 void calcQDot(const State& s,
2395  const Vector& u,
2396  Vector& qdot) const;
2397 
2403 void calcQDotDot(const State& s,
2404  const Vector& udot,
2405  Vector& qdotdot) const;
2406 
2417 void addInStationForce(const State& state,
2418  MobilizedBodyIndex bodyB,
2419  const Vec3& stationOnB,
2420  const Vec3& forceInG,
2421  Vector_<SpatialVec>& bodyForcesInG) const;
2422 
2429 void addInBodyTorque(const State& state,
2430  MobilizedBodyIndex mobodIx,
2431  const Vec3& torqueInG,
2432  Vector_<SpatialVec>& bodyForcesInG) const;
2433 
2442 void addInMobilityForce(const State& state,
2443  MobilizedBodyIndex mobodIx,
2444  MobilizerUIndex which,
2445  Real f,
2446  Vector& mobilityForces) const;
2451 //==============================================================================
2466  // POSITION STAGE realizations //
2467 
2476 void realizeCompositeBodyInertias(const State&) const;
2477 
2487 void realizeArticulatedBodyInertias(const State&) const;
2488 
2489 
2490  // INSTANCE STAGE responses //
2491 
2492 const Array_<QIndex>& getFreeQIndex(const State& state) const;
2493 const Array_<UIndex>& getFreeUIndex(const State& state) const;
2494 const Array_<UIndex>& getFreeUDotIndex(const State& state) const;
2495 const Array_<UIndex>& getKnownUDotIndex(const State& state) const;
2496 void packFreeQ
2497  (const State& s, const Vector& allQ, Vector& packedFreeQ) const;
2498 void unpackFreeQ
2499  (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const;
2500 void packFreeU
2501  (const State& s, const Vector& allU, Vector& packedFreeU) const;
2502 void unpackFreeU
2503  (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const;
2504 
2505 
2506  // POSITION STAGE responses //
2507 
2517 const SpatialInertia&
2518 getCompositeBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
2519 
2530 const ArticulatedInertia&
2531 getArticulatedBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
2532 
2533 
2534  // VELOCITY STAGE responses //
2535 
2540 const SpatialVec&
2541 getGyroscopicForce(const State& state, MobilizedBodyIndex mbx) const;
2542 
2548 const SpatialVec&
2549 getMobilizerCoriolisAcceleration(const State& state,
2550  MobilizedBodyIndex mbx) const;
2551 
2560 const SpatialVec&
2561 getTotalCoriolisAcceleration(const State& state, MobilizedBodyIndex mbx) const;
2562 
2563 
2564  // DYNAMICS STAGE responses //
2565 
2572 const SpatialVec&
2573 getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
2574 
2583 const SpatialVec&
2584 getTotalCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
2589 //==============================================================================
2620 void calcMobilizerReactionForcesUsingFreebodyMethod
2621  (const State& state,
2622  Vector_<SpatialVec>& forcesAtMInG) const;
2623 
2627 void invalidateCompositeBodyInertias(const State& state) const;
2628 
2634 void invalidateArticulatedBodyInertias(const State& state) const;
2638 //==============================================================================
2649 int getNumParticles() const;
2651 
2652 // The generalized coordinates for a particle are always the three measure numbers
2653 // (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized
2654 // speeds are always the three corresponding measure numbers of the particle's
2655 // Ground-relative Cartesian velocity. The generalized applied forces are
2656 // always the three measure numbers of a Ground-relative force vector.
2657 const Vector_<Vec3>& getAllParticleLocations (const State&) const;
2658 const Vector_<Vec3>& getAllParticleVelocities (const State&) const;
2659 
2660 const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {
2661  return getAllParticleLocations(s)[p];
2662 }
2663 const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {
2664  return getAllParticleVelocities(s)[p];
2665 }
2666 
2667 Vector& updAllParticleMasses(State& s) const;
2668 
2669 void setAllParticleMasses(State& s, const Vector& masses) const {
2670  updAllParticleMasses(s) = masses;
2671 }
2672 
2673 // Note that particle generalized coordinates, speeds, and applied forces
2674 // are defined to be the particle Cartesian locations, velocities, and
2675 // applied force vectors, so can be set directly at Stage::Model or higher.
2676 
2677 // These are the only routines that must be provided by the concrete MatterSubsystem.
2678 Vector_<Vec3>& updAllParticleLocations(State&) const;
2679 Vector_<Vec3>& updAllParticleVelocities(State&) const;
2680 
2681 // The following inline routines are provided by the generic MatterSubsystem
2682 // class for convenience.
2683 
2684 Vec3& updParticleLocation(State& s, ParticleIndex p) const {
2685  return updAllParticleLocations(s)[p];
2686 }
2687 Vec3& updParticleVelocity(State& s, ParticleIndex p) const {
2688  return updAllParticleVelocities(s)[p];
2689 }
2690 
2691 void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {
2692  updAllParticleLocations(s)[p] = r;
2693 }
2694 void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {
2695  updAllParticleVelocities(s)[p] = v;
2696 }
2697 
2698 void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const {
2699  updAllParticleLocations(s) = r;
2700 }
2702  updAllParticleVelocities(s) = v;
2703 }
2704 
2705 const Vector& getAllParticleMasses(const State&) const;
2706 
2707 const Vector_<Vec3>& getAllParticleAccelerations(const State&) const;
2708 
2709 const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {
2710  return getAllParticleAccelerations(s)[p];
2711 }
2714 //==============================================================================
2723 private:
2726 void calcSpatialKinematicsFromInternal(const State& state,
2727  const Vector& u,
2728  Vector_<SpatialVec>& Ju) const
2729 { multiplyBySystemJacobian(state,u,Ju); }
2730 
2733 void calcInternalGradientFromSpatial(const State& state,
2734  const Vector_<SpatialVec>& F_G,
2735  Vector& f) const
2736 { multiplyBySystemJacobianTranspose(state, F_G, f); }
2737 
2740 void calcMV(const State& state, const Vector& v, Vector& MV) const
2741 { multiplyByM(state,v,MV); }
2742 
2745 void calcMInverseV(const State& state,
2746  const Vector& v,
2747  Vector& MinvV) const
2748 { multiplyByMInv(state,v,MinvV); }
2749 
2752 void calcPNInv(const State& state, Matrix& PNInv) const;
2753 
2756 void calcGt(const State&, Matrix& Gt) const;
2757 
2758 
2762 //==============================================================================
2763 // Bookkeeping methods and internal types -- hide from Doxygen
2765 public:
2766 class Subtree; // used for working with a connected subgraph of the MobilizedBody tree
2767 class SubtreeResults;
2768 
2769 
2771 const SimbodyMatterSubsystemRep& getRep() const;
2772 SimbodyMatterSubsystemRep& updRep();
2775 private:
2776 };
2777 
2781 SimTK_SIMBODY_EXPORT std::ostream&
2782 operator<<(std::ostream&, const SimbodyMatterSubsystem&);
2783 
2784 
2785 } // namespace SimTK
2786 
2787 #endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
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
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2684
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:2669
This is a special type of "mobilized" body generated automatically by Simbody as a placeholder for Gr...
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:856
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:50
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2687
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:918
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:2694
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:1060
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:745
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:593
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...
(Experimental – API will change – use at your own risk) A unilateral contact constraint uses a sing...
Definition: ConditionalConstraint.h:120
void setAllParticleVelocities(State &s, const Vector_< Vec3 > &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2701
TODO: not implemented yet.
Definition: ConditionalConstraint.h:289
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:996
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:1690
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:282
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2663
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:837
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:1214
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:48
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2698
#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:280
A MobilizedBody is Simbody&#39;s fundamental body-and-joint object used to parameterize a system&#39;s motion...
Definition: MobilizedBody.h:167
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
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:2709
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:1145
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133
void setParticleLocation(State &s, ParticleIndex p, const Vec3 &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2691
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:1125
const Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2660
#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:580
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:1490
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...