diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py index 5cb6cc110..329649a18 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_inverse_kinematics.py +++ b/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( diff --git a/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py b/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py index b15d3da7c..06d164c6e 100644 --- a/src/compas_fab/backends/pybullet/backend_features/pybullet_plan_cartesian_motion.py +++ b/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) diff --git a/src/compas_fab/backends/ros/backend_features/helpers.py b/src/compas_fab/backends/ros/backend_features/helpers.py index 738ef4ba4..b9d8944d4 100644 --- a/src/compas_fab/backends/ros/backend_features/helpers.py +++ b/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] diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py index 81e75fc8d..c80a812c1 100644 --- a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py +++ b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/code.py @@ -14,7 +14,9 @@ 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: @@ -22,6 +24,8 @@ def RunScript(self, point, target_z_axis, tolerance_position, tool_coordinate_fr 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 diff --git a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json b/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json index b5d39ea87..b587bd56c 100644 --- a/src/compas_fab/ghpython/components/Cf_PointAxisTarget/metadata.json +++ b/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" }, { diff --git a/src/compas_fab/robots/targets.py b/src/compas_fab/robots/targets.py index e76ec762d..c99b0d565 100644 --- a/src/compas_fab/robots/targets.py +++ b/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,11 +711,13 @@ 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): @@ -708,6 +725,7 @@ def __data__(self): "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: diff --git a/tests/robots/test_targets.py b/tests/robots/test_targets.py index c737b8bb7..4c51aef6a 100644 --- a/tests/robots/test_targets.py +++ b/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