petercorke / robotics-toolbox-python

Robotics Toolbox for Python
MIT License
2.18k stars 454 forks source link

For the same pose T, EROBOT.ikine_LM solve failed, but DHRobot.ikine_LM can solve #440

Closed beta1scat closed 6 months ago

beta1scat commented 6 months ago

Describe the bug For the same pose T, EROBOT.ikine_LM solve failed, but DHRobot.ikine_LM can solve

Version information 1.1.0, 1.1.1 install from PyPI

To Reproduce Rokae XB25 DH:

dh_params = [
    {"a": 0, "alpha": 0, "offset": 0, "d": 490/1000},
    {"a": 160/1000, "alpha": -pi/2, "offset": -pi/2, "d": 0},
    {"a": 780/1000, "alpha": 0, "offset": 0, "d": 0},
    {"a": 150/1000, "alpha": -pi/2, "offset": 0, "d": 660/1000},
    {"a": 0, "alpha": pi/2, "offset": 0, "d": 0},
    {"a": 0, "alpha": -pi/2, "offset": pi, "d": 110/1000},
]

urdf: xb25.txt

T = SE3.Rt(SO3.Ry(pi), [0.2834, 0.2788, 0.3])

Expected behavior Two ikine method return:

IKSolution: q=[-2.364, -2.723, 1.012, -1.199e-05, -3.002, -2.364], success=False, reason=iteration and search limit reached, solution found but violates joint limits, iterations=1620, searches=100, residual=0
IKSolution: q=[0.7772, 0.9013, 1.479, 0, -0.8091, 0.7772], success=True, iterations=9, searches=1, residual=5.53e-10

Environment (please complete the following information): ubuntu 20.04 python 3.10

Partial code

class xb25:
    def __init__(self):
        self.dh_params = [
            {"a": 0, "alpha": 0, "offset": 0, "d": 490/1000},
            {"a": 160/1000, "alpha": -pi/2, "offset": -pi/2, "d": 0},
            {"a": 780/1000, "alpha": 0, "offset": 0, "d": 0},
            {"a": 150/1000, "alpha": -pi/2, "offset": 0, "d": 660/1000},
            {"a": 0, "alpha": pi/2, "offset": 0, "d": 0},
            {"a": 0, "alpha": -pi/2, "offset": pi, "d": 110/1000},
        ]
        self.links = []
        for i, params in enumerate(self.dh_params):
            link = RevoluteMDH(**params)
            self.links.append(link)
        self.rtbRobot = DHRobot(self.links)

class XB25(ERobot):
    def __init__(self):
        links, name, urdf_string, urdf_filepath = self.URDF_read(
            "xb25.urdf", tld=XB25.load_path()
        )
        super().__init__(
            links,
            name=name,
            manufacturer="Rokae",
            gripper_links=links[7],
            urdf_string=urdf_string,
            urdf_filepath=urdf_filepath,
        )

T = SE3.Rt(SO3.Ry(pi), [0.2834, 0.2788, 0.3])
q = [0,pi/3,0,0,pi/2,0]
robot = XB25()
rob = xb25()
print(robot.ikine_LM(T, q0=q))
print(rob.rtbRobot.ikine_LM(T, q0=q))
beta1scat commented 6 months ago

It's my mistake, this problem is cause by joint limit.