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.

[petercorke]

Thanks for the detailed report and repro — this led to a genuine bug hunt. Here's what we found:

Real bug #1: YuMi's model had r_gripper/l_gripper wired to the wrong arm. In YuMi.py, the two gripper mount links were swapped — end='r_gripper' was silently solving/evaluating against the left arm's kinematic chain, and vice versa. That's almost certainly the core of what you're seeing: which arm you actually get for a given end= was mislabeled. Fixed in #649.

Real bug #2, found while fixing #1: the IK solvers' failure-return path had a separate bug where a failed solve could return a wrong-length joint vector instead of a properly-sized one, for certain arms — also fixed in #649.

One more thing in your own script, unrelated to the above: both ik_LM calls pass Tep_right:

sol_r = robot.ik_LM(Tep_right, end='r_gripper')
sol_l = robot.ik_LM(Tep_right, end='l_gripper')   # <- probably meant Tep_left here

Your two target poses also aren't equally reachable, independent of any bug. With the fixes above applied, I tested Tep_left = SE3(0.6, -0.2, 0.3) * SE3.Rx(0.2) directly (200 random restarts, joint limits disabled even) and it never converges below a residual of ~0.001 — it appears to be outside l_gripper's reachable workspace, or right at its edge. SE3.Rx(0.2)/SE3.Rx(-0.2) is just a small rotation from identity, which likely isn't a natural grasp-facing orientation for YuMi's tool frame, and a position that's fine for one orientation can be unreachable for another.

To confirm the actual fixes work correctly end-to-end, I re-ran your script's structure with two reachable targets instead (closer in and on each arm's own side):

Tep_left  = SE3(0.4,  0.2, 0.3) * SE3.Rx(0.2)
Tep_right = SE3(0.4, -0.2, 0.3) * SE3.Rx(-0.2)

Both converge cleanly now (success=True, residuals ~1e-9 to 1e-12), and robot.fkine(q, end=...) matches each target correctly for the correct arm.

One usage note that matters right now, before a further fix lands: robot.fkine(q, end=...) on a branched robot like YuMi expects q indexed across the whole robot (length 14: 7 right-arm joints + 7 left-arm joints), not just the 7 values IK returns for one arm's sub-chain. So rather than robot.fkine(q_pickup_l, 'l_gripper'), build a full-length vector first:

q_full = robot.qr.copy()
q_full[7:] = q_pickup_l
robot.fkine(q_full, end='l_gripper')

We're also working on making fkine accept the shorter per-arm vector directly (tracked separately), which will make this more forgiving — will update this thread when that lands.

#649 is merged now. Could you re-run your repro (ideally with reachable target poses for both arms) and let us know if the results are consistent?

[petercorke]

Update: the fkine/jacob0 compact-q issue mentioned above now has a fix open — #653. Once that merges, you'll be able to pass IK's per-arm solution straight into fkine/jacob0 without the manual q_full workaround I described earlier — e.g. robot.fkine(sol.q, end='l_gripper') will just work directly.

Status of all the fixes from this thread:

• fix(models,ik): correct YuMi gripper parents and IK failure-path q compaction #649 (gripper mislabeling + IK failure-path bug) — merged
• fix(examples,dhrobot): repair broken ikine_LMS/ikine_min example references #650 (broken ikine_LMS example references) — merged
• fix(ik): unify ik_XX/ikine_XX return types and fix IK docs #652 (ik_LM/ikine_LM return-type consistency + doc fixes) — merged
• feat(ets): fkine/jacob0/jacobe/hessian0/hessiane accept compact q #653 (fkine/jacob0 compact-q support) — open, pending review

Will follow up here once #653 merges too.