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 {
    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
    };
 
    class 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
    };
 
    class 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;
    };
 
    class 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
    };
 
    class 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
    };
 
    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;
};
 
} // namespace SimTK
 
#endif // SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_