Merge pull request #409 from compas-dev/new-planning-target-class

Introduce `compas_fab.robot.Target` class

AUTHORS.rst

@@ -51,4 +51,3 @@ Authors
 * Chen Kasirer <[email protected]> `@chenkasirer <https://github.com/chenkasirer>`_
 * Edvard Bruun <[email protected]> `@ebruun <https://github.com/ebruun>`_
 * Victor Pok Yin Leung <[email protected]> `@yck011522 <https://github.com/yck011522>`_
-

CHANGELOG.md

@@ -9,12 +9,26 @@ and this project adheres to [Semantic Versioning](https://semver.org/spec/v2.0.0
 
 ### Added
 
+* Added `compas_fab.robots.Target` class to represent a motion planning target.
+* Added also child classes `FrameTarget`, `PointAxisTarget`, `ConfigurationTarget`, `ConstraintSetTarget`
+* Unlike previous constraints, `Targets` do not contain `group` as parameter. Instead, group parameter is passed to the planning call.
+* Target scaling function is now embeded in the code for Targets. `scaled()` should be called by the user before passing the target to the `plan_motion` function.
+
 ### Changed
 
+* 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.
+* Moved `Robot.position_constraint_from_frame()` to `PositionConstraint.from_frame()`, as constraints are no longer intended for users to use directly.
+* Moved `Robot.constraints_from_frame()` to ros.backend_features and is handled by `convert_target_to_goal_constraints()`. Users who wish to use a frame as target should use a `FrameTarget` instead.
 * Changed the behavior of Duration class when accepting both seconds (float) and nanoseconds (int) where the decimal point of seconds and the nanoseconds add up to more than 1 second.
+* Changed GH Component `ConstraintsFromPlane` to `FrameTargetFromPlane`.
+* Changed GH Component `ConstraintsFromTargetConfiguration` to `ConfigurationTarget`.
 
 ### Removed
 
+* Removed `plan_cartesian_motion_deprecated` and `plan_motion_deprecated` methods from `Robot` class
+* Removed `forward_kinematics_deprecated` and `inverse_kinematics_deprecated` method from `Robot` class
 
 ## [1.0.2] 2024-02-22
 

docs/examples/01_fundamentals/01_frame_and_transformation.rst

@@ -1,3 +1,5 @@
+.. _frame_and_transformation:
+
 *******************************************************************************
 Frame and Transformation
 *******************************************************************************

docs/examples/01_fundamentals/02_coordinate_frames.rst

@@ -1,3 +1,5 @@
+.. _coordinate_frames:
+
 *******************************************************************************
 Coordinate frames
 *******************************************************************************

docs/examples/02_description_models/01_kinematic_model.rst

@@ -1,3 +1,5 @@
+.. _kinematic_model:
+
 *******************************************************************************
 Kinematic model
 *******************************************************************************

docs/examples/02_description_models/02_robot.rst

@@ -1,3 +1,5 @@
+.. _robot_model:
+
 *******************************************************************************
 Robot models
 *******************************************************************************

docs/examples/02_description_models/03_targets.rst

@@ -0,0 +1,40 @@
+.. _targets:
+
+*******************************************************************************
+Targets
+*******************************************************************************
+
+-----------------------
+Single Targets (Static)
+-----------------------
+
+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).
+
+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).
+A frame target is commonly created from a :class:`compas.geometry.Frame` object, or alternatively from a :class:`compas.geometry.Transformation` object.
+
+The :class:`compas_fab.robots.PointAxisTarget` class is used for specifying a target
+based on a point (`target_point`) and a Z-Axis (`target_z_axis`).
+This is useful for example when the robot is using a cylindrical tool to perform a task,
+for example 3D printing, welding or drilling.
+In a more general case, it can be used for any tools for which the rotation
+around its own Z axis is acceptable during use.
+The point and the Z-Axis are defined relative to the tool coordinate frame (TCF).
+The goal is (1) for the tool's TCF point to coincide with the `target_point`,
+and (2) for the TCF's Z-axis to become parallel to the `target_z_axis`.
+Note that the exact orientatio of the TCF is not determined until after the target is planned.
+
+The :class:`compas_fab.robots.ConfigurationTarget` class is used to specify a target
+based on a specific robot configuration (joint values).
+For example, it can be used to move the robot to a taught position acquired by jogging.
+Typically, the ConfigurationTarget should have the same number of joints as the planning group
+of the robot. However, it is possible to specify a subset of the joints, in which
+case the remaining joints are left unspecified.
+
+The :class:`compas_fab.robots.ConstraintSetTarget` class is used to specify a list of
+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.

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

@@ -3,26 +3,27 @@
 from compas.geometry import Frame
 
 from compas_fab.backends import RosClient
+from compas_fab.robots import FrameTarget
 
 with RosClient() as client:
     robot = client.load_robot()
     assert robot.name == 'ur5_robot'
 
     frame = Frame([0.4, 0.3, 0.4], [0, 1, 0], [0, 0, 1])
     tolerance_position = 0.001
-    tolerance_axes = [math.radians(1)] * 3
+    tolerance_orientation = math.radians(1)
 
     start_configuration = robot.zero_configuration()
     start_configuration.joint_values = (-3.530, 3.830, -0.580, -3.330, 4.760, 0.000)
     group = robot.main_group_name
 
-    # create goal constraints from frame
-    goal_constraints = robot.constraints_from_frame(frame,
-                                                    tolerance_position,
-                                                    tolerance_axes,
-                                                    group)
+    # create target from frame
+    target = FrameTarget(frame,
+                         tolerance_position,
+                         tolerance_orientation,
+                         )
 
-    trajectory = robot.plan_motion(goal_constraints,
+    trajectory = robot.plan_motion(target,
                                    start_configuration,
                                    group,
                                    options=dict(

src/compas_fab/backends/interfaces/backend_features.py

@@ -87,22 +87,18 @@ class PlanMotion(object):
     <https://en.wikipedia.org/wiki/Function_object#In_Python>.
     """
 
-    def __call__(self, robot, goal_constraints, start_configuration=None, group=None, options=None):
-        return self.plan_motion(robot, goal_constraints, start_configuration, group, options)
+    def __call__(self, robot, target, start_configuration=None, group=None, options=None):
+        return self.plan_motion(robot, target, start_configuration, group, options)
 
-    def plan_motion(self, robot, goal_constraints, start_configuration=None, group=None, options=None):
+    def plan_motion(self, robot, target, start_configuration=None, group=None, options=None):
         """Calculates a motion path.
 
         Parameters
         ----------
         robot : :class:`compas_fab.robots.Robot`
             The robot instance for which the motion path is being calculated.
-        goal_constraints: list of :class:`compas_fab.robots.Constraint`
-            The goal to be achieved, defined in a set of constraints.
-            Constraints can be very specific, for example defining value domains
-            for each joint, such that the goal configuration is included,
-            or defining a volume in space, to which a specific robot link (e.g.
-            the end-effector) is required to move to.
+        target: :class:`compas_fab.robots.Target`
+            The goal for the robot to achieve.
         start_configuration: :class:`compas_fab.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/ros/backend_features/helpers.py

@@ -12,9 +12,16 @@
 from compas_fab.backends.ros.messages import MoveItErrorCodes
 from compas_fab.backends.ros.messages import OrientationConstraint
 from compas_fab.backends.ros.messages import PositionConstraint
+from compas_fab.robots import ConfigurationTarget
+from compas_fab.robots import ConstraintSetTarget
 from compas_fab.robots import Duration
+from compas_fab.robots import FrameTarget
 from compas_fab.robots import JointTrajectory
 from compas_fab.robots import JointTrajectoryPoint
+from compas_fab.robots import PointAxisTarget
+from compas_fab.robots.constraints import JointConstraint as CF_JointConstraint
+from compas_fab.robots.constraints import PositionConstraint as CF_PositionConstraint
+from compas_fab.robots.constraints import OrientationConstraint as CF_OrientationConstraint
 
 
 def validate_response(response):
@@ -23,6 +30,63 @@ def validate_response(response):
         raise RosError(response.error_code.human_readable, int(response.error_code))
 
 
+def convert_target_to_goal_constraints(target, ee_link_name):
+    """Convert COMPAS FAB `Target` objects into `Constraint` objects for passing it to MoveIt backend.
+    This function is intended to be called only by MoveIt backend when handling different Target types.
+
+    Parameters
+    ----------
+    target: :class:`compas_fab.robots.Target`
+        The target for the robot to reach.
+
+    Returns
+    -------
+    list of :class:`Constraint`
+        List of constraint objects.
+    """
+
+    if isinstance(target, ConstraintSetTarget):
+        return target.constraint_set
+
+    elif isinstance(target, ConfigurationTarget):
+        configuration = target.target_configuration
+        tolerance_above = target.tolerance_above
+        tolerance_below = target.tolerance_below
+        return CF_JointConstraint.joint_constraints_from_configuration(configuration, tolerance_above, tolerance_below)
+
+    elif isinstance(target, FrameTarget):
+        tcf_frame_in_wcf = target.target_frame
+        tool_coordinate_frame = target.tool_coordinate_frame
+        pc = CF_PositionConstraint.from_frame(
+            tcf_frame_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame
+        )
+        oc = CF_OrientationConstraint.from_frame(
+            tcf_frame_in_wcf, [target.tolerance_orientation] * 3, ee_link_name, tool_coordinate_frame
+        )
+        return [pc, oc]
+
+    elif isinstance(target, PointAxisTarget):
+        tcf_point_in_wcf = target.target_point
+        tool_coordinate_frame = target.tool_coordinate_frame
+
+        if tool_coordinate_frame:
+            raise NotImplementedError(
+                "Tool coordinate frame is not yet supported when converting PointAxisTarget to ConstraintSetTarget."
+            )
+
+        pc = CF_PositionConstraint.from_point(
+            tcf_point_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame
+        )
+        OC_TOLERANCE_FOR_FREE_ROTATION = [0.01, 0.01, 6.3]
+        oc = CF_OrientationConstraint.from_frame(
+            tcf_frame_in_wcf, OC_TOLERANCE_FOR_FREE_ROTATION, ee_link_name, tool_coordinate_frame
+        )
+        return [pc, oc]
+
+    else:
+        raise NotImplementedError("Target type {} not supported by ROS planning backend.".format(type(target)))
+
+
 def convert_constraints_to_rosmsg(constraints, header):
     """Convert COMPAS FAB constraints into ROS Messages."""
     if not constraints:

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

@@ -6,6 +6,7 @@
 
 from compas_fab.backends.interfaces import PlanMotion
 from compas_fab.backends.ros.backend_features.helpers import convert_constraints_to_rosmsg
+from compas_fab.backends.ros.backend_features.helpers import convert_target_to_goal_constraints
 from compas_fab.backends.ros.backend_features.helpers import convert_trajectory
 from compas_fab.backends.ros.backend_features.helpers import validate_response
 from compas_fab.backends.ros.messages import AttachedCollisionObject
@@ -22,7 +23,7 @@
 
 
 class MoveItPlanMotion(PlanMotion):
-    """Callable to find a path plan to move the selected robot from its current position within the `goal_constraints`."""
+    """Callable to find a path in joint space for the robot to move from its `start_configuration` to the `target`."""
 
     GET_MOTION_PLAN = ServiceDescription(
         "/plan_kinematic_path", "GetMotionPlan", MotionPlanRequest, MotionPlanResponse, validate_response
@@ -31,19 +32,15 @@ class MoveItPlanMotion(PlanMotion):
     def __init__(self, ros_client):
         self.ros_client = ros_client
 
-    def plan_motion(self, robot, goal_constraints, start_configuration=None, group=None, options=None):
+    def plan_motion(self, robot, target, start_configuration=None, group=None, options=None):
         """Calculates a motion path.
 
         Parameters
         ----------
         robot : :class:`compas_fab.robots.Robot`
             The robot instance for which the motion plan is being calculated.
-        goal_constraints: list of :class:`compas_fab.robots.Constraint`
-            The goal to be achieved, defined in a set of constraints.
-            Constraints can be very specific, for example defining value domains
-            for each joint, such that the goal configuration is included,
-            or defining a volume in space, to which a specific robot link (e.g.
-            the end-effector) is required to move to.
+        target: list of :class:`compas_fab.robots.Target`
+            The target for the robot to reach.
         start_configuration: :class:`compas_fab.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 +82,7 @@ def plan_motion(self, robot, goal_constraints, start_configuration=None, group=N
         """
         options = options or {}
         kwargs = {}
-        kwargs["goal_constraints"] = goal_constraints
+        kwargs["target"] = target
         kwargs["start_configuration"] = start_configuration
         kwargs["group"] = group
         kwargs["options"] = options
@@ -94,12 +91,11 @@ def plan_motion(self, robot, goal_constraints, start_configuration=None, group=N
         # Use base_link or fallback to model's root link
         options["base_link"] = options.get("base_link", robot.model.root.name)
         options["joints"] = {j.name: j.type for j in robot.model.joints}
+        options["ee_link_name"] = robot.get_end_effector_link_name(group)
 
         return await_callback(self.plan_motion_async, **kwargs)
 
-    def plan_motion_async(
-        self, callback, errback, goal_constraints, start_configuration=None, group=None, options=None
-    ):
+    def plan_motion_async(self, callback, errback, target, start_configuration=None, group=None, options=None):
         """Asynchronous handler of MoveIt motion planner service."""
         # http://docs.ros.org/jade/api/moveit_core/html/utils_8cpp_source.html
         # TODO: if list of frames (goals) => receive multiple solutions?
@@ -120,6 +116,8 @@ def plan_motion_async(
         start_state.filter_fields_for_distro(self.ros_client.ros_distro)
 
         # convert constraints
+        ee_link_name = options["ee_link_name"]
+        goal_constraints = convert_target_to_goal_constraints(target, ee_link_name)
         goal_constraints = [convert_constraints_to_rosmsg(goal_constraints, header)]
         path_constraints = convert_constraints_to_rosmsg(options.get("path_constraints"), header)
         trajectory_constraints = options.get("trajectory_constraints")

src/compas_fab/ghpython/components/Cf_ConfigurationTarget/code.py

@@ -0,0 +1,30 @@
+"""
+Create configuration target for the robot's end-effector motion planning.
+
+COMPAS FAB v1.0.2
+"""
+
+import math
+
+from ghpythonlib.componentbase import executingcomponent as component
+
+from compas_fab.robots import ConfigurationTarget
+
+
+class ConfigurationTargetComponent(component):
+    DEFAULT_TOLERANCE_METERS = 0.001
+    DEFAULT_TOLERANCE_RADIANS = math.radians(1)
+
+    def RunScript(self, robot, target_configuration, tolerances_above, tolerances_below):
+        if robot and target_configuration:
+            default_tolerances_above, default_tolerances_below = ConfigurationTarget.generate_default_tolerances(
+                target_configuration, self.DEFAULT_TOLERANCE_METERS, self.DEFAULT_TOLERANCE_RADIANS
+            )
+
+            target = ConfigurationTarget(
+                target_configuration=target_configuration,
+                tolerances_above=tolerances_above or default_tolerances_above,
+                tolerances_below=tolerances_below or default_tolerances_below,
+            )
+
+            return target

src/compas_fab/ghpython/components/Cf_ConstraintsFromConfiguration/metadata.json → src/compas_fab/ghpython/components/Cf_ConfigurationTarget/metadata.json

@@ -1,11 +1,10 @@
 {
-    "name": "Constraints From Target Configuration",
-    "nickname": "Constraints From Configuration",
+    "name": "Target from a configuration",
+    "nickname": "Configuration target",
     "category": "COMPAS FAB",
     "subcategory": "Planning",
-    "description": "Create joint constraints for each of the robot's configurable joints based on a given target configuration.",
+    "description": "Create configuration target for the robot's end-effector motion planning.",
     "exposure": 8,
-
     "ghpython": {
         "isAdvancedMode": true,
         "iconDisplay": 2,
@@ -17,7 +16,7 @@
             },
             {
                 "name": "target_configuration",
-                "description": "The target configuration."
+                "description": "The target configuration, in radians/meters."
             },
             {
                 "name": "tolerance_above",
@@ -28,18 +27,13 @@
                 "name": "tolerance_below",
                 "description": "The tolerances below the targeted joint value of each configurable joint, defining the lower bound in radians/meters to be achieved.",
                 "typeHintID": "float"
-            },
-            {
-                "name": "group",
-                "description": "The planning group for which we specify the constraint. Defaults to the robot's main planning group.",
-                "typeHintID": "str"
             }
         ],
         "outputParameters": [
             {
-                "name": "constraints",
-                "description": "A list joint constraints in radians/meters."
+                "name": "target",
+                "description": "A configuration target"
             }
         ]
     }
-}
+}