diff --git a/CHANGELOG.md b/CHANGELOG.md index 1a19d2377e..aeb112dd69 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -8,6 +8,7 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Fixed - Fix linkage of Boost.Serialization on Windows ([#2400](https://github.com/stack-of-tasks/pinocchio/pull/2400)) +- Fix mjcf parser appending of inertias at root joint ([#2403](https://github.com/stack-of-tasks/pinocchio/pull/2403)) ## [3.2.0] - 2024-08-27 diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index 623ccf3079..ae60fabae9 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -500,15 +500,29 @@ namespace pinocchio namespace fs = boost::filesystem; MjcfTexture text; auto file = el.get_optional(".file"); - if (!file) - throw std::invalid_argument("Only textures with files are supported"); - - fs::path filePath(*file); - std::string name = getName(el, filePath); + auto name_ = el.get_optional(".name"); + auto type = el.get_optional(".type"); - text.filePath = - updatePath(compilerInfo.strippath, compilerInfo.texturedir, modelPath, filePath).string(); + std::string name; + if (name_) + name = *name_; + else if (type && *type == "skybox") + name = *type; + if (!file) + { + std::cout << "Warning - Only texture with files are supported" << std::endl; + if (name.empty()) + throw std::invalid_argument("Textures need a name."); + } + else + { + fs::path filePath(*file); + name = getName(el, filePath); + text.filePath = + updatePath(compilerInfo.strippath, compilerInfo.texturedir, modelPath, filePath) + .string(); + } auto str_v = el.get_optional(".type"); if (str_v) text.textType = *str_v; @@ -785,15 +799,12 @@ namespace pinocchio { FrameIndex parentFrameId = 0; - Inertia inert = Inertia::Zero(); if (!currentBody.bodyParent.empty()) - { parentFrameId = urdfVisitor.getBodyId(currentBody.bodyParent); - inert = currentBody.bodyInertia; - } + // get body pose in body parent const SE3 bodyPose = currentBody.bodyPlacement; - + Inertia inert = currentBody.bodyInertia; SE3 jointInParent = bodyPose * joint.jointPlacement; bodyInJoint = joint.jointPlacement.inverse(); UrdfVisitor::JointType jType; @@ -1036,7 +1047,8 @@ namespace pinocchio // get name and inertia of first root link std::string rootLinkName = bodiesList.at(0); MjcfBody rootBody = mapOfBodies.find(rootLinkName)->second; - urdfVisitor.addRootJoint(rootBody.bodyInertia, rootLinkName); + if (rootBody.jointChildren.size() == 0) + urdfVisitor.addRootJoint(rootBody.bodyInertia, rootLinkName); fillReferenceConfig(rootBody); for (const auto & entry : bodiesList) diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index 775d5c74f6..7ef52dbd8b 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -940,6 +940,39 @@ BOOST_AUTO_TEST_CASE(adding_keyframes) BOOST_CHECK(vect_model == vect_ref); } +// Test on which joints inertias are append +BOOST_AUTO_TEST_CASE(joint_and_inertias) +{ + typedef pinocchio::SE3::Vector3 Vector3; + typedef pinocchio::SE3::Matrix3 Matrix3; + + std::istringstream xmlData(R"( + + + + + + + + )"); + + auto namefile = createTempFile(xmlData); + + pinocchio::Model model_m; + pinocchio::mjcf::buildModel(namefile.name(), model_m); + + BOOST_CHECK(model_m.inertias[0].isApprox(pinocchio::Inertia::Zero())); + + Matrix3 inertia_matrix = Eigen::Matrix3d::Zero(); + inertia_matrix(0, 0) = 0.00315; + inertia_matrix(1, 1) = 0.00388; + inertia_matrix(2, 2) = 0.004285; + pinocchio::Inertia real_inertia(0.629769, Vector3(-0.041018, -0.00014, 0.049974), inertia_matrix); + + BOOST_CHECK(model_m.inertias[1].isApprox(real_inertia)); +} + // Test reference positions and how it's included in keyframe BOOST_AUTO_TEST_CASE(reference_positions) {