Simbody  3.5
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), jointTypeNum(jointTypeNum),
448  parentBodyNum(parentBodyNum), childBodyNum(childBodyNum),
449  mustBeLoopJoint(mustBeLoopJoint), userRef(userRef),
450  isAddedBaseJoint(false), mobilizer(-1), loopConstraint(-1) {}
451 
454  bool forgetGraph(MultibodyGraphMaker& graph);
455 
456  // Only one of these will be true -- we don't consider it a LoopConstraint
457  // if we split a body and weld it back.
458  bool hasMobilizer() const {return mobilizer>=0;}
459  bool hasLoopConstraint() const {return loopConstraint>=0;}
460 
461  // Inputs
462  std::string name;
464  void* userRef;
465 
466  // Mapping of strings to indices for fast lookup.
467  int parentBodyNum, childBodyNum;
469 
470  bool isAddedBaseJoint; // true if this wasn't one of the input joints
471 
472  // Disposition of this joint in the multibody graph.
473  int mobilizer; // if this joint is part of the spanning tree, else -1
474  int loopConstraint; // if this joint used a loop constraint, else -1
475 };
476 
477 //------------------------------------------------------------------------------
478 // MULTIBODY GRAPH MAKER :: JOINT TYPE
479 //------------------------------------------------------------------------------
482 public:
483  JointType(const std::string& name, int numMobilities,
484  bool haveGoodLoopJointAvailable, void* userRef)
485  : name(name), numMobilities(numMobilities),
486  haveGoodLoopJointAvailable(haveGoodLoopJointAvailable),
487  userRef(userRef) {}
488  std::string name;
491  void* userRef;
492 };
493 
494 //------------------------------------------------------------------------------
495 // MULTIBODY GRAPH MAKER :: MOBILIZER
496 //------------------------------------------------------------------------------
501 public:
503  : joint(-1), level(-1), inboardBody(-1), outboardBody(-1),
504  isReversed(false), mgm(0) {}
505  Mobilizer(int jointNum, int level, int inboardBodyNum, int outboardBodyNum,
506  bool isReversed, MultibodyGraphMaker* graphMaker)
507  : joint(jointNum), level(level), inboardBody(inboardBodyNum),
508  outboardBody(outboardBodyNum), isReversed(isReversed),
509  mgm(graphMaker) {}
510 
517  bool isAddedBaseMobilizer() const
518  { return mgm->getJoint(joint).isAddedBaseJoint; }
522  void* getJointRef() const
523  { return mgm->getJoint(joint).userRef; }
528  void* getInboardBodyRef() const
529  { return mgm->getBody(inboardBody).userRef; }
535  void* getOutboardBodyRef() const
536  { return mgm->getBody(outboardBody).userRef; }
542  { return mgm->getBody(getOutboardMasterBodyNum()).userRef; }
544  const std::string& getJointTypeName() const
545  { return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).name; }
548  void* getJointTypeRef() const
549  { return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).userRef; }
552  bool isSlaveMobilizer() const
553  { return mgm->getBody(outboardBody).isSlave(); }
559  int getNumFragments() const
560  { return mgm->getBody(getOutboardMasterBodyNum()).getNumFragments(); }
565  bool isReversedFromJoint() const {return isReversed;}
566 
567 private:
568 friend class MultibodyGraphMaker;
569 
570  int getOutboardMasterBodyNum() const
571  { const Body& outb = mgm->getBody(outboardBody);
572  return outb.isSlave() ? outb.master : outboardBody; }
573 
574  int joint;
575  int level;
576  int inboardBody;
577  int outboardBody;
578  bool isReversed;
579 
580  MultibodyGraphMaker* mgm; // just a reference to container
581 };
582 
583 
584 //------------------------------------------------------------------------------
585 // MULTIBODY GRAPH MAKER :: LOOP CONSTRAINT
586 //------------------------------------------------------------------------------
590 public:
591  LoopConstraint() : joint(-1), parentBody(-1), childBody(-1), mgm(0) {}
592  LoopConstraint(const std::string& type, int jointNum,
593  int parentBodyNum, int childBodyNum,
594  MultibodyGraphMaker* graphMaker)
595  : type(type), joint(jointNum),
596  parentBody(parentBodyNum), childBody(childBodyNum),
597  mgm(graphMaker) {}
598 
601  void* getJointRef() const
602  { return mgm->getJoint(joint).userRef; }
605  const std::string& getJointTypeName() const
606  { return type; }
609  void* getParentBodyRef() const
610  { return mgm->getBody(parentBody).userRef; }
613  void* getChildBodyRef() const
614  { return mgm->getBody(childBody).userRef; }
615 
616 private:
617 friend class MultibodyGraphMaker;
618 
619  std::string type; // e.g., ball
620  int joint; // always one of the input joints
621  int parentBody; // parent from the joint
622  int childBody; // child from the joint
623 
624  MultibodyGraphMaker* mgm; // just a reference to container
625 };
626 
627 
628 } // namespace SimTK
629 
630 #endif // SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_
631 
bool hasMobilizer() const
Definition: MultibodyGraphMaker.h:458
std::string name
Definition: MultibodyGraphMaker.h:488
bool isSlave() const
Definition: MultibodyGraphMaker.h:415
bool mustBeBaseBody
Definition: MultibodyGraphMaker.h:422
const std::string & getJointTypeName() const
Get the loop constraint type name of the constraint that should be used here.
Definition: MultibodyGraphMaker.h:605
LoopConstraint(const std::string &type, int jointNum, int parentBodyNum, int childBodyNum, MultibodyGraphMaker *graphMaker)
Definition: MultibodyGraphMaker.h:592
int getJointNum(const std::string &jointName) const
Return the joint number assigned to the input joint with the given name.
Definition: MultibodyGraphMaker.h:321
void * userRef
Definition: MultibodyGraphMaker.h:464
Local class that collects information about joints.
Definition: MultibodyGraphMaker.h:442
int getNumFragments() const
Definition: MultibodyGraphMaker.h:411
int jointTypeNum
Definition: MultibodyGraphMaker.h:468
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:473
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
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:463
int getJointTypeNum(const std::string &jointTypeName) const
Get the assigned number for a joint type from the type name.
Definition: MultibodyGraphMaker.h:333
Mobilizer()
Definition: MultibodyGraphMaker.h:502
int getBodyNum(const std::string &bodyName) const
Return the body number assigned to the input body with the given name.
Definition: MultibodyGraphMaker.h:304
const LoopConstraint & getLoopConstraint(int loopConstraintNum) const
Get a loop constraint by its assigned number.
Definition: MultibodyGraphMaker.h:291
int getNumJoints() const
Return the number of joints, including all input joints, and all joints added to connect otherwise di...
Definition: MultibodyGraphMaker.h:314
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:541
const std::string & getJointTypeName() const
Get the joint type name of the joint that this mobilizer represents.
Definition: MultibodyGraphMaker.h:544
int numMobilities
Definition: MultibodyGraphMaker.h:489
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:481
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:552
int getNumMobilizers() const
Returns the number of mobilizers (tree joints) in the spanning tree.
Definition: MultibodyGraphMaker.h:275
int getNumJoints() const
Definition: MultibodyGraphMaker.h:413
bool isAddedBaseJoint
Definition: MultibodyGraphMaker.h:470
int getNumSlaves() const
Definition: MultibodyGraphMaker.h:412
int level
Definition: MultibodyGraphMaker.h:431
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:517
Body(const std::string &name, double mass, bool mustBeBaseBody, void *userRef)
Definition: MultibodyGraphMaker.h:403
Local class that represents one of the mobilizers (tree joints) in the generated spanning tree...
Definition: MultibodyGraphMaker.h:500
Includes internal headers providing declarations for the basic SimTK Core classes, including Simmatrix.
int mobilizer
Definition: MultibodyGraphMaker.h:432
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:548
int parentBodyNum
Definition: MultibodyGraphMaker.h:467
int getNumFragments() const
Return the number of fragments into which we chopped the outboard body of this mobilizer.
Definition: MultibodyGraphMaker.h:559
void * getJointRef() const
Get the user reference pointer for the joint associated with this mobilizer, if there is such a joint...
Definition: MultibodyGraphMaker.h:522
std::vector< int > jointsAsParent
Definition: MultibodyGraphMaker.h:427
Mobilizer(int jointNum, int level, int inboardBodyNum, int outboardBodyNum, bool isReversed, MultibodyGraphMaker *graphMaker)
Definition: MultibodyGraphMaker.h:505
LoopConstraint()
Definition: MultibodyGraphMaker.h:591
void * getJointRef() const
Get the user reference pointer for the joint associated with this loop constraint.
Definition: MultibodyGraphMaker.h:601
int master
Definition: MultibodyGraphMaker.h:434
std::string name
Definition: MultibodyGraphMaker.h:420
bool isInTree() const
Definition: MultibodyGraphMaker.h:417
bool isMaster() const
Definition: MultibodyGraphMaker.h:416
int getNumJointTypes() const
Return the number of registered joint types.
Definition: MultibodyGraphMaker.h:328
double mass
Definition: MultibodyGraphMaker.h:421
void * getParentBodyRef() const
Get the user reference pointer for the parent body defined by the joint associated with this loop con...
Definition: MultibodyGraphMaker.h:609
const JointType & getJointType(int jointTypeNum) const
Get a JointType object by its assigned number.
Definition: MultibodyGraphMaker.h:330
JointType(const std::string &name, int numMobilities, bool haveGoodLoopJointAvailable, void *userRef)
Definition: MultibodyGraphMaker.h:483
void * userRef
Definition: MultibodyGraphMaker.h:491
int loopConstraint
Definition: MultibodyGraphMaker.h:474
Local class that collects information about bodies.
Definition: MultibodyGraphMaker.h:401
This is the header file that every Simmath compilation unit should include first. ...
int getNumLoopConstraints() const
Return the number of loop joint constraints that were used to close loops in the graph topology...
Definition: MultibodyGraphMaker.h:288
bool haveGoodLoopJointAvailable
Definition: MultibodyGraphMaker.h:490
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
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
void * getInboardBodyRef() const
Get the user reference pointer for the inboard body of this mobilizer.
Definition: MultibodyGraphMaker.h:528
void * getChildBodyRef() const
Get the user reference pointer for the child body defined by the joint associated with this loop cons...
Definition: MultibodyGraphMaker.h:613
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
Local class that represents one of the constraints that were added to close topological loops that we...
Definition: MultibodyGraphMaker.h:589
std::string name
Definition: MultibodyGraphMaker.h:462
void * getOutboardBodyRef() const
Get the user reference pointer for the outboard body of this mobilizer.
Definition: MultibodyGraphMaker.h:535
std::vector< int > jointsAsChild
Definition: MultibodyGraphMaker.h:426
const Body & getBody(int bodyNum) const
Get a Body object by its assigned number.
Definition: MultibodyGraphMaker.h:300
int getNumBodies() const
Return the number of bodies, including all input bodies, a ground body, and any slave bodies...
Definition: MultibodyGraphMaker.h:296
bool hasLoopConstraint() const
Definition: MultibodyGraphMaker.h:459
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
bool isReversedFromJoint() const
Return true if this mobilizer represents one of the input joints but the sense of inboard->outboard i...
Definition: MultibodyGraphMaker.h:565
const Joint & getJoint(int jointNum) const
Get a Joint object by its assigned number.
Definition: MultibodyGraphMaker.h:317
Construct a reasonably good spanning-tree-plus-constraints structure for modeling a given set of bodi...
Definition: MultibodyGraphMaker.h:152