Skip to content

Conversation

@tlpss
Copy link
Contributor

@tlpss tlpss commented Dec 20, 2024

This PR changes the orientation of the base body of the UR robots to match the frame of the robot with the frame used by a real robot controlbox. See the original URDF file for reference, where they also added a separate link that corresponds to the real robot base frame.

This ensures that a given joint configuration corresponds to the same pose for a real robot (expressed in the base frame) and a simulated robot (expressed in the 'attachment site' / world frame). This avoids confusion and allows to use real world trajectories/IK tools/... in the simulator.

This PR closes #75.

One thing to note is that there is currently no site/body within the robot model that corresponds to the actual base frame. I'm not sure if there is a convention for this within the repository? (@kevinzakka).

Following code snippet can be used to check the poses corresponding to a joint configuration match for a real robot and simulated robot:

    import robot_descriptions
   from dm_control import mjcf

    robot = robot_descriptions.ur5e_mj_description.MJCF_PATH
    robot = mjcf.from_path(robot)
    physics = mjcf.Physics.from_mjcf_model(robot)

    joint_config = np.zeros(6)
    physics.bind(robot.find_all("joint")).qpos = joint_config
    
    # get attachment site pose
    attachment_site = robot.find("site", "attachment_site")
    attachment_site_pose = physics.bind(attachment_site).xpos
    attachment_site_orientation = physics.bind(attachment_site).xmat.reshape(3,3)
    pose = np.eye(4)
    pose[:3, :3] = attachment_site_orientation
    pose[:3, 3] = attachment_site_pose
    print(f"attachment site pose: \r\n {pose}")

    # get real robot pose for same joint config
    # ur_analytic_ik has been validated to match the base frame used by real robot controlboxes
    import ur_analytic_ik
    FK_pose = ur_analytic_ik.ur5e.forward_kinematics(*joint_config)
    print(f"forward kinematics pose: \r\n {FK_pose}")

@google-cla
Copy link

google-cla bot commented Dec 20, 2024

Thanks for your pull request! It looks like this may be your first contribution to a Google open source project. Before we can look at your pull request, you'll need to sign a Contributor License Agreement (CLA).

View this failed invocation of the CLA check for more information.

For the most up to date status, view the checks section at the bottom of the pull request.

@tlpss
Copy link
Contributor Author

tlpss commented Jan 6, 2025

@kevinzakka , I have signed the CLA
Is there anything else blocking this PR?

@copybara-service copybara-service bot merged commit 832d908 into google-deepmind:main Jan 10, 2025
3 checks passed
donghoon11 pushed a commit to donghoon11/mujoco_menagerie that referenced this pull request May 7, 2025
PiperOrigin-RevId: 713997576
Change-Id: Ic75b71f3cccbcce3b9cf6c67543309dd26dc13be
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Labels

None yet

Projects

None yet

Development

Successfully merging this pull request may close these issues.

Forward Kinematics of UR5e

2 participants