Skip to content

Commit 470ec5a

Browse files
jorisvjorisv
authored
Merge pull request #2477 from MegMll/fix/mjcf_armature
Mjcf : Fix parsing of armature parameters
2 parents 57bd452 + fdb2fa6 commit 470ec5a

File tree

5 files changed

+102
-17
lines changed

5 files changed

+102
-17
lines changed

‎CHANGELOG.md

+4
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,10 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
99
### Added
1010
- Add `pinocchio_python_parser` target ([#2475](https://github.com/stack-of-tasks/pinocchio/pull/2475))
1111

12+
### Fixed
13+
- Fix mjcf parsing of armature and of the default tag in models ([#2477](https://github.com/stack-of-tasks/pinocchio/pull/2477))
14+
- Fix undefined behavior when using the site attribute in mjcf ([#2477](https://github.com/stack-of-tasks/pinocchio/pull/2477))
15+
1216
### Changed
1317
- On GNU/Linux and macOS, hide all symbols by default ([#2469](https://github.com/stack-of-tasks/pinocchio/pull/2469))
1418

‎include/pinocchio/parsers/mjcf/mjcf-graph.hpp

+3-3
Original file line numberDiff line numberDiff line change
@@ -135,7 +135,7 @@ namespace pinocchio
135135
Eigen::VectorXd damping;
136136

137137
// Armature inertia created by this joint
138-
double armature = 0.;
138+
Eigen::VectorXd armature;
139139
// Dry friction.
140140
double frictionLoss = 0.;
141141

@@ -149,9 +149,9 @@ namespace pinocchio
149149
maxConfig = Eigen::VectorXd::Constant(1, infty);
150150
springStiffness = Eigen::VectorXd::Constant(1, v);
151151
springReference = Eigen::VectorXd::Constant(1, v);
152-
;
153152
friction = Eigen::VectorXd::Constant(1, 0.);
154153
damping = Eigen::VectorXd::Constant(1, 0.);
154+
armature = Eigen::VectorXd::Constant(1, 0.);
155155
}
156156

157157
/// @brief Set dimension to the limits to match the joint nq and nv.
@@ -446,7 +446,7 @@ namespace pinocchio
446446
/// @brief Go through the default part of the file and get all the class name. Fill the
447447
/// mapOfDefault for later use.
448448
/// @param el ptree element. Root of the default
449-
void parseDefault(ptree & el, const ptree & parent);
449+
void parseDefault(ptree & el, const ptree & parent, const std::string & parentTag);
450450

451451
/// @brief Go through the main body of the mjcf file "worldbody" to get all the info ready
452452
/// to create the model.

‎src/parsers/mjcf/mjcf-graph-geom.cpp

+14-2
Original file line numberDiff line numberDiff line change
@@ -410,7 +410,13 @@ namespace pinocchio
410410
geomName =
411411
currentBody.bodyName + "Geom_" + std::to_string(currentBody.geomChildren.size());
412412

413-
// ChildClass < Class < Real Joint
413+
// default < ChildClass < Class < Real Joint
414+
if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end())
415+
{
416+
const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default");
417+
if (auto geom_p = classD.classElement.get_child_optional("geom"))
418+
goThroughElement(*geom_p, currentGraph);
419+
}
414420
// childClass
415421
if (currentBody.childClass != "")
416422
{
@@ -474,7 +480,13 @@ namespace pinocchio
474480
siteName =
475481
currentBody.bodyName + "Site_" + std::to_string(currentBody.siteChildren.size());
476482

477-
// ChildClass < Class < Real Joint
483+
// default < ChildClass < Class < Real Joint
484+
if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end())
485+
{
486+
const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default");
487+
if (auto site_p = classD.classElement.get_child_optional("site"))
488+
goThroughElement(*site_p, currentGraph);
489+
}
478490
// childClass
479491
if (currentBody.childClass != "")
480492
{

‎src/parsers/mjcf/mjcf-graph.cpp

+39-11
Original file line numberDiff line numberDiff line change
@@ -127,7 +127,7 @@ namespace pinocchio
127127
ret.minConfig = Vector::Constant(Nq, -1.01);
128128
ret.friction = Vector::Constant(Nv, 0.);
129129
ret.damping = Vector::Constant(Nv, 0.);
130-
ret.armature = armature;
130+
ret.armature = Vector::Constant(Nv, armature[0]);
131131
ret.frictionLoss = frictionLoss;
132132
ret.springStiffness = springStiffness;
133133
ret.springReference = springReference;
@@ -160,6 +160,10 @@ namespace pinocchio
160160
ret.springReference.tail(1) = range.springReference;
161161
ret.springStiffness.conservativeResize(springStiffness.size() + 1);
162162
ret.springStiffness.tail(1) = range.springStiffness;
163+
164+
ret.armature.conservativeResize(armature.size() + Nv);
165+
ret.armature.tail(Nv) = range.armature;
166+
163167
return ret;
164168
}
165169

@@ -208,7 +212,7 @@ namespace pinocchio
208212

209213
value = el.get_optional<double>("<xmlattr>.armature");
210214
if (value)
211-
range.armature = *value;
215+
range.armature[0] = *value;
212216

213217
// friction loss
214218
value = el.get_optional<double>("<xmlattr>.frictionloss");
@@ -251,7 +255,13 @@ namespace pinocchio
251255
// Placement
252256
jointPlacement = currentGraph.convertPosition(el);
253257

254-
// ChildClass < Class < Real Joint
258+
// default < ChildClass < Class < Real Joint
259+
if (currentGraph.mapOfClasses.find("mujoco_default") != currentGraph.mapOfClasses.end())
260+
{
261+
const MjcfClass & classD = currentGraph.mapOfClasses.at("mujoco_default");
262+
if (auto joint_p = classD.classElement.get_child_optional("joint"))
263+
goThroughElement(*joint_p, use_limit, currentGraph.compilerInfo);
264+
}
255265
// childClass
256266
if (currentBody.childClass != "")
257267
{
@@ -545,7 +555,13 @@ namespace pinocchio
545555
else
546556
throw std::invalid_argument("Material was given without a name");
547557

548-
// Class < Attributes
558+
// default < Class < Attributes
559+
if (mapOfClasses.find("mujoco_default") != mapOfClasses.end())
560+
{
561+
const MjcfClass & classD = mapOfClasses.at("mujoco_default");
562+
if (auto mat_p = classD.classElement.get_child_optional("material"))
563+
mat.goThroughElement(*mat_p);
564+
}
549565
auto cl_s = el.get_optional<std::string>("<xmlattr>.class");
550566
if (cl_s)
551567
{
@@ -600,9 +616,10 @@ namespace pinocchio
600616
}
601617
}
602618

603-
void MjcfGraph::parseDefault(ptree & el, const ptree & parent)
619+
void MjcfGraph::parseDefault(ptree & el, const ptree & parent, const std::string & parentTag)
604620
{
605621
boost::optional<std::string> nameClass;
622+
// Classes
606623
for (ptree::value_type & v : el)
607624
{
608625
if (v.first == "<xmlattr>")
@@ -619,8 +636,15 @@ namespace pinocchio
619636
else
620637
throw std::invalid_argument("Class does not have a name. Cannot parse model.");
621638
}
622-
if (v.first == "default")
623-
parseDefault(v.second, el);
639+
else if (v.first == "default")
640+
parseDefault(v.second, el, v.first);
641+
else if (parentTag == "mujoco" && v.first != "<xmlattr>")
642+
{
643+
MjcfClass classDefault;
644+
classDefault.className = "mujoco_default";
645+
classDefault.classElement = el;
646+
mapOfClasses.insert(std::make_pair("mujoco_default", classDefault));
647+
}
624648
}
625649
}
626650

@@ -808,7 +832,7 @@ namespace pinocchio
808832
parseCompiler(el.get_child("compiler"));
809833

810834
if (v.first == "default")
811-
parseDefault(el.get_child("default"), el);
835+
parseDefault(el.get_child("default"), el, "mujoco");
812836

813837
if (v.first == "asset")
814838
parseAsset(el.get_child("asset"));
@@ -898,7 +922,9 @@ namespace pinocchio
898922

899923
// Add armature info
900924
JointIndex j_id = urdfVisitor.getJointId(joint.jointName);
901-
urdfVisitor.model.armature[static_cast<Eigen::Index>(j_id) - 1] = range.armature;
925+
urdfVisitor.model.armature.segment(
926+
urdfVisitor.model.joints[j_id].idx_v(), urdfVisitor.model.joints[j_id].nv()) =
927+
range.armature;
902928
}
903929

904930
void MjcfGraph::fillModel(const std::string & nameOfBody)
@@ -912,7 +938,7 @@ namespace pinocchio
912938
if (!currentBody.bodyParent.empty())
913939
parentFrameId = urdfVisitor.getBodyId(currentBody.bodyParent);
914940

915-
const Frame & frame = urdfVisitor.model.frames[parentFrameId];
941+
Frame frame = urdfVisitor.model.frames[parentFrameId];
916942
// get body pose in body parent
917943
const SE3 bodyPose = currentBody.bodyPlacement;
918944
Inertia inert = currentBody.bodyInertia;
@@ -1007,7 +1033,9 @@ namespace pinocchio
10071033
FrameIndex jointFrameId = urdfVisitor.model.addJointFrame(joint_id, (int)parentFrameId);
10081034
urdfVisitor.appendBodyToJoint(jointFrameId, inert, bodyInJoint, nameOfBody);
10091035

1010-
urdfVisitor.model.armature[static_cast<Eigen::Index>(joint_id) - 1] = rangeCompo.armature;
1036+
urdfVisitor.model.armature.segment(
1037+
urdfVisitor.model.joints[joint_id].idx_v(), urdfVisitor.model.joints[joint_id].nv()) =
1038+
rangeCompo.armature;
10111039
}
10121040

10131041
FrameIndex previousFrameId = urdfVisitor.model.frames.size();

‎unittest/mjcf.cpp

+42-1
Original file line numberDiff line numberDiff line change
@@ -386,7 +386,7 @@ BOOST_AUTO_TEST_CASE(merge_default)
386386
MjcfGraph::UrdfVisitor visitor(model);
387387

388388
MjcfGraph graph(visitor, "fakeMjcf");
389-
graph.parseDefault(ptr.get_child("default"), ptr);
389+
graph.parseDefault(ptr.get_child("default"), ptr, "default");
390390

391391
std::unordered_map<std::string, pt::ptree> TrueMap;
392392

@@ -974,6 +974,47 @@ BOOST_AUTO_TEST_CASE(joint_and_inertias)
974974
BOOST_CHECK(model_m.inertias[1].isApprox(real_inertia));
975975
}
976976

977+
BOOST_AUTO_TEST_CASE(armature)
978+
{
979+
typedef pinocchio::SE3::Vector3 Vector3;
980+
typedef pinocchio::SE3::Matrix3 Matrix3;
981+
std::cout << " Armature ------------ " << std::endl;
982+
std::istringstream xmlData(R"(
983+
<mujoco model="model_RX">
984+
<default>
985+
<joint armature="1" damping="1" limited="true"/>
986+
</default>
987+
<worldbody>
988+
<body name="link0">
989+
<body name="link1" pos="0 0 0">
990+
<joint name="joint1" type="hinge" axis="1 0 0" armature="1.3"/>
991+
<joint name="joint2" type="hinge" axis="0 1 0" armature="2.4"/>
992+
<joint name="joint3" type="hinge" axis="0 0 1" armature="0.4"/>
993+
<body pos=".2 0 0" name="body2">
994+
<joint type="ball"/>
995+
</body>
996+
</body>
997+
</body>
998+
</worldbody>
999+
</mujoco>)");
1000+
1001+
auto namefile = createTempFile(xmlData);
1002+
1003+
typedef ::pinocchio::mjcf::details::MjcfGraph MjcfGraph;
1004+
pinocchio::Model model_m;
1005+
MjcfGraph::UrdfVisitor visitor(model_m);
1006+
1007+
MjcfGraph graph(visitor, "fakeMjcf");
1008+
graph.parseGraphFromXML(namefile.name());
1009+
graph.parseRootTree();
1010+
1011+
Eigen::VectorXd armature_real(model_m.nv);
1012+
armature_real << 1.3, 2.4, 0.4, 1, 1, 1;
1013+
1014+
for (size_t i = 0; i < size_t(model_m.nv); i++)
1015+
BOOST_CHECK_EQUAL(model_m.armature[i], armature_real[i]);
1016+
}
1017+
9771018
// Test reference positions and how it's included in keyframe
9781019
BOOST_AUTO_TEST_CASE(reference_positions)
9791020
{

0 commit comments

Comments
 (0)