Dual Arm robot (YuMi robot) gives different solutions, when check using forward kinematics, gives different trajectories each time...

[pratik2394]

I am trying ik_LM and fkine functions to generate solutions and verify if the solution is correct.

below is my code...

import roboticstoolbox as rtb

robot = rtb.models.YuMi()

Tep_left = SE3(0.6, -0.2, 0.3) * SE3.Rx(0.2) # Desired pose for the left end-effector Tep_right = SE3(0.6, 0.2, 0.3) * SE3.Rx(-0.2) # Desired pose for the right end-effector

print(Tep_left) print(Tep_right)

sol_r = robot.ik_LM(Tep_right, end = 'r_gripper') # solve IK sol_l = robot.ik_LM(Tep_right, end = 'l_gripper') # solve IK

print(sol_r, sol_l)

q_pickup_r = sol_r[0] q_pickup_l = sol_l[0]

print(robot.fkine(q_pickup_r, 'r_gripper')) print(robot.fkine(q_pickup_l, 'l_gripper'))

qt_r = rtb.jtraj(robot.qr[:7], q_pickup_r, 50) qt_l = rtb.jtraj(robot.qr[7:], q_pickup_l, 50)

array = np.concatenate((qt_r.q, qt_l.q), axis = 1)

print(array.shape) """ robot.plot(array, backend='pyplot', movie='panda1.gif')

robot.plot(array) """

everytime I run, it gives me different answers for print(robot.fkine(q_pickup_l, 'l_gripper')) and for print(robot.fkine(q_pickup_r, 'r_gripper')) it is consistent. its same answer for 7/10. (1/10 -->nan).

I am finding solutions for both the arms separately and merging the solutions manually. (As I understand, robot.qr represents joint position and first 7 belongs to right hand and last 7 belongs to left hand.)

Can anyone explain me what I am doing wrong? or if there exists a straight forward method?

(Answers I get:

print(robot.fkine(q_pickup_r, 'r_gripper')) print(robot.fkine(q_pickup_l, 'l_gripper'))

-0.8138 0.1048 0.5716 0.5686
-0.342 0.7088 -0.617 -0.004967
-0.4698 -0.6976 -0.541 0.5136
0 0 0 1

0.02501 0.3248 0.9455 0.5114
-0.1655 0.9341 -0.3165 -0.04516
-0.9859 -0.1486 0.07712 0.3639
0 0 0 1

---

print(robot.fkine(q_pickup_r, 'r_gripper')) print(robot.fkine(q_pickup_l, 'l_gripper'))

0.9114 -0.02343 0.4107 -0.01951
0.2121 -0.8287 -0.518 -0.09416
0.3525 0.5592 -0.7503 0.2366
0 0 0 1

0.4833 0.5289 -0.6977 0.3742
-0.3731 0.8453 0.3824 -0.4569
0.792 0.07548 0.6059 0.8473
0 0 0 1

) I believe these should be same as print(Tep_left) print(Tep_right)

( 1 0 0 0.6
0 0.9801 -0.1987 -0.2
0 0.1987 0.9801 0.3
0 0 0 1

1 0 0 0.6
0 0.9801 0.1987 0.2
0 -0.1987 0.9801 0.3
0 0 0 1
)

Thank you in advance.