Merge pull request #158 from resibots/deep_copy_shapes

[robot]: deep copy shapes when cloning
NOSALRO · Dec 16, 2021 · 25267f8 · 25267f8
2 parents dba9f73 + e0f172f
commit 25267f8
Show file tree

Hide file tree

Showing 2 changed files with 29 additions and 2 deletions.
diff --git a/docs/installation.md b/docs/installation.md
@@ -57,7 +57,7 @@ sudo apt-get install liburdfdom-dev liburdfdom-headers-dev
 cd /path/to/tmp/folder
 git clone git://github.com/dartsim/dart.git
 cd dart
-git checkout v6.10.1
+git checkout v6.12.1
 
 mkdir build
 cd build
@@ -84,7 +84,7 @@ brew install urdfdom
 cd /path/to/tmp/folder
 git clone git://github.com/dartsim/dart.git
 cd dart
-git checkout v6.10.1
+git checkout v6.12.1
 
 mkdir build
 cd build

diff --git a/src/robot_dart/robot.cpp b/src/robot_dart/robot.cpp
@@ -279,6 +279,20 @@ namespace robot_dart {
 #endif
         _skeleton->getMutex().unlock();
         auto robot = std::make_shared<Robot>(tmp_skel, _robot_name);
+
+#if DART_VERSION_AT_LEAST(6, 13, 0)
+        // Deep copy everything
+        for (auto& bd : robot->skeleton()->getBodyNodes()) {
+            auto& visual_shapes = bd->getShapeNodesWith<dart::dynamics::VisualAspect>();
+            for (auto& shape : visual_shapes) {
+                if (shape->getShape()->getType() != dart::dynamics::SoftMeshShape::getStaticType())
+                    shape->setShape(shape->getShape()->clone());
+            }
+        }
+#endif
+
+        robot->set_positions(this->positions());
+
         robot->_model_filename = _model_filename;
         robot->_controllers.clear();
         for (auto& ctrl : _controllers) {
@@ -311,13 +325,26 @@ namespace robot_dart {
                 shape->removeAspect<dart::dynamics::CollisionAspect>();
             }
 
+            // ghost robots do not have dynamics
+            auto& dyn_shapes = bd->getShapeNodesWith<dart::dynamics::DynamicsAspect>();
+            for (auto& shape : dyn_shapes) {
+                shape->removeAspect<dart::dynamics::DynamicsAspect>();
+            }
+
             // ghost robots have a different color (same for all bodies)
             auto& visual_shapes = bd->getShapeNodesWith<dart::dynamics::VisualAspect>();
             for (auto& shape : visual_shapes) {
+#if DART_VERSION_AT_LEAST(6, 13, 0)
+                if (shape->getShape()->getType() != dart::dynamics::SoftMeshShape::getStaticType())
+                    shape->setShape(shape->getShape()->clone());
+#endif
                 shape->getVisualAspect()->setRGBA(ghost_color);
             }
         }
 
+        // set positions
+        robot->set_positions(this->positions());
+
         // ghost robots, by default, use the color from the VisualAspect
         robot->set_color_mode("aspect");