Simbody  3.6
MultibodyGraphMaker.h
Go to the documentation of this file.
1 #ifndef SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
2 #define SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
3 
4 /* -------------------------------------------------------------------------- *
5  * Simbody(tm): Multibody Graph Maker *
6  * -------------------------------------------------------------------------- *
7  * This is part of the SimTK biosimulation toolkit originating from *
8  * Simbios, the NIH National Center for Physics-Based Simulation of *
9  * Biological Structures at Stanford, funded under the NIH Roadmap for *
10  * Medical Research, grant U54 GM072970. See https://simtk.org/home/simbody. *
11  * *
12  * Portions copyright (c) 2013-4 Stanford University and the Authors. *
13  * Authors: Michael Sherman *
14  * Contributors: Kevin He *
15  * *
16  * Licensed under the Apache License, Version 2.0 (the "License"); you may *
17  * not use this file except in compliance with the License. You may obtain a *
18  * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
19  * *
20  * Unless required by applicable law or agreed to in writing, software *
21  * distributed under the License is distributed on an "AS IS" BASIS, *
22  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
23  * See the License for the specific language governing permissions and *
24  * limitations under the License. *
25  * -------------------------------------------------------------------------- */
26 
32 #include "SimTKcommon.h"
34 
35 #include <utility>
36 #include <string>
37 #include <vector>
38 #include <map>
39 #include <iosfwd>
40 
41 namespace SimTK {
42 
43 //==============================================================================
44 // MULTIBODY GRAPH MAKER
45 //==============================================================================
153 public:
154  // Local classes.
155  class Body;
156  class Joint;
157  class JointType;
158  class Mobilizer;
159  class LoopConstraint;
160 
164 
165 
170  int addJointType(const std::string& name,
171  int numMobilities,
172  bool haveGoodLoopJointAvailable = false,
173  void* userRef = 0);
174 
175 
197  void addBody(const std::string& name,
198  double mass,
199  bool mustBeBaseBody,
200  void* userRef = 0);
201 
209  bool deleteBody(const std::string& name);
210 
243  void addJoint(const std::string& name,
244  const std::string& type,
245  const std::string& parentBodyName,
246  const std::string& childBodyName,
247  bool mustBeLoopJoint,
248  void* userRef = 0);
249 
258  bool deleteJoint(const std::string& name);
259 
262  void generateGraph();
264  void clearGraph();
265 
267  void dumpGraph(std::ostream& out) const;
268 
275  int getNumMobilizers() const {return (int)mobilizers.size();}
278  const Mobilizer& getMobilizer(int mobilizerNum) const
279  { return mobilizers[mobilizerNum]; }
280 
288  int getNumLoopConstraints() const {return (int)constraints.size();}
291  const LoopConstraint& getLoopConstraint(int loopConstraintNum) const
292  { return constraints[loopConstraintNum]; }
293 
296  int getNumBodies() const {return (int)bodies.size();}
300  const Body& getBody(int bodyNum) const {return bodies[bodyNum];}
304  int getBodyNum(const std::string& bodyName) const {
305  std::map<std::string,int>::const_iterator p =
306  bodyName2Num.find(bodyName);
307  return p==bodyName2Num.end() ? -1 : p->second;
308  }
309 
314  int getNumJoints() const {return (int)joints.size();}
317  const Joint& getJoint(int jointNum) const {return joints[jointNum];}
321  int getJointNum(const std::string& jointName) const {
322  std::map<std::string,int>::const_iterator p =
323  jointName2Num.find(jointName);
324  return p==jointName2Num.end() ? -1 : p->second;
325  }
326 
328  int getNumJointTypes() const {return (int)jointTypes.size();}
330  const JointType& getJointType(int jointTypeNum) const
331  { return jointTypes[jointTypeNum]; }
333  int getJointTypeNum(const std::string& jointTypeName) const {
334  std::map<std::string,int>::const_iterator p =
335  jointTypeName2Num.find(jointTypeName);
336  return p==jointTypeName2Num.end() ? -1 : p->second;
337  }
338 
342  void setWeldJointTypeName(const std::string& name)
343  { weldTypeName=name; initialize(); }
346  const std::string& getWeldJointTypeName() const {return weldTypeName;}
347 
351  void setFreeJointTypeName(const std::string& name)
352  { freeTypeName=name; initialize(); }
355  const std::string& getFreeJointTypeName() const {return freeTypeName;}
356 
359  const std::string& getGroundBodyName() const;
360 private:
361  // Get writable access to bodies and joints.
362  Body& updBody(int bodyNum) {return bodies[bodyNum];}
363  Joint& updJoint(int jointNum) {return joints[jointNum];}
364  Joint& updJoint(const std::string& name) {return joints[jointName2Num[name]];}
365 
366  void initialize();
367  int splitBody(int bodyNum);
368  int chooseNewBaseBody() const;
369  void connectBodyToGround(int bodyNum);
370  int addMobilizerForJoint(int jointNum);
371  int findHeaviestUnassignedForwardJoint(int inboardBody) const;
372  int findHeaviestUnassignedReverseJoint(int inboardBody) const;
373  void growTree();
374  void breakLoops();
375  bool bodiesAreConnected(int b1, int b2) const;
376 
377  // Clear everything except for default names.
378  void clear() {
379  bodies.clear(); joints.clear(); jointTypes.clear();
380  bodyName2Num.clear(); jointName2Num.clear(); jointTypeName2Num.clear();
381  mobilizers.clear(); constraints.clear();
382  }
383 
384  std::string weldTypeName, freeTypeName;
385  std::vector<Body> bodies; // ground + input bodies + slaves
386  std::vector<Joint> joints; // input joints + added joints
387  std::vector<JointType> jointTypes;
388  std::map<std::string,int> bodyName2Num;
389  std::map<std::string,int> jointName2Num;
390  std::map<std::string,int> jointTypeName2Num;
391 
392  // Calculated by generateGraph()
393  std::vector<Mobilizer> mobilizers; // mobilized bodies
394  std::vector<LoopConstraint> constraints;
395 };
396 
397 //------------------------------------------------------------------------------
398 // MULTIBODY GRAPH MAKER :: BODY
399 //------------------------------------------------------------------------------
402 public:
403  explicit Body(const std::string& name,
404  double mass,
405  bool mustBeBaseBody,
406  void* userRef)
407  : name(name), mass(mass), mustBeBaseBody(mustBeBaseBody),
408  userRef(userRef), level(-1), mobilizer(-1), master(-1) {}
409 
410  void forgetGraph(MultibodyGraphMaker& graph);
411  int getNumFragments() const {return 1 + getNumSlaves();}
412  int getNumSlaves() const {return (int)slaves.size();}
413  int getNumJoints() const
414  { return int(jointsAsChild.size() + jointsAsParent.size()); }
415  bool isSlave() const {return master >= 0;}
416  bool isMaster() const {return getNumSlaves()>0;}
417  bool isInTree() const {return level>=0;}
418 
419  // Inputs
420  std::string name;
421  double mass;
423  void* userRef;
424 
425  // How this body appears in joints (input and added).
426  std::vector<int> jointsAsChild; // where this body is the child
427  std::vector<int> jointsAsParent; // where this body is the parent
428 
429  // Disposition of this body in the spanning tree.
430 
431  int level; // Ground=0, connected to Ground=1, contact to that=2, etc.
432  int mobilizer; // the unique mobilizer where this is the outboard body
433 
434  int master; // >=0 if this is a slave
435  std::vector<int> slaves; // slave links, if this is a master
436 };
437 
438 //------------------------------------------------------------------------------
439 // MULTIBODY GRAPH MAKER :: JOINT
440 //------------------------------------------------------------------------------
443 public:
444  Joint(const std::string& name, int jointTypeNum,
445  int parentBodyNum, int childBodyNum,
446  bool mustBeLoopJoint, void* userRef)
447  : name(name),
448  mustBeLoopJoint(mustBeLoopJoint),
449  userRef(userRef),
450  parentBodyNum(parentBodyNum),
451  childBodyNum(childBodyNum),
452  jointTypeNum(jointTypeNum),
453  isAddedBaseJoint(false),
454  mobilizer(-1),
455  loopConstraint(-1) {}
456 
459  bool forgetGraph(MultibodyGraphMaker& graph);
460 
461  // Only one of these will be true -- we don't consider it a LoopConstraint
462  // if we split a body and weld it back.
463  bool hasMobilizer() const {return mobilizer>=0;}
464  bool hasLoopConstraint() const {return loopConstraint>=0;}
465 
466  // Inputs
467  std::string name;
469  void* userRef;
470 
471  // Mapping of strings to indices for fast lookup.
472  int parentBodyNum, childBodyNum;
474 
475  bool isAddedBaseJoint; // true if this wasn't one of the input joints
476 
477  // Disposition of this joint in the multibody graph.
478  int mobilizer; // if this joint is part of the spanning tree, else -1
479  int loopConstraint; // if this joint used a loop constraint, else -1
480 };
481 
482 //------------------------------------------------------------------------------
483 // MULTIBODY GRAPH MAKER :: JOINT TYPE
484 //------------------------------------------------------------------------------
487 public:
488  JointType(const std::string& name, int numMobilities,
489  bool haveGoodLoopJointAvailable, void* userRef)
490  : name(name), numMobilities(numMobilities),
491  haveGoodLoopJointAvailable(haveGoodLoopJointAvailable),
492  userRef(userRef) {}
493  std::string name;
496  void* userRef;
497 };
498 
499 //------------------------------------------------------------------------------
500 // MULTIBODY GRAPH MAKER :: MOBILIZER
501 //------------------------------------------------------------------------------
506 public:
508  : joint(-1), level(-1), inboardBody(-1), outboardBody(-1),
509  isReversed(false), mgm(0) {}
510  Mobilizer(int jointNum, int level, int inboardBodyNum, int outboardBodyNum,
511  bool isReversed, MultibodyGraphMaker* graphMaker)
512  : joint(jointNum), level(level), inboardBody(inboardBodyNum),
513  outboardBody(outboardBodyNum), isReversed(isReversed),
514  mgm(graphMaker) {}
515 
522  bool isAddedBaseMobilizer() const
523  { return mgm->getJoint(joint).isAddedBaseJoint; }
527  void* getJointRef() const
528  { return mgm->getJoint(joint).userRef; }
533  void* getInboardBodyRef() const
534  { return mgm->getBody(inboardBody).userRef; }
540  void* getOutboardBodyRef() const
541  { return mgm->getBody(outboardBody).userRef; }
547  { return mgm->getBody(getOutboardMasterBodyNum()).userRef; }
549  const std::string& getJointTypeName() const
550  { return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).name; }
553  void* getJointTypeRef() const
554  { return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).userRef; }
557  bool isSlaveMobilizer() const
558  { return mgm->getBody(outboardBody).isSlave(); }
564  int getNumFragments() const
565  { return mgm->getBody(getOutboardMasterBodyNum()).getNumFragments(); }
570  bool isReversedFromJoint() const {return isReversed;}
572  int getLevel() const {return level;}
573 
574 private:
575 friend class MultibodyGraphMaker;
576 
577  int getOutboardMasterBodyNum() const
578  { const Body& outb = mgm->getBody(outboardBody);
579  return outb.isSlave() ? outb.master : outboardBody; }
580 
581  int joint;
582  int level;
583  int inboardBody;
584  int outboardBody;
585  bool isReversed;
586 
587  MultibodyGraphMaker* mgm; // just a reference to container
588 };
589 
590 
591 //------------------------------------------------------------------------------
592 // MULTIBODY GRAPH MAKER :: LOOP CONSTRAINT
593 //------------------------------------------------------------------------------
597 public:
598  LoopConstraint() : joint(-1), parentBody(-1), childBody(-1), mgm(0) {}
599  LoopConstraint(const std::string& type, int jointNum,
600  int parentBodyNum, int childBodyNum,
601  MultibodyGraphMaker* graphMaker)
602  : type(type), joint(jointNum),
603  parentBody(parentBodyNum), childBody(childBodyNum),
604  mgm(graphMaker) {}
605 
608  void* getJointRef() const
609  { return mgm->getJoint(joint).userRef; }
612  const std::string& getJointTypeName() const
613  { return type; }
616  void* getParentBodyRef() const
617  { return mgm->getBody(parentBody).userRef; }
620  void* getChildBodyRef() const
621  { return mgm->getBody(childBody).userRef; }
622 
623 private:
624 friend class MultibodyGraphMaker;
625 
626  std::string type; // e.g., ball
627  int joint; // always one of the input joints
628  int parentBody; // parent from the joint
629  int childBody; // child from the joint
630 
631  MultibodyGraphMaker* mgm; // just a reference to container
632 };
633 
634 
635 } // namespace SimTK
636 
637 #endif // SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
638 
int getNumJointTypes() const
Return the number of registered joint types.
Definition: MultibodyGraphMaker.h:328
int getJointTypeNum(const std::string &jointTypeName) const
Get the assigned number for a joint type from the type name.
Definition: MultibodyGraphMaker.h:333
std::string name
Definition: MultibodyGraphMaker.h:493
const Joint & getJoint(int jointNum) const
Get a Joint object by its assigned number.
Definition: MultibodyGraphMaker.h:317
int getNumSlaves() const
Definition: MultibodyGraphMaker.h:412
bool mustBeBaseBody
Definition: MultibodyGraphMaker.h:422
LoopConstraint(const std::string &type, int jointNum, int parentBodyNum, int childBodyNum, MultibodyGraphMaker *graphMaker)
Definition: MultibodyGraphMaker.h:599
bool isAddedBaseMobilizer() const
Return true if this mobilizer does not represent one of the input joints, but is instead a joint we a...
Definition: MultibodyGraphMaker.h:522
void * userRef
Definition: MultibodyGraphMaker.h:469
Local class that collects information about joints.
Definition: MultibodyGraphMaker.h:442
void * getJointRef() const
Get the user reference pointer for the joint associated with this loop constraint.
Definition: MultibodyGraphMaker.h:608
int getBodyNum(const std::string &bodyName) const
Return the body number assigned to the input body with the given name.
Definition: MultibodyGraphMaker.h:304
int jointTypeNum
Definition: MultibodyGraphMaker.h:473
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37
void * userRef
Definition: MultibodyGraphMaker.h:423
std::vector< int > slaves
Definition: MultibodyGraphMaker.h:435
int mobilizer
Definition: MultibodyGraphMaker.h:478
void setFreeJointTypeName(const std::string &name)
Change the name to be used to identify the free (6 dof) joint type and free (0 constraints) loop cons...
Definition: MultibodyGraphMaker.h:351
bool mustBeLoopJoint
Definition: MultibodyGraphMaker.h:468
Mobilizer()
Definition: MultibodyGraphMaker.h:507
int getLevel() const
Return the level of the outboard body (Ground is level 0)
Definition: MultibodyGraphMaker.h:572
const std::string & getJointTypeName() const
Get the joint type name of the joint that this mobilizer represents.
Definition: MultibodyGraphMaker.h:549
int getNumBodies() const
Return the number of bodies, including all input bodies, a ground body, and any slave bodies...
Definition: MultibodyGraphMaker.h:296
int numMobilities
Definition: MultibodyGraphMaker.h:494
Joint(const std::string &name, int jointTypeNum, int parentBodyNum, int childBodyNum, bool mustBeLoopJoint, void *userRef)
Definition: MultibodyGraphMaker.h:444
Local class that defines the properties of a known joint type.
Definition: MultibodyGraphMaker.h:486
bool isAddedBaseJoint
Definition: MultibodyGraphMaker.h:475
int level
Definition: MultibodyGraphMaker.h:431
Body(const std::string &name, double mass, bool mustBeBaseBody, void *userRef)
Definition: MultibodyGraphMaker.h:403
const std::string & getWeldJointTypeName() const
Return the name currently being used to identify the weld joint type and weld loop constraint type...
Definition: MultibodyGraphMaker.h:346
void * getOutboardBodyRef() const
Get the user reference pointer for the outboard body of this mobilizer.
Definition: MultibodyGraphMaker.h:540
const Mobilizer & getMobilizer(int mobilizerNum) const
Get a Mobilizer object by its mobilizer number, ordered outwards by topological distance from Ground...
Definition: MultibodyGraphMaker.h:278
int getNumLoopConstraints() const
Return the number of loop joint constraints that were used to close loops in the graph topology...
Definition: MultibodyGraphMaker.h:288
int getNumJoints() const
Return the number of joints, including all input joints, and all joints added to connect otherwise di...
Definition: MultibodyGraphMaker.h:314
Local class that represents one of the mobilizers (tree joints) in the generated spanning tree...
Definition: MultibodyGraphMaker.h:505
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
int mobilizer
Definition: MultibodyGraphMaker.h:432
int parentBodyNum
Definition: MultibodyGraphMaker.h:472
bool hasMobilizer() const
Definition: MultibodyGraphMaker.h:463
std::vector< int > jointsAsParent
Definition: MultibodyGraphMaker.h:427
Mobilizer(int jointNum, int level, int inboardBodyNum, int outboardBodyNum, bool isReversed, MultibodyGraphMaker *graphMaker)
Definition: MultibodyGraphMaker.h:510
bool isMaster() const
Definition: MultibodyGraphMaker.h:416
LoopConstraint()
Definition: MultibodyGraphMaker.h:598
int master
Definition: MultibodyGraphMaker.h:434
std::string name
Definition: MultibodyGraphMaker.h:420
double mass
Definition: MultibodyGraphMaker.h:421
bool isSlave() const
Definition: MultibodyGraphMaker.h:415
JointType(const std::string &name, int numMobilities, bool haveGoodLoopJointAvailable, void *userRef)
Definition: MultibodyGraphMaker.h:488
void * userRef
Definition: MultibodyGraphMaker.h:496
const LoopConstraint & getLoopConstraint(int loopConstraintNum) const
Get a loop constraint by its assigned number.
Definition: MultibodyGraphMaker.h:291
const JointType & getJointType(int jointTypeNum) const
Get a JointType object by its assigned number.
Definition: MultibodyGraphMaker.h:330
int loopConstraint
Definition: MultibodyGraphMaker.h:479
bool hasLoopConstraint() const
Definition: MultibodyGraphMaker.h:464
Local class that collects information about bodies.
Definition: MultibodyGraphMaker.h:401
const std::string & getFreeJointTypeName() const
Return the name currently being used to identify the free joint type and free loop constraint type...
Definition: MultibodyGraphMaker.h:355
int getJointNum(const std::string &jointName) const
Return the joint number assigned to the input joint with the given name.
Definition: MultibodyGraphMaker.h:321
This is the header file that every Simmath compilation unit should include first. ...
const Body & getBody(int bodyNum) const
Get a Body object by its assigned number.
Definition: MultibodyGraphMaker.h:300
bool haveGoodLoopJointAvailable
Definition: MultibodyGraphMaker.h:495
void setWeldJointTypeName(const std::string &name)
Change the name to be used to identify the weld joint type (0 dof) and weld loop constraint type (6 c...
Definition: MultibodyGraphMaker.h:342
int getNumJoints() const
Definition: MultibodyGraphMaker.h:413
void * getJointRef() const
Get the user reference pointer for the joint associated with this mobilizer, if there is such a joint...
Definition: MultibodyGraphMaker.h:527
void * getChildBodyRef() const
Get the user reference pointer for the child body defined by the joint associated with this loop cons...
Definition: MultibodyGraphMaker.h:620
void * getJointTypeRef() const
Get the reference pointer (if any) that was provided when this mobilizer&#39;s joint type was defined in ...
Definition: MultibodyGraphMaker.h:553
Local class that represents one of the constraints that were added to close topological loops that we...
Definition: MultibodyGraphMaker.h:596
const std::string & getJointTypeName() const
Get the loop constraint type name of the constraint that should be used here.
Definition: MultibodyGraphMaker.h:612
std::string name
Definition: MultibodyGraphMaker.h:467
bool isReversedFromJoint() const
Return true if this mobilizer represents one of the input joints but the sense of inboard->outboard i...
Definition: MultibodyGraphMaker.h:570
void * getOutboardMasterBodyRef() const
Get the user reference pointer for the outboard body of this mobilizer, if it is one of the input bod...
Definition: MultibodyGraphMaker.h:546
void * getParentBodyRef() const
Get the user reference pointer for the parent body defined by the joint associated with this loop con...
Definition: MultibodyGraphMaker.h:616
std::vector< int > jointsAsChild
Definition: MultibodyGraphMaker.h:426
bool isInTree() const
Definition: MultibodyGraphMaker.h:417
int getNumFragments() const
Definition: MultibodyGraphMaker.h:411
int getNumMobilizers() const
Returns the number of mobilizers (tree joints) in the spanning tree.
Definition: MultibodyGraphMaker.h:275
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
bool isSlaveMobilizer() const
Return true if the outboard body of this mobilizer is a slave we created in order to cut a loop...
Definition: MultibodyGraphMaker.h:557
int getNumFragments() const
Return the number of fragments into which we chopped the outboard body of this mobilizer.
Definition: MultibodyGraphMaker.h:564
void * getInboardBodyRef() const
Get the user reference pointer for the inboard body of this mobilizer.
Definition: MultibodyGraphMaker.h:533
Construct a reasonably good spanning-tree-plus-constraints structure for modeling a given set of bodi...
Definition: MultibodyGraphMaker.h:152