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

Jump to bottom

MJCF parse site #2534

Open
1190201119 opened this issue Dec 30, 2024 · 1 comment
Open

MJCF parse site #2534

1190201119 opened this issue Dec 30, 2024 · 1 comment
Assignees
@MegMll

Comments

@1190201119
Copy link

Bug description

pinocchio parse site element of MJCF file unexpectedly. The parent joint of site frame parsed as its parent body's parent body's parent joint, instead of the parent's body's parent joint.

Expected behavior

A site's frame's parent joint should be the joint of the body the site belongs to.

Reproduction steps

run the scripts below:

import pinocchio as pin

mjcf_str = """
<mujoco model="site_example">
  <worldbody>

    <body name="base_link" pos="1.0 0 0">
      <freejoint/>
      <site name="mysite" pos="1.0 0 0"/>
    </body>
  </worldbody>
</mujoco>
"""
with open("/tmp/site_example.xml", "w") as f:
    f.write(mjcf_str)

model, collision_model, visual_model = pin.buildModelsFromMJCF("/tmp/site_example.xml")
data = model.createData()

q = pin.neutral(model)
pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
print(model.frames[model.getFrameId("mysite")])
print("mysite placement", data.oMf[model.getFrameId("mysite")])

output:

Frame name: mysite paired to (parent joint/ parent frame)(0/3)
with relative placement wrt parent joint:
  R =
1 0 0
0 1 0
0 0 1
  p = 1 0 0
containing inertia:
  m = 0
  c = 0 0 0
  I = 
0 0 0
0 0 0
0 0 0

mysite placement   R =
1 0 0
0 1 0
0 0 1
  p = 1 0 0

the parent joint of mysite is universe instead of base_link_free, resulting in incorrect forward kinematics.
the pos of mysite should be (2., 0, 0) instead of (1., 0, 0).

System

  • Pinocchio version: 3.3.1
@jcarpent
Copy link
Contributor

@MegMll Could you handle this issue?

@MegMll MegMll mentioned this issue Jan 6, 2025
Open
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Assignees

@MegMll MegMll

Labels
None yet
Projects
None yet
Milestone
No milestone
Development

No branches or pull requests
3 participants
@jcarpent @MegMll @1190201119