Inaccurate end effector position when use inverse kinematic solution

[xf-zhao]

Hello, I refered Magnebot codes for robot control but ran into problems of inaccurate end_effector positions. As is in Magnebot, ikpy is used to calculate the inverse kinematic solution. I use it too, but for a ur5 arm. See codes below for details (some of the codes are also cherry-picked from tdw-transport-challenge).

from pathlib import Path
from typing import Dict, List, Optional, Union

import ikpy.chain
import numpy as np
from magnebot.joint_static import JointStatic
from magnebot.util import check_version, get_data
from tdw.controller import Controller
from tdw.librarian import ModelLibrarian, RobotLibrarian
from tdw.output_data import Bounds, Images, OutputData, Robot, StaticRobot
from tdw.robot_creator import RobotCreator
from tdw.tdw_utils import AudioUtils, TDWUtils

lib_path = "resources/robot_librarian.json"
if not Path(lib_path).exists():
    RobotLibrarian.create_library(path=lib_path, description="Custom Robot Librarian")
lib = RobotLibrarian(lib_path)
robot_name = "ur5"

robot_urdf_urls = {
    "ur5": "https://github.com/ros-industrial/robot_movement_interface/blob/master/dependencies/ur_description/urdf/ur5_robot.urdf.xacro",
}
robot_creator_base_path = "/Users/xufeng/robot_creator/Assets/robots/"
chain_urdf_files = {
    "ur5": robot_creator_base_path + "ur5_robot.urdf",
}
urdf_base_elements = {
    "ur5": ["base_link"],
}
if lib.get_record(robot_name) is None:
    r = RobotCreator()
    record = r.create_asset_bundles(
        urdf_url=robot_urdf_urls[robot_name],
    )
    lib.add_or_update_record(record=record, overwrite=False, write=True)

class SceneState:
    def __init__(self, resp: List[bytes], only_get_img=False):
        r = get_data(resp=resp, d_type=Robot)
        self.joint_positions: Dict[int, np.array] = dict()
        self.joint_angles: Dict[int, np.array] = dict()
        for i in range(r.get_num_joints()):
            j_id = r.get_joint_id(i)
            self.joint_positions[j_id] = r.get_joint_position(i)
            self.joint_angles[j_id] = r.get_joint_positions(i)

class RobotStatic:
    def __init__(
        self,
        static_robot: StaticRobot,
        chain_urdf_file=None,
        urdf_base_element=None,
    ):
        self.joints: Dict[int, JointStatic] = dict()
        self.joint_name_id: Dict[str, int] = dict()
        for i in range(static_robot.get_num_joints()):
            joint_id = static_robot.get_joint_id(i)
            # Cache the body parts.
            joint_static = JointStatic(sr=static_robot, index=i)
            self.joints[joint_id] = joint_static
            joint_name = joint_static.name
            self.joint_name_id[joint_name] = joint_id
        self.chain = (
            ikpy.chain.Chain.from_urdf_file(
                chain_urdf_file, base_elements=urdf_base_element
            )
            if chain_urdf_file is not None
            else None
        )

class Test(Controller):
    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.robot_id: int = 0

    def get_robot(self, resp: List[bytes]) -> Robot:
        robot: Optional[Robot] = None
        for i in range(len(resp) - 1):
            r_id = OutputData.get_data_type_id(resp[i])
            if r_id == "robo":
                r = Robot(resp[i])
                if r.get_id() == self.robot_id:
                    robot = r
                    break
        return robot

    def trial(self):
        def get_ik_commands(
            target_position, scene_state: SceneState, robot_static: RobotStatic
        ) -> np.array:
            def convert_for_ik(x, y, z):
                return z, -x, y

            angles = robot_static.chain.inverse_kinematics(
                convert_for_ik(*target_position),
            )
            angles = [float(np.rad2deg(angle)) for angle in angles]
            ik_commands = []
            for angle, joint_id in zip(angles, scene_state.joint_positions):
                if robot_static.joints[joint_id].joint_type != "revolute":
                    continue
                ik_commands.append(
                    {
                        "$type": "set_revolute_target",
                        "id": robot_id,
                        "joint_id": joint_id,
                        "target": angle,
                    }
                )
            return ik_commands

        commands = [
            TDWUtils.create_empty_room(12, 12),
            {"$type": "destroy_all_objects"},
            {"$type": "set_screen_size", "width": 800, "height": 800},
            {"$type": "set_target_framerate", "framerate": 100},
        ]
        commands.extend(
            TDWUtils.create_avatar(
                avatar_type="A_Img_Caps_Kinematic",
                position={"x": 0.5, "y": 1, "z": 0.5},
                look_at={"x": 0.3, "y": 0, "z": 0.3},
            )
        )
        resp = self.communicate(commands)
        robot_id = self.robot_id
        robot_commands = [
            self.get_add_robot(
                name=robot_name,
                robot_id=robot_id,
                library=lib_path,
            ),
            {"$type": "send_static_robots", "frequency": "once"},
            {"$type": "send_robots", "frequency": "always"},
        ]
        commands.extend(robot_commands)
        resp = self.communicate(commands)
        self.robot_static = RobotStatic(
            static_robot=get_data(resp=resp, d_type=StaticRobot),
            chain_urdf_file=chain_urdf_files[robot_name],
            urdf_base_element=urdf_base_elements[robot_name],
        )
        state_0 = SceneState(resp=resp)
        next_tip_position = [0.3, 0.3, 0.3]
        ik_commands = get_ik_commands(next_tip_position, state_0, self.robot_static)
        resp = self.communicate(ik_commands)
        # waiting
        for i in range(200):
            print(".", end="")
            resp = self.communicate([])
        state_1 = SceneState(resp=resp)
        tip_pos = state_1.joint_positions[self.robot_static.joint_name_id["ee_link"]]
        print(f"End effector position error: f{next_tip_position - tip_pos}.")

    def run(self):
        self.start()
        self.trial()
        self.communicate({"$type": "terminate"})

if __name__ == "__main__":
    Test().run()

Output:

........................................................................................................................................................................................................End effector position error: f[ 0.12968929 -0.0595466   0.03743686].

• Note that after 200 frames, the robotic arm is almost being static, but the position error between desired and real is quite large (while we always suppose a quick response and a small error like 1e-4).
• PS: I have coordinate mapping problems when directly utilizing ikpy, so I defined a function called convert_for_ik() to convert. According to the movement of the arm, it works well.

[alters-mit]

I'm impressed that you've managed to get this much working! For what it's worth, there's an upcoming major update to TDW (v1.9.0) that ports a lot of code from Magnebot to TDW and simplifies it. The update has a Robot "add-on" class with dynamic and static data and a separate "ObjectManager" scene-state class.

Regarding the chain inaccuracy: Can you attach the urdf file you're using? My initial guess is that either the file doesn't exactly match the robot, or that there's a conversion (such as the height of the shoulder link from the ground) that you're missing.

[xf-zhao]

@alters-mit Thank you for your quick response.

The update has a Robot "add-on" class with dynamic and static data and a separate "ObjectManager" scene-state class.

Really looking forward to seeing the next release. Thank you all for the great work.

The urdf file of ur5 robot was automatically downloaded by the robot_creator (if url is provided as "https://github.com/ros-industrial/robot_movement_interface/blob/master/dependencies/ur_description/urdf/ur5_robot.urdf.xacro"). I noticed that this ur5 is different from the built-in one. So I use robot_creator module to build all the asset stuff, and use the generated urdf file for IK calculation.

For debug purpose, you can also find the urdf file here: https://www.dropbox.com/s/djxm90mvwohswca/ur5_robot.urdf?dl=0

My initial guess is that either the file doesn't exactly match the robot, or that there's a conversion (such as the height of the shoulder link from the ground) that you're missing.

Actually, I tested another robot (OpenManipulatorPro from own building of assets rather than ur5 in the aforementioned codes) and it has the same inaccurate problems.

[alters-mit]

@xf-zhao I tried testing this and found a few problems:

• Our UR5 robot is actually missing a few joints that are in the urdf file. For the sake of fixing some bugs in Unity, we removed base_link and ee_link. The root object of the robot is shoulder_link and the end object is wrist_3_link.
• You aren't setting the initial position of the robot between motions. The initial position are where its joints are on this frame:

        angles = self.chain.inverse_kinematics(target_position=UR5._convert_for_ik(*target),
                                               initial_position=self._get_initial_angles())

• You're not setting the bounds or rotation parameters of the links. These might need to be converted.

[xf-zhao]

@alters-mit Hi, as far as I understand, the initial_position parameter should not affect the result much since the IK solution is just a mapping from xyz to joint space.

• However, I tried adding this parameter and re-run, but the condition did not turn better.
• Then I removed all the stuff related to IK and tested set_revolute function alone. I found that this function is not setting the joint angles as I want.

Codes here for detail:

from pathlib import Path
from ikpy.chain import Chain
from ikpy.link import OriginLink, URDFLink, Link
from typing import Dict, List, Optional, Union

import ikpy.chain
import numpy as np
from magnebot.joint_static import JointStatic
from magnebot.util import check_version, get_data
from tdw.controller import Controller
from tdw.librarian import ModelLibrarian, RobotLibrarian
from tdw.output_data import Bounds, Images, OutputData, Robot, StaticRobot
from tdw.robot_creator import RobotCreator
from tdw.tdw_utils import AudioUtils, TDWUtils

lib_path = "resources/robot_librarian.json"
if not Path(lib_path).exists():
    RobotLibrarian.create_library(path=lib_path, description="Custom Robot Librarian")
lib = RobotLibrarian(lib_path)
robot_name = "ur5"

robot_urdf_urls = {
    "ur5": "https://github.com/ros-industrial/robot_movement_interface/blob/master/dependencies/ur_description/urdf/ur5_robot.urdf.xacro",
}
robot_creator_base_path = "/Users/xufeng/robot_creator/Assets/robots/"
chain_urdf_files = {
    "ur5": robot_creator_base_path + "ur5_robot.urdf",
}
urdf_base_elements = {
    "ur5": ["base_link"],
}
if lib.get_record(robot_name) is None:
    r = RobotCreator()
    record = r.create_asset_bundles(
        urdf_url=robot_urdf_urls[robot_name],
    )
    lib.add_or_update_record(record=record, overwrite=False, write=True)

class SceneState:
    def __init__(self, resp: List[bytes]):
        r = get_data(resp=resp, d_type=Robot)
        self.joint_positions: Dict[int, np.array] = dict()
        self.joint_angles: Dict[int, np.array] = dict()
        for i in range(r.get_num_joints()):
            j_id = r.get_joint_id(i)
            self.joint_positions[j_id] = r.get_joint_position(i)
            angle = r.get_joint_positions(i)
            self.joint_angles[j_id] = np.array([0]) if len(angle) == 0 else angle

class RobotStatic:
    def __init__(
        self,
        static_robot: StaticRobot,
        chain_urdf_file=None,
        urdf_base_element=None,
    ):
        self.joints: Dict[int, JointStatic] = dict()
        self.joint_name_id: Dict[str, int] = dict()
        for i in range(static_robot.get_num_joints()):
            joint_id = static_robot.get_joint_id(i)
            # Cache the body parts.
            joint_static = JointStatic(sr=static_robot, index=i)
            self.joints[joint_id] = joint_static
            joint_name = joint_static.name
            self.joint_name_id[joint_name] = joint_id
        self.chain = (
            ikpy.chain.Chain.from_urdf_file(
                chain_urdf_file,
                base_elements=urdf_base_element,
                # active_links_mask=[False] + [True] * 6 + [False],
            )
            if chain_urdf_file is not None
            else None
        )
        print(self.chain.links)

class Test(Controller):
    def __init__(self, *args, **kwargs):
        super().__init__(*args, **kwargs)
        self.robot_id: int = 0

    def _get_current_angles(self):
        current_angles = np.array(
            [self.state.joint_angles[joint_id] for joint_id in self.state.joint_angles],
            dtype=float,
        )
        # print(f"current angles: {current_angles}")
        assert len(self.robot_static.chain.links) == len(current_angles)
        return current_angles.ravel()

    def get_set_angles_commands(self, angles):
        commands = []
        for angle, joint_id in zip(angles, self.state.joint_angles):
            if self.robot_static.joints[joint_id].joint_type != "revolute":
                continue
            commands.append(
                {
                    "$type": "set_revolute_target",
                    "id": self.robot_id,
                    "joint_id": joint_id,
                    "target": angle,
                }
            )
            # print(self.robot_static.joints[joint_id].name)
        print(f"commands: {commands}")
        return commands

    def trial(self):

        commands = [
            TDWUtils.create_empty_room(12, 12),
            {"$type": "destroy_all_objects"},
            {"$type": "set_screen_size", "width": 800, "height": 800},
            {"$type": "set_target_framerate", "framerate": 100},
        ]
        commands.extend(
            TDWUtils.create_avatar(
                avatar_type="A_Img_Caps_Kinematic",
                position={"x": 2, "y": 2, "z": 2},
                look_at={"x": 0, "y": 0, "z": 0},
            )
        )
        resp = self.communicate(commands)
        robot_id = self.robot_id
        robot_commands = [
            self.get_add_robot(
                name=robot_name,
                robot_id=robot_id,
                library=lib_path,
            ),
            {"$type": "send_static_robots", "frequency": "once"},
            {"$type": "send_robots", "frequency": "always"},
        ]
        commands.extend(robot_commands)
        resp = self.communicate(commands)
        self.robot_static = RobotStatic(
            static_robot=get_data(resp=resp, d_type=StaticRobot),
            chain_urdf_file=chain_urdf_files[robot_name],
            urdf_base_element=urdf_base_elements[robot_name],
        )
        self.state = SceneState(resp=resp)

        ## RUN
        aim_target = [
            0.0,
            -54.59446312770834,
            20.22381451198383,
            -89.53663023310024,
            117.33775164996955,
            -117.84157890716145,
            -0.0003045910176742583,
            0.0,
        ]
        print(f"Aimed angles: {aim_target}")
        commands = self.get_set_angles_commands(aim_target)
        resp = self.communicate(commands)
        # waiting
        for _ in range(200):
            resp = self.communicate([])
        self.state = SceneState(resp=resp)
        current_angle = self._get_current_angles()
        print(f"Real current angles: {current_angle}")
        print(f"Joint angle error: {np.array(aim_target)-current_angle}")

    def run(self):
        self.start()
        self.trial()
        self.communicate({"$type": "terminate"})

if __name__ == "__main__":
    Test().run()

Output:

Aimed angles: [0.0, -54.59446312770834, 20.22381451198383, -89.53663023310024, 117.33775164996955, -117.84157890716145, -0.0003045910176742583, 0.0]

commands: [{'$type': 'set_revolute_target', 'id': 0, 'joint_id': 256195378, 'target': -54.59446312770834}, {'$type': 'set_revolute_target', 'id': 0, 'joint_id': -3876511, 'target': 20.22381451198383}, {'$type': 'set_revolute_target', 'id': 0, 'joint_id': -435720876, 'target': -89.53663023310024}, {'$type': 'set_revolute_target', 'id': 0, 'joint_id': -616874256, 'target': 117.33775164996955}, {'$type': 'set_revolute_target', 'id': 0, 'joint_id': 1280692096, 'target': -117.84157890716145}, {'$type': 'set_revolute_target', 'id': 0, 'joint_id': 1278300232, 'target': -0.0003045910176742583}]

Real current angles: [ 0.00000000e+00 -2.87523174e+01  4.26738214e+00 -8.95483856e+01
  1.17241173e+02 -1.17850861e+02 -1.39841097e-04  0.00000000e+00]

Joint angle error: [ 0.00000000e+00 -2.58421457e+01  1.59564324e+01  1.17553870e-02
  9.65788594e-02  9.28168854e-03 -1.64749921e-04  0.00000000e+00]

Sadly, sometimes the error could be very large (like > 1.0) and lead to bad performance of control.

Thank you very much for your help!

[alters-mit]

@xf-zhao

Thank you very much for your help!

Thanks for being so thorough!

I don't think I'll finish working on this by the end of today (Friday). So I'll need to keep working on it starting on Monday.

This is the chain I've figured out so far:

    CHAIN: Chain = Chain(name="ur5", links=[OriginLink(),
                                            URDFLink(name="shoulder_link",
                                                     translation_vector=np.array([0, 0.08915899, 0]),
                                                     orientation=np.array([0, 0, 0]),
                                                     rotation=np.array([0, -1, 0]),
                                                     bounds=(np.deg2rad(-360), np.deg2rad(360))),
                                            URDFLink(name="upper_arm_link",
                                                     translation_vector=np.array([-0.13585, 0, 0]),
                                                     orientation=np.array([0, 0, 0]),
                                                     rotation=np.array([-1, 0, 0]),
                                                     bounds=(np.deg2rad(-360), np.deg2rad(360))),
                                            URDFLink(name="forearm_link",
                                                     translation_vector=np.array([0.1196999, 0.425001, 0]),
                                                     orientation=np.array([0, 0, 0]),
                                                     rotation=np.array([-1, 0, 0]),
                                                     bounds=(np.deg2rad(-360), np.deg2rad(360))),
                                            URDFLink(name="wrist_1_link",
                                                     translation_vector=np.array([0, 0.3922516, 0]),
                                                     orientation=np.array([0, 0, 0]),
                                                     rotation=np.array([-1, 0, 0]),
                                                     bounds=(np.deg2rad(-360), np.deg2rad(360))),
                                            URDFLink(name="wrist_2_link",
                                                     translation_vector=np.array([-0.093, 0, 0]),
                                                     orientation=np.array([0, 0, 0]),
                                                     rotation=np.array([0, -1, 0]),
                                                     bounds=(np.deg2rad(-360), np.deg2rad(360))),
                                            URDFLink(name="wrist_3_link",
                                                     translation_vector=np.array([0, 0.09465025, 0]),
                                                     orientation=np.array([0, 0, 0]),
                                                     rotation=np.array([-1, 0, 0]),
                                                     bounds=(np.deg2rad(-360), np.deg2rad(360))),
                                            URDFLink(name="end_effector",
                                                     translation_vector=np.array([-0.0802, 0, 0]),
                                                     orientation=np.array([0, 0, 0]),
                                                     rotation=None)])

I can see that it's still not accurate enough, but this might get you closer. This chain adds a dummy "end effector" link for where the actual end effector is.

The chain is based on data from the Unity Engine.

[xf-zhao]

The chain is based on data from the Unity Engine.

Hi, thank you for the update. Any thoughts for the inaccuracy of the result of set_revolute_target ? (As I mentioned in the former comment, the problem seems to have nothing to do with IK.) And I found that after initilization, if I get the initial joint angles and set the arm with these initial values again, the arm will still move a little bit, which is not what we expect.

BTW, I am curious about that will the TDW build be open-source ? :-)

[alters-mit]

Any thoughts for the inaccuracy of the result of set_revolute_target ?

I'm still working on this. set_revolute_target is very accurate but won't be totally accurate because it exists within a physics simulation. The larger problem is that there is something wrong with the ikpy chain for the UR5 robot.

We know that the TDW robotics API is accurate because it's highly accurate within the Magnebot API.

BTW, I am curious about that will the TDW build be open-source ? :-)

The TDW Python module is currently open-source. The build's C# code will remain closed-source. See: https://github.com/threedworld-mit/tdw/blob/master/Documentation/contributions/c_sharp_sources.md

[alters-mit]

@xf-zhao Good news! This IK chain seems to be very accurate:

    def _get_links(self) -> List[Link]:
        bounds = (np.deg2rad(-360), np.deg2rad(360))
        orientation = np.array([0, 0, 0])
        return [OriginLink(),
                URDFLink(name="shoulder_link",
                         translation_vector=np.array([0, 0.08915899, 0]),
                         orientation=orientation,
                         rotation=np.array([0, -1, 0]),
                         bounds=bounds),
                URDFLink(name="upper_arm_link",
                         translation_vector=np.array([-0.13585, 0, 0]),
                         orientation=orientation,
                         rotation=np.array([1, 0, 0]),
                         bounds=bounds),
                URDFLink(name="forearm_link",
                         translation_vector=np.array([0.1196999, 0, 0.425001]),
                         orientation=orientation,
                         rotation=np.array([1, 0, 0]),
                         bounds=bounds),
                URDFLink(name="wrist_1_link",
                         translation_vector=np.array([0, 0, 0.3922516]),
                         orientation=orientation,
                         rotation=np.array([1, 0, 0]),
                         bounds=bounds),
                URDFLink(name="wrist_2_link",
                         translation_vector=np.array([-0.093, 0, 0]),
                         orientation=orientation,
                         rotation=np.array([0, -1, 0]),
                         bounds=bounds),
                URDFLink(name="wrist_3_link",
                         translation_vector=np.array([0, -0.09465025, 0]),
                         orientation=orientation,
                         rotation=None)]

[xf-zhao]

set_revolute_target is very accurate but won't be totally accurate because it exists within a physics simulation.

Hello, thank you very much for the work and for the updated urdf chain. Actually I also use another robot called "OpenManipulatorPro" , the similar thing happens there, too. And in the future, one could also use their own robot setup, so I am curious how did you tuned the robot parameters. (I will test it later.)

Fort the function set_revolute_target, as I tested in the former comment, it will produce angle errors to a level of 0.25 degree (I did not test to much cases.). I am not sure if this is not a bug but a "feature" within the expected range due to the physics engine. However, with many joints' angle error together, the end_effector's position could be very inaccurate -- at least will not allow us to do centimeter-level manipulation, in which case the error could be larger than our actions.max().

And I still I think the problem is not caused by the chain that ikpy uses to calculate IK solution, since

1. I can confirm that ikpy could give us an accurate FK prediction (almost the same with current end_effector's position) when given current joint angles. So when pass an IK solution (for target position p_0) to FK function, it returns the predicted position p_1 that is very similar to p_0, as expected. So I believe that if the joint angles are set properly by set_revolute_target, there will be no need to tune URDF chain to get everything work (I am curious whether any fine-tuning on ik chain could actually result in an accurate position).
2. It seems that ikpy works well in other simulator like PyBullet (at my first glance/test).

[alters-mit]

And in the future, one could also use their own robot setup, so I am curious how did you tuned the robot parameters. (I will test it later.)

I was able to look at the robot in Unity Editor and adjust the chain parameters accordingly. In the future I hope to have a process that is more user-friendly. For now, you can probably get the joint positions from the output data and use that as a basis for defining the chain.

I am not sure if this is not a bug but a "feature" within the expected range due to the physics engine.

A feature. This is almost certainly due to the physics engine. Your joints might not have sufficient force limits. Or, the damping values or stiffness values might be too high.

[alters-mit]

@xf-zhao Hi, are you still having issues with the IK chain? I'd like to know if I can close this GitHub Issue.

[alters-mit]

@xf-zhao

This document explains how to add robot arms with IK. Right now I've only added IK for UR5.