Forward Kinematics of UR5e

[karthyyy]

Hi,

I am simulating UR5e model from the repo. I could implement the Forward and Inverse Kinematics of the manipulator correctly as per the MuJoCo model and its reference frames. However, when trying to correlate the transformation matrix from the MuJoCo (formulated from the xmat and xpos values), with that generated from the D-H parameters as given in the following link, I am not getting identical results. The translation values are same but with the axis changed, whereas the rotation matrix cannot be correlated.

https://www.universal-robots.com/articles/ur/application-installation/dh-parameters-for-calculations-of-kinematics-and-dynamics/

It was found that the world frame and base frame are having a rotation. So, I included correction for the same but still I couldn't get identical transformation matrix from MuJoCo and that from the excel sheet in the UR site. (https://s3-eu-west-1.amazonaws.com/ur-support-site/45257/DH-Transformation.xlsx).

Can someone help me with the same ?

Regards Karthik

[kevinzakka]

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

[karthyyy]

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

[loerting]

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

[kevinzakka]

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

[karthyyy]

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