From f874e974b1e79d883601a42e84a8b1ada6bd3735 Mon Sep 17 00:00:00 2001 From: Amal Nanavati Date: Fri, 15 Sep 2023 19:18:52 -0700 Subject: [PATCH] Added Global MoveIt2 Object, ClearConstraints decorator - Added a global MoveIt2 object, accesed via helper function - Added a ClearConstraints decorator to add above every MoveTo branch - Tested in-person on all actions but MovetoMouth --- ada_feeding/ada_feeding/behaviors/move_to.py | 105 +++++++++--------- .../behaviors/toggle_collision_object.py | 27 ++--- .../ada_feeding/decorators/__init__.py | 3 + .../decorators/clear_constraints.py | 65 +++++++++++ .../decorators/move_to_constraint.py | 44 -------- .../decorators/set_joint_goal_constraint.py | 23 ++-- .../decorators/set_joint_path_constraint.py | 23 ++-- .../set_orientation_goal_constraint.py | 27 +++-- .../set_orientation_path_constraint.py | 27 +++-- .../set_position_goal_constraint.py | 25 +++-- .../set_position_path_constraint.py | 25 +++-- ada_feeding/ada_feeding/helpers.py | 82 +++++++++++++- .../idioms/add_pose_path_constraints.py | 28 ++++- .../trees/move_to_configuration_tree.py | 35 +++++- ...o_configuration_with_ft_thresholds_tree.py | 6 + ...uration_with_pose_path_constraints_tree.py | 8 ++ .../ada_feeding/trees/move_to_pose_tree.py | 32 +++++- ...to_pose_with_pose_path_constraints_tree.py | 8 ++ 18 files changed, 415 insertions(+), 178 deletions(-) create mode 100644 ada_feeding/ada_feeding/decorators/clear_constraints.py diff --git a/ada_feeding/ada_feeding/behaviors/move_to.py b/ada_feeding/ada_feeding/behaviors/move_to.py index be7f5f91..90fcd22f 100644 --- a/ada_feeding/ada_feeding/behaviors/move_to.py +++ b/ada_feeding/ada_feeding/behaviors/move_to.py @@ -13,16 +13,15 @@ from action_msgs.msg import GoalStatus from moveit_msgs.msg import MoveItErrorCodes import py_trees -from pymoveit2 import MoveIt2, MoveIt2State +from pymoveit2 import MoveIt2State from pymoveit2.robots import kinova -from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from rclpy.qos import QoSPresetProfiles from sensor_msgs.msg import JointState from trajectory_msgs.msg import JointTrajectory # Local imports -from ada_feeding.helpers import get_from_blackboard_with_default +from ada_feeding.helpers import get_from_blackboard_with_default, get_moveit2_object class MoveTo(py_trees.behaviour.Behaviour): @@ -118,19 +117,10 @@ def __init__( key="motion_curr_distance", access=py_trees.common.Access.WRITE ) - # Create MoveIt 2 interface for moving the Jaco arm. This must be done - # in __init__ and not setup since the MoveIt2 interface must be - # initialized before the ROS2 node starts spinning. - # Using ReentrantCallbackGroup to align with the examples from pymoveit2. - # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. - callback_group = ReentrantCallbackGroup() - self.moveit2 = MoveIt2( - node=self.node, - joint_names=kinova.joint_names(), - base_link_name=kinova.base_link_name(), - end_effector_name="forkTip", - group_name=kinova.MOVE_GROUP_ARM, - callback_group=callback_group, + # Get the MoveIt2 object. + self.moveit2, self.moveit2_lock = get_moveit2_object( + self.move_to_blackboard, + self.node, ) # Subscribe to the joint state and track the distance to goal while the @@ -153,20 +143,21 @@ def initialise(self) -> None: """ self.logger.info(f"{self.name} [MoveTo::initialise()]") - # Set the planner_id - self.moveit2.planner_id = get_from_blackboard_with_default( - self.move_to_blackboard, "planner_id", "RRTstarkConfigDefault" - ) + with self.moveit2_lock: + # Set the planner_id + self.moveit2.planner_id = get_from_blackboard_with_default( + self.move_to_blackboard, "planner_id", "RRTstarkConfigDefault" + ) - # Set the max velocity - self.moveit2.max_velocity = get_from_blackboard_with_default( - self.move_to_blackboard, "max_velocity_scaling_factor", 0.1 - ) + # Set the max velocity + self.moveit2.max_velocity = get_from_blackboard_with_default( + self.move_to_blackboard, "max_velocity_scaling_factor", 0.1 + ) - # Set the allowed planning time - self.moveit2.allowed_planning_time = get_from_blackboard_with_default( - self.move_to_blackboard, "allowed_planning_time", 0.5 - ) + # Set the allowed planning time + self.moveit2.allowed_planning_time = get_from_blackboard_with_default( + self.move_to_blackboard, "allowed_planning_time", 0.5 + ) # Reset local state variables self.prev_query_state = None @@ -218,7 +209,8 @@ def update(self) -> py_trees.common.Status: ) return py_trees.common.Status.FAILURE # Initiate an asynchronous planning call - planning_future = self.moveit2.plan_async(cartesian=self.cartesian) + with self.moveit2_lock: + planning_future = self.moveit2.plan_async(cartesian=self.cartesian) if planning_future is None: self.logger.error( f"{self.name} [MoveTo::update()] Failed to initiate planning!" @@ -234,9 +226,10 @@ def update(self) -> py_trees.common.Status: self.motion_start_time = time.time() # Get the trajectory - traj = self.moveit2.get_trajectory( - self.planning_future, cartesian=self.cartesian - ) + with self.moveit2_lock: + traj = self.moveit2.get_trajectory( + self.planning_future, cartesian=self.cartesian + ) self.logger.info(f"Trajectory: {traj} | type {type(traj)}") if traj is None: self.logger.error( @@ -253,7 +246,8 @@ def update(self) -> py_trees.common.Status: ) # Send the trajectory to MoveIt - self.moveit2.execute(traj) + with self.moveit2_lock: + self.moveit2.execute(traj) return py_trees.common.Status.RUNNING # Still planning @@ -262,16 +256,20 @@ def update(self) -> py_trees.common.Status: # Is moving self.tree_blackboard.motion_time = time.time() - self.motion_start_time if self.motion_future is None: - if self.moveit2.query_state() == MoveIt2State.REQUESTING: + with self.moveit2_lock: + query_state = self.moveit2.query_state() + if query_state == MoveIt2State.REQUESTING: # The goal has been sent to the action server, but not yet accepted return py_trees.common.Status.RUNNING - if self.moveit2.query_state() == MoveIt2State.EXECUTING: + if query_state == MoveIt2State.EXECUTING: # The goal has been accepted and is executing. In this case # don't return a status since we drop down into the below # for when the robot is in motion. - self.motion_future = self.moveit2.get_execution_future() - elif self.moveit2.query_state() == MoveIt2State.IDLE: - last_error_code = self.moveit2.get_last_execution_error_code() + with self.moveit2_lock: + self.motion_future = self.moveit2.get_execution_future() + elif query_state == MoveIt2State.IDLE: + with self.moveit2_lock: + last_error_code = self.moveit2.get_last_execution_error_code() if last_error_code is None or last_error_code.val != 1: # If we get here then something went wrong (e.g., controller # is already executing a trajectory, action server not @@ -327,23 +325,24 @@ def terminate(self, new_status: py_trees.common.Status) -> None: terminate_requested_time = time.time() rate = self.node.create_rate(self.terminate_rate_hz) # A termination request has not succeeded until the MoveIt2 action server is IDLE - while self.moveit2.query_state() != MoveIt2State.IDLE: - self.node.get_logger().info( - f"MoveTo Update MoveIt2State not Idle {time.time()} {terminate_requested_time} " - f"{self.terminate_timeout_s}" - ) - # If the goal is executing, cancel it - if self.moveit2.query_state() == MoveIt2State.EXECUTING: - self.moveit2.cancel_execution() - - # Check for terminate timeout - if time.time() - terminate_requested_time > self.terminate_timeout_s: - self.logger.error( - f"{self.name} [MoveTo::terminate()] Terminate timed out!" + with self.moveit2_lock: + while self.moveit2.query_state() != MoveIt2State.IDLE: + self.node.get_logger().info( + f"MoveTo Update MoveIt2State not Idle {time.time()} {terminate_requested_time} " + f"{self.terminate_timeout_s}" ) - break + # If the goal is executing, cancel it + if self.moveit2.query_state() == MoveIt2State.EXECUTING: + self.moveit2.cancel_execution() + + # Check for terminate timeout + if time.time() - terminate_requested_time > self.terminate_timeout_s: + self.logger.error( + f"{self.name} [MoveTo::terminate()] Terminate timed out!" + ) + break - rate.sleep() + rate.sleep() def shutdown(self) -> None: """ diff --git a/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py b/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py index e802c4c4..abbb8914 100644 --- a/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py +++ b/ada_feeding/ada_feeding/behaviors/toggle_collision_object.py @@ -10,12 +10,10 @@ # Third-party imports import py_trees -from pymoveit2 import MoveIt2 -from pymoveit2.robots import kinova -from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node # Local imports +from ada_feeding.helpers import get_moveit2_object class ToggleCollisionObject(py_trees.behaviour.Behaviour): @@ -53,19 +51,13 @@ def __init__( self.collision_object_id = collision_object_id self.allow = allow - # Create MoveIt 2 interface for moving the Jaco arm. This must be done - # in __init__ and not setup since the MoveIt2 interface must be - # initialized before the ROS2 node starts spinning. - # Using ReentrantCallbackGroup to align with the examples from pymoveit2. - # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. - callback_group = ReentrantCallbackGroup() - self.moveit2 = MoveIt2( - node=self.node, - joint_names=kinova.joint_names(), - base_link_name=kinova.base_link_name(), - end_effector_name="forkTip", - group_name=kinova.MOVE_GROUP_ARM, - callback_group=callback_group, + # Get the MoveIt2 object. + self.blackboard = self.attach_blackboard_client( + name=name + " ToggleCollisionObject", namespace=name + ) + self.moveit2, self.moveit2_lock = get_moveit2_object( + self.blackboard, + self.node, ) def update(self) -> py_trees.common.Status: @@ -79,7 +71,8 @@ def update(self) -> py_trees.common.Status: """ self.logger.info(f"{self.name} [ToggleCollisionObject::update()]") # (Dis)allow collisions between the robot and the collision object - succ = self.moveit2.allow_collisions(self.collision_object_id, self.allow) + with self.moveit2_lock: + succ = self.moveit2.allow_collisions(self.collision_object_id, self.allow) # Return success if succ: diff --git a/ada_feeding/ada_feeding/decorators/__init__.py b/ada_feeding/ada_feeding/decorators/__init__.py index 3258a92e..3a4f9236 100644 --- a/ada_feeding/ada_feeding/decorators/__init__.py +++ b/ada_feeding/ada_feeding/decorators/__init__.py @@ -10,6 +10,9 @@ # Parent class for all decorators that add constraints from .move_to_constraint import MoveToConstraint +# Clear constraints +from .clear_constraints import ClearConstraints + # Goal constraints from .set_joint_goal_constraint import SetJointGoalConstraint from .set_position_goal_constraint import SetPositionGoalConstraint diff --git a/ada_feeding/ada_feeding/decorators/clear_constraints.py b/ada_feeding/ada_feeding/decorators/clear_constraints.py new file mode 100644 index 00000000..8e54a298 --- /dev/null +++ b/ada_feeding/ada_feeding/decorators/clear_constraints.py @@ -0,0 +1,65 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +This module defines the ClearConstraints decorator, which clears all constraints +on the MoveIt2 object. +""" +# Third-party imports +import py_trees +from rclpy.node import Node + +# Local imports +from ada_feeding.decorators import MoveToConstraint +from ada_feeding.helpers import get_moveit2_object + +# pylint: disable=duplicate-code +# All the constraints have similar code when registering and setting blackboard +# keys, since the parameters for constraints are similar. This is not a problem. + + +class ClearConstraints(MoveToConstraint): + """ + ClearConstraints clears all constraints on the MoveIt2 object. This + Should be at the top of a branch of constraints and the MoveTo behavior, + in case any previous behavior left lingering constraints (e.g., due to + an error). + """ + + def __init__( + self, + name: str, + child: py_trees.behaviour.Behaviour, + node: Node, + ): + """ + Initialize the MoveToConstraint decorator. + + Parameters + ---------- + name: The name of the behavior. + child: The child behavior. + """ + # Initiatilize the decorator + super().__init__(name=name, child=child) + + # Define inputs from the blackboard + self.blackboard = self.attach_blackboard_client( + name=name + " ClearConstraints", namespace=name + ) + + # Get the MoveIt2 object. + self.moveit2, self.moveit2_lock = get_moveit2_object( + self.blackboard, + node, + ) + + def set_constraint(self) -> None: + """ + Sets the joint goal constraint. + """ + self.logger.info(f"{self.name} [ClearConstraints::set_constraint()]") + + # Set the constraint + with self.moveit2_lock: + self.moveit2.clear_goal_constraints() + self.moveit2.clear_path_constraints() diff --git a/ada_feeding/ada_feeding/decorators/move_to_constraint.py b/ada_feeding/ada_feeding/decorators/move_to_constraint.py index a1f075a5..a96887e0 100644 --- a/ada_feeding/ada_feeding/decorators/move_to_constraint.py +++ b/ada_feeding/ada_feeding/decorators/move_to_constraint.py @@ -12,10 +12,8 @@ # Third-party imports import py_trees -from pymoveit2 import MoveIt2 # Local imports -from ada_feeding.behaviors import MoveTo class MoveToConstraint(py_trees.decorators.Decorator, ABC): @@ -24,36 +22,6 @@ class MoveToConstraint(py_trees.decorators.Decorator, ABC): the robot using MoveIt2. """ - def __init__( - self, - name: str, - child: py_trees.behaviour.Behaviour, - ): - """ - Initialize the MoveToConstraint decorator. - - Parameters - ---------- - name: The name of the behavior. - child: The child behavior. - """ - # Check the child behavior type - if not isinstance(child, (MoveTo, MoveToConstraint)): - raise TypeError( - f"{name} [MoveToConstraint::__init__()] Child must be of " - "type MoveTo or MoveToConstraint!" - ) - - # Initiatilize the decorator - super().__init__(name=name, child=child) - - @property - def moveit2(self) -> MoveIt2: - """ - Get the MoveIt2 interface. - """ - return self.decorated.moveit2 - def initialise(self) -> None: """ Set the constraints before the child behavior starts executing. @@ -78,15 +46,3 @@ def update(self) -> py_trees.common.Status: Just pass through the child's status """ return self.decorated.status - - def terminate(self, new_status: py_trees.common.Status) -> None: - """ - Clear the constraints. - """ - self.logger.info( - f"{self.name} [MoveToConstraint::terminate()][{self.status}->{new_status}]" - ) - - # Clear the constraints - self.moveit2.clear_goal_constraints() - self.moveit2.clear_path_constraints() diff --git a/ada_feeding/ada_feeding/decorators/set_joint_goal_constraint.py b/ada_feeding/ada_feeding/decorators/set_joint_goal_constraint.py index 85ba1413..4669ab88 100644 --- a/ada_feeding/ada_feeding/decorators/set_joint_goal_constraint.py +++ b/ada_feeding/ada_feeding/decorators/set_joint_goal_constraint.py @@ -6,10 +6,11 @@ """ # Third-party imports import py_trees +from rclpy.node import Node # Local imports from ada_feeding.decorators import MoveToConstraint -from ada_feeding.helpers import get_from_blackboard_with_default +from ada_feeding.helpers import get_from_blackboard_with_default, get_moveit2_object # pylint: disable=duplicate-code # All the constraints have similar code when registering and setting blackboard @@ -26,6 +27,7 @@ def __init__( self, name: str, child: py_trees.behaviour.Behaviour, + node: Node, ): """ Initialize the MoveToConstraint decorator. @@ -53,6 +55,12 @@ def __init__( ) self.blackboard.register_key(key="weight", access=py_trees.common.Access.READ) + # Get the MoveIt2 object. + self.moveit2, self.moveit2_lock = get_moveit2_object( + self.blackboard, + node, + ) + def set_constraint(self) -> None: """ Sets the joint goal constraint. @@ -70,9 +78,10 @@ def set_constraint(self) -> None: weight = get_from_blackboard_with_default(self.blackboard, "weight", 1.0) # Set the constraint - self.moveit2.set_joint_goal( - joint_positions=joint_positions, - joint_names=joint_names, - tolerance=tolerance, - weight=weight, - ) + with self.moveit2_lock: + self.moveit2.set_joint_goal( + joint_positions=joint_positions, + joint_names=joint_names, + tolerance=tolerance, + weight=weight, + ) diff --git a/ada_feeding/ada_feeding/decorators/set_joint_path_constraint.py b/ada_feeding/ada_feeding/decorators/set_joint_path_constraint.py index 31d1803e..78343a7d 100644 --- a/ada_feeding/ada_feeding/decorators/set_joint_path_constraint.py +++ b/ada_feeding/ada_feeding/decorators/set_joint_path_constraint.py @@ -6,10 +6,11 @@ """ # Third-party imports import py_trees +from rclpy.node import Node # Local imports from ada_feeding.decorators import MoveToConstraint -from ada_feeding.helpers import get_from_blackboard_with_default +from ada_feeding.helpers import get_from_blackboard_with_default, get_moveit2_object # pylint: disable=duplicate-code # All the constraints have similar code when registering and setting blackboard @@ -26,6 +27,7 @@ def __init__( self, name: str, child: py_trees.behaviour.Behaviour, + node: Node, ): """ Initialize the MoveToConstraint decorator. @@ -53,6 +55,12 @@ def __init__( ) self.blackboard.register_key(key="weight", access=py_trees.common.Access.READ) + # Get the MoveIt2 object. + self.moveit2, self.moveit2_lock = get_moveit2_object( + self.blackboard, + node, + ) + def set_constraint(self) -> None: """ Sets the joint path constraint. @@ -70,9 +78,10 @@ def set_constraint(self) -> None: weight = get_from_blackboard_with_default(self.blackboard, "weight", 1.0) # Set the constraint - self.moveit2.set_path_joint_constraint( - joint_positions=joint_positions, - joint_names=joint_names, - tolerance=tolerance, - weight=weight, - ) + with self.moveit2_lock: + self.moveit2.set_path_joint_constraint( + joint_positions=joint_positions, + joint_names=joint_names, + tolerance=tolerance, + weight=weight, + ) diff --git a/ada_feeding/ada_feeding/decorators/set_orientation_goal_constraint.py b/ada_feeding/ada_feeding/decorators/set_orientation_goal_constraint.py index a75ace84..2c45a9d5 100644 --- a/ada_feeding/ada_feeding/decorators/set_orientation_goal_constraint.py +++ b/ada_feeding/ada_feeding/decorators/set_orientation_goal_constraint.py @@ -6,10 +6,11 @@ """ # Third-party imports import py_trees +from rclpy.node import Node # Local imports from ada_feeding.decorators import MoveToConstraint -from ada_feeding.helpers import get_from_blackboard_with_default +from ada_feeding.helpers import get_from_blackboard_with_default, get_moveit2_object # pylint: disable=duplicate-code # All the constraints have similar code when registering and setting blackboard @@ -26,6 +27,7 @@ def __init__( self, name: str, child: py_trees.behaviour.Behaviour, + node: Node, ): """ Initialize the MoveToConstraint decorator. @@ -57,6 +59,12 @@ def __init__( key="parameterization", access=py_trees.common.Access.READ ) + # Get the MoveIt2 object. + self.moveit2, self.moveit2_lock = get_moveit2_object( + self.blackboard, + node, + ) + def set_constraint(self) -> None: """ Sets the orientation goal constraint. @@ -80,11 +88,12 @@ def set_constraint(self) -> None: ) # Set the constraint - self.moveit2.set_orientation_goal( - quat_xyzw=quat_xyzw, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance, - weight=weight, - parameterization=parameterization, - ) + with self.moveit2_lock: + self.moveit2.set_orientation_goal( + quat_xyzw=quat_xyzw, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + parameterization=parameterization, + ) diff --git a/ada_feeding/ada_feeding/decorators/set_orientation_path_constraint.py b/ada_feeding/ada_feeding/decorators/set_orientation_path_constraint.py index 0ff4fd51..1df0107f 100644 --- a/ada_feeding/ada_feeding/decorators/set_orientation_path_constraint.py +++ b/ada_feeding/ada_feeding/decorators/set_orientation_path_constraint.py @@ -7,10 +7,11 @@ """ # Third-party imports import py_trees +from rclpy.node import Node # Local imports from ada_feeding.decorators import MoveToConstraint -from ada_feeding.helpers import get_from_blackboard_with_default +from ada_feeding.helpers import get_from_blackboard_with_default, get_moveit2_object # pylint: disable=duplicate-code # All the constraints have similar code when registering and setting blackboard @@ -27,6 +28,7 @@ def __init__( self, name: str, child: py_trees.behaviour.Behaviour, + node: Node, ): """ Initialize the MoveToConstraint decorator. @@ -58,6 +60,12 @@ def __init__( key="parameterization", access=py_trees.common.Access.READ ) + # Get the MoveIt2 object. + self.moveit2, self.moveit2_lock = get_moveit2_object( + self.blackboard, + node, + ) + def set_constraint(self) -> None: """ Sets the orientation goal constraint. @@ -81,11 +89,12 @@ def set_constraint(self) -> None: ) # Set the constraint - self.moveit2.set_path_orientation_constraint( - quat_xyzw=quat_xyzw, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance, - weight=weight, - parameterization=parameterization, - ) + with self.moveit2_lock: + self.moveit2.set_path_orientation_constraint( + quat_xyzw=quat_xyzw, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + parameterization=parameterization, + ) diff --git a/ada_feeding/ada_feeding/decorators/set_position_goal_constraint.py b/ada_feeding/ada_feeding/decorators/set_position_goal_constraint.py index 45b74b21..f19db40c 100644 --- a/ada_feeding/ada_feeding/decorators/set_position_goal_constraint.py +++ b/ada_feeding/ada_feeding/decorators/set_position_goal_constraint.py @@ -6,10 +6,11 @@ """ # Third-party imports import py_trees +from rclpy.node import Node # Local imports from ada_feeding.decorators import MoveToConstraint -from ada_feeding.helpers import get_from_blackboard_with_default +from ada_feeding.helpers import get_from_blackboard_with_default, get_moveit2_object # pylint: disable=duplicate-code # All the constraints have similar code when registering and setting blackboard @@ -26,6 +27,7 @@ def __init__( self, name: str, child: py_trees.behaviour.Behaviour, + node: Node, ): """ Initialize the MoveToConstraint decorator. @@ -52,6 +54,12 @@ def __init__( ) self.blackboard.register_key(key="weight", access=py_trees.common.Access.READ) + # Get the MoveIt2 object. + self.moveit2, self.moveit2_lock = get_moveit2_object( + self.blackboard, + node, + ) + def set_constraint(self) -> None: """ Sets the position goal constraint. @@ -70,10 +78,11 @@ def set_constraint(self) -> None: weight = get_from_blackboard_with_default(self.blackboard, "weight", 1.0) # Set the constraint - self.moveit2.set_position_goal( - position=position, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance, - weight=weight, - ) + with self.moveit2_lock: + self.moveit2.set_position_goal( + position=position, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + ) diff --git a/ada_feeding/ada_feeding/decorators/set_position_path_constraint.py b/ada_feeding/ada_feeding/decorators/set_position_path_constraint.py index 2a811597..33598831 100644 --- a/ada_feeding/ada_feeding/decorators/set_position_path_constraint.py +++ b/ada_feeding/ada_feeding/decorators/set_position_path_constraint.py @@ -7,10 +7,11 @@ """ # Third-party imports import py_trees +from rclpy.node import Node # Local imports from ada_feeding.decorators import MoveToConstraint -from ada_feeding.helpers import get_from_blackboard_with_default +from ada_feeding.helpers import get_from_blackboard_with_default, get_moveit2_object # pylint: disable=duplicate-code # All the constraints have similar code when registering and setting blackboard @@ -27,6 +28,7 @@ def __init__( self, name: str, child: py_trees.behaviour.Behaviour, + node: Node, ): """ Initialize the MoveToConstraint decorator. @@ -53,6 +55,12 @@ def __init__( ) self.blackboard.register_key(key="weight", access=py_trees.common.Access.READ) + # Get the MoveIt2 object. + self.moveit2, self.moveit2_lock = get_moveit2_object( + self.blackboard, + node, + ) + def set_constraint(self) -> None: """ Sets the position path constraint. @@ -71,10 +79,11 @@ def set_constraint(self) -> None: weight = get_from_blackboard_with_default(self.blackboard, "weight", 1.0) # Set the constraint - self.moveit2.set_path_position_constraint( - position=position, - frame_id=frame_id, - target_link=target_link, - tolerance=tolerance, - weight=weight, - ) + with self.moveit2_lock: + self.moveit2.set_path_position_constraint( + position=position, + frame_id=frame_id, + target_link=target_link, + tolerance=tolerance, + weight=weight, + ) diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 1493d5e0..a92616bd 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -4,10 +4,16 @@ """ # Standard imports -from typing import Any, Set +from threading import Lock +from typing import Any, Optional, Set, Tuple # Third-party imports import py_trees +from py_trees.common import Access +from pymoveit2 import MoveIt2 +from pymoveit2.robots import kinova +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node # These prefixes are used to separate namespaces from each other. # For example, if we have a behavior `foo` that has a position goal constraint, @@ -19,6 +25,7 @@ # Note that this norm can only be used if each MoveTo behavior has maximally # one constraint of each type. When creating a behavior with multiple constraints # of the same time, you'll have to create custom namespaces. +CLEAR_CONSTRAINTS_NAMESPACE_PREFIX = "clear_constraints" POSITION_GOAL_CONSTRAINT_NAMESPACE_PREFIX = "position_goal_constraint" ORIENTATION_GOAL_CONSTRAINT_NAMESPACE_PREFIX = "orientation_goal_constraint" JOINT_GOAL_CONSTRAINT_NAMESPACE_PREFIX = "joint_goal_constraint" @@ -28,6 +35,79 @@ MOVE_TO_NAMESPACE_PREFIX = "move_to" +def get_moveit2_object( + blackboard: py_trees.blackboard.Client, + node: Optional[Node] = None, +) -> Tuple[MoveIt2, Lock]: + """ + Gets the MoveIt2 object and its corresponding lock from the blackboard. + If they do not exist on the blackboard, they are created. + + Parameters + ---------- + blackboard: The blackboard client. Any blackboard client can be used, as + this function: (a) uses absolute paths; and (b) registers the keys if + they are not already registered. + node: The ROS2 node that the MoveIt2 object should be associated with, if + we need to create it from scratch. If None, this function will not create + the MoveIt2 object if it doesn't exist, and will instead raise a KeyError. + + Returns + ------- + moveit2: The MoveIt2 object. + lock: The lock for the MoveIt2 object. + + Raises + ------- + KeyError: if the MoveIt2 object does not exist and node is None. + """ + # These Blackboard keys are used to store the single, global MoveIt2 object + # and its corresponding lock. Note that it is important that these keys start with + # a "/" because to indicate it is an absolute path, so all behaviors can access + # the same object. + moveit2_blackboard_key = "/moveit2" + moveit2_lock_blackboard_key = "/moveit2_lock" + + # First, register the MoveIt2 object and its corresponding lock for READ access + if not blackboard.is_registered(moveit2_blackboard_key, Access.READ): + blackboard.register_key(moveit2_blackboard_key, Access.READ) + if not blackboard.is_registered(moveit2_lock_blackboard_key, Access.READ): + blackboard.register_key(moveit2_lock_blackboard_key, Access.READ) + + # Second, check if the MoveIt2 object and its corresponding lock exist on the + # blackboard. If they do not, register the blackboard for WRITE access to those + # keys and create them. + try: + moveit2 = blackboard.get(moveit2_blackboard_key) + lock = blackboard.get(moveit2_lock_blackboard_key) + except KeyError as exc: + # If no node is passed in, raise an error. + if node is None: + raise KeyError("MoveIt2 object does not exist on the blackboard") from exc + + # If a node is passed in, create a new MoveIt2 object and lock. + node.get_logger().info( + "MoveIt2 object and lock do not exist on the blackboard. Creating them now." + ) + blackboard.register_key(moveit2_blackboard_key, Access.WRITE) + blackboard.register_key(moveit2_lock_blackboard_key, Access.WRITE) + # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. + callback_group = ReentrantCallbackGroup() + moveit2 = MoveIt2( + node=node, + joint_names=kinova.joint_names(), + base_link_name=kinova.base_link_name(), + end_effector_name="forkTip", + group_name=kinova.MOVE_GROUP_ARM, + callback_group=callback_group, + ) + lock = Lock() + blackboard.set(moveit2_blackboard_key, moveit2) + blackboard.set(moveit2_lock_blackboard_key, lock) + + return moveit2, lock + + def get_from_blackboard_with_default( blackboard: py_trees.blackboard.Client, key: str, default: Any ) -> Any: diff --git a/ada_feeding/ada_feeding/idioms/add_pose_path_constraints.py b/ada_feeding/ada_feeding/idioms/add_pose_path_constraints.py index 5b39eae2..b9cb3981 100644 --- a/ada_feeding/ada_feeding/idioms/add_pose_path_constraints.py +++ b/ada_feeding/ada_feeding/idioms/add_pose_path_constraints.py @@ -13,13 +13,16 @@ # Third-party imports import py_trees from py_trees.blackboard import Blackboard +from rclpy.node import Node # Local imports from ada_feeding.decorators import ( + ClearConstraints, SetPositionPathConstraint, SetOrientationPathConstraint, ) from ada_feeding.helpers import ( + CLEAR_CONSTRAINTS_NAMESPACE_PREFIX, POSITION_PATH_CONSTRAINT_NAMESPACE_PREFIX, ORIENTATION_PATH_CONSTRAINT_NAMESPACE_PREFIX, set_to_blackboard, @@ -38,6 +41,7 @@ def add_pose_path_constraints( name: str, blackboard: py_trees.blackboard.Blackboard, logger: logging.Logger, + node: Node, keys_to_not_write_to_blackboard: Set[str] = set(), # Optional parameters for the pose path constraint position_path: Optional[Tuple[float, float, float]] = None, @@ -49,6 +53,7 @@ def add_pose_path_constraints( parameterization_orientation_path: int = 0, weight_position_path: float = 1.0, weight_orientation_path: float = 1.0, + clear_constraints: bool = True, ) -> py_trees.behaviour.Behaviour: """ Adds a SetPositionPathConstraint and/or SetOrientationPathConstraint to a @@ -62,6 +67,7 @@ def add_pose_path_constraints( name: The name for the tree that this behavior is in. blackboard: The blackboard for the tree that this behavior is in. logger: The logger for the tree that this behavior is in. + node: The ROS2 node that the MoveIt2 object is associated with. keys_to_not_write_to_blackboard: the keys to not write to the blackboard. Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," @@ -78,6 +84,9 @@ def add_pose_path_constraints( orientation tolerance. weight_position_path: the weight for the position path. weight_orientation_path: the weight for the orientation path. + clear_constraints: Whether or not to put a ClearConstraints decorator at the top + of this branch. If you will be adding additional Constraints on top of this + tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # Separate blackboard namespaces for decorators @@ -89,6 +98,7 @@ def add_pose_path_constraints( orientation_path_constraint_namespace_prefix = ( ORIENTATION_PATH_CONSTRAINT_NAMESPACE_PREFIX ) + clear_constraints_namespace_prefix = CLEAR_CONSTRAINTS_NAMESPACE_PREFIX # Position constraints if position_path is not None: @@ -230,7 +240,7 @@ def add_pose_path_constraints( [name, position_path_constraint_namespace_prefix] ) position_constraint = SetPositionPathConstraint( - position_path_constaint_name, child + position_path_constaint_name, child, node ) position_constraint.logger = logger else: @@ -242,10 +252,22 @@ def add_pose_path_constraints( [name, orientation_path_constraint_namespace_prefix] ) orientation_constraint = SetOrientationPathConstraint( - orientation_goal_constaint_name, position_constraint + orientation_goal_constaint_name, position_constraint, node ) orientation_constraint.logger = logger else: orientation_constraint = position_constraint - return orientation_constraint + # Clear any previous constraints that may be in the MoveIt2 object + if clear_constraints: + clear_constraints_name = Blackboard.separator.join( + [name, clear_constraints_namespace_prefix] + ) + clear_constraints_behavior = ClearConstraints( + clear_constraints_name, orientation_constraint, node + ) + clear_constraints_behavior.logger = logger + else: + clear_constraints_behavior = orientation_constraint + + return clear_constraints_behavior diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py index b46c81c6..26a54578 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_tree.py @@ -16,8 +16,13 @@ # Local imports from ada_feeding.behaviors import MoveTo -from ada_feeding.decorators import SetJointGoalConstraint -from ada_feeding.helpers import set_to_blackboard +from ada_feeding.decorators import ClearConstraints, SetJointGoalConstraint +from ada_feeding.helpers import ( + CLEAR_CONSTRAINTS_NAMESPACE_PREFIX, + JOINT_GOAL_CONSTRAINT_NAMESPACE_PREFIX, + MOVE_TO_NAMESPACE_PREFIX, + set_to_blackboard, +) from ada_feeding.trees import MoveToTree # pylint: disable=duplicate-code @@ -45,6 +50,7 @@ def __init__( allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, keys_to_not_write_to_blackboard: Set[str] = set(), + clear_constraints: bool = True, ): """ Initializes tree-specific parameters. @@ -63,6 +69,9 @@ def __init__( Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," etc. + clear_constraints: Whether or not to put a ClearConstraints decorator at the top + of this branch. If you will be adding additional Constraints on top of this + tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # Initialize MoveToTree super().__init__() @@ -76,6 +85,7 @@ def __init__( self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard + self.clear_constraints = clear_constraints # pylint: disable=too-many-locals # Unfortunately, many local variables are required here to isolate the keys @@ -105,8 +115,9 @@ def create_move_to_tree( tree: The behavior tree that moves the robot above the plate. """ # Separate blackboard namespaces for children - joint_constraint_namespace_prefix = "joint_goal_constraint" - move_to_namespace_prefix = "move_to" + joint_constraint_namespace_prefix = JOINT_GOAL_CONSTRAINT_NAMESPACE_PREFIX + clear_constraints_namespace_prefix = CLEAR_CONSTRAINTS_NAMESPACE_PREFIX + move_to_namespace_prefix = MOVE_TO_NAMESPACE_PREFIX # Inputs for MoveToConfiguration joint_positions_key = Blackboard.separator.join( @@ -195,8 +206,20 @@ def create_move_to_tree( joint_goal_constaint_name = Blackboard.separator.join( [name, joint_constraint_namespace_prefix] ) - root = SetJointGoalConstraint(joint_goal_constaint_name, move_to) - root.logger = logger + joint_constraints = SetJointGoalConstraint( + joint_goal_constaint_name, move_to, node + ) + joint_constraints.logger = logger + + # Clear the constraints + if self.clear_constraints: + clear_constraints_name = Blackboard.separator.join( + [name, clear_constraints_namespace_prefix] + ) + root = ClearConstraints(clear_constraints_name, joint_constraints, node) + root.logger = logger + else: + root = joint_constraints tree = py_trees.trees.BehaviourTree(root) return tree diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py index 940a8f7a..48efd2f1 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_ft_thresholds_tree.py @@ -51,6 +51,7 @@ def __init__( t_y: float = 0.0, t_z: float = 0.0, keys_to_not_write_to_blackboard: Set[str] = set(), + clear_constraints: bool = True, ): """ Initializes tree-specific parameters. @@ -80,6 +81,9 @@ def __init__( Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," etc. + clear_constraints: Whether or not to put a ClearConstraints decorator at the top + of this branch. If you will be adding additional Constraints on top of this + tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # pylint: disable=too-many-locals @@ -110,6 +114,7 @@ def __init__( self.t_z = t_z self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard + self.clear_constraints = clear_constraints def create_move_to_tree( self, @@ -143,6 +148,7 @@ def create_move_to_tree( allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, + clear_constraints=self.clear_constraints, ) .create_tree(name, self.action_type, tree_root_name, logger, node) .root diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py index 6e8145a4..cb442512 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_pose_path_constraints_tree.py @@ -55,6 +55,7 @@ def __init__( weight_position_path: float = 1.0, weight_orientation_path: float = 1.0, keys_to_not_write_to_blackboard: Set[str] = set(), + clear_constraints: bool = True, ): """ Initializes tree-specific parameters. @@ -88,6 +89,9 @@ def __init__( Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," etc. + clear_constraints: Whether or not to put a ClearConstraints decorator at the top + of this branch. If you will be adding additional Constraints on top of this + tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # Initialize MoveToTree super().__init__() @@ -113,6 +117,7 @@ def __init__( self.weight_orientation_path = weight_orientation_path self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard + self.clear_constraints = clear_constraints def create_move_to_tree( self, @@ -149,6 +154,7 @@ def create_move_to_tree( allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, + clear_constraints=False, ) .create_tree(name, self.action_type, tree_root_name, logger, node) .root @@ -170,6 +176,8 @@ def create_move_to_tree( parameterization_orientation_path=self.parameterization_orientation_path, weight_position_path=self.weight_position_path, weight_orientation_path=self.weight_orientation_path, + node=node, + clear_constraints=self.clear_constraints, ) tree = py_trees.trees.BehaviourTree(root) diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py index 1de96925..58b0a01f 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_tree.py @@ -17,10 +17,12 @@ # Local imports from ada_feeding.behaviors import MoveTo from ada_feeding.decorators import ( + ClearConstraints, SetPositionGoalConstraint, SetOrientationGoalConstraint, ) from ada_feeding.helpers import ( + CLEAR_CONSTRAINTS_NAMESPACE_PREFIX, POSITION_GOAL_CONSTRAINT_NAMESPACE_PREFIX, ORIENTATION_GOAL_CONSTRAINT_NAMESPACE_PREFIX, MOVE_TO_NAMESPACE_PREFIX, @@ -39,7 +41,7 @@ class MoveToPoseTree(MoveToTree): A behavior tree that consists of a single behavior, MoveToPose. """ - # pylint: disable=too-many-instance-attributes, too-many-arguments + # pylint: disable=too-many-instance-attributes, too-many-arguments, too-many-locals # Many arguments is fine for this class since it has to be able to configure all parameters # of its constraints. # pylint: disable=dangerous-default-value @@ -60,6 +62,7 @@ def __init__( allowed_planning_time: float = 0.5, max_velocity_scaling_factor: float = 0.1, keys_to_not_write_to_blackboard: Set[str] = set(), + clear_constraints: bool = True, ): """ Initializes tree-specific parameters. @@ -86,6 +89,9 @@ def __init__( Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," etc. + clear_constraints: Whether or not to put a ClearConstraints decorator at the top + of this branch. If you will be adding additional Constraints on top of this + tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # Initialize MoveTo super().__init__() @@ -105,6 +111,7 @@ def __init__( self.allowed_planning_time = allowed_planning_time self.max_velocity_scaling_factor = max_velocity_scaling_factor self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard + self.clear_constraints = clear_constraints # pylint: disable=too-many-locals, too-many-statements # Unfortunately, many locals/statements are required here to isolate the keys @@ -142,6 +149,7 @@ def create_move_to_tree( orientation_goal_constraint_namespace_prefix = ( ORIENTATION_GOAL_CONSTRAINT_NAMESPACE_PREFIX ) + clear_constraints_namespace_prefix = CLEAR_CONSTRAINTS_NAMESPACE_PREFIX move_to_namespace_prefix = MOVE_TO_NAMESPACE_PREFIX # Position constraints @@ -348,7 +356,7 @@ def create_move_to_tree( [name, position_goal_constraint_namespace_prefix] ) position_constraint = SetPositionGoalConstraint( - position_goal_constaint_name, move_to + position_goal_constaint_name, move_to, node ) position_constraint.logger = logger else: @@ -356,15 +364,27 @@ def create_move_to_tree( # Add the orientation goal constraint to the MoveTo behavior if self.quat_xyzw is not None: - orientation_goal_constaint_name = Blackboard.separator.join( + orientation_goal_constraint_name = Blackboard.separator.join( [name, orientation_goal_constraint_namespace_prefix] ) - root = SetOrientationGoalConstraint( - orientation_goal_constaint_name, position_constraint + orientation_constraint = SetOrientationGoalConstraint( + orientation_goal_constraint_name, position_constraint, node + ) + orientation_constraint.logger = logger + else: + orientation_constraint = position_constraint + + # Clear the constraints + if self.clear_constraints: + clear_constraints_name = Blackboard.separator.join( + [name, clear_constraints_namespace_prefix] + ) + root = ClearConstraints( + clear_constraints_name, orientation_constraint, node ) root.logger = logger else: - root = position_constraint + root = orientation_constraint tree = py_trees.trees.BehaviourTree(root) return tree diff --git a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py index 5a0d9300..a01d3ad0 100644 --- a/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_pose_with_pose_path_constraints_tree.py @@ -62,6 +62,7 @@ def __init__( weight_position_path: float = 1.0, weight_orientation_path: float = 1.0, keys_to_not_write_to_blackboard: Set[str] = set(), + clear_constraints: bool = True, ): """ Initializes tree-specific parameters. @@ -101,6 +102,9 @@ def __init__( Note that the keys need to be exact e.g., "move_to.cartesian," "position_goal_constraint.tolerance," "orientation_goal_constraint.tolerance," etc. + clear_constraints: Whether or not to put a ClearConstraints decorator at the top + of this branch. If you will be adding additional Constraints on top of this + tree, this should be False. Else (e.g., if this is a standalone tree), True. """ # Initialize MoveToTree super().__init__() @@ -132,6 +136,7 @@ def __init__( self.weight_orientation_path = weight_orientation_path self.keys_to_not_write_to_blackboard = keys_to_not_write_to_blackboard + self.clear_constraints = clear_constraints def create_move_to_tree( self, @@ -175,6 +180,7 @@ def create_move_to_tree( allowed_planning_time=self.allowed_planning_time, max_velocity_scaling_factor=self.max_velocity_scaling_factor, keys_to_not_write_to_blackboard=self.keys_to_not_write_to_blackboard, + clear_constraints=False, ) .create_tree(name, self.action_type, tree_root_name, logger, node) .root @@ -195,6 +201,8 @@ def create_move_to_tree( parameterization_orientation_path=self.parameterization_orientation_path, weight_position_path=self.weight_position_path, weight_orientation_path=self.weight_orientation_path, + node=node, + clear_constraints=self.clear_constraints, ) tree = py_trees.trees.BehaviourTree(root)