Usage Examples

All examples use the rclpy Python client library. Joint names assume a Yaskawa MH50 robot (s_joint, l_joint, u_joint, r_joint, b_joint, t_joint). Replace these with the joint names from your URDF.

Setup

import rclpy
from rclpy.node import Node
import motion_planning_interfaces.srv as mpi_srv
import motion_planning_interfaces.msg as mpi_msg
from trajectory_msgs.msg import JointTrajectory
from geometry_msgs.msg import TransformStamped

JOINT_NAMES = ['s_joint', 'l_joint', 'u_joint', 'r_joint', 'b_joint', 't_joint']
HOME_POS    = [0.0, 0.4, 0.4, 0.1, -0.1, 0.1]

rclpy.init()
node = Node('mp_example_client')

def call(client, request, timeout=10.0):
    """Synchronous service call helper."""
    future = client.call_async(request)
    rclpy.spin_until_future_complete(node, future, timeout_sec=timeout)
    return future.result()

Forward Kinematics

Compute the Cartesian pose of the tool frame from joint positions.

fk_client = node.create_client(mpi_srv.ComputeFK, 'compute_fk')
fk_client.wait_for_service()

req = mpi_srv.ComputeFK.Request()
req.robot_state.joint_state.name     = JOINT_NAMES
req.robot_state.joint_state.position = HOME_POS
req.root_frame = 'base_link'
req.tip_frame  = 'tool0'

resp = call(fk_client, req)

if resp.error_code.val == mpi_msg.ErrorCodes.SUCCESS:
    tf = resp.tf.transform
    print(f"Position:    x={tf.translation.x:.4f}  y={tf.translation.y:.4f}  z={tf.translation.z:.4f}")
    print(f"Orientation: x={tf.rotation.x:.4f}  y={tf.rotation.y:.4f}  z={tf.rotation.z:.4f}  w={tf.rotation.w:.4f}")
else:
    print(f"FK failed: error_code={resp.error_code.val}")

Inverse Kinematics

Find joint configurations that place the tool at a given Cartesian pose. Returns multiple solutions; clients typically pick the one closest to a reference state.

from geometry_msgs.msg import Transform, Vector3, Quaternion

ik_client = node.create_client(mpi_srv.ComputeIK, 'compute_ik')
ik_client.wait_for_service()

# Target pose (example: 0.8 m in front, 0.1 m up, no rotation)
target_tf = TransformStamped()
target_tf.header.frame_id   = 'base_link'   # root_frame
target_tf.child_frame_id    = 'tool0'        # tip_frame
target_tf.transform.translation.x = 0.8
target_tf.transform.translation.y = 0.0
target_tf.transform.translation.z = 0.1
target_tf.transform.rotation.w    = 1.0      # identity rotation

req = mpi_srv.ComputeIK.Request()
req.update_objects  = True
req.root_to_tip_tf  = target_tf
# Optional: bias toward a reference configuration
req.reference_state.joint_state.name     = JOINT_NAMES
req.reference_state.joint_state.position = HOME_POS
# IK config: check collision, return up to 5 solutions
req.config.check_collision = True
req.config.max_output_num  = 5

resp = call(ik_client, req)

if resp.error_code.val == mpi_msg.ErrorCodes.SUCCESS:
    print(f"Found {len(resp.goal_states)} IK solution(s)")
    best = resp.goal_states[0]
    print(f"Best solution: {best.joint_state.position}")
elif resp.error_code.val == mpi_msg.ErrorCodes.NO_IK_SOLUTION:
    print("No IK solution found for the given pose")
else:
    print(f"IK failed: error_code={resp.error_code.val}")

Motion Planning

Plan and time-parameterize a collision-free trajectory from a start state to a goal state.

mp_client = node.create_client(mpi_srv.ComputeMotionPlan, 'compute_motion_plan')
mp_client.wait_for_service()

goal_pos = [0.5, 0.3, 0.6, -0.2, 0.4, -0.3]

req = mpi_srv.ComputeMotionPlan.Request()
req.update_objects = True

# Start state
req.start_state.joint_state.name     = JOINT_NAMES
req.start_state.joint_state.position = HOME_POS

# Goal: joint-space target
goal = mpi_msg.Goal()
goal.type = mpi_msg.Goal.ROBOT_STATE
goal.robot_state.joint_state.name     = JOINT_NAMES
goal.robot_state.joint_state.position = goal_pos
req.goals = [goal]

# Joint-space planning
from motion_planning_interfaces.msg import PathPlanType
req.path_plan_types = [mpi_msg.PathPlanType.JOINT_PLAN]

# Enable time parameterization (TOPPRA)
req.do_parameterize = True
req.parameterize_config.type = mpi_msg.ParameterizeConfig.TOPPRA

# Joint velocity and acceleration limits (must match URDF or WorkcellDescription)
for name in JOINT_NAMES:
    req.parameterize_config.joint_velocity_constraint.names.append(name)
    req.parameterize_config.joint_velocity_constraint.max.append(2.18)
    req.parameterize_config.joint_velocity_constraint.min.append(-2.18)
    req.parameterize_config.joint_acceleration_constraint.names.append(name)
    req.parameterize_config.joint_acceleration_constraint.max.append(5.0)
    req.parameterize_config.joint_acceleration_constraint.min.append(-5.0)

resp = call(mp_client, req, timeout=30.0)

if resp.error_code.val == mpi_msg.ErrorCodes.SUCCESS:
    traj = resp.trajectory
    print(f"Trajectory: {len(traj.points)} points, "
          f"duration={traj.points[-1].time_from_start.sec:.2f}s")
elif resp.error_code.val == mpi_msg.ErrorCodes.PARTIAL_SUCCESS:
    print(f"Partial plan: reached {resp.reached_goals} of {len(req.goals)} goals")
    print(f"Planning error: {resp.planning_error_code.val}")
else:
    print(f"Motion plan failed: error_code={resp.error_code.val}")

Cartesian Interpolation

Move the tool along a straight-line Cartesian path.

# Re-use ComputeMotionPlan with CARTESIAN_INTERPOLATE type
req = mpi_srv.ComputeMotionPlan.Request()
req.update_objects = True
req.start_state.joint_state.name     = JOINT_NAMES
req.start_state.joint_state.position = HOME_POS

# Goal: a Cartesian pose
goal = mpi_msg.Goal()
goal.type = mpi_msg.Goal.TRANSFORM
goal.tf.header.frame_id = 'base_link'
goal.tf.child_frame_id  = 'tool0'
goal.tf.transform.translation.x = 0.9
goal.tf.transform.translation.z = 0.2
goal.tf.transform.rotation.w    = 1.0
req.goals           = [goal]
req.path_plan_types = [mpi_msg.PathPlanType.CARTESIAN_INTERPOLATE]

req.cartesian_interpolate_config.type    = mpi_msg.CartesianInterpolateConfig.R3XSO3
req.cartesian_interpolate_config.n_points = 50

req.do_parameterize = True
req.parameterize_config.type = mpi_msg.ParameterizeConfig.TOTG

resp = call(mp_client, req, timeout=10.0)
if resp.error_code.val == mpi_msg.ErrorCodes.SUCCESS:
    print(f"Cartesian trajectory: {len(resp.trajectory.points)} points")

Collision Checking

Check whether a robot state or path is in collision before executing it.

Single-state check

cc_client = node.create_client(mpi_srv.CheckRobotCollision, 'check_robot_collision')
cc_client.wait_for_service()

req = mpi_srv.CheckRobotCollision.Request()
req.update_objects = True
req.robot_state.joint_state.name     = JOINT_NAMES
req.robot_state.joint_state.position = HOME_POS
req.get_contact_infos = False          # set True for contact normals/distances

resp = call(cc_client, req)
print(f"In collision: {resp.is_in_collision}")

Path collision check

from trajectory_msgs.msg import JointTrajectoryPoint

pc_client = node.create_client(mpi_srv.CheckPathCollision, 'check_path_collision')
pc_client.wait_for_service()

path = JointTrajectory()
path.joint_names = JOINT_NAMES
for pos in [HOME_POS, goal_pos]:
    pt = JointTrajectoryPoint()
    pt.positions = pos
    path.points.append(pt)

req = mpi_srv.CheckPathCollision.Request()
req.update_objects = True
req.path = path
req.type = mpi_msg.CheckPathCollisionType.CHECK_ALL_POINTS   # or CHECK_UNTIL_COLLISION

resp = call(pc_client, req)
if resp.error_code.val == mpi_msg.ErrorCodes.SUCCESS:
    if not resp.index_ranges_in_collision:
        print("Path is collision-free")
    else:
        for r in resp.index_ranges_in_collision:
            upper = r.upper if r.upper >= 0 else "unknown"
            print(f"Collision at points [{r.lower}, {upper}]")

Standalone Parameterization

Time-parameterize an existing waypoint path without re-planning.

from trajectory_msgs.msg import JointTrajectoryPoint

param_client = node.create_client(mpi_srv.ComputeParameterize, 'compute_parameterize')
param_client.wait_for_service()

waypoints = [HOME_POS, [0.2, 0.3, 0.5, -0.1, 0.2, 0.0], goal_pos]

path = JointTrajectory()
path.joint_names = JOINT_NAMES
for pos in waypoints:
    pt = JointTrajectoryPoint()
    pt.positions = pos
    path.points.append(pt)

req = mpi_srv.ComputeParameterize.Request()
req.path = path
req.config.type = mpi_msg.ParameterizeConfig.TOPPRA
for name in JOINT_NAMES:
    req.config.joint_velocity_constraint.names.append(name)
    req.config.joint_velocity_constraint.max.append(2.18)
    req.config.joint_velocity_constraint.min.append(-2.18)
    req.config.joint_acceleration_constraint.names.append(name)
    req.config.joint_acceleration_constraint.max.append(5.0)
    req.config.joint_acceleration_constraint.min.append(-5.0)

resp = call(param_client, req)
if resp.error_code.val == mpi_msg.ErrorCodes.SUCCESS:
    traj = resp.trajectory
    print(f"Parameterized {len(traj.points)} points, "
          f"total time={traj.points[-1].time_from_start.sec:.2f}s")

Note

Always source install/setup.bash before running examples to ensure motion_planning_interfaces is on the Python path.