SimbodyMatterSubsystem.h

Go to the documentation of this file.

#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 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&);

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_