diff --git a/CHANGELOG.md b/CHANGELOG.md index 604216d8e1d31..7ca63e364fed1 100644 --- 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 index 5020ba1d78dc3..6a17f7c00f374 100644 --- 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(urdf_color.r), + static_cast(urdf_color.g), + static_cast(urdf_color.b), + static_cast(urdf_color.a)); visualAspect->setColor(color); + auto shapeFrame = visualAspect->getComposite(); + auto shape = shapeFrame->getShape(); + if (auto mesh = std::dynamic_pointer_cast(shape)) + mesh->setColorMode(dynamics::MeshShape::ColorMode::SHAPE_COLOR); } } diff --git a/examples/osgExamples/osgOperationalSpaceControl/osgOperationalSpaceControl.cpp b/examples/osgExamples/osgOperationalSpaceControl/osgOperationalSpaceControl.cpp index 361088dd6cac5..a0e934aa0b19f 100644 --- 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; igetNumBodyNodes(); ++i) - { - BodyNode* bn = robot->getBodyNode(i); - auto shapeNodes = bn->getShapeNodesWith(); - for(auto shapeNode : shapeNodes) - { - std::shared_ptr mesh = - std::dynamic_pointer_cast(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 index 942ebf56088bc..367894986f68a 100644 --- 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(); - for (auto shapeNode : shapeNodes) - { - std::shared_ptr mesh - = std::dynamic_pointer_cast(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 index f0622de5aa965..11e0d23a82fdb 100644 --- a/unittests/unit/test_DartLoader.cpp +++ b/unittests/unit/test_DartLoader.cpp @@ -33,8 +33,10 @@ #include #include #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()) + { + auto shape = shapeNode->getShape(); + if (auto mesh = std::dynamic_pointer_cast(shape)) + { + EXPECT_EQ(mesh->getColorMode(), dynamics::MeshShape::SHAPE_COLOR); + } + } + } +}