diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py index f9860876..ea020fc2 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py @@ -531,8 +531,14 @@ def get_path_len( return total_len, joint_lens j6_i = None + af1_i = None + af2_i = None if exclude_j6 and "j2n6s200_joint_6" in path.joint_names: j6_i = path.joint_names.index("j2n6s200_joint_6") + if exclude_j6 and "af_joint_1" in path.joint_names: + af1_i = path.joint_names.index("af_joint_1") + if exclude_j6 and "af_joint_2" in path.joint_names: + af2_i = path.joint_names.index("af_joint_2") prev_pos = np.array(path.points[0].positions) for point in path.points: @@ -540,9 +546,15 @@ def get_path_len( seg_len = np.abs(curr_pos - prev_pos) if j6_i is not None: j6_len = seg_len[j6_i] + af1_len = seg_len[af1_i] + af2_len = seg_len[af2_i] seg_len[j6_i] = 0.0 + seg_len[af1_i] = 0.0 + seg_len[af2_i] = 0.0 total_len += np.linalg.norm(seg_len) seg_len[j6_i] = j6_len + seg_len[af1_i] = af1_len + seg_len[af2_i] = af2_len else: total_len += np.linalg.norm(seg_len) for index, name in enumerate(path.joint_names): diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 38573ba3..6e1d93ec 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -9,7 +9,7 @@ # Standard imports import logging from threading import Lock -from typing import Any, Optional, Set, Tuple +from typing import Any, Optional, Set, Tuple, List # Third-party imports import numpy as np @@ -324,6 +324,14 @@ def get_moveit2_object( Raises ------- KeyError: if the MoveIt2 object does not exist and node is None. + + Expects + ------- + The blackboard key `/end_effector_tool` to be set. If this key is not set, + the default value "fork" is used. Other possible values are "none" and + "articulable_fork". It is crucial that the `get_tool_joints` function + can handle these string values. If `/end_effector_tool` is not set, a + warning message will be logged. """ # 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 @@ -331,12 +339,15 @@ def get_moveit2_object( # the same object. moveit2_blackboard_key = "/moveit2" moveit2_lock_blackboard_key = "/moveit2_lock" + end_effector_tool_blackboard_key = "/end_effector_tool" # 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) + if not blackboard.is_registered(end_effector_tool_blackboard_key, Access.READ): + blackboard.register_key(end_effector_tool_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 @@ -355,14 +366,30 @@ def get_moveit2_object( ) blackboard.register_key(moveit2_blackboard_key, Access.WRITE) blackboard.register_key(moveit2_lock_blackboard_key, Access.WRITE) + blackboard.register_key(end_effector_tool_blackboard_key, Access.WRITE) + end_effector_tool = "fork" # Assign to default value + if blackboard.get(end_effector_tool_blackboard_key) is None: # Check if not set + node.get_logger().warn( + f"end_effector_tool not set, using default: {end_effector_tool}" + ) + blackboard.set( + end_effector_tool_blackboard_key, end_effector_tool + ) # Set the value on the blackboard + else: + end_effector_tool = blackboard.get( + end_effector_tool_blackboard_key + ) # Get the value from the blackboard # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. callback_group = ReentrantCallbackGroup() + tool_joints = get_tool_joints(blackboard.get(end_effector_tool_blackboard_key)) + joint_names = kinova.joint_names() + tool_joints + node.get_logger().info(f"Creating MoveIt2 object with: {joint_names}") moveit2 = MoveIt2( node=node, - joint_names=kinova.joint_names(), + joint_names=joint_names, base_link_name=kinova.base_link_name(), end_effector_name="forkTip", - group_name="jaco_arm", + group_name="ada", callback_group=callback_group, ) lock = Lock() @@ -454,3 +481,30 @@ def import_from_string(import_string: str) -> Any: ) except Exception as exc: raise ImportError(f"Error importing {import_string}") from exc + + +def get_tool_joints(end_effector_tool: str) -> List[str]: + """ + Returns a list of joint names associated with the given end-effector tool. + + Args: + end_effector_tool: The name of the end-effector tool. + + Returns: + A list of joint names, or an empty list if no joints are associated + with the tool. + + Raises: + ValueError: If an unsupported tool is provided. + """ + + tool_joints = { + "articulable_fork": ["af_joint_1", "af_joint_2"], + "none": [], + "fork": [], + } + + if end_effector_tool in tool_joints: + return tool_joints[end_effector_tool] + + raise ValueError(f"Unknown end_effector_tool: {end_effector_tool}") diff --git a/ada_feeding/ada_feeding/trees/activate_controller.py b/ada_feeding/ada_feeding/trees/activate_controller.py index 1f18561c..52143b2f 100644 --- a/ada_feeding/ada_feeding/trees/activate_controller.py +++ b/ada_feeding/ada_feeding/trees/activate_controller.py @@ -60,6 +60,8 @@ def __init__( "jaco_arm_cartesian_controller", "jaco_arm_controller", "jaco_arm_servo_controller", + "af_controller", + "af_servo_controller", ] self.re_tare = re_tare diff --git a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py index 3bf7f32b..08e59056 100644 --- a/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py +++ b/ada_feeding/ada_feeding/trees/move_from_mouth_tree.py @@ -152,8 +152,6 @@ def __init__( self.staging_configuration_position = staging_configuration_position self.staging_configuration_quat_xyzw = staging_configuration_quat_xyzw self.end_configuration = end_configuration - if self.end_configuration is not None: - assert len(self.end_configuration) == 6, "Must provide 6 joint positions" self.staging_configuration_tolerance_position = ( staging_configuration_tolerance_position ) 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 f8a42f00..386adc4c 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 @@ -264,9 +264,6 @@ def send_goal(self, tree: py_trees.trees.BehaviourTree, goal: object) -> bool: assert ( self.joint_positions is not None ), "For action MoveTo, must provide hardcoded joint_positions" - assert ( - len(self.joint_positions) == 6 - ), "For action MoveTo, must provide 6 joint positions" # Adds MoveToVisitor for Feedback return super().send_goal(tree, goal) diff --git a/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py b/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py index 4245e17b..e2584462 100644 --- a/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py +++ b/ada_feeding/ada_feeding/trees/move_to_configuration_with_wheelchair_wall_tree.py @@ -95,7 +95,6 @@ def __init__( # Store the parameters self.goal_configuration = goal_configuration - assert len(self.goal_configuration) == 6, "Must provide 6 joint positions" self.goal_configuration_tolerance = goal_configuration_tolerance self.orientation_constraint_quaternion = orientation_constraint_quaternion self.orientation_constraint_tolerances = orientation_constraint_tolerances diff --git a/ada_feeding/config/ada_feeding_action_servers_custom.yaml b/ada_feeding/config/ada_feeding_action_servers_custom.yaml index 2c463282..4a271efc 100644 --- a/ada_feeding/config/ada_feeding_action_servers_custom.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_custom.yaml @@ -118,12 +118,12 @@ ada_feeding_action_servers: - -2.0289974534007733 - -3.1847696853949206 MoveAbovePlate.tree_kwargs.joint_positions: - - 6.794832881070045 - - 3.031285852819256 - - 4.479355460783922 - - 7.187106922258792 - - -1.9369287787234262 - - -3.5496749526931417 + - -2.4538579336877304 + - 3.07974419938212 + - 1.8320725365979 + - 4.096143890468605 + - -2.003422584820525 + - -3.2123560395465063 MoveFromMouth.tree_kwargs.staging_configuration_position: - -0.6081959669757366 - 0.07835060871598665 diff --git a/ada_feeding/config/ada_feeding_action_servers_default.yaml b/ada_feeding/config/ada_feeding_action_servers_default.yaml index ce333b10..ead904b7 100644 --- a/ada_feeding/config/ada_feeding_action_servers_default.yaml +++ b/ada_feeding/config/ada_feeding_action_servers_default.yaml @@ -39,12 +39,12 @@ ada_feeding_action_servers: - max_velocity_scaling_factor tree_kwargs: # optional joint_positions: # required - - -2.3149168248766614 # j2n6s200_joint_1 - - 3.1444595465032634 # j2n6s200_joint_2 - - 1.7332586075115999 # j2n6s200_joint_3 - - -2.3609596843308234 # j2n6s200_joint_4 - - 4.43936623280362 # j2n6s200_joint_5 - - 3.06866544924739 # j2n6s200_joint_6 + - -2.4538579336877304 # j2n6s200_joint_1 + - 3.07974419938212 # j2n6s200_joint_2 + - 1.8320725365979 # j2n6s200_joint_3 + - 4.096143890468605 # j2n6s200_joint_4 + - -2.003422584820525 # j2n6s200_joint_5 + - -3.2123560395465063 # j2n6s200_joint_6 toggle_watchdog_listener: false # optional, default: true f_mag: 4.0 # N max_velocity_scaling_factor: 1.0 # optional in (0.0, 1.0], default: 0.1 diff --git a/ada_feeding/launch/ada_feeding_launch.xml b/ada_feeding/launch/ada_feeding_launch.xml index 6e89a401..f8e792ce 100644 --- a/ada_feeding/launch/ada_feeding_launch.xml +++ b/ada_feeding/launch/ada_feeding_launch.xml @@ -9,6 +9,7 @@ + @@ -26,6 +27,7 @@ + diff --git a/ada_feeding/scripts/create_action_servers.py b/ada_feeding/scripts/create_action_servers.py index e7a35f3c..bfdc79f4 100755 --- a/ada_feeding/scripts/create_action_servers.py +++ b/ada_feeding/scripts/create_action_servers.py @@ -20,6 +20,7 @@ from ada_watchdog_listener import ADAWatchdogListener from ament_index_python.packages import get_package_share_directory import py_trees +from py_trees.common import Access from rcl_interfaces.msg import ( Parameter as ParameterMsg, ParameterDescriptor, @@ -33,13 +34,14 @@ from rclpy.action.server import ServerGoalHandle from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.executors import MultiThreadedExecutor +from rclpy.exceptions import ParameterNotDeclaredException from rclpy.node import Node from rclpy.parameter import Parameter import yaml # Local imports from ada_feeding import ActionServerBT -from ada_feeding.helpers import import_from_string, register_logger +from ada_feeding.helpers import import_from_string, register_logger, get_tool_joints from ada_feeding.visitors import DebugVisitor @@ -115,7 +117,7 @@ def __init__(self) -> None: super().__init__("create_action_servers", allow_undeclared_parameters=True) register_logger(self.get_logger()) - def initialize(self) -> None: + def initialize(self, blackboard: py_trees.blackboard.Client) -> None: """ Initialize the node. This is a separate function from above so rclpy can be spinning while this function is called. @@ -134,6 +136,26 @@ def initialize(self) -> None: callback_group=MutuallyExclusiveCallbackGroup(), ) + try: + # Get the end_effector_tool parameter + self.declare_parameter("end_effector_tool") # Declaring the parameter first + self.end_effector_tool = self.get_parameter("end_effector_tool").value + self.get_logger().info(f"End effector tool: {self.end_effector_tool}") + except ParameterNotDeclaredException: + self.get_logger().warn( + "Parameter 'end_effector_tool' not declared. Using default value." + ) + self.end_effector_tool = "fork" # Provide a sensible default + + self.tool_joints = get_tool_joints(self.end_effector_tool) + self.get_logger().info(f"Tool joints: {self.tool_joints}") + + # Set the end_effector_tool on the global blackboard using the provided client + blackboard.register_key("/end_effector_tool", Access.WRITE) + blackboard.set( + "/end_effector_tool", self.end_effector_tool + ) # Set the global key + # Read the parameters that specify what action servers to create. self.namespace_to_use = CreateActionServers.DEFAULT_PARAMETER_NAMESPACE self.action_server_params = self.read_params() @@ -175,7 +197,7 @@ def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParam ------- action_server_params: A dict mapping server names to ActionServerParams objects. """ - # pylint: disable=too-many-locals + # pylint: disable=too-many-locals, too-many-branches # Okay because we are providing a lot of generic capabilities through parameters default_namespace = CreateActionServers.DEFAULT_PARAMETER_NAMESPACE @@ -351,11 +373,27 @@ def read_params(self) -> Tuple[Parameter, Parameter, Dict[str, ActionServerParam full_name ] = CreateActionServers.get_parameter_value(custom_value) if self.parameters[self.namespace_to_use][full_name] is not None: - tree_kwargs[kw] = self.parameters[self.namespace_to_use][ - full_name - ] + value = self.parameters[self.namespace_to_use][full_name] else: - tree_kwargs[kw] = self.parameters[default_namespace][full_name] + value = self.parameters[default_namespace][full_name] + + if kw.endswith("joint_positions") or kw.endswith( + "goal_configuration" + ): + if isinstance(value, list): # Check if the value is a list + zeros_to_append = [0.0] * len(self.tool_joints) + value.extend(zeros_to_append) # Append tool joints + self.get_logger().info( + f"Appending tool joints to {full_name}: {self.tool_joints}" + ) + self.get_logger().info( + f"Value of {full_name} after appending: {value}" + ) + else: + self.get_logger().warn( + f"tree_kwarg {full_name} is not a list, cannot append tool joints" + ) + tree_kwargs[kw] = value action_server_params[server_name] = ActionServerParams( server_name=server_name, @@ -1159,6 +1197,8 @@ def main(args: List = None) -> None: create_action_servers = CreateActionServers() + blackboard = py_trees.blackboard.Client() + # Use a MultiThreadedExecutor to enable processing goals concurrently executor = MultiThreadedExecutor(num_threads=multiprocessing.cpu_count() * 2) @@ -1175,7 +1215,7 @@ def main(args: List = None) -> None: # All exceptions need printing at shutdown try: # Initialize the node - create_action_servers.initialize() + create_action_servers.initialize(blackboard) # Spin in the foreground spin_thread.join() diff --git a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py index c16e1e49..4b9f1ad5 100755 --- a/ada_planning_scene/ada_planning_scene/ada_planning_scene.py +++ b/ada_planning_scene/ada_planning_scene/ada_planning_scene.py @@ -15,6 +15,7 @@ from rcl_interfaces.msg import ParameterDescriptor, ParameterType, SetParametersResult from rcl_interfaces.srv import GetParameters import rclpy +from rclpy.exceptions import ParameterNotDeclaredException from rclpy.callback_groups import MutuallyExclusiveCallbackGroup from rclpy.duration import Duration from rclpy.executors import MultiThreadedExecutor @@ -31,6 +32,7 @@ from ada_planning_scene.update_from_face_detection import UpdateFromFaceDetection from ada_planning_scene.update_from_table_detection import UpdateFromTableDetection from ada_planning_scene.workspace_walls import WorkspaceWalls +from ada_feeding.helpers import get_tool_joints class ADAPlanningScene(Node): @@ -53,13 +55,29 @@ def __init__(self): """ super().__init__("ada_planning_scene") + try: + # Get the end_effector_tool parameter + self.declare_parameter("end_effector_tool") # Declaring the parameter first + self.end_effector_tool = self.get_parameter("end_effector_tool").value + self.get_logger().info(f"End effector tool: {self.end_effector_tool}") + except ParameterNotDeclaredException: + self.get_logger().warn( + "Parameter 'end_effector_tool' not declared. Using default value." + ) + self.end_effector_tool = "fork" # Provide a sensible default + + self.tool_joints = get_tool_joints(self.end_effector_tool) + self.get_logger().info(f"Tool joints: {self.tool_joints}") + # Load the parameters. Note that each of the other classes below may # initialize their own parameters, all of which will be in the same # namespace as the parameters loaded here. self.__load_parameters() # Create an object to add collision objects to the planning scene - self.__collision_object_manager = CollisionObjectManager(node=self) + self.__collision_object_manager = CollisionObjectManager( + node=self, tool_joints=self.tool_joints + ) # Create the initializer self.__initializer = PlanningSceneInitializer( @@ -86,6 +104,7 @@ def __init__(self): tf_buffer=self.__tf_buffer, namespaces=self.__namespaces, namespace_to_use=self.__namespace_to_use, + tool_joints=self.tool_joints, ) # Add a callback to update the namespace to use diff --git a/ada_planning_scene/ada_planning_scene/collision_object_manager.py b/ada_planning_scene/ada_planning_scene/collision_object_manager.py index 149b89c0..5cd57418 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -9,7 +9,7 @@ # Standard imports from threading import Lock -from typing import Callable, Dict, Optional, Union +from typing import Callable, Dict, List, Optional, Union # Third-party imports from moveit_msgs.msg import CollisionObject, PlanningScene @@ -36,7 +36,7 @@ class CollisionObjectManager: __GLOBAL_BATCH_ID = "global" __BATCH_ID_FORMAT = "batch_{:d}" - def __init__(self, node: Node): + def __init__(self, node: Node, tool_joints: List): """ Initialize the CollisionObjectManager. @@ -50,12 +50,13 @@ def __init__(self, node: Node): # Using ReentrantCallbackGroup to align with the examples from pymoveit2. # TODO: Assess whether ReentrantCallbackGroup is necessary for MoveIt2. callback_group = ReentrantCallbackGroup() + joint_names = kinova.joint_names() + tool_joints self.moveit2 = MoveIt2( node=self.__node, - joint_names=kinova.joint_names(), + joint_names=joint_names, base_link_name=kinova.base_link_name(), end_effector_name="forkTip", - group_name="jaco_arm", + group_name="ada", callback_group=callback_group, ) diff --git a/ada_planning_scene/ada_planning_scene/workspace_walls.py b/ada_planning_scene/ada_planning_scene/workspace_walls.py index 424e327c..58205e56 100644 --- a/ada_planning_scene/ada_planning_scene/workspace_walls.py +++ b/ada_planning_scene/ada_planning_scene/workspace_walls.py @@ -71,6 +71,7 @@ def __init__( tf_buffer: Buffer, namespaces: List[str], namespace_to_use: str, + tool_joints: List, ): """ Initialize the workspace walls. @@ -96,9 +97,10 @@ def __init__( self.__tf_buffer = tf_buffer self.__namespaces = namespaces self.__namespace_to_use = namespace_to_use + self.__tool_joints = tool_joints # Load parameters - self.__load_parameters() + self.__load_parameters(self.__tool_joints) # Check if the necessary parameters are set to use the robot model self.__use_robot_model = True @@ -153,7 +155,7 @@ def __init__( self.__active_goal_request_lock = Lock() self.__active_goal_request = None - def __load_parameters(self): + def __load_parameters(self, tool_joints: List): """ Load parameters relevant to the workspace walls. """ @@ -361,7 +363,8 @@ def __load_parameters(self): "j2n6s200_joint_4", "j2n6s200_joint_5", "j2n6s200_joint_6", - ], # default value + ] + + tool_joints, # default value ParameterDescriptor( name="articulated_joint_names", type=ParameterType.PARAMETER_STRING_ARRAY, diff --git a/ada_planning_scene/config/ada_planning_scene.yaml b/ada_planning_scene/config/ada_planning_scene.yaml index 40d5696e..b63702d4 100644 --- a/ada_planning_scene/config/ada_planning_scene.yaml +++ b/ada_planning_scene/config/ada_planning_scene.yaml @@ -93,7 +93,7 @@ ada_planning_scene: in_front_of_face_wall: # a wall in front of the user's face so robot motions don't unnecessarily move towards them. primitive_type: 1 # shape_msgs/SolidPrimitive.BOX primitive_dims: [0.65, 0.01, 0.4] - position: [0.37, 0.17, 0.85] + position: [0.37, 0.22, 0.85] quat_xyzw: [0.0, 0.0, 0.0, 1.0] frame_id: root # the frame_id that the pose is relative to add_on_initialize: False # don't add this to the planning scene initially diff --git a/ada_planning_scene/launch/ada_moveit_launch.xml b/ada_planning_scene/launch/ada_moveit_launch.xml index d436250c..6842d15a 100644 --- a/ada_planning_scene/launch/ada_moveit_launch.xml +++ b/ada_planning_scene/launch/ada_moveit_launch.xml @@ -10,6 +10,7 @@ + @@ -18,6 +19,7 @@ + @@ -26,6 +28,7 @@ + diff --git a/start.py b/start.py index 8230f448..5205a458 100755 --- a/start.py +++ b/start.py @@ -71,6 +71,12 @@ "These options are all named learning policies for ada_feeding_action_select." ), ) +parser.add_argument( + "--end_effector_tool", + default="fork", + help=("Which end-effector tool to use"), + choices=["fork", "articulable_fork"], +) async def get_existing_screens(): @@ -204,11 +210,13 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "feeding": [ ( "ros2 launch ada_feeding ada_feeding_launch.xml use_estop:=false " - f"policy:={args.policy}" + f"policy:={args.policy} " + f"end_effector_tool:={args.end_effector_tool}" ), ], "moveit": [ - "ros2 launch ada_planning_scene ada_moveit_launch.xml sim:=mock" + "ros2 launch ada_planning_scene ada_moveit_launch.xml sim:=mock " + f"end_effector_tool:={args.end_effector_tool}" ], "browser": [ "cd ./src/feeding_web_interface/feedingwebapp", @@ -285,13 +293,16 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "moveit": [ "Xvfb :5 -screen 0 800x600x24 &" if not args.dev else "", "export DISPLAY=:5" if not args.dev else "", - f"ros2 launch ada_planning_scene ada_moveit_launch.xml use_rviz:={'true' if args.dev else 'false'}", + "ros2 launch ada_planning_scene ada_moveit_launch.xml " + f"use_rviz:={'true' if args.dev else 'false'} " + f"end_effector_tool:={args.end_effector_tool}", ], "feeding": [ "sudo ./src/ada_feeding/configure_lovelace.sh", ( "ros2 launch ada_feeding ada_feeding_launch.xml " - f"use_estop:={'false' if args.dev else 'true'} run_web_bridge:=false policy:={args.policy}" + f"use_estop:={'false' if args.dev else 'true'} run_web_bridge:=false policy:={args.policy} " + f"end_effector_tool:={args.end_effector_tool}" ), ], "browser": [