Hi @karthyyy, can you try removing the quaternion at the base here and let me know if that solves your problem?

Hi @kevinzakka,

Thanks for the response. I have removed the same but the issue persists. I will explain in detail. Following are the results with "quat" removed from the base.

My joint angles are [3pi/2, -pi/2, pi/2, 3pi/2, 3*pi/2, 0] as shown below

1. The transformation matrix between world frame and base frame in MuJoCo is Identity matrix as expected.

2. The first transformation matrix (T_01, Joint-1 frame with respect to Base frame/0) from UR excel file is

3. In MuJoCo, the Rmatrix/Quaternion etc are with reference to world frame. In this case world frame and base frame are same. So the first transformation matrix (T_W1) from MuJoCo is given as follows. If we see the rotation matrix portion, this is the rotation matrix for 3*pi/2 (or -pi/2) as per rotation matrix definition. So its correct. But different from that got with D-H parameters

The code I used for estimating transformation matrix in MuJoCo is given as

### World to Joint-1

T_W1_sim_pos   =  data.body('shoulder_link').xpos.copy()
T_W1_sim_quat  =  data.body('shoulder_link').xquat.copy()
T_W1_sim_xmat  =  data.body('shoulder_link').xmat.copy()

r = R.from_quat([T_W1_sim_quat[1], T_W1_sim_quat[2], T_W1_sim_quat[3], T_W1_sim_quat[0]])
print(r.as_euler('xyz', degrees=False))

T_W1_mj = Matrix(np.eye(4))
T_W1_mj[:3,:3] = TR10i(T_W1_sim_xmat.reshape(3,3))
T_W1_mj[:3, 3] = T_W1_sim_pos
print(T_W1_mj)

I understand that the transformation matrix is different for Standard D-H and Modified D-H Parameters. U-R excel file follows Standard D-H parameters. There is an online literature analyzing UR5 with modified D-H parameters. If we take modified D-H parameters, still the transformation matrices are different from MuJoCo model.

I believe for D-H parameters, the joint rotation axis is always 'Z' but in the xml file the default joint axis is 'Y'. What may be the reason ?

Since I am relatively new to this, I believe I am wrong somewhere but couldn't figure it out. Any help will be much appreciated.

Regards,
Karthik

I do not have a solution, but a similar problem in my issue #85.

Hi @karthyyy, could you let me know if the code example I posted in #85 is helpful?

Hi @kevinzakka,

I had tried Standard and Modified D-H parameters for UR5e but was not successful. So I manually extracted the transformation matrices of each joints and found the cumulative matrix. The method now holds good for FK, IK and even Jacobian computation and served my purpose.

I will try the code mentioned in the other thread for UR5e and get back, as it gives a standard method which can be extended to other manipulators.

Regards
Karthik

Going to assume this has been resolved. Feel free to re-open if you encounter further issues.

@kevinzakka sorry to reopen this but I was wondering why this rotation is included in the base body?

The base frame of the mujoco model is now not the actual base frame as used by the UR controller I think. (Implying that for a joint configuration, the pose obtained from the controlbox differs from the one obtained in mujoco).

The original URDF file states that a rotation around the z axis over -pi was needed to convert from the ROS frame convention (the base element in the URDF) to the UR controlbox (the base_link element in the URDF)

I think it would be easier if the base frame of the xml is consistent with the base frame of the robot controlbox?

This would be solved by setting the quat orientation of the base body to 0,0,0,-1.

Following code snippet confirms this:

   # check robot pose -joint pairs correspond to IK 
    robot = robot_descriptions.ur5e_mj_description.MJCF_PATH
    robot = mjcf.from_path(robot)

    robot.find("body","base").quat = [0,0,0,-1]
    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}")

    import ur_analytic_ik
    FK_pose = ur_analytic_ik.ur5e.forward_kinematics(*joint_config)
    print(f"forward kinematics pose: \r\n {FK_pose}")

ur_analytic_ik has been tested to ensure it's base frame matches the controlbox.

Hey @tlpss, agreed we should change it just to consistent.

@kevinzakka, I can make a PR if you want

Go for it!

I changed the orientations here.

Do you want to add a site/body that corresponds to the frame of the controlbox as well? because now the base body 's frame does not correspond to the 'base frame' of the controlbox. (hence the additional 'base_link' in the URDF)