Bug in inverse kinematics from v1.1.0

[asparc]

Hi!

I'm trying to compute the inverse kinematics of a very simple robot (3d stage + spherical joint) with the robotics toolbox, but I'm seeing errors in the inverse kinematics.

Conceptually, I'm verifying the inverse kinematics as follows:

q_in = [...]
T_in = robot.fkine(q_in)
q_out = robot.ikine(T_in)
T_out = robot.fkine(q_out)
assert T_out == T_in

If this assertion fails (while the inverse kinematics report success), I would think there is a bug in the inverse kinematics.

For certain poses of my simple robot, this assertion does fail royally on the translation component. I tested different versions of the robotics toolbox, and it seems that the problem was introduced in v1.1.0:

• For python=3.10 and roboticstoolbox-python=1.0.3: all is fine.
• For python=3.10 and roboticstoolbox-python=1.1.0: failing assertion.

To reproduce:

import math
import numpy as np
from roboticstoolbox import DHRobot, RevoluteDH, PrismaticDH

class MyRobot(DHRobot):
    def __init__(self):
        qlim_trans = np.array([-10, 10])
        qlim_rot = np.array([-1.5, 1.5])
        super().__init__(
            [
                PrismaticDH(theta=math.pi / 2, alpha=math.pi / 2, qlim=qlim_trans),
                PrismaticDH(theta=math.pi / 2, alpha=math.pi / 2, qlim=qlim_trans),
                PrismaticDH(qlim=qlim_trans),
                RevoluteDH(alpha=-math.pi / 2, qlim=qlim_rot),
                RevoluteDH(alpha=math.pi / 2, qlim=qlim_rot),
                RevoluteDH(qlim=qlim_rot),
            ],
            name="MyRobot",
        )

robot = MyRobot()

q_in = [4, 0, 0, 0, math.pi / 3, 0]
T_in = robot.fkine(q_in)

q_out = robot.ikine_LM(T_in).q
T_out = robot.fkine(q_out)

print("Joint angles in:  ", np.round(q_in, 2))
print("Joint angles out: ", np.round(q_out, 2))
print()
print("Translation in:  ", np.round(T_in.t, 2))
print("Translation out: ", np.round(T_out.t, 2))
print()

assert np.allclose(T_in, T_out)