1 #ifndef SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_ 2 #define SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_ 170 int addJointType(
const std::string& name,
172 bool haveGoodLoopJointAvailable =
false,
197 void addBody(
const std::string& name,
209 bool deleteBody(
const std::string& name);
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,
258 bool deleteJoint(
const std::string& name);
262 void generateGraph();
267 void dumpGraph(std::ostream& out)
const;
279 {
return mobilizers[mobilizerNum]; }
292 {
return constraints[loopConstraintNum]; }
305 std::map<std::string,int>::const_iterator p =
306 bodyName2Num.find(bodyName);
307 return p==bodyName2Num.end() ? -1 : p->second;
322 std::map<std::string,int>::const_iterator p =
323 jointName2Num.find(jointName);
324 return p==jointName2Num.end() ? -1 : p->second;
331 {
return jointTypes[jointTypeNum]; }
334 std::map<std::string,int>::const_iterator p =
335 jointTypeName2Num.find(jointTypeName);
336 return p==jointTypeName2Num.end() ? -1 : p->second;
343 { weldTypeName=name; initialize(); }
352 { freeTypeName=name; initialize(); }
359 const std::string& getGroundBodyName()
const;
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]];}
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;
375 bool bodiesAreConnected(
int b1,
int b2)
const;
379 bodies.clear(); joints.clear(); jointTypes.clear();
380 bodyName2Num.clear(); jointName2Num.clear(); jointTypeName2Num.clear();
381 mobilizers.clear(); constraints.clear();
384 std::string weldTypeName, freeTypeName;
385 std::vector<Body> bodies;
386 std::vector<Joint> 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;
393 std::vector<Mobilizer> mobilizers;
394 std::vector<LoopConstraint> constraints;
403 explicit Body(
const std::string& name,
407 : name(name), mass(mass), mustBeBaseBody(mustBeBaseBody),
408 userRef(userRef), level(-1), mobilizer(-1), master(-1) {}
414 {
return int(jointsAsChild.size() + jointsAsParent.size()); }
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) {}
484 bool haveGoodLoopJointAvailable,
void* userRef)
485 : name(name), numMobilities(numMobilities),
486 haveGoodLoopJointAvailable(haveGoodLoopJointAvailable),
503 : joint(-1), level(-1), inboardBody(-1), outboardBody(-1),
504 isReversed(false), mgm(0) {}
505 Mobilizer(
int jointNum,
int level,
int inboardBodyNum,
int outboardBodyNum,
507 : joint(jointNum), level(level), inboardBody(inboardBodyNum),
508 outboardBody(outboardBodyNum), isReversed(isReversed),
518 {
return mgm->getJoint(joint).isAddedBaseJoint; }
523 {
return mgm->getJoint(joint).userRef; }
529 {
return mgm->getBody(inboardBody).userRef; }
536 {
return mgm->getBody(outboardBody).userRef; }
542 {
return mgm->getBody(getOutboardMasterBodyNum()).userRef; }
545 {
return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).name; }
549 {
return mgm->getJointType(mgm->getJoint(joint).jointTypeNum).userRef; }
553 {
return mgm->getBody(outboardBody).isSlave(); }
560 {
return mgm->getBody(getOutboardMasterBodyNum()).getNumFragments(); }
570 int getOutboardMasterBodyNum()
const 571 {
const Body& outb = mgm->getBody(outboardBody);
593 int parentBodyNum,
int childBodyNum,
595 : type(type), joint(jointNum),
596 parentBody(parentBodyNum), childBody(childBodyNum),
602 {
return mgm->getJoint(joint).userRef; }
610 {
return mgm->getBody(parentBody).userRef; }
614 {
return mgm->getBody(childBody).userRef; }
630 #endif // SimTK_SIMMATH_MULTIBODY_GRAPH_MAKER_H_ 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'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