diff --git a/ada_feeding/ada_feeding/behaviors/__init__.py b/ada_feeding/ada_feeding/behaviors/__init__.py index b8e1365a..5f2aea6c 100644 --- a/ada_feeding/ada_feeding/behaviors/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/__init__.py @@ -2,3 +2,4 @@ This package contains custom py_tree behaviors for the Ada Feeding project. """ from .blackboard_behavior import BlackboardBehavior +from .ros_utility import UpdateTimestamp diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py b/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py index 85be0ab7..5180a8ca 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py @@ -4,6 +4,6 @@ from .compute_food_frame import ComputeFoodFrame from .compute_action_constraints import ( - ComputeApproachConstraints, - ComputeExtractConstraints, + ComputeActionConstraints, + ComputeActionTwist, ) diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py b/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py index f182de69..9069e8c0 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py @@ -11,7 +11,14 @@ from typing import Union, Optional # Third-party imports -from geometry_msgs.msg import Point, Pose, Quaternion, Transform, TransformStamped +from geometry_msgs.msg import ( + Point, + Pose, + Transform, + TransformStamped, + TwistStamped, + Vector3Stamped, +) import numpy as np from overrides import override import py_trees @@ -32,7 +39,7 @@ from ada_feeding_msgs.srv import AcquisitionSelect -class ComputeApproachConstraints(BlackboardBehavior): +class ComputeActionConstraints(BlackboardBehavior): """ Checks AcquisitionSelect response, implements stochastic policy choice, and then decomposes into individual @@ -77,7 +84,9 @@ def blackboard_outputs( self, move_above_pose: Optional[BlackboardKey], # Pose, in Food Frame move_into_pose: Optional[BlackboardKey], # Pose, in Food Frame - ft_thresh: Optional[BlackboardKey], # SetParameters.Request + approach_thresh: Optional[BlackboardKey], # SetParameters.Request + grasp_thresh: Optional[BlackboardKey], # SetParameters.Request + ext_thresh: Optional[BlackboardKey], # SetParameters.Request action: Optional[BlackboardKey], # AcquisitionSchema.msg ) -> None: """ @@ -88,7 +97,9 @@ def blackboard_outputs( ---------- move_above_pose: Pose constraint when moving above food move_into_pose: Pose constraint when moving into food - ft_thresh: SetParameters request to set thresholds pre-approach + approach_thresh: SetParameters request to set thresholds pre-approach + grasp_thresh: SetParameters request to set thresholds in grasp + ext_thresh: SetParameters request to set thresholds for extraction action: AcquisitionSchema object to use in later computations """ # pylint: disable=unused-argument, duplicate-code @@ -133,15 +144,6 @@ def update(self) -> py_trees.common.Status: index = np.random.choice(np.arange(prob.size), p=prob) action = response.actions[index] - ### Calculate Approach Frame - # TODO: Calculate Approach Frame - identity = TransformStamped() - identity.transform = ros2_numpy.msgify(Transform, np.eye(4)) - identity.header.stamp = self.node.get_clock().now().to_msg() - identity.header.frame_id = "food" - identity.child_frame_id = "approach" - set_static_tf(identity, self.blackboard, self.node) - ### Construct Constraints # Re-scale pre-transform to move_above_dist_m @@ -166,23 +168,43 @@ def update(self) -> py_trees.common.Status: move_into_pose.position = ros2_numpy.msgify(Point, offset) self.blackboard_set("move_into_pose", move_into_pose) - # Calculate Approach F/T Thresholds + ### Calculate Approach Frame + approach_vec = offset - position + approach_frame = TransformStamped() + approach_mat = np.eye(4) + if not np.all(np.isclose(approach_vec[:2], np.zeros(2))): + approach_mat[:3, :3] = R.from_rotvec( + np.arctan2(approach_vec[1], approach_vec[0]) * np.array([0, 0, 1]) + ).as_matrix() + approach_frame.transform = ros2_numpy.msgify(Transform, approach_mat) + approach_frame.header.stamp = self.node.get_clock().now().to_msg() + approach_frame.header.frame_id = "food" + approach_frame.child_frame_id = "approach" + set_static_tf(approach_frame, self.blackboard, self.node) + + ### Calculate F/T Thresholds self.blackboard_set( - "ft_thresh", + "approach_thresh", create_ft_thresh_request(action.pre_force, action.pre_torque), ) + self.blackboard_set( + "grasp_thresh", + create_ft_thresh_request(action.grasp_force, action.grasp_torque), + ) + self.blackboard_set( + "ext_thresh", + create_ft_thresh_request(action.ext_force, action.ext_torque), + ) + ### Final write to Blackboard self.blackboard_set("action", action) return py_trees.common.Status.SUCCESS -# TODO: ComputeGraspConstraints - - -class ComputeExtractConstraints(BlackboardBehavior): +class ComputeActionTwist(BlackboardBehavior): """ - Decomposes AcquisitionSchema msg into individual - BlackboardKey objects for MoveIt2 Extraction Behaviors. + Converts grasp/extraction parameters into a single + TwistStamped for use with moveit2_servo """ # pylint: disable=arguments-differ @@ -197,6 +219,7 @@ class ComputeExtractConstraints(BlackboardBehavior): def blackboard_inputs( self, action: Union[BlackboardKey, AcquisitionSchema], + is_grasp: Union[BlackboardKey, bool] = True, approach_frame_id: Union[BlackboardKey, str] = "approach", ) -> None: """ @@ -205,6 +228,7 @@ def blackboard_inputs( Parameters ---------- action: AcquisitionSchema msg object + is_grasp: if true, use the grasp action elements, else use extraction approach_frame_id: approach frame defined in AcquisitionSchema.msg """ # pylint: disable=unused-argument, duplicate-code @@ -215,10 +239,8 @@ def blackboard_inputs( def blackboard_outputs( self, - extract_position: Optional[BlackboardKey], # Position, in approach frame - extract_orientation: Optional[BlackboardKey], # Quaternion, in forkTip frame - ft_thresh: Optional[BlackboardKey], # SetParameters.Request - ee_frame_id: Optional[BlackboardKey] = None, # str + twist: Optional[BlackboardKey], # TwistStamped + duration: Optional[BlackboardKey], # float ) -> None: """ Blackboard Outputs @@ -226,10 +248,8 @@ def blackboard_outputs( Parameters ---------- - extract_position: Postition constraint when moving out of food - extract_orientation: Orientation constraint when moving out of food - ft_thresh: SetParameters request to set thresholds pre-extraction - ee_frame_id: end-effector frame for the extract_orientation constraint + twist: twist to send to MoveIt2 Servo + duration: how long to twist in seconds """ # pylint: disable=unused-argument, duplicate-code # Arguments are handled generically in base class. @@ -264,10 +284,17 @@ def update(self) -> py_trees.common.Status: # Docstring copied from @override # Input Validation - if not self.blackboard_exists("action"): + if not self.blackboard_exists(["action", "is_grasp", "approach_frame_id"]): self.logger.error("Missing AcquisitionSchema action") return py_trees.common.Status.FAILURE action = self.blackboard_get("action") + linear = action.ext_linear + angular = action.ext_angular + duration = action.ext_duration + if self.blackboard_get("is_grasp"): + linear = action.grasp_linear + angular = action.grasp_angular + duration = action.grasp_duration approach_frame_id = self.blackboard_get("approach_frame_id") ### Lock used objects @@ -276,49 +303,46 @@ def update(self) -> py_trees.common.Status: # Use a Timeout decorator to determine failure. return py_trees.common.Status.RUNNING with self.tf_lock, self.moveit2_lock: - # Get TF EE frame -> approach frame + twist = TwistStamped() + twist.header.stamp = self.node.get_clock().now().to_msg() + twist.header.frame_id = self.moveit2.base_link_name + ### Move Linear to Base Link Frame + # Get TF approach frame -> base link frame if not self.tf_buffer.can_transform( + self.moveit2.base_link_name, approach_frame_id, - self.moveit2.end_effector_name, rclpy.time.Time(), ): # Not yet, wait for it # Use a Timeout decorator to determine failure. return py_trees.common.Status.RUNNING - utensil_to_approach_transform = self.tf_buffer.lookup_transform( - approach_frame_id, + linear_stamped = Vector3Stamped() + linear_stamped.header.frame_id = approach_frame_id + linear_stamped.vector = linear + twist.twist.linear = self.tf_buffer.transform( + linear_stamped, self.moveit2.base_link_name + ).vector + + ### Move Angular to Approach Frame + # Get TF EE frame -> base link frame + if not self.tf_buffer.can_transform( + self.moveit2.base_link_name, self.moveit2.end_effector_name, rclpy.time.Time(), - ) - ee_position_np = ros2_numpy.numpify( - utensil_to_approach_transform.transform.translation - ) - self.blackboard_set("ee_frame_id", self.moveit2.end_effector_name) - - ### Construct Constraints - - # Calculate Extract position (in approach frame) - position = ros2_numpy.numpify(action.ext_linear) - dur_s = float(action.ext_duration.sec) + ( - float(action.ext_duration.nanosec) / 10e9 - ) - position = position * dur_s - position = position + ee_position_np # Offset from current position - extract_position = ros2_numpy.msgify(Point, position[:3]) - self.blackboard_set("extract_position", extract_position) - - # Calculate extract orientation (in forktip frame) - rot_vec = ros2_numpy.numpify(action.ext_angular) - rot_vec = rot_vec * dur_s - extract_orientation = ros2_numpy.msgify( - Quaternion, R.from_rotvec(rot_vec).as_quat() - ) - self.blackboard_set("extract_orientation", extract_orientation) - - # Calculate Approach F/T Thresholds - self.blackboard_set( - "ft_thresh", - create_ft_thresh_request(action.ext_force, action.ext_torque), - ) + ): + # Not yet, wait for it + # Use a Timeout decorator to determine failure. + return py_trees.common.Status.RUNNING + angular_stamped = Vector3Stamped() + angular_stamped.header.frame_id = self.moveit2.end_effector_name + angular_stamped.vector = angular + twist.twist.angular = self.tf_buffer.transform( + angular_stamped, self.moveit2.base_link_name + ).vector + self.blackboard_set("twist", twist) + + # Compute Duration + dur_s = float(duration.sec) + (float(duration.nanosec) / 1e9) + self.blackboard_set("duration", dur_s) return py_trees.common.Status.SUCCESS diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py index adc19970..6d12db1e 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py @@ -148,6 +148,11 @@ def update(self) -> py_trees.common.Status: # We can't get around all the conversions # to ROS2 msg types, which take 3-4 statements each. + # Validate inputs + if not self.blackboard_exists(["camera_info", "world_frame"]): + self.logger.error("Missing camera_info or world_frame") + return py_trees.common.Status.FAILURE + camera_frame = self.blackboard_get("camera_info").header.frame_id world_frame = self.blackboard_get("world_frame") diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py b/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py index 6458e184..761e80e2 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py @@ -10,6 +10,7 @@ MoveIt2OrientationConstraint, MoveIt2PoseConstraint, ) +from .servo_move import ServoMove # Modifying the planning scene from .modify_collision_object import ( diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py index 8c453079..c9d36dc6 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py @@ -94,15 +94,8 @@ def blackboard_outputs( def update(self) -> py_trees.common.Status: # Docstring copied from @override - # pylint: disable=too-many-boolean-expressions - # This is just checking all inputs, should be - # easy to read. - if ( - not self.blackboard_exists("joint_positions") - or not self.blackboard_exists("joint_names") - or not self.blackboard_exists("tolerance") - or not self.blackboard_exists("weight") - or not self.blackboard_exists("constraints") + if not self.blackboard_exists( + ["joint_positions", "joint_names", "tolerance", "weight", "constraints"] ): self.logger.error("MoveIt2Constraint: Missing input arguments.") return py_trees.common.Status.FAILURE @@ -200,16 +193,15 @@ def blackboard_outputs( def update(self) -> py_trees.common.Status: # Docstring copied from @override - # pylint: disable=too-many-boolean-expressions - # This is just checking all inputs, should be - # easy to read. - if ( - not self.blackboard_exists("position") - or not self.blackboard_exists("frame_id") - or not self.blackboard_exists("target_link") - or not self.blackboard_exists("tolerance") - or not self.blackboard_exists("weight") - or not self.blackboard_exists("constraints") + if not self.blackboard_exists( + [ + "position", + "frame_id", + "target_link", + "tolerance", + "weight", + "constraints", + ] ): self.logger.error("MoveIt2PositionConstraint: Missing input arguments.") self.logger.error(str(self.blackboard)) @@ -324,17 +316,16 @@ def blackboard_outputs( def update(self) -> py_trees.common.Status: # Docstring copied from @override - # pylint: disable=too-many-boolean-expressions - # This is just checking all inputs, should be - # easy to read. - if ( - not self.blackboard_exists("quat_xyzw") - or not self.blackboard_exists("frame_id") - or not self.blackboard_exists("target_link") - or not self.blackboard_exists("tolerance") - or not self.blackboard_exists("weight") - or not self.blackboard_exists("constraints") - or not self.blackboard_exists("parameterization") + if not self.blackboard_exists( + [ + "quat_xyzw", + "frame_id", + "target_link", + "tolerance", + "weight", + "constraints", + "parameterization", + ] ): self.logger.error("MoveIt2OrientationConstraint: Missing input arguments.") return py_trees.common.Status.FAILURE @@ -451,19 +442,18 @@ def blackboard_outputs( def update(self) -> py_trees.common.Status: # Docstring copied from @override - # pylint: disable=too-many-boolean-expressions - # This is just checking all inputs, should be - # easy to read. - if ( - not self.blackboard_exists("pose") - or not self.blackboard_exists("frame_id") - or not self.blackboard_exists("target_link") - or not self.blackboard_exists("tolerance_position") - or not self.blackboard_exists("weight_position") - or not self.blackboard_exists("tolerance_orientation") - or not self.blackboard_exists("weight_orientation") - or not self.blackboard_exists("constraints") - or not self.blackboard_exists("parameterization") + if not self.blackboard_exists( + [ + "pose", + "frame_id", + "target_link", + "tolerance_position", + "weight_position", + "tolerance_orientation", + "weight_orientation", + "constraints", + "parameterization", + ] ): self.logger.error("MoveIt2Constraint: Missing input arguments.") return py_trees.common.Status.FAILURE diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/servo_move.py b/ada_feeding/ada_feeding/behaviors/moveit2/servo_move.py new file mode 100644 index 00000000..220a8e41 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/moveit2/servo_move.py @@ -0,0 +1,137 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +This module defines the ServoMove behavior, which publishes +a Twist to the Moveit2 Servo node. +""" + +# Standard imports +from typing import Union + +# Third-party imports +from overrides import override +from geometry_msgs.msg import Twist, TwistStamped +import py_trees +from rclpy.duration import Duration +from rclpy.qos import QoSProfile + +# Local imports +from ada_feeding.helpers import BlackboardKey, float_to_duration +from ada_feeding.behaviors import BlackboardBehavior + + +class ServoMove(BlackboardBehavior): + """ + Publishes a Twist (or TwistStamped) to the + MoveIt2Servo object for specified duration. + """ + + # pylint: disable=arguments-differ + # We *intentionally* violate Liskov Substitution Princple + # in that blackboard config (inputs + outputs) are not + # meant to be called in a generic setting. + + # pylint: disable=too-many-arguments + # These are effectively config definitions + # They require a lot of arguments. + + def blackboard_inputs( + self, + twist: Union[BlackboardKey, Twist, TwistStamped], + duration: Union[BlackboardKey, Duration, float] = Duration(seconds=1.0), + pub_topic: Union[BlackboardKey, str] = "~/servo_twist_cmds", + pub_qos: Union[BlackboardKey, QoSProfile] = QoSProfile(depth=1), + default_frame_id: Union[BlackboardKey, str] = "world", + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + twist: Twist or TwistStamped to publish, with updated time stamps. + duration: How long to publish the twist + pub_topic: Where to publish servo TwistStamped messages + pub_qos: QoS for publisher + default_frame_id: frame_id to use if Twist type is provided. + """ + # pylint: disable=unused-argument, duplicate-code + # Arguments are handled generically in base class. + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + # Docstring copied from @override + + # pylint: disable=attribute-defined-outside-init + # It is okay for attributes in behaviors to be + # defined in the setup / initialise functions. + + # Get Node from Kwargs + self.node = kwargs["node"] + + @override + def initialise(self) -> None: + # Docstring copied from @override + + # pylint: disable=attribute-defined-outside-init + # It is okay for attributes in behaviors to be + # defined in the setup / initialise functions. + + # Create publisher + self.pub = self.node.create_publisher( + TwistStamped, + self.blackboard_get("pub_topic"), + self.blackboard_get("pub_qos"), + ) + + # Record start time + self.start_time = self.node.get_clock().now() + + @override + def update(self) -> py_trees.common.Status: + # Docstring copied from @override + + # pylint: disable=attribute-defined-outside-init, too-many-return-statements + # It is okay for attributes in behaviors to be + # defined in the setup / initialise functions. + + # Validate inputs + if not self.blackboard_exists(["twist", "duration"]): + self.logger.error("Missing input arguments") + return py_trees.common.Status.FAILURE + + # Return success if duration is exceeded + duration = self.blackboard_get("duration") + if isinstance(duration, float): + duration = float_to_duration(duration) + if self.node.get_clock().now() > (self.start_time + duration): + return py_trees.common.Status.SUCCESS + + # Publish twist + twist = self.blackboard_get("twist") + if isinstance(twist, Twist): + twist = TwistStamped() + twist.header.frame_id = self.blackboard_get("default_frame_id") + twist.twist = self.blackboard_get("twist") + twist.header.stamp = self.node.get_clock().now().to_msg() + self.pub.publish(twist) + + # Servo is still executing + return py_trees.common.Status.RUNNING + + @override + def terminate(self, new_status: py_trees.common.Status) -> None: + # Docstring copied from @override + + # Publish Zero Twist + zero_twist = TwistStamped() + zero_twist.header.frame_id = ( + self.blackboard_get("twist").header.frame_id + if isinstance(self.blackboard_get("twist"), TwistStamped) + else self.blackboard_get("default_frame_id") + ) + zero_twist.header.stamp = self.node.get_clock().now().to_msg() + + self.pub.publish(zero_twist) diff --git a/ada_feeding/ada_feeding/behaviors/ros_utility.py b/ada_feeding/ada_feeding/behaviors/ros_utility.py new file mode 100644 index 00000000..948a73ac --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/ros_utility.py @@ -0,0 +1,104 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +This module defines ROS utility behaviors. +""" + +# Standard imports +from typing import Union, Optional, Any + +# Third-party imports +from overrides import override +import py_trees +import rclpy + +# Local imports +from ada_feeding.helpers import BlackboardKey +from .blackboard_behavior import BlackboardBehavior + + +class UpdateTimestamp(BlackboardBehavior): + """ + Adds a custom timestamp (or current timestamp) + to any stamped message object + """ + + # pylint: disable=arguments-differ + # We *intentionally* violate Liskov Substitution Princple + # in that blackboard config (inputs + outputs) are not + # meant to be called in a generic setting. + + # pylint: disable=too-many-arguments + # These are effectively config definitions + # They require a lot of arguments. + + def blackboard_inputs( + self, + stamped_msg: Union[BlackboardKey, Any], + timestamp: Union[BlackboardKey, Optional[rclpy.time.Time]] = None, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + stamped_msg: Any ROS msg with a header + timestamp: if None, use current time + """ + # pylint: disable=unused-argument, duplicate-code + # Arguments are handled generically in base class. + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + stamped_msg: Optional[BlackboardKey], # Same type as input + ) -> None: + """ + Blackboard Outputs + By convention (to avoid collisions), avoid non-None default arguments. + + Parameters + ---------- + stamped_msg: Any ROS msg with a header + """ + # pylint: disable=unused-argument, duplicate-code + # Arguments are handled generically in base class. + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + # Docstring copied from @override + + # pylint: disable=attribute-defined-outside-init + # It is okay for attributes in behaviors to be + # defined in the setup / initialise functions. + + # Get Node from Kwargs + self.node = kwargs["node"] + + @override + def update(self) -> py_trees.common.Status: + # Docstring copied from @override + + # Input Validation + if not self.blackboard_exists(["stamped_msg", "timestamp"]): + self.logger.error("Missing input arguments") + return py_trees.common.Status.FAILURE + + msg = self.blackboard_get("stamped_msg") + time = self.blackboard_get("timestamp") + if time is None: + time = self.node.get_clock().now() + + try: + msg.header.stamp = time.to_msg() + except AttributeError as error: + self.logger.error(f"Malformed Stamped Message. Error: {error}") + return py_trees.common.Status.FAILURE + + self.blackboard_set("stamped_msg", msg) + return py_trees.common.Status.SUCCESS diff --git a/ada_feeding/ada_feeding/decorators/__init__.py b/ada_feeding/ada_feeding/decorators/__init__.py index c6a63b6a..4d097a0e 100644 --- a/ada_feeding/ada_feeding/decorators/__init__.py +++ b/ada_feeding/ada_feeding/decorators/__init__.py @@ -9,3 +9,6 @@ # On Preempt from .on_preempt import OnPreempt + +# Timeout From Blackboard +from .timeout_from_blackboard import TimeoutFromBlackboard diff --git a/ada_feeding/ada_feeding/decorators/timeout_from_blackboard.py b/ada_feeding/ada_feeding/decorators/timeout_from_blackboard.py new file mode 100644 index 00000000..8e118257 --- /dev/null +++ b/ada_feeding/ada_feeding/decorators/timeout_from_blackboard.py @@ -0,0 +1,48 @@ +""" +This module extends the Timeout decorator +to accept a blackboard namespace and +""" + +# Standard imports + +# Third-party imports +from overrides import override +from py_trees.decorators import Timeout +import py_trees + +# Local imports +from ada_feeding.helpers import BlackboardKey + + +class TimeoutFromBlackboard(Timeout): + """ + A decorator that has the same functionality + as the parent class, but allows the passing + in of a blackboard key for the duration + parameter. + + """ + + def __init__( + self, + name: str, + ns: str, + duration_key: BlackboardKey, + child: py_trees.behaviour.Behaviour, + ) -> None: + """ + Call parent w/ name and child + Just store local blackboard client + duration + """ + self._blackboard = py_trees.blackboard.Client(name=name, namespace=ns) + self._blackboard.register_key( + key=duration_key, access=py_trees.common.Access.READ + ) + self._duration_key = duration_key + + super().__init__(name=name, child=child) + + @override + def initialise(self): + self.duration = self._blackboard.get(self._duration_key) + super().initialise() diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 49f5d2cb..5b419384 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -15,6 +15,7 @@ from pymoveit2 import MoveIt2 from pymoveit2.robots import kinova from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.duration import Duration from rclpy.node import Node from tf2_ros.buffer import Buffer from tf2_ros.static_transform_broadcaster import StaticTransformBroadcaster @@ -37,6 +38,22 @@ def __new__(cls, *args, **kwargs): return str.__new__(cls, *args, **kwargs) +def duration_to_float(duration: Duration) -> float: + """ + Convert ROS duration to float seconds + """ + return float(duration.nanoseconds) / 1e9 + + +def float_to_duration(duration: float) -> Duration: + """ + Convert float seconds to ROS duration + """ + sec = int(duration) + nanosec = int((duration - sec) * 1e9) + return Duration(seconds=sec, nanoseconds=nanosec) + + def quat_between_vectors(vec_from: Vector3, vec_to: Vector3) -> Quaternion: """ Returns the Quaternion rotation (shortest arc) diff --git a/ada_feeding/ada_feeding/idioms/pre_moveto_config.py b/ada_feeding/ada_feeding/idioms/pre_moveto_config.py index d9c24744..021888ff 100644 --- a/ada_feeding/ada_feeding/idioms/pre_moveto_config.py +++ b/ada_feeding/ada_feeding/idioms/pre_moveto_config.py @@ -99,6 +99,7 @@ def pre_moveto_config( t_x: float = 0.0, t_y: float = 0.0, t_z: float = 0.0, + param_service_name: str = "~/set_force_gate_controller_parameters", ) -> py_trees.behaviour.Behaviour: """ Returns a behavior that calls the ROS services that should be called before @@ -194,7 +195,7 @@ def pre_moveto_config( set_force_torque_thresholds = retry_call_ros_service( name=set_force_torque_thresholds_name, service_type=SetParameters, - service_name="~/set_force_gate_controller_parameters", + service_name=param_service_name, key_request=None, request=ft_threshold_request, key_response=set_force_torque_thresholds_key_response, diff --git a/ada_feeding/ada_feeding/trees/__init__.py b/ada_feeding/ada_feeding/trees/__init__.py index 2295e23b..e2ec3e0c 100644 --- a/ada_feeding/ada_feeding/trees/__init__.py +++ b/ada_feeding/ada_feeding/trees/__init__.py @@ -8,8 +8,6 @@ # We import all of the trees here so that they can be imported as # ada_feeding.trees. instead of ada_feeding.trees.. -from .acquire_food_tree import AcquireFoodTree - from .move_to_tree import MoveToTree from .move_to_configuration_with_ft_thresholds_tree import ( MoveToConfigurationWithFTThresholdsTree, @@ -21,3 +19,5 @@ from .trigger_tree import TriggerTree from .start_servo_tree import StartServoTree from .stop_servo_tree import StopServoTree + +from .acquire_food_tree import AcquireFoodTree diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index de242ad7..4ce47f73 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -9,27 +9,30 @@ from typing import List, Optional # Third-party imports +from geometry_msgs.msg import Twist, TwistStamped from overrides import override import py_trees from py_trees.blackboard import Blackboard import py_trees_ros from rcl_interfaces.srv import SetParameters from rclpy.node import Node +from std_msgs.msg import Header # Local imports -from ada_feeding import ActionServerBT +from ada_feeding_msgs.action import AcquireFood +from ada_feeding_msgs.srv import AcquisitionSelect from ada_feeding.behaviors.acquisition import ( ComputeFoodFrame, - ComputeApproachConstraints, - ComputeExtractConstraints, + ComputeActionConstraints, + ComputeActionTwist, ) from ada_feeding.behaviors.moveit2 import ( MoveIt2JointConstraint, - MoveIt2OrientationConstraint, MoveIt2PoseConstraint, - MoveIt2PositionConstraint, MoveIt2Plan, MoveIt2Execute, + ServoMove, + ToggleCollisionObject, ) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( @@ -38,12 +41,10 @@ retry_call_ros_service, ) from ada_feeding.idioms.pre_moveto_config import set_parameter_response_all_success -from ada_feeding.visitors import MoveToVisitor -from ada_feeding_msgs.action import AcquireFood -from ada_feeding_msgs.srv import AcquisitionSelect +from ada_feeding.trees import MoveToTree, StartServoTree, StopServoTree -class AcquireFoodTree(ActionServerBT): +class AcquireFoodTree(MoveToTree): """ A behvaior tree to select and execute an acquisition action (see ada_feeding_msgs.action.AcquisitionSchema) @@ -81,10 +82,21 @@ def create_tree( ) -> py_trees.trees.BehaviourTree: # Docstring copied by @override - # Sub-trees in general should not need knowledge of their parent. + # pylint: disable=line-too-long + # This is the entire tree rolled out. - ### Define Tree Logic + ### Blackboard Constants + blackboard = py_trees.blackboard.Client(name=name, namespace=name) + blackboard.register_key(key="zero_twist", access=py_trees.common.Access.WRITE) + blackboard.zero_twist = TwistStamped( + header=Header( + stamp=self._node.get_clock().now().to_msg(), + frame_id="world", + ), + twist=Twist(), + ) + ### Define Tree Logic # Root Sequence root_seq = py_trees.composites.Sequence( name=name, @@ -125,21 +137,27 @@ def create_tree( wait_for_server_timeout_sec=0.0, ), # Get MoveIt2 Constraints - ComputeApproachConstraints( - name="ComputeApproachConstraints", - ns=name, - inputs={ - "action_select_response": BlackboardKey("action_response"), - # Default move_above_dist_m = 0.05 - # Default food_frame_id = "food" - # Default approach_frame_id = "approach" - }, - outputs={ - "move_above_pose": BlackboardKey("move_above_pose"), - "move_into_pose": BlackboardKey("move_into_pose"), - "ft_thresh": BlackboardKey("ft_thresh"), - "action": BlackboardKey("action"), - }, + py_trees.decorators.Timeout( + name="ComputeActionConstraintsTimeout", + duration=1.0, + child=ComputeActionConstraints( + name="ComputeActionConstraints", + ns=name, + inputs={ + "action_select_response": BlackboardKey("action_response"), + # Default move_above_dist_m = 0.05 + # Default food_frame_id = "food" + # Default approach_frame_id = "approach" + }, + outputs={ + "move_above_pose": BlackboardKey("move_above_pose"), + "move_into_pose": BlackboardKey("move_into_pose"), + "approach_thresh": BlackboardKey("approach_thresh"), + "grasp_thresh": BlackboardKey("grasp_thresh"), + "ext_thresh": BlackboardKey("ext_thresh"), + "action": BlackboardKey("action"), + }, + ), ), # Re-Tare FT Sensor and default to 4N threshold pre_moveto_config(name="PreAquireFTTare"), @@ -172,34 +190,61 @@ def create_tree( outputs={}, ), # If Anything goes wrong, reset FT to safe levels + # Also disable and re-enable table collision scoped_behavior( name="SafeFTPreempt", # Set Approach F/T Thresh - pre_behavior=retry_call_ros_service( - name="ApproachFTThresh", - service_type=SetParameters, - service_name="~/set_force_gate_controller_parameters", - # Blackboard, not Constant - request=None, - # Need absolute Blackboard name - key_request=Blackboard.separator.join( - [name, BlackboardKey("ft_thresh")] - ), - key_response=Blackboard.separator.join( - [name, BlackboardKey("ft_response")] - ), - response_checks=[ - py_trees.common.ComparisonExpression( - variable=Blackboard.separator.join( + pre_behavior=py_trees.composites.Sequence( + name=name, + memory=True, + children=[ + retry_call_ros_service( + name="ApproachFTThresh", + service_type=SetParameters, + service_name="~/set_force_gate_controller_parameters", + # Blackboard, not Constant + request=None, + # Need absolute Blackboard name + key_request=Blackboard.separator.join( + [name, BlackboardKey("approach_thresh")] + ), + key_response=Blackboard.separator.join( [name, BlackboardKey("ft_response")] ), - value=SetParameters.Response(), # Unused - operator=set_parameter_response_all_success, - ) + response_checks=[ + py_trees.common.ComparisonExpression( + variable=Blackboard.separator.join( + [name, BlackboardKey("ft_response")] + ), + value=SetParameters.Response(), # Unused + operator=set_parameter_response_all_success, + ) + ], + ), + ToggleCollisionObject( + name="AllowTable", + ns=name, + inputs={ + "collision_object_ids": ["table"], + "allow": True, + }, + ), ], ), - post_behavior=pre_moveto_config( - name="PostAcquireFTSet", re_tare=False + post_behavior=py_trees.composites.Sequence( + name=name, + memory=True, + children=[ + pre_moveto_config(name="PostAcquireFTSet", re_tare=False), + ToggleCollisionObject( + name="DisallowTable", + ns=name, + inputs={ + "collision_object_ids": ["table"], + "allow": False, + }, + ), + ], ), on_preempt_timeout=5.0, # Starts a new Sequence w/ Memory internally @@ -229,75 +274,142 @@ def create_tree( }, outputs={"trajectory": BlackboardKey("trajectory")}, ), - MoveIt2Execute( - name="MoveInto", - ns=name, - inputs={"trajectory": BlackboardKey("trajectory")}, - outputs={}, + # MoveInto expect F/T failure + py_trees.decorators.FailureIsSuccess( + name="MoveIntoExecuteSucceed", + child=MoveIt2Execute( + name="MoveInto", + ns=name, + inputs={"trajectory": BlackboardKey("trajectory")}, + outputs={}, + ), ), - ### Extraction - ComputeExtractConstraints( - name="ComputeExtractConstraints", - ns=name, - inputs={ - "action": BlackboardKey("action"), - # Default approach_frame_id = "approach" - }, - outputs={ - "extract_position": BlackboardKey("extract_position"), - "extract_orientation": BlackboardKey( - "extract_orientation" + ### Scoped Behavior for Moveit2_Servo + scoped_behavior( + name="MoveIt2Servo", + # Set Approach F/T Thresh + pre_behavior=py_trees.composites.Sequence( + name=name, + memory=True, + children=[ + StartServoTree(self._node) + .create_tree(name="StartServoScoped") + .root, + ], + ), + # Reset FT and Stop Servo + post_behavior=py_trees.composites.Sequence( + name=name, + memory=True, + children=[ + pre_moveto_config( + name="PostServoFTSet", + re_tare=False, + f_mag=1.0, + param_service_name="~/set_servo_controller_parameters", + ), + StopServoTree(self._node) + .create_tree(name="StopServoScoped") + .root, + ], + ), + on_preempt_timeout=5.0, + # Starts a new Sequence w/ Memory internally + workers=[ + ### Grasp + retry_call_ros_service( + name="GraspFTThresh", + service_type=SetParameters, + service_name="~/set_servo_controller_parameters", + # Blackboard, not Constant + request=None, + # Need absolute Blackboard name + key_request=Blackboard.separator.join( + [name, BlackboardKey("grasp_thresh")] + ), + key_response=Blackboard.separator.join( + [name, BlackboardKey("ft_response")] + ), + response_checks=[ + py_trees.common.ComparisonExpression( + variable=Blackboard.separator.join( + [name, BlackboardKey("ft_response")] + ), + value=SetParameters.Response(), # Unused + operator=set_parameter_response_all_success, + ) + ], ), - "ft_thresh": BlackboardKey("ft_thresh"), - "ee_frame_id": BlackboardKey("ee_frame_id"), - }, - ), - MoveIt2PositionConstraint( - name="ExtractPosition", - ns=name, - inputs={ - "position": BlackboardKey("extract_position"), - "frame_id": "approach", - }, - outputs={ - "constraints": BlackboardKey("goal_constraints"), - }, - ), - MoveIt2OrientationConstraint( - name="ExtractOrientation", - ns=name, - inputs={ - "quat_xyzw": BlackboardKey("extract_orientation"), - "frame_id": BlackboardKey("ee_frame_id"), - "constraints": BlackboardKey("goal_constraints"), - }, - outputs={ - "constraints": BlackboardKey("goal_constraints"), - }, - ), - MoveIt2Plan( - name="ExtractPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), - "max_velocity_scale": 0.8, - "max_acceleration_scale": 0.8, - "cartesian": True, - "cartesian_max_step": 0.001, - "cartesian_fraction_threshold": 0.95, - }, - outputs={"trajectory": BlackboardKey("trajectory")}, - ), - MoveIt2Execute( - name="Extraction", - ns=name, - inputs={"trajectory": BlackboardKey("trajectory")}, - outputs={}, - ), - ], - ), - ], - ) + ComputeActionTwist( + name="ComputeGrasp", + ns=name, + inputs={ + "action": BlackboardKey("action"), + "is_grasp": True, + }, + outputs={ + "twist": BlackboardKey("twist"), + "duration": BlackboardKey("duration"), + }, + ), + ServoMove( + name="GraspServo", + ns=name, + inputs={ + "twist": BlackboardKey("twist"), + "duration": BlackboardKey("duration"), + }, + ), # Auto Zero-Twist on terminate() + ### Extraction + ComputeActionTwist( + name="ComputeGrasp", + ns=name, + inputs={ + "action": BlackboardKey("action"), + "is_grasp": False, + }, + outputs={ + "twist": BlackboardKey("twist"), + "duration": BlackboardKey("duration"), + }, + ), + retry_call_ros_service( + name="ExtractionFTThresh", + service_type=SetParameters, + service_name="~/set_servo_controller_parameters", + # Blackboard, not Constant + request=None, + # Need absolute Blackboard name + key_request=Blackboard.separator.join( + [name, BlackboardKey("ext_thresh")] + ), + key_response=Blackboard.separator.join( + [name, BlackboardKey("ft_response")] + ), + response_checks=[ + py_trees.common.ComparisonExpression( + variable=Blackboard.separator.join( + [name, BlackboardKey("ft_response")] + ), + value=SetParameters.Response(), # Unused + operator=set_parameter_response_all_success, + ) + ], + ), + ServoMove( + name="ExtractServo", + ns=name, + inputs={ + "twist": BlackboardKey("twist"), + "duration": BlackboardKey("duration"), + }, + ), # Auto Zero-Twist on terminate() + ], # End MoveIt2Servo.workers + ), # End MoveIt2Servo + ], # End SafeFTPreempt.workers + ), # End SafeFTPreempt + ], # End root_seq.children + ) # End root_seq ### Add Resting Position if self.resting_joint_positions is not None: @@ -354,16 +466,8 @@ def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: blackboard.register_key(key="camera_info", access=py_trees.common.Access.WRITE) blackboard.camera_info = goal.camera_info - # Add MoveToVisitor for Feedback - feedback_visitor = None - for visitor in tree.visitors: - if isinstance(visitor, MoveToVisitor): - visitor.reinit() - feedback_visitor = visitor - if feedback_visitor is None: - tree.add_visitor(MoveToVisitor(self._node)) - - return True + # Adds MoveToVisitor for Feedback + return super().send_goal(tree, goal) # Override result to handle timing outside MoveTo Behaviors @override @@ -375,24 +479,7 @@ def get_feedback( if action_type is not AcquireFood: return None - feedback_msg = action_type.Feedback() - - # Get Feedback Visitor - feedback_visitor = None - for visitor in tree.visitors: - if isinstance(visitor, MoveToVisitor): - feedback_visitor = visitor - - # Copy everything from the visitor - if feedback_visitor is not None: - feedback = feedback_visitor.get_feedback() - feedback_msg.is_planning = feedback.is_planning - feedback_msg.planning_time = feedback.planning_time - feedback_msg.motion_time = feedback.motion_time - feedback_msg.motion_initial_distance = feedback.motion_initial_distance - feedback_msg.motion_curr_distance = feedback.motion_curr_distance - - return feedback_msg + return super().get_feedback(tree, action_type) # Override result to add other elements to result msg @override @@ -404,36 +491,5 @@ def get_result( if action_type is not AcquireFood: return None - result_msg = action_type.Result() - - # If the tree succeeded, return success - if tree.root.status == py_trees.common.Status.SUCCESS: - result_msg.status = result_msg.STATUS_SUCCESS - # If the tree failed, detemine whether it was a planning or motion failure - elif tree.root.status == py_trees.common.Status.FAILURE: - # Get Feedback Visitor to investigate failure cause - feedback_visitor = None - for visitor in tree.visitors: - if isinstance(visitor, MoveToVisitor): - feedback_visitor = visitor - if feedback_visitor is None: - result_msg.status = result_msg.STATUS_UNKNOWN - else: - feedback = feedback_visitor.get_feedback() - if feedback.is_planning: - result_msg.status = result_msg.STATUS_PLANNING_FAILED - else: - result_msg.status = result_msg.STATUS_MOTION_FAILED - # If the tree has an invalid status, return unknown - elif tree.root.status == py_trees.common.Status.INVALID: - result_msg.status = result_msg.STATUS_UNKNOWN - # If the tree is running, the fact that `get_result` was called is - # indicative of an error. Return unknown error. - else: - tree.root.logger.error( - f"Called get_result with status RUNNING: {tree.root.status}" - ) - result_msg.status = result_msg.STATUS_UNKNOWN - # TODO: add action_index, posthoc, action_select_hash - return result_msg + return super().get_result(tree, action_type) diff --git a/ada_feeding/ada_feeding/trees/start_servo_tree.py b/ada_feeding/ada_feeding/trees/start_servo_tree.py index a70c0b2d..87fa6533 100644 --- a/ada_feeding/ada_feeding/trees/start_servo_tree.py +++ b/ada_feeding/ada_feeding/trees/start_servo_tree.py @@ -101,6 +101,7 @@ def create_tree( ) # Put them together in a sequence + # pylint: disable=duplicate-code return py_trees.trees.BehaviourTree( root=py_trees.composites.Sequence( name=name, diff --git a/ada_feeding/ada_feeding/trees/stop_servo_tree.py b/ada_feeding/ada_feeding/trees/stop_servo_tree.py index 81461703..3de0f707 100644 --- a/ada_feeding/ada_feeding/trees/stop_servo_tree.py +++ b/ada_feeding/ada_feeding/trees/stop_servo_tree.py @@ -105,6 +105,7 @@ def create_tree( ) # Put them together in a sequence with memory + # pylint: disable=duplicate-code return py_trees.trees.BehaviourTree( root=py_trees.composites.Sequence( name=name, diff --git a/ada_feeding/ada_feeding/visitors/moveto_visitor.py b/ada_feeding/ada_feeding/visitors/moveto_visitor.py index be053e52..ca4f16fd 100644 --- a/ada_feeding/ada_feeding/visitors/moveto_visitor.py +++ b/ada_feeding/ada_feeding/visitors/moveto_visitor.py @@ -22,8 +22,8 @@ # Local imports from ada_feeding_msgs.action import MoveTo -from ada_feeding.behaviors.moveit2 import MoveIt2Execute -from ada_feeding.helpers import get_moveit2_object +from ada_feeding.behaviors.moveit2 import MoveIt2Execute, ServoMove +from ada_feeding.helpers import duration_to_float, float_to_duration, get_moveit2_object class MoveToVisitor(VisitorBase): @@ -166,8 +166,26 @@ def run(self, behaviour: py_trees.behaviour.Behaviour) -> None: if traj is not None: self.feedback.motion_initial_distance = float(len(traj.points)) self.feedback.motion_curr_distance = self.get_distance_to_goal(traj) + elif ( + isinstance(behaviour, ServoMove) + and behaviour.status == py_trees.common.Status.RUNNING + ): + # Flip to execute + if self.feedback.is_planning: + self.start_time = self.node.get_clock().now() + self.feedback.is_planning = False + # Distance == time_elapsed vs duration + if behaviour.blackboard_exists("duration"): + dur = behaviour.blackboard_get("duration") + if isinstance(dur, float): + dur = float_to_duration(dur) + self.feedback.motion_initial_distance = duration_to_float(dur) + self.feedback.motion_curr_distance = ( + self.feedback.motion_initial_distance + - duration_to_float(self.node.get_clock().now() - self.start_time) + ) else: - # Catch end of MoveIt2Execute behavior + # Catch end of MoveIt2Execute behaviors if behaviour.status == py_trees.common.Status.SUCCESS: self.feedback.motion_curr_distance = 0.0 # Switch to planning start time diff --git a/ada_feeding/launch/ada_feeding_launch.xml b/ada_feeding/launch/ada_feeding_launch.xml index a2f88799..b5143b7d 100644 --- a/ada_feeding/launch/ada_feeding_launch.xml +++ b/ada_feeding/launch/ada_feeding_launch.xml @@ -25,6 +25,7 @@ + diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index d5670c16..c28c2db3 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -4,23 +4,23 @@ actions: - # Default Action (VS90) pre_pos: [0.0, 0.0, 1.0] pre_quat: [0.0, 0.7071068, -0.7071068, 0.0] - pre_offset: [0.0, 0.0, 0.0] - pre_force: 15.0 + pre_offset: [0.0, 0.0, -0.015] + pre_force: 20.0 pre_torque: 4.0 grasp_linear: [0.0, 0.0, 0.0] grasp_angular: [0.0, 0.0, 0.0] grasp_duration: 0.0 grasp_force: 15.0 grasp_torque: 4.0 - ext_linear: [0.0, 0.0, 0.05] + ext_linear: [0.0, 0.0, 0.1] ext_angular: [0.0, 0.0, 0.0] - ext_duration: 1.0 + ext_duration: 1.5 ext_force: 50.0 ext_torque: 4.0 - # Tilted Vertical (TV90) pre_pos: [0.0, 0.0, 1.0] pre_quat: [-0.174941, 0.6851245, 0.6851245, -0.174941] - pre_offset: [0.0, 0.0, 0.0] + pre_offset: [0.0, 0.0, -0.01] pre_force: 15.0 pre_torque: 4.0 grasp_linear: [0.0, 0.0, 0.0] @@ -28,15 +28,15 @@ actions: grasp_duration: 0.0 grasp_force: 15.0 grasp_torque: 4.0 - ext_linear: [0.0, 0.0, 0.05] + ext_linear: [0.0, 0.0, 0.1] ext_angular: [0.0, 0.0, 0.0] - ext_duration: 1.0 + ext_duration: 1.5 ext_force: 50.0 ext_torque: 4.0 - # Tilted Angled (TA90) pre_pos: [-0.4949747468305832, 0.0, 0.6363961030678928] pre_quat: [0.1379497, 0.6935199, 0.6935199, 0.1379497] - pre_offset: [0.0, 0.0, 0.0] + pre_offset: [0.0, 0.0, -0.01] pre_force: 15.0 pre_torque: 4.0 grasp_linear: [0.0, 0.0, 0.0] @@ -44,9 +44,9 @@ actions: grasp_duration: 0.0 grasp_force: 15.0 grasp_torque: 4.0 - ext_linear: [0.0, 0.0, 0.05] + ext_linear: [0.0, 0.0, 0.1] ext_angular: [0.0, 0.0, 0.0] - ext_duration: 1.0 + ext_duration: 1.5 ext_force: 50.0 ext_torque: 4.0 #### k=11 Medioids (Add 3 for Index)