Skip to content

Commit

Permalink
[parser/mjcf] Add parsing of the default without name class
Browse files Browse the repository at this point in the history
  • Loading branch information
Megane Millan committed Nov 13, 2024
1 parent fa22d52 commit d5e502b
Show file tree
Hide file tree
Showing 4 changed files with 55 additions and 16 deletions.
2 changes: 1 addition & 1 deletion include/pinocchio/parsers/mjcf/mjcf-graph.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down
16 changes: 14 additions & 2 deletions src/parsers/mjcf/mjcf-graph-geom.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 != "")
{
Expand Down Expand Up @@ -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 != "")
{
Expand Down
40 changes: 32 additions & 8 deletions src/parsers/mjcf/mjcf-graph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -256,7 +256,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 != "")
{
Expand Down Expand Up @@ -550,7 +556,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<std::string>("<xmlattr>.class");
if (cl_s)
{
Expand Down Expand Up @@ -605,9 +617,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<std::string> nameClass;
// Classes
for (ptree::value_type & v : el)
{
if (v.first == "<xmlattr>")
Expand All @@ -624,8 +637,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 != "<xmlattr>")
{
MjcfClass classDefault;
classDefault.className = "mujoco_default";
classDefault.classElement = el;
mapOfClasses.insert(std::make_pair("mujoco_default", classDefault));
}
}
}

Expand Down Expand Up @@ -813,7 +833,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"));
Expand Down Expand Up @@ -903,7 +923,9 @@ namespace pinocchio

// Add armature info
JointIndex j_id = urdfVisitor.getJointId(joint.jointName);
urdfVisitor.model.armature.segment(urdfVisitor.model.joints[j_id].idx_v(), urdfVisitor.model.joints[j_id].nv()) = 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)
Expand Down Expand Up @@ -1012,7 +1034,9 @@ namespace pinocchio
FrameIndex jointFrameId = urdfVisitor.model.addJointFrame(joint_id, (int)parentFrameId);
urdfVisitor.appendBodyToJoint(jointFrameId, inert, bodyInJoint, nameOfBody);

urdfVisitor.model.armature.segment(urdfVisitor.model.joints[joint_id].idx_v(), urdfVisitor.model.joints[joint_id].nv()) = 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();
Expand Down
13 changes: 8 additions & 5 deletions unittest/mjcf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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<std::string, pt::ptree> TrueMap;

Expand Down Expand Up @@ -978,17 +978,20 @@ BOOST_AUTO_TEST_CASE(armature)
{
typedef pinocchio::SE3::Vector3 Vector3;
typedef pinocchio::SE3::Matrix3 Matrix3;

std::cout << " Armature ------------ " << std::endl;
std::istringstream xmlData(R"(
<mujoco model="model_RX">
<default>
<joint armature="1" damping="1" limited="true"/>
</default>
<worldbody>
<body name="link0">
<body name="link1" pos="0 0 0">
<joint name="joint1" type="hinge" axis="1 0 0" armature="1.3"/>
<joint name="joint2" type="hinge" axis="0 1 0" armature="2.4"/>
<joint name="joint3" type="hinge" axis="0 0 1" armature="0.4"/>
<body pos=".2 0 0" name="body2">
<joint type="ball" armature=".1"/>
<joint type="ball"/>
</body>
</body>
</body>
Expand All @@ -1006,8 +1009,8 @@ BOOST_AUTO_TEST_CASE(armature)
graph.parseRootTree();

Eigen::VectorXd armature_real(model_m.nv);
armature_real << 1.3, 2.4, 0.4, 0.1, 0.1, 0.1;
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]);
}
Expand Down

0 comments on commit d5e502b

Please sign in to comment.