Simbody  3.5
SimbodyMatterSubsystem.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
00002 #define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                               Simbody(tm)                                  *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2006-12 Stanford University and the Authors.        *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors: Paul Mitiguy                                                 *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00027 #include "SimTKcommon.h"
00028 #include "simbody/internal/common.h"
00029 #include "simbody/internal/MobilizedBody.h"
00030 
00031 #include <cassert>
00032 #include <vector>
00033 #include <iostream>
00034 
00035 class SimbodyMatterSubsystemRep;
00036 
00037 namespace SimTK {
00038 
00039 class MobilizedBody;
00040 class MultibodySystem;
00041 class Constraint;
00042 
00043 class UnilateralContact;
00044 class StateLimitedFriction;
00045 
00133 class SimTK_SIMBODY_EXPORT SimbodyMatterSubsystem : public Subsystem {
00134 public:
00135 
00136 //==============================================================================
00152 explicit SimbodyMatterSubsystem(MultibodySystem&);
00156 SimbodyMatterSubsystem();
00161 ~SimbodyMatterSubsystem() {} // invokes ~Subsystem()
00162 
00167 const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;
00168 
00173 MobilizedBody& updMobilizedBody(MobilizedBodyIndex);
00174 
00177 const MobilizedBody::Ground& getGround() const;
00181 MobilizedBody::Ground& updGround();
00188 MobilizedBody::Ground& Ground() {return updGround();}
00189 
00193 const Constraint& getConstraint(ConstraintIndex) const;
00194 
00198 Constraint& updConstraint(ConstraintIndex);
00199 
00203 void setShowDefaultGeometry(bool show);
00206 bool getShowDefaultGeometry() const;
00207 
00218 int getNumBodies() const;
00219 
00223 int getNumConstraints() const;
00224 
00227 int getNumMobilities() const;
00228 
00231 int getTotalQAlloc() const;
00232 
00236 int getTotalMultAlloc() const;
00237 
00238 
00251 MobilizedBodyIndex   adoptMobilizedBody(MobilizedBodyIndex  parent, 
00252                                         MobilizedBody&      child);
00253 
00264 ConstraintIndex adoptConstraint(Constraint&);
00265 
00267 UnilateralContactIndex adoptUnilateralContact(UnilateralContact*);
00268 int getNumUnilateralContacts() const;
00269 const UnilateralContact& getUnilateralContact(UnilateralContactIndex) const;
00270 UnilateralContact& updUnilateralContact(UnilateralContactIndex);
00272 StateLimitedFrictionIndex adoptStateLimitedFriction(StateLimitedFriction*);
00273 int getNumStateLimitedFrictions() const;
00274 const StateLimitedFriction&
00275 getStateLimitedFriction(StateLimitedFrictionIndex) const;
00276 StateLimitedFriction& 
00277 updStateLimitedFriction(StateLimitedFrictionIndex);
00278 
00280 SimbodyMatterSubsystem(const SimbodyMatterSubsystem& ss) : Subsystem(ss) {}
00282 SimbodyMatterSubsystem& operator=(const SimbodyMatterSubsystem& ss) 
00283 {   Subsystem::operator=(ss); return *this; }
00284 
00285 
00286 //==============================================================================
00301 void setUseEulerAngles(State& state, bool useEulerAngles) const;
00302 
00305 bool getUseEulerAngles  (const State& state) const;
00306 
00311 int  getNumQuaternionsInUse(const State& state) const;
00312 
00317 bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const;
00323 QuaternionPoolIndex getQuaternionPoolIndex(const State& state, 
00324                                            MobilizedBodyIndex mobodIx) const;
00325 
00332 void setConstraintIsDisabled(State&          state, 
00333                              ConstraintIndex constraintIx, 
00334                              bool            shouldDisableConstraint) const;
00335 
00339 bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;
00340 
00347 void convertToEulerAngles(const State& inputState, State& outputState) const;
00348 
00355 void convertToQuaternions(const State& inputState, State& outputState) const; 
00356 
00364 void normalizeQuaternions(State& state) const;
00365 
00369 //==============================================================================
00383 Real calcSystemMass(const State& s) const;
00384 
00389 Vec3 calcSystemMassCenterLocationInGround(const State& s) const;
00390 
00395 MassProperties calcSystemMassPropertiesInGround(const State& s) const;
00396 
00401 Inertia calcSystemCentralInertiaInGround(const State& s) const;
00402 
00407 Vec3 calcSystemMassCenterVelocityInGround(const State& s) const;
00408 
00413 Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const;
00414 
00421 SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const;
00422 
00430 SpatialVec calcSystemCentralMomentum(const State& s) const;
00431 
00436 Real calcKineticEnergy(const State& state) const;
00439 //==============================================================================
00552 void multiplyBySystemJacobian( const State&         state,
00553                                const Vector&        u,
00554                                Vector_<SpatialVec>& Ju) const;
00555 
00583 void calcBiasForSystemJacobian(const State&         state,
00584                                Vector_<SpatialVec>& JDotu) const;
00585 
00586 
00590 void calcBiasForSystemJacobian(const State&         state,
00591                                Vector&              JDotu) const;
00592 
00644 void multiplyBySystemJacobianTranspose( const State&                state,
00645                                         const Vector_<SpatialVec>&  F_G,
00646                                         Vector&                     f) const;
00647 
00648 
00681 void calcSystemJacobian(const State&            state,
00682                         Matrix_<SpatialVec>&    J_G) const; // nb X nu
00683 
00688 void calcSystemJacobian(const State&            state,
00689                         Matrix&                 J_G) const; // 6 nb X nu
00690 
00691 
00737 void multiplyByStationJacobian(const State&                      state,
00738                                const Array_<MobilizedBodyIndex>& onBodyB,
00739                                const Array_<Vec3>&               stationPInB,
00740                                const Vector&                     u,
00741                                Vector_<Vec3>&                    JSu) const;
00742 
00745 Vec3 multiplyByStationJacobian(const State&         state,
00746                                MobilizedBodyIndex   onBodyB,
00747                                const Vec3&          stationPInB,
00748                                const Vector&        u) const
00749 {
00750     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
00751     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
00752     Vector_<Vec3>                       JSu(1);
00753     multiplyByStationJacobian(state, bodies, stations, u, JSu);
00754     return JSu[0];
00755 }
00756 
00757 
00770 void multiplyByStationJacobianTranspose
00771    (const State&                        state,
00772     const Array_<MobilizedBodyIndex>&   onBodyB,
00773     const Array_<Vec3>&                 stationPInB,
00774     const Vector_<Vec3>&                f_GP,
00775     Vector&                             f) const;
00776 
00778 void multiplyByStationJacobianTranspose
00779    (const State&                        state,
00780     MobilizedBodyIndex                  onBodyB,
00781     const Vec3&                         stationPInB,
00782     const Vec3&                         f_GP,
00783     Vector&                             f) const
00784 {
00785     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
00786     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
00787     Vector_<Vec3>                       forces(1, f_GP);
00788     multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);
00789 }
00790 
00831 void calcStationJacobian(const State&                        state,
00832                          const Array_<MobilizedBodyIndex>&   onBodyB,
00833                          const Array_<Vec3>&                 stationPInB,
00834                          Matrix_<Vec3>&                      JS) const;
00835 
00837 void calcStationJacobian(const State&       state,
00838                          MobilizedBodyIndex onBodyB,
00839                          const Vec3&        stationPInB,
00840                          RowVector_<Vec3>&  JS) const
00841 {
00842     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
00843     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
00844     calcStationJacobian(state, bodies, stations, JS);
00845 }
00846 
00850 void calcStationJacobian(const State&                        state,
00851                          const Array_<MobilizedBodyIndex>&   onBodyB,
00852                          const Array_<Vec3>&                 stationPInB,
00853                          Matrix&                             JS) const;
00854 
00856 void calcStationJacobian(const State&       state,
00857                          MobilizedBodyIndex onBodyB,
00858                          const Vec3&        stationPInB,
00859                          Matrix&            JS) const
00860 {
00861     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
00862     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
00863     calcStationJacobian(state, bodies, stations, JS);
00864 }
00865 
00866 
00903 void calcBiasForStationJacobian(const State&                      state,
00904                                 const Array_<MobilizedBodyIndex>& onBodyB,
00905                                 const Array_<Vec3>&               stationPInB,
00906                                 Vector_<Vec3>&                    JSDotu) const;
00907 
00911 void calcBiasForStationJacobian(const State&                      state,
00912                                 const Array_<MobilizedBodyIndex>& onBodyB,
00913                                 const Array_<Vec3>&               stationPInB,
00914                                 Vector&                           JSDotu) const;
00915 
00918 Vec3 calcBiasForStationJacobian(const State&         state,
00919                                 MobilizedBodyIndex   onBodyB,
00920                                 const Vec3&          stationPInB) const
00921 {
00922     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
00923     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);
00924     Vector_<Vec3>                       JSDotu(1);
00925     calcBiasForStationJacobian(state, bodies, stations, JSDotu);
00926     return JSDotu[0];
00927 }
00928 
00929 
00986 void multiplyByFrameJacobian
00987    (const State&                        state,
00988     const Array_<MobilizedBodyIndex>&   onBodyB,
00989     const Array_<Vec3>&                 originAoInB,
00990     const Vector&                       u,
00991     Vector_<SpatialVec>&                JFu) const;
00992 
00996 SpatialVec multiplyByFrameJacobian( const State&         state,
00997                                     MobilizedBodyIndex   onBodyB,
00998                                     const Vec3&          originAoInB,
00999                                     const Vector&        u) const
01000 {
01001     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
01002     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
01003     Vector_<SpatialVec>                 JFu(1);
01004     multiplyByFrameJacobian(state, bodies, stations, u, JFu);
01005     return JFu[0];
01006 }
01007 
01008 
01051 void multiplyByFrameJacobianTranspose
01052    (const State&                        state,
01053     const Array_<MobilizedBodyIndex>&   onBodyB,
01054     const Array_<Vec3>&                 originAoInB,
01055     const Vector_<SpatialVec>&          F_GAo,
01056     Vector&                             f) const;
01057 
01060 void multiplyByFrameJacobianTranspose(  const State&        state,
01061                                         MobilizedBodyIndex  onBodyB,
01062                                         const Vec3&         originAoInB,
01063                                         const SpatialVec&   F_GAo,
01064                                         Vector&             f) const
01065 {
01066     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
01067     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
01068     const Vector_<SpatialVec>           forces(1, F_GAo);
01069     multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);
01070 }
01071 
01072 
01073 
01118 void calcFrameJacobian(const State&                         state,
01119                        const Array_<MobilizedBodyIndex>&    onBodyB,
01120                        const Array_<Vec3>&                  originAoInB,
01121                        Matrix_<SpatialVec>&                 JF) const;
01122 
01125 void calcFrameJacobian(const State&             state,
01126                        MobilizedBodyIndex       onBodyB,
01127                        const Vec3&              originAoInB,
01128                        RowVector_<SpatialVec>&  JF) const
01129 {
01130     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
01131     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
01132     calcFrameJacobian(state, bodies, stations, JF);
01133 }
01134 
01138 void calcFrameJacobian(const State&                         state,
01139                        const Array_<MobilizedBodyIndex>&    onBodyB,
01140                        const Array_<Vec3>&                  originAoInB,
01141                        Matrix&                              JF) const;
01142 
01145 void calcFrameJacobian(const State&             state,
01146                        MobilizedBodyIndex       onBodyB,
01147                        const Vec3&              originAoInB,
01148                        Matrix&                  JF) const 
01149 {
01150     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
01151     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
01152     calcFrameJacobian(state, bodies, stations, JF);
01153 }
01154 
01197 void calcBiasForFrameJacobian
01198    (const State&                        state,
01199     const Array_<MobilizedBodyIndex>&   onBodyB,
01200     const Array_<Vec3>&                 originAoInB,
01201     Vector_<SpatialVec>&                JFDotu) const;
01202 
01206 void calcBiasForFrameJacobian(const State&                      state,
01207                               const Array_<MobilizedBodyIndex>& onBodyB,
01208                               const Array_<Vec3>&               originAoInB,
01209                               Vector&                           JFDotu) const;
01210 
01214 SpatialVec calcBiasForFrameJacobian(const State&         state,
01215                                     MobilizedBodyIndex   onBodyB,
01216                                     const Vec3&          originAoInB) const
01217 {
01218     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);
01219     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);
01220     Vector_<SpatialVec>                 JFDotu(1);
01221     calcBiasForFrameJacobian(state, bodies, stations, JFDotu);
01222     return JFDotu[0];
01223 }
01224 
01227 //==============================================================================
01260 void multiplyByM(const State& state, const Vector& a, Vector& Ma) const;
01261 
01341 void multiplyByMInv(const State& state,
01342     const Vector&               v,
01343     Vector&                     MinvV) const;
01344 
01354 void calcM(const State&, Matrix& M) const;
01355 
01369 void calcMInv(const State&, Matrix& MInv) const;
01370 
01415 void calcProjectedMInv(const State&   s,
01416                        Matrix&        GMInvGt) const;
01417 
01462 void solveForConstraintImpulses(const State&     state,
01463                                 const Vector&    deltaV,
01464                                 Vector&          impulse) const;
01465 
01466 
01490 void multiplyByG(const State&  state,
01491                  const Vector& ulike,
01492                  Vector&       Gulike) const {
01493     Vector bias;
01494     calcBiasForMultiplyByG(state, bias);
01495     multiplyByG(state, ulike, bias, Gulike);
01496 }
01497 
01498 
01502 void multiplyByG(const State&  state,
01503                  const Vector& ulike,
01504                  const Vector& bias,
01505                  Vector&       Gulike) const;
01506 
01532 void calcBiasForMultiplyByG(const State& state,
01533                             Vector&      bias) const;
01534 
01548 void calcG(const State& state, Matrix& G) const;
01549 
01550 
01585 void calcBiasForAccelerationConstraints(const State& state,
01586                                         Vector&      bias) const;
01587 
01588 
01622 void multiplyByGTranspose(const State&  state,
01623                           const Vector& lambda,
01624                           Vector&       f) const;
01625     
01637 void calcGTranspose(const State&, Matrix& Gt) const;
01638 
01639 
01690 void multiplyByPq(const State&  state,
01691                   const Vector& qlike,
01692                   Vector&       PqXqlike) const {
01693     Vector biasp;
01694     calcBiasForMultiplyByPq(state, biasp);
01695     multiplyByPq(state, qlike, biasp, PqXqlike);
01696 }
01697 
01698 
01702 void multiplyByPq(const State&  state,
01703                  const Vector&  qlike,
01704                  const Vector&  biasp,
01705                  Vector&        PqXqlike) const;
01706 
01723 void calcBiasForMultiplyByPq(const State& state,
01724                              Vector&      biasp) const;
01725 
01753 void calcPq(const State& state, Matrix& Pq) const;
01754 
01755 
01788 void multiplyByPqTranspose(const State&  state,
01789                            const Vector& lambdap,
01790                            Vector&       f) const;
01791 
01819 void calcPqTranspose(const State& state, Matrix& Pqt) const;
01820 
01837 void calcP(const State& state, Matrix& P) const;
01838 
01842 void calcPt(const State& state, Matrix& Pt) const;
01843 
01844 
01853 void multiplyByN(const State& s, bool transpose, 
01854                  const Vector& in, Vector& out) const;
01855 
01864 void multiplyByNInv(const State& s, bool transpose, 
01865                     const Vector& in, Vector& out) const;
01866 
01876 void multiplyByNDot(const State& s, bool transpose, 
01877                     const Vector& in, Vector& out) const;
01878 
01882 //==============================================================================
01943 void calcAcceleration
01944    (const State&               state,
01945     const Vector&              appliedMobilityForces,
01946     const Vector_<SpatialVec>& appliedBodyForces,
01947     Vector&                    udot,    // output only; no prescribed motions
01948     Vector_<SpatialVec>&       A_GB) const;
01949 
01973 void calcAccelerationIgnoringConstraints
01974    (const State&                state,
01975     const Vector&               appliedMobilityForces,
01976     const Vector_<SpatialVec>&  appliedBodyForces,
01977     Vector&                     udot,    
01978     Vector_<SpatialVec>&        A_GB) const;
01979 
01980 
01981 
02036 void calcResidualForceIgnoringConstraints
02037    (const State&               state,
02038     const Vector&              appliedMobilityForces,
02039     const Vector_<SpatialVec>& appliedBodyForces,
02040     const Vector&              knownUdot,
02041     Vector&                    residualMobilityForces) const;
02042 
02043 
02106 void calcResidualForce
02107    (const State&               state,
02108     const Vector&              appliedMobilityForces,
02109     const Vector_<SpatialVec>& appliedBodyForces,
02110     const Vector&              knownUdot,
02111     const Vector&              knownLambda,
02112     Vector&                    residualMobilityForces) const;
02113 
02114 
02125 void calcCompositeBodyInertias(const State&    state,
02126     Array_<SpatialInertia,MobilizedBodyIndex>& R) const;
02127 
02128 
02129 
02167 void calcBodyAccelerationFromUDot(const State&         state,
02168                                   const Vector&        knownUDot,
02169                                   Vector_<SpatialVec>& A_GB) const;
02170 
02171 
02194 void calcConstraintForcesFromMultipliers
02195   (const State&         state, 
02196    const Vector&        multipliers,
02197    Vector_<SpatialVec>& bodyForcesInG,
02198    Vector&              mobilityForces) const;
02199 
02282 void calcMobilizerReactionForces
02283    (const State&         state, 
02284     Vector_<SpatialVec>& forcesAtMInG) const;
02285 
02292 const Vector& getMotionMultipliers(const State& state) const;
02293 
02307 Vector calcMotionErrors(const State& state, const Stage& stage) const;
02308 
02314 void findMotionForces
02315    (const State&         state,
02316     Vector&              mobilityForces) const;
02317 
02324 const Vector& getConstraintMultipliers(const State& state) const;
02325 
02330 void findConstraintForces
02331   (const State&         state, 
02332    Vector_<SpatialVec>& bodyForcesInG,
02333    Vector&              mobilityForces) const;
02334 
02350 Real calcMotionPower(const State& state) const;
02351 
02378 Real calcConstraintPower(const State& state) const;
02379 
02385 void calcTreeEquivalentMobilityForces(const State&, 
02386     const Vector_<SpatialVec>& bodyForces,
02387     Vector&                    mobilityForces) const;
02388 
02389 
02394 void calcQDot(const State& s,
02395     const Vector& u,
02396     Vector&       qdot) const;
02397 
02403 void calcQDotDot(const State& s,
02404     const Vector& udot,
02405     Vector&       qdotdot) const;
02406 
02417 void addInStationForce(const State&         state, 
02418                        MobilizedBodyIndex   bodyB, 
02419                        const Vec3&          stationOnB, 
02420                        const Vec3&          forceInG, 
02421                        Vector_<SpatialVec>& bodyForcesInG) const;
02422 
02429 void addInBodyTorque(const State&           state, 
02430                      MobilizedBodyIndex     mobodIx, 
02431                      const Vec3&            torqueInG, 
02432                      Vector_<SpatialVec>&   bodyForcesInG) const;
02433 
02442 void addInMobilityForce(const State&        state,
02443                         MobilizedBodyIndex  mobodIx, 
02444                         MobilizerUIndex     which, 
02445                         Real                f,
02446                         Vector&             mobilityForces) const;
02451 //==============================================================================
02466     // POSITION STAGE realizations //
02467 
02476 void realizeCompositeBodyInertias(const State&) const;
02477 
02487 void realizeArticulatedBodyInertias(const State&) const;
02488 
02489 
02490     // INSTANCE STAGE responses //
02491 
02492 const Array_<QIndex>& getFreeQIndex(const State& state) const;
02493 const Array_<UIndex>& getFreeUIndex(const State& state) const;
02494 const Array_<UIndex>& getFreeUDotIndex(const State& state) const;
02495 const Array_<UIndex>& getKnownUDotIndex(const State& state) const;
02496 void packFreeQ
02497    (const State& s, const Vector& allQ, Vector& packedFreeQ) const;
02498 void unpackFreeQ
02499    (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const;
02500 void packFreeU
02501    (const State& s, const Vector& allU, Vector& packedFreeU) const;
02502 void unpackFreeU
02503    (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const;
02504 
02505 
02506     // POSITION STAGE responses //
02507 
02517 const SpatialInertia& 
02518 getCompositeBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
02519 
02530 const ArticulatedInertia& 
02531 getArticulatedBodyInertia(const State& state, MobilizedBodyIndex mbx) const;
02532 
02533 
02534     // VELOCITY STAGE responses //
02535 
02540 const SpatialVec& 
02541 getGyroscopicForce(const State& state, MobilizedBodyIndex mbx) const;
02542 
02548 const SpatialVec& 
02549 getMobilizerCoriolisAcceleration(const State&       state, 
02550                                  MobilizedBodyIndex mbx) const;
02551 
02560 const SpatialVec& 
02561 getTotalCoriolisAcceleration(const State& state, MobilizedBodyIndex mbx) const;
02562 
02563 
02564     // DYNAMICS STAGE responses //
02565 
02572 const SpatialVec& 
02573 getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
02574 
02583 const SpatialVec& 
02584 getTotalCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;
02589 //==============================================================================
02620 void calcMobilizerReactionForcesUsingFreebodyMethod
02621    (const State&         state, 
02622     Vector_<SpatialVec>& forcesAtMInG) const;
02623 
02627 void invalidateCompositeBodyInertias(const State& state) const;
02628 
02634 void invalidateArticulatedBodyInertias(const State& state) const;
02638 //==============================================================================
02649 
02650 int getNumParticles() const;
02651 
02652 // The generalized coordinates for a particle are always the three measure numbers
02653 // (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized
02654 // speeds are always the three corresponding measure numbers of the particle's
02655 // Ground-relative Cartesian velocity. The generalized applied forces are
02656 // always the three measure numbers of a Ground-relative force vector.
02657 const Vector_<Vec3>& getAllParticleLocations    (const State&) const;
02658 const Vector_<Vec3>& getAllParticleVelocities   (const State&) const;
02659 
02660 const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {
02661     return getAllParticleLocations(s)[p];
02662 }
02663 const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {
02664     return getAllParticleVelocities(s)[p];
02665 }
02666 
02667 Vector& updAllParticleMasses(State& s) const;
02668 
02669 void setAllParticleMasses(State& s, const Vector& masses) const {
02670     updAllParticleMasses(s) = masses;
02671 }
02672 
02673 // Note that particle generalized coordinates, speeds, and applied forces
02674 // are defined to be the particle Cartesian locations, velocities, and
02675 // applied force vectors, so can be set directly at Stage::Model or higher.
02676 
02677 // These are the only routines that must be provided by the concrete MatterSubsystem.
02678 Vector_<Vec3>& updAllParticleLocations(State&)     const;
02679 Vector_<Vec3>& updAllParticleVelocities(State&)    const;
02680 
02681 // The following inline routines are provided by the generic MatterSubsystem 
02682 // class for convenience.
02683 
02684 Vec3& updParticleLocation(State& s, ParticleIndex p) const {
02685     return updAllParticleLocations(s)[p];
02686 }
02687 Vec3& updParticleVelocity(State& s, ParticleIndex p) const {
02688     return updAllParticleVelocities(s)[p];
02689 }
02690 
02691 void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {
02692     updAllParticleLocations(s)[p] = r;
02693 }
02694 void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {
02695     updAllParticleVelocities(s)[p] = v;
02696 }
02697 
02698 void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const {
02699     updAllParticleLocations(s) = r;
02700 }
02701 void setAllParticleVelocities(State& s, const Vector_<Vec3>& v) const {
02702     updAllParticleVelocities(s) = v;
02703 }
02704 
02705 const Vector& getAllParticleMasses(const State&) const;
02706 
02707 const Vector_<Vec3>& getAllParticleAccelerations(const State&) const;
02708 
02709 const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {
02710     return getAllParticleAccelerations(s)[p];
02711 }
02714 //==============================================================================
02723 private:
02726 void calcSpatialKinematicsFromInternal(const State&         state,
02727                                        const Vector&        u,
02728                                        Vector_<SpatialVec>& Ju) const
02729 {   multiplyBySystemJacobian(state,u,Ju); }
02730 
02733 void calcInternalGradientFromSpatial(const State&               state,
02734                                      const Vector_<SpatialVec>& F_G,
02735                                      Vector&                    f) const
02736 {   multiplyBySystemJacobianTranspose(state, F_G, f); }
02737 
02740 void calcMV(const State& state, const Vector& v, Vector& MV) const
02741 {   multiplyByM(state,v,MV); }
02742 
02745 void calcMInverseV(const State& state,
02746     const Vector&               v,
02747     Vector&                     MinvV) const
02748 {   multiplyByMInv(state,v,MinvV); }
02749 
02752 void calcPNInv(const State& state, Matrix& PNInv) const;
02753 
02756 void calcGt(const State&, Matrix& Gt) const;
02757 
02758 
02762 //==============================================================================
02763 //     Bookkeeping methods and internal types -- hide from Doxygen
02765 public:
02766 class Subtree; // used for working with a connected subgraph of the MobilizedBody tree
02767 class SubtreeResults;
02768 
02769 
02770 SimTK_PIMPL_DOWNCAST(SimbodyMatterSubsystem, Subsystem);
02771 const SimbodyMatterSubsystemRep& getRep() const;
02772 SimbodyMatterSubsystemRep&       updRep();
02775 private:
02776 };
02777 
02781 SimTK_SIMBODY_EXPORT std::ostream& 
02782 operator<<(std::ostream&, const SimbodyMatterSubsystem&);
02783 
02784 
02785 } // namespace SimTK
02786 
02787 #endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines