Merge pull request #422 from compas-dev/feature_plan_c_motion_uses_wa…

…ypoints

Plan_cartesian_motion uses Waypoints class

CHANGELOG.md

@@ -17,6 +17,7 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Changed
 
+* `Robot.plan_cartesian_motion()` now accepts `Waypoints` as target. Implementation for `FrameWaypoints` is supported with same functionality as before. Simply wrap `Frame` objects using `FrameWaypoints(frames)`.
 * Changed `BoundingVolume`, `Constraint`, `JointConstraint`, `OrientationConstraint`, `PositionConstraint` to inherit from `compas.data.Data` class.
 * Change the signature of `plan_motion()` to use `target` (`Target` class) instead of `goal_constraints`. Only one target is accepted. Users who wish to compose their own constraint sets can still use `ConstraintSetTarget`.
 * Moved `Robot.orientation_constraint_from_frame()` to `OrientationConstraint.from_frame()`, as constraints are no longer intended for users to use directly.

docs/examples/02_description_models/03_targets.rst

@@ -1,16 +1,15 @@
 .. _targets:
 
 *******************************************************************************
-Targets
+Targets and Waypoints
 *******************************************************************************
 
 -----------------------
-Single Targets (Static)
+Targets (Single Goal)
 -----------------------
 
 Target classes are used to describe the goal condition (i.e. end condition) of a robot
-for motion planning. They can be used for both Free Motion Planning (FMP) and Cartesian
-Motion Planning (CMP).
+for motion planning. They can be used for Free-space Motion Planning with :meth:`compas_fab.robots.Robot.plan_motion`.
 
 The :class:`compas_fab.robots.FrameTarget` is the most common target for motion planning.
 It defines the complete pose of the end-effector (or the robot flange, if no tool is attached).
@@ -38,3 +37,29 @@ The :class:`compas_fab.robots.ConstraintSetTarget` class is used to specify a li
 constraints as a planning target. This is intended for advanced users who want to create custom
 combination of constraints. See :class:`compas_fab.robots.Constraint` for available
 constraints. At the moment, only the ROS MoveIt planning backend supports this target type.
+
+.. _waypoints:
+
+------------------------------------------
+Waypoints (Multiple Points / Segments)
+------------------------------------------
+
+Waypoints classes are used to describe a sequence of
+waypoints that the robot should pass through in a planned motion. They are similar to Targets classes
+but contain a list of targets instead of a single target, which is useful for tasks such as
+drawing, welding or 3D printing.
+They can be used for Cartesian Motion Planning with :meth:`compas_fab.robots.Robot.plan_cartesian_motion`.
+
+The :class:`compas_fab.robots.FrameWaypoints` is the most common waypoint for Cartesian motion planning.
+It defines a list of complete pose for the end-effector (or the robot flange, if no tool is attached).
+It is created by a list of :class:`compas.geometry.Frame` objects or alternatively from a list of
+:class:`compas.geometry.Transformation` objects.
+
+The :class:`compas_fab.robots.PointAxisWaypoints` class is used for specifying a list of waypoints based on
+the Point-Axis concept used in the :class:`compas_fab.robots.PointAxisTarget`. Compared to
+:class:`~compas_fab.robots.FrameWaypoints`, this class allows for specifying targets where the rotation
+around the Z-axis is not fixed. This is useful for example when the robot is using a cylindrical tool
+to perform a task, for example 3D printing, welding or drilling. The freely rotating axis is defined relative
+to the Z-axis of the tool coordinate frame (TCF). Note that the orientation of the tool
+at the end of the motion is not determined until after the motion is planned.
+

docs/examples/03_backends_ros/files/04_plan_cartesian_motion.py

@@ -1,25 +1,25 @@
 from compas.geometry import Frame
 
 from compas_fab.backends import RosClient
+from compas_fab.robots import FrameWaypoints
 
 with RosClient() as client:
     robot = client.load_robot()
-    assert robot.name == 'ur5_robot'
+    assert robot.name == "ur5_robot"
 
     frames = []
     frames.append(Frame([0.3, 0.1, 0.5], [1, 0, 0], [0, 1, 0]))
     frames.append(Frame([0.5, 0.1, 0.6], [1, 0, 0], [0, 1, 0]))
+    waypoints = FrameWaypoints(frames)
 
     start_configuration = robot.zero_configuration()
     start_configuration.joint_values = (-0.042, 0.033, -2.174, 5.282, -1.528, 0.000)
     options = {
-        'max_step': 0.01,
-        'avoid_collisions': True,
+        "max_step": 0.01,
+        "avoid_collisions": True,
     }
 
-    trajectory = robot.plan_cartesian_motion(frames,
-                                             start_configuration,
-                                             options=options)
+    trajectory = robot.plan_cartesian_motion(waypoints, start_configuration, options=options)
 
     print("Computed cartesian path with %d configurations, " % len(trajectory.points))
     print("following %d%% of requested trajectory." % (trajectory.fraction * 100))

docs/examples/03_backends_ros/files/gh_plan_cartesian_motion.py

@@ -22,11 +22,12 @@
         :class:`compas_fab.robots.JointTrajectory`
             The calculated trajectory.
 """
+
 from __future__ import print_function
 import scriptcontext as sc
 
 from compas.geometry import Frame
-
+from compas_fab.robots import FrameWaypoints
 
 guid = str(ghenv.Component.InstanceGuid)
 response_key = "response_" + guid
@@ -38,14 +39,14 @@
 if robot and robot.client and start_configuration and compute:
     if robot.client.is_connected:
         options = {
-            'max_step': float(max_step),
-            'avoid_collisions': bool(avoid_collisions),
-            'attached_collision_meshes': list(attached_colllision_meshes),
+            "max_step": float(max_step),
+            "avoid_collisions": bool(avoid_collisions),
+            "attached_collision_meshes": list(attached_colllision_meshes),
         }
-        sc.sticky[response_key] = robot.plan_cartesian_motion(frames,
-                                                              start_configuration=start_configuration,
-                                                              group=group,
-                                                              options=options)
+        waypoints = FrameWaypoints(frames)
+        sc.sticky[response_key] = robot.plan_cartesian_motion(
+            waypoints, start_configuration=start_configuration, group=group, options=options
+        )
     else:
         print("Robot client is not connected")
 

docs/examples/06_backends_kinematics/files/04_cartesian_path_analytic_pybullet.py

@@ -2,6 +2,7 @@
 from compas.geometry import Frame
 
 from compas_fab.backends import AnalyticalPyBulletClient
+from compas_fab.robots import FrameWaypoints
 
 frames_WCF = [
     Frame((0.407, 0.073, 0.320), (0.922, 0.000, 0.388), (0.113, 0.956, -0.269)),
@@ -16,7 +17,8 @@
 
     options = {"solver": "ur5", "check_collision": True}
     start_configuration = list(robot.iter_inverse_kinematics(frames_WCF[0], options=options))[-1]
-    trajectory = robot.plan_cartesian_motion(frames_WCF, start_configuration=start_configuration, options=options)
+    waypoints = FrameWaypoints(frames_WCF)
+    trajectory = robot.plan_cartesian_motion(waypoints, start_configuration=start_configuration, options=options)
     assert trajectory.fraction == 1.0
 
     j = [c.joint_values for c in trajectory.points]

src/compas_fab/backends/interfaces/backend_features.py

@@ -125,18 +125,18 @@ class PlanCartesianMotion(object):
     <https://en.wikipedia.org/wiki/Function_object#In_Python>.
     """
 
-    def __call__(self, robot, frames_WCF, start_configuration=None, group=None, options=None):
-        return self.plan_cartesian_motion(robot, frames_WCF, start_configuration, group, options)
+    def __call__(self, robot, waypoints, start_configuration=None, group=None, options=None):
+        return self.plan_cartesian_motion(robot, waypoints, start_configuration, group, options)
 
-    def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None):
+    def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, group=None, options=None):
         """Calculates a cartesian motion path (linear in tool space).
 
         Parameters
         ----------
         robot : :class:`compas_fab.robots.Robot`
             The robot instance for which the cartesian motion path is being calculated.
-        frames_WCF: list of :class:`compas.geometry.Frame`
-            The frames through which the path is defined.
+        waypoints : :class:`compas_fab.robots.Waypoints`
+            The waypoints for the robot to follow.
         start_configuration: :class:`compas_robots.Configuration`, optional
             The robot's full configuration, i.e. values for all configurable
             joints of the entire robot, at the starting position.

src/compas_fab/backends/kinematics/analytical_plan_cartesian_motion.py

@@ -7,6 +7,9 @@
 from compas_fab.backends.kinematics.utils import smallest_joint_angles
 from compas_fab.robots import JointTrajectory
 from compas_fab.robots import JointTrajectoryPoint
+from compas_fab.robots import FrameWaypoints
+from compas_fab.robots import PointAxisWaypoints
+from compas_fab.utilities import from_tcf_to_t0cf
 
 
 class AnalyticalPlanCartesianMotion(PlanCartesianMotion):
@@ -15,23 +18,22 @@ class AnalyticalPlanCartesianMotion(PlanCartesianMotion):
     def __init__(self, client=None):
         self.client = client
 
-    def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None):
+    def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, group=None, options=None):
         """Calculates a cartesian motion path (linear in tool space).
 
         Parameters
         ----------
         robot : :class:`compas_fab.robots.Robot`
             The robot instance for which the cartesian motion path is being calculated.
-        frames_WCF : list of :class:`compas.geometry.Frame`
-            The frames through which the path is defined.
+        waypoints : :class:`compas_fab.robots.Waypoints`
+            The waypoints for the robot to follow.
         start_configuration : :class:`compas_robots.Configuration`, optional
             The robot's full configuration, i.e. values for all configurable
             joints of the entire robot, at the starting position.
         group : str, optional
             The planning group used for calculation.
         options : dict, optional
-            Dictionary containing kwargs for arguments specific to
-            the client being queried.
+            Dictionary containing the key-value pairs that are passed to :func:`compas_fab.robots.Robot.iter_inverse_kinematics`
 
         Returns
         -------
@@ -42,22 +44,55 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro
         -----
         This will only work with robots that have 6 revolute joints.
         """
-        # what is the expected behaviour of that function?
-        # - Return all possible paths or select only the one that is closest to the start_configuration?
-        # - Do we use a stepsize to sample in between frames or use only the input frames?
+
+        if isinstance(waypoints, FrameWaypoints):
+            return self._plan_cartesian_motion_with_frame_waypoints(
+                robot, waypoints, start_configuration, group, options
+            )
+        elif isinstance(waypoints, PointAxisWaypoints):
+            return self._plan_cartesian_motion_with_point_axis_waypoints(
+                robot, waypoints, start_configuration, group, options
+            )
+        else:
+            raise TypeError("Unsupported waypoints type {}".format(type(waypoints)))
+
+    def _plan_cartesian_motion_with_frame_waypoints(
+        self, robot, waypoints, start_configuration=None, group=None, options=None
+    ):
+        """Calculates a cartesian motion path with frame waypoints.
+
+        Planner behavior:
+        - If multiple paths are possible (i.e. due to multiple IK results), only the one that is closest to the start_configuration is returned.
+        - The path is checked to ensure that the joint values are continuous and that revolution values are the smallest possible.
+        - There is no interpolation in between frames (i.e. 'max_step' parameter is not supported), only the input frames are used.
+        """
+        # convert the target frames to the robot's base frame
+        if waypoints.tool_coordinate_frame is not None:
+            frames_WCF = [from_tcf_to_t0cf(frame, waypoints.tool_coordinate_frame) for frame in waypoints.target_frames]
+        else:
+            frames_WCF = waypoints.target_frames
 
         # convert the frame WCF to RCF
         base_frame = robot.get_base_frame(group=group, full_configuration=start_configuration)
         frames_RCF = [base_frame.to_local_coordinates(frame_WCF) for frame_WCF in frames_WCF]
 
+        # 'keep_order' is set to True, so that iter_inverse_kinematics will return the configurations in the same order across all frames
         options = options or {}
         options.update({"keep_order": True})
 
+        # iterate over all input frames and calculate the inverse kinematics, no interpolation in between frames
         configurations_along_path = []
         for frame in frames_RCF:
             configurations = list(robot.iter_inverse_kinematics(frame, options=options))
             configurations_along_path.append(configurations)
 
+        # Analytical backend only supports robots with finite IK solutions
+        # For 6R articulated robots, there is a maximum of 8 possible paths, corresponding to the 8 possible IK solutions for each frame
+        # The `options.update({"keep_order": True})` ensures that the order of the configurations is the same across all frames
+        # but this also cause some configurations to be None, if no solution was found.
+
+        # The `all(configurations)` below is used to check if all configurations in a path are present.
+        # indicating that a complete trajectory was found.
         paths = []
         for configurations in zip(*configurations_along_path):
             if all(configurations):
@@ -75,11 +110,20 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro
         path = self.smooth_configurations(path)
         trajectory = JointTrajectory()
         trajectory.fraction = len(path) / len(frames_RCF)
+        # Technically trajectory.fraction should always be 1.0 because otherwise, the path would be rejected earlier
         trajectory.joint_names = path[0].joint_names
         trajectory.points = [JointTrajectoryPoint(config.joint_values, config.joint_types) for config in path]
         trajectory.start_configuration = robot.merge_group_with_full_configuration(path[0], start_configuration, group)
         return trajectory
 
+    def _plan_cartesian_motion_with_point_axis_waypoints(
+        self, robot, waypoints, start_configuration=None, group=None, options=None
+    ):
+        """Planning Cartesian motion with PointAxisWaypoints is not yet implemented in the Analytical backend."""
+        raise NotImplementedError(
+            "Planning Cartesian motion with PointAxisWaypoints is not yet implemented in the Analytical backend."
+        )
+
     def smooth_configurations(self, configurations):
         joint_values_corrected = []
         prev = smallest_joint_angles(configurations[0].joint_values)

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

@@ -18,6 +18,9 @@
 from compas_fab.backends.ros.messages import RobotState
 from compas_fab.backends.ros.service_description import ServiceDescription
 
+from compas_fab.robots import FrameWaypoints
+from compas_fab.robots import PointAxisWaypoints
+
 __all__ = [
     "MoveItPlanCartesianMotion",
 ]
@@ -37,15 +40,15 @@ class MoveItPlanCartesianMotion(PlanCartesianMotion):
     def __init__(self, ros_client):
         self.ros_client = ros_client
 
-    def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, group=None, options=None):
+    def plan_cartesian_motion(self, robot, waypoints, start_configuration=None, group=None, options=None):
         """Calculates a cartesian motion path (linear in tool space).
 
         Parameters
         ----------
         robot : :class:`compas_fab.robots.Robot`
             The robot instance for which the cartesian motion plan is being calculated.
-        frames_WCF: list of :class:`compas.geometry.Frame`
-            The frames through which the path is defined.
+        waypoints : :class:`compas_fab.robots.Waypoints`
+            The waypoints for the robot to follow.
         start_configuration: :class:`compas_robots.Configuration`, optional
             The robot's full configuration, i.e. values for all configurable
             joints of the entire robot, at the starting position. Defaults to
@@ -85,7 +88,7 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro
         options = options or {}
         kwargs = {}
         kwargs["options"] = options
-        kwargs["frames_WCF"] = frames_WCF
+        kwargs["waypoints"] = waypoints
         kwargs["start_configuration"] = start_configuration
         kwargs["group"] = group
 
@@ -99,16 +102,30 @@ def plan_cartesian_motion(self, robot, frames_WCF, start_configuration=None, gro
         if options["link"] not in robot.get_link_names(group):
             raise ValueError("Link name {} does not exist in planning group".format(options["link"]))
 
-        return await_callback(self.plan_cartesian_motion_async, **kwargs)
+        # This function wraps multiple implementations depending on the type of waypoints
+        if isinstance(waypoints, FrameWaypoints):
+            return await_callback(self.plan_cartesian_motion_with_frame_waypoints_async, **kwargs)
+        elif isinstance(waypoints, PointAxisWaypoints):
+            return self.plan_cartesian_motion_with_point_axis_waypoints_async(**kwargs)
+        else:
+            raise TypeError("Unsupported waypoints type {} for MoveIt planning backend.".format(type(waypoints)))
 
-    def plan_cartesian_motion_async(
-        self, callback, errback, frames_WCF, start_configuration=None, group=None, options=None
+    def plan_cartesian_motion_with_frame_waypoints_async(
+        self, callback, errback, waypoints, start_configuration=None, group=None, options=None
     ):
-        """Asynchronous handler of MoveIt cartesian motion planner service."""
+        """Asynchronous handler of MoveIt cartesian motion planner service.
+
+        :class:`compas_fab.robots.FrameWaypoints` are converted to :class:`compas_fab.backends.ros.messages.Pose` that is native to ROS communication
+
+        """
+
         joints = options["joints"]
 
         header = Header(frame_id=options["base_link"])
-        waypoints = [Pose.from_frame(frame) for frame in frames_WCF]
+
+        # Convert compas_fab.robots.FrameWaypoints to list of Pose for ROS
+        list_of_pose = [Pose.from_frame(frame) for frame in waypoints.target_frames]
+
         joint_state = JointState(
             header=header, name=start_configuration.joint_names, position=start_configuration.joint_values
         )
@@ -129,7 +146,7 @@ def plan_cartesian_motion_async(
             start_state=start_state,
             group_name=group,
             link_name=options["link"],
-            waypoints=waypoints,
+            waypoints=list_of_pose,
             max_step=float(options.get("max_step", 0.01)),
             jump_threshold=float(options.get("jump_threshold", 1.57)),
             avoid_collisions=bool(options.get("avoid_collisions", True)),
@@ -146,3 +163,12 @@ def response_handler(response):
                 errback(e)
 
         self.GET_CARTESIAN_PATH(self.ros_client, request, response_handler, errback)
+
+    def plan_cartesian_motion_with_point_axis_waypoints_async(
+        self, callback, errback, waypoints, start_configuration=None, group=None, options=None
+    ):
+        """Asynchronous handler of MoveIt cartesian motion planner service.
+
+        AFAIK MoveIt does not support planning for a relaxed axis under this
+        """
+        raise NotImplementedError("PointAxisWaypoints are not supported by MoveIt backend")