diff --git a/dart/dynamics/Group.cpp b/dart/dynamics/Group.cpp index f9c551cf2ea30..9572220a93961 100644 --- a/dart/dynamics/Group.cpp +++ b/dart/dynamics/Group.cpp @@ -45,18 +45,29 @@ namespace dynamics { //============================================================================== GroupPtr Group::create(const std::string& _name, - const std::vector& _bodyNodes) + const std::vector& _bodyNodes, + bool _includeJoints, bool _includeDofs) { - GroupPtr group(new Group(_name, _bodyNodes)); + GroupPtr group(new Group(_name, _bodyNodes, _includeJoints, _includeDofs)); group->mPtr = group; return group; } //============================================================================== GroupPtr Group::create(const std::string& _name, - const std::vector& _dofs) + const std::vector& _dofs, + bool _includeBodyNodes, bool _includeJoints) { - GroupPtr group(new Group(_name, _dofs)); + GroupPtr group(new Group(_name, _dofs, _includeBodyNodes, _includeJoints)); + group->mPtr = group; + return group; +} + +//============================================================================== +GroupPtr Group::create(const std::string& _name, + const MetaSkeletonPtr& _metaSkeleton) +{ + GroupPtr group(new Group(_name, _metaSkeleton)); group->mPtr = group; return group; } @@ -166,8 +177,109 @@ void Group::swapDofIndices(size_t _index1, size_t _index2) } //============================================================================== -void Group::addBodyNode(BodyNode* _bn, bool _warning) +bool Group::addComponent(BodyNode* _bn, bool _warning) +{ + if(nullptr == _bn) + { + if(_warning) + { + dtwarn << "[Group::addComponent] Attempting to add a nullptr component " + << "to the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return false; + } + + bool added = false; + + added |= addBodyNode(_bn, false); + + for(size_t i=0; i < _bn->getParentJoint()->getNumDofs(); ++i) + added |= addDof(_bn->getParentJoint()->getDof(i), false); + + if(_warning && !added) + { + dtwarn << "[Group::addComponent] The BodyNode named [" << _bn->getName() + << "] (" << _bn << ") and all of its parent DegreesOfFreedom are " + << "already in the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return added; +} + +//============================================================================== +bool Group::addComponents(const std::vector& _bodyNodes, + bool _warning) +{ + bool added = false; + for(BodyNode* bn : _bodyNodes) + added |= addComponent(bn, _warning); + + return added; +} + +//============================================================================== +bool Group::removeComponent(BodyNode* _bn, bool _warning) +{ + if(nullptr == _bn) + { + if(_warning) + { + dtwarn << "[Group::removeComponent] Attempting to remove a nullptr " + << "component from the Group [" << getName() << "] (" << this + << ")\n"; + assert(false); + } + + return false; + } + + bool removed = false; + + removed |= removeBodyNode(_bn, false); + + for(size_t i=0; i < _bn->getParentJoint()->getNumDofs(); ++i) + removed |= removeDof(_bn->getParentJoint()->getDof(i), false); + + if(_warning && !removed) + { + dtwarn << "[Group::removeComponent] The BodyNode named [" << _bn->getName() + << "] (" << _bn << ") and its parent DegreesOfFreedom were not in " + << "the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return removed; +} + +//============================================================================== +bool Group::removeComponents(const std::vector& _bodyNodes, + bool _warning) +{ + bool removed = false; + for(BodyNode* bn : _bodyNodes) + removed |= removeComponent(bn, _warning); + + return removed; +} + +//============================================================================== +bool Group::addBodyNode(BodyNode* _bn, bool _warning) { + if(nullptr == _bn) + { + if(_warning) + { + dtwarn << "[Group::addBodyNode] Attempting to add a nullptr BodyNode " + << "to the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return false; + } + if(INVALID_INDEX != getIndexOf(_bn, false)) { if(_warning) @@ -176,26 +288,42 @@ void Group::addBodyNode(BodyNode* _bn, bool _warning) << "] (" << _bn << ") is already in the Group [" << getName() << "] (" << this << ")\n"; assert(false); - return; } - return; + return false; } registerBodyNode(_bn); + return true; } //============================================================================== -void Group::addBodyNodes(const std::vector& _bodyNodes, +bool Group::addBodyNodes(const std::vector& _bodyNodes, bool _warning) { + bool added = false; for(BodyNode* bn : _bodyNodes) - addBodyNode(bn, _warning); + added |= addBodyNode(bn, _warning); + + return added; } //============================================================================== bool Group::removeBodyNode(BodyNode* _bn, bool _warning) { + if(nullptr == _bn) + { + if(_warning) + { + dtwarn << "[Group::removeBodyNode] Attempting to remove a nullptr " + << "BodyNode from the Group [" << getName() << "] (" << this + << ")\n"; + assert(false); + } + + return false; + } + if(INVALID_INDEX == getIndexOf(_bn, false)) { if(_warning) @@ -209,8 +337,7 @@ bool Group::removeBodyNode(BodyNode* _bn, bool _warning) return false; } - unregisterBodyNode(_bn); - + unregisterBodyNode(_bn, false); return true; } @@ -218,86 +345,321 @@ bool Group::removeBodyNode(BodyNode* _bn, bool _warning) bool Group::removeBodyNodes(const std::vector& _bodyNodes, bool _warning) { - bool allGood = true; + bool removed = false; for(BodyNode* bn : _bodyNodes) - allGood &= removeBodyNode(bn, _warning); + removed |= removeBodyNode(bn, _warning); - return allGood; + return removed; } //============================================================================== -void Group::addDof(DegreeOfFreedom* _dof, bool _warning) +bool Group::addJoint(Joint* _joint, bool _addDofs, bool _warning) { - if(INVALID_INDEX != getIndexOf(_dof, false)) + if(nullptr == _joint) { if(_warning) + { + dtwarn << "[Group::addJoint] Attempting to add a nullptr Joint to the " + << "Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return false; + } + + bool added = false; + if(INVALID_INDEX == getIndexOf(_joint, false)) + { + registerJoint(_joint); + added = true; + } + + if(_addDofs) + { + for(size_t i=0; i < _joint->getNumDofs(); ++i) + added |= addDof(_joint->getDof(i), false, false); + } + + if(!added && _warning) + { + if(_addDofs) + { + dtwarn << "[Group::addJoint] The Joint named [" << _joint->getName() + << "] (" << _joint << ") and all its DOFs are already in the " + << "Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + else + { + dtwarn << "[Group::addJoint] The Joint named [" << _joint->getName() + << "] (" << _joint << ") is already in the Group [" << getName() + << "] (" << this << ")\n"; + assert(false); + } + } + + return added; +} + +//============================================================================== +bool Group::addJoints(const std::vector& _joints, + bool _addDofs, bool _warning) +{ + bool added = false; + for(Joint* joint : _joints) + added |= addJoint(joint, _addDofs, _warning); + + return added; +} + +//============================================================================== +bool Group::removeJoint(Joint* _joint, bool _removeDofs, bool _warning) +{ + if(nullptr == _joint) + { + if(_warning) + { + dtwarn << "[Group::removeJoint] Attempting to remove a nullptr Joint " + << "from the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return false; + } + + // Make sure the Joint continues to exist for the duration of this scope + JointPtr hold(_joint); + + bool removed = false; + if(INVALID_INDEX != getIndexOf(_joint, false)) + { + unregisterJoint(_joint->getChildBodyNode()); + removed = true; + } + + if(_removeDofs) + { + for(size_t i=0; i < _joint->getNumDofs(); ++i) + removed |= removeDof(_joint->getDof(i), false, false); + } + + if(!removed && _warning) + { + if(_removeDofs) + { + dtwarn << "[Group::removeJoint] The Joint named [" << _joint->getName() + << "] (" << _joint << ") and its DOFs were NOT in the Group [" + << getName() << "] (" << this << ")\n"; + assert(false); + } + else + { + dtwarn << "[Group::removeJoint] The Joint named [" << _joint->getName() + << "] (" << _joint << ") was NOT in the Group [" << getName() + << "] (" << this << ")\n"; + assert(false); + } + } + + return removed; +} + +//============================================================================== +bool Group::addDof(DegreeOfFreedom* _dof, bool _addJoint, bool _warning) +{ + if(nullptr == _dof) + { + if(_warning) + { + dtwarn << "[Group::addDof] Attempting to add a nullptr DegreeOfFreedom " + << "to the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + + return false; + } + + bool added = false; + if(INVALID_INDEX == getIndexOf(_dof, false)) + { + registerDegreeOfFreedom(_dof); + added = true; + } + + if(_addJoint) + added |= addJoint(_dof->getJoint(), false, false); + + if(!added && _warning) + { + if(_addJoint) + { + dtwarn << "[Group::addDof] The DegreeOfFreedom named [" << _dof->getName() + << "] (" << _dof << ") and its Joint are already in the Group [" + << getName() << "] (" << this << ")\n"; + assert(false); + } + else { dtwarn << "[Group::addDof] The DegreeOfFreedom named [" << _dof->getName() << "] (" << _dof << ") is already in the Group [" << getName() << "] (" << this << ")\n"; assert(false); - return; } - - return; } - registerDegreeOfFreedom(_dof); + return added; } //============================================================================== -void Group::addDofs(const std::vector& _dofs, bool _warning) +bool Group::addDofs(const std::vector& _dofs, + bool _addJoint, bool _warning) { + bool added = false; for(DegreeOfFreedom* dof : _dofs) - addDof(dof, _warning); + added |= addDof(dof, _addJoint, _warning); + + return added; } //============================================================================== -bool Group::removeDof(DegreeOfFreedom* _dof, bool _warning) +bool Group::removeDof(DegreeOfFreedom* _dof, bool _cleanupJoint, bool _warning) { - if(INVALID_INDEX == getIndexOf(_dof, false)) + if(nullptr == _dof) { if(_warning) { - dtwarn << "[Group::removeDof] The DegreeOfFreedom named [" - << _dof->getName() << "] (" << _dof << ") is NOT in the Group [" - << getName() << "] (" << this << ")\n"; + dtwarn << "[Group::removeDof] Attempting to remove a nullptr " + << "DegreeOfFreedom from the Group [" << getName() << "] (" + << this << ")\n"; assert(false); } return false; } - unregisterDegreeOfFreedom(_dof->getChildBodyNode(), _dof->getIndexInJoint()); + // Make sure the DegreeOfFreedom continues to exist for the duration of this + // scope + DegreeOfFreedomPtr hold(_dof); - return true; + bool removed = false; + if(INVALID_INDEX != getIndexOf(_dof, false)) + { + unregisterDegreeOfFreedom(_dof->getChildBodyNode(), _dof->getIndexInJoint()); + removed = true; + } + + if(_cleanupJoint) + { + // Check whether any DOFs belonging to the Joint are remaining in the Group + bool performCleanup = true; + Joint* joint = _dof->getJoint(); + for(size_t i=0; i < joint->getNumDofs(); ++i) + { + if(getIndexOf(joint->getDof(i), false) == INVALID_INDEX) + { + performCleanup = false; + break; + } + } + + // Remove the Joint if none of its DOFs remain + if(performCleanup) + removed |= removeJoint(joint, false, false); + } + + if(!removed && _warning) + { + if(_cleanupJoint) + { + dtwarn << "[Group::removeDof] The DegreeOfFreedom named [" + << _dof->getName() << "] (" << _dof << ") and its Joint were NOT " + << "in the Group [" << getName() << "] (" << this << ")\n"; + assert(false); + } + else + { + dtwarn << "[Group::removeDof] The DegreeOfFreedom named [" + << _dof->getName() << "] (" << _dof << ") was NOT in the Group [" + << getName() << "] (" << this << ")\n"; + assert(false); + } + } + + return removed; } //============================================================================== bool Group::removeDofs(const std::vector& _dofs, - bool _warning) + bool _cleanupJoint, bool _warning) { - bool allGood = true; + bool removed = false; for(DegreeOfFreedom* dof : _dofs) - allGood &= removeDof(dof, _warning); + removed |= removeDof(dof, _cleanupJoint, _warning); - return allGood; + return removed; } //============================================================================== Group::Group(const std::string& _name, - const std::vector& _bodyNodes) + const std::vector& _bodyNodes, + bool _includeJoints, bool _includeDofs) { setName(_name); addBodyNodes(_bodyNodes); + + if(_includeDofs || _includeJoints) + { + for(size_t i=0; i < _bodyNodes.size(); ++i) + { + Joint* joint = _bodyNodes[i]->getParentJoint(); + + if(_includeJoints) + addJoint(joint, false); + + if(_includeDofs) + { + for(size_t j=0; j < joint->getNumDofs(); ++j) + addDof(joint->getDof(j)); + } + } + } } //============================================================================== Group::Group(const std::string& _name, - const std::vector& _dofs) + const std::vector& _dofs, + bool _includeBodyNodes, bool _includeJoints) { setName(_name); - addDofs(_dofs); + addDofs(_dofs, _includeJoints); + + if(_includeBodyNodes) + { + for(size_t i=0; i < _dofs.size(); ++i) + { + DegreeOfFreedom* dof = _dofs[i]; + addBodyNode(dof->getChildBodyNode(), false); + } + } +} + +//============================================================================== +Group::Group(const std::string& _name, + const MetaSkeletonPtr& _metaSkeleton) +{ + setName(_name); + + if(_metaSkeleton) + { + for(size_t i=0; i < _metaSkeleton->getNumBodyNodes(); ++i) + addBodyNode(_metaSkeleton->getBodyNode(i)); + + for(size_t i=0; i < _metaSkeleton->getNumJoints(); ++i) + addJoint(_metaSkeleton->getJoint(i), false); + + for(size_t i=0; i < _metaSkeleton->getNumDofs(); ++i) + addDof(_metaSkeleton->getDof(i), false); + } } } // namespace dynamics diff --git a/dart/dynamics/Group.h b/dart/dynamics/Group.h index ec26e9fabdabb..a2f90da5283c6 100644 --- a/dart/dynamics/Group.h +++ b/dart/dynamics/Group.h @@ -46,15 +46,30 @@ class Group : public ReferentialSkeleton { public: - /// Create a Group out of a set of BodyNodes + /// Create a Group out of a set of BodyNodes. If _includeJoints is true, the + /// parent Joint of each BodyNode will also be added to the Group. If + /// _includeDofs is true, then the parent DegreesOfFreedom of each BodyNode + /// will also be added to the Group. static GroupPtr create( const std::string& _name = "Group", - const std::vector& _bodyNodes = std::vector()); + const std::vector& _bodyNodes = std::vector(), + bool _includeJoints = true, + bool _includeDofs = true); + + /// Create a Group out of a set of DegreesOfFreedom. If _includeBodyNodes is + /// true, then the child BodyNode of each DegreeOfFreedom will also be added + /// to the Group. If _includeJoints is true, then the Joint of each + /// DegreeOfFreedom will also be added to the Group. + static GroupPtr create( + const std::string& _name, + const std::vector& _dofs, + bool _includeBodyNodes = true, + bool _includeJoints = true); - /// Create a Group out of a set of DegreesOfFreedom + /// Create a Group that mimics the given MetaSkeleton static GroupPtr create( const std::string& _name, - const std::vector& _dofs); + const MetaSkeletonPtr& _metaSkeleton); /// Destructor virtual ~Group() = default; @@ -65,72 +80,169 @@ class Group : public ReferentialSkeleton /// Swap the index of DegreeOfFreedom _index1 with _index2 void swapDofIndices(size_t _index1, size_t _index2); + /// Add a BodyNode and its parent DegreesOfFreedom to this Group. If _warning + /// is true, you will be warned when the BodyNode and all its DegreesOfFreedom + /// were already in the Group, and an assertion will be thrown. + /// + /// This function will return false if the BodyNode and all its + /// DegreesOfFreedom were already in the Group. + bool addComponent(BodyNode* _bn, bool _warning=true); + + /// Add set of BodyNodes and their parent DegreesOfFreedom to this Group. If + /// _warning is true, you will be warned when an entire component was already + /// in the Group, and an assertion will be thrown. + /// + /// This function will return false if all of the components in the set were + /// already in this Group. + bool addComponents(const std::vector& _bodyNodes, + bool _warning=true); + + /// Remove a BodyNode and its parent DegreesOfFreedom from this Group. If + /// _warning is true, you will be warned if this Group does not have the + /// BodyNode or any of its DegreesOfFreedom, and an assertion will be thrown. + /// + /// This function will return false if the Group did not include the BodyNode + /// or any of its parent DegreesOfFreedom. + bool removeComponent(BodyNode* _bn, bool _warning=true); + + /// Remove a set of BodyNodes and their parent DegreesOfFreedom from this + /// Group. If _warning is true, you will be warned if any of the components + /// were completely missing from this Group, and an assertion will be thrown. + /// + /// This function will return false if none of the components in this set were + /// in the Group. + bool removeComponents(const std::vector& _bodyNodes, + bool _warning=true); + /// Add a BodyNode to this Group. If _warning is true, you will be warned when - /// you attempt to add the same BodyNode twice, and assertion will be thrown. - void addBodyNode(BodyNode* _bn, bool _warning=true); + /// you attempt to add the same BodyNode twice, and an assertion will be + /// thrown. + /// + /// This function will return false if the BodyNode was already in the Group. + bool addBodyNode(BodyNode* _bn, bool _warning=true); /// Add a set of BodyNodes to this Group. If _warning is true, you will be - /// warned when you attempt to add the same BodyNode twice, and an assertion - /// will be thrown. - void addBodyNodes(const std::vector& _bodyNodes, + /// warned when you attempt to add a BodyNode that is already in the Group, + /// and an assertion will be thrown. + /// + /// This function will return false if all of the BodyNodes were already in + /// the Group. + bool addBodyNodes(const std::vector& _bodyNodes, bool _warning=true); - /// Remove a BodyNode from this Group. Note: All DegreesOfFreedom belonging to - /// this BodyNode will also be removed. If _warning is true, you will be - /// warned when you attempt to remove BodyNode that is not in this Group, and - /// an assertion will be thrown. + /// Remove a BodyNode from this Group. If _warning is true, you will be warned + /// when you attempt to remove a BodyNode that is not in this Group, and an + /// assertion will be thrown. /// - /// The function will return false if the BodyNode was not already in this - /// Group. + /// The function will return false if the BodyNode was not in this Group. bool removeBodyNode(BodyNode* _bn, bool _warning=true); - /// Remove a set of BodyNodes from this Group. Note: All DegreesOfFreedom - /// belonging to each BodyNode will also be removed. If _warning is true, you - /// will be warned when you attempt to remove a BodyNode that is not in this - /// Group, and an assertion will be thrown. + /// Remove a set of BodyNodes from this Group. If _warning is true, you will + /// be warned when you attempt to remove a BodyNode that is not in this Group, + /// and an assertion will be thrown. /// - /// The function will return false if one of the BodyNodes was not already in - /// this Group. + /// The function will return false if none of the BodyNodes were in this Group. bool removeBodyNodes(const std::vector& _bodyNodes, bool _warning=true); - /// Add a DegreeOfFreedom to this Group. Note: The BodyNode of this - /// DegreeOfFreedom will also be added. If _warning is true, you will be - /// warned when you attempt to add the same DegreeOfFreedom twice, and an + /// Add a Joint to this Group. If _addDofs is true, it will also add all the + /// DegreesOfFreedom of the Joint. If _warning is true, you will be warned + /// if the Joint (and all its DOFs if _addDofs is set to true) was already in + /// the Group, and an assertion will be thrown. + /// + /// This function will return false if the Joint (and all its DOFs, if + /// _addDofs is true) was already in the Group. + bool addJoint(Joint* _joint, bool _addDofs=true, bool _warning=true); + + /// Add a set of Joints to this Group. If _addDofs is true, it will also add + /// all the DOFs of each Joint. If _warning is true, you will be warned when + /// you attempt to add a Joint that is already in the Group (and all its DOFs + /// are in the Group, if _addDofs is set to true), and an assertion will be + /// thrown. + /// + /// This function will return false if all the Joints (and their DOFs if + /// _addDofs is set to true) were already in the Group. + bool addJoints(const std::vector& _joints, bool _addDofs=true, + bool _warning=true); + + /// Remove a Joint from this Group. If _removeDofs is true, it will also + /// remove any DOFs belonging to this Joint. If _warning is true, you will + /// be warned if you attempt to remove a Joint which is not in this Group (and + /// none of its DOFs are in the Group if _removeDofs is set to true), and an /// assertion will be thrown. - void addDof(DegreeOfFreedom* _dof, bool _warning=true); + /// + /// This function will return false if the Joint was not in the Group (and + /// neither were any of its DOFs, if _removeDofs is set to true). + bool removeJoint(Joint* _joint, bool _removeDofs=true, bool _warning=true); + + /// Remove a set of Joints from this Group. If _removeDofs is true, it will + /// also remove any DOFs belonging to any of the Joints. If _warning is true, + /// you will be warned if you attempt to remove a Joint which is not in this + /// Group (and none of its DOFs are in the Group if _removeDofs is set to + /// true), and an assertion will be thrown. + /// + /// This function will return false if none of the Joints (nor any of their + /// DOFs if _removeDofs is set to true) were in the Group. + bool removeJoints(const std::vector& _joints, bool _removeDofs=true, + bool _warning=true); - /// Add a set of DegreesOfFreedom to this Group. Note: The BodyNodes of these - /// DegreesOfFreedom will also be added. If _warning is true, you will be + /// Add a DegreeOfFreedom to this Group. If _addJoint is true, the Joint of + /// this DOF will also be added to the Group. If _warning is true, you will be /// warned when you attempt to add the same DegreeOfFreedom twice, and an /// assertion will be thrown. - void addDofs(const std::vector& _dofs, bool _warning=true); + /// + /// This function will return false if the DegreeOfFreedom was already in the + /// Group. + bool addDof(DegreeOfFreedom* _dof, bool _addJoint=true, bool _warning=true); - /// Remove a DegreeOfFreedom from this Group. If _warning is true, you will be + /// Add a set of DegreesOfFreedom to this Group. If _addJoint is true, the + /// Joint of each DOF will also be added to the Group. If _warning is true, + /// you will be warned when you attempt to add the same DegreeOfFreedom twice, + /// and an assertion will be thrown. + /// + /// This function will return false if all of the DegreesOfFreedom was already + /// in the Group. + bool addDofs(const std::vector& _dofs, + bool _addJoint = true, bool _warning=true); + + /// Remove a DegreeOfFreedom from this Group. If _cleanupJoint is true, the + /// Joint of this DOF will be removed, but only if no other DOFs belonging to + /// the Joint are remaining in the Group. If _warning is true, you will be /// warned when you attempt to remove a DegreeOfFreedom that is not in this /// Group, and an assertion will be thrown. /// - /// This function will return false if the DegreeOfFreedom was not already in - /// this Group. - bool removeDof(DegreeOfFreedom* _dof, bool _warning=true); - - /// Remove a set of DegreesOfFreedom from this Group. If _warning is true, you - /// will be warned when you attempt to remove a DegreeOfFreedom that is not - /// in this Group, and an assertion will be thrown. + /// This function will return false if the DegreeOfFreedom was not in this + /// Group. + bool removeDof(DegreeOfFreedom* _dof, bool _cleanupJoint=true, + bool _warning=true); + + /// Remove a set of DegreesOfFreedom from this Group. If _cleanupJoint is + /// true, the Joint of this DOF will be removed, but only if no other DOFs + /// belonging to the Joint are remaining in the Group. If _warning is true, + /// you will be warned when you attempt to remove a DegreeOfFreedom that is + /// not in this Group, and an assertion will be thrown. /// - /// This function will return false if the DegreeOfFreedom was not alraedy in + /// This function will return false if none of the DegreesOfFreedom were in /// this Group. bool removeDofs(const std::vector& _dofs, - bool _warning=true); + bool _cleanupJoint=true, bool _warning=true); protected: /// Default constructor - Group(const std::string& _name = "Group", - const std::vector& _bodyNodes = std::vector()); + Group(const std::string& _name, + const std::vector& _bodyNodes, + bool _includeJoints, + bool _includeDofs); /// Alternative constructor Group(const std::string& _name, - const std::vector& _dofs); + const std::vector& _dofs, + bool _includeBodyNodes, + bool _includeJoints); + + /// MetaSkeleton-based constructor + Group(const std::string& _name, + const MetaSkeletonPtr& _metaSkeleton); }; } // dynamics diff --git a/dart/dynamics/Linkage.cpp b/dart/dynamics/Linkage.cpp index 8da04a5797cf7..18b2f83781a54 100644 --- a/dart/dynamics/Linkage.cpp +++ b/dart/dynamics/Linkage.cpp @@ -517,11 +517,11 @@ void Linkage::satisfyCriteria() { std::vector bns = mCriteria.satisfy(); while(getNumBodyNodes() > 0) - unregisterBodyNode(mBodyNodes.back()); + unregisterComponent(mBodyNodes.back()); for(BodyNode* bn : bns) { - registerBodyNode(bn); + registerComponent(bn); } update(); diff --git a/dart/dynamics/ReferentialSkeleton.cpp b/dart/dynamics/ReferentialSkeleton.cpp index b5f8666631195..476ba48edc99e 100644 --- a/dart/dynamics/ReferentialSkeleton.cpp +++ b/dart/dynamics/ReferentialSkeleton.cpp @@ -151,27 +151,19 @@ size_t ReferentialSkeleton::getIndexOf(const BodyNode* _bn, bool _warning) const //============================================================================== size_t ReferentialSkeleton::getNumJoints() const { - return mBodyNodes.size(); + return mJoints.size(); } //============================================================================== Joint* ReferentialSkeleton::getJoint(size_t _idx) { - BodyNode* bn = getVectorObjectIfAvailable(_idx, mBodyNodes); - if(nullptr == bn) - return nullptr; - - return bn->getParentJoint(); + return getVectorObjectIfAvailable(_idx, mJoints); } //============================================================================== const Joint* ReferentialSkeleton::getJoint(size_t _idx) const { - const BodyNode* bn = getVectorObjectIfAvailable(_idx, mBodyNodes); - if(nullptr == bn) - return nullptr; - - return bn->getParentJoint(); + return getVectorObjectIfAvailable(_idx, mJoints); } //============================================================================== @@ -202,7 +194,7 @@ size_t ReferentialSkeleton::getIndexOf(const Joint* _joint, bool _warning) const return INVALID_INDEX; } - return it->second.mBodyNodeIndex; + return it->second.mJointIndex; } //============================================================================== @@ -955,13 +947,81 @@ math::LinearJacobian ReferentialSkeleton::getCOMLinearJacobianDeriv( } //============================================================================== -void ReferentialSkeleton::registerBodyNode(BodyNode* _bn) +void ReferentialSkeleton::registerComponent(BodyNode* _bn) { + registerBodyNode(_bn); + registerJoint(_bn->getParentJoint()); + size_t nDofs = _bn->getParentJoint()->getNumDofs(); for(size_t i=0; i < nDofs; ++i) - { registerDegreeOfFreedom(_bn->getParentJoint()->getDof(i)); +} + +//============================================================================== +void ReferentialSkeleton::registerBodyNode(BodyNode* _bn) +{ + std::unordered_map::iterator it = + mIndexMap.find(_bn); + + if( it == mIndexMap.end() ) + { + // Create an index map entry for this BodyNode, and only add the BodyNode's + // index to it. + IndexMap indexing; + + mBodyNodes.push_back(_bn); + indexing.mBodyNodeIndex = mBodyNodes.size()-1; + + mIndexMap[_bn] = indexing; } + else + { + IndexMap& indexing = it->second; + + if(INVALID_INDEX == indexing.mBodyNodeIndex) + { + mBodyNodes.push_back(_bn); + indexing.mBodyNodeIndex = mBodyNodes.size()-1; + } + } + + updateCaches(); +} + +//============================================================================== +void ReferentialSkeleton::registerJoint(Joint* _joint) +{ + BodyNode* bn = _joint->getChildBodyNode(); + + std::unordered_map::iterator it = + mIndexMap.find(bn); + + if( it == mIndexMap.end() ) + { + // Create an index map entry for this Joint, and only add the Joint's index + // to it + IndexMap indexing; + + mJoints.push_back(_joint); + indexing.mJointIndex = mJoints.size()-1; + + mIndexMap[bn] = indexing; + } + else + { + IndexMap& indexing = it->second; + + if(INVALID_INDEX == indexing.mJointIndex) + { + mJoints.push_back(_joint); + indexing.mJointIndex = mJoints.size()-1; + } + } + + // Updating the caches isn't necessary after registering a joint right now, + // but it might matter in the future, so it might be better to be safe than + // sorry. + updateCaches(); } //============================================================================== @@ -975,9 +1035,9 @@ void ReferentialSkeleton::registerDegreeOfFreedom(DegreeOfFreedom* _dof) if( it == mIndexMap.end() ) { + // Create an index map entry for this DegreeOfFreedom, and only add the + // DegreeOfFreedom's index to it IndexMap indexing; - mBodyNodes.push_back(bn); - indexing.mBodyNodeIndex = mBodyNodes.size()-1; indexing.mDofIndices.resize(localIndex+1, INVALID_INDEX); mDofs.push_back(_dof); @@ -988,17 +1048,30 @@ void ReferentialSkeleton::registerDegreeOfFreedom(DegreeOfFreedom* _dof) else { IndexMap& indexing = it->second; + if(indexing.mDofIndices.size() < localIndex+1) indexing.mDofIndices.resize(localIndex+1, INVALID_INDEX); - mDofs.push_back(_dof); - indexing.mDofIndices[localIndex] = mDofs.size()-1; + + if(INVALID_INDEX == indexing.mDofIndices[localIndex]) + { + mDofs.push_back(_dof); + indexing.mDofIndices[localIndex] = mDofs.size()-1; + } } updateCaches(); } //============================================================================== -void ReferentialSkeleton::unregisterBodyNode(BodyNode* _bn) +void ReferentialSkeleton::unregisterComponent(BodyNode* _bn) +{ + unregisterBodyNode(_bn, true); + unregisterJoint(_bn); +} + +//============================================================================== +void ReferentialSkeleton::unregisterBodyNode( + BodyNode* _bn, bool _unregisterDofs) { if(nullptr == _bn) { @@ -1022,27 +1095,88 @@ void ReferentialSkeleton::unregisterBodyNode(BodyNode* _bn) return; } - const IndexMap& indexing = it->second; + IndexMap& indexing = it->second; + size_t bnIndex = indexing.mBodyNodeIndex; + mBodyNodes.erase(mBodyNodes.begin() + bnIndex); + indexing.mBodyNodeIndex = INVALID_INDEX; - for(size_t i=0; igetParentJoint(); + + std::unordered_map::iterator it = + mIndexMap.find(_child); + + if( it == mIndexMap.end() || INVALID_INDEX == it->second.mJointIndex) + { + dterr << "[ReferentialSkeleton::unregisterJoint] Attempting to unregister " + << "a Joint named [" << joint->getName() << "] (" << joint << "), " + << "which is the parent Joint of BodyNode [" << _child->getName() + << "] (" << _child << "), but the Joint is not currently in this " + << "ReferentialSkeleton! This is most likely a bug. Please report " + << "this!\n"; + assert(false); + return; + } + + size_t jointIndex = it->second.mJointIndex; + mJoints.erase(mJoints.begin() + jointIndex); + it->second.mJointIndex = INVALID_INDEX; + + for(size_t i = jointIndex; i < mJoints.size(); ++i) + { + // Re-index all of the Joints in this ReferentialSkeleton which came after + // the Joint that was removed. + JointPtr alteredJoint = mJoints[i]; + IndexMap& indexing = mIndexMap[alteredJoint.getBodyNodePtr()]; + indexing.mJointIndex = i; + } + + if(it->second.isExpired()) + mIndexMap.erase(it); + + // Updating the caches isn't necessary after unregistering a joint right now, + // but it might matter in the future, so it might be better to be safe than + // sorry. + updateCaches(); } //============================================================================== void ReferentialSkeleton::unregisterDegreeOfFreedom( - BodyNode* _bn, size_t _localIndex, bool removeBnIfEmpty) + BodyNode* _bn, size_t _localIndex) { if(nullptr == _bn) { @@ -1061,10 +1195,10 @@ void ReferentialSkeleton::unregisterDegreeOfFreedom( it->second.mDofIndices[_localIndex] == INVALID_INDEX) { dterr << "[ReferentialSkeleton::unregisterDegreeOfFreedom] Attempting to " - << "unregister a DegreeOfFreedom from a BodyNode named [" - << _bn->getName() << "] (" << _bn << ") that is not currently in the " - << "ReferentialSkeleton! This is most likely a bug. Please report " - << "this!\n"; + << "unregister DegreeOfFreedom #" << _localIndex << " of a BodyNode " + << "named [" << _bn->getName() << "] (" << _bn << "), but it is not " + << "currently in the ReferentialSkeleton! This is most likely a bug. " + << "Please report this!\n"; assert(false); return; } @@ -1075,27 +1209,15 @@ void ReferentialSkeleton::unregisterDegreeOfFreedom( for(size_t i = dofIndex; i < mDofs.size(); ++i) { + // Re-index all the DOFs in this ReferentialSkeleton which came after the + // DOF that was removed. DegreeOfFreedomPtr dof = mDofs[i]; IndexMap& indexing = mIndexMap[dof.getBodyNodePtr()]; indexing.mDofIndices[dof.getLocalIndex()] = i; } - if(removeBnIfEmpty) - { - const std::vector& dofIndices = it->second.mDofIndices; - bool removeBn = true; - for(size_t i=0; isecond.isExpired()) + mIndexMap.erase(it); updateCaches(); } @@ -1142,5 +1264,31 @@ void ReferentialSkeleton::updateCaches() mFc = Eigen::VectorXd::Zero(nDofs); } +//============================================================================== +ReferentialSkeleton::IndexMap::IndexMap() + : mBodyNodeIndex(INVALID_INDEX), + mJointIndex(INVALID_INDEX) +{ + // Do nothing +} + +//============================================================================== +bool ReferentialSkeleton::IndexMap::isExpired() const +{ + if(INVALID_INDEX != mBodyNodeIndex) + return false; + + if(INVALID_INDEX != mJointIndex) + return false; + + for(size_t i=0; i < mDofIndices.size(); ++i) + { + if(mDofIndices[i] != INVALID_INDEX) + return false; + } + + return true; +} + } // namespace dynamics } // namespace dart diff --git a/dart/dynamics/ReferentialSkeleton.h b/dart/dynamics/ReferentialSkeleton.h index 1d06219474d9c..f7baeb0f8231f 100644 --- a/dart/dynamics/ReferentialSkeleton.h +++ b/dart/dynamics/ReferentialSkeleton.h @@ -327,21 +327,37 @@ class ReferentialSkeleton : public MetaSkeleton /// of ReferentialSkeleton. ReferentialSkeleton() = default; - /// Add a BodyNode to this ReferentialSkeleton. Only usable by derived classes + /// Add a BodyNode, along with its parent Joint and parent DegreesOfFreedom + /// to this ReferentialSkeleton. This can only be used by derived classes. + void registerComponent(BodyNode* _bn); + + /// Add a BodyNode to this ReferentialSkeleton, ignoring its Joint and + /// DegreesOfFreedom. This can only be used by derived classes. void registerBodyNode(BodyNode* _bn); - /// Add a DegreeOfFreedom to this ReferentialSkeleton. Only usable by - /// derived classes. + /// Add a Joint to this Referential Skeleton, ignoring its DegreesOfFreedom. + /// This can only be used by derived classes. + void registerJoint(Joint* _joint); + + /// Add a DegreeOfFreedom to this ReferentialSkeleton. This can only be used + /// by derived classes. void registerDegreeOfFreedom(DegreeOfFreedom* _dof); - /// Completely remove a BodyNode from this ReferentialSkeleton. Only usable - /// by derived classes - void unregisterBodyNode(BodyNode* _bn); + /// Completely remove a BodyNode and its parent DegreesOfFreedom from this + /// ReferentialSkeleton. This can only be used by derived classes. + void unregisterComponent(BodyNode* _bn); - /// Remove a DegreeOfFreedom from this ReferentialSkeleton. Only usable by + /// Remove a BodyNode from this ReferentialSkeleton, ignoring its parent + /// DegreesOfFreedom. This can only be used by derived classes. + void unregisterBodyNode(BodyNode* _bn, bool _unregisterDofs); + + /// Remove a Joint from this ReferentialSkeleton. This can only be used by /// derived classes. - void unregisterDegreeOfFreedom(BodyNode* _bn, size_t _localIndex, - bool removeBnIfEmpty = true); + void unregisterJoint(BodyNode* _child); + + /// Remove a DegreeOfFreedom from this ReferentialSkeleton. This can only be + /// used by derived classes. + void unregisterDegreeOfFreedom(BodyNode* _bn, size_t _localIndex); /// Update the caches to match any changes to the structure of this /// ReferentialSkeleton @@ -357,8 +373,19 @@ class ReferentialSkeleton : public MetaSkeleton /// Index of the BodyNode size_t mBodyNodeIndex; - /// Indices of the DegreesOfFreedom + /// Index of the parent Joint + size_t mJointIndex; + + /// Indices of the parent DegreesOfFreedom std::vector mDofIndices; + + /// Default constructor. Initializes mBodyNodeIndex and mJointIndex to + /// INVALID_INDEX + IndexMap(); + + /// Returns true if nothing in this entry is mapping to a valid index any + /// longer. + bool isExpired() const; }; /// Name of this ReferentialSkeleton @@ -374,6 +401,9 @@ class ReferentialSkeleton : public MetaSkeleton /// Raw const BodyNode pointers. This vector is used for the MetaSkeleton API mutable std::vector mRawConstBodyNodes; + /// Joints that this ReferentialSkeleton references + std::vector mJoints; + /// DegreesOfFreedom that this ReferentialSkeleton references std::vector mDofs; diff --git a/dart/dynamics/detail/JointPtr.h b/dart/dynamics/detail/JointPtr.h index dc63bc1e5286f..d9a71956f1098 100644 --- a/dart/dynamics/detail/JointPtr.h +++ b/dart/dynamics/detail/JointPtr.h @@ -106,6 +106,12 @@ class TemplateJointPtr return mBodyNodePtr->getParentJoint(); } + /// Get the BodyNode that this JointPtr is tied to + TemplateBodyNodePtr getBodyNodePtr() const + { + return mBodyNodePtr; + } + /// Set the Joint for this JointPtr void set(JointT* _ptr) { diff --git a/unittests/testSkeleton.cpp b/unittests/testSkeleton.cpp index 29cff58d6235b..c3c0c9cc13f25 100644 --- a/unittests/testSkeleton.cpp +++ b/unittests/testSkeleton.cpp @@ -623,6 +623,23 @@ TEST(Skeleton, NodePersistence) } } +TEST(Skeleton, ZeroDofJointInReferential) +{ + // This is a regression test which makes sure that the BodyNodes of + // ZeroDofJoints will be correctly included in linkages. + SkeletonPtr skel = Skeleton::create(); + + BodyNode* bn = skel->createJointAndBodyNodePair().second; + BodyNode* zeroDof1 = skel->createJointAndBodyNodePair(bn).second; + bn = skel->createJointAndBodyNodePair(zeroDof1).second; + BodyNode* zeroDof2 = skel->createJointAndBodyNodePair(bn).second; + + BranchPtr branch = Branch::create(skel->getBodyNode(0)); + EXPECT_EQ(branch->getNumBodyNodes(), skel->getNumBodyNodes()); + EXPECT_FALSE(branch->getIndexOf(zeroDof1) == INVALID_INDEX); + EXPECT_FALSE(branch->getIndexOf(zeroDof2) == INVALID_INDEX); +} + TEST(Skeleton, Referential) { std::vector skeletons = getSkeletons(); @@ -837,6 +854,19 @@ size_t checkForBodyNodes( return count; } +void checkLinkageJointConsistency(const ReferentialSkeletonPtr& refSkel) +{ + EXPECT_TRUE(refSkel->getNumBodyNodes() == refSkel->getNumJoints()); + + // Linkages should have the property: + // getJoint(i) == getBodyNode(i)->getParentJoint() + for(size_t i=0; i < refSkel->getNumJoints(); ++i) + { + EXPECT_EQ(refSkel->getJoint(i), refSkel->getBodyNode(i)->getParentJoint()); + EXPECT_EQ(refSkel->getIndexOf(refSkel->getJoint(i)), i); + } +} + TEST(Skeleton, Linkage) { // Test a variety of uses of Linkage::Criteria @@ -849,6 +879,7 @@ TEST(Skeleton, Linkage) ChainPtr midchain = Chain::create(skel->getBodyNode("c1b3"), skel->getBodyNode("c3b4"), "midchain"); checkForBodyNodes(midchain, skel, true, "c3b1", "c3b2", "c3b3"); + checkLinkageJointConsistency(midchain); Linkage::Criteria criteria; criteria.mStart = skel->getBodyNode("c5b2"); @@ -857,6 +888,7 @@ TEST(Skeleton, Linkage) LinkagePtr path = Linkage::create(criteria, "path"); checkForBodyNodes(path, skel, true, "c5b2", "c5b1", "c1b3", "c3b1", "c3b2", "c3b3", "c4b1", "c4b2", "c4b3"); + checkLinkageJointConsistency(path); skel->getBodyNode(0)->copyTo(nullptr); criteria.mTargets.clear(); @@ -872,6 +904,7 @@ TEST(Skeleton, Linkage) "c3b1(1)", "c1b3(1)", "c2b1(1)", "c2b2(1)", "c2b3(1)", "c5b1", "c5b2", "c1b2", "c1b1", "c5b1(1)", "c5b2(1)", "c1b2(1)", "c1b1(1)"); + checkLinkageJointConsistency(combinedTreeBases); SkeletonPtr skel2 = skel->getBodyNode(0)->copyAs("skel2"); criteria.mTargets.clear(); @@ -891,23 +924,28 @@ TEST(Skeleton, Linkage) ChainPtr downstreamFreeJoint = Chain::create(skel->getBodyNode("c1b1"), skel->getBodyNode("c1b3"), Chain::IncludeBoth, "downstreamFreeJoint"); checkForBodyNodes(downstreamFreeJoint, skel, true, "c1b1"); + checkLinkageJointConsistency(downstreamFreeJoint); ChainPtr emptyChain = Chain::create(skel->getBodyNode("c1b1"), skel->getBodyNode("c1b3"), "emptyChain"); checkForBodyNodes(emptyChain, skel, true); + checkLinkageJointConsistency(emptyChain); ChainPtr chainFromNull = Chain::create(nullptr, skel->getBodyNode("c1b2"), "chainFromNull"); checkForBodyNodes(chainFromNull, skel, true, "c1b1"); + checkLinkageJointConsistency(chainFromNull); ChainPtr upstreamFreeJoint = Chain::create(skel->getBodyNode("c1b3"), skel->getBodyNode("c1b1"), "upstreamFreeJoint"); checkForBodyNodes(upstreamFreeJoint, skel, true, "c1b3", "c1b2"); + checkLinkageJointConsistency(upstreamFreeJoint); // Using nullptr as the target should bring us towards the root of the tree ChainPtr upTowardsRoot = Chain::create(skel->getBodyNode("c1b3"), nullptr, "upTowardsRoot"); checkForBodyNodes(upTowardsRoot, skel, true, "c1b3", "c1b2"); + checkLinkageJointConsistency(upTowardsRoot); criteria.mTargets.clear(); criteria.mTargets.push_back(skel->getBodyNode("c4b3")); @@ -916,6 +954,7 @@ TEST(Skeleton, Linkage) LinkagePtr terminatedLinkage = Linkage::create(criteria, "terminatedLinkage"); checkForBodyNodes(terminatedLinkage, skel, true, "c1b3", "c3b1", "c3b2"); + checkLinkageJointConsistency(terminatedLinkage); criteria.mStart = skel->getBodyNode("c1b1"); criteria.mStart.mPolicy = Linkage::Criteria::DOWNSTREAM; @@ -928,12 +967,81 @@ TEST(Skeleton, Linkage) checkForBodyNodes(terminatedSubtree, skel, true, "c1b1", "c1b2", "c1b3", "c5b1", "c5b2", "c3b1", "c3b2", "c3b3"); + checkLinkageJointConsistency(terminatedSubtree); criteria.mStart.mPolicy = Linkage::Criteria::UPSTREAM; criteria.mStart.mNode = skel->getBodyNode("c3b1"); LinkagePtr terminatedUpstream = Linkage::create(criteria, "terminatedUpstream"); checkForBodyNodes(terminatedUpstream, skel, true, "c3b1", "c1b3", "c5b1", "c5b2", "c1b2", "c1b1"); + checkLinkageJointConsistency(terminatedUpstream); +} + +TEST(Skeleton, Group) +{ + SkeletonPtr skel = constructLinkageTestSkeleton(); + + // Make twice as many BodyNodes in the Skeleton + SkeletonPtr skel2 = constructLinkageTestSkeleton(); + skel2->getRootBodyNode()->moveTo(skel, nullptr); + + // Test nullptr construction + GroupPtr nullGroup = Group::create("null_group", nullptr); + EXPECT_EQ(nullGroup->getNumBodyNodes(), 0u); + EXPECT_EQ(nullGroup->getNumJoints(), 0u); + EXPECT_EQ(nullGroup->getNumDofs(), 0u); + + // Test conversion from Skeleton + GroupPtr skel1Group = Group::create("skel1_group", skel); + EXPECT_EQ(skel1Group->getNumBodyNodes(), skel->getNumBodyNodes()); + EXPECT_EQ(skel1Group->getNumJoints(), skel->getNumJoints()); + EXPECT_EQ(skel1Group->getNumDofs(), skel->getNumDofs()); + + for(size_t i=0; i < skel1Group->getNumBodyNodes(); ++i) + EXPECT_EQ(skel1Group->getBodyNode(i), skel->getBodyNode(i)); + + for(size_t i=0; i < skel1Group->getNumJoints(); ++i) + EXPECT_EQ(skel1Group->getJoint(i), skel->getJoint(i)); + + for(size_t i=0; i < skel1Group->getNumDofs(); ++i) + EXPECT_EQ(skel1Group->getDof(i), skel->getDof(i)); + + // Test arbitrary Groups by plucking random BodyNodes, Joints, and + // DegreesOfFreedom from a Skeleton. + GroupPtr group = Group::create(); + std::vector bodyNodes; + std::vector joints; + std::vector dofs; + for(size_t i=0; i < 2*skel->getNumBodyNodes(); ++i) + { + size_t randomIndex = floor(random(0, skel->getNumBodyNodes())); + BodyNode* bn = skel->getBodyNode(randomIndex); + if(group->addBodyNode(bn, false)) + bodyNodes.push_back(bn); + + randomIndex = floor(random(0, skel->getNumJoints())); + Joint* joint = skel->getJoint(randomIndex); + if(group->addJoint(joint, false, false)) + joints.push_back(joint); + + randomIndex = floor(random(0, skel->getNumDofs())); + DegreeOfFreedom* dof = skel->getDof(randomIndex); + if(group->addDof(dof, false, false)) + dofs.push_back(dof); + } + + EXPECT_EQ(group->getNumBodyNodes(), bodyNodes.size()); + EXPECT_EQ(group->getNumJoints(), joints.size()); + EXPECT_EQ(group->getNumDofs(), dofs.size()); + + for(size_t i=0; i < group->getNumBodyNodes(); ++i) + EXPECT_EQ(group->getBodyNode(i), bodyNodes[i]); + + for(size_t i=0; i < group->getNumJoints(); ++i) + EXPECT_EQ(group->getJoint(i), joints[i]); + + for(size_t i=0; i < group->getNumDofs(); ++i) + EXPECT_EQ(group->getDof(i), dofs[i]); } int main(int argc, char* argv[])