robotics-toolbox-python
robotics-toolbox-python copied to clipboard
solutions from ikine_LM are not correct when a tool is added in a new robot
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
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)
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