Simbody
3.5
|
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