Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Bug in inverse kinematics from v1.1.0 #457

Open
asparc opened this issue Sep 27, 2024 · 0 comments
Open

Bug in inverse kinematics from v1.1.0 #457

asparc opened this issue Sep 27, 2024 · 0 comments

Comments

@asparc
Copy link

asparc commented Sep 27, 2024

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)
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

1 participant