SimbodyMatterSubsystem.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_MATTER_SUBSYSTEM_H_
#define SimTK_SIMBODY_MATTER_SUBSYSTEM_H_

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simbody(tm)                         *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core 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.               *
 *                                                                            *
 * Portions copyright (c) 2006-9 Stanford University and the Authors.         *
 * Authors: Michael Sherman                                                   *
 * Contributors: Paul Mitiguy                                                 *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

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

SimbodyMatterSubsystem();
explicit SimbodyMatterSubsystem(MultibodySystem&);

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

// These are the same as the compiler defaults but are handy to
// have around explicitly for debugging.
~SimbodyMatterSubsystem() {
}
SimbodyMatterSubsystem(const SimbodyMatterSubsystem& ss) : Subsystem(ss) {
}
SimbodyMatterSubsystem& operator=(const SimbodyMatterSubsystem& ss) {
    Subsystem::operator=(ss);
    return *this;
}

bool getShowDefaultGeometry() const;

void setShowDefaultGeometry(bool show);


    // PAUL'S FRIENDLY INTERFACE //

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;

    // CONSTRUCTION //

MobilizedBodyIndex   adoptMobilizedBody(MobilizedBodyIndex  parent, 
                                        MobilizedBody&      child);

const MobilizedBody& getMobilizedBody(MobilizedBodyIndex) const;

MobilizedBody&       updMobilizedBody(MobilizedBodyIndex);


// Note: topology is not marked invalid upon returning a writable reference
// here; that will be done only if a non-const method of the returned 
// MobilizedBody is called. That means it is OK to use Ground() to satisfy 
// a const argument; it won't have an "invalidate topology" side effect.

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

ConstraintIndex   adoptConstraint(Constraint&);

const Constraint& getConstraint(ConstraintIndex) const;

Constraint&       updConstraint(ConstraintIndex);

    // OPERATORS //

    // Operators make use of the State but do not write their results back
    // into the State, not even into the State cache.

void calcAcceleration(const State&,
    const Vector&              mobilityForces,
    const Vector_<SpatialVec>& bodyForces,
    Vector&                    udot,    // output only; no prescribed motions
    Vector_<SpatialVec>&       A_GB) const;

void calcDynamicsFromForcesAndMotions(const State&,
    const Vector&              appliedMobilityForces,  // [nu] or [0]
    const Vector_<SpatialVec>& appliedBodyForces,      // [nb] or [0]
    const Vector&              udot_p,                 // [npres] or [0]
    Vector&                    udot,                   // [nu]    (output only)
    Vector&                    lambda_p,               // [npres] (output only)
    Vector&                    lambda_r) const;        // [m]     (output only)

void calcDynamicsFromForces(const State&,
    const Vector&              appliedMobilityForces,  // [nu] or [0]
    const Vector_<SpatialVec>& appliedBodyForces,      // [nb] or [0]
    Vector&                    udot,                   // [nu]    (output only)
    Vector&                    lambda_p,               // [npres] (output only)
    Vector&                    lambda_r) const;        // [m]     (output only) 

void calcAccelerationIgnoringConstraints(const State&,
    const Vector&              mobilityForces,
    const Vector_<SpatialVec>& bodyForces,
    Vector&                    udot,    
    Vector_<SpatialVec>&       A_GB) const;

void calcMInverseV(const State&,
    const Vector&        v,
    Vector&              MinvV) const;

void calcResidualForce
   (const State&               state,
    const Vector&              appliedMobilityForces,
    const Vector_<SpatialVec>& appliedBodyForces,
    const Vector&              knownUdot,
    const Vector&              knownMultipliers,
    Vector&                    residualMobilityForces) const;

void calcResidualForceIgnoringConstraints
   (const State&               state,
    const Vector&              appliedMobilityForces,
    const Vector_<SpatialVec>& appliedBodyForces,
    const Vector&              knownUdot,
    Vector&                    residualMobilityForces) const;

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

void calcCompositeBodyInertias(const State&,
    Vector_<SpatialMat>& R) const;

void calcAccelerationFromUDot(const State&         state,
                              const Vector&        knownUDot,
                              Vector_<SpatialVec>& A_GB) const;

void calcAccConstraintErr(const State&,
    const Vector&   knownUdot,
    Vector&         constraintErr) const;

void calcGV(const State&,
    const Vector&   v,
    Vector&         Gv) const;

void calcGtV(const State&,
    const Vector&   v,
    Vector&         GtV) const;


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

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

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

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

void calcPtV(const State& s, const Vector& v, Vector& PtV) const;

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

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

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

void calcConstraintForcesFromMultipliers
  (const State& s, const Vector& multipliers,
   Vector_<SpatialVec>& bodyForcesInG,
   Vector&              mobilityForces) const;

void calcMobilizerReactionForces
   (const State&         state, 
    Vector_<SpatialVec>& forcesAtMInG) const;

void calcSpatialKinematicsFromInternal(const State&,
    const Vector&        v,
    Vector_<SpatialVec>& Jv) const;

void calcInternalGradientFromSpatial(const State&,
    const Vector_<SpatialVec>& dEdR,
    Vector&                    dEdQ) const; // really Qbar

Real calcKineticEnergy(const 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 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;

// These are available after realizeTopology().

int getNumBodies() const;

int getNumConstraints() const;

int getNumParticles() const;

int getNumMobilities() const;

int getTotalQAlloc() const;

int getTotalMultAlloc() const;

void setUseEulerAngles(State&, bool) const;
bool getUseEulerAngles  (const State&) const;

int  getNumQuaternionsInUse(const State&) const;

void setMobilizerIsPrescribed(State&, MobilizedBodyIndex, bool) const;
bool isMobilizerPrescribed  (const State&, MobilizedBodyIndex) const;
bool isUsingQuaternion(const State&, MobilizedBodyIndex) const;
QuaternionPoolIndex getQuaternionPoolIndex(const State&, MobilizedBodyIndex) const;
AnglePoolIndex      getAnglePoolIndex(const State&, MobilizedBodyIndex) const;
void setConstraintIsDisabled(State&, ConstraintIndex constraint, bool) const;
bool isConstraintDisabled(const State&, ConstraintIndex constraint) const;

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

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


    // PARTICLES
    // TODO: not currently implemented. Use a point mass with a Cartesian (translation)
    // mobilizer to Ground instead. The idea here would be to special-case particles
    // to make them faster; there would be no additional functionality.

// 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];
}

    // POSITION STAGE realizations //

void realizeCompositeBodyInertias(const State&) const;

void realizeArticulatedBodyInertias(const State&) const;


    // POSITION STAGE responses //

const SpatialMat& getCompositeBodyInertia(const State&, MobilizedBodyIndex) const;

const SpatialMat& getArticulatedBodyInertia(const State&, MobilizedBodyIndex) const;

    // POSITION STAGE operators //

void addInStationForce(const State&, MobilizedBodyIndex bodyB, const Vec3& stationOnB, 
                       const Vec3& forceInG, Vector_<SpatialVec>& bodyForcesInG) const;

void addInBodyTorque(const State&, MobilizedBodyIndex, const Vec3& torqueInG, 
                     Vector_<SpatialVec>& bodyForcesInG) const;

void addInMobilityForce(const State&, MobilizedBodyIndex, MobilizerUIndex which, Real f,
                        Vector& mobilityForces) const;

    // POSITION STAGE solvers //

bool prescribe(State&, Stage) const;

bool projectQConstraints(State& s, Real consAccuracy, const Vector& yWeights,
                         const Vector& ooTols, Vector& yErrest, System::ProjectOptions) const;

    // VELOCITY STAGE responses //

const SpatialVec& getCoriolisAcceleration(const State&, MobilizedBodyIndex) const;

const SpatialVec& getTotalCoriolisAcceleration(const State&, MobilizedBodyIndex) const;

const SpatialVec& getGyroscopicForce(const State&, MobilizedBodyIndex) const;

const SpatialVec& getCentrifugalForces(const State&, MobilizedBodyIndex) const;

const SpatialVec& getTotalCentrifugalForces(const State&, MobilizedBodyIndex) const;

    // VELOCITY STAGE operators //

    // VELOCITY STAGE solvers //

bool projectUConstraints(State& s, Real consAccuracy, const Vector& yWeights,
                         const Vector& ooTols, Vector& yErrest, System::ProjectOptions) const;

    // ACCELERATION STAGE reponses


    // Bookkeeping
SimTK_PIMPL_DOWNCAST(SimbodyMatterSubsystem, Subsystem);
const SimbodyMatterSubsystemRep& getRep() const;
SimbodyMatterSubsystemRep&       updRep();


private:
// OBSOLETE; TODO: remove in SimTK 2.0
void multiplyByQMatrix(const State& s, bool transposeMatrix, const Vector& in, Vector& out) const
{   multiplyByN(s,transposeMatrix,in,out);}
// OBSOLETE; TODO: remove in SimTK 2.0
void multiplyByQMatrixInverse(const State& s, bool transposeMatrix, const Vector& in, Vector& out) const
{   multiplyByNInv(s,transposeMatrix,in,out);}
// OBSOLETE; TODO: remove in SimTK 2.0
int getNBodies() const {return getNumBodies();}
// OBSOLETE; TODO: remove in SimTK 2.0
int getNConstraints() const {return getNumConstraints();}
// OBSOLETE; TODO: remove in SimTK 2.0
int getNMobilities() const {return getNumMobilities();}
// OBSOLETE; TODO: remove in SimTK 2.0
int getNParticles() const {return getNumParticles();}

};

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


} // namespace SimTK

#endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_