From d2e8b253c2347e5a421045b6f87773acf88dead9 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 6 Jan 2025 14:05:24 +0100 Subject: [PATCH 1/7] [mjcf] Add unittest for parentJoint of sites --- unittest/mjcf.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index aa9a67acc..afea6c094 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -980,7 +980,7 @@ BOOST_AUTO_TEST_CASE(armature) { typedef pinocchio::SE3::Vector3 Vector3; typedef pinocchio::SE3::Matrix3 Matrix3; - std::cout << " Armature ------------ " << std::endl; + std::istringstream xmlData(R"( @@ -1120,6 +1120,7 @@ BOOST_AUTO_TEST_CASE(adding_site) pinocchio::SE3 real_placement(rotation_matrix, Vector3(0.03, 0, -0.05)); BOOST_CHECK(model_m.frames[model_m.getFrameId("testSite")].placement.isApprox(real_placement)); + BOOST_CHECK(model_m.frames[model_m.getFrameId("testSite")].parentJoint == model_m.frames[model_m.getFrameId("body3")].parentJoint); } /// @brief test that a fixed model is well parsed From 189c1444891566bc890d30e031da89d7d1dc7800 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 6 Jan 2025 14:05:44 +0100 Subject: [PATCH 2/7] [mjcf] Use the frame of the parent body of the sites --- src/parsers/mjcf/mjcf-graph.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index 9cc217fa4..8a4f71a86 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -1070,12 +1070,13 @@ namespace pinocchio rangeCompo.armature; } - FrameIndex previousFrameId = urdfVisitor.model.frames.size(); + FrameIndex bodyId = urdfVisitor.model.getFrameId(nameOfBody, BODY); + frame = urdfVisitor.model.frames[bodyId]; for (const auto & site : currentBody.siteChildren) { SE3 placement = bodyInJoint * site.sitePlacement; - previousFrameId = urdfVisitor.model.addFrame( - Frame(site.siteName, frame.parentJoint, previousFrameId, placement, OP_FRAME)); + FrameIndex previousFrameId = urdfVisitor.model.addFrame( + Frame(site.siteName, frame.parentJoint, bodyId, placement, OP_FRAME)); } } From 06ef4eb7248a29704691db9e9c679286cfeda620 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 6 Jan 2025 14:09:58 +0100 Subject: [PATCH 3/7] [mjcf] replace with throw pretty --- src/parsers/mjcf/mjcf-graph.cpp | 42 +++++++++++++++++---------------- 1 file changed, 22 insertions(+), 20 deletions(-) diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index 8a4f71a86..74e7c6ace 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -59,13 +59,13 @@ namespace pinocchio else { if (filePath.extension().empty()) - throw std::invalid_argument("Cannot find extension for one of the mesh/texture"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Cannot find extension for one of the mesh/texture"); auto st = filePath.stem(); if (!st.empty()) return st.string(); else - throw std::invalid_argument("Cannot find a name for one of the mesh.texture"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Cannot find a name for one of the mesh.texture"); } } @@ -172,7 +172,7 @@ namespace pinocchio { if (!use_limits && el.get_optional(".range")) - throw std::invalid_argument("Range limit is specified but it was not specify to use it."); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Range limit is specified but it was not specify to use it."); // Type auto type_s = el.get_optional(".type"); @@ -227,7 +227,7 @@ namespace pinocchio else if (jointType == "hinge") posRef = currentCompiler.convertAngle(*value); else - throw std::invalid_argument( + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Reference position can only be used with hinge or slide joints."); } } @@ -358,7 +358,7 @@ namespace pinocchio double mass = std::max(el.get(".mass"), compilerInfo.boundMass); ; if (mass < 0) - throw std::invalid_argument("Mass of body is not supposed to be negative"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Mass of body is not supposed to be negative"); Inertia::Vector3 com; auto com_s = el.get_optional(".pos"); @@ -410,7 +410,7 @@ namespace pinocchio auto chcl_s = childClass; // if inertiafromgeom is false and inertia does not exist - throw if (!compilerInfo.inertiafromgeom && !el.get_child_optional("inertial")) - throw std::invalid_argument("Cannot get inertia from geom and no inertia was found"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Cannot get inertia from geom and no inertia was found"); bool usegeominertia = false; if (compilerInfo.inertiafromgeom) @@ -524,7 +524,7 @@ namespace pinocchio { std::cout << "Warning - Only texture with files are supported" << std::endl; if (name.empty()) - throw std::invalid_argument("Textures need a name."); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Textures need a name."); } else { @@ -553,7 +553,7 @@ namespace pinocchio if (n) name = *n; else - throw std::invalid_argument("Material was given without a name"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Material was given without a name"); // default < Class < Attributes if (mapOfClasses.find("mujoco_default") != mapOfClasses.end()) @@ -644,7 +644,7 @@ namespace pinocchio parseTexture(v.second); if (v.first == "hfield") - throw std::invalid_argument("hfields are not supported yet"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "hfields are not supported yet"); } } @@ -666,7 +666,7 @@ namespace pinocchio mapOfClasses.insert(std::make_pair(*nameClass, classDefault)); } else - throw std::invalid_argument("Class does not have a name. Cannot parse model."); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Class does not have a name. Cannot parse model."); } else if (v.first == "default") parseDefault(v.second, el, v.first); @@ -735,8 +735,10 @@ namespace pinocchio { std::string eulerseq = *eulerS; if (eulerseq.find_first_not_of("xyzXYZ") != std::string::npos || eulerseq.size() != 3) - throw std::invalid_argument( + { + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Model tried to use euler angles but euler sequence is wrong"); + } else { // get index combination @@ -764,7 +766,7 @@ namespace pinocchio compilerInfo.mapEulerAngles.col(ci) = Eigen::Vector3d::UnitZ(); break; default: - throw std::invalid_argument("Euler Axis does not exist"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Euler Axis does not exist"); break; } } @@ -817,7 +819,7 @@ namespace pinocchio if (body1) eq.body1 = *body1; else - throw std::invalid_argument("Equality constraint needs a first body"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Equality constraint needs a first body"); // get the name of second body auto body2 = v.second.get_optional(".body2"); @@ -846,7 +848,7 @@ namespace pinocchio if (pt.get_child_optional("mujoco")) el = pt.get_child("mujoco"); else - throw std::invalid_argument("This is not a standard mujoco model. Cannot parse it."); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "This is not a standard mujoco model. Cannot parse it."); for (const ptree::value_type & v : el) { @@ -857,7 +859,7 @@ namespace pinocchio if (n_s) modelName = *n_s; else - throw std::invalid_argument("Model is missing a name. Cannot parse it"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Model is missing a name. Cannot parse it"); } if (v.first == "compiler") @@ -945,7 +947,7 @@ namespace pinocchio jType = UrdfVisitor::REVOLUTE; } else - throw std::invalid_argument("Unknown joint type"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Unknown joint type"); urdfVisitor.addJointAndBody( jType, joint.axis, parentFrameId, jointInParent, joint.jointName, inert, bodyInJoint, @@ -1017,7 +1019,7 @@ namespace pinocchio for (const auto & joint : currentBody.jointChildren) { if (joint.jointType == "free") - throw std::invalid_argument("Joint Composite trying to be created with a freeFlyer"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Joint Composite trying to be created with a freeFlyer"); SE3 jointInParent = bodyPose * joint.jointPlacement; bodyInJoint = joint.jointPlacement.inverse(); @@ -1052,7 +1054,7 @@ namespace pinocchio rangeCompo = rangeCompo.concatenate<1, 1>(joint.range); } else - throw std::invalid_argument("Unknown joint type trying to be parsed."); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Unknown joint type trying to be parsed."); prevJointPlacement = jointInParent; } @@ -1152,7 +1154,7 @@ namespace pinocchio urdfVisitor.model.referenceConfigurations.insert(std::make_pair(keyName, qpos)); } else - throw std::invalid_argument("Keyframe size does not match model size"); + PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Keyframe size does not match model size"); } void MjcfGraph::parseContactInformation( @@ -1189,7 +1191,7 @@ namespace pinocchio void MjcfGraph::parseRootTree() { urdfVisitor.setName(modelName); - + // get name and inertia of first root link std::string rootLinkName = bodiesList.at(0); MjcfBody rootBody = mapOfBodies.find(rootLinkName)->second; From 9ae4a1f39fc5c85abfc6fb8c804068891f5efd73 Mon Sep 17 00:00:00 2001 From: Megane Millan Date: Mon, 6 Jan 2025 18:40:55 +0100 Subject: [PATCH 4/7] [Changelog] Add Entry --- CHANGELOG.md | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 9dfdf51d1..a49bd546f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -6,14 +6,17 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/). ## [Unreleased] +### Added +- Add parsing meshes with vertices for MJCF format ([#2537](https://github.com/stack-of-tasks/pinocchio/pull/2537)) + ### Fixed - Fix mjcf Euler angle parsing: use xyz as a default value for eulerseq compiler option ([#2526](https://github.com/stack-of-tasks/pinocchio/pull/2526)) - Fix variable naming in Python ([#2530](https://github.com/stack-of-tasks/pinocchio/pull/2530)) - Fix aba explicit template instantiation ([#2541](https://github.com/stack-of-tasks/pinocchio/pull/2541)) -- Add parsing meshes with vertices for MJCF format ([#2537](https://github.com/stack-of-tasks/pinocchio/pull/2537)) - CMake: fix RPATH on macos ([#2546](https://github.com/stack-of-tasks/pinocchio/pull/2546)) - Fix aba explicit template instantiation ([#2541](https://github.com/stack-of-tasks/pinocchio/pull/2541)) - Fix mjcf parsing of keyframe qpos with newlines ([#2535](https://github.com/stack-of-tasks/pinocchio/pull/2535)) +- Fix sites parsing for MJCF format ([#2548](https://github.com/stack-of-tasks/pinocchio/pull/2548)) ## [3.3.1] - 2024-12-13 From d320b163059f0c849aca24b4f16ab54a44318279 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9gane=20Millan?= <37700908+MegMll@users.noreply.github.com> Date: Fri, 10 Jan 2025 10:17:21 +0100 Subject: [PATCH 5/7] [Mjcf] Remove trailing white space Co-authored-by: Joris Vaillant --- src/parsers/mjcf/mjcf-graph.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index 74e7c6ace..c66a83d27 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -1191,7 +1191,6 @@ namespace pinocchio void MjcfGraph::parseRootTree() { urdfVisitor.setName(modelName); - // get name and inertia of first root link std::string rootLinkName = bodiesList.at(0); MjcfBody rootBody = mapOfBodies.find(rootLinkName)->second; From 3ea4df34e9a834b980c1cf251db36bc04ebc5619 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?M=C3=A9gane=20Millan?= <37700908+MegMll@users.noreply.github.com> Date: Fri, 10 Jan 2025 10:17:43 +0100 Subject: [PATCH 6/7] [Mjcf] Do not store frame Co-authored-by: Joris Vaillant --- src/parsers/mjcf/mjcf-graph.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index c66a83d27..667bbf0b8 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -1077,7 +1077,7 @@ namespace pinocchio for (const auto & site : currentBody.siteChildren) { SE3 placement = bodyInJoint * site.sitePlacement; - FrameIndex previousFrameId = urdfVisitor.model.addFrame( + urdfVisitor.model.addFrame( Frame(site.siteName, frame.parentJoint, bodyId, placement, OP_FRAME)); } } From faae757c585dc895e8e5337a07a4113b12a068d5 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Fri, 10 Jan 2025 13:30:14 +0000 Subject: [PATCH 7/7] [pre-commit.ci] auto fixes from pre-commit.com hooks for more information, see https://pre-commit.ci --- src/parsers/mjcf/mjcf-graph.cpp | 37 +++++++++++++++++++++------------ unittest/mjcf.cpp | 4 +++- 2 files changed, 27 insertions(+), 14 deletions(-) diff --git a/src/parsers/mjcf/mjcf-graph.cpp b/src/parsers/mjcf/mjcf-graph.cpp index 667bbf0b8..cdd73691d 100644 --- a/src/parsers/mjcf/mjcf-graph.cpp +++ b/src/parsers/mjcf/mjcf-graph.cpp @@ -59,13 +59,15 @@ namespace pinocchio else { if (filePath.extension().empty()) - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Cannot find extension for one of the mesh/texture"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Cannot find extension for one of the mesh/texture"); auto st = filePath.stem(); if (!st.empty()) return st.string(); else - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Cannot find a name for one of the mesh.texture"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Cannot find a name for one of the mesh.texture"); } } @@ -172,7 +174,8 @@ namespace pinocchio { if (!use_limits && el.get_optional(".range")) - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Range limit is specified but it was not specify to use it."); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Range limit is specified but it was not specify to use it."); // Type auto type_s = el.get_optional(".type"); @@ -227,7 +230,8 @@ namespace pinocchio else if (jointType == "hinge") posRef = currentCompiler.convertAngle(*value); else - PINOCCHIO_THROW_PRETTY(std::invalid_argument, + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Reference position can only be used with hinge or slide joints."); } } @@ -358,7 +362,8 @@ namespace pinocchio double mass = std::max(el.get(".mass"), compilerInfo.boundMass); ; if (mass < 0) - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Mass of body is not supposed to be negative"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Mass of body is not supposed to be negative"); Inertia::Vector3 com; auto com_s = el.get_optional(".pos"); @@ -410,7 +415,8 @@ namespace pinocchio auto chcl_s = childClass; // if inertiafromgeom is false and inertia does not exist - throw if (!compilerInfo.inertiafromgeom && !el.get_child_optional("inertial")) - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Cannot get inertia from geom and no inertia was found"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Cannot get inertia from geom and no inertia was found"); bool usegeominertia = false; if (compilerInfo.inertiafromgeom) @@ -666,7 +672,8 @@ namespace pinocchio mapOfClasses.insert(std::make_pair(*nameClass, classDefault)); } else - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Class does not have a name. Cannot parse model."); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Class does not have a name. Cannot parse model."); } else if (v.first == "default") parseDefault(v.second, el, v.first); @@ -736,8 +743,8 @@ namespace pinocchio std::string eulerseq = *eulerS; if (eulerseq.find_first_not_of("xyzXYZ") != std::string::npos || eulerseq.size() != 3) { - PINOCCHIO_THROW_PRETTY(std::invalid_argument, - "Model tried to use euler angles but euler sequence is wrong"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Model tried to use euler angles but euler sequence is wrong"); } else { @@ -848,7 +855,8 @@ namespace pinocchio if (pt.get_child_optional("mujoco")) el = pt.get_child("mujoco"); else - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "This is not a standard mujoco model. Cannot parse it."); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "This is not a standard mujoco model. Cannot parse it."); for (const ptree::value_type & v : el) { @@ -859,7 +867,8 @@ namespace pinocchio if (n_s) modelName = *n_s; else - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Model is missing a name. Cannot parse it"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Model is missing a name. Cannot parse it"); } if (v.first == "compiler") @@ -1019,7 +1028,8 @@ namespace pinocchio for (const auto & joint : currentBody.jointChildren) { if (joint.jointType == "free") - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Joint Composite trying to be created with a freeFlyer"); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Joint Composite trying to be created with a freeFlyer"); SE3 jointInParent = bodyPose * joint.jointPlacement; bodyInJoint = joint.jointPlacement.inverse(); @@ -1054,7 +1064,8 @@ namespace pinocchio rangeCompo = rangeCompo.concatenate<1, 1>(joint.range); } else - PINOCCHIO_THROW_PRETTY(std::invalid_argument, "Unknown joint type trying to be parsed."); + PINOCCHIO_THROW_PRETTY( + std::invalid_argument, "Unknown joint type trying to be parsed."); prevJointPlacement = jointInParent; } diff --git a/unittest/mjcf.cpp b/unittest/mjcf.cpp index afea6c094..972adc637 100644 --- a/unittest/mjcf.cpp +++ b/unittest/mjcf.cpp @@ -1120,7 +1120,9 @@ BOOST_AUTO_TEST_CASE(adding_site) pinocchio::SE3 real_placement(rotation_matrix, Vector3(0.03, 0, -0.05)); BOOST_CHECK(model_m.frames[model_m.getFrameId("testSite")].placement.isApprox(real_placement)); - BOOST_CHECK(model_m.frames[model_m.getFrameId("testSite")].parentJoint == model_m.frames[model_m.getFrameId("body3")].parentJoint); + BOOST_CHECK( + model_m.frames[model_m.getFrameId("testSite")].parentJoint + == model_m.frames[model_m.getFrameId("body3")].parentJoint); } /// @brief test that a fixed model is well parsed