Simbody  3.5
MultibodyGraphMaker.h
Go to the documentation of this file.
00001 #ifndef SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
00002 #define SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
00003 
00004 /* -------------------------------------------------------------------------- *
00005  *                     Simbody(tm): Multibody Graph Maker                     *
00006  * -------------------------------------------------------------------------- *
00007  * This is part of the SimTK biosimulation toolkit originating from           *
00008  * Simbios, the NIH National Center for Physics-Based Simulation of           *
00009  * Biological Structures at Stanford, funded under the NIH Roadmap for        *
00010  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody.  *
00011  *                                                                            *
00012  * Portions copyright (c) 2013-4 Stanford University and the Authors.         *
00013  * Authors: Michael Sherman                                                   *
00014  * Contributors: Kevin He                                                     *
00015  *                                                                            *
00016  * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
00017  * not use this file except in compliance with the License. You may obtain a  *
00018  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
00019  *                                                                            *
00020  * Unless required by applicable law or agreed to in writing, software        *
00021  * distributed under the License is distributed on an "AS IS" BASIS,          *
00022  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
00023  * See the License for the specific language governing permissions and        *
00024  * limitations under the License.                                             *
00025  * -------------------------------------------------------------------------- */
00026 
00032 #include "SimTKcommon.h"
00033 #include "simmath/internal/common.h"
00034 
00035 #include <utility>
00036 #include <string>
00037 #include <vector>
00038 #include <map>
00039 #include <iosfwd>
00040 
00041 namespace SimTK {
00042 
00043 //==============================================================================
00044 //                          MULTIBODY GRAPH MAKER
00045 //==============================================================================
00152 class SimTK_SIMMATH_EXPORT MultibodyGraphMaker {
00153 public:
00154     // Local classes.
00155     class Body;
00156     class Joint;
00157     class JointType;
00158     class Mobilizer;
00159     class LoopConstraint;
00160 
00163     MultibodyGraphMaker();
00164 
00165 
00170     int addJointType(const std::string& name,
00171                      int                numMobilities,
00172                      bool               haveGoodLoopJointAvailable = false,
00173                      void*              userRef                    = 0);
00174 
00175 
00197     void addBody(const std::string&  name, 
00198                  double              mass, 
00199                  bool                mustBeBaseBody,
00200                  void*               userRef = 0);
00201 
00209     bool deleteBody(const std::string&  name);
00210 
00243     void addJoint(const std::string& name,
00244                   const std::string& type,
00245                   const std::string& parentBodyName,
00246                   const std::string& childBodyName,
00247                   bool               mustBeLoopJoint,
00248                   void*              userRef = 0);
00249 
00258     bool deleteJoint(const std::string& name);
00259 
00262     void generateGraph();
00264     void clearGraph();
00265 
00267     void dumpGraph(std::ostream& out) const;
00268 
00275     int getNumMobilizers() const {return (int)mobilizers.size();}
00278     const Mobilizer& getMobilizer(int mobilizerNum) const
00279     {   return mobilizers[mobilizerNum]; }
00280 
00288     int getNumLoopConstraints() const {return (int)constraints.size();}
00291     const LoopConstraint& getLoopConstraint(int loopConstraintNum) const
00292     {   return constraints[loopConstraintNum]; }
00293 
00296     int getNumBodies() const {return (int)bodies.size();}
00300     const Body& getBody(int bodyNum) const {return bodies[bodyNum];}
00304     int getBodyNum(const std::string& bodyName) const {
00305         std::map<std::string,int>::const_iterator p = 
00306             bodyName2Num.find(bodyName);
00307         return p==bodyName2Num.end() ? -1 : p->second;
00308     }
00309 
00314     int getNumJoints() const {return (int)joints.size();}
00317     const Joint& getJoint(int jointNum) const {return joints[jointNum];}
00321     int getJointNum(const std::string& jointName) const {
00322         std::map<std::string,int>::const_iterator p = 
00323             jointName2Num.find(jointName);
00324         return p==jointName2Num.end() ? -1 : p->second;
00325     }
00326 
00328     int getNumJointTypes() const {return (int)jointTypes.size();}
00330     const JointType& getJointType(int jointTypeNum) const 
00331     {   return jointTypes[jointTypeNum]; }
00333     int getJointTypeNum(const std::string& jointTypeName) const {
00334         std::map<std::string,int>::const_iterator p = 
00335             jointTypeName2Num.find(jointTypeName);
00336         return p==jointTypeName2Num.end() ? -1 : p->second;
00337     }
00338 
00342     void setWeldJointTypeName(const std::string& name) 
00343     {   weldTypeName=name; initialize(); }
00346     const std::string& getWeldJointTypeName() const {return weldTypeName;}
00347 
00351     void setFreeJointTypeName(const std::string& name) 
00352     {   freeTypeName=name; initialize(); }
00355     const std::string& getFreeJointTypeName() const {return freeTypeName;}
00356 
00359     const std::string& getGroundBodyName() const;
00360 private:
00361     // Get writable access to bodies and joints.
00362     Body& updBody(int bodyNum) {return bodies[bodyNum];}
00363     Joint& updJoint(int jointNum) {return joints[jointNum];}
00364     Joint& updJoint(const std::string& name) {return joints[jointName2Num[name]];}
00365 
00366     void initialize();
00367     int splitBody(int bodyNum);
00368     int chooseNewBaseBody() const;
00369     void connectBodyToGround(int bodyNum);
00370     int addMobilizerForJoint(int jointNum);
00371     int findHeaviestUnassignedForwardJoint(int inboardBody) const;
00372     int findHeaviestUnassignedReverseJoint(int inboardBody) const;
00373     void growTree();
00374     void breakLoops();
00375     bool bodiesAreConnected(int b1, int b2) const;
00376 
00377     // Clear everything except for default names.
00378     void clear() {
00379         bodies.clear(); joints.clear(); jointTypes.clear();
00380         bodyName2Num.clear(); jointName2Num.clear(); jointTypeName2Num.clear();
00381         mobilizers.clear(); constraints.clear();
00382     }
00383 
00384     std::string                 weldTypeName, freeTypeName;
00385     std::vector<Body>           bodies; // ground + input bodies + slaves
00386     std::vector<Joint>          joints; // input joints + added joints
00387     std::vector<JointType>      jointTypes;
00388     std::map<std::string,int>   bodyName2Num;
00389     std::map<std::string,int>   jointName2Num;
00390     std::map<std::string,int>   jointTypeName2Num;
00391 
00392     // Calculated by generateGraph()
00393     std::vector<Mobilizer>      mobilizers; // mobilized bodies
00394     std::vector<LoopConstraint> constraints;
00395 };
00396 
00397 //------------------------------------------------------------------------------
00398 //                      MULTIBODY GRAPH MAKER :: BODY
00399 //------------------------------------------------------------------------------
00401 class MultibodyGraphMaker::Body {
00402 public:
00403     explicit Body(const std::string&    name, 
00404                     double              mass, 
00405                     bool                mustBeBaseBody,
00406                     void*               userRef) 
00407     :   name(name), mass(mass), mustBeBaseBody(mustBeBaseBody), 
00408         userRef(userRef), level(-1), mobilizer(-1), master(-1) {}
00409 
00410     void forgetGraph(MultibodyGraphMaker& graph);
00411     int getNumFragments() const {return 1 + getNumSlaves();}
00412     int getNumSlaves() const {return (int)slaves.size();}
00413     int getNumJoints() const 
00414     {   return int(jointsAsChild.size() + jointsAsParent.size()); }
00415     bool isSlave() const {return master >= 0;}
00416     bool isMaster() const {return getNumSlaves()>0;}
00417     bool isInTree() const {return level>=0;}
00418 
00419     // Inputs
00420     std::string name;
00421     double      mass;
00422     bool        mustBeBaseBody;
00423     void*       userRef;
00424 
00425     // How this body appears in joints (input and added).
00426     std::vector<int>    jointsAsChild;  // where this body is the child
00427     std::vector<int>    jointsAsParent; // where this body is the parent
00428 
00429     // Disposition of this body in the spanning tree.
00430 
00431     int level; // Ground=0, connected to Ground=1, contact to that=2, etc.
00432     int mobilizer; // the unique mobilizer where this is the outboard body
00433 
00434     int                 master; // >=0 if this is a slave
00435     std::vector<int>    slaves; // slave links, if this is a master
00436 };
00437 
00438 //------------------------------------------------------------------------------
00439 //                      MULTIBODY GRAPH MAKER :: JOINT
00440 //------------------------------------------------------------------------------
00442 class MultibodyGraphMaker::Joint {
00443 public:
00444     Joint(const std::string& name, int jointTypeNum, 
00445           int parentBodyNum, int childBodyNum,
00446           bool mustBeLoopJoint, void* userRef)
00447     :   name(name), jointTypeNum(jointTypeNum), 
00448         parentBodyNum(parentBodyNum), childBodyNum(childBodyNum),
00449         mustBeLoopJoint(mustBeLoopJoint), userRef(userRef),
00450         isAddedBaseJoint(false), mobilizer(-1), loopConstraint(-1) {}
00451 
00454     bool forgetGraph(MultibodyGraphMaker& graph);
00455 
00456     // Only one of these will be true -- we don't consider it a LoopConstraint
00457     // if we split a body and weld it back.
00458     bool hasMobilizer() const {return mobilizer>=0;}
00459     bool hasLoopConstraint() const {return loopConstraint>=0;}
00460 
00461     // Inputs
00462     std::string name;
00463     bool        mustBeLoopJoint;
00464     void*       userRef;
00465 
00466     // Mapping of strings to indices for fast lookup.
00467     int parentBodyNum, childBodyNum;
00468     int jointTypeNum;
00469 
00470     bool isAddedBaseJoint; // true if this wasn't one of the input joints
00471 
00472     // Disposition of this joint in the multibody graph.
00473     int mobilizer;      // if this joint is part of the spanning tree, else -1
00474     int loopConstraint; // if this joint used a loop constraint, else -1
00475 };
00476 
00477 //------------------------------------------------------------------------------
00478 //                   MULTIBODY GRAPH MAKER :: JOINT TYPE
00479 //------------------------------------------------------------------------------
00481 class MultibodyGraphMaker::JointType {
00482 public:
00483     JointType(const std::string& name, int numMobilities, 
00484               bool haveGoodLoopJointAvailable, void* userRef)
00485     :   name(name), numMobilities(numMobilities), 
00486         haveGoodLoopJointAvailable(haveGoodLoopJointAvailable),
00487         userRef(userRef) {}
00488     std::string name;
00489     int         numMobilities;
00490     bool        haveGoodLoopJointAvailable;
00491     void*       userRef;
00492 };
00493 
00494 //------------------------------------------------------------------------------
00495 //                   MULTIBODY GRAPH MAKER :: MOBILIZER
00496 //------------------------------------------------------------------------------
00500 class MultibodyGraphMaker::Mobilizer {
00501 public:
00502     Mobilizer() 
00503     :   joint(-1), level(-1), inboardBody(-1), outboardBody(-1),
00504         isReversed(false), mgm(0) {}
00505     Mobilizer(int jointNum, int level, int inboardBodyNum, int outboardBodyNum, 
00506               bool isReversed, MultibodyGraphMaker* graphMaker)
00507     :   joint(jointNum), level(level), inboardBody(inboardBodyNum), 
00508         outboardBody(outboardBodyNum), isReversed(isReversed),
00509         mgm(graphMaker) {}
00510 
00517     bool isAddedBaseMobilizer() const
00518     {   return mgm->getJoint(joint).isAddedBaseJoint; }
00522     void* getJointRef() const
00523     {   return mgm->getJoint(joint).userRef; } 
00528     void* getInboardBodyRef() const
00529     {   return mgm->getBody(inboardBody).userRef; }   
00535     void* getOutboardBodyRef() const
00536     {   return mgm->getBody(outboardBody).userRef; }
00541     void* getOutboardMasterBodyRef() const
00542     {   return mgm->getBody(getOutboardMasterBodyNum()).userRef; }
00544     const std::string& getJointTypeName() const
00545     {   return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).name; }
00548     void* getJointTypeRef() const
00549     {   return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).userRef; }
00552     bool isSlaveMobilizer() const
00553     {   return mgm->getBody(outboardBody).isSlave(); }
00559     int getNumFragments() const 
00560     {   return mgm->getBody(getOutboardMasterBodyNum()).getNumFragments(); }
00565     bool isReversedFromJoint() const {return isReversed;}
00566 
00567 private:
00568 friend class MultibodyGraphMaker;
00569 
00570     int getOutboardMasterBodyNum() const
00571     {   const Body& outb = mgm->getBody(outboardBody);
00572         return outb.isSlave() ? outb.master : outboardBody; }
00573 
00574     int  joint;         
00575     int  level;         
00576     int  inboardBody;   
00577     int  outboardBody;  
00578     bool isReversed;    
00579 
00580     MultibodyGraphMaker*    mgm; // just a reference to container
00581 };
00582 
00583 
00584 //------------------------------------------------------------------------------
00585 //                  MULTIBODY GRAPH MAKER :: LOOP CONSTRAINT
00586 //------------------------------------------------------------------------------
00589 class MultibodyGraphMaker::LoopConstraint {
00590 public:
00591     LoopConstraint() : joint(-1), parentBody(-1), childBody(-1), mgm(0) {}
00592     LoopConstraint(const std::string& type, int jointNum, 
00593                    int parentBodyNum, int childBodyNum,
00594                    MultibodyGraphMaker* graphMaker) 
00595     :   type(type), joint(jointNum), 
00596         parentBody(parentBodyNum), childBody(childBodyNum), 
00597         mgm(graphMaker) {}
00598 
00601     void* getJointRef() const
00602     {   return mgm->getJoint(joint).userRef; } 
00605     const std::string& getJointTypeName() const
00606     {   return type; }
00609     void* getParentBodyRef() const
00610     {   return mgm->getBody(parentBody).userRef; }   
00613     void* getChildBodyRef() const
00614     {   return mgm->getBody(childBody).userRef; }
00615 
00616 private:
00617 friend class MultibodyGraphMaker;
00618 
00619     std::string type;        // e.g., ball
00620     int         joint;       // always one of the input joints
00621     int         parentBody;  // parent from the joint
00622     int         childBody;   // child from the joint
00623 
00624     MultibodyGraphMaker*    mgm; // just a reference to container
00625 };
00626 
00627 
00628 } // namespace SimTK
00629 
00630 #endif // SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
00631 
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Defines