Add tolerance_orientation to PointAxisTarget class.

src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py

@@ -665,7 +665,9 @@ def set_random_config():
                 rotated_frame = initial_frame.rotated(theta, initial_frame.zaxis, initial_frame.point)
                 if options["verbose"]:
                     print("Rotated Frame: {}, theta: {}".format(rotated_frame, theta))
-                frame_target = FrameTarget(rotated_frame, target.target_mode)
+                frame_target = FrameTarget(
+                    rotated_frame, target.target_mode, target.tolerance_position, target.tolerance_orientation
+                )
 
                 # Call underlying FrameTarget function to get IK solutions
                 ik_frame_target = self.iter_inverse_kinematics_frame_target(

src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py

@@ -372,6 +372,7 @@ def plan_cartesian_motion_point_axis_waypoints(self, waypoints, start_state, gro
             start_frame.zaxis,
             target_mode=waypoints.target_mode,
             tolerance_position=waypoints.tolerance_position,
+            tolerance_orientation=waypoints.tolerance_orientation,
         )
 
         # Interpolation is performed for all the waypoints before planning.
@@ -398,6 +399,7 @@ def plan_cartesian_motion_point_axis_waypoints(self, waypoints, start_state, gro
                     interpolated_axis,
                     target_mode=waypoints.target_mode,
                     tolerance_position=waypoints.tolerance_position,
+                    tolerance_orientation=waypoints.tolerance_orientation,
                 )
                 all_targets.append(target)
 

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

@@ -76,8 +76,12 @@ def convert_target_to_goal_constraints(target, ee_link_name, tool_coordinate_fra
             tcf_point_in_wcf, target.tolerance_position, ee_link_name, tool_coordinate_frame
         )
         OC_TOLERANCE_FOR_FREE_ROTATION = [0.01, 0.01, 6.3]
+        if target.tolerance_orientation:
+            tolerances_orientation = [target.tolerance_orientation, target.tolerance_orientation, 6.3]
+        else:
+            tolerances_orientation = OC_TOLERANCE_FOR_FREE_ROTATION
         oc = CF_OrientationConstraint.from_frame(
-            tcf_frame_in_wcf, OC_TOLERANCE_FOR_FREE_ROTATION, ee_link_name, tool_coordinate_frame
+            tcf_frame_in_wcf, tolerances_orientation, ee_link_name, tool_coordinate_frame
         )
         return [pc, oc]
 

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

@@ -14,14 +14,18 @@
 
 
 class PointAxisTargetComponent(component):
-    def RunScript(self, point, target_z_axis, tolerance_position, tool_coordinate_frame):
+    def RunScript(
+        self, point, target_z_axis, target_mode, tolerance_position, tolerance_orientation, tool_coordinate_frame
+    ):
         target = None
         if point:
 
             # Convert Rhino geometry to COMPAS geometry
             point = point if isinstance(point, Point) else point_to_compas(point)
             target_z_axis = target_z_axis if isinstance(target_z_axis, Vector) else vector_to_compas(target_z_axis)
 
-            target = PointAxisTarget(point, target_z_axis, tolerance_position, tool_coordinate_frame)
+            target = PointAxisTarget(
+                point, target_z_axis, target_mode, tolerance_position, tolerance_orientation, tool_coordinate_frame
+            )
 
         return target

src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json

@@ -17,9 +17,19 @@
                 "name": "target_z_vector",
                 "description": "The target axis is defined by the target_point and pointing towards this vector. The tool tip coordinate frame (TCF)'s Z axis can rotate around this axis. Accepts Rhino / Grasshopper Vector or COMPAS Vector."
             },
+            {
+                "name": "target_mode",
+                "description": "The target mode. Options are: 'ROBOT', 'TOOL' or 'WORKPIECE'",
+                "typeHintID": "str"
+            },
             {
                 "name": "tolerance_position",
-                "description": "The allowed tolerance of the tool reaching the target point.",
+                "description": "The allowed tolerance when reaching the target point.",
+                "typeHintID": "float"
+            },
+            {
+                "name": "tolerance_orientation",
+                "description": "The allowed tolerance when reaching the target orientation.",
                 "typeHintID": "float"
             },
             {

src/compas_fab/robots/targets.py

@@ -241,6 +241,11 @@ class PointAxisTarget(Target):
         See :class:`TargetMode` for more details.
     tolerance_position : float, optional
         The tolerance for the position of the target point.
+        Unit is meters.
+        If not specified, the default value from the planner is used.
+    tolerance_orientation : float, optional
+        The tolerance for matching the target axis orientation.
+        Unit is in radians.
         If not specified, the default value from the planner is used.
     name : str, optional
         The human-readable name of the target.
@@ -253,14 +258,16 @@ def __init__(
         target_z_axis,
         target_mode,
         tolerance_position=None,
+        tolerance_orientation=None,
         name="Point-Axis Target",
     ):
-        # type: (Point, Vector, TargetMode | str, Optional[float], Optional[str]) -> None
+        # type: (Point, Vector, TargetMode | str, Optional[float], Optional[float], Optional[str]) -> None
         super(PointAxisTarget, self).__init__(target_mode=target_mode, name=name)
         # Note: The following input are converted to class because it can simplify functions that use this class
         self.target_point = Point(*target_point)
         self.target_z_axis = Vector(*target_z_axis)
         self.tolerance_position = tolerance_position
+        self.tolerance_orientation = tolerance_orientation
 
     @property
     def __data__(self):
@@ -269,6 +276,7 @@ def __data__(self):
             "target_mode": self.target_mode,
             "target_z_axis": self.target_z_axis,
             "tolerance_position": self.tolerance_position,
+            "tolerance_orientation": self.tolerance_orientation,
             "name": self.name,
         }
 
@@ -289,7 +297,9 @@ def scaled(self, factor):
         target_point = self.target_point.scaled(factor)
         tolerance_position = self.tolerance_position * factor if self.tolerance_position else None
         target_z_axis = self.target_z_axis  # Vector is unitized and is not scaled
-        return PointAxisTarget(target_point, target_z_axis, self.target_mode, tolerance_position, self.name)
+        return PointAxisTarget(
+            target_point, target_z_axis, self.target_mode, tolerance_position, self.tolerance_orientation, self.name
+        )
 
 
 class ConfigurationTarget(Target):
@@ -684,6 +694,11 @@ class PointAxisWaypoints(Waypoints):
         See :class:`TargetMode` for more details.
     tolerance_position : float, optional
         The tolerance for the position of the target point.
+        Unit is meters.
+        If not specified, the default value from the planner is used.
+    tolerance_orientation : float, optional
+        The tolerance for matching the target axis orientation.
+        Unit is in radians.
         If not specified, the default value from the planner is used.
     name : str, optional
         The human-readable name of the target.
@@ -696,18 +711,21 @@ def __init__(
         target_points_and_axes,
         target_mode,
         tolerance_position=None,
+        tolerance_orientation=None,
         name="Point-Axis Waypoints",
     ):
         super(PointAxisWaypoints, self).__init__(target_mode=target_mode, name=name)
         self.target_points_and_axes = target_points_and_axes
         self.tolerance_position = tolerance_position
+        self.tolerance_orientation = tolerance_orientation
 
     @property
     def __data__(self):
         return {
             "target_points_and_axes": self.target_points_and_axes,
             "target_mode": self.target_mode,
             "tolerance_position": self.tolerance_position,
+            "tolerance_orientation": self.tolerance_orientation,
             "name": self.name,
         }
 
@@ -728,7 +746,9 @@ def scaled(self, factor):
         # Axis is a unitized vector and is not scaled
         target_points_and_axes = [(point.scaled(factor), axis) for point, axis in self.target_points_and_axes]
         tolerance_position = self.tolerance_position * factor if self.tolerance_position else None
-        return PointAxisWaypoints(target_points_and_axes, self.target_mode, tolerance_position, self.name)
+        return PointAxisWaypoints(
+            target_points_and_axes, self.target_mode, tolerance_position, self.tolerance_orientation, self.name
+        )
 
 
 class TargetMode:

tests/robots/test_targets.py

@@ -50,8 +50,11 @@ def point_axis_target():
     target_point = Point(1.0, -2.0, 3.0)
     target_vector = Vector(1.0, -1.0, 0.0)
     tolerance_position = 0.001
+    tolerance_orientation = 0.001
     name = "my testing name"
-    return PointAxisTarget(target_point, target_vector, TargetMode.ROBOT, tolerance_position, name)
+    return PointAxisTarget(
+        target_point, target_vector, TargetMode.ROBOT, tolerance_position, tolerance_orientation, name
+    )
 
 
 @pytest.fixture
@@ -77,6 +80,7 @@ def test_serialization_targets(frame_target, point_axis_target, configuration_ta
     assert point_axis_target.target_z_axis == nt.target_z_axis
     assert point_axis_target.target_mode == nt.target_mode
     assert point_axis_target.tolerance_position == nt.tolerance_position
+    assert point_axis_target.tolerance_orientation == nt.tolerance_orientation
     assert point_axis_target.name == nt.name
 
     # ConfigurationTarget