Path planning

[kweonhj]

I hope this message finds you well. I am currently working with the robot system LBR MED14 and have encountered an issue related to path planning. When I issue a command for the robot's end effector to move approximately 1cm along the X-axis, I observe that the path planning is not optimized. Instead of making a slight movement, the robot tends to traverse a longer path before finally achieving the desired 1cm displacement.

I would appreciate it if you could provide some insights into this behavior and perhaps suggest any adjustments or optimizations that can be made to ensure a more precise and efficient movement of the end effector for small displacements.

Thank you for your time and assistance. I look forward to your guidance on resolving this matter.

[mhubii]

hi @kweonhj , and thank you for the feedback.

Could you please give some details on how you executed this motion? Assuming you used moveit through rviz, you can try to select the cartesian path box, see below

Alternatively, you can find some very minimal code for executing a linear motion here: https://github.com/lbr-stack/lbr_fri_ros2_stack/issues/133#issuecomment-1822631334

There is also someone who turned this logic into a service: https://github.com/andreasHovaldt/pose_commander

I hope this answers your question somewhat, please let me know. Maybe this specific 1cm displacement would require the robot to run through joint limits, so moveit finds another solution?

[kweonhj]

Thank you for reaching out and providing additional information on the motion execution. I used the code available at the following link to move the robot's end effector to the desired position: [Link to the provided code:move_robot.py in #133 ].

In response to your suggestion, I attempted to address the path planning issue by checking the cartesian path box in RViz while using MoveIt. However, even with this adjustment, the problem persists, and the robot continues to exhibit suboptimal path planning behavior.

If there are any specific parameters or configurations I should consider, or if there are alternative approaches to enhance path planning for small displacements, your guidance would be highly appreciated.

Thank you for your assistance.

[mhubii]

okay, got you. Seems moveit related. Maybe we'll have to do some digging there. I'll try to have a look. This repo really misses good moveits example, sorry about that

[mhubii]

could you give me a helping hand @kweonhj and send me your script, so I can test this in simulation. Thank you :)

[kweonhj]

I appreciate your willingness to assist. Below is the script I used to execute the motion.

from typing import List

import rclpy
from geometry_msgs.msg import Point, Pose, Quaternion
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import (
    BoundingVolume,
    Constraints,
    OrientationConstraint,
    PositionConstraint,
)
from rclpy.action import ActionClient
from rclpy.node import Node
from shape_msgs.msg import SolidPrimitive
from std_msgs.msg import Header
import numpy as np
from scipy.spatial.transform import Rotation
import time

class MoveGroupActionClientNode(Node):
    def __init__(self, node_name: str) -> None:
        super().__init__(node_name)

        self.action_server = "/lbr/move_action"
        self.move_group_name = "arm"
        self.base = "link_0"
        self.end_effector = "link_ee"

        self.move_group_action_client = ActionClient(
            self, MoveGroup, self.action_server
        )

        self.get_logger().info(f"Waiting for action server {self.action_server}...")
        if not self.move_group_action_client.wait_for_server(timeout_sec=1):
            raise RuntimeError(
                f"Couldn't connect to action server {self.action_server}."
            )
        self.get_logger().info(f"Done.")

    def send_goal_async(self, target: Pose):
        goal = MoveGroup.Goal()
        goal.request.allowed_planning_time = 1.0
        goal.request.goal_constraints.append(
            Constraints(
                position_constraints=[
                    PositionConstraint(
                        header=Header(frame_id=self.base),
                        link_name=self.end_effector,
                        constraint_region=BoundingVolume(
                            primitives=[SolidPrimitive(type=2, dimensions=[0.0001])],
                            primitive_poses=[Pose(position=target.position)],
                        ),
                        weight=1.0,
                    )
                ],
                orientation_constraints=[
                    OrientationConstraint(
                        header=Header(frame_id=self.base),
                        link_name=self.end_effector,
                        orientation=target.orientation,
                        absolute_x_axis_tolerance=0.0001,
                        absolute_y_axis_tolerance=0.0001,
                        absolute_z_axis_tolerance=0.0001,
                        weight=1.0,
                    )
                ],
            )
        )
        goal.request.group_name = self.move_group_name
        goal.request.max_acceleration_scaling_factor = 0.1
        goal.request.max_velocity_scaling_factor = 0.05
        goal.request.num_planning_attempts = 1

        return self.move_group_action_client.send_goal_async(goal)

def euler_to_quaternion(euler_angles):
    rotation = Rotation.from_euler('xyz', euler_angles)  # Adjust the order of rotations if needed
    quaternion = rotation.as_quat()
    return Quaternion(x=quaternion[0], y=quaternion[1], z=quaternion[2], w=quaternion[3])

def main(args: List = None) -> None:

    rclpy.init(args=args)
    move_group_action_client_node = MoveGroupActionClientNode(
        "move_group_action_client_node"
    )

    # List of poses to move the robot to
    poses = [

        Pose(
            position=Point(x=0.3, y=0.3, z=0.8),
            orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0),
        ),

        Pose(
            position=Point(x=0.35, y=0.3, z=0.8),
            orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0),
        ),

        Pose(
            position=Point(x=0.4, y=0.3, z=0.8),
            orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0),
        )
        # Add more poses as needed
    ]

    for pose in poses:
        future = move_group_action_client_node.send_goal_async(pose)
        rclpy.spin_until_future_complete(move_group_action_client_node, future)
        time.sleep(100)

    rclpy.shutdown()

if __name__ == "__main__":
    main()

Please let me know if there are any insights you can provide to optimize the robot's simple movement. Thank you!!

[mhubii]

okay just had a look, can confirm this behavior. Will see what can be done

[mhubii]

hi @kweonhj , here is an updated version, that computes inverse kinematics and sends a joint state next. This seems to work better. Let me know if this works for you :)

Launch

ros2 launch lbr_bringup bringup.launch.py moveit:=true model:=med14

Run

from typing import List

import rclpy
from geometry_msgs.msg import Point, Pose, Quaternion
from moveit_msgs.action import MoveGroup
from moveit_msgs.msg import Constraints, JointConstraint, MoveItErrorCodes
from moveit_msgs.srv import GetPositionIK
from rclpy.action import ActionClient
from rclpy.node import Node
from sensor_msgs.msg import JointState

class MoveGroupActionClientNode(Node):
    def __init__(self, node_name: str, namespace: str = "lbr") -> None:
        super().__init__(node_name, namespace=namespace)

        self.action_server = "move_action"
        self.move_group_name = "arm"
        self.base = "link_0"
        self.end_effector = "link_ee"

        # MoveIt action client
        self.move_group_action_client = ActionClient(
            self, MoveGroup, self.action_server
        )
        if not self.move_group_action_client.wait_for_server(timeout_sec=1):
            raise RuntimeError(
                f"Couldn't connect to action server {self.action_server}."
            )

        # Inverse kinematics client
        self.ik_client = self.create_client(GetPositionIK, "compute_ik")
        if not self.ik_client.wait_for_service(timeout_sec=1):
            raise RuntimeError(
                f"Couldn't connect to service {self.ik_client.srv_name}."
            )

    def request_inverse_kinematics(self, pose: Pose) -> JointState:
        r"""Request inverse kinematics for a given pose."""
        request = GetPositionIK.Request()
        request.ik_request.group_name = self.move_group_name
        tf_prefix = self.get_namespace().removeprefix("/")
        request.ik_request.pose_stamped.header.frame_id = f"{tf_prefix}/{self.base}"
        request.ik_request.pose_stamped.header.stamp = self.get_clock().now().to_msg()
        request.ik_request.pose_stamped.pose = pose
        request.ik_request.avoid_collisions = True
        future = self.ik_client.call_async(request)
        rclpy.spin_until_future_complete(self, future)
        response = future.result()
        if response is None:
            self.get_logger().error("Inverse kinematics service call failed")
            return
        if response.error_code.val != MoveItErrorCodes.SUCCESS:
            self.get_logger().error(
                f"Failed to compute inverse kinematics: {response.error_code.val}"
            )
            return
        return response.solution.joint_state

    def move_to_pose(self, target: Pose):
        r"""Move the robot to a given pose. Do so by requesting inverse kinematics and then sending a move group action."""
        joint_state = self.request_inverse_kinematics(target)
        if joint_state is None:
            return

        # Prepare goal
        goal = MoveGroup.Goal()
        goal.request.allowed_planning_time = 5.0
        goal.request.num_planning_attempts = 10
        goal.request.group_name = self.move_group_name
        goal.request.max_acceleration_scaling_factor = 0.1
        goal.request.max_velocity_scaling_factor = 0.05

        # Set joint constraints
        goal.request.goal_constraints.append(
            Constraints(
                joint_constraints=[
                    JointConstraint(
                        joint_name=joint_state.name[i],
                        position=joint_state.position[i],
                        tolerance_above=0.001,
                        tolerance_below=0.001,
                        weight=1.0,
                    )
                    for i in range(len(joint_state.name))
                ]
            )
        )

        # Send goal
        goal_future = self.move_group_action_client.send_goal_async(goal)
        rclpy.spin_until_future_complete(self, goal_future)
        goal_handle = goal_future.result()
        if not goal_handle.accepted:
            self.get_logger().error("MoveGroup goal rejected")
            return

        # Wait for result
        self.get_logger().info("MoveGroup goal accepted")
        self.get_logger().info("Waiting for result...")
        result_future = goal_handle.get_result_async()
        rclpy.spin_until_future_complete(self, result_future)
        result = result_future.result().result
        if result.error_code.val != MoveItErrorCodes.SUCCESS:
            self.get_logger().error(f"MoveGroup action failed: {result.error_code.val}")
            return
        self.get_logger().info("MoveGroup action succeeded")

def main(args: List = None) -> None:

    rclpy.init(args=args)
    move_group_action_client_node = MoveGroupActionClientNode(
        "move_group_action_client_node"
    )

    # List of poses to move the robot to
    poses = [
        Pose(
            position=Point(x=0.3, y=0.3, z=0.8),
            orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0),
        ),
        Pose(
            position=Point(x=0.35, y=0.3, z=0.8),
            orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0),
        ),
        Pose(
            position=Point(x=0.4, y=0.3, z=0.8),
            orientation=Quaternion(x=1.0, y=0.0, z=0.0, w=0.0),
        ),
        # Add more poses as needed
    ]

    for pose in poses:
        move_group_action_client_node.move_to_pose(pose)

    rclpy.shutdown()

if __name__ == "__main__":
    main()

In the code, there are a lot of futures. Takes some getting used to. Here a tutorial to calling actions: https://docs.ros.org/en/humble/Tutorials/Intermediate/Writing-an-Action-Server-Client/Py.html

For future reference: There seems to be a moveit python API, note: this library is not distributed in humble, but rolling

• moveit_py tutorial: https://moveit.picknik.ai/main/doc/examples/motion_planning_python_api/motion_planning_python_api_tutorial.html
  • source code: https://github.com/ros-planning/moveit2_tutorials/tree/main/doc/examples/motion_planning_python_api

[kweonhj]

I've tested the updated version of the code, and I'm pleased to inform you that it works perfectly! The computation of inverse kinematics and subsequent sending of joint states seem to have resolved the path planning issues I encountered earlier. Thank you for your prompt attention to this matter and for providing the improved version of the code.

[mhubii]

great! Happy to hear this worked. I do agree that this behavior is rather strange. Sometimes wonder whether there will be a replacement for Moveit at some point.

There is curobo https://github.com/NVlabs/curobo, for which currently no good ros integration is available. This library also comes with the caveat that it requires proprietary software.