Forward Kinematics with DH parameters

[caioconti]

I'm trying to solve a simple forward kinematics problems. I desire to compute the position of the end-effector of a manipulator using the Denavit-Hartenberg parameters extracted by coppeliaSim. However, my code does not retrieve the correct (x,y,z) position, and I'm not sure what I'm doing wrong.

Using the UR5, and extracting DH parameters from a dummy located in the last link (0,0,0 relative to last link), outputs:
Denavit-Hartenberg parameters (classic DH convention):
- between joint '/joint{0}' and joint '/joint{1}': d=0.0661 [m], theta=90.0 [deg], r=0.0000 [m], alpha=-90.0 [deg]
- between joint '/joint{1}' and joint '/joint{2}': d=0.0000 [m], theta=0.0 [deg], r=0.4251 [m], alpha=0.0 [deg]
- between joint '/joint{2}' and joint '/joint{3}': d=0.0000 [m], theta=0.0 [deg], r=0.3922 [m], alpha=0.0 [deg]
- between joint '/joint{3}' and joint '/joint{4}': d=0.0397 [m], theta=-90.0 [deg], r=0.0000 [m], alpha=-90.0 [deg]
- between joint '/joint{4}' and joint '/joint{5}': d=0.0492 [m], theta=90.0 [deg], r=0.0000 [m], alpha=-90.0 [deg]
- between joint '/joint{5}' and object '/TipUR5': d=0.0000 [m], theta=-0.0 [deg], r=0.0000 [m], alpha=0.0 [deg]

Then, I'm using the following code to compute the position of /TipUR5:

Code: Select all

# Classic DH Transformation Matrix
def dh(theta, d, r, alpha):
    ct, st = np.cos(theta), np.sin(theta)
    ca, sa = np.cos(alpha), np.sin(alpha)

    return np.array([
        [ct, -st * ca,  st * sa, r * ct],
        [st,  ct * ca, -ct * sa, r * st],
        [0,       sa,      ca,     d],
        [0,        0,       0,     1]
    ])
    
# DH parameters (theta, d, r, alpha)
dh_params = [
    (np.deg2rad(90),   0.0661, 0.0,     -np.deg2rad(90)),
    (0.0,              0.0,    0.4251,  np.deg2rad(0)),
    (0.0,              0.0,    0.3922,  np.deg2rad(0)),
    (-np.deg2rad(90),  0.0397, 0.0,     -np.deg2rad(90)),
    (np.deg2rad(90),   0.0492, 0.0,     -np.deg2rad(90)),
    (0.0,              0.0,    0.0,     np.deg2rad(0))
]

# Compute the full transformation matrix
FK = np.eye(4)
for param in dh_params:
    theta, d, r, alpha = param
    tj = dh(theta, d, r, alpha)
    FK = np.dot(FK, tj)

position = FK[:3, 3]

The robot is placed at (0,0,0) and no joints have been moved. The FK results for the DH computation compared to the sim.getObjectPosition(tipUR5,joint0) and sim.getObjectPosition(tipUR5,sim.handle_world) are:

FK result by DH: [-0.04 0.866 0.066]
Sim (relative to joint): [-0.172 -0. 0.978]
Sim (relative to World): [-0.172 -0. 0.987]

My result is far from the true position, what am I missing?

[yeah]

I have the same problem. May I ask how you obtained the DH parameters of the UR5 in CoppeliaSim? I tried to write the forward and inverse kinematics myself, and they can verify each other now. However, when I apply them in CoppeliaSim, there is a significant difference. I don't know why. Could it be that the theta values in the DH parameters of the UR5 in CoppeliaSim are different from those found on the UR official website?

Code: Select all

import numpy as np
# UR10的DH参数表
ur10_dh_params = [
    # a      alpha     d       theta
    [0.0   , np.pi/2,  0.1273, -np.pi/2],    # Joint 1 
    [0.612 , 0      ,  0     , np.pi/2],    # Joint 2 
    [0.5723, 0      ,  0     , 0],    # Joint 3
    [0.0   , np.pi/2,  0.1639, np.pi/2],    # Joint 4  
    [0.0   ,-np.pi/2, 0.1157,  0],    # Joint 5
    [0.0   , 0      , 0.0922,  0]     # Joint 6 
]

# DH变换矩阵
def dh_transform(a, alpha, d, theta):
    return np.array([
        [np.cos(theta), -np.sin(theta) * np.cos(alpha), np.sin(theta) * np.sin(alpha), a * np.cos(theta)],
        [np.sin(theta), np.cos(theta) * np.cos(alpha), -np.cos(theta) * np.sin(alpha), a * np.sin(theta)],
        [0, np.sin(alpha), np.cos(alpha), d],
        [0, 0, 0, 1]
    ])

# 计算UR10正运动学
def FK(joint_angles):
    t = np.eye(4)  # 初始变换矩阵（单位矩阵）
    for i in range(6):
        a, alpha, d, theta = ur10_dh_params[i]
        theta=theta+joint_angles[i]  
        A = dh_transform(a, alpha, d, theta)
        t = np.dot(t, A)  # 累积变换矩阵
    return t

joint_angles = [0,np.pi/6, 0,np.pi/2, np.pi/4, 0]  

end_effector_transform = FK(joint_angles)
end_effector_transform=np.round(end_effector_transform,4)
print("末端执行器变换矩阵:")
print(end_effector_transform)

position = end_effector_transform[:3, 3]
print(f"末端执行器位置: x={position[0]:.3f}, y={position[1]:.3f}, z={position[2]:.3f}")

Code: Select all

import numpy as np
import math
# UR10的DH参数表
ur10_dh_params = [
    # a      alpha     d       theta
    [0.0   , np.pi/2,  0.1273, -np.pi/2],    # Joint 1 
    [0.612 , 0      ,  0     , np.pi/2],    # Joint 2 
    [0.5723, 0      ,  0     , 0],    # Joint 3
    [0.0   , np.pi/2,  0.1639, np.pi/2],    # Joint 4  
    [0.0   ,-np.pi/2, 0.1157,  0],    # Joint 5
    [0.0   , 0      , 0.0922,  0]     # Joint 6 
]
def IK(T):
    #获取相应的参数
    a1,alpha1,d1,theta1_off=ur10_dh_params[0]
    a2,alpha2,d2,theta2_off=ur10_dh_params[1]
    a3,alpha3,d3,theta3_off=ur10_dh_params[2]
    a4,alpha4,d4,theta4_off=ur10_dh_params[3]
    a5,alpha5,d5,theta5_off=ur10_dh_params[4]
    a6,alpha6,d6,theta6_off=ur10_dh_params[5]

    nx=T[0][0]
    ny=T[1][0]
    nz=T[2][0]
    ox=T[0][1]
    oy=T[1][1]
    oz=T[2][1]
    ax=T[0][2]
    ay=T[1][2]
    az=T[2][2]
    px=T[0][3]
    py=T[1][3]
    pz=T[2][3]
    solutions=[]
    #theta1
    m=py-ay*d6
    n=px-ax*d6
    phi=np.atan2(m,n)
    theta1_1=phi-np.atan2(-d2-d4,np.sqrt(np.pow(m,2)+np.pow(n,2)-pow(d2+d4,2)))
    theta1_2=phi-np.atan2(-d2-d4,-np.sqrt(np.pow(m,2)+np.pow(n,2)-pow(d2+d4,2)))

    #theta5
    for theta1 in [theta1_1,theta1_2]:
        theta5_1=np.acos(np.sin(theta1)*ax-np.cos(theta1)*ay)
        theta5_2=-theta5_1
        for theta5 in [theta5_1,theta5_2]:
            #theta6
            s=np.cos(theta1)*ny-np.sin(theta1)*nx
            t=np.cos(theta1)*oy-np.sin(theta1)*ox
            theta6=np.atan2(s,t)-np.atan2(-np.sin(theta5),0)
            #theta3
            r14=np.cos(theta1)*px+np.sin(theta1)*py-d6*(np.sin(theta1)*ay+np.cos(theta1)*ax)+d5*(np.cos(theta6)*(np.cos(theta1)*ox+np.sin(theta1)*oy)+np.sin(theta6)*(np.cos(theta1)*nx+np.sin(theta1)*ny))
            r34=pz-d1-az*d6+d5*(np.sin(theta6)*nz+np.cos(theta6)*oz)
            theta3_1=np.acos(np.clip((np.pow(r14,2)+np.pow(r34,2)-np.pow(a3,2)-np.pow(a2,2))/(2*a2*a3),-1.0,1.0))
            theta3_2=-theta3_1
            for theta3 in [theta3_1,theta3_2]:
                #theta2
                sin_theta2=((a3*np.cos(theta3)+a2)*r34-a3*np.sin(theta3)*r14)/(np.pow(a3,2)+np.pow(a2,2)+2*a2*a3*np.cos(theta3))
                cos_theta2=(a3*np.sin(theta3)*r34+(a3*np.cos(theta3)+a2)*r14)/(np.pow(a3,2)+np.pow(a2,2)+2*a2*a3*np.cos(theta3))
                theta2=np.atan2(sin_theta2,cos_theta2)
                #theta4
                sin_theta234=-np.sin(theta6)*(np.cos(theta1)*nx+np.sin(theta1)*ny)-np.cos(theta6)*(np.cos(theta1)*ox+np.sin(theta1)*oy)
                cos_theta234=np.sin(theta6)*nz+np.cos(theta6)*oz
                theta4=np.atan2(sin_theta234,cos_theta234)-theta2-theta3

                theta1_end=(theta1-theta1_off)%(2*np.pi)
                theta2_end=(theta2-theta2_off)%(2*np.pi)
                theta3_end=(theta3-theta3_off)%(2*np.pi)
                theta4_end=(theta4-theta4_off)%(2*np.pi)
                theta5_end=(theta5-theta5_off)%(2*np.pi)
                theta6_end=(theta6-theta6_off)%(2*np.pi)
                solutions.append([theta1_end,theta2_end,theta3_end,theta4_end,theta5_end,theta6_end])
    solutions=[row for row in solutions if not any(math.isnan(x) for x in row)]
    #归一化角度[-pi,pi]
    for i in range(len(solutions)):
        for j in range(len(solutions[i])):
            if solutions[i][j]>np.pi:
                solutions[i][j]=solutions[i][j]-2*np.pi
                       
    return solutions       
T=[
    [-0.7071,0,-0.7071,-0.2291],
    [-0.3536,-0.866,0.3536,0.7249],
    [-0.6124,0.5,0.6124,1.1515],
    [0,0,0,1]
]
print(IK(T))

The above is my code for forward and inverse kinematics. I have no idea how to correct the DH parameter table so that the values I calculate match the coordinates and postures in CoppeliaSim. Could a master help me out?

[caioconti]

To obtain the DH parameters via CoppeliaSim: select the desired object (usually the end-effector) then go to [Modules] -> [Kinematics] -> [Denavit-Hartenberg Extractor]. It will output the parameters in the text box. Let me know if you need additional help.

I found that yes, the CoppeliaSim DH table does not match the one from the official UR5 website, and they are not equivalent (same joint inputs give different outputs). I don't know why that is the case. However, It does not matter if I employ the CoppeliaSim DH parameters or the official parameters, neither matches the simulation results.

Hence, I still don't know how to correct the table or my code to retrieve the correct result yielded by the simulation. Any help would be much appreciated.

[yeah]

Thanks for your help!But after several days,I didn't solve this problem!I want know whether you have some progress.

[caioconti]

I've made some progress and wanted to share a few insights. It seems that the frames in the default UR model in CoppeliaSim are not correctly set up to extract the Denavit-Hartenberg (DH) parameters, they don't follow the standard conventions. As a result, you can't directly apply the DH method to the default model without first correcting the frames.

Furthermore, while the official UR documentation states that all theta parameters are zero and includes a reference figure, this can be misleading. It implies that all theta values of the figure are zero, but that’s not actually the case. If you compute the theta values manually, you'll notice that.

If I'm mistaken in my findings, I'd really appreciate any corrections or input! If you need more details feel free to reach out to me at caiocontig [at] gmail [dot] com.

[yeah]

I have sent an email via QQ, and I’m not sure if Google will classify the QQ email as spam. I hope you can check your mailbox status (sent from 3328785663@qq.com). If there are any issues, we can also discuss them on the forum.