diff --git a/CHANGELOG.md b/CHANGELOG.md index 069d94c6b43c1..6f73cb994ae54 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -19,6 +19,7 @@ * Parsers + * Added `sdf` parsing for `fixed` joint and `material` tag of visual shape: [#775](https://github.com/dartsim/dart/pull/775) * Added support of urdfdom_headers 1.0: [#766](https://github.com/dartsim/dart/pull/766) * Misc improvements and bug fixes diff --git a/dart/utils/sdf/SdfParser.cpp b/dart/utils/sdf/SdfParser.cpp index 5dcbd7ef0e6af..c546de53575c1 100644 --- a/dart/utils/sdf/SdfParser.cpp +++ b/dart/utils/sdf/SdfParser.cpp @@ -167,6 +167,11 @@ dynamics::ShapeNode* readShapeNode( const std::string& skelPath, const common::ResourceRetrieverPtr& retriever); + +void readMaterial( + tinyxml2::XMLElement* materialEle, + dynamics::ShapeNode* shapeNode); + void readVisualizationShapeNode( dynamics::BodyNode* bodyNode, tinyxml2::XMLElement* vizShapeNodeEle, @@ -634,6 +639,10 @@ std::pair createJointAndNodePair( return skeleton->createJointAndBodyNodePair(parent, static_cast(*joint.properties), static_cast(*node.properties)); + else if (std::string("fixed") == type) + return skeleton->createJointAndBodyNodePair(parent, + static_cast(*joint.properties), + static_cast(*node.properties)); else if (std::string("free") == type) return skeleton->createJointAndBodyNodePair(parent, static_cast(*joint.properties), @@ -981,6 +990,29 @@ dynamics::ShapeNode* readShapeNode( return shapeNode; } + +//============================================================================== +void readMaterial( + tinyxml2::XMLElement* materialEle, + dynamics::ShapeNode* shapeNode) { + + auto visualAspect = shapeNode->getVisualAspect(); + if (hasElement(materialEle, "diffuse")) { + Eigen::VectorXd color = getValueVectorXd(materialEle, "diffuse"); + if (color.size() == 3) { + Eigen::Vector3d color3d = color; + visualAspect->setColor(color3d); + } else if (color.size() == 4) { + Eigen::Vector4d color4d = color; + visualAspect->setColor(color4d); + } else { + dterr << "[SdfParse::readMaterial] Unsupported color vector size: " + << color.size() << "\n"; + } + } +} + + //============================================================================== void readVisualizationShapeNode( dynamics::BodyNode* bodyNode, @@ -994,6 +1026,13 @@ void readVisualizationShapeNode( skelPath, retriever); newShapeNode->createVisualAspect(); + + // Material + if (hasElement(vizShapeNodeEle, "material")) + { + tinyxml2::XMLElement* materialEle = getElement(vizShapeNodeEle, "material"); + readMaterial(materialEle, newShapeNode); + } } //============================================================================== diff --git a/data/sdf/quad.sdf b/data/sdf/quad.sdf new file mode 100644 index 0000000000000..244ec6ad93df0 --- /dev/null +++ b/data/sdf/quad.sdf @@ -0,0 +1,867 @@ + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 7.200000 + + 0.870000 + 0.000000 + 0.000000 + 0.222000 + 0.000000 + 1.080000 + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + + + 0.600000 1.200000 0.100000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + + + 0.600000 1.200000 0.100000 + + + + 0.500000 0.600000 0.800000 + + + + + -0.300000 0.600000 -0.060000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + 0.012500 + + 0.000005 + 0.000000 + 0.000000 + 0.000005 + 0.000000 + 0.000005 + + + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + + + 0.050000 0.050000 0.050000 + + + + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + + + 0.050000 0.050000 0.050000 + + + + 0.500000 0.600000 0.800000 + + + + + -0.300000 0.600000 -0.120000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + 0.400000 + + 0.005667 + 0.000000 + 0.000000 + 0.005667 + 0.000000 + 0.000667 + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + 0.500000 0.600000 0.800000 + + + + + -0.300000 0.600000 -0.520000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + 0.400000 + + 0.005667 + 0.000000 + 0.000000 + 0.005667 + 0.000000 + 0.000667 + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + 0.500000 0.600000 0.800000 + + + + + -0.300000 0.600000 -0.920000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + 0.300000 + + 0.002500 + 0.000000 + 0.000000 + 0.002500 + 0.000000 + 0.000500 + + + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.300000 + + + + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.300000 + + + + 1.000000 0.900000 0.800000 + + + + 0.000000 0.000000 -0.300000 0.000000 0.000000 0.000000 + + + 0.020000 + + + + 1.000000 0.000000 0.000000 + + + + + 0.300000 0.600000 -0.060000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + 0.012500 + + 0.000005 + 0.000000 + 0.000000 + 0.000005 + 0.000000 + 0.000005 + + + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + + + 0.050000 0.050000 0.050000 + + + + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + + + 0.050000 0.050000 0.050000 + + + + 0.500000 0.600000 0.800000 + + + + + 0.300000 0.600000 -0.120000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + 0.400000 + + 0.005667 + 0.000000 + 0.000000 + 0.005667 + 0.000000 + 0.000667 + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + 0.500000 0.600000 0.800000 + + + + + 0.300000 0.600000 -0.520000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + 0.400000 + + 0.005667 + 0.000000 + 0.000000 + 0.005667 + 0.000000 + 0.000667 + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + 0.500000 0.600000 0.800000 + + + + + 0.300000 0.600000 -0.920000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + 0.300000 + + 0.002500 + 0.000000 + 0.000000 + 0.002500 + 0.000000 + 0.000500 + + + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.300000 + + + + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.300000 + + + + 0.300000 0.400000 0.900000 + + + + 0.000000 0.000000 -0.300000 0.000000 0.000000 0.000000 + + + 0.020000 + + + + 1.000000 0.000000 0.000000 + + + + + -0.300000 -0.600000 -0.060000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + 0.012500 + + 0.000005 + 0.000000 + 0.000000 + 0.000005 + 0.000000 + 0.000005 + + + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + + + 0.050000 0.050000 0.050000 + + + + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + + + 0.050000 0.050000 0.050000 + + + + 0.500000 0.600000 0.800000 + + + + + -0.300000 -0.600000 -0.120000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + 0.400000 + + 0.005667 + 0.000000 + 0.000000 + 0.005667 + 0.000000 + 0.000667 + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + 0.500000 0.600000 0.800000 + + + + + -0.300000 -0.600000 -0.520000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + 0.400000 + + 0.005667 + 0.000000 + 0.000000 + 0.005667 + 0.000000 + 0.000667 + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + 0.500000 0.600000 0.800000 + + + + + -0.300000 -0.600000 -0.920000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + 0.300000 + + 0.002500 + 0.000000 + 0.000000 + 0.002500 + 0.000000 + 0.000500 + + + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.300000 + + + + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.300000 + + + + 0.300000 0.400000 0.900000 + + + + 0.000000 0.000000 -0.300000 0.000000 0.000000 0.000000 + + + 0.020000 + + + + 1.000000 0.000000 0.000000 + + + + + 0.300000 -0.600000 -0.060000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + 0.012500 + + 0.000005 + 0.000000 + 0.000000 + 0.000005 + 0.000000 + 0.000005 + + + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + + + 0.050000 0.050000 0.050000 + + + + + 0.000000 0.000000 -0.025000 0.000000 0.000000 0.000000 + + + 0.050000 0.050000 0.050000 + + + + 0.500000 0.600000 0.800000 + + + + + 0.300000 -0.600000 -0.120000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + 0.400000 + + 0.005667 + 0.000000 + 0.000000 + 0.005667 + 0.000000 + 0.000667 + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + 0.500000 0.600000 0.800000 + + + + + 0.300000 -0.600000 -0.520000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + 0.400000 + + 0.005667 + 0.000000 + 0.000000 + 0.005667 + 0.000000 + 0.000667 + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + + 0.000000 0.000000 -0.200000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.400000 + + + + 0.500000 0.600000 0.800000 + + + + + 0.300000 -0.600000 -0.920000 0.000000 0.000000 0.000000 + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + 0.300000 + + 0.002500 + 0.000000 + 0.000000 + 0.002500 + 0.000000 + 0.000500 + + + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.300000 + + + + + 0.000000 0.000000 -0.150000 0.000000 0.000000 0.000000 + + + 0.100000 0.100000 0.300000 + + + + 0.300000 0.400000 0.900000 + + + + 0.000000 0.000000 -0.300000 0.000000 0.000000 0.000000 + + + 0.020000 + + + + 1.000000 0.000000 0.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + Body + 00FLHip + + 0.000000 1.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 00FLHip + 01FLThigh + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 01FLThigh + 02FLShin + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 02FLShin + 03FLFoot + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + Body + 04FRHip + + 0.000000 1.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 04FRHip + 05FRThigh + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 05FRThigh + 06FRShin + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 06FRShin + 07FRFoot + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + Body + 08BLHip + + 0.000000 1.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 08BLHip + 09BLThigh + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 09BLThigh + 10BLShin + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 10BLShin + 11BLFoot + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + Body + 12BRHip + + 0.000000 1.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 12BRHip + 13BRThigh + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 13BRThigh + 14BRShin + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + 0.000000 0.000000 0.000000 0.000000 0.000000 0.000000 + 0 + 14BRShin + 15BRFoot + + 1.000000 0.000000 0.000000 + + -3.141593 + 3.141593 + 200.000000 + 12.000000 + + + + + diff --git a/unittests/testSdfParser.cpp b/unittests/testSdfParser.cpp index f11086368045d..ec84f9bcf4dc1 100644 --- a/unittests/testSdfParser.cpp +++ b/unittests/testSdfParser.cpp @@ -119,6 +119,24 @@ TEST(SdfParser, ParsingSDFFiles) } } +//============================================================================== +TEST(SdfParser, ReadMaterial) +{ + std::string sdf_filename = DART_DATA_PATH"sdf/quad.sdf"; + SkeletonPtr skeleton = SdfParser::readSkeleton(sdf_filename); + EXPECT_TRUE(nullptr != skeleton); auto bodynode = skeleton->getBodyNode(0); + + for (auto shapenode : bodynode->getShapeNodes()) { + if (shapenode->has()) { + Eigen::Vector4d color = shapenode->getVisualAspect()->getRGBA(); + Eigen::Vector4d expected_color(0.5, 0.6, 0.8, 1.0); + double diff = (color - expected_color).norm(); + EXPECT_LT(diff, 1e-4); + } + } + +} + //============================================================================== int main(int argc, char* argv[]) {