robotics-toolbox-python icon indicating copy to clipboard operation
robotics-toolbox-python copied to clipboard

solutions from ikine_LM are not correct when a tool is added in a new robot

Open paul218 opened this issue 1 year ago • 0 comments

When I try to find all possible pose configurations at a specific point, the Ik solvers return some incorrect values .

I modified the DH-parameters class to create a new robot UR5e, additionally, I added the tool to the D-H matrix. The robot and the tool are the next picture

robot_and_tool

Then, the D-H matrix are the following

ur5e = DHRobot(
    [
        RevoluteDH(d=0.1625, a=0, alpha=pi/2),
        RevoluteDH(d=0, a=-0.425, alpha=0),
        RevoluteDH(d=0, a=-0.3922, alpha=0),
        RevoluteDH(d=0.1333, a=0, alpha=pi/2),
        RevoluteDH(d=0.0997, a=0, alpha=-pi/2),
        RevoluteDH(d=0.0996, a=0, alpha=0),
        RevoluteDH(d=0.0687, a=0.125, alpha=0)#tool 3
    ], name="ur5e_tool_3")

I use the next point position to find all the pose configurations.

position = [6.21, -103.14, -126.96, -41.06, 90.71, 8.21, 0]
position = [np.deg2rad(x) for x in position]
T_tool = ur5e.fkine(position)  # forward kinematics

now I iterated 100 times to find different solutions from ikine_LM and store in a list 'sol_list_deg'

sol_list_deg = []
sol_list_rad = []

for i in range(1,100):
    sol = ur5e.ikine_LM(T_tool)   
    if sol.success:
        sol_d = [round(np.rad2deg(x), 0) for x in list(sol.q)]
        sol_list_deg.append(sol_d) 
        sol_rad = [round(x, 0) for x in list(sol.q)]
        sol_list_rad.append(sol_rad)

then I delete the duplicate solutions

k_deg = sol_list_deg
k_deg.sort()
total_sol_deg = list(k_deg for k_deg,_ in itertools.groupby(k_deg))

Finally, check the solutions (values are in degrees) image

Here, as you can see the solutions of joint 1,2,3,4,5 are the same value, and the solutions of joint 6 and tool are different. If I checked the joint values into another software like Robodk some solutions to reach the position point are not correct. How can I correct this? Are there another parameters to set up in ikine_LM to get the correct values?

Version information I am using the latest version of the robotics-toolbox-python installed with pip

paul218 avatar Mar 19 '24 15:03 paul218