Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Mjcf Parser : Fix adding the sites frames #2548

Merged
merged 7 commits into from
Jan 10, 2025
Merged

Mjcf Parser : Fix adding the sites frames #2548

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
7 commits
Select commit Hold shift + click to select a range
d2e8b25
[mjcf] Add unittest for parentJoint of sites
Jan 6, 2025
189c144
[mjcf] Use the frame of the parent body of the sites
Jan 6, 2025
06ef4eb
[mjcf] replace with throw pretty
Jan 6, 2025
9ae4a1f
[Changelog] Add Entry
Jan 6, 2025
d320b16
[Mjcf] Remove trailing white space
MegMll Jan 10, 2025
3ea4df3
[Mjcf] Do not store frame
MegMll Jan 10, 2025
faae757
[pre-commit.ci] auto fixes from pre-commit.com hooks
pre-commit-ci[bot] Jan 10, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
5 changes: 4 additions & 1 deletion CHANGELOG.md
View file
Open in desktop
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
Open in desktop
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
Open in desktop
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
Loading