simbody/3.5/SimbodyMatterSubsystem_8h_source.html

 #ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_

 #define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_


 /* -------------------------------------------------------------------------- *

  *                               Simbody(tm)                                  *

  * -------------------------------------------------------------------------- *

  * This is part of the SimTK biosimulation toolkit originating from           *

  * Simbios, the NIH National Center for Physics-Based Simulation of           *

  * Biological Structures at Stanford, funded under the NIH Roadmap for        *

  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *

  *                                                                            *

  * Portions copyright (c) 2006-12 Stanford University and the Authors.        *

  * Authors: Michael Sherman                                                   *

  * Contributors: Paul Mitiguy                                                 *

  *                                                                            *

  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *

  * not use this file except in compliance with the License. You may obtain a  *

  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *

  *                                                                            *

  * Unless required by applicable law or agreed to in writing, software        *

  * distributed under the License is distributed on an "AS IS" BASIS,          *

  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *

  * See the License for the specific language governing permissions and        *

  * limitations under the License.                                             *

  * -------------------------------------------------------------------------- */


 #include "SimTKcommon.h"

 #include "simbody/internal/common.h"

 #include "simbody/internal/MobilizedBody.h"


 #include <cassert>

 #include <vector>

 #include <iostream>


 class SimbodyMatterSubsystemRep;


 namespace SimTK {


 class MobilizedBody;

 class MultibodySystem;

 class Constraint;


 class UnilateralContact;

 class StateLimitedFriction;


 class SimTK_SIMBODY_EXPORT SimbodyMatterSubsystem : public Subsystem {

 public:


 //==============================================================================

 explicit SimbodyMatterSubsystem(MultibodySystem&);

 SimbodyMatterSubsystem();

 ~SimbodyMatterSubsystem() {} // invokes ~Subsystem()


 const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;


 MobilizedBody& updMobilizedBody(MobilizedBodyIndex);


 const MobilizedBody::Ground& getGround() const;

 MobilizedBody::Ground& updGround();

 MobilizedBody::Ground& Ground() {return updGround();}


 const Constraint& getConstraint(ConstraintIndex) const;


 Constraint& updConstraint(ConstraintIndex);


 void setShowDefaultGeometry(bool show);

 bool getShowDefaultGeometry() const;


 int getNumBodies() const;


 int getNumConstraints() const;


 int getNumMobilities() const;


 int getTotalQAlloc() const;


 int getTotalMultAlloc() const;


 MobilizedBodyIndex   adoptMobilizedBody(MobilizedBodyIndex  parent,

                                         MobilizedBody&      child);


 ConstraintIndex adoptConstraint(Constraint&);


 UnilateralContactIndex adoptUnilateralContact(UnilateralContact*);

 int getNumUnilateralContacts() const;

 const UnilateralContact& getUnilateralContact(UnilateralContactIndex) const;

 UnilateralContact& updUnilateralContact(UnilateralContactIndex);

 StateLimitedFrictionIndex adoptStateLimitedFriction(StateLimitedFriction*);

 int getNumStateLimitedFrictions() const;

 const StateLimitedFriction&

 getStateLimitedFriction(StateLimitedFrictionIndex) const;

 StateLimitedFriction&

 updStateLimitedFriction(StateLimitedFrictionIndex);


 SimbodyMatterSubsystem(const SimbodyMatterSubsystem& ss) : Subsystem(ss) {}

 SimbodyMatterSubsystem& operator=(const SimbodyMatterSubsystem& ss)

 {   Subsystem::operator=(ss); return *this; }


 //==============================================================================

 void setUseEulerAngles(State& state, bool useEulerAngles) const;


 bool getUseEulerAngles  (const State& state) const;


 int  getNumQuaternionsInUse(const State& state) const;


 bool isUsingQuaternion(const State& state, MobilizedBodyIndex mobodIx) const;

 QuaternionPoolIndex getQuaternionPoolIndex(const State& state,

                                            MobilizedBodyIndex mobodIx) const;


 void setConstraintIsDisabled(State&          state,

                              ConstraintIndex constraintIx,

                              bool            shouldDisableConstraint) const;


 bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;


 void convertToEulerAngles(const State& inputState, State& outputState) const;


 void convertToQuaternions(const State& inputState, State& outputState) const;


 void normalizeQuaternions(State& state) const;


 //==============================================================================

 Real calcSystemMass(const State& s) const;


 Vec3 calcSystemMassCenterLocationInGround(const State& s) const;


 MassProperties calcSystemMassPropertiesInGround(const State& s) const;


 Inertia calcSystemCentralInertiaInGround(const State& s) const;


 Vec3 calcSystemMassCenterVelocityInGround(const State& s) const;


 Vec3 calcSystemMassCenterAccelerationInGround(const State& s) const;


 SpatialVec calcSystemMomentumAboutGroundOrigin(const State& s) const;


 SpatialVec calcSystemCentralMomentum(const State& s) const;


 Real calcKineticEnergy(const State& state) const;

 //==============================================================================

 void multiplyBySystemJacobian( const State&         state,

                                const Vector&        u,

                                Vector_<SpatialVec>& Ju) const;


 void calcBiasForSystemJacobian(const State&         state,

                                Vector_<SpatialVec>& JDotu) const;


 void calcBiasForSystemJacobian(const State&         state,

                                Vector&              JDotu) const;


 void multiplyBySystemJacobianTranspose( const State&                state,

                                         const Vector_<SpatialVec>&  F_G,

                                         Vector&                     f) const;


 void calcSystemJacobian(const State&            state,

                         Matrix_<SpatialVec>&    J_G) const; // nb X nu


 void calcSystemJacobian(const State&            state,

                         Matrix&                 J_G) const; // 6 nb X nu


 void multiplyByStationJacobian(const State&                      state,

                                const Array_<MobilizedBodyIndex>& onBodyB,

                                const Array_<Vec3>&               stationPInB,

                                const Vector&                     u,

                                Vector_<Vec3>&                    JSu) const;


 Vec3 multiplyByStationJacobian(const State&         state,

                                MobilizedBodyIndex   onBodyB,

                                const Vec3&          stationPInB,

                                const Vector&        u) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);

     Vector_<Vec3>                       JSu(1);

     multiplyByStationJacobian(state, bodies, stations, u, JSu);

     return JSu[0];

 }


 void multiplyByStationJacobianTranspose

    (const State&                        state,

     const Array_<MobilizedBodyIndex>&   onBodyB,

     const Array_<Vec3>&                 stationPInB,

     const Vector_<Vec3>&                f_GP,

     Vector&                             f) const;


 void multiplyByStationJacobianTranspose

    (const State&                        state,

     MobilizedBodyIndex                  onBodyB,

     const Vec3&                         stationPInB,

     const Vec3&                         f_GP,

     Vector&                             f) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);

     Vector_<Vec3>                       forces(1, f_GP);

     multiplyByStationJacobianTranspose(state, bodies, stations, forces, f);

 }


 void calcStationJacobian(const State&                        state,

                          const Array_<MobilizedBodyIndex>&   onBodyB,

                          const Array_<Vec3>&                 stationPInB,

                          Matrix_<Vec3>&                      JS) const;


 void calcStationJacobian(const State&       state,

                          MobilizedBodyIndex onBodyB,

                          const Vec3&        stationPInB,

                          RowVector_<Vec3>&  JS) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);

     calcStationJacobian(state, bodies, stations, JS);

 }


 void calcStationJacobian(const State&                        state,

                          const Array_<MobilizedBodyIndex>&   onBodyB,

                          const Array_<Vec3>&                 stationPInB,

                          Matrix&                             JS) const;


 void calcStationJacobian(const State&       state,

                          MobilizedBodyIndex onBodyB,

                          const Vec3&        stationPInB,

                          Matrix&            JS) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);

     calcStationJacobian(state, bodies, stations, JS);

 }


 void calcBiasForStationJacobian(const State&                      state,

                                 const Array_<MobilizedBodyIndex>& onBodyB,

                                 const Array_<Vec3>&               stationPInB,

                                 Vector_<Vec3>&                    JSDotu) const;


 void calcBiasForStationJacobian(const State&                      state,

                                 const Array_<MobilizedBodyIndex>& onBodyB,

                                 const Array_<Vec3>&               stationPInB,

                                 Vector&                           JSDotu) const;


 Vec3 calcBiasForStationJacobian(const State&         state,

                                 MobilizedBodyIndex   onBodyB,

                                 const Vec3&          stationPInB) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&stationPInB, &stationPInB+1);

     Vector_<Vec3>                       JSDotu(1);

     calcBiasForStationJacobian(state, bodies, stations, JSDotu);

     return JSDotu[0];

 }


 void multiplyByFrameJacobian

    (const State&                        state,

     const Array_<MobilizedBodyIndex>&   onBodyB,

     const Array_<Vec3>&                 originAoInB,

     const Vector&                       u,

     Vector_<SpatialVec>&                JFu) const;


 SpatialVec multiplyByFrameJacobian( const State&         state,

                                     MobilizedBodyIndex   onBodyB,

                                     const Vec3&          originAoInB,

                                     const Vector&        u) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);

     Vector_<SpatialVec>                 JFu(1);

     multiplyByFrameJacobian(state, bodies, stations, u, JFu);

     return JFu[0];

 }


 void multiplyByFrameJacobianTranspose

    (const State&                        state,

     const Array_<MobilizedBodyIndex>&   onBodyB,

     const Array_<Vec3>&                 originAoInB,

     const Vector_<SpatialVec>&          F_GAo,

     Vector&                             f) const;


 void multiplyByFrameJacobianTranspose(  const State&        state,

                                         MobilizedBodyIndex  onBodyB,

                                         const Vec3&         originAoInB,

                                         const SpatialVec&   F_GAo,

                                         Vector&             f) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);

     const Vector_<SpatialVec>           forces(1, F_GAo);

     multiplyByFrameJacobianTranspose(state, bodies, stations, forces, f);

 }


 void calcFrameJacobian(const State&                         state,

                        const Array_<MobilizedBodyIndex>&    onBodyB,

                        const Array_<Vec3>&                  originAoInB,

                        Matrix_<SpatialVec>&                 JF) const;


 void calcFrameJacobian(const State&             state,

                        MobilizedBodyIndex       onBodyB,

                        const Vec3&              originAoInB,

                        RowVector_<SpatialVec>&  JF) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);

     calcFrameJacobian(state, bodies, stations, JF);

 }


 void calcFrameJacobian(const State&                         state,

                        const Array_<MobilizedBodyIndex>&    onBodyB,

                        const Array_<Vec3>&                  originAoInB,

                        Matrix&                              JF) const;


 void calcFrameJacobian(const State&             state,

                        MobilizedBodyIndex       onBodyB,

                        const Vec3&              originAoInB,

                        Matrix&                  JF) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);

     calcFrameJacobian(state, bodies, stations, JF);

 }


 void calcBiasForFrameJacobian

    (const State&                        state,

     const Array_<MobilizedBodyIndex>&   onBodyB,

     const Array_<Vec3>&                 originAoInB,

     Vector_<SpatialVec>&                JFDotu) const;


 void calcBiasForFrameJacobian(const State&                      state,

                               const Array_<MobilizedBodyIndex>& onBodyB,

                               const Array_<Vec3>&               originAoInB,

                               Vector&                           JFDotu) const;


 SpatialVec calcBiasForFrameJacobian(const State&         state,

                                     MobilizedBodyIndex   onBodyB,

                                     const Vec3&          originAoInB) const

 {

     ArrayViewConst_<MobilizedBodyIndex> bodies(&onBodyB, &onBodyB+1);

     ArrayViewConst_<Vec3>               stations(&originAoInB, &originAoInB+1);

     Vector_<SpatialVec>                 JFDotu(1);

     calcBiasForFrameJacobian(state, bodies, stations, JFDotu);

     return JFDotu[0];

 }


 //==============================================================================

 void multiplyByM(const State& state, const Vector& a, Vector& Ma) const;


 void multiplyByMInv(const State& state,

     const Vector&               v,

     Vector&                     MinvV) const;


 void calcM(const State&, Matrix& M) const;


 void calcMInv(const State&, Matrix& MInv) const;


 void calcProjectedMInv(const State&   s,

                        Matrix&        GMInvGt) const;


 void solveForConstraintImpulses(const State&     state,

                                 const Vector&    deltaV,

                                 Vector&          impulse) const;


 void multiplyByG(const State&  state,

                  const Vector& ulike,

                  Vector&       Gulike) const {

     Vector bias;

     calcBiasForMultiplyByG(state, bias);

     multiplyByG(state, ulike, bias, Gulike);

 }


 void multiplyByG(const State&  state,

                  const Vector& ulike,

                  const Vector& bias,

                  Vector&       Gulike) const;


 void calcBiasForMultiplyByG(const State& state,

                             Vector&      bias) const;


 void calcG(const State& state, Matrix& G) const;


 void calcBiasForAccelerationConstraints(const State& state,

                                         Vector&      bias) const;


 void multiplyByGTranspose(const State&  state,

                           const Vector& lambda,

                           Vector&       f) const;


 void calcGTranspose(const State&, Matrix& Gt) const;


 void multiplyByPq(const State&  state,

                   const Vector& qlike,

                   Vector&       PqXqlike) const {

     Vector biasp;

     calcBiasForMultiplyByPq(state, biasp);

     multiplyByPq(state, qlike, biasp, PqXqlike);

 }


 void multiplyByPq(const State&  state,

                  const Vector&  qlike,

                  const Vector&  biasp,

                  Vector&        PqXqlike) const;


 void calcBiasForMultiplyByPq(const State& state,

                              Vector&      biasp) const;


 void calcPq(const State& state, Matrix& Pq) const;


 void multiplyByPqTranspose(const State&  state,

                            const Vector& lambdap,

                            Vector&       f) const;


 void calcPqTranspose(const State& state, Matrix& Pqt) const;


 void calcP(const State& state, Matrix& P) const;


 void calcPt(const State& state, Matrix& Pt) const;


 void multiplyByN(const State& s, bool transpose,

                  const Vector& in, Vector& out) const;


 void multiplyByNInv(const State& s, bool transpose,

                     const Vector& in, Vector& out) const;


 void multiplyByNDot(const State& s, bool transpose,

                     const Vector& in, Vector& out) const;


 //==============================================================================

 void calcAcceleration

    (const State&               state,

     const Vector&              appliedMobilityForces,

     const Vector_<SpatialVec>& appliedBodyForces,

     Vector&                    udot,    // output only; no prescribed motions

     Vector_<SpatialVec>&       A_GB) const;


 void calcAccelerationIgnoringConstraints

    (const State&                state,

     const Vector&               appliedMobilityForces,

     const Vector_<SpatialVec>&  appliedBodyForces,

     Vector&                     udot,

     Vector_<SpatialVec>&        A_GB) const;


 void calcResidualForceIgnoringConstraints

    (const State&               state,

     const Vector&              appliedMobilityForces,

     const Vector_<SpatialVec>& appliedBodyForces,

     const Vector&              knownUdot,

     Vector&                    residualMobilityForces) const;


 void calcResidualForce

    (const State&               state,

     const Vector&              appliedMobilityForces,

     const Vector_<SpatialVec>& appliedBodyForces,

     const Vector&              knownUdot,

     const Vector&              knownLambda,

     Vector&                    residualMobilityForces) const;


 void calcCompositeBodyInertias(const State&    state,

     Array_<SpatialInertia,MobilizedBodyIndex>& R) const;


 void calcBodyAccelerationFromUDot(const State&         state,

                                   const Vector&        knownUDot,

                                   Vector_<SpatialVec>& A_GB) const;


 void calcConstraintForcesFromMultipliers

   (const State&         state,

    const Vector&        multipliers,

    Vector_<SpatialVec>& bodyForcesInG,

    Vector&              mobilityForces) const;


 void calcMobilizerReactionForces

    (const State&         state,

     Vector_<SpatialVec>& forcesAtMInG) const;


 const Vector& getMotionMultipliers(const State& state) const;


 Vector calcMotionErrors(const State& state, const Stage& stage) const;


 void findMotionForces

    (const State&         state,

     Vector&              mobilityForces) const;


 const Vector& getConstraintMultipliers(const State& state) const;


 void findConstraintForces

   (const State&         state,

    Vector_<SpatialVec>& bodyForcesInG,

    Vector&              mobilityForces) const;


 Real calcMotionPower(const State& state) const;


 Real calcConstraintPower(const State& state) const;


 void calcTreeEquivalentMobilityForces(const State&,

     const Vector_<SpatialVec>& bodyForces,

     Vector&                    mobilityForces) const;


 void calcQDot(const State& s,

     const Vector& u,

     Vector&       qdot) const;


 void calcQDotDot(const State& s,

     const Vector& udot,

     Vector&       qdotdot) const;


 void addInStationForce(const State&         state,

                        MobilizedBodyIndex   bodyB,

                        const Vec3&          stationOnB,

                        const Vec3&          forceInG,

                        Vector_<SpatialVec>& bodyForcesInG) const;


 void addInBodyTorque(const State&           state,

                      MobilizedBodyIndex     mobodIx,

                      const Vec3&            torqueInG,

                      Vector_<SpatialVec>&   bodyForcesInG) const;


 void addInMobilityForce(const State&        state,

                         MobilizedBodyIndex  mobodIx,

                         MobilizerUIndex     which,

                         Real                f,

                         Vector&             mobilityForces) const;

 //==============================================================================

     // POSITION STAGE realizations //


 void realizeCompositeBodyInertias(const State&) const;


 void realizeArticulatedBodyInertias(const State&) const;


     // INSTANCE STAGE responses //


 const Array_<QIndex>& getFreeQIndex(const State& state) const;

 const Array_<UIndex>& getFreeUIndex(const State& state) const;

 const Array_<UIndex>& getFreeUDotIndex(const State& state) const;

 const Array_<UIndex>& getKnownUDotIndex(const State& state) const;

 void packFreeQ

    (const State& s, const Vector& allQ, Vector& packedFreeQ) const;

 void unpackFreeQ

    (const State& s, const Vector& packedFreeQ, Vector& unpackedFreeQ) const;

 void packFreeU

    (const State& s, const Vector& allU, Vector& packedFreeU) const;

 void unpackFreeU

    (const State& s, const Vector& packedFreeU, Vector& unpackedFreeU) const;


     // POSITION STAGE responses //


 const SpatialInertia&

 getCompositeBodyInertia(const State& state, MobilizedBodyIndex mbx) const;


 const ArticulatedInertia&

 getArticulatedBodyInertia(const State& state, MobilizedBodyIndex mbx) const;


     // VELOCITY STAGE responses //


 const SpatialVec&

 getGyroscopicForce(const State& state, MobilizedBodyIndex mbx) const;


 const SpatialVec&

 getMobilizerCoriolisAcceleration(const State&       state,

                                  MobilizedBodyIndex mbx) const;


 const SpatialVec&

 getTotalCoriolisAcceleration(const State& state, MobilizedBodyIndex mbx) const;


     // DYNAMICS STAGE responses //


 const SpatialVec&

 getMobilizerCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;


 const SpatialVec&

 getTotalCentrifugalForces(const State& state, MobilizedBodyIndex mbx) const;

 //==============================================================================

 void calcMobilizerReactionForcesUsingFreebodyMethod

    (const State&         state,

     Vector_<SpatialVec>& forcesAtMInG) const;


 void invalidateCompositeBodyInertias(const State& state) const;


 void invalidateArticulatedBodyInertias(const State& state) const;

 //==============================================================================

 int getNumParticles() const;


 // The generalized coordinates for a particle are always the three measure numbers

 // (x,y,z) of the particle's Ground-relative Cartesian location vector. The generalized

 // speeds are always the three corresponding measure numbers of the particle's

 // Ground-relative Cartesian velocity. The generalized applied forces are

 // always the three measure numbers of a Ground-relative force vector.

 const Vector_<Vec3>& getAllParticleLocations    (const State&) const;

 const Vector_<Vec3>& getAllParticleVelocities   (const State&) const;


 const Vec3& getParticleLocation(const State& s, ParticleIndex p) const {

     return getAllParticleLocations(s)[p];

 }

 const Vec3& getParticleVelocity(const State& s, ParticleIndex p) const {

     return getAllParticleVelocities(s)[p];

 }


 Vector& updAllParticleMasses(State& s) const;


 void setAllParticleMasses(State& s, const Vector& masses) const {

     updAllParticleMasses(s) = masses;

 }


 // Note that particle generalized coordinates, speeds, and applied forces

 // are defined to be the particle Cartesian locations, velocities, and

 // applied force vectors, so can be set directly at Stage::Model or higher.


 // These are the only routines that must be provided by the concrete MatterSubsystem.

 Vector_<Vec3>& updAllParticleLocations(State&)     const;

 Vector_<Vec3>& updAllParticleVelocities(State&)    const;


 // The following inline routines are provided by the generic MatterSubsystem

 // class for convenience.


 Vec3& updParticleLocation(State& s, ParticleIndex p) const {

     return updAllParticleLocations(s)[p];

 }

 Vec3& updParticleVelocity(State& s, ParticleIndex p) const {

     return updAllParticleVelocities(s)[p];

 }


 void setParticleLocation(State& s, ParticleIndex p, const Vec3& r) const {

     updAllParticleLocations(s)[p] = r;

 }

 void setParticleVelocity(State& s, ParticleIndex p, const Vec3& v) const {

     updAllParticleVelocities(s)[p] = v;

 }


 void setAllParticleLocations(State& s, const Vector_<Vec3>& r) const {

     updAllParticleLocations(s) = r;

 }

 void setAllParticleVelocities(State& s, const Vector_<Vec3>& v) const {

     updAllParticleVelocities(s) = v;

 }


 const Vector& getAllParticleMasses(const State&) const;


 const Vector_<Vec3>& getAllParticleAccelerations(const State&) const;


 const Vec3& getParticleAcceleration(const State& s, ParticleIndex p) const {

     return getAllParticleAccelerations(s)[p];

 }

 //==============================================================================

 private:

 void calcSpatialKinematicsFromInternal(const State&         state,

                                        const Vector&        u,

                                        Vector_<SpatialVec>& Ju) const

 {   multiplyBySystemJacobian(state,u,Ju); }


 void calcInternalGradientFromSpatial(const State&               state,

                                      const Vector_<SpatialVec>& F_G,

                                      Vector&                    f) const

 {   multiplyBySystemJacobianTranspose(state, F_G, f); }


 void calcMV(const State& state, const Vector& v, Vector& MV) const

 {   multiplyByM(state,v,MV); }


 void calcMInverseV(const State& state,

     const Vector&               v,

     Vector&                     MinvV) const

 {   multiplyByMInv(state,v,MinvV); }


 void calcPNInv(const State& state, Matrix& PNInv) const;


 void calcGt(const State&, Matrix& Gt) const;


 //==============================================================================

 //     Bookkeeping methods and internal types -- hide from Doxygen

 public:

 class Subtree; // used for working with a connected subgraph of the MobilizedBody tree

 class SubtreeResults;


 SimTK_PIMPL_DOWNCAST(SimbodyMatterSubsystem, Subsystem);

 const SimbodyMatterSubsystemRep& getRep() const;

 SimbodyMatterSubsystemRep&       updRep();

 private:

 };


 SimTK_SIMBODY_EXPORT std::ostream&

 operator<<(std::ostream&, const SimbodyMatterSubsystem&);


 } // namespace SimTK


 #endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_

SimTK_PIMPL_DOWNCAST
#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:564

SimTK::transpose
PhiMatrixTranspose transpose(const PhiMatrix &phi)
Definition: SpatialAlgebra.h:720

SimTK::Subsystem
A Subsystem is expected to be part of a larger System and to have interdependencies with other subsys...
Definition: Subsystem.h:55

SimTK::SimbodyMatterSubsystem::updParticleLocation
Vec3 & updParticleLocation(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2684

SimTK::ArrayViewConst_
This Array_ helper class is the base class for ArrayView_ which is the base class for Array_; here we...
Definition: Array.h:48

SimTK::MobilizedBodyIndex
This is for arrays indexed by mobilized body number within a subsystem (typically the SimbodyMatterSu...

SimTK::SimbodyMatterSubsystem::setAllParticleMasses
void setAllParticleMasses(State &s, const Vector &masses) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2669

SimTK::MobilizedBody::Ground
This is a special type of "mobilized" body generated automatically by Simbody as a placeholder for Gr...
Definition: MobilizedBody_Ground.h:45

SimTK::SimbodyMatterSubsystem::calcStationJacobian
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

SimTK
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37

SimTK::Stage
This class is basically a glorified enumerated type, type-safe and range checked but permitting conve...
Definition: Stage.h:50

SimTK::SimbodyMatterSubsystem::updParticleVelocity
Vec3 & updParticleVelocity(State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2687

SimTK::SimbodyMatterSubsystem::calcBiasForStationJacobian
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

common.h
Every Simbody header and source file should include this header before any other Simbody header...

SimTK::SimbodyMatterSubsystem::setParticleVelocity
void setParticleVelocity(State &s, ParticleIndex p, const Vec3 &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2694

SimTK::SimbodyMatterSubsystem::multiplyByFrameJacobianTranspose
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

SimTK::Array_
The SimTK::Array_ container class is a plug-compatible replacement for the C++ standard template l...
Definition: Array.h:50

SimTK::SimbodyMatterSubsystem::multiplyByStationJacobian
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
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:577

SimTK::State
This object is intended to contain all state information for a SimTK::System, except topological info...
Definition: State.h:276

SimTK::ConstraintIndex
This is for arrays indexed by constraint number within a subsystem (typically the SimbodyMatterSubsys...

SimTK::UnilateralContact
(Experimental – API will change – use at your own risk) A unilateral contact constraint uses a sing...
Definition: ConditionalConstraint.h:120

SimTK::SimbodyMatterSubsystem::setAllParticleVelocities
void setAllParticleVelocities(State &s, const Vector_< Vec3 > &v) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2701

SimTK::StateLimitedFriction
TODO: not implemented yet.
Definition: ConditionalConstraint.h:289

SimTKcommon.h
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.

SimTK::SimbodyMatterSubsystem::multiplyByFrameJacobian
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

SimTK::SimbodyMatterSubsystem::multiplyByPq
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

SimTK::MultibodySystem
The job of the MultibodySystem class is to coordinate the activities of various subsystems which can ...
Definition: MultibodySystem.h:48

SimTK::MassProperties_
This class contains the mass, center of mass, and unit inertia matrix of a rigid body B...
Definition: MassProperties.h:85

SimTK::SimbodyMatterSubsystem::operator=
SimbodyMatterSubsystem & operator=(const SimbodyMatterSubsystem &ss)
Copy assignment is not very useful.
Definition: SimbodyMatterSubsystem.h:282

SimTK::Matrix
Matrix_< Real > Matrix
Variable-size 2D matrix of Real elements; abbreviation for Matrix_.
Definition: BigMatrix.h:1478

SimTK::RowVector_
Represents a variable size row vector; much less common than the column vector type Vector_...
Definition: BigMatrix.h:174

SimTK::SimbodyMatterSubsystem::getParticleVelocity
const Vec3 & getParticleVelocity(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2663

SimTK::operator<<
std::ostream & operator<<(std::ostream &o, const ContactForce &f)
Definition: CompliantContactSubsystem.h:387

SimTK::SimbodyMatterSubsystem::calcStationJacobian
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

SimTK::SimbodyMatterSubsystem::calcBiasForFrameJacobian
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

SimTK::SimbodyMatterSubsystem::Ground
MobilizedBody::Ground & Ground()
This is a synonym for updGround() that makes for nicer-looking examples.
Definition: SimbodyMatterSubsystem.h:188

MobilizedBody.h
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.

SimTK::Vector_< Real >

SimTK::SimbodyMatterSubsystem::setAllParticleLocations
void setAllParticleLocations(State &s, const Vector_< Vec3 > &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2698

SimTK_SIMBODY_EXPORT
#define SimTK_SIMBODY_EXPORT
Definition: Simbody/include/simbody/internal/common.h:72

SimTK::SimbodyMatterSubsystem::SimbodyMatterSubsystem
SimbodyMatterSubsystem(const SimbodyMatterSubsystem &ss)
Copy constructor is not very useful.
Definition: SimbodyMatterSubsystem.h:280

SimTK::MobilizedBody
A MobilizedBody is Simbody's fundamental body-and-joint object used to parameterize a system's motion...
Definition: MobilizedBody.h:167

SimTK::Vector
Vector_< Real > Vector
Variable-size column vector of Real elements; abbreviation for Vector_.
Definition: BigMatrix.h:1473

SimTK::Constraint
This is the base class for all Constraint classes, which is just a handle for the underlying hidden i...
Definition: Constraint.h:66

SimTK::SimbodyMatterSubsystem::~SimbodyMatterSubsystem
~SimbodyMatterSubsystem()
The destructor destroys the subsystem implementation object only if this handle is the last reference...
Definition: SimbodyMatterSubsystem.h:161

SimTK::SpatialInertia_
A spatial inertia contains the mass, center of mass point, and inertia matrix for a rigid body...
Definition: MassProperties.h:83

SimTK::Vec< 3 >

SimTK::ArticulatedInertia_
An articulated body inertia (ABI) matrix P(q) contains the spatial inertia properties that a body app...
Definition: MassProperties.h:84

SimTK::SimbodyMatterSubsystem::getParticleAcceleration
const Vec3 & getParticleAcceleration(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2709

SimTK::SimbodyMatterSubsystem::calcFrameJacobian
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

SimTK::SimbodyMatterSubsystem
This subsystem contains the bodies ("matter") in the multibody system, the mobilizers (joints) that d...
Definition: SimbodyMatterSubsystem.h:133

SimTK::SimbodyMatterSubsystem::setParticleLocation
void setParticleLocation(State &s, ParticleIndex p, const Vec3 &r) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2691

SimTK::SimbodyMatterSubsystem::calcFrameJacobian
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

SimTK::SimbodyMatterSubsystem::getParticleLocation
const Vec3 & getParticleLocation(const State &s, ParticleIndex p) const
TODO: total number of particles.
Definition: SimbodyMatterSubsystem.h:2660

SimTK::SimbodyMatterSubsystem::multiplyByG
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

SimTK::Matrix_
This is the matrix class intended to appear in user code for large, variable size matrices...
Definition: BigMatrix.h:168

SimTK::MobilizerUIndex
The Mobilizer associated with each MobilizedBody, once modeled, has a specific number of generalized ...

SimTK::Subsystem::operator=
Subsystem & operator=(const Subsystem &)
Copy assignment deletes the Subsystem::Guts object if there is one and then behaves like the copy con...

SimTK::Inertia_
The physical meaning of an inertia is the distribution of a rigid body's mass about a particular poin...
Definition: MassProperties.h:82