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
-----

.. code-block:: python

   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.

.. code-block:: python

   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.

.. code-block:: python

   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.

.. code-block:: python

   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.

.. code-block:: python

   # 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**

.. code-block:: python

   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**

.. code-block:: python

   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.

.. code-block:: python

   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.