Moveit2 Python API: Incorrect Joint Positions from RobotState.

[pinakjani]

Description

I am using the Moveit2 Python API for a custom robot. I try to use the moveit_py to plan and execute a goal. I want to set goal to be the an end-effector pose and thus I parse my end-effector pose to the set_from_ikmethod of RobotState. However the resulted RobotState after the IK method gives me joint positions which look like random garbage values.

Your environment

• ROS Distro: [Humble]
• OS Version: Ubuntu 22.04
• Binary build

Steps to reproduce

Following is my implementation,

        # instantiate a RobotState instance using the current robot model
        robot_model = self.robot.get_robot_model()
        robot_state = RobotState(robot_model)
        robot_state.update(force=True)
        pose_goal = Pose()
        pose_goal.position.x = 0.223
        pose_goal.position.y = -0.0056
        pose_goal.position.z = 0.1192
        pose_goal.orientation.x = 0.74
        pose_goal.orientation.y = -0.642
        pose_goal.orientation.z = 0.181
        pose_goal.orientation.w = -0.087

        robot_state.update(force=True)

        # randomize the robot state
        robot_state.set_from_ik(joint_model_group_name="interbotix_arm",
            geometry_pose=pose_goal,
            tip_name="dx400/link_5",
            timeout=2.0)

        robot_state.update(force=True)
        print("see joints:",robot_state.joint_positions)

        # set plan start state to current state
        self.planning_component.set_start_state_to_current_state()

        # set goal state to the initialized robot state
        self.logger.info("Sending goal")
        self.planning_component.set_goal_state(robot_state=robot_state)
        self.logger.info("Reached EE Pose")
        robot_state.update(force=True)
        # plan to goal
        self.plan_and_execute(sleep_time=3.0)

Expected behaviour

I expect the IK solver to result in valid joint positions so that the planner can plan and execute the goal successfully.

Actual behaviour

Following are the values of joint positions upon execution of the above code,

see joints: {'gripper': 4.6715785264223e-310, 'j1': 1.4534446040336567e+45, 'j2': 1.692015438857914e+190, 'j3': 5.5345553743477745e+203, 'j4': 1.19e-321, 'j5': 4.6715785219504e-310, 'j6': 6.9481792001805e-310, 'left_finger': 0.0, 'right_finger': 1.63e-322}

[pinakjani]

Update

The joint positions for the RobotState before passing it to the set_from_ik method also seem to be garbage value which is why the IK solver results in garbage joint positions. However, I still cannot figure out the cause of the incorrect joint positions in RobotState.

[henningkayser]

@pinakjani, you are not initializing the robot state to any positions. There is an API for setting joint positions directly, or you can get them from your robot instance through MoveItPy's planning scene (simulation or hardware required). Here is an example in the tutorial where the robot state is set to random positions first and then used right after https://moveit.picknik.ai/main/doc/examples/motion_planning_python_api/motion_planning_python_api_tutorial.html#single-pipeline-planning-robot-state. Also, I would suggest using something like numpy-quaternion for making sure your input quaternion is valid and properly normalized, since it's very easy to run into precision issues

[pinakjani]

@henningkayser Thanks for suggesting the fix for the problem. Since I am working with hardware, I want to initialize the joint positions to be the current joint positions of my robot. I tried initializing the robot state joint positions through the planning scene as suggested here MoveItPy's planning scene. That did fix the issue of random garbage values in the joint positions and I see the correct values for joint positions now. However, the joint positions after the IK solver results in the same values as my initial joint positions. I do not see my goal joint positions after the set_from_ik method.

Here's my script:

def go_to_pose(self):
        planning_scene_monitor = self.robot.get_planning_scene_monitor()
        with planning_scene_monitor.read_write() as scene:

            # instantiate a RobotState instance using the current robot model
            robot_state = scene.current_state
            robot_state.update()
            original_joint_positions = robot_state.get_joint_group_positions("interbotix_arm")

            robot_state.update()
            print("See initial joints:",original_joint_positions)

            q = np.quaternion(-0.087, 0.74, -0.642, 0.181).normalized()
            pose_goal = Pose()
            pose_goal.position.x = 0.223
            pose_goal.position.y = -0.0056
            pose_goal.position.z = 0.1192
            pose_goal.orientation.x = q.x
            pose_goal.orientation.y = q.y
            pose_goal.orientation.z = q.z
            pose_goal.orientation.w = q.w

            # randomize the robot state
            robot_state.set_from_ik(joint_model_group_name="interbotix_arm",
                geometry_pose=pose_goal,
                tip_name="dx400/link_5",
                timeout=2.0)

            robot_state.update()
            print("see joints:",robot_state.joint_positions)

            # set plan start state to current state
            self.planning_component.set_start_state_to_current_state()

            # set goal state to the initialized robot state
            self.logger.info("Go to goal")
            self.planning_component.set_goal_state(robot_state=robot_state)

            robot_state.update()
            # plan to goal
        self.plan_and_execute(sleep_time=3.0)

Is there something that I am missing over here?

[henningkayser]

Please check the result of set_from_ik to see if the IK call is successfull or not. Which solver are you using? I've not seen namespaced link names like "dx400/link_5", is it really configured like that in the URDF? My other guess is that the reference frame of your pose and the IK solver are different. If you call robot_state.get_pose() (after update()), does the reported pose match your expectation? A good sanity check is to get a pose from the current robot state, add a small offset, and solve IK again.

[pinakjani]

Hi @henningkayser,

I am using the kdl_kinematics_plugin as my solver. Following is my kinematics.yaml passed to the MoveitConfigBuilder.

interbotix_arm:
  kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
  kinematics_solver_search_resolution: 0.005
  kinematics_solver_timeout: 0.005

And yes my URDF is configured with namespaced link names.

I did try check the pose before and after using the the set_from_ik but the pose does not change after the IK Solver.

Here's my code for same:

def go_to_pose(self):
        planning_scene_monitor = self.robot.get_planning_scene_monitor()
        with planning_scene_monitor.read_write() as scene:

            # instantiate a RobotState instance using the current robot model
            robot_state = scene.current_state
            original_joint_positions = robot_state.get_joint_group_positions("interbotix_arm")
            robot_state.update()

            check_init_pose = robot_state.get_pose("dx400/gripper_link")
            print("Initial_pose:", check_init_pose)

            pose_goal = Pose()
            pose_goal.position.x = check_init_pose.position.x
            pose_goal.position.y = check_init_pose.position.y
            pose_goal.position.z = check_init_pose.position.z + 0.5
            pose_goal.orientation.x = check_init_pose.orientation.x
            pose_goal.orientation.y = check_init_pose.orientation.y
            pose_goal.orientation.z = check_init_pose.orientation.z
            pose_goal.orientation.w = check_init_pose.orientation.w
            print("Offset_pose:", pose_goal)

            # randomize the robot state
            robot_state.set_from_ik(joint_model_group_name="interbotix_arm",
                geometry_pose=pose_goal,
                tip_name="dx400/gripper_link",
                timeout=1.0)

            robot_state.update()

            check_updated_pose = robot_state.get_pose("dx400/gripper_link")

            print("New_pose:", check_updated_pose)

            # set plan start state to current state
            self.planning_component.set_start_state_to_current_state()

            # set goal state to the initialized robot state
            self.logger.info("Go to goal")
            self.planning_component.set_goal_state(robot_state=robot_state)
            robot_state.update()

         # plan to goal
        self.plan_and_execute(sleep_time=3.0)

The results look like this:

Initial_pose: Position (x=-0.05277248056581893, y=-0.10765406961097726, z=0.18588278777746325), Orientation(x=0.46150855022560056, y=-0.5272393453195586, z=0.4720017615025274, w=0.5350166987619904))

Goal pose passed to set_from_ik after applying +0.5 offset to position.z:

Offset_pose: Position (x=-0.05277248056581893, y=-0.10765406961097726, z=0.6858827877774633), Orientation (x=0.46150855022560056, y=-0.5272393453195586, z=0.4720017615025274, w=0.5350166987619904))

Final pose after executing the set_from_ik method:

New_pose: Position (x=-0.05277248056581893, y=-0.10765406961097726, z=0.18588278777746325), Orientation (x=0.46150855022560056, y=-0.5272393453195586, z=0.4720017615025274, w=0.5350166987619904))

[ManuPer3z]

Hi @pinakjani

It appears that the IK solver (set_from_ik method) isn't updating the robot's joint state for the new pose, even though you've given it a significantly different z-value (a 0.5 offset).

Given that the KDL kinematics plugin is being used, the reason for this could be:

IK Solution Not Found: The KDL kinematics solver might not find a solution in the provided timeout or within the solution search resolution. Although a 0.5 offset in the z-direction is significant, the kinematics of your robot may have constraints that make certain poses hard or impossible to reach. However, it's more likely the solver just didn't find a solution within the specified time or resolution.

Namespace Configuration: You mentioned that your URDF has namespaced link names. This might cause some issues if not properly configured everywhere. Ensure that all references to links and joints are correctly namespaced in your MoveIt configuration.

Solver Configuration: Your kinematics.yaml specifies a very short timeout (0.005s). This might not be enough time for the solver to find a valid solution. Consider increasing the timeout value to give the solver more time.

Error Handling: It would be beneficial to check the result of the set_from_ik method. If it returns a boolean, you can directly check its value. If it's an object, there might be methods or attributes that indicate the success or reason for failure.

Here's how to modify your code to address these points:

# Increase the timeout for the IK solver
result = robot_state.set_from_ik(joint_model_group_name="interbotix_arm",
                geometry_pose=pose_goal,
                tip_name="dx400/gripper_link",
                timeout=2.0)  # Increase the timeout

# Check the result of the IK solver
if not result:
    self.logger.error("IK solution was not found!")
    return

Implement the above suggestions, especially increasing the timeout and checking the result of the set_from_ik method. If the IK solver still fails to find a solution, then further investigation would be needed into the kinematic constraints, robot's workspace, and any potential obstacles in the scene.

[pinakjani]

Hi @ManuPer3z,

Thanks for your suggestion! You are right the IK solver was unable to find the solution within the timeout bound and upon increasing the timeout in kinematics.yaml things work for the offset pose.

I am also seeing many reachable poses that the IK solver is unable to solve. Is there a recommended solver that maximizes reachability?

Again thanks @henningkayser and @ManuPer3z !

[ManuPer3z]

I'm glad you found it helpful!!

In answer to your other question:

When it comes to inverse kinematics solvers for robot arms in MoveIt, there are a few common options, each with its own advantages and drawbacks:

KDL Kinematics Plugin (kdl_kinematics_plugin):

Advantages: It comes by default with MoveIt and does not require external dependencies. Drawbacks: It can be slower for certain configurations and might not find a solution for all reachable poses, especially for robots with many degrees of freedom.

TRAC-IK (trac_ik_kinematics_plugin):

Advantages: It often performs better than KDL in terms of speed and solution accuracy. It's designed to be a drop-in replacement for KDL. Drawbacks: Requires installation of the external TRAC-IK library.

If you're encountering issues with KDL, TRAC-IK is often the next solver to try. To use it, you'd need to install it and then adjust your kinematics.yaml to use the TRAC-IK plugin.

IKFast (ikfast_kinematics_plugin):

Advantages: Very fast since it generates an analytical solution for your robot. It's more likely to find solutions for all reachable poses. Drawbacks: Setting up IKFast can be complex, especially for robots with many degrees of freedom. The generated solver might not handle singularities gracefully. If you decide to use IKFast, you'd generate a solver specifically for your robot's kinematics and then use that solver as a MoveIt plugin.

BioIK:

Advantages: A newer solver that considers multiple objectives and can avoid solutions where the robot is close to its joint limits. It's known for its performance and ability to find solutions in complex scenarios. Drawbacks: Not open source (though there is a version available for academic and evaluation purposes).

Choosing the best solver depends on your specific needs. If you're looking for more reliable solutions and are okay with a slightly more complex setup process, then IKFast or TRAC-IK might be the way to go. If you're looking for state-of-the-art performance and have the budget for it, then BioIK might be a good choice.

[henningkayser]

Thanks for jumping in @ManuPer3z! Just for completeness, I'd add that bio_ik is Open Source and available here, an unofficial ros2 branch is here. For ros2, an arguably better option for optimization-based IK is pick_ik.

If the timeout argument has no effect, this looks like a bug in the Python API to me...

[pinakjani]

Thanks @henningkayser,

As per @ManuPer3z's suggestion changing the timeout argument did work. Also, thanks for the additional resources and information on other IK solvers.