SimbodyMatterSubtree.h

Go to the documentation of this file.

#ifndef SimTK_SIMBODY_MATTER_SUBTREE_H_
#define SimTK_SIMBODY_MATTER_SUBTREE_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) 2007-8 Stanford University and the Authors.         *
 * Authors: Michael Sherman                                                   *
 * Contributors:                                                              *
 *                                                                            *
 * 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 <cassert>
#include <vector>
#include <iosfwd>

namespace SimTK {

class SimbodyMatterSubsystem;
class MobilizedBody;
class SimbodyMatterSubtree;
class SimbodyMatterSubtreeResults;

class SimTK_SIMBODY_EXPORT SimbodyMatterSubtree {
public:
    SimbodyMatterSubtree();
    SimbodyMatterSubtree(const SimbodyMatterSubtree&);
    SimbodyMatterSubtree& operator=(const SimbodyMatterSubtree&);
    ~SimbodyMatterSubtree();

    explicit SimbodyMatterSubtree(const SimbodyMatterSubsystem&);
    SimbodyMatterSubtree(const SimbodyMatterSubsystem&, 
            const std::vector<MobilizedBodyIndex>& terminalBodies);

    void setSimbodyMatterSubsystem(const SimbodyMatterSubsystem& matter);
    const SimbodyMatterSubsystem& getSimbodyMatterSubsystem() const;

    // This doesn't change the associated SimbodyMatterSubsystem if there
    // is one, but does remove all the bodies from the SimbodyMatterSubtree.
    void clear();

    SimbodyMatterSubtree& addTerminalBody(MobilizedBodyIndex);

    void realizeTopology();

    int getNumSubtreeBodies() const; // includes ancestor
    MobilizedBodyIndex getAncestorMobilizedBodyIndex() const;

    // These are in the same order they were added; body[i] is the terminus
    // of branch i.
    const std::vector<MobilizedBodyIndex>& getTerminalBodies() const;

    // These are indexed by SubtreeBodyIndex starting with 0 for the ancestor body
    // and monotonically increasing outwards along a branch.
    const std::vector<MobilizedBodyIndex>& getAllBodies() const;

    SubtreeBodyIndex getParentSubtreeBodyIndex(SubtreeBodyIndex) const; // 0 returns an invalid Index
    const std::vector<SubtreeBodyIndex>& getChildSubtreeBodyIndices(SubtreeBodyIndex) const;

        // MODEL STAGE

    // State must be realized to at least Stage::Model for this call to work. 
    // The supplied SimbodyMatterSubtreeResults object is allocated and properly initialized to
    // be able to hold computation results from this SimbodyMatterSubtree.
    void initializeSubtreeResults(const State&, SimbodyMatterSubtreeResults&) const;

    // This can be used as a sanity check that initializeSubtreeResults() was already called
    // in this SimbodyMatterSubtree to produce these SimbodyMatterSubtreeResults. It is by no means exhaustive but
    // will catch egregious errors.
    bool isCompatibleSubtreeResults(const SimbodyMatterSubtreeResults&) const;

        // POSITION STAGE

    // State must be realized to at least Stage::Position for this to work. SimbodyMatterSubtreeResults
    // must have already been initialized to work with this SimbodyMatterSubtree. SimbodyMatterSubtreeResults stage
    // will be Stage::Position after this call. All body transforms will be the same as
    // the corresponding ones in the state, except they will be measured from the ancestor
    // frame instead of ground. SimbodyMatterSubtree q's will be identical to corresponding State q's.
    void copyPositionsFromState(const State&, SimbodyMatterSubtreeResults&) const;

    // State must be realized to Stage::Instance. subQ must be the right length for this
    // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must have been properly initialized. SimbodyMatterSubtreeResults
    // stage will be Stage::Position after this call.
    void calcPositionsFromSubtreeQ(const State&, const Vector& subQ, SimbodyMatterSubtreeResults&) const;

    // Calculates a perturbed position result starting with the subQ's and position results
    // which must already be in SimbodyMatterSubtreeResults.
    void perturbPositions(const State&, SubtreeQIndex subQIndex, Real perturbation, SimbodyMatterSubtreeResults&) const;


        // VELOCITY STAGE

    // State must be realized to at least Stage::Velocity for this to work. SimbodyMatterSubtreeResults
    // must already be at Stage::Position. SimbodyMatterSubtreeResults stage
    // will be Stage::Velocity after this call. All subtree body spatial velocities will be
    // the same as in the State, except measured relative to A and expressed in A. SimbodyMatterSubtree u's
    // will be identical to corresponding State u's.
    void copyVelocitiesFromState(const State&, SimbodyMatterSubtreeResults&) const;

    // State must be realized to Stage::Instance. subU must be the right length for this
    // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must already be at Stage::Position. SimbodyMatterSubtreeResults
    // stage will be Stage::Velocity after this call.
    void calcVelocitiesFromSubtreeU(const State&, const Vector& subU, SimbodyMatterSubtreeResults&) const;

    // State must be realized to Stage::Instance and SimbodyMatterSubtreeResults must already be at
    // Stage::Position. SimbodyMatterSubtreeResults stage will be Stage::Velocity after this call, but
    // all SimbodyMatterSubtree u's and body velocities will be zero.
    void calcVelocitiesFromZeroU(const State&, SimbodyMatterSubtreeResults&) const;

    // Calculates a perturbed velocity result starting with the subU's and velocity results
    // which must already be in SimbodyMatterSubtreeResults.
    void perturbVelocities(const State&, SubtreeUIndex subUIndex, Real perturbation, SimbodyMatterSubtreeResults&) const;


        // ACCELERATION STAGE

    // State must be realized to at least Stage::Acceleration for this to work. SimbodyMatterSubtreeResults
    // must already be at Stage::Velocity. SimbodyMatterSubtreeResults stage
    // will be Stage::Acceleration after this call. All subtree body spatial accelerations will be
    // the same as in the State, except measured relative to A and expressed in A. SimbodyMatterSubtree udots
    // will be identical to corresponding State udots.
    void copyAccelerationsFromState(const State&, SimbodyMatterSubtreeResults&) const;

    // State must be realized to Stage::Instance. subUDot must be the right length for this
    // SimbodyMatterSubtree, and SimbodyMatterSubtreeResults must already be at Stage::Velocity. SimbodyMatterSubtreeResults
    // stage will be Stage::Acceleration after this call.
    void calcAccelerationsFromSubtreeUDot(const State&, const Vector& subUDot, SimbodyMatterSubtreeResults&) const;

    // State must be realized to Stage::Instance and SimbodyMatterSubtreeResults must already be at
    // Stage::Velocity. SimbodyMatterSubtreeResults stage will be Stage::Acceleration after this call.
    // All SimbodyMatterSubtree udots's will be zero, body accelerations will have only their bias values
    // (coriolis accelerations from nonzero u's).
    void calcAccelerationsFromZeroUDot(const State&, SimbodyMatterSubtreeResults&) const;

    // Calculates a perturbed velocity result starting with the subUDot's and acceleration results
    // which must already be in SimbodyMatterSubtreeResults.
    void perturbAccelerations(const State&, SubtreeUIndex subUDotIndex, Real perturbation, SimbodyMatterSubtreeResults&) const;

    class SubtreeRep;
private:
    SubtreeRep* rep;
    const SubtreeRep& getRep() const {assert(rep);return *rep;}
    SubtreeRep&       updRep()       {assert(rep);return *rep;}
};

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

/*
 * This is the writable "cache" for a SimbodyMatterSubtree. Once the full State has
 * been realized to the Model stage, a SimbodyMatterSubtree can initialize one of these
 * objects and then use it to hold operator results.
 */
class SimTK_SIMBODY_EXPORT SimbodyMatterSubtreeResults {
public:
    SimbodyMatterSubtreeResults();
    SimbodyMatterSubtreeResults(const SimbodyMatterSubtreeResults&);
    SimbodyMatterSubtreeResults& operator=(const SimbodyMatterSubtreeResults&);
    ~SimbodyMatterSubtreeResults();

    void clear();

    void reallocateBodies(int nBodies);
    void addMobilities(SubtreeBodyIndex, QIndex qStart, int nq, UIndex uStart, int nu);
    void realizeModel(const Vector& stateQ, const Vector& stateU);

    Stage getStage() const;

    int getNumSubtreeBodies() const;
    int getNumSubtreeQs() const;
    int getNumSubtreeUs() const;

    const Vector&     getSubtreeQ() const;
    const Transform&  getSubtreeBodyTransform(SubtreeBodyIndex) const; // from ancestor frame

    const Vector&     getSubtreeU() const;
    const SpatialVec& getSubtreeBodyVelocity(SubtreeBodyIndex) const; // measured & expressed  in ancestor frame

    const Vector&     getSubtreeUDot() const;
    const SpatialVec& getSubtreeBodyAcceleration(SubtreeBodyIndex) const; // measured & expressed in ancestor frame

    // These are indexed by SubtreeQIndex and SubtreeUIndex.
    const std::vector<QIndex>& getQSubset() const; // subset of Subsystem Qs used by this SimbodyMatterSubtree
    const std::vector<UIndex>& getUSubset() const; // subset of Subsystem Us used by this SimbodyMatterSubtree

    void findSubtreeBodyQ(SubtreeBodyIndex, SubtreeQIndex& qStart, int& nq) const; // indices into QSubset
    void findSubtreeBodyU(SubtreeBodyIndex, SubtreeUIndex& uStart, int& nu) const; // indices into USubset

    class SubtreeResultsRep;
private:
    friend class SimbodyMatterSubtree;
    SubtreeResultsRep* rep;
    const SubtreeResultsRep& getRep() const {assert(rep);return *rep;}
    SubtreeResultsRep&       updRep()       {assert(rep);return *rep;}
};

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

} // namespace SimTK

#endif // SimTK_SIMBODY_MATTER_SUBTREE_H_