Skip to content

Commit

Permalink
Merge pull request #2548 from MegMll/topic/fix_mjcf_site
Browse files Browse the repository at this point in the history 
Mjcf Parser : Fix adding the sites frames
  • Loading branch information
@jorisv
jorisv authored Jan 10, 2025
2 parents 0f1d998 + faae757 commit 6acfc6b
Show file tree
Hide file tree
Showing 3 changed files with 45 additions and 26 deletions.
5 changes: 4 additions & 1 deletion CHANGELOG.md
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down
61 changes: 37 additions & 24 deletions src/parsers/mjcf/mjcf-graph.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -59,13 +59,15 @@ 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");
}
}

Expand Down Expand Up @@ -172,7 +174,8 @@ namespace pinocchio
{

if (!use_limits && el.get_optional<std::string>("<xmlattr>.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<std::string>("<xmlattr>.type");
Expand Down Expand Up @@ -227,7 +230,8 @@ 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.");
}
}
Expand Down Expand Up @@ -358,7 +362,8 @@ namespace pinocchio
double mass = std::max(el.get<double>("<xmlattr>.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<std::string>("<xmlattr>.pos");
Expand Down Expand Up @@ -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"))
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)
Expand Down Expand Up @@ -524,7 +530,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
{
Expand Down Expand Up @@ -553,7 +559,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())
Expand Down Expand Up @@ -644,7 +650,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");
}
}

Expand All @@ -666,7 +672,8 @@ 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);
Expand Down Expand Up @@ -735,8 +742,10 @@ namespace pinocchio
{
std::string eulerseq = *eulerS;
if (eulerseq.find_first_not_of("xyzXYZ") != std::string::npos || eulerseq.size() != 3)
throw 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
{
// get index combination
Expand Down Expand Up @@ -764,7 +773,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;
}
}
Expand Down Expand Up @@ -817,7 +826,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<std::string>("<xmlattr>.body2");
Expand Down Expand Up @@ -846,7 +855,8 @@ 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)
{
Expand All @@ -857,7 +867,8 @@ 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")
Expand Down Expand Up @@ -945,7 +956,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,
Expand Down Expand Up @@ -1017,7 +1028,8 @@ 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();
Expand Down Expand Up @@ -1052,7 +1064,8 @@ 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;
}
Expand All @@ -1070,12 +1083,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));
urdfVisitor.model.addFrame(
Frame(site.siteName, frame.parentJoint, bodyId, placement, OP_FRAME));
}
}

Expand Down Expand Up @@ -1151,7 +1165,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(
Expand Down Expand Up @@ -1188,7 +1202,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;
Expand Down
5 changes: 4 additions & 1 deletion unittest/mjcf.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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"(
<mujoco model="model_RX">
<default>
Expand Down Expand Up @@ -1120,6 +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);
}

/// @brief test that a fixed model is well parsed
Expand Down

0 comments on commit 6acfc6b

Please sign in to comment.