MultibodyGraphMaker.h

Go to the documentation of this file.

#ifndef SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
#define SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_

/* -------------------------------------------------------------------------- *
 *                     Simbody(tm): Multibody Graph Maker                     *
 * -------------------------------------------------------------------------- *
 * 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) 2013-4 Stanford University and the Authors.         *
 * Authors: Michael Sherman                                                   *
 * Contributors: Kevin He                                                     *
 *                                                                            *
 * 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 "simmath/internal/common.h"

#include <utility>
#include <string>
#include <vector>
#include <map>
#include <iosfwd>

namespace SimTK {

//==============================================================================
//                          MULTIBODY GRAPH MAKER
//==============================================================================
class SimTK_SIMMATH_EXPORT MultibodyGraphMaker {
public:
    // Local classes.
    class Body;
    class Joint;
    class JointType;
    class Mobilizer;
    class LoopConstraint;

    MultibodyGraphMaker();


    int addJointType(const std::string& name,
                     int                numMobilities,
                     bool               haveGoodLoopJointAvailable = false,
                     void*              userRef                    = 0);


    void addBody(const std::string&  name, 
                 double              mass, 
                 bool                mustBeBaseBody,
                 void*               userRef = 0);

    bool deleteBody(const std::string&  name);

    void addJoint(const std::string& name,
                  const std::string& type,
                  const std::string& parentBodyName,
                  const std::string& childBodyName,
                  bool               mustBeLoopJoint,
                  void*              userRef = 0);

    bool deleteJoint(const std::string& name);

    void generateGraph();
    void clearGraph();

    void dumpGraph(std::ostream& out) const;

    int getNumMobilizers() const {return (int)mobilizers.size();}
    const Mobilizer& getMobilizer(int mobilizerNum) const
    {   return mobilizers[mobilizerNum]; }

    int getNumLoopConstraints() const {return (int)constraints.size();}
    const LoopConstraint& getLoopConstraint(int loopConstraintNum) const
    {   return constraints[loopConstraintNum]; }

    int getNumBodies() const {return (int)bodies.size();}
    const Body& getBody(int bodyNum) const {return bodies[bodyNum];}
    int getBodyNum(const std::string& bodyName) const {
        std::map<std::string,int>::const_iterator p = 
            bodyName2Num.find(bodyName);
        return p==bodyName2Num.end() ? -1 : p->second;
    }

    int getNumJoints() const {return (int)joints.size();}
    const Joint& getJoint(int jointNum) const {return joints[jointNum];}
    int getJointNum(const std::string& jointName) const {
        std::map<std::string,int>::const_iterator p = 
            jointName2Num.find(jointName);
        return p==jointName2Num.end() ? -1 : p->second;
    }

    int getNumJointTypes() const {return (int)jointTypes.size();}
    const JointType& getJointType(int jointTypeNum) const 
    {   return jointTypes[jointTypeNum]; }
    int getJointTypeNum(const std::string& jointTypeName) const {
        std::map<std::string,int>::const_iterator p = 
            jointTypeName2Num.find(jointTypeName);
        return p==jointTypeName2Num.end() ? -1 : p->second;
    }

    void setWeldJointTypeName(const std::string& name) 
    {   weldTypeName=name; initialize(); }
    const std::string& getWeldJointTypeName() const {return weldTypeName;}

    void setFreeJointTypeName(const std::string& name) 
    {   freeTypeName=name; initialize(); }
    const std::string& getFreeJointTypeName() const {return freeTypeName;}

    const std::string& getGroundBodyName() const;
private:
    // Get writable access to bodies and joints.
    Body& updBody(int bodyNum) {return bodies[bodyNum];}
    Joint& updJoint(int jointNum) {return joints[jointNum];}
    Joint& updJoint(const std::string& name) {return joints[jointName2Num[name]];}

    void initialize();
    int splitBody(int bodyNum);
    int chooseNewBaseBody() const;
    void connectBodyToGround(int bodyNum);
    int addMobilizerForJoint(int jointNum);
    int findHeaviestUnassignedForwardJoint(int inboardBody) const;
    int findHeaviestUnassignedReverseJoint(int inboardBody) const;
    void growTree();
    void breakLoops();
    bool bodiesAreConnected(int b1, int b2) const;

    // Clear everything except for default names.
    void clear() {
        bodies.clear(); joints.clear(); jointTypes.clear();
        bodyName2Num.clear(); jointName2Num.clear(); jointTypeName2Num.clear();
        mobilizers.clear(); constraints.clear();
    }

    std::string                 weldTypeName, freeTypeName;
    std::vector<Body>           bodies; // ground + input bodies + slaves
    std::vector<Joint>          joints; // input joints + added joints
    std::vector<JointType>      jointTypes;
    std::map<std::string,int>   bodyName2Num;
    std::map<std::string,int>   jointName2Num;
    std::map<std::string,int>   jointTypeName2Num;

    // Calculated by generateGraph()
    std::vector<Mobilizer>      mobilizers; // mobilized bodies
    std::vector<LoopConstraint> constraints;
};

//------------------------------------------------------------------------------
//                      MULTIBODY GRAPH MAKER :: BODY
//------------------------------------------------------------------------------
class MultibodyGraphMaker::Body {
public:
    explicit Body(const std::string&    name, 
                    double              mass, 
                    bool                mustBeBaseBody,
                    void*               userRef) 
    :   name(name), mass(mass), mustBeBaseBody(mustBeBaseBody), 
        userRef(userRef), level(-1), mobilizer(-1), master(-1) {}

    void forgetGraph(MultibodyGraphMaker& graph);
    int getNumFragments() const {return 1 + getNumSlaves();}
    int getNumSlaves() const {return (int)slaves.size();}
    int getNumJoints() const 
    {   return int(jointsAsChild.size() + jointsAsParent.size()); }
    bool isSlave() const {return master >= 0;}
    bool isMaster() const {return getNumSlaves()>0;}
    bool isInTree() const {return level>=0;}

    // Inputs
    std::string name;
    double      mass;
    bool        mustBeBaseBody;
    void*       userRef;

    // How this body appears in joints (input and added).
    std::vector<int>    jointsAsChild;  // where this body is the child
    std::vector<int>    jointsAsParent; // where this body is the parent

    // Disposition of this body in the spanning tree.

    int level; // Ground=0, connected to Ground=1, contact to that=2, etc.
    int mobilizer; // the unique mobilizer where this is the outboard body

    int                 master; // >=0 if this is a slave
    std::vector<int>    slaves; // slave links, if this is a master
};

//------------------------------------------------------------------------------
//                      MULTIBODY GRAPH MAKER :: JOINT
//------------------------------------------------------------------------------
class MultibodyGraphMaker::Joint {
public:
    Joint(const std::string& name, int jointTypeNum, 
          int parentBodyNum, int childBodyNum,
          bool mustBeLoopJoint, void* userRef)
    :   name(name), 
        mustBeLoopJoint(mustBeLoopJoint), 
        userRef(userRef),
        parentBodyNum(parentBodyNum), 
        childBodyNum(childBodyNum),
        jointTypeNum(jointTypeNum), 
        isAddedBaseJoint(false),
        mobilizer(-1), 
        loopConstraint(-1) {}

    bool forgetGraph(MultibodyGraphMaker& graph);

    // Only one of these will be true -- we don't consider it a LoopConstraint
    // if we split a body and weld it back.
    bool hasMobilizer() const {return mobilizer>=0;}
    bool hasLoopConstraint() const {return loopConstraint>=0;}

    // Inputs
    std::string name;
    bool        mustBeLoopJoint;
    void*       userRef;

    // Mapping of strings to indices for fast lookup.
    int parentBodyNum, childBodyNum;
    int jointTypeNum;

    bool isAddedBaseJoint; // true if this wasn't one of the input joints

    // Disposition of this joint in the multibody graph.
    int mobilizer;      // if this joint is part of the spanning tree, else -1
    int loopConstraint; // if this joint used a loop constraint, else -1
};

//------------------------------------------------------------------------------
//                   MULTIBODY GRAPH MAKER :: JOINT TYPE
//------------------------------------------------------------------------------
class MultibodyGraphMaker::JointType {
public:
    JointType(const std::string& name, int numMobilities, 
              bool haveGoodLoopJointAvailable, void* userRef)
    :   name(name), numMobilities(numMobilities), 
        haveGoodLoopJointAvailable(haveGoodLoopJointAvailable),
        userRef(userRef) {}
    std::string name;
    int         numMobilities;
    bool        haveGoodLoopJointAvailable;
    void*       userRef;
};

//------------------------------------------------------------------------------
//                   MULTIBODY GRAPH MAKER :: MOBILIZER
//------------------------------------------------------------------------------
class MultibodyGraphMaker::Mobilizer {
public:
    Mobilizer() 
    :   joint(-1), level(-1), inboardBody(-1), outboardBody(-1),
        isReversed(false), mgm(0) {}
    Mobilizer(int jointNum, int level, int inboardBodyNum, int outboardBodyNum, 
              bool isReversed, MultibodyGraphMaker* graphMaker)
    :   joint(jointNum), level(level), inboardBody(inboardBodyNum), 
        outboardBody(outboardBodyNum), isReversed(isReversed),
        mgm(graphMaker) {}

    bool isAddedBaseMobilizer() const
    {   return mgm->getJoint(joint).isAddedBaseJoint; }
    void* getJointRef() const
    {   return mgm->getJoint(joint).userRef; } 
    void* getInboardBodyRef() const
    {   return mgm->getBody(inboardBody).userRef; }   
    void* getOutboardBodyRef() const
    {   return mgm->getBody(outboardBody).userRef; }
    void* getOutboardMasterBodyRef() const
    {   return mgm->getBody(getOutboardMasterBodyNum()).userRef; }
    const std::string& getJointTypeName() const
    {   return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).name; }
    void* getJointTypeRef() const
    {   return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).userRef; }
    bool isSlaveMobilizer() const
    {   return mgm->getBody(outboardBody).isSlave(); }
    int getNumFragments() const 
    {   return mgm->getBody(getOutboardMasterBodyNum()).getNumFragments(); }
    bool isReversedFromJoint() const {return isReversed;}
    int getLevel() const {return level;}

private:
friend class MultibodyGraphMaker;

    int getOutboardMasterBodyNum() const
    {   const Body& outb = mgm->getBody(outboardBody);
        return outb.isSlave() ? outb.master : outboardBody; }

    int  joint;         
    int  level;         
    int  inboardBody;   
    int  outboardBody;  
    bool isReversed;    

    MultibodyGraphMaker*    mgm; // just a reference to container
};


//------------------------------------------------------------------------------
//                  MULTIBODY GRAPH MAKER :: LOOP CONSTRAINT
//------------------------------------------------------------------------------
class MultibodyGraphMaker::LoopConstraint {
public:
    LoopConstraint() : joint(-1), parentBody(-1), childBody(-1), mgm(0) {}
    LoopConstraint(const std::string& type, int jointNum, 
                   int parentBodyNum, int childBodyNum,
                   MultibodyGraphMaker* graphMaker) 
    :   type(type), joint(jointNum), 
        parentBody(parentBodyNum), childBody(childBodyNum), 
        mgm(graphMaker) {}

    void* getJointRef() const
    {   return mgm->getJoint(joint).userRef; } 
    const std::string& getJointTypeName() const
    {   return type; }
    void* getParentBodyRef() const
    {   return mgm->getBody(parentBody).userRef; }   
    void* getChildBodyRef() const
    {   return mgm->getBody(childBody).userRef; }

private:
friend class MultibodyGraphMaker;

    std::string type;        // e.g., ball
    int         joint;       // always one of the input joints
    int         parentBody;  // parent from the joint
    int         childBody;   // child from the joint

    MultibodyGraphMaker*    mgm; // just a reference to container
};


} // namespace SimTK

#endif // SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_