diff --git a/CHANGELOG.md b/CHANGELOG.md index cc75a73e06..2942a4b95c 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -9,6 +9,10 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ### Added - Add `pinocchio_python_parser` target ([#2475](https://github.com/stack-of-tasks/pinocchio/pull/2475)) +### Fixed +- Fix mjcf parsing of armature and of the default tag in models ([#2477](https://github.com/stack-of-tasks/pinocchio/pull/2477)) +- Fix undefined behavior when using the site attribute in mjcf ([#2477](https://github.com/stack-of-tasks/pinocchio/pull/2477)) + ### Changed - On GNU/Linux and macOS, hide all symbols by default ([#2469](https://github.com/stack-of-tasks/pinocchio/pull/2469)) diff --git a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp index ca2667f06f..25d69f7152 100644 --- a/include/pinocchio/parsers/mjcf/mjcf-graph.hpp +++ b/include/pinocchio/parsers/mjcf/mjcf-graph.hpp @@ -135,7 +135,7 @@ namespace pinocchio Eigen::VectorXd damping; // Armature inertia created by this joint - double armature = 0.; + Eigen::VectorXd armature; // Dry friction. double frictionLoss = 0.; @@ -149,9 +149,9 @@ namespace pinocchio maxConfig = Eigen::VectorXd::Constant(1, infty); springStiffness = Eigen::VectorXd::Constant(1, v); springReference = Eigen::VectorXd::Constant(1, v); - ; friction = Eigen::VectorXd::Constant(1, 0.); damping = Eigen::VectorXd::Constant(1, 0.); + armature = Eigen::VectorXd::Constant(1, 0.); } /// @brief Set dimension to the limits to match the joint nq and nv. @@ -446,7 +446,7 @@ namespace pinocchio /// @brief Go through the default part of the file and get all the class name. Fill the /// mapOfDefault for later use. /// @param el ptree element. Root of the default - void parseDefault(ptree & el, const ptree & parent); + void parseDefault(ptree & el, const ptree & parent, const std::string & parentTag); /// @brief Go through the main body of the mjcf file "worldbody" to get all the info ready /// to create the model. diff --git a/src/parsers/mjcf/mjcf-graph-geom.cpp b/src/parsers/mjcf/mjcf-graph-geom.cpp index 0948a9c464..27c85c6714 100644 --- a/src/parsers/mjcf/mjcf-graph-geom.cpp +++ b/src/parsers/mjcf/mjcf-graph-geom.cpp @@ -410,7 +410,13 @@ namespace pinocchio geomName = currentBody.bodyName + "Geom_" + std::to_string(currentBody.geomChildren.size()); - // ChildClass < Class < Real Joint + // default < ChildClass < Class < Real Joint + if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end()) + { + const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default"); + if (auto geom_p = classD.classElement.get_child_optional("geom")) + goThroughElement(*geom_p, currentGraph); + } // childClass if (currentBody.childClass != "") { @@ -474,7 +480,13 @@ namespace pinocchio siteName = currentBody.bodyName + "Site_" + std::to_string(currentBody.siteChildren.size()); - // ChildClass < Class < Real Joint + // default < ChildClass < Class < Real Joint + if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end()) + { + const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default"); + if (auto site_p = classD.classElement.get_child_optional("site")) + goThroughElement(*site_p, currentGraph); + } // childClass if (currentBody.childClass != "") { diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index cb40a0b4ef..66847fcef5 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -127,7 +127,7 @@ namespace pinocchio ret.minConfig = Vector::Constant(Nq, -1.01); ret.friction = Vector::Constant(Nv, 0.); ret.damping = Vector::Constant(Nv, 0.); - ret.armature = armature; + ret.armature = Vector::Constant(Nv, armature[0]); ret.frictionLoss = frictionLoss; ret.springStiffness = springStiffness; ret.springReference = springReference; @@ -160,6 +160,10 @@ namespace pinocchio ret.springReference.tail(1) = range.springReference; ret.springStiffness.conservativeResize(springStiffness.size() + 1); ret.springStiffness.tail(1) = range.springStiffness; + + ret.armature.conservativeResize(armature.size() + Nv); + ret.armature.tail(Nv) = range.armature; + return ret; } @@ -208,7 +212,7 @@ namespace pinocchio value = el.get_optional(".armature"); if (value) - range.armature = *value; + range.armature[0] = *value; // friction loss value = el.get_optional(".frictionloss"); @@ -251,7 +255,13 @@ namespace pinocchio // Placement jointPlacement = currentGraph.convertPosition(el); - // ChildClass < Class < Real Joint + // default < ChildClass < Class < Real Joint + if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end()) + { + const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default"); + if (auto joint_p = classD.classElement.get_child_optional("joint")) + goThroughElement(*joint_p, use_limit, currentGraph.compilerInfo); + } // childClass if (currentBody.childClass != "") { @@ -545,7 +555,13 @@ namespace pinocchio else throw std::invalid_argument("Material was given without a name"); - // Class < Attributes + // default < Class < Attributes + if (mapOfClasses.find("mujoco_default") != mapOfClasses.end()) + { + const MjcfClass & classD = mapOfClasses.at("mujoco_default"); + if (auto mat_p = classD.classElement.get_child_optional("material")) + mat.goThroughElement(*mat_p); + } auto cl_s = el.get_optional(".class"); if (cl_s) { @@ -600,9 +616,10 @@ namespace pinocchio } } - void MjcfGraph::parseDefault(ptree & el, const ptree & parent) + void MjcfGraph::parseDefault(ptree & el, const ptree & parent, const std::string & parentTag) { boost::optional nameClass; + // Classes for (ptree::value_type & v : el) { if (v.first == "") @@ -619,8 +636,15 @@ namespace pinocchio else throw std::invalid_argument("Class does not have a name. Cannot parse model."); } - if (v.first == "default") - parseDefault(v.second, el); + else if (v.first == "default") + parseDefault(v.second, el, v.first); + else if (parentTag == "mujoco" && v.first != "") + { + MjcfClass classDefault; + classDefault.className = "mujoco_default"; + classDefault.classElement = el; + mapOfClasses.insert(std::make_pair("mujoco_default", classDefault)); + } } } @@ -808,7 +832,7 @@ namespace pinocchio parseCompiler(el.get_child("compiler")); if (v.first == "default") - parseDefault(el.get_child("default"), el); + parseDefault(el.get_child("default"), el, "mujoco"); if (v.first == "asset") parseAsset(el.get_child("asset")); @@ -898,7 +922,9 @@ namespace pinocchio // Add armature info JointIndex j_id = urdfVisitor.getJointId(joint.jointName); - urdfVisitor.model.armature[static_cast(j_id) - 1] = range.armature; + urdfVisitor.model.armature.segment( + urdfVisitor.model.joints[j_id].idx_v(), urdfVisitor.model.joints[j_id].nv()) = + range.armature; } void MjcfGraph::fillModel(const std::string & nameOfBody) @@ -912,7 +938,7 @@ namespace pinocchio if (!currentBody.bodyParent.empty()) parentFrameId = urdfVisitor.getBodyId(currentBody.bodyParent); - const Frame & frame = urdfVisitor.model.frames[parentFrameId]; + Frame frame = urdfVisitor.model.frames[parentFrameId]; // get body pose in body parent const SE3 bodyPose = currentBody.bodyPlacement; Inertia inert = currentBody.bodyInertia; @@ -1007,7 +1033,9 @@ namespace pinocchio FrameIndex jointFrameId = urdfVisitor.model.addJointFrame(joint_id, (int)parentFrameId); urdfVisitor.appendBodyToJoint(jointFrameId, inert, bodyInJoint, nameOfBody); - urdfVisitor.model.armature[static_cast(joint_id) - 1] = rangeCompo.armature; + urdfVisitor.model.armature.segment( + urdfVisitor.model.joints[joint_id].idx_v(), urdfVisitor.model.joints[joint_id].nv()) = + rangeCompo.armature; } FrameIndex previousFrameId = urdfVisitor.model.frames.size(); diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index 1c1f6e5a12..bd3ddc6493 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -386,7 +386,7 @@ BOOST_AUTO_TEST_CASE(merge_default) MjcfGraph::UrdfVisitor visitor(model); MjcfGraph graph(visitor, "fakeMjcf"); - graph.parseDefault(ptr.get_child("default"), ptr); + graph.parseDefault(ptr.get_child("default"), ptr, "default"); std::unordered_map TrueMap; @@ -974,6 +974,47 @@ BOOST_AUTO_TEST_CASE(joint_and_inertias) BOOST_CHECK(model_m.inertias[1].isApprox(real_inertia)); } +BOOST_AUTO_TEST_CASE(armature) +{ + typedef pinocchio::SE3::Vector3 Vector3; + typedef pinocchio::SE3::Matrix3 Matrix3; + std::cout << " Armature ------------ " << std::endl; + std::istringstream xmlData(R"( + + + + + + + + + + + + + + + + + )"); + + auto namefile = createTempFile(xmlData); + + typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph; + pinocchio::Model model_m; + MjcfGraph::UrdfVisitor visitor(model_m); + + MjcfGraph graph(visitor, "fakeMjcf"); + graph.parseGraphFromXML(namefile.name()); + graph.parseRootTree(); + + Eigen::VectorXd armature_real(model_m.nv); + armature_real << 1.3, 2.4, 0.4, 1, 1, 1; + + for (size_t i = 0; i < size_t(model_m.nv); i++) + BOOST_CHECK_EQUAL(model_m.armature[i], armature_real[i]); +} + // Test reference positions and how it's included in keyframe BOOST_AUTO_TEST_CASE(reference_positions) {