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-8 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;

        // CONSTRUCTION //

    // Attach new matter using the indicated parent body as the reference
    // frame, with the mobilizer and mass properties provided by 'child'.
    // We take over ownership of child's representation from the given
    // handle, leaving that handle as a reference to our new matter object.
    // It is an error if the given handle wasn't the owner of the
    // matter representation.
    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,
        Vector_<SpatialVec>&       A_GB) const;


    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,
        Vector_<SpatialVec>& A_GB) const; // <-- TODO: get rid of this A

    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 multiplyByQMatrix(const State& s, bool transposeMatrix, const Vector& in, Vector& out) const;

    void multiplyByQMatrixInverse(const State& s, bool transposeMatrix, const Vector& in, Vector& out) const;
    
    void calcMobilizerReactionForces(const State& s, Vector_<SpatialVec>& forces) const;

    // These are available after realizeTopology().

    int getNBodies() const;

    int getNConstraints() const;

    int getNParticles() const;

    int getNMobilities() const; 

    int getTotalQAlloc() const;

    int getTotalMultAlloc() const;

    void setUseEulerAngles(State&, bool) const;
    bool getUseEulerAngles  (const State&) const;
    int  getNQuaternionsInUse(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; 

    // Dynamics stage responses.

    // Cross joint
    const SpatialVec& getCoriolisAcceleration(const State&, MobilizedBodyIndex) const;

    // Including parent
    const SpatialVec& getTotalCoriolisAcceleration(const State&, MobilizedBodyIndex) const;

    const SpatialVec& getGyroscopicForce(const State&, MobilizedBodyIndex) const;
    const SpatialVec& getCentrifugalForces(const State&, MobilizedBodyIndex) const;
    const SpatialMat& getArticulatedBodyInertia(const State& s, MobilizedBodyIndex) 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 responses //

        // 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 projectQConstraints(State& s, Real consAccuracy, const Vector& yWeights,
                             const Vector& ooTols, Vector& yErrest, System::ProjectOptions) const;

        // VELOCITY STAGE responses //

        // 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();
};

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


} // namespace SimTK

#endif // SimTK_SIMBODY_MATTER_SUBSYSTEM_H_