Simbody 3.7
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
41namespace SimTK {
42
43//==============================================================================
44// MULTIBODY GRAPH MAKER
45//==============================================================================
153public:
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
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;
360private:
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//------------------------------------------------------------------------------
402public:
403 explicit Body(const std::string& name,
404 double mass,
405 bool mustBeBaseBody,
406 void* userRef)
408 userRef(userRef), level(-1), mobilizer(-1), master(-1) {}
409
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//------------------------------------------------------------------------------
443public:
444 Joint(const std::string& name, int jointTypeNum,
446 bool mustBeLoopJoint, void* userRef)
447 : name(name),
453 isAddedBaseJoint(false),
454 mobilizer(-1),
455 loopConstraint(-1) {}
456
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.
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//------------------------------------------------------------------------------
487public:
488 JointType(const std::string& name, int numMobilities,
492 userRef(userRef) {}
493 std::string name;
496 void* userRef;
497};
498
499//------------------------------------------------------------------------------
500// MULTIBODY GRAPH MAKER :: MOBILIZER
501//------------------------------------------------------------------------------
506public:
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
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
574private:
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//------------------------------------------------------------------------------
597public:
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
623private:
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
Includes internal headers providing declarations for the basic SimTK Core classes,...
This is the header file that every Simmath compilation unit should include first.
#define SimTK_SIMMATH_EXPORT
Definition: SimTKmath/include/simmath/internal/common.h:64
Local class that collects information about bodies.
Definition: MultibodyGraphMaker.h:401
Body(const std::string &name, double mass, bool mustBeBaseBody, void *userRef)
Definition: MultibodyGraphMaker.h:403
int master
Definition: MultibodyGraphMaker.h:434
void forgetGraph(MultibodyGraphMaker &graph)
int level
Definition: MultibodyGraphMaker.h:431
std::vector< int > jointsAsChild
Definition: MultibodyGraphMaker.h:426
bool isMaster() const
Definition: MultibodyGraphMaker.h:416
int getNumJoints() const
Definition: MultibodyGraphMaker.h:413
int getNumFragments() const
Definition: MultibodyGraphMaker.h:411
int mobilizer
Definition: MultibodyGraphMaker.h:432
std::vector< int > jointsAsParent
Definition: MultibodyGraphMaker.h:427
double mass
Definition: MultibodyGraphMaker.h:421
int getNumSlaves() const
Definition: MultibodyGraphMaker.h:412
std::string name
Definition: MultibodyGraphMaker.h:420
std::vector< int > slaves
Definition: MultibodyGraphMaker.h:435
void * userRef
Definition: MultibodyGraphMaker.h:423
bool isInTree() const
Definition: MultibodyGraphMaker.h:417
bool mustBeBaseBody
Definition: MultibodyGraphMaker.h:422
bool isSlave() const
Definition: MultibodyGraphMaker.h:415
Local class that defines the properties of a known joint type.
Definition: MultibodyGraphMaker.h:486
JointType(const std::string &name, int numMobilities, bool haveGoodLoopJointAvailable, void *userRef)
Definition: MultibodyGraphMaker.h:488
bool haveGoodLoopJointAvailable
Definition: MultibodyGraphMaker.h:495
std::string name
Definition: MultibodyGraphMaker.h:493
int numMobilities
Definition: MultibodyGraphMaker.h:494
void * userRef
Definition: MultibodyGraphMaker.h:496
Local class that collects information about joints.
Definition: MultibodyGraphMaker.h:442
std::string name
Definition: MultibodyGraphMaker.h:467
bool isAddedBaseJoint
Definition: MultibodyGraphMaker.h:475
int mobilizer
Definition: MultibodyGraphMaker.h:478
bool hasMobilizer() const
Definition: MultibodyGraphMaker.h:463
Joint(const std::string &name, int jointTypeNum, int parentBodyNum, int childBodyNum, bool mustBeLoopJoint, void *userRef)
Definition: MultibodyGraphMaker.h:444
bool mustBeLoopJoint
Definition: MultibodyGraphMaker.h:468
int parentBodyNum
Definition: MultibodyGraphMaker.h:472
void * userRef
Definition: MultibodyGraphMaker.h:469
int jointTypeNum
Definition: MultibodyGraphMaker.h:473
bool hasLoopConstraint() const
Definition: MultibodyGraphMaker.h:464
bool forgetGraph(MultibodyGraphMaker &graph)
Return true if the joint is deleted as a result of restoring it to the state prior to generateGraph()...
int childBodyNum
Definition: MultibodyGraphMaker.h:472
int loopConstraint
Definition: MultibodyGraphMaker.h:479
Local class that represents one of the constraints that were added to close topological loops that we...
Definition: MultibodyGraphMaker.h:596
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
LoopConstraint(const std::string &type, int jointNum, int parentBodyNum, int childBodyNum, MultibodyGraphMaker *graphMaker)
Definition: MultibodyGraphMaker.h:599
const std::string & getJointTypeName() const
Get the loop constraint type name of the constraint that should be used here.
Definition: MultibodyGraphMaker.h:612
void * getJointRef() const
Get the user reference pointer for the joint associated with this loop constraint.
Definition: MultibodyGraphMaker.h:608
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
LoopConstraint()
Definition: MultibodyGraphMaker.h:598
Local class that represents one of the mobilizers (tree joints) in the generated spanning tree.
Definition: MultibodyGraphMaker.h:505
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
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
int getNumFragments() const
Return the number of fragments into which we chopped the outboard body of this mobilizer.
Definition: MultibodyGraphMaker.h:564
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
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 * 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 * getOutboardBodyRef() const
Get the user reference pointer for the outboard body of this mobilizer.
Definition: MultibodyGraphMaker.h:540
const std::string & getJointTypeName() const
Get the joint type name of the joint that this mobilizer represents.
Definition: MultibodyGraphMaker.h:549
void * getInboardBodyRef() const
Get the user reference pointer for the inboard body of this mobilizer.
Definition: MultibodyGraphMaker.h:533
Mobilizer(int jointNum, int level, int inboardBodyNum, int outboardBodyNum, bool isReversed, MultibodyGraphMaker *graphMaker)
Definition: MultibodyGraphMaker.h:510
Mobilizer()
Definition: MultibodyGraphMaker.h:507
int getLevel() const
Return the level of the outboard body (Ground is level 0)
Definition: MultibodyGraphMaker.h:572
void * getJointTypeRef() const
Get the reference pointer (if any) that was provided when this mobilizer's joint type was defined in ...
Definition: MultibodyGraphMaker.h:553
Construct a reasonably good spanning-tree-plus-constraints structure for modeling a given set of bodi...
Definition: MultibodyGraphMaker.h:152
const Body & getBody(int bodyNum) const
Get a Body object by its assigned number.
Definition: MultibodyGraphMaker.h:300
int getNumLoopConstraints() const
Return the number of loop joint constraints that were used to close loops in the graph topology.
Definition: MultibodyGraphMaker.h:288
const JointType & getJointType(int jointTypeNum) const
Get a JointType object by its assigned number.
Definition: MultibodyGraphMaker.h:330
int addJointType(const std::string &name, int numMobilities, bool haveGoodLoopJointAvailable=false, void *userRef=0)
Specify relevant properties of a joint type.
void dumpGraph(std::ostream &out) const
Output a text representation of the multibody graph for debugging.
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 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
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 clearGraph()
Throw away the multibody graph produced by generateGraph().
const std::string & getGroundBodyName() const
Return the name we recognize as the Ground (or World) body.
int getJointNum(const std::string &jointName) const
Return the joint number assigned to the input joint with the given name.
Definition: MultibodyGraphMaker.h:321
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
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 getNumBodies() const
Return the number of bodies, including all input bodies, a ground body, and any slave bodies.
Definition: MultibodyGraphMaker.h:296
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 getJointTypeNum(const std::string &jointTypeName) const
Get the assigned number for a joint type from the type name.
Definition: MultibodyGraphMaker.h:333
const LoopConstraint & getLoopConstraint(int loopConstraintNum) const
Get a loop constraint by its assigned number.
Definition: MultibodyGraphMaker.h:291
MultibodyGraphMaker()
Construct an empty MultibodyGraphMaker object and set the default names for weld and free joints to "...
bool deleteBody(const std::string &name)
Delete a body (link) from the set of input bodies.
void addBody(const std::string &name, double mass, bool mustBeBaseBody, void *userRef=0)
Add a new body (link) to the set of input bodies.
int getNumJointTypes() const
Return the number of registered joint types.
Definition: MultibodyGraphMaker.h:328
void addJoint(const std::string &name, const std::string &type, const std::string &parentBodyName, const std::string &childBodyName, bool mustBeLoopJoint, void *userRef=0)
Add a new joint to the set of input joints.
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
bool deleteJoint(const std::string &name)
Delete an existing joint from the set of input joints.
void generateGraph()
Generate a new multibody graph from the input data.
const Joint & getJoint(int jointNum) const
Get a Joint object by its assigned number.
Definition: MultibodyGraphMaker.h:317
int getNumMobilizers() const
Returns the number of mobilizers (tree joints) in the spanning tree.
Definition: MultibodyGraphMaker.h:275
This is the top-level SimTK namespace into which all SimTK names are placed to avoid collision with o...
Definition: Assembler.h:37