Raise exception on partial trajectories

Author: gonzalocasas

CHANGELOG.md

@@ -130,6 +130,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 * Changed `RosDistro` to be an enum instead of string.
 * Changed `TargetMode` to be an enum instead of string.
 * Changed default values for number of planning attempts in MoveIt to 5, and the allowed planning time to 1 second.
+* Changed cartesian motion planner of MoveIt to raise an exception (including the partial trajectory) if fraction is below 1.0 (ie. the trajectory is not possible).
 
 ### Removed
 

docs/backends/ros/files/06_cartesian_motion_partial.py

@@ -0,0 +1,49 @@
+from compas.geometry import Box
+from compas.geometry import Frame
+
+from compas_fab.backends import MotionPlanningError
+from compas_fab.backends import MoveItPlanner
+from compas_fab.backends import RosClient
+from compas_fab.robots import FrameWaypoints
+from compas_fab.robots import RigidBody
+from compas_fab.robots import TargetMode
+
+with RosClient() as client:
+    robot_cell = client.load_robot_cell()
+    planner = MoveItPlanner(client)
+    assert robot_cell.robot_model.name == "ur5_robot"
+
+    # Add an obstacle
+    box_mesh = Box(0.1, 0.1, 1.2).to_mesh(triangulated=True)
+    robot_cell.rigid_body_models["box"] = RigidBody.from_mesh(box_mesh)
+    planner.set_robot_cell(robot_cell)
+
+    # Define Starting State
+    start_state = robot_cell.default_cell_state()
+    start_state.robot_configuration.joint_values = [-3.53, 3.83, -0.58, -3.33, 4.76, 0.00]
+    # Set the obstacle's position
+    start_state.rigid_body_states["box"].frame.point = [0.3, 0.0, 0.0]
+
+    # Create waypoints
+    frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
+    waypoints = FrameWaypoints([frame], TargetMode.ROBOT)
+
+    # This planning is expected to fail because it's impossible to follow the trajectory
+    # in a linear motion, so the planner will fail but return a partial trajectory.
+    try:
+        trajectory = planner.plan_cartesian_motion(waypoints, start_state)
+        print(f"Trajectory fraction: {trajectory.fraction}")
+        print(f"Trajectory contains {len(trajectory.points)} configurations (JointTrajectoryPoint).")
+        print(f"Executing this path at full speed would take approx. {trajectory.time_from_start} seconds.")
+    except MotionPlanningError as e:
+        print(f"Motion planning failed: {e}")
+        print(f"Trajectory fraction: {e.partial_trajectory.fraction}")
+        print(f"Trajectory contains {len(e.partial_trajectory.points)} configurations (JointTrajectoryPoint).")
+
+
+"""
+Output: (may vary)
+>>> Motion planning failed: Motion planning failed
+>>> Trajectory fraction: 0.14285714285714285
+>>> Trajectory contains 12 configurations (JointTrajectoryPoint).
+"""

src/compas_fab/backends/ros/backend_features/move_it_plan_cartesian_motion.py

@@ -3,6 +3,7 @@
 
 from compas.utilities import await_callback
 
+from compas_fab.backends.exceptions import MotionPlanningError
 from compas_fab.backends.interfaces import PlanCartesianMotion
 from compas_fab.backends.ros.backend_features.helpers import convert_constraints_to_rosmsg
 from compas_fab.backends.ros.backend_features.helpers import convert_trajectory
@@ -181,6 +182,8 @@ def response_handler(response):
                 trajectory = convert_trajectory(
                     joints, response.solution, response.start_state, response.fraction, None, response
                 )
+                if response.fraction < 1:
+                    errback(MotionPlanningError("Motion planning failed", trajectory))
                 callback(trajectory)
             except Exception as e:
                 errback(e)