Use URDF material color when specified (#1295)

dartsim · Apr 29, 2019 · 1fc6d26 · 1fc6d26
1 parent ad965e4
commit 1fc6d26
Show file tree

Hide file tree

Showing 5 changed files with 41 additions and 31 deletions.
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -4,6 +4,10 @@
 
 #### Changes
 
+* Parser
+
+  * Changed URDF parser to use URDF material color when specified: [#1295](https://github.com/dartsim/dart/pull/1295)
+
 * GUI
 
   * Improved voxel grid and point cloud rendering performance: [#1294](https://github.com/dartsim/dart/pull/1294)

diff --git a/dart/utils/urdf/DartLoader.cpp b/dart/utils/urdf/DartLoader.cpp
@@ -529,6 +529,7 @@ bool DartLoader::createDartNodeProperties(
   return true;
 }
 
+//==============================================================================
 void setMaterial(
   const urdf::ModelInterface* model,
   dynamics::VisualAspect* visualAspect,
@@ -542,10 +543,16 @@ void setMaterial(
     if(m_it != model->materials_.end())
       urdf_color = m_it->second->color;
 
-    Eigen::Vector4d color(urdf_color.r, urdf_color.g,
-                          urdf_color.b, urdf_color.a);
-
+    const Eigen::Vector4d color(
+        static_cast<double>(urdf_color.r),
+        static_cast<double>(urdf_color.g),
+        static_cast<double>(urdf_color.b),
+        static_cast<double>(urdf_color.a));
     visualAspect->setColor(color);
+    auto shapeFrame = visualAspect->getComposite();
+    auto shape = shapeFrame->getShape();
+    if (auto mesh = std::dynamic_pointer_cast<dynamics::MeshShape>(shape))
+      mesh->setColorMode(dynamics::MeshShape::ColorMode::SHAPE_COLOR);
   }
 }
 

diff --git a/examples/osgExamples/osgOperationalSpaceControl/osgOperationalSpaceControl.cpp b/examples/osgExamples/osgOperationalSpaceControl/osgOperationalSpaceControl.cpp
@@ -293,20 +293,6 @@ int main()
       loader.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf");
   world->addSkeleton(robot);
 
-  // Set the colors of the models to obey the shape's color specification
-  for(std::size_t i=0; i<robot->getNumBodyNodes(); ++i)
-  {
-    BodyNode* bn = robot->getBodyNode(i);
-    auto shapeNodes = bn->getShapeNodesWith<dart::dynamics::VisualAspect>();
-    for(auto shapeNode : shapeNodes)
-    {
-      std::shared_ptr<MeshShape> mesh =
-          std::dynamic_pointer_cast<MeshShape>(shapeNode->getShape());
-      if(mesh)
-        mesh->setColorMode(MeshShape::SHAPE_COLOR);
-    }
-  }
-
   // Rotate the robot so that z is upwards (default transform is not Identity)
   robot->getJoint(0)->setTransformFromParentBodyNode(Eigen::Isometry3d::Identity());
 

diff --git a/examples/osgExamples/osgPointCloud/main.cpp b/examples/osgExamples/osgPointCloud/main.cpp
@@ -432,20 +432,6 @@ dynamics::SkeletonPtr createRobot(const std::string& name)
   auto robot
       = urdfParser.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf");
 
-  // Set the colors of the models to obey the shape's color specification
-  for (std::size_t i = 0; i < robot->getNumBodyNodes(); ++i)
-  {
-    BodyNode* bn = robot->getBodyNode(i);
-    auto shapeNodes = bn->getShapeNodesWith<dart::dynamics::VisualAspect>();
-    for (auto shapeNode : shapeNodes)
-    {
-      std::shared_ptr<MeshShape> mesh
-          = std::dynamic_pointer_cast<MeshShape>(shapeNode->getShape());
-      if (mesh)
-        mesh->setColorMode(MeshShape::SHAPE_COLOR);
-    }
-  }
-
   // Rotate the robot so that z is upwards (default transform is not Identity)
   robot->getJoint(0)->setTransformFromParentBodyNode(
       Eigen::Isometry3d::Identity());

diff --git a/unittests/unit/test_DartLoader.cpp b/unittests/unit/test_DartLoader.cpp
@@ -33,8 +33,10 @@
 #include <iostream>
 #include <gtest/gtest.h>
 #include "dart/dynamics/FreeJoint.hpp"
+#include "dart/dynamics/MeshShape.hpp"
 #include "dart/utils/urdf/DartLoader.hpp"
 
+using namespace dart;
 using dart::common::Uri;
 using dart::utils::DartLoader;
 
@@ -407,3 +409,28 @@ TEST(DartLoader, MultiTreeRobot)
   EXPECT_EQ(robot->getNumBodyNodes(), 2u);
   EXPECT_EQ(robot->getNumTrees(), 2u);
 }
+
+//==============================================================================
+TEST(DartLoader, KR5MeshColor)
+{
+  DartLoader loader;
+  auto robot =
+      loader.parseSkeleton("dart://sample/urdf/KR5/KR5 sixx R650.urdf");
+  EXPECT_TRUE(nullptr != robot);
+
+  EXPECT_EQ(robot->getNumBodyNodes(), 7u);
+  EXPECT_EQ(robot->getNumTrees(), 1u);
+
+  for (auto i = 0u; i < robot->getNumBodyNodes(); ++i)
+  {
+    auto body = robot->getBodyNode(i);
+    for (auto shapeNode : body->getShapeNodesWith<dynamics::VisualAspect>())
+    {
+      auto shape = shapeNode->getShape();
+      if (auto mesh = std::dynamic_pointer_cast<dynamics::MeshShape>(shape))
+      {
+        EXPECT_EQ(mesh->getColorMode(), dynamics::MeshShape::SHAPE_COLOR);
+      }
+    }
+  }
+}