Skip to content

fix(mjcf): propagate fixed-base root body pos/quat and inertia #2934

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

Open
isaka1022 wants to merge 2 commits into
base: devel
Choose a base branch
from
+81 −4
Open

fix(mjcf): propagate fixed-base root body pos/quat and inertia #2934

Show file tree
Hide file tree
Changes from 1 commit
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
345db0f
fix(mjcf): propagate fixed-base root body pos/quat and inertia
isaka1022 Aug 16, 2026
3e9da4d
fix(mjcf): place jointless body sites in their supporting joint
isaka1022 Aug 16, 2026
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
3 changes: 3 additions & 0 deletions CHANGELOG.md
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -14,6 +14,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
- Add `DataTpl::lastChild` deprecation notice in Python binding
- Fix `addFrame` to ignore frame without inertial to preserse parent body's CoM

### Fixed
- Fix MJCF parser dropping the `pos`/`quat` of a fixed base root body, which offset the whole kinematic tree and left its inertia at the origin

## [4.1.0] - 2026-07-07

### Added
Expand Down
12 changes: 12 additions & 0 deletions include/pinocchio/src/parsers/mjcf/mjcf-graph.hxx
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -39,11 +39,15 @@ namespace pinocchio
void addRootJoint(
const Inertia & Y,
const std::string & body_name,
const SE3 & body_placement,
Eigen::VectorXd & reference_config,
Eigen::VectorXd & qpos0,
const boost::optional<const JointModel &> root_joint,
const boost::optional<const std::string &> root_joint_name)
{
// Base::addRootJoint folds Y into the universe inertia untransformed.
const Inertia universe_inertia_before = Base::model.inertias[0];

Base::addRootJoint(Y, body_name, root_joint, root_joint_name);

if (root_joint.has_value())
Expand Down Expand Up @@ -82,6 +86,14 @@ namespace pinocchio
}
}
}
else
{
// Unlike URDF, MJCF lets the fixed root body carry its own pos/quat, which
// Base::addRootJoint ignores for both the body frame and the universe inertia.
const FrameIndex bodyFrameId = Base::model.getFrameId(body_name, BODY);
Base::model.inertias[0] = universe_inertia_before + body_placement.act(Y);
Base::model.frames[bodyFrameId].placement = body_placement;
}
}
};

Expand Down
3 changes: 2 additions & 1 deletion src/parsers/mjcf/mjcf-graph.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1472,7 +1472,8 @@ namespace pinocchio
// We only add the root joint if we have a fixed base
// (first body doesn't have any joint). Otherwise, the root joint is ignored.
mjcfVisitor.addRootJoint(
rootBody.bodyInertia, rootLinkName, referenceConfig, qpos0, rootJoint, rootJointName);
rootBody.bodyInertia, rootLinkName, rootBody.bodyPlacement, referenceConfig, qpos0,
rootJoint, rootJointName);
}
else
{
Expand Down
50 changes: 50 additions & 0 deletions unittest/mjcf.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1189,6 +1189,56 @@ BOOST_AUTO_TEST_CASE(build_model_no_root_joint)
BOOST_CHECK_EQUAL(model_m.nq, 29);
}

/// @brief test that a fixed-base root body's own pos/quat is not dropped,
/// and that it is correctly propagated to its children (regression test for #2782)
/// @param
BOOST_AUTO_TEST_CASE(build_model_fixed_base_root_body_placement)
{
typedef pinocchio::SE3::Vector3 Vector3;
typedef pinocchio::SE3::Matrix3 Matrix3;

std::istringstream xmlData(R"(
<mujoco model="fixed_base_test">
<worldbody>
<body name="base" pos="1 2 3" quat="0.7071068 0 0 0.7071068">
<geom type="box" size="0.1 0.1 0.1"/>
<body name="link1" pos="0 0 0.5">
<joint name="j1" type="hinge" axis="0 1 0"/>
<geom type="box" size="0.05 0.05 0.2"/>
</body>
</body>
</worldbody>
</mujoco>)");

auto namefile = createTempFile(xmlData);

pinocchio::Model model_m;
pinocchio::mjcf::buildModel(namefile.name(), model_m);

pinocchio::Data data(model_m);
pinocchio::framesForwardKinematics(model_m, data, pinocchio::neutral(model_m));

Matrix3 rotation_matrix;
rotation_matrix << 0., -1., 0., 1., 0., 0., 0., 0., 1.;

const pinocchio::SE3 expected_base(rotation_matrix, Vector3(1., 2., 3.));
const pinocchio::SE3 expected_link1(rotation_matrix, Vector3(1., 2., 3.5));

const pinocchio::FrameIndex baseFrameId = model_m.getFrameId("base", pinocchio::BODY);
const pinocchio::FrameIndex link1FrameId = model_m.getFrameId("link1", pinocchio::BODY);

BOOST_CHECK(data.oMf[baseFrameId].isApprox(expected_base, 1e-6));
BOOST_CHECK(data.oMf[link1FrameId].isApprox(expected_link1, 1e-6));

// The base geom has no offset of its own, so its inertia must be carried by
// the exact same placement as the base frame.
const double massBase = 1000 * 0.2 * 0.2 * 0.2; // density * volume
const pinocchio::Inertia expected_universe_inertia =
expected_base.act(pinocchio::Inertia::FromBox(massBase, 0.2, 0.2, 0.2));

BOOST_CHECK(model_m.inertias[0].isApprox(expected_universe_inertia, 1e-6));
}

double degreesToRadian(double degrees)
{
return degrees * (M_PI / 180.0);
Expand Down