Add Metaskeleton::cloneMetaSkeleton()

CHANGELOG.md

@@ -20,6 +20,7 @@
 
   * Refactor constraint solver: [#1099](https://github.com/dartsim/dart/pull/1099), [#1101](https://github.com/dartsim/dart/pull/1101)
   * Added mimic joint functionality as a new actuator type: [#1178](https://github.com/dartsim/dart/pull/1178)
+  * Added clone function to MetaSkeleton: [#1201](https://github.com/dartsim/dart/pull/1178)
 
 * Optimization
 

dart/dynamics/Branch.cpp

@@ -65,6 +65,21 @@ Branch::Criteria::operator Linkage::Criteria() const
   return convert();
 }
 
+//==============================================================================
+Branch::Criteria Branch::Criteria::convert(const Linkage::Criteria& criteria)
+{
+  BodyNodePtr startBodyNode = criteria.mStart.mNode.lock();
+  if (!startBodyNode)
+  {
+    dtwarn << "[Chain::Criteria::convert] Failed in conversion because the "
+           << "start node of the input criteria is not valid anymore. Using "
+           << "the returning Criteria will lead to creating an empty Branch.\n";
+    return Branch::Criteria(nullptr);
+  }
+
+  return Branch::Criteria(startBodyNode.get());
+}
+
 //==============================================================================
 BranchPtr Branch::create(const Branch::Criteria& _criteria,
                          const std::string& _name)
@@ -74,6 +89,44 @@ BranchPtr Branch::create(const Branch::Criteria& _criteria,
   return branch;
 }
 
+//==============================================================================
+BranchPtr Branch::cloneBranch() const
+{
+  return cloneBranch(getName());
+}
+
+//==============================================================================
+BranchPtr Branch::cloneBranch(const std::string& cloneName) const
+{
+  // Clone the skeleton (assuming one skeleton is involved)
+  BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock();
+  if (!bodyNode)
+  {
+    dtwarn << "[Branch::cloneMetaSkeleton] Failed to clone because the "
+           << "start node of the criteria in this Branch is not valid anymore. "
+           << "Returning nullptr.\n";
+    return nullptr;
+  }
+  SkeletonPtr skelClone = bodyNode->getSkeleton()->cloneSkeleton();
+
+  // Create a Criteria
+  Criteria newCriteria = Criteria::convert(mCriteria);
+  assert(newCriteria.mStart.lock());
+  newCriteria.mStart
+      = skelClone->getBodyNode(newCriteria.mStart.lock()->getName());
+
+  // Create a Branch clone with the Criteria
+  BranchPtr newBranch = create(newCriteria, cloneName);
+
+  return newBranch;
+}
+
+//==============================================================================
+MetaSkeletonPtr Branch::cloneMetaSkeleton(const std::string& cloneName) const
+{
+  return cloneBranch(cloneName);
+}
+
 //==============================================================================
 bool Branch::isStillBranch() const
 {

dart/dynamics/Branch.hpp

@@ -62,12 +62,27 @@ class Branch : public Linkage
 
     /// Operator for implicit conversion to a Linkage::Criteria
     operator Linkage::Criteria() const;
+
+    /// Converts Linkage::Criteria to Branch::Criteria
+    static Criteria convert(const Linkage::Criteria& criteria);
   };
 
   /// Create a Branch
   static BranchPtr create(const Branch::Criteria& _criteria,
                           const std::string& _name = "Branch");
 
+  /// Creates and returns a clone of this Branch.
+  BranchPtr cloneBranch() const;
+
+  /// Creates and returns a clone of this Branch.
+  BranchPtr cloneBranch(const std::string& cloneName) const;
+
+  // To expose MetaSkeleton::cloneMetaSkeleton(), which takes no cloneName.
+  using MetaSkeleton::cloneMetaSkeleton;
+
+  // Documentation inherited
+  MetaSkeletonPtr cloneMetaSkeleton(const std::string& cloneName) const override;
+
   /// Returns false if a new BodyNode has been attached to any BodyNode of this
   /// Branch, or if a BodyNode of this Branch has been detached.
   bool isStillBranch() const;

dart/dynamics/Chain.cpp

@@ -84,6 +84,47 @@ Linkage::Criteria Chain::Criteria::convert() const
   return criteria;
 }
 
+//==============================================================================
+Chain::Criteria Chain::Criteria::convert(const Linkage::Criteria& criteria)
+{
+  BodyNodePtr startBodyNode = criteria.mStart.mNode.lock();
+  if (!startBodyNode)
+  {
+    dtwarn << "[Chain::Criteria::convert] Failed in conversion because the "
+           << "start node of the input criteria is not valid anymore. Using "
+           << "the returning Criteria will lead to creating an empty Chain.\n";
+    return Chain::Criteria(nullptr, nullptr);
+  }
+
+  if (criteria.mTargets.size() != 1u)
+  {
+    dtwarn << "[Chain::Criteria::convert] Failed in conversion because the "
+           << "input criteria is not for Chain. The number of targets should "
+           << "be one while the input is " << criteria.mTargets.size() << ". "
+           << "Using the returning Criteria will lead to creating an empty "
+           << "Chain.\n";
+    return Chain::Criteria(nullptr, nullptr);
+  }
+  const Linkage::Criteria::Target& target = criteria.mTargets[0];
+  BodyNodePtr targetBodyNode = target.mNode.lock();
+  if (!targetBodyNode)
+  {
+    dtwarn << "[Chain::Criteria::convert] Failed in conversion because the "
+           << "end node of the input criteria is not valid anymore. Using the "
+           << "returning Criteria will lead to creating an empty Chain.\n";
+    return Chain::Criteria(nullptr, nullptr);
+  }
+
+  bool includeBoth = true;
+  if (criteria.mStart.mPolicy != Linkage::Criteria::INCLUDE)
+    includeBoth = false;
+  if (target.mPolicy != Linkage::Criteria::INCLUDE)
+    includeBoth = false;
+
+  return Chain::Criteria(
+      startBodyNode.get(), targetBodyNode.get(), includeBoth);
+}
+
 //==============================================================================
 Chain::Criteria::operator Linkage::Criteria() const
 {
@@ -117,6 +158,47 @@ ChainPtr Chain::create(BodyNode* _start, BodyNode* _target,
   return chain;
 }
 
+//==============================================================================
+ChainPtr Chain::cloneChain() const
+{
+  return cloneChain(getName());
+}
+
+//==============================================================================
+ChainPtr Chain::cloneChain(const std::string& cloneName) const
+{
+  // Clone the skeleton (assuming one skeleton is involved)
+  BodyNodePtr bodyNode = mCriteria.mStart.mNode.lock();
+  if (!bodyNode)
+  {
+    dtwarn << "[Chain::cloneMetaSkeleton] Failed to clone because the "
+           << "start node of the criteria in this Chain is not valid anymore. "
+           << "Returning nullptr.\n";
+    return nullptr;
+  }
+  SkeletonPtr skelClone = bodyNode->getSkeleton()->cloneSkeleton();
+
+  // Create a Criteria
+  Criteria newCriteria = Criteria::convert(mCriteria);
+  assert(newCriteria.mStart.lock());
+  assert(newCriteria.mTarget.lock());
+  newCriteria.mStart
+      = skelClone->getBodyNode(newCriteria.mStart.lock()->getName());
+  newCriteria.mTarget
+      = skelClone->getBodyNode(newCriteria.mTarget.lock()->getName());
+
+  // Create a Chain clone with the Criteria
+  ChainPtr newChain = create(newCriteria, cloneName);
+
+  return newChain;
+}
+
+//==============================================================================
+MetaSkeletonPtr Chain::cloneMetaSkeleton(const std::string& cloneName) const
+{
+  return cloneChain(cloneName);
+}
+
 //==============================================================================
 bool Chain::isStillChain() const
 {

dart/dynamics/Chain.hpp

@@ -72,6 +72,9 @@ class Chain : public Linkage
 
     /// Operator for implicit conversion to a Linkage::Criteria
     operator Linkage::Criteria() const;
+
+    /// Converts Linkage::Criteria to Chain::Criteria
+    static Criteria convert(const Linkage::Criteria& criteria);
   };
 
   /// This enum is used to specify to the create() function that both the start
@@ -93,6 +96,18 @@ class Chain : public Linkage
   static ChainPtr create(BodyNode* _start, BodyNode* _target,
                          IncludeBothTag, const std::string& _name = "Chain");
 
+  /// Creates and returns a clone of this Chain.
+  ChainPtr cloneChain() const;
+
+  /// Creates and returns a clone of this Chain.
+  ChainPtr cloneChain(const std::string& cloneName) const;
+
+  // To expose MetaSkeleton::cloneMetaSkeleton(), which takes no cloneName.
+  using MetaSkeleton::cloneMetaSkeleton;
+
+  // Documentation inherited
+  MetaSkeletonPtr cloneMetaSkeleton(const std::string& cloneName) const override;
+
   /// Returns false if this Chain has been broken, or some new Branching has
   /// been added.
   bool isStillChain() const;

dart/dynamics/Group.cpp

@@ -68,6 +68,81 @@ GroupPtr Group::create(const std::string& _name,
   return group;
 }
 
+//==============================================================================
+GroupPtr Group::cloneGroup() const
+{
+  return cloneGroup(getName());
+}
+
+//==============================================================================
+GroupPtr Group::cloneGroup(const std::string& cloneName) const
+{
+  // Create an empty Group
+  GroupPtr newGroup = create(cloneName, nullptr);
+
+  if (getNumBodyNodes() == 0u && (getNumJoints() != 0u || getNumDofs() != 0u))
+  {
+    dtwarn << "[Group::cloneMetaSkeletonHelper] Attempting to "
+           << "clone a ReferentialSkeleton that doesn't include any BodyNodes "
+           << "but including some Joints or DegreeOfFreedoms. This will lead "
+           << "to dangling Joints or DegreeOfFreedoms in the cloned "
+           << "ReferentialSkeleton because it only holds the stong reference "
+           << "to the BodyNodes but not others.\n";
+  }
+
+  // Array for Skeleton clones that will be collected durig cloning BodyNodes,
+  // Joints, DegreeOfFreedoms.
+  //
+  // The clones will not be destroyed even after the map is destroyed because
+  // the new Linkage will hold the skeleton by holding the strong referecnes of
+  // the body nodes.
+  std::unordered_map<const Skeleton*, SkeletonPtr> mapToSkeletonClones;
+  mapToSkeletonClones.reserve(mSkeletons.size());
+  for (const Skeleton* skel : mSkeletons)
+  {
+    SkeletonPtr skelClone = skel->cloneSkeleton();
+    mapToSkeletonClones.insert(std::make_pair(skel, skelClone));
+  }
+
+  // Add BodyNodes
+  for (const BodyNodePtr& bodyNode : mBodyNodes)
+  {
+    SkeletonPtr skelClone = mapToSkeletonClones[bodyNode->getSkeleton().get()];
+    assert(skelClone);
+    BodyNode* bodyNodeClone = skelClone->getBodyNode(bodyNode->getName());
+    assert(bodyNodeClone);
+    newGroup->addBodyNode(bodyNodeClone);
+  }
+
+  // Add Joints
+  for (const JointPtr& joint : mJoints)
+  {
+    SkeletonPtr skelClone = mapToSkeletonClones[joint->getSkeleton().get()];
+    assert(skelClone);
+    Joint* jointClone = skelClone->getJoint(joint->getName());
+    assert(jointClone);
+    newGroup->addJoint(jointClone, false);
+  }
+
+  // Add DegreeOfFreedoms
+  for (const DegreeOfFreedomPtr& dof : mDofs)
+  {
+    SkeletonPtr skelClone = mapToSkeletonClones[dof->getSkeleton().get()];
+    assert(skelClone);
+    DegreeOfFreedom* dofClone = skelClone->getDof(dof->getName());
+    assert(dofClone);
+    newGroup->addDof(dofClone, false);
+  }
+
+  return newGroup;
+}
+
+//==============================================================================
+MetaSkeletonPtr Group::cloneMetaSkeleton(const std::string& cloneName) const
+{
+  return cloneGroup(cloneName);
+}
+
 //==============================================================================
 void Group::swapBodyNodeIndices(std::size_t _index1, std::size_t _index2)
 {

dart/dynamics/Group.hpp

@@ -70,6 +70,18 @@ class Group : public ReferentialSkeleton
   /// Destructor
   virtual ~Group() = default;
 
+  /// Creates and returns a clone of this Group.
+  GroupPtr cloneGroup() const;
+
+  /// Creates and returns a clone of this Group.
+  GroupPtr cloneGroup(const std::string& cloneName) const;
+
+  // To expose MetaSkeleton::cloneMetaSkeleton(), which takes no cloneName.
+  using MetaSkeleton::cloneMetaSkeleton;
+
+  // Documentation inherited
+  MetaSkeletonPtr cloneMetaSkeleton(const std::string& cloneName) const override;
+
   /// Swap the index of BodyNode _index1 with _index2
   void swapBodyNodeIndices(std::size_t _index1, std::size_t _index2);