From e4a9f9f3ebbeab2f702483cf8b99d1afc8a32e1f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 2 Apr 2025 07:47:09 -0700 Subject: [PATCH 001/238] Reposition in_front_of_face_wall to account for extra length of Articutool --- ada_planning_scene/config/ada_planning_scene.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 From ac74b5c861c9f167c2f17e25b25e7a45794839de Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 2 Apr 2025 07:56:16 -0700 Subject: [PATCH 002/238] Use tool_tip frame instead of forkTip, and use jaco_arm_with_articutool move group --- .../ada_feeding/behaviors/transfer/compute_mouth_frame.py | 2 +- ada_feeding/ada_feeding/helpers.py | 4 ++-- ada_feeding/ada_feeding/idioms/servo_until.py | 2 +- .../ada_planning_scene/collision_object_manager.py | 4 ++-- 4 files changed, 6 insertions(+), 6 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py b/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py index ea6b7bad..8c503ee2 100644 --- a/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py +++ b/ada_feeding/ada_feeding/behaviors/transfer/compute_mouth_frame.py @@ -49,7 +49,7 @@ def blackboard_inputs( world_frame: Union[BlackboardKey, str] = "root", # +z will match this frame frame_to_orient_towards: Union[ BlackboardKey, str - ] = "forkTip", # +x will point towards this frame + ] = "tool_tip", # +x will point towards this frame ) -> None: """ Blackboard Inputs diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index 38573ba3..e3f33d8f 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -361,8 +361,8 @@ def get_moveit2_object( node=node, joint_names=kinova.joint_names(), base_link_name=kinova.base_link_name(), - end_effector_name="forkTip", - group_name="jaco_arm", + end_effector_name="tool_tip", + group_name="jaco_arm_with_articutool", callback_group=callback_group, ) lock = Lock() diff --git a/ada_feeding/ada_feeding/idioms/servo_until.py b/ada_feeding/ada_feeding/idioms/servo_until.py index 04d0d6ec..3e373695 100644 --- a/ada_feeding/ada_feeding/idioms/servo_until.py +++ b/ada_feeding/ada_feeding/idioms/servo_until.py @@ -174,7 +174,7 @@ def servo_until_pose( name: str, ns: str, target_pose_stamped_key: BlackboardKey, - end_effector_frame: str = "forkTip", + end_effector_frame: str = "tool_tip", tolerance_position: float = 0.005, tolerance_orientation: Union[float, Tuple[float, float, float]] = 0.09, relaxed_tolerance_position: float = 0.005, 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..67ac5ef6 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -54,8 +54,8 @@ def __init__(self, node: Node): node=self.__node, joint_names=kinova.joint_names(), base_link_name=kinova.base_link_name(), - end_effector_name="forkTip", - group_name="jaco_arm", + end_effector_name="tool_tip", + group_name="jaco_arm_with_articutool", callback_group=callback_group, ) From 161c71eaab0bd8bde1c0a7452fd08c37571322ac Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 7 Apr 2025 21:04:36 -0700 Subject: [PATCH 003/238] Update planner_benchmark.py to use jaco_arm_with_articutool planning group --- ada_feeding/scripts/planner_benchmark.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_feeding/scripts/planner_benchmark.py b/ada_feeding/scripts/planner_benchmark.py index 481846c3..eb41741a 100755 --- a/ada_feeding/scripts/planner_benchmark.py +++ b/ada_feeding/scripts/planner_benchmark.py @@ -332,8 +332,8 @@ def main(out_dir: Optional[str]): node=node, joint_names=kinova.joint_names(), base_link_name=kinova.base_link_name(), - end_effector_name="forkTip", - group_name="jaco_arm", + end_effector_name="tool_tip", + group_name="jaco_arm_with_articutool", callback_group=callback_group, ) From 0d360b2f758cd295e3fc835ebed37cba08ac2074 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 8 Apr 2025 15:57:02 -0700 Subject: [PATCH 004/238] Adjust above plate configuration to account for extra Articutool length --- .../config/ada_feeding_action_servers_custom.yaml | 12 ++++++------ .../config/ada_feeding_action_servers_default.yaml | 12 ++++++------ 2 files changed, 12 insertions(+), 12 deletions(-) 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 From 6a3379d149142bad12813d8434c97d551566d9ee Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 10 Apr 2025 20:58:15 -0700 Subject: [PATCH 005/238] Add GetJointStates behavior to read from the global /joint_states topic. We will use this to read the Articutool joint states and pass them as a constraint during motion planning. This is a WIP change because we still need to manage the node that subscribes to /joint_states. Simply creating a node without interacting with the executor in create_action_servers.py will not suffice --- .../ada_feeding/behaviors/state/__init__.py | 9 ++++ .../behaviors/state/get_joint_states.py | 54 +++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 24 +++++++++ 3 files changed, 87 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/state/__init__.py create mode 100644 ada_feeding/ada_feeding/behaviors/state/get_joint_states.py diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py new file mode 100644 index 00000000..fdcaf748 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -0,0 +1,9 @@ +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This package contains custom py_tree behaviors for interacting with ROS. +""" +from .get_joint_states import ( + GetJointStates, +) diff --git a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py new file mode 100644 index 00000000..c5d79865 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py @@ -0,0 +1,54 @@ +import rclpy +from rclpy.node import Node +from sensor_msgs.msg import JointState +import py_trees + +from ada_feeding.behaviors import BlackboardBehavior +from ada_feeding.helpers import BlackboardKey + +class GetJointStates(BlackboardBehavior): + def __init__(self, name, joint_names, output_key, ns="/"): + super().__init__(name, ns=ns, inputs={"joint_names": joint_names}, outputs={"joint_states": output_key}) + self.ns = ns + self.node = None + self.joint_states_sub = None + self.latest_joint_states = {} + self.received_all_joints = False + + def setup(self, **kwargs): + # Initialize ROS 2 node + if self.node is None: + self.node = Node(self.name + "_node", namespace=self.ns) + # Subscribe to JointState topic + self.joint_states_sub = self.node.create_subscription( + JointState, "/joint_states", self.joint_states_callback, 10 + ) + + def joint_states_callback(self, msg: JointState): + self.node.get_logger().info("Joint states callback received") + joint_names = self.blackboard_get("joint_names") + for i, name in enumerate(msg.name): + if name in joint_names: + self.node.get_logger().info(f"Storing joint state: {name} = {msg.position[i]}") + self.latest_joint_states[name] = msg.position[i] + self.node.get_logger().info(f"Joint names in message: {msg.name}") + + # Check if we have received all the required joints + if all(name in self.latest_joint_states for name in self.blackboard_get("joint_names")): + self.received_all_joints = True + self.node.get_logger().info("Received all joints!") + + def update(self): + self.node.get_logger().info("GetJointStates.update() called") + self.node.get_logger().info(f"Current joint states: {self.latest_joint_states}") + # Check if we have the joint states we need + if self.received_all_joints: + self.blackboard_set("joint_states", self.latest_joint_states) + return py_trees.common.Status.SUCCESS + else: + return py_trees.common.Status.RUNNING + + def terminate(self, new_status): + self.joint_states_sub = None + self.latest_joint_states.clear() + self.received_all_joints = False diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 1bea2c2e..f62db3cc 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -44,6 +44,7 @@ ServoMove, ToggleCollisionObject, ) +from ada_feeding.behaviors.state import GetJointStates from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( pre_moveto_config, @@ -377,6 +378,29 @@ def move_above_plan( name="MoveAbovePlanningSeq", memory=True, children=[ + # Get Articutool Joint States with Timeout + py_trees.decorators.Timeout( + name="GetArticutoolJointsTimeout", + duration=1.0, + child=GetJointStates( + name="GetArticutoolJoints", + joint_names=["atool_joint1", "atool_joint2"], + output_key="atool_joints", + ns=name, + ), + ), + # Create Joint Constraint + MoveIt2JointConstraint( + name="AtoolJointConstraint", + ns=name, + inputs={ + "joint_positions": BlackboardKey("atool_joints"), + "tolerance": 0.01, + }, + outputs={ + "constraints": BlackboardKey("atool_constraints"), + }, + ), # Compute Food Frame py_trees.decorators.Timeout( name="ComputeFoodFrameTimeout", From eb9fff8c05e0069bfdc215c57ebd6c686629d078 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 11 Apr 2025 09:27:42 -0700 Subject: [PATCH 006/238] Remove unused imports --- ada_feeding/ada_feeding/behaviors/state/get_joint_states.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py index c5d79865..b8dd864a 100644 --- a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py +++ b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py @@ -1,10 +1,8 @@ -import rclpy from rclpy.node import Node from sensor_msgs.msg import JointState import py_trees from ada_feeding.behaviors import BlackboardBehavior -from ada_feeding.helpers import BlackboardKey class GetJointStates(BlackboardBehavior): def __init__(self, name, joint_names, output_key, ns="/"): From d2965a97dc0d7001772af2789af36e5406f476e0 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 11 Apr 2025 09:48:03 -0700 Subject: [PATCH 007/238] Use main node to read joint states, and update logic of GetJointStates behavior to correctly terminate when all desired joint states have been found --- .../behaviors/state/get_joint_states.py | 31 ++++++++----------- .../ada_feeding/trees/acquire_food_tree.py | 1 + 2 files changed, 14 insertions(+), 18 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py index b8dd864a..7735f370 100644 --- a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py +++ b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py @@ -5,10 +5,10 @@ from ada_feeding.behaviors import BlackboardBehavior class GetJointStates(BlackboardBehavior): - def __init__(self, name, joint_names, output_key, ns="/"): + def __init__(self, name, joint_names, output_key, node=None, ns="/"): super().__init__(name, ns=ns, inputs={"joint_names": joint_names}, outputs={"joint_states": output_key}) self.ns = ns - self.node = None + self.node = node self.joint_states_sub = None self.latest_joint_states = {} self.received_all_joints = False @@ -17,36 +17,31 @@ def setup(self, **kwargs): # Initialize ROS 2 node if self.node is None: self.node = Node(self.name + "_node", namespace=self.ns) - # Subscribe to JointState topic - self.joint_states_sub = self.node.create_subscription( - JointState, "/joint_states", self.joint_states_callback, 10 - ) + # Subscribe to JointState topic + self.joint_states_sub = self.node.create_subscription( + JointState, "/joint_states", self.joint_states_callback, 10 + ) def joint_states_callback(self, msg: JointState): - self.node.get_logger().info("Joint states callback received") + # self.node.get_logger().info("Joint states callback received") joint_names = self.blackboard_get("joint_names") for i, name in enumerate(msg.name): if name in joint_names: - self.node.get_logger().info(f"Storing joint state: {name} = {msg.position[i]}") + # self.node.get_logger().info(f"Storing joint state: {name} = {msg.position[i]}") self.latest_joint_states[name] = msg.position[i] - self.node.get_logger().info(f"Joint names in message: {msg.name}") - - # Check if we have received all the required joints - if all(name in self.latest_joint_states for name in self.blackboard_get("joint_names")): - self.received_all_joints = True - self.node.get_logger().info("Received all joints!") def update(self): - self.node.get_logger().info("GetJointStates.update() called") - self.node.get_logger().info(f"Current joint states: {self.latest_joint_states}") # Check if we have the joint states we need - if self.received_all_joints: + if all(name in self.latest_joint_states for name in self.blackboard_get("joint_names")): + self.node.get_logger().info(f"Received all joint states") self.blackboard_set("joint_states", self.latest_joint_states) return py_trees.common.Status.SUCCESS else: return py_trees.common.Status.RUNNING def terminate(self, new_status): + if self.node is not None and self.node != self.node: + self.node.destroy_node() + self.node = None self.joint_states_sub = None self.latest_joint_states.clear() - self.received_all_joints = False diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index f62db3cc..c56cad5d 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -387,6 +387,7 @@ def move_above_plan( joint_names=["atool_joint1", "atool_joint2"], output_key="atool_joints", ns=name, + node=self._node, ), ), # Create Joint Constraint From 049a394f4414d9a9bc092a76a7465846399dd005 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 11 Apr 2025 09:54:27 -0700 Subject: [PATCH 008/238] Use proper BlackboardKey for output of joint states behavior --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index c56cad5d..3a0b492e 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -385,7 +385,7 @@ def move_above_plan( child=GetJointStates( name="GetArticutoolJoints", joint_names=["atool_joint1", "atool_joint2"], - output_key="atool_joints", + output_key=BlackboardKey("atool_joints"), ns=name, node=self._node, ), From c968f8227bfc7bab82d71e7b462578200cd0c445 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 11 Apr 2025 10:09:04 -0700 Subject: [PATCH 009/238] Write to correct goal_constraints BlackboardKey --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 3a0b492e..e79b72c7 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -399,7 +399,7 @@ def move_above_plan( "tolerance": 0.01, }, outputs={ - "constraints": BlackboardKey("atool_constraints"), + "constraints": BlackboardKey("goal_constraints"), }, ), # Compute Food Frame From aefc7b2b9e5ba49140823856a1b68c8851480493 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 12 Apr 2025 17:13:34 -0700 Subject: [PATCH 010/238] Return GetJointStates behavior result as two lists for joint_names and joint_positions, also adhere to BlackboardBehavior style for passing input and output behavior keys --- .../behaviors/state/get_joint_states.py | 51 +++++++++++++++++-- 1 file changed, 46 insertions(+), 5 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py index 7735f370..6ba469bf 100644 --- a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py +++ b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py @@ -2,17 +2,55 @@ from sensor_msgs.msg import JointState import py_trees +from typing import Any, Dict, List, Optional, Union from ada_feeding.behaviors import BlackboardBehavior +from ada_feeding.helpers import BlackboardKey class GetJointStates(BlackboardBehavior): - def __init__(self, name, joint_names, output_key, node=None, ns="/"): - super().__init__(name, ns=ns, inputs={"joint_names": joint_names}, outputs={"joint_states": output_key}) + def __init__(self, name: str, node=None, ns:str="/", inputs: Optional[Dict[str, Union[BlackboardKey, Any]]] = None, outputs: Optional[Dict[str, Optional[BlackboardKey]]] = None, +): + super().__init__(name, ns=ns, inputs=inputs, outputs=outputs) self.ns = ns self.node = node self.joint_states_sub = None self.latest_joint_states = {} self.received_all_joints = False + def blackboard_inputs( + self, + joint_names: Union[BlackboardKey, Optional[List[str]]] = None, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + constraints: previous set of constraints to append to + """ + # 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, + joint_positions: Union[BlackboardKey, List[float]] = None, + joint_names: Union[BlackboardKey, Optional[List[str]]] = None, + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + constraints: list of constraints to send to MoveIt2Plan + """ + # 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"} + ) + def setup(self, **kwargs): # Initialize ROS 2 node if self.node is None: @@ -23,18 +61,21 @@ def setup(self, **kwargs): ) def joint_states_callback(self, msg: JointState): - # self.node.get_logger().info("Joint states callback received") joint_names = self.blackboard_get("joint_names") for i, name in enumerate(msg.name): if name in joint_names: - # self.node.get_logger().info(f"Storing joint state: {name} = {msg.position[i]}") self.latest_joint_states[name] = msg.position[i] def update(self): # Check if we have the joint states we need if all(name in self.latest_joint_states for name in self.blackboard_get("joint_names")): + joint_names = list(self.latest_joint_states.keys()) + joint_positions = list(self.latest_joint_states.values()) self.node.get_logger().info(f"Received all joint states") - self.blackboard_set("joint_states", self.latest_joint_states) + self.node.get_logger().info(f"Joint Names: {joint_names}") + self.node.get_logger().info(f"Joint Positions: {joint_positions}") + self.blackboard_set("joint_names", joint_names) + self.blackboard_set("joint_positions", joint_positions) return py_trees.common.Status.SUCCESS else: return py_trees.common.Status.RUNNING From 7b40e8f6e647061650ce868810015af8a0788db8 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 13 Apr 2025 09:13:38 -0700 Subject: [PATCH 011/238] Add Articutool joints to MoveIt2 object. We can and should do this more elegantly in the future by adding these joints only conditionally when the user has specified that they are using the Articutool --- ada_feeding/ada_feeding/helpers.py | 2 +- ada_feeding/scripts/planner_benchmark.py | 2 +- .../ada_planning_scene/collision_object_manager.py | 2 +- 3 files changed, 3 insertions(+), 3 deletions(-) diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index e3f33d8f..e2500f0a 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -359,7 +359,7 @@ def get_moveit2_object( callback_group = ReentrantCallbackGroup() moveit2 = MoveIt2( node=node, - joint_names=kinova.joint_names(), + joint_names=kinova.joint_names() + ["atool_joint1", "atool_joint2"], base_link_name=kinova.base_link_name(), end_effector_name="tool_tip", group_name="jaco_arm_with_articutool", diff --git a/ada_feeding/scripts/planner_benchmark.py b/ada_feeding/scripts/planner_benchmark.py index eb41741a..8cb269c3 100755 --- a/ada_feeding/scripts/planner_benchmark.py +++ b/ada_feeding/scripts/planner_benchmark.py @@ -330,7 +330,7 @@ def main(out_dir: Optional[str]): callback_group = ReentrantCallbackGroup() moveit2 = MoveIt2( node=node, - joint_names=kinova.joint_names(), + joint_names=kinova.joint_names() + ["atool_joint1", "atool_joint2"], base_link_name=kinova.base_link_name(), end_effector_name="tool_tip", group_name="jaco_arm_with_articutool", 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 67ac5ef6..739406c7 100644 --- a/ada_planning_scene/ada_planning_scene/collision_object_manager.py +++ b/ada_planning_scene/ada_planning_scene/collision_object_manager.py @@ -52,7 +52,7 @@ def __init__(self, node: Node): callback_group = ReentrantCallbackGroup() self.moveit2 = MoveIt2( node=self.__node, - joint_names=kinova.joint_names(), + joint_names=kinova.joint_names() + ["atool_joint1", "atool_joint2"], base_link_name=kinova.base_link_name(), end_effector_name="tool_tip", group_name="jaco_arm_with_articutool", From 01bc598eef1889aa243b75cf00e0743b4aaaf76a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 13 Apr 2025 09:15:15 -0700 Subject: [PATCH 012/238] Update error logging for missing arguments in each constraint type so that the error message actually tells you which argument was missing --- .../behaviors/moveit2/moveit2_constraints.py | 72 ++++++++++--------- 1 file changed, 39 insertions(+), 33 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py index 83b12871..9c5f6814 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py @@ -101,11 +101,21 @@ def blackboard_outputs( def update(self) -> py_trees.common.Status: # Docstring copied from @override - 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 + for argument in [ + "joint_positions", + "joint_names", + "tolerance", + "weight", + "constraints" + ]: + if not self.blackboard_exists(argument): + self.logger.error(f"MoveIt2JointConstraint: Missing input argument: {argument}") + return py_trees.common.Status.FAILURE + + # # Check if joint positions are valid (not None or empty) + # if self.blackboard_get("joint_positions") is None or not self.blackboard_get("joint_positions"): + # self.logger.debug("MoveIt2Constraint: Joint positions not yet available.") + # return py_trees.common.Status.RUNNING constraint = ( MoveIt2ConstraintType.JOINT, @@ -223,21 +233,18 @@ def setup(self, **kwargs): def update(self) -> py_trees.common.Status: # Docstring copied from @override - if not self.blackboard_exists( - [ + for argument in [ "offset", "frame_id", "target_link", "tolerance", "weight", "constraints", - ] - ): - self.logger.error( - "MoveIt2PositionOffsetConstraint: Missing input arguments." - ) - self.logger.error(str(self.blackboard)) - return py_trees.common.Status.FAILURE + ]: + if not self.blackboard_exists(argument): + self.logger.error(f"MoveIt2PositionOffsetConstraint: Missing input argument: {argument}") + self.logger.error(str(self.blackboard)) + return py_trees.common.Status.FAILURE if self.tf_lock.locked(): return py_trees.common.Status.RUNNING @@ -380,19 +387,18 @@ def blackboard_outputs( def update(self) -> py_trees.common.Status: # Docstring copied from @override - if not self.blackboard_exists( - [ + for argument in [ "position", "frame_id", "target_link", "tolerance", "weight", "constraints", - ] - ): - self.logger.error("MoveIt2PositionConstraint: Missing input arguments.") - self.logger.error(str(self.blackboard)) - return py_trees.common.Status.FAILURE + ]: + if not self.blackboard_exists(argument): + self.logger.error(f"MoveIt2PositionConstraint: Missing input argument: {argument}") + self.logger.error(str(self.blackboard)) + return py_trees.common.Status.FAILURE position = self.blackboard_get("position") constraint = ( @@ -505,8 +511,7 @@ def blackboard_outputs( def update(self) -> py_trees.common.Status: # Docstring copied from @override - if not self.blackboard_exists( - [ + for argument in [ "quat_xyzw", "frame_id", "target_link", @@ -514,10 +519,11 @@ def update(self) -> py_trees.common.Status: "weight", "constraints", "parameterization", - ] - ): - self.logger.error("MoveIt2OrientationConstraint: Missing input arguments.") - return py_trees.common.Status.FAILURE + ]: + if not self.blackboard_exists(argument): + self.logger.error(f"MoveIt2OrientationConstraint: Missing input argument: {argument}") + self.logger.error(str(self.blackboard)) + return py_trees.common.Status.FAILURE quat_xyzw = self.blackboard_get("quat_xyzw") constraint = ( @@ -631,8 +637,7 @@ def blackboard_outputs( def update(self) -> py_trees.common.Status: # Docstring copied from @override - if not self.blackboard_exists( - [ + for argument in [ "pose", "frame_id", "target_link", @@ -642,10 +647,11 @@ def update(self) -> py_trees.common.Status: "weight_orientation", "constraints", "parameterization", - ] - ): - self.logger.error("MoveIt2Constraint: Missing input arguments.") - return py_trees.common.Status.FAILURE + ]: + if not self.blackboard_exists(argument): + self.logger.error(f"MoveIt2PoseConstraint: Missing input argument: {argument}") + self.logger.error(str(self.blackboard)) + return py_trees.common.Status.FAILURE pose = self.blackboard_get("pose") constraint_orient = ( From 65f314a5c284b4522cc501638eb427c4c0531822 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 13 Apr 2025 09:16:42 -0700 Subject: [PATCH 013/238] Revert adding Articutool joint constraints in the acquire food behavior tree for now. I left a commented out block for reference once I've figured out a simpler joint constraint when moving to a pre-defined configuration --- .../ada_feeding/trees/acquire_food_tree.py | 57 +++++++++++-------- 1 file changed, 33 insertions(+), 24 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index e79b72c7..69691ae9 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -378,30 +378,6 @@ def move_above_plan( name="MoveAbovePlanningSeq", memory=True, children=[ - # Get Articutool Joint States with Timeout - py_trees.decorators.Timeout( - name="GetArticutoolJointsTimeout", - duration=1.0, - child=GetJointStates( - name="GetArticutoolJoints", - joint_names=["atool_joint1", "atool_joint2"], - output_key=BlackboardKey("atool_joints"), - ns=name, - node=self._node, - ), - ), - # Create Joint Constraint - MoveIt2JointConstraint( - name="AtoolJointConstraint", - ns=name, - inputs={ - "joint_positions": BlackboardKey("atool_joints"), - "tolerance": 0.01, - }, - outputs={ - "constraints": BlackboardKey("goal_constraints"), - }, - ), # Compute Food Frame py_trees.decorators.Timeout( name="ComputeFoodFrameTimeout", @@ -713,6 +689,39 @@ def move_above_plan( on_preempt_timeout=5.0, # Starts a new Sequence w/ Memory internally workers=[ + # py_trees.composites.Sequence( + # name="GetAndConstrainArticutoolJoints", + # memory=True, + # children=[ + # py_trees.decorators.Timeout( + # name="GetArticutoolJointsTimeout", + # duration=1.0, + # child=GetJointStates( + # name="GetArticutoolJoints", + # ns=name, + # node=self._node, + # inputs={ + # "joint_names": ["atool_joint1", "atool_joint2"], + # }, + # outputs={ + # "joint_names": BlackboardKey("atool_joint_names"), + # "joint_positions": BlackboardKey("atool_joint_positions"), + # } + # ), + # ), + # MoveIt2JointConstraint( + # name="ArticutoolJointConstraint", + # ns=name, + # inputs={ + # "joint_names": BlackboardKey("atool_joint_names"), + # "joint_positions": BlackboardKey("atool_joint_positions"), + # }, + # outputs={ + # "constraints": BlackboardKey("goal_constraints"), + # }, + # ), + # ], + # ), ### Move Into Food MoveIt2PoseConstraint( name="MoveIntoPose", From eaad944c3e9b5262023a57273097707396112ad6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 13 Apr 2025 09:18:04 -0700 Subject: [PATCH 014/238] Add Articutool joint constraints when planning for a MoveToConfiguration. This should be applied to all other motion plans once it behaves as desired, i.e. the contraint is correctly applied to the plan --- ...o_configuration_with_ft_thresholds_tree.py | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) 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 ab98c495..9be70d36 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 @@ -22,6 +22,7 @@ MoveIt2Execute, MoveIt2JointConstraint, ) +from ada_feeding.behaviors.state import GetJointStates from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import pre_moveto_config, scoped_behavior from ada_feeding.idioms.bite_transfer import get_toggle_watchdog_listener_behavior @@ -162,6 +163,39 @@ def create_tree( name=name, memory=True, children=[ + py_trees.composites.Sequence( + name="GetAndConstrainArticutoolJoints", + memory=True, + children=[ + py_trees.decorators.Timeout( + name="GetArticutoolJointsTimeout", + duration=1.0, + child=GetJointStates( + name="GetArticutoolJoints", + ns=name, + node=self._node, + inputs={ + "joint_names": ["atool_joint1", "atool_joint2"], + }, + outputs={ + "joint_names": BlackboardKey("joint_names"), + "joint_positions": BlackboardKey("joint_positions"), + } + ), + ), + MoveIt2JointConstraint( + name="ArticutoolJointConstraint", + ns=name, + inputs={ + "joint_names": BlackboardKey("joint_names"), + "joint_positions": BlackboardKey("joint_positions"), + }, + outputs={ + "constraints": BlackboardKey("goal_constraints"), + }, + ), + ], + ), MoveIt2JointConstraint( name="JointConstraint", ns=name, @@ -169,6 +203,7 @@ def create_tree( "joint_positions": BlackboardKey("joint_positions"), "tolerance": self.tolerance_joint, "weight": self.weight_joint, + "constraints": BlackboardKey("goal_constraints"), }, outputs={ "constraints": BlackboardKey("goal_constraints"), From 365d1ea23e27b1cf1550df2e4b1bcfe6d2394343 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 13 Apr 2025 14:19:26 -0700 Subject: [PATCH 015/238] Separate blackboard keys between constraints to avoid conflicts --- .../move_to_configuration_with_ft_thresholds_tree.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 9be70d36..69319796 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 @@ -178,8 +178,8 @@ def create_tree( "joint_names": ["atool_joint1", "atool_joint2"], }, outputs={ - "joint_names": BlackboardKey("joint_names"), - "joint_positions": BlackboardKey("joint_positions"), + "joint_names": BlackboardKey("atool_joint_names"), + "joint_positions": BlackboardKey("atool_joint_positions"), } ), ), @@ -187,8 +187,8 @@ def create_tree( name="ArticutoolJointConstraint", ns=name, inputs={ - "joint_names": BlackboardKey("joint_names"), - "joint_positions": BlackboardKey("joint_positions"), + "joint_names": BlackboardKey("atool_joint_names"), + "joint_positions": BlackboardKey("atool_joint_positions"), }, outputs={ "constraints": BlackboardKey("goal_constraints"), From c50eef769e69eadfd77c026c6e47bd1339659792 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 14 Apr 2025 18:38:10 -0700 Subject: [PATCH 016/238] Add Articutool joint constraints to motions that go to pre-defined configurations. For those that desire a pose determined at runtime, e.g. move above food, we'll need to be more clever since simply constraining the Articutool joints to not move during the planned trajectory causes the planner to fail to find valid solutions --- .../ada_feeding/trees/acquire_food_tree.py | 67 ++++++++++--------- ...configuration_with_wheelchair_wall_tree.py | 36 ++++++++++ 2 files changed, 70 insertions(+), 33 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 69691ae9..c93e59c9 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -192,11 +192,45 @@ def create_tree( # Default fail if service is down wait_for_server_timeout_sec=0.0, ), + py_trees.composites.Sequence( + name="GetAndConstrainArticutoolJoints", + memory=True, + children=[ + py_trees.decorators.Timeout( + name="GetArticutoolJointsTimeout", + duration=1.0, + child=GetJointStates( + name="GetArticutoolJoints", + ns=name, + node=self._node, + inputs={ + "joint_names": ["atool_joint1", "atool_joint2"], + }, + outputs={ + "joint_names": BlackboardKey("atool_joint_names"), + "joint_positions": BlackboardKey("atool_joint_positions"), + } + ), + ), + MoveIt2JointConstraint( + name="ArticutoolJointConstraint", + ns=name, + inputs={ + "joint_names": BlackboardKey("atool_joint_names"), + "joint_positions": BlackboardKey("atool_joint_positions"), + }, + outputs={ + "constraints": BlackboardKey("goal_constraints"), + }, + ), + ], + ), MoveIt2JointConstraint( name="RestingConstraint", ns=name, inputs={ "joint_positions": self.resting_joint_positions, + "constraints": BlackboardKey("goal_constraints"), }, outputs={ "constraints": BlackboardKey("goal_constraints"), @@ -689,39 +723,6 @@ def move_above_plan( on_preempt_timeout=5.0, # Starts a new Sequence w/ Memory internally workers=[ - # py_trees.composites.Sequence( - # name="GetAndConstrainArticutoolJoints", - # memory=True, - # children=[ - # py_trees.decorators.Timeout( - # name="GetArticutoolJointsTimeout", - # duration=1.0, - # child=GetJointStates( - # name="GetArticutoolJoints", - # ns=name, - # node=self._node, - # inputs={ - # "joint_names": ["atool_joint1", "atool_joint2"], - # }, - # outputs={ - # "joint_names": BlackboardKey("atool_joint_names"), - # "joint_positions": BlackboardKey("atool_joint_positions"), - # } - # ), - # ), - # MoveIt2JointConstraint( - # name="ArticutoolJointConstraint", - # ns=name, - # inputs={ - # "joint_names": BlackboardKey("atool_joint_names"), - # "joint_positions": BlackboardKey("atool_joint_positions"), - # }, - # outputs={ - # "constraints": BlackboardKey("goal_constraints"), - # }, - # ), - # ], - # ), ### Move Into Food MoveIt2PoseConstraint( name="MoveIntoPose", 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 35d51970..74938485 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 @@ -27,6 +27,7 @@ MoveIt2JointConstraint, MoveIt2OrientationConstraint, ) +from ada_feeding.behaviors.state import GetJointStates from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import pre_moveto_config, scoped_behavior from ada_feeding.idioms.bite_transfer import ( @@ -115,11 +116,45 @@ def create_tree( ### Define Tree Logic constraints = [ + py_trees.composites.Sequence( + name="GetAndConstrainArticutoolJoints", + memory=True, + children=[ + py_trees.decorators.Timeout( + name="GetArticutoolJointsTimeout", + duration=1.0, + child=GetJointStates( + name="GetArticutoolJoints", + ns=name, + node=self._node, + inputs={ + "joint_names": ["atool_joint1", "atool_joint2"], + }, + outputs={ + "joint_names": BlackboardKey("atool_joint_names"), + "joint_positions": BlackboardKey("atool_joint_positions"), + } + ), + ), + MoveIt2JointConstraint( + name="ArticutoolJointConstraint", + ns=name, + inputs={ + "joint_names": BlackboardKey("atool_joint_names"), + "joint_positions": BlackboardKey("atool_joint_positions"), + }, + outputs={ + "constraints": BlackboardKey("goal_constraints"), + }, + ), + ], + ), # Goal configuration: staging configuration MoveIt2JointConstraint( name="StagingConfigurationGoalConstraint", ns=name, inputs={ + "constraints": BlackboardKey("goal_constraints"), "joint_positions": self.goal_configuration, "tolerance": self.goal_configuration_tolerance, }, @@ -135,6 +170,7 @@ def create_tree( name="KeepForkStraightPathConstraint", ns=name, inputs={ + "constraints": BlackboardKey("goal_constraints"), "quat_xyzw": self.orientation_constraint_quaternion, "tolerance": self.orientation_constraint_tolerances, "parameterization": 1, # Rotation vector From f76ef7763e2cc6af5d284cbfef3155f48abc08e6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 17 Apr 2025 18:52:53 -0700 Subject: [PATCH 017/238] Modify get_moveit2_object to allow for planning with any group. A unique blackboard key will be used depending on the planning group so there is no chance to overwrite existing keys --- ada_feeding/ada_feeding/helpers.py | 32 ++++++++++++++++++++++++------ 1 file changed, 26 insertions(+), 6 deletions(-) diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index e2500f0a..f7caf4e2 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -302,6 +302,7 @@ def get_tf_object( def get_moveit2_object( blackboard: py_trees.blackboard.Client, node: Optional[Node] = None, + group_name: str = "jaco_arm_with_articutool", ) -> Tuple[MoveIt2, Lock]: """ Gets the MoveIt2 object and its corresponding lock from the blackboard. @@ -315,6 +316,7 @@ def get_moveit2_object( 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. + group_name: The name of the MoveIt2 planning group Returns ------- @@ -329,8 +331,8 @@ def get_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" + moveit2_blackboard_key = f"/moveit2/{group_name}" + moveit2_lock_blackboard_key = f"/moveit2_lock/{group_name}" # First, register the MoveIt2 object and its corresponding lock for READ access if not blackboard.is_registered(moveit2_blackboard_key, Access.READ): @@ -353,22 +355,40 @@ def get_moveit2_object( node.get_logger().info( "MoveIt2 object and lock do not exist on the blackboard. Creating them now." ) + + if group_name == "jaco_arm": + _joint_names = kinova.joint_names() + _base_link = kinova.base_link_name() + _end_effector = "j2n6s200_end_effector" + elif group_name == "articutool": + _joint_names = ["atool_joint1", "atool_joint2"] + _base_link = "atool_base" + _end_effector = "tool_tip" + elif group_name == "jaco_arm_with_articutool": + _joint_names = kinova.joint_names() + ["atool_joint1", "atool_joint2"] + _base_link = kinova.base_link_name() + _end_effector = "tool_tip" + else: + raise ValueError(f"Unknown planning group name provided: {group_name}") + 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() + ["atool_joint1", "atool_joint2"], - base_link_name=kinova.base_link_name(), - end_effector_name="tool_tip", - group_name="jaco_arm_with_articutool", + joint_names=_joint_names, + base_link_name=_base_link, + end_effector_name=_end_effector, + group_name=group_name, callback_group=callback_group, ) lock = Lock() blackboard.set(moveit2_blackboard_key, moveit2) blackboard.set(moveit2_lock_blackboard_key, lock) + node.get_logger().info(f"Successfully created and stored MoveIt2 object and lock for group '{group_name}'.") + return moveit2, lock From 3c19625c1c6f8bd9446134ba66039e12a7da6849 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 17 Apr 2025 19:16:24 -0700 Subject: [PATCH 018/238] Add group_name parameter to MoveIt2 constrain, plan, and execute behaviors, as well as the compute action constraint behavior. --- .../behaviors/acquisition/compute_action_constraints.py | 6 ++++++ .../ada_feeding/behaviors/moveit2/moveit2_constraints.py | 8 ++++++++ .../ada_feeding/behaviors/moveit2/moveit2_execute.py | 6 ++++++ ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py | 6 ++++++ 4 files changed, 26 insertions(+) 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 5c12b8c3..25164820 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py @@ -232,6 +232,7 @@ def blackboard_inputs( action: Union[BlackboardKey, AcquisitionSchema], is_grasp: Union[BlackboardKey, bool] = True, approach_frame_id: Union[BlackboardKey, str] = "approach", + group_name: Union[BlackboardKey, str] = "jaco_arm_with_articutool", ) -> None: """ Blackboard Inputs @@ -241,6 +242,7 @@ def blackboard_inputs( 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 + group_name: The name of the MoveIt2 planning group """ # pylint: disable=unused-argument, duplicate-code # Arguments are handled generically in base class. @@ -279,6 +281,9 @@ def setup(self, **kwargs): # Get Node from Kwargs self.node = kwargs["node"] + # Get group name from blackboard + self.group_name = self.blackboard_get("group_name") + # Get TF Listener from blackboard # For transform approach -> end_effector_frame self.tf_buffer, _, self.tf_lock = get_tf_object(self.blackboard, self.node) @@ -288,6 +293,7 @@ def setup(self, **kwargs): self.moveit2, self.moveit2_lock = get_moveit2_object( self.blackboard, self.node, + self.group_name, ) @override diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py index 9c5f6814..a2db3a3e 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py @@ -25,6 +25,7 @@ import numpy as np from overrides import override import py_trees +from py_trees.blackboard import Blackboard import rclpy import ros2_numpy @@ -165,6 +166,7 @@ def blackboard_inputs( constraints: Union[ BlackboardKey, Optional[List[Tuple[MoveIt2ConstraintType, Dict[str, Any]]]] ] = None, + group_name: Union[BlackboardKey, str] = "jaco_arm_with_articutool", ) -> None: """ Blackboard Inputs @@ -179,6 +181,8 @@ def blackboard_inputs( Note: if position is Vector3Stamped, use position.header.frame_id for frame_id (if not "") + group_name: The name of the MoveIt2 planning group + Parameters ---------- constraints: previous set of constraints to append to @@ -220,10 +224,14 @@ def setup(self, **kwargs): # Get Node from Kwargs self.node = kwargs["node"] + # Get group name from blackboard + self.group_name = self.blackboard_get("group_name") + # Get the MoveIt2 object, don't need lock (const read only) self.moveit2, _ = get_moveit2_object( self.blackboard, self.node, + self.group_name, ) # Get TF Listener from blackboard diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_execute.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_execute.py index 1c85fe7d..8f115ee3 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_execute.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_execute.py @@ -40,6 +40,7 @@ def blackboard_inputs( trajectory: Union[BlackboardKey, Optional[JointTrajectory]], terminate_timeout: Union[BlackboardKey, Duration] = Duration(seconds=10.0), terminate_rate_hz: Union[BlackboardKey, float] = 30.0, + group_name: Union[BlackboardKey, str] = "jaco_arm_with_articutool", ) -> None: """ Blackboard Inputs @@ -51,6 +52,7 @@ def blackboard_inputs( response from the MoveIt2 action server. terminate_rate_hz: How often to check whether a terminate request has been processed. + group_name: The name of the MoveIt2 planning group """ # pylint: disable=unused-argument, duplicate-code # Arguments are handled generically in base class. @@ -95,10 +97,14 @@ def setup(self, **kwargs): # Get Node from Kwargs self.node = kwargs["node"] + # Get group name from blackboard + self.group_name = self.blackboard_get("group_name") + # Get the MoveIt2 object. self.moveit2, self.moveit2_lock = get_moveit2_object( self.blackboard, self.node, + self.group_name, ) @override diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py index e6fd923c..8d2e85d9 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_plan.py @@ -109,6 +109,7 @@ def blackboard_inputs( Union[BlackboardKey, float] ] = 10.0, # np.sqrt(6.0) * np.pi, max_path_len_joint: Optional[Union[BlackboardKey, Dict[str, float]]] = None, + group_name: Union[BlackboardKey, str] = "jaco_arm_with_articutool", ) -> None: """ Blackboard Inputs @@ -147,6 +148,7 @@ def blackboard_inputs( (likely radians). Default sqrt(6.0 * pi) (i.e. 180 degrees each for a 6DOF robot) max_path_len_joint: Maximum distance each joint (or subset of joints) is allowed to travel. As dictionary: -> + group_name: The name of the MoveIt2 planning group """ # TODO: consider cartesian parameter struct # pylint: disable=unused-argument, duplicate-code @@ -190,10 +192,14 @@ def setup(self, **kwargs): # Get Node from Kwargs self.node = kwargs["node"] + # Get group name from blackboard + self.group_name = self.blackboard_get("group_name") + # Get the MoveIt2 object. self.moveit2, self.moveit2_lock = get_moveit2_object( self.blackboard, self.node, + self.group_name, ) # Get TF Listener from blackboard From c13de2f2a8d5fe36aeb927a192917e068c827d81 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 18 Apr 2025 10:24:28 -0700 Subject: [PATCH 019/238] For motions that simply move to a predefined configuration, we use the jaco_arm planning group instead of constraining the Articutool joints to be static when planning with the full jaco_arm_with_articutool planning group --- .../ada_feeding/trees/acquire_food_tree.py | 40 +++---------------- ...o_configuration_with_ft_thresholds_tree.py | 40 +++---------------- ...configuration_with_wheelchair_wall_tree.py | 40 +++---------------- 3 files changed, 15 insertions(+), 105 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index c93e59c9..0c95c30b 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -192,45 +192,11 @@ def create_tree( # Default fail if service is down wait_for_server_timeout_sec=0.0, ), - py_trees.composites.Sequence( - name="GetAndConstrainArticutoolJoints", - memory=True, - children=[ - py_trees.decorators.Timeout( - name="GetArticutoolJointsTimeout", - duration=1.0, - child=GetJointStates( - name="GetArticutoolJoints", - ns=name, - node=self._node, - inputs={ - "joint_names": ["atool_joint1", "atool_joint2"], - }, - outputs={ - "joint_names": BlackboardKey("atool_joint_names"), - "joint_positions": BlackboardKey("atool_joint_positions"), - } - ), - ), - MoveIt2JointConstraint( - name="ArticutoolJointConstraint", - ns=name, - inputs={ - "joint_names": BlackboardKey("atool_joint_names"), - "joint_positions": BlackboardKey("atool_joint_positions"), - }, - outputs={ - "constraints": BlackboardKey("goal_constraints"), - }, - ), - ], - ), MoveIt2JointConstraint( name="RestingConstraint", ns=name, inputs={ "joint_positions": self.resting_joint_positions, - "constraints": BlackboardKey("goal_constraints"), }, outputs={ "constraints": BlackboardKey("goal_constraints"), @@ -251,6 +217,7 @@ def create_tree( "max_velocity_scale": self.max_velocity_scaling_to_resting_configuration, "max_acceleration_scale": self.max_acceleration_scaling_to_resting_configuration, "allowed_planning_time": self.allowed_planning_time_to_resting_configuration, + "group_name": "jaco_arm", }, outputs={ "trajectory": BlackboardKey("resting_trajectory") @@ -260,7 +227,10 @@ def create_tree( MoveIt2Execute( name="Resting", ns=name, - inputs={"trajectory": BlackboardKey("resting_trajectory")}, + inputs={ + "trajectory": BlackboardKey("resting_trajectory"), + "group_name": "jaco_arm", + }, outputs={}, ), ], 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 69319796..06e2ccec 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 @@ -163,39 +163,6 @@ def create_tree( name=name, memory=True, children=[ - py_trees.composites.Sequence( - name="GetAndConstrainArticutoolJoints", - memory=True, - children=[ - py_trees.decorators.Timeout( - name="GetArticutoolJointsTimeout", - duration=1.0, - child=GetJointStates( - name="GetArticutoolJoints", - ns=name, - node=self._node, - inputs={ - "joint_names": ["atool_joint1", "atool_joint2"], - }, - outputs={ - "joint_names": BlackboardKey("atool_joint_names"), - "joint_positions": BlackboardKey("atool_joint_positions"), - } - ), - ), - MoveIt2JointConstraint( - name="ArticutoolJointConstraint", - ns=name, - inputs={ - "joint_names": BlackboardKey("atool_joint_names"), - "joint_positions": BlackboardKey("atool_joint_positions"), - }, - outputs={ - "constraints": BlackboardKey("goal_constraints"), - }, - ), - ], - ), MoveIt2JointConstraint( name="JointConstraint", ns=name, @@ -203,7 +170,6 @@ def create_tree( "joint_positions": BlackboardKey("joint_positions"), "tolerance": self.tolerance_joint, "weight": self.weight_joint, - "constraints": BlackboardKey("goal_constraints"), }, outputs={ "constraints": BlackboardKey("goal_constraints"), @@ -223,6 +189,7 @@ def create_tree( "allowed_planning_time": self.allowed_planning_time, "max_velocity_scale": self.max_velocity_scaling_factor, "max_acceleration_scale": self.max_acceleration_scaling_factor, + "group_name": "jaco_arm", }, outputs={"trajectory": BlackboardKey("trajectory")}, ), @@ -230,7 +197,10 @@ def create_tree( MoveIt2Execute( name="MoveToConfigurationExecute", ns=name, - inputs={"trajectory": BlackboardKey("trajectory")}, + inputs={ + "trajectory": BlackboardKey("trajectory"), + "group_name": "jaco_arm", + }, outputs={}, ), ], 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 74938485..7f1855af 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 @@ -116,45 +116,11 @@ def create_tree( ### Define Tree Logic constraints = [ - py_trees.composites.Sequence( - name="GetAndConstrainArticutoolJoints", - memory=True, - children=[ - py_trees.decorators.Timeout( - name="GetArticutoolJointsTimeout", - duration=1.0, - child=GetJointStates( - name="GetArticutoolJoints", - ns=name, - node=self._node, - inputs={ - "joint_names": ["atool_joint1", "atool_joint2"], - }, - outputs={ - "joint_names": BlackboardKey("atool_joint_names"), - "joint_positions": BlackboardKey("atool_joint_positions"), - } - ), - ), - MoveIt2JointConstraint( - name="ArticutoolJointConstraint", - ns=name, - inputs={ - "joint_names": BlackboardKey("atool_joint_names"), - "joint_positions": BlackboardKey("atool_joint_positions"), - }, - outputs={ - "constraints": BlackboardKey("goal_constraints"), - }, - ), - ], - ), # Goal configuration: staging configuration MoveIt2JointConstraint( name="StagingConfigurationGoalConstraint", ns=name, inputs={ - "constraints": BlackboardKey("goal_constraints"), "joint_positions": self.goal_configuration, "tolerance": self.goal_configuration_tolerance, }, @@ -226,6 +192,7 @@ def create_tree( "allowed_planning_time": self.allowed_planning_time, "max_velocity_scale": self.max_velocity_scaling_factor, "ignore_violated_path_constraints": True, + "group_name": "jaco_arm" }, outputs={"trajectory": BlackboardKey("trajectory")}, ), @@ -234,7 +201,10 @@ def create_tree( MoveIt2Execute( name="MoveToStagingConfigurationExecute", ns=name, - inputs={"trajectory": BlackboardKey("trajectory")}, + inputs={ + "trajectory": BlackboardKey("trajectory"), + "group_name": "jaco_arm", + }, outputs={}, ), ], From 24a912130474567e8392047089126e60468016d9 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 18 Apr 2025 12:52:58 -0700 Subject: [PATCH 020/238] Add MoveIt2ComputeIK behavior --- .../behaviors/moveit2/moveit2_compute_ik.py | 200 ++++++++++++++++++ 1 file changed, 200 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py new file mode 100644 index 00000000..7d41a1c9 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py @@ -0,0 +1,200 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This module defines the MoveIt2ComputeIK behavior, which uses the +ada_feeding MoveIt2 wrapper to compute Inverse Kinematics for a target pose. +""" + +# Standard imports +from typing import Any, Union, Optional, Dict, List, Tuple +from threading import Lock + +# Third-party imports +from geometry_msgs.msg import PoseStamped, Point, Quaternion +from sensor_msgs.msg import JointState +from moveit_msgs.msg import Constraints, MoveItErrorCodes # Added MoveItErrorCodes potentially used by wrapper +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access +import rclpy.node + +# Local imports from ada_feeding (adjust paths if necessary) +from ada_feeding.helpers import ( + BlackboardKey, + get_moveit2_object, + # get_tf_object, # Likely not needed here if pose is already in desired frame for IK +) +from ada_feeding.behaviors import BlackboardBehavior + +from pymoveit2 import MoveIt2 + + +class MoveIt2ComputeIK(BlackboardBehavior): + """ + Computes Inverse Kinematics (IK) using the ada_feeding MoveIt2 interface wrapper. + + Takes a target pose (PoseStamped) and computes a joint state configuration + that achieves that pose for the planning group associated with the MoveIt2 object. + """ + + # pylint: disable=arguments-differ + # pylint: disable=too-many-arguments + + @override + def blackboard_inputs( + self, + target_pose: Union[BlackboardKey, PoseStamped], + group_name: Union[BlackboardKey, str], + start_joint_state: Optional[Union[BlackboardKey, JointState]] = None, + constraints: Optional[Union[BlackboardKey, Constraints]] = None, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + target_pose: The target pose (PoseStamped) for the end-effector of the group. + The frame_id within PoseStamped is important for the IK solver. + group_name: The MoveIt2 planning group name. Used during setup to retrieve + the correct MoveIt2 object instance. + start_joint_state: Optional seed state (JointState or List[float]) for the IK solver. + constraints: Optional MoveIt constraints message to respect during IK solving. + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def blackboard_outputs( + self, + ik_solution_joint_state: Optional[BlackboardKey], # -> Optional[JointState] + success: Optional[BlackboardKey], # -> bool + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + ik_solution_joint_state: The resulting JointState if IK is successful, otherwise None. + success: Boolean flag indicating whether a valid IK solution was found (True) or not (False). + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + """Get the ROS2 node and acquire the MoveIt2 object for the specified group.""" + # pylint: disable=attribute-defined-outside-init + self.node: rclpy.node.Node = kwargs["node"] + self.moveit2_obj: Optional[MoveIt2] = None + self.moveit2_lock: Optional[Lock] = None + self.ik_group_name: Optional[str] = None + + # Get group name from blackboard - needed to select the correct MoveIt2 instance + try: + self.ik_group_name = self.blackboard_get("group_name") + if not isinstance(self.ik_group_name, str) or not self.ik_group_name: + raise ValueError("group_name must be a non-empty string") + except (KeyError, ValueError) as e: + self.logger.error(f"[{self.name}] Invalid or missing input 'group_name': {e}") + return + + # Get the MoveIt2 object using the specific group name + try: + self.moveit2_obj, self.moveit2_lock = get_moveit2_object( + blackboard=self.blackboard, + group_name=self.ik_group_name, + node=self.node, + ) + if self.moveit2_obj is None or self.moveit2_lock is None: + # Ensure setup fails completely if objects aren't retrieved + raise RuntimeError("get_moveit2_object returned None for MoveIt2 object or lock") + self.logger.info(f"[{self.name}] Successfully obtained MoveIt2 object for IK group '{self.ik_group_name}'") + except Exception as e: + self.logger.error(f"[{self.name}] Failed to get MoveIt2 object for IK group '{self.ik_group_name}': {e}") + self.moveit2_obj = None + self.moveit2_lock = None + + @override + def initialise(self) -> None: + """Reset status variables if needed.""" + self.logger.debug(f"[{self.name}] Initialising IK computation.") + self.blackboard_set("success", False) + self.blackboard_set("ik_solution_joint_state", None) + + + @override + def update(self) -> py_trees.common.Status: + """Perform the IK computation using the synchronous compute_ik method.""" + # Check if setup was successful + if self.moveit2_obj is None or self.moveit2_lock is None: + self.logger.error(f"[{self.name}] MoveIt2 object not initialized. Setup likely failed.") + return py_trees.common.Status.FAILURE + + # Check if MoveIt2 is currently locked by another behavior + if self.moveit2_lock.locked(): + self.logger.debug(f"[{self.name}] MoveIt2 is locked, waiting.") + return py_trees.common.Status.RUNNING + + # Acquire lock and perform IK computation + with self.moveit2_lock: + try: + # Read inputs from blackboard at runtime + target_pose_stamped: PoseStamped = self.blackboard_get("target_pose") + start_state_seed = self.blackboard_get("start_joint_state", None) # Optional + ik_constraints = self.blackboard_get("constraints", None) # Optional + + # Validate target_pose_stamped type + if not isinstance(target_pose_stamped, PoseStamped): + self.logger.error(f"[{self.name}] Input 'target_pose' is not a PoseStamped message.") + return py_trees.common.Status.FAILURE + + # --- Call the synchronous compute_ik method provided in the API --- + # It takes position and orientation separately. + self.logger.info(f"[{self.name}] Computing IK for group '{self.ik_group_name}' targeting pose in frame '{target_pose_stamped.header.frame_id}'...") + + # The MoveIt service internally uses the frame_id from the PoseStamped in its request, + # even though the wrapper function takes position/orientation separately. + # We pass the pose components directly. + ik_result_state: Optional[JointState] = self.moveit2_obj.compute_ik( + position=target_pose_stamped.pose.position, + quat_xyzw=target_pose_stamped.pose.orientation, + start_joint_state=start_state_seed, + constraints=ik_constraints + ) + + # Determine success based on whether a result was returned + ik_success = ik_result_state is not None + + # Write results to blackboard + self.blackboard_set("success", ik_success) + self.blackboard_set("ik_solution_joint_state", ik_result_state) + + if ik_success: + self.logger.info(f"[{self.name}] IK computation successful.") + return py_trees.common.Status.SUCCESS + else: + self.logger.warning(f"[{self.name}] IK computation failed (see MoveIt2 wrapper logs for error code).") + return py_trees.common.Status.FAILURE + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error during IK update: {e}") + return py_trees.common.Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Unexpected error during IK computation: {e}", exc_info=True) + return py_trees.common.Status.FAILURE + + # This line should not be reachable if the lock logic is correct + return py_trees.common.Status.FAILURE + + + @override + def terminate(self, new_status: py_trees.common.Status) -> None: + """Log termination status.""" + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") From 4dc443fcb171f11146404880980270126cc4ef93 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 18 Apr 2025 21:30:06 -0700 Subject: [PATCH 021/238] Add StampPoseFromPose behavior that adds a frame ID to an unstamped Pose --- ada_feeding/ada_feeding/behaviors/ros/msgs.py | 123 +++++++++++++++++- 1 file changed, 121 insertions(+), 2 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/ros/msgs.py b/ada_feeding/ada_feeding/behaviors/ros/msgs.py index 2b01ad3e..ea4f9587 100644 --- a/ada_feeding/ada_feeding/behaviors/ros/msgs.py +++ b/ada_feeding/ada_feeding/behaviors/ros/msgs.py @@ -8,7 +8,7 @@ """ # Standard imports -from typing import Any, Callable, List, Optional, Tuple, Union +from typing import Any, Callable, List, Optional, Tuple, Union, Dict # Third-party imports from geometry_msgs.msg import ( @@ -22,15 +22,21 @@ from overrides import override import py_trees import rclpy +import rclpy.node +import rclpy.time from rclpy.duration import Duration import ros2_numpy from scipy.spatial.transform import Rotation as R -from tf2_geometry_msgs import PointStamped, PoseStamped +from tf2_geometry_msgs import Pose, PointStamped, PoseStamped # Local imports from ada_feeding.helpers import BlackboardKey from ada_feeding.behaviors import BlackboardBehavior +# Third-party imports +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status class UpdateTimestamp(BlackboardBehavior): """ @@ -392,3 +398,116 @@ def update(self) -> py_trees.common.Status: self.blackboard_set("twist_stamped", twist_stamped) return py_trees.common.Status.SUCCESS + +# -*- coding: utf-8 -*- +# (Add appropriate Copyright/License if desired) + +""" +This module defines the StampPoseFromPose behavior, which converts a +geometry_msgs/Pose message from the blackboard to a geometry_msgs/PoseStamped +message by adding a header with a specified frame_id and the current timestamp. +""" + +class StampPoseFromPose(BlackboardBehavior): + """ + Takes a geometry_msgs/Pose from the blackboard, adds a header + (specified frame_id and current timestamp), and writes the resulting + geometry_msgs/PoseStamped back to the blackboard. + + This is useful for providing correctly framed input to services or behaviors + that require PoseStamped, such as MoveIt2ComputeIK, when the source only + provides a Pose. + """ + + def blackboard_inputs( + self, + input_pose: Union[BlackboardKey, Pose], + frame_id: Union[BlackboardKey, str], + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + input_pose: BlackboardKey resolving to the geometry_msgs/Pose message to stamp. + frame_id: BlackboardKey resolving to the string frame_id to put in the header. + This frame_id should represent the coordinate system in which the + input_pose values are actually defined. + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + output_pose_stamped: Optional[BlackboardKey], # -> Optional[PoseStamped] + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + output_pose_stamped: BlackboardKey where the resulting PoseStamped will be written. + The value will be None or unchanged if the behavior fails. + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + """Gets the ROS2 node from the arguments passed by the tree runner.""" + # pylint: disable=attribute-defined-outside-init + try: + self.node: rclpy.node.Node = kwargs['node'] + except KeyError as e: + self.logger.error(f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}") + self.node = None + + @override + def initialise(self) -> None: + """Optionally log initialization.""" + self.logger.debug(f"[{self.name}] Initializing.") + + + @override + def update(self) -> Status: + """ + Reads the input Pose and frame_id, creates a PoseStamped with a + current timestamp, and writes it to the output blackboard key. + Returns SUCCESS if successful, FAILURE otherwise. + """ + # Verify setup provided the node needed for timestamping + if not hasattr(self, 'node') or self.node is None: + self.logger.error(f"[{self.name}] Node object not available. Setup likely failed.") + return Status.FAILURE + + try: + pose_in = self.blackboard_get("input_pose") + frame_id_str = self.blackboard_get("frame_id") + + if not isinstance(pose_in, Pose): + self.logger.error(f"[{self.name}] Input 'input_pose' is not a geometry_msgs/Pose (is type: {type(pose_in)}).") + return Status.FAILURE + + if not isinstance(frame_id_str, str) or not frame_id_str: + self.logger.error(f"[{self.name}] Input 'frame_id' must be a non-empty string (is: '{frame_id_str}').") + return Status.FAILURE + + pose_stamped_out = PoseStamped() + pose_stamped_out.header.frame_id = frame_id_str + pose_stamped_out.header.stamp = self.node.get_clock().now().to_msg() + pose_stamped_out.pose = pose_in + + self.blackboard_set("output_pose_stamped", pose_stamped_out) + self.logger.info(f"[{self.name}] Successfully stamped Pose into frame '{frame_id_str}'.") + return Status.SUCCESS + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error: {e}") + return Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Unexpected error stamping pose: {e}") + return Status.FAILURE From a638694dc668674d360eb65cad2e07c19145ce96 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 18 Apr 2025 22:04:53 -0700 Subject: [PATCH 022/238] Update MoveIt2ComputeIK --- .../ada_feeding/behaviors/moveit2/__init__.py | 1 + .../behaviors/moveit2/moveit2_compute_ik.py | 20 +++++++++++++------ 2 files changed, 15 insertions(+), 6 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py b/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py index 0c0c9422..5ad01c39 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py @@ -14,6 +14,7 @@ MoveIt2OrientationConstraint, MoveIt2PoseConstraint, ) +from .moveit2_compute_ik import MoveIt2ComputeIK from .servo_move import ServoMove # Modifying the planning scene diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py index 7d41a1c9..5648f6bd 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py @@ -43,13 +43,12 @@ class MoveIt2ComputeIK(BlackboardBehavior): # pylint: disable=arguments-differ # pylint: disable=too-many-arguments - @override def blackboard_inputs( self, target_pose: Union[BlackboardKey, PoseStamped], group_name: Union[BlackboardKey, str], start_joint_state: Optional[Union[BlackboardKey, JointState]] = None, - constraints: Optional[Union[BlackboardKey, Constraints]] = None, + ik_constraints: Optional[Union[BlackboardKey, Constraints]] = None, ) -> None: """ Blackboard Inputs @@ -61,14 +60,13 @@ def blackboard_inputs( group_name: The MoveIt2 planning group name. Used during setup to retrieve the correct MoveIt2 object instance. start_joint_state: Optional seed state (JointState or List[float]) for the IK solver. - constraints: Optional MoveIt constraints message to respect during IK solving. + ik_constraints: Optional MoveIt constraints message to respect during IK solving. """ # pylint: disable=unused-argument, duplicate-code super().blackboard_inputs( **{key: value for key, value in locals().items() if key != "self"} ) - @override def blackboard_outputs( self, ik_solution_joint_state: Optional[BlackboardKey], # -> Optional[JointState] @@ -147,8 +145,18 @@ def update(self) -> py_trees.common.Status: try: # Read inputs from blackboard at runtime target_pose_stamped: PoseStamped = self.blackboard_get("target_pose") - start_state_seed = self.blackboard_get("start_joint_state", None) # Optional - ik_constraints = self.blackboard_get("constraints", None) # Optional + start_state_seed = None + try: + start_state_seed = self.blackboard_get("start_joint_state") + except KeyError: + self.logger.debug(f"[{self.name}] Optional input 'start_joint_state' not found, using None.") + pass + ik_constraints = None + try: + ik_constraints = self.blackboard_get("ik_constraints") + except KeyError: + self.logger.debug(f"[{self.name}] Optional input 'ik_constraints' not found, using None.") + pass # Validate target_pose_stamped type if not isinstance(target_pose_stamped, PoseStamped): From d5c2aa779b1fc8d1a8aca7069232ba599d386e63 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 18 Apr 2025 22:06:04 -0700 Subject: [PATCH 023/238] Add StampPoseFromPose to __init__ so that it can be imported locally --- ada_feeding/ada_feeding/behaviors/ros/__init__.py | 1 + 1 file changed, 1 insertion(+) diff --git a/ada_feeding/ada_feeding/behaviors/ros/__init__.py b/ada_feeding/ada_feeding/behaviors/ros/__init__.py index fc63d952..6528cc88 100644 --- a/ada_feeding/ada_feeding/behaviors/ros/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/ros/__init__.py @@ -8,6 +8,7 @@ UpdateTimestamp, CreatePoseStamped, PoseStampedToTwistStamped, + StampPoseFromPose, ) from .tf import ( GetTransform, From e4bfc9c8f181d3b6e4a5f92c3039fec082572b34 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 18 Apr 2025 22:07:00 -0700 Subject: [PATCH 024/238] Request latest transform to get an up to date timestamp when applying transform --- ada_feeding/ada_feeding/behaviors/ros/tf.py | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/behaviors/ros/tf.py b/ada_feeding/ada_feeding/behaviors/ros/tf.py index 9c0cda29..605a02bc 100644 --- a/ada_feeding/ada_feeding/behaviors/ros/tf.py +++ b/ada_feeding/ada_feeding/behaviors/ros/tf.py @@ -348,10 +348,16 @@ def update(self) -> py_trees.common.Status: transformed_msg = None with self.tf_lock: if target_frame is not None: + + import copy + msg_to_transform = copy.deepcopy(stamped_msg) + # Set stamp to zero to request latest transform + msg_to_transform.header.stamp = Time(seconds=0, nanoseconds=0).to_msg() + # Compute the transform from the TF tree try: transformed_msg = self.tf_buffer.transform( - stamped_msg, + msg_to_transform, target_frame, timeout, ) From 31abe6388d3d76b5a339e142e748d3913a5b93ed Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 18 Apr 2025 22:48:51 -0700 Subject: [PATCH 025/238] Add ExtractJointsFromState behavior --- .../ada_feeding/behaviors/state/__init__.py | 3 + .../state/extract_joints_from_state.py | 143 ++++++++++++++++++ 2 files changed, 146 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/state/extract_joints_from_state.py diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py index fdcaf748..f88485ec 100644 --- a/ada_feeding/ada_feeding/behaviors/state/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -7,3 +7,6 @@ from .get_joint_states import ( GetJointStates, ) +from .extract_joints_from_state import ( + ExtractJointsFromState, +) diff --git a/ada_feeding/ada_feeding/behaviors/state/extract_joints_from_state.py b/ada_feeding/ada_feeding/behaviors/state/extract_joints_from_state.py new file mode 100644 index 00000000..3ae02dac --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/extract_joints_from_state.py @@ -0,0 +1,143 @@ +# -*- coding: utf-8 -*- + +""" +This module defines the ExtractJointsFromState behavior, which extracts specific +joint names and their corresponding positions from a JointState message. +""" + +# Standard imports +from typing import Union, Optional, Dict, Any, List + +# Third-party imports +from sensor_msgs.msg import JointState +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status +import rclpy.node + +# Local imports (adjust paths as needed) +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class ExtractJointsFromState(BlackboardBehavior): + """ + Extracts specified joint names and their corresponding positions from a + source JointState message read from the blackboard. Writes the results + as separate lists (names and positions) to the blackboard. + """ + + def blackboard_inputs( + self, + source_joint_state: Union[BlackboardKey, JointState], + target_joint_names: Union[BlackboardKey, List[str]], + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + source_joint_state: BlackboardKey resolving to the input JointState message + (e.g., the result of an IK computation). + target_joint_names: BlackboardKey resolving to the List[str] of joint names + to extract from the source_joint_state. + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + output_joint_names: Optional[BlackboardKey], # -> Optional[List[str]] + output_joint_positions: Optional[BlackboardKey], # -> Optional[List[float]] + success: Optional[BlackboardKey], # -> bool (Optional success flag) + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + output_joint_names: BlackboardKey where the extracted List[str] of joint names + (in the order requested) will be written. + output_joint_positions: BlackboardKey where the extracted List[float] of corresponding + joint positions will be written. + success: Optional BlackboardKey to write a boolean indicating successful extraction. + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def initialise(self) -> None: + """Clear output keys on initialization.""" + self.logger.debug(f"[{self.name}] Initializing.") + # Clear potential previous results + self.blackboard_set("output_joint_names", None) + self.blackboard_set("output_joint_positions", None) + self.blackboard_set("success", False) + + + @override + def update(self) -> Status: + """ + Reads inputs, performs extraction, writes outputs. + Returns SUCCESS if all target joints are found, FAILURE otherwise. + """ + try: + source_state: JointState = self.blackboard_get("source_joint_state") + target_names: List[str] = self.blackboard_get("target_joint_names") + + if not isinstance(source_state, JointState): + self.logger.error(f"[{self.name}] Input 'source_joint_state' is not a JointState message (is type: {type(source_state)}).") + return Status.FAILURE + + if not isinstance(target_names, list) or not all(isinstance(n, str) for n in target_names): + self.logger.error(f"[{self.name}] Input 'target_joint_names' must be a List[str].") + return Status.FAILURE + + if not target_names: + self.logger.warning(f"[{self.name}] Input 'target_joint_names' is empty. Nothing to extract.") + self.blackboard_set("output_joint_names", []) + self.blackboard_set("output_joint_positions", []) + self.blackboard_set("success", True) + return Status.SUCCESS + + # Create a lookup dictionary for efficient access + source_positions_dict = {name: pos for name, pos in zip(source_state.name, source_state.position)} + + extracted_names = [] + extracted_positions = [] + all_found = True + + for name in target_names: + if name in source_positions_dict: + extracted_names.append(name) + extracted_positions.append(source_positions_dict[name]) + else: + self.logger.error(f"[{self.name}] Target joint '{name}' not found in source_joint_state names: {list(source_positions_dict.keys())}") + all_found = False + break + + if all_found: + self.blackboard_set("output_joint_names", extracted_names) + self.blackboard_set("output_joint_positions", extracted_positions) + self.blackboard_set("success", True) + self.logger.info(f"[{self.name}] Successfully extracted {len(extracted_names)} joints.") + return Status.SUCCESS + else: + self.blackboard_set("output_joint_names", None) + self.blackboard_set("output_joint_positions", None) + self.blackboard_set("success", False) + return Status.FAILURE + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error: {e}") + self.blackboard_set("success", False) + return Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Unexpected error during joint extraction: {e}", exc_info=True) + self.blackboard_set("success", False) + return Status.FAILURE From 4f8261059ff422d05dc8599d404c0fdc91f7b4e6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Apr 2025 10:41:43 -0700 Subject: [PATCH 026/238] (WIP) Rework move above plan to achieve desired pose by defining joint constraints for Jaco arm and Articutool separately. Currently, we only constrain and execute the plan for the Jaco arm and disable all further behaviors to observe the desired motion. TODO: Implement Articutool motion by sending trajectory --- .../ada_feeding/trees/acquire_food_tree.py | 923 +++++++++++------- 1 file changed, 553 insertions(+), 370 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 0c95c30b..c4032f29 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -9,6 +9,7 @@ # Standard imports import pickle +import operator from typing import List, Optional # Third-party imports @@ -17,6 +18,7 @@ from overrides import override import py_trees from py_trees.blackboard import Blackboard +from py_trees.common import Status, ComparisonExpression from py_trees.behaviours import Success import py_trees_ros from rcl_interfaces.srv import SetParameters @@ -41,10 +43,13 @@ MoveIt2PositionOffsetConstraint, MoveIt2Plan, MoveIt2Execute, + MoveIt2ComputeIK, ServoMove, ToggleCollisionObject, ) -from ada_feeding.behaviors.state import GetJointStates +from ada_feeding.behaviors.state import GetJointStates, ExtractJointsFromState +from ada_feeding.behaviors.ros.msgs import StampPoseFromPose +from ada_feeding.behaviors.ros.tf import ApplyTransform from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( pre_moveto_config, @@ -452,24 +457,125 @@ def move_above_plan( ), # Re-Tare FT Sensor and default to 4N threshold pre_moveto_config(name="PreAcquireFTTare"), - ### Move Above Food - MoveIt2PoseConstraint( - name="MoveAbovePose", + # Get Current Joint State (for IK Seed) + py_trees.decorators.Timeout( + name="GetCurrentState", + duration=1.0, + child=GetJointStates( + name="GetArticutoolJoints", + ns=name, + node=self._node, + inputs={ + "joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + "atool_joint1", + "atool_joint2", + ] + }, + outputs={ + "joint_positions": BlackboardKey("current_joint_positions"), + "joint_names": BlackboardKey("current_joint_names") + } + ), + ), + + # Convert Pose to PoseStamped using the defined frame_id + StampPoseFromPose( + name="StampMoveAbovePose", ns=name, inputs={ - "pose": BlackboardKey("move_above_pose"), - "frame_id": "food", - "tolerance_orientation": [ - 0.01, - 0.01, - 0.01, - ], # x, y, z rotvec - "parameterization": 1, + "input_pose": BlackboardKey("move_above_pose"), + "frame_id": "food" }, outputs={ - "constraints": BlackboardKey("goal_constraints"), + "output_pose_stamped": BlackboardKey("move_above_pose_stamped_food_frame") + } + ), + + # Use ApplyTransform to transform into the MoveIt Planning Frame + ApplyTransform( + name="TransformPoseToIKFrame", + ns=name, + inputs={ + "stamped_msg": BlackboardKey("move_above_pose_stamped_food_frame"), + "target_frame": "j2n6s200_link_base", }, + outputs={ + "transformed_msg": BlackboardKey("move_above_pose_stamped_base_frame") + } + ), + + # Compute IK for the target pose using the full jaco_arm_with_articutool planning group + MoveIt2ComputeIK( + name="ComputeJacoArmWithArticutoolIK", + ns=name, + inputs={ + "target_pose": BlackboardKey("move_above_pose_stamped_base_frame"), + "group_name": "jaco_arm_with_articutool", + # "start_joint_state": BlackboardKey("current_joint_positions"), + }, + outputs={ + "ik_solution_joint_state": BlackboardKey("move_above_ik_solution"), + "success": BlackboardKey("move_above_ik_success"), + } + ), + ExtractJointsFromState( + name="ExtractJacoArmJoints", + ns=name, + inputs={ + "source_joint_state": BlackboardKey("move_above_ik_solution"), + "target_joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ] + + }, + outputs={ + "output_joint_names": BlackboardKey("move_above_jaco_arm_joint_names"), + "output_joint_positions": BlackboardKey("move_above_jaco_arm_joint_positions"), + "success": BlackboardKey("extract_jaco_arm_joints_success") + } + ), + MoveIt2JointConstraint( + name="SetJacoArmJointConstraint", + ns=name, + inputs={ + "joint_positions": BlackboardKey("move_above_jaco_arm_joint_positions"), + "joint_names": BlackboardKey("move_above_jaco_arm_joint_names"), + "tolerance": 0.001, + "constraints": None, + }, + outputs={ + "constraints": BlackboardKey("move_above_jaco_arm_constraints"), + } ), + # ### Move Above Food + # MoveIt2PoseConstraint( + # name="MoveAbovePose", + # ns=name, + # inputs={ + # "pose": BlackboardKey("move_above_pose"), + # "frame_id": "food", + # "tolerance_orientation": [ + # 0.01, + # 0.01, + # 0.01, + # ], # x, y, z rotvec + # "parameterization": 1, + # }, + # outputs={ + # "constraints": BlackboardKey("goal_constraints"), + # }, + # ), py_trees.decorators.Timeout( name="MoveAbovePlanTimeout", # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such @@ -478,11 +584,12 @@ def move_above_plan( name="MoveAbovePlan", ns=name, inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), + "goal_constraints": BlackboardKey("move_above_jaco_arm_constraints"), "max_velocity_scale": self.max_velocity_scaling_move_above, "max_acceleration_scale": self.max_acceleration_scaling_move_above, "allowed_planning_time": self.allowed_planning_time_for_move_above, "max_path_len_joint": max_path_len_joint, + "group_name": "jaco_arm", }, outputs={ "trajectory": BlackboardKey("move_above_trajectory"), @@ -490,41 +597,116 @@ def move_above_plan( }, ), ), - ### Test MoveIntoFood - MoveIt2PoseConstraint( - name="MoveIntoPose", - ns=name, - inputs={ - "pose": BlackboardKey("move_into_pose"), - "frame_id": "food", - }, - outputs={ - "constraints": BlackboardKey("goal_constraints"), - }, - ), - py_trees.decorators.Timeout( - name="MoveIntoPlanTimeout", - # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such - duration=10.0 * self.allowed_planning_time_for_move_into, - child=MoveIt2Plan( - name="MoveIntoPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey("goal_constraints"), - "max_velocity_scale": self.max_velocity_scaling_move_into, - "max_acceleration_scale": self.max_acceleration_scaling_move_into, - "cartesian": True, - "cartesian_max_step": 0.001, - "cartesian_fraction_threshold": 0.92, - "start_joint_state": BlackboardKey("test_into_joints"), - "max_path_len_joint": max_path_len_joint, - "allowed_planning_time": self.allowed_planning_time_for_move_into, - }, - outputs={ - "trajectory": BlackboardKey("move_into_trajectory") - }, - ), - ), + # # Convert Pose to PoseStamped using the defined frame_id + # StampPoseFromPose( + # name="StampMoveIntoPose", + # ns=name, + # inputs={ + # "input_pose": BlackboardKey("move_into_pose"), + # "frame_id": "food" + # }, + # outputs={ + # "output_pose_stamped": BlackboardKey("move_into_pose_stamped_food_frame") + # } + # ), + # + # # Use ApplyTransform to transform into the MoveIt Planning Frame + # ApplyTransform( + # name="TransformPoseToIKFrame", + # ns=name, + # inputs={ + # "stamped_msg": BlackboardKey("move_into_pose_stamped_food_frame"), + # "target_frame": "j2n6s200_link_base", + # }, + # outputs={ + # "transformed_msg": BlackboardKey("move_into_pose_stamped_base_frame") + # } + # ), + # + # # Compute IK for the target pose using the full jaco_arm_with_articutool planning group + # MoveIt2ComputeIK( + # name="ComputeJacoArmWithArticutoolIK", + # ns=name, + # inputs={ + # "target_pose": BlackboardKey("move_into_pose_stamped_base_frame"), + # "group_name": "jaco_arm_with_articutool", + # # "start_joint_state": BlackboardKey("current_joint_positions"), + # }, + # outputs={ + # "ik_solution_joint_state": BlackboardKey("move_into_ik_solution"), + # "success": BlackboardKey("move_into_ik_success"), + # } + # ), + # ExtractJointsFromState( + # name="ExtractJacoArmJoints", + # ns=name, + # inputs={ + # "source_joint_state": BlackboardKey("move_into_ik_solution"), + # "target_joint_names": [ + # "j2n6s200_joint_1", + # "j2n6s200_joint_2", + # "j2n6s200_joint_3", + # "j2n6s200_joint_4", + # "j2n6s200_joint_5", + # "j2n6s200_joint_6", + # ] + # + # }, + # outputs={ + # "output_joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), + # "output_joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), + # "success": BlackboardKey("extract_jaco_arm_joints_success") + # } + # ), + # MoveIt2JointConstraint( + # name="SetJacoArmJointConstraint", + # ns=name, + # inputs={ + # "joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), + # "joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), + # "tolerance": 0.001, + # "constraints": None, + # }, + # outputs={ + # "constraints": BlackboardKey("move_into_jaco_arm_constraints"), + # } + # ), + # ### Test MoveIntoFood + # MoveIt2PoseConstraint( + # name="MoveIntoPose", + # ns=name, + # inputs={ + # "pose": BlackboardKey("move_into_pose"), + # "frame_id": "food", + # }, + # outputs={ + # "constraints": BlackboardKey("goal_constraints"), + # }, + # ), + # py_trees.decorators.Timeout( + # name="MoveIntoPlanTimeout", + # # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + # duration=10.0 * self.allowed_planning_time_for_move_into, + # child=MoveIt2Plan( + # name="MoveIntoPlan", + # ns=name, + # inputs={ + # "goal_constraints": BlackboardKey("move_into_jaco_arm_constraints"), + # "max_velocity_scale": self.max_velocity_scaling_move_into, + # "max_acceleration_scale": self.max_acceleration_scaling_move_into, + # "cartesian": True, + # "cartesian_max_step": 0.001, + # "cartesian_fraction_threshold": 0.92, + # "start_joint_state": BlackboardKey("test_into_joints"), + # "max_path_len_joint": max_path_len_joint, + # "allowed_planning_time": self.allowed_planning_time_for_move_into, + # "group_name": "jaco_arm", + # }, + # outputs={ + # "trajectory": BlackboardKey("move_into_trajectory") + # }, + # ), + # ), ], ) @@ -634,330 +816,331 @@ def move_above_plan( inputs={ "trajectory": BlackboardKey( "move_above_trajectory" - ) + ), + "group_name": "jaco_arm", }, outputs={}, ), - # If Anything goes wrong, reset FT to safe levels - scoped_behavior( - name="SafeFTPreempt", - # Set Approach F/T Thresh - 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")] - ), - response_checks=[ - py_trees.common.ComparisonExpression( - variable=Blackboard.separator.join( - [ - name, - BlackboardKey( - "ft_response" - ), - ] - ), - value=SetParameters.Response(), # Unused - operator=set_parameter_response_all_success, - ) - ], - ), - ], - ), - post_behavior=py_trees.composites.Sequence( - name=name, - memory=True, - children=[ - pre_moveto_config( - name="PostAcquireFTSet", re_tare=False - ), - ], - ), - on_preempt_timeout=5.0, - # Starts a new Sequence w/ Memory internally - workers=[ - ### Move Into Food - MoveIt2PoseConstraint( - name="MoveIntoPose", - ns=name, - inputs={ - "pose": BlackboardKey("move_into_pose"), - "frame_id": "food", - }, - outputs={ - "constraints": BlackboardKey( - "goal_constraints" - ), - }, - ), - # If this fails - # Auto-fallback to precomputed MoveInto - # From move_above_plan() - py_trees.decorators.FailureIsSuccess( - name="MoveIntoPlanFallbackPrecomputed", - child=py_trees.decorators.Timeout( - name="MoveIntoPlanTimeout", - # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such - duration=10.0 - * self.allowed_planning_time_for_move_into, - child=MoveIt2Plan( - name="MoveIntoPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey( - "goal_constraints" - ), - "max_velocity_scale": self.max_velocity_scaling_move_into, - "max_acceleration_scale": self.max_acceleration_scaling_move_into, - "cartesian": True, - "cartesian_max_step": 0.001, - "cartesian_fraction_threshold": 0.92, - "max_path_len_joint": max_path_len_joint, - "allowed_planning_time": self.allowed_planning_time_for_move_into, - }, - outputs={ - "trajectory": BlackboardKey( - "move_into_trajectory" - ) - }, - ), - ), - ), - # MoveInto expect F/T failure - py_trees.decorators.FailureIsSuccess( - name="MoveIntoExecuteSucceed", - child=MoveIt2Execute( - name="MoveInto", - ns=name, - inputs={ - "trajectory": BlackboardKey( - "move_into_trajectory" - ) - }, - outputs={}, - ), - ), - ### 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, - servo_controller_name="jaco_arm_cartesian_controller", - start_moveit_servo=False, - ) - .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_cartesian_controller_parameters", - ), - StopServoTree( - self._node, - servo_controller_name="jaco_arm_cartesian_controller", - stop_moveit_servo=False, - ) - .create_tree(name="StopServoScoped") - .root, - ], - ), - on_preempt_timeout=5.0, - # Starts a new Sequence w/ Memory internally - workers=[ - py_trees.composites.Selector( - name="InFoodErrorSelector", - memory=True, - children=[ - py_trees.composites.Sequence( - name="InFoodGraspExtract", - memory=True, - children=[ - ### Grasp - retry_call_ros_service( - name="GraspFTThresh", - service_type=SetParameters, - service_name="~/set_cartesian_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, - ) - ], - ), - 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" - ), - "pub_topic": "~/cartesian_twist_cmds", - "servo_status_sub_topic": None, - }, - ), # Auto Zero-Twist on terminate() - ### Extraction - ComputeActionTwist( - name="ComputeExtract", - 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_cartesian_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" - ), - "pub_topic": "~/cartesian_twist_cmds", - "servo_status_sub_topic": None, - }, - ), # Auto Zero-Twist on terminate() - ft_thresh_satisfied( - name="CheckFTForkOffPlate" - ), - ], # End InFoodGraspExtract.children - ), # End InFoodGraspExtract - recovery_tree, - ], # End InFoodErrorSelector.children - ), # End InFoodErrorSelector - ], # End MoveIt2Servo.workers - ), # End MoveIt2Servo - ], # End SafeFTPreempt.workers - ), # End SafeFTPreempt + # # If Anything goes wrong, reset FT to safe levels + # scoped_behavior( + # name="SafeFTPreempt", + # # Set Approach F/T Thresh + # 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")] + # ), + # response_checks=[ + # py_trees.common.ComparisonExpression( + # variable=Blackboard.separator.join( + # [ + # name, + # BlackboardKey( + # "ft_response" + # ), + # ] + # ), + # value=SetParameters.Response(), # Unused + # operator=set_parameter_response_all_success, + # ) + # ], + # ), + # ], + # ), + # post_behavior=py_trees.composites.Sequence( + # name=name, + # memory=True, + # children=[ + # pre_moveto_config( + # name="PostAcquireFTSet", re_tare=False + # ), + # ], + # ), + # on_preempt_timeout=5.0, + # # Starts a new Sequence w/ Memory internally + # workers=[ + # ### Move Into Food + # MoveIt2PoseConstraint( + # name="MoveIntoPose", + # ns=name, + # inputs={ + # "pose": BlackboardKey("move_into_pose"), + # "frame_id": "food", + # }, + # outputs={ + # "constraints": BlackboardKey( + # "goal_constraints" + # ), + # }, + # ), + # # If this fails + # # Auto-fallback to precomputed MoveInto + # # From move_above_plan() + # py_trees.decorators.FailureIsSuccess( + # name="MoveIntoPlanFallbackPrecomputed", + # child=py_trees.decorators.Timeout( + # name="MoveIntoPlanTimeout", + # # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + # duration=10.0 + # * self.allowed_planning_time_for_move_into, + # child=MoveIt2Plan( + # name="MoveIntoPlan", + # ns=name, + # inputs={ + # "goal_constraints": BlackboardKey( + # "goal_constraints" + # ), + # "max_velocity_scale": self.max_velocity_scaling_move_into, + # "max_acceleration_scale": self.max_acceleration_scaling_move_into, + # "cartesian": True, + # "cartesian_max_step": 0.001, + # "cartesian_fraction_threshold": 0.92, + # "max_path_len_joint": max_path_len_joint, + # "allowed_planning_time": self.allowed_planning_time_for_move_into, + # }, + # outputs={ + # "trajectory": BlackboardKey( + # "move_into_trajectory" + # ) + # }, + # ), + # ), + # ), + # # MoveInto expect F/T failure + # py_trees.decorators.FailureIsSuccess( + # name="MoveIntoExecuteSucceed", + # child=MoveIt2Execute( + # name="MoveInto", + # ns=name, + # inputs={ + # "trajectory": BlackboardKey( + # "move_into_trajectory" + # ) + # }, + # outputs={}, + # ), + # ), + # ### 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, + # servo_controller_name="jaco_arm_cartesian_controller", + # start_moveit_servo=False, + # ) + # .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_cartesian_controller_parameters", + # ), + # StopServoTree( + # self._node, + # servo_controller_name="jaco_arm_cartesian_controller", + # stop_moveit_servo=False, + # ) + # .create_tree(name="StopServoScoped") + # .root, + # ], + # ), + # on_preempt_timeout=5.0, + # # Starts a new Sequence w/ Memory internally + # workers=[ + # py_trees.composites.Selector( + # name="InFoodErrorSelector", + # memory=True, + # children=[ + # py_trees.composites.Sequence( + # name="InFoodGraspExtract", + # memory=True, + # children=[ + # ### Grasp + # retry_call_ros_service( + # name="GraspFTThresh", + # service_type=SetParameters, + # service_name="~/set_cartesian_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, + # ) + # ], + # ), + # 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" + # ), + # "pub_topic": "~/cartesian_twist_cmds", + # "servo_status_sub_topic": None, + # }, + # ), # Auto Zero-Twist on terminate() + # ### Extraction + # ComputeActionTwist( + # name="ComputeExtract", + # 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_cartesian_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" + # ), + # "pub_topic": "~/cartesian_twist_cmds", + # "servo_status_sub_topic": None, + # }, + # ), # Auto Zero-Twist on terminate() + # ft_thresh_satisfied( + # name="CheckFTForkOffPlate" + # ), + # ], # End InFoodGraspExtract.children + # ), # End InFoodGraspExtract + # recovery_tree, + # ], # End InFoodErrorSelector.children + # ), # End InFoodErrorSelector + # ], # End MoveIt2Servo.workers + # ), # End MoveIt2Servo + # ], # End SafeFTPreempt.workers + # ), # End SafeFTPreempt ], # End OctomapAndTableCollision.workers ), # OctomapAndTableCollision ] From cff700967903dd5b7448e8d3fb65c640639e42aa Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Apr 2025 11:05:15 -0700 Subject: [PATCH 027/238] Add move above plan for Articutool joints --- .../ada_feeding/trees/acquire_food_tree.py | 59 +++++++++++++++++-- 1 file changed, 55 insertions(+), 4 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index c4032f29..f460a818 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -545,6 +545,23 @@ def move_above_plan( "success": BlackboardKey("extract_jaco_arm_joints_success") } ), + ExtractJointsFromState( + name="ExtractArticutoolJoints", + ns=name, + inputs={ + "source_joint_state": BlackboardKey("move_above_ik_solution"), + "target_joint_names": [ + "atool_joint1", + "atool_joint2", + ] + + }, + outputs={ + "output_joint_names": BlackboardKey("move_above_articutool_joint_names"), + "output_joint_positions": BlackboardKey("move_above_articutool_joint_positions"), + "success": BlackboardKey("extract_articutool_joints_success") + } + ), MoveIt2JointConstraint( name="SetJacoArmJointConstraint", ns=name, @@ -558,6 +575,19 @@ def move_above_plan( "constraints": BlackboardKey("move_above_jaco_arm_constraints"), } ), + MoveIt2JointConstraint( + name="SetArticutoolJointConstraint", + ns=name, + inputs={ + "joint_positions": BlackboardKey("move_above_articutool_joint_positions"), + "joint_names": BlackboardKey("move_above_articutool_joint_names"), + "tolerance": 0.001, + "constraints": None, + }, + outputs={ + "constraints": BlackboardKey("move_above_articutool_constraints"), + } + ), # ### Move Above Food # MoveIt2PoseConstraint( # name="MoveAbovePose", @@ -577,11 +607,11 @@ def move_above_plan( # }, # ), py_trees.decorators.Timeout( - name="MoveAbovePlanTimeout", + name="MoveAboveJacoArmPlanTimeout", # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such duration=10.0 * self.allowed_planning_time_for_move_above, child=MoveIt2Plan( - name="MoveAbovePlan", + name="MoveAboveJacoArmPlan", ns=name, inputs={ "goal_constraints": BlackboardKey("move_above_jaco_arm_constraints"), @@ -592,7 +622,28 @@ def move_above_plan( "group_name": "jaco_arm", }, outputs={ - "trajectory": BlackboardKey("move_above_trajectory"), + "trajectory": BlackboardKey("move_above_jaco_arm_trajectory"), + "end_joint_state": BlackboardKey("test_into_joints"), + }, + ), + ), + py_trees.decorators.Timeout( + name="MoveAboveArticutoolPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 * self.allowed_planning_time_for_move_above, + child=MoveIt2Plan( + name="MoveAboveArticutoolPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey("move_above_articutool_constraints"), + "max_velocity_scale": self.max_velocity_scaling_move_above, + "max_acceleration_scale": self.max_acceleration_scaling_move_above, + "allowed_planning_time": self.allowed_planning_time_for_move_above, + "max_path_len_joint": max_path_len_joint, + "group_name": "articutool", + }, + outputs={ + "trajectory": BlackboardKey("move_above_articutool_trajectory"), "end_joint_state": BlackboardKey("test_into_joints"), }, ), @@ -815,7 +866,7 @@ def move_above_plan( ns=name, inputs={ "trajectory": BlackboardKey( - "move_above_trajectory" + "move_above_jaco_arm_trajectory" ), "group_name": "jaco_arm", }, From 96eec3090d6a9604bfa2c3822400aa44c52b6dc4 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Apr 2025 12:13:55 -0700 Subject: [PATCH 028/238] Add ExecuteArticutoolTrajectory behavior --- .../behaviors/articutool/__init__.py | 6 + .../execute_articutool_trajectory.py | 402 ++++++++++++++++++ 2 files changed, 408 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/articutool/__init__.py create mode 100644 ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py diff --git a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py new file mode 100644 index 00000000..09033ed1 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py @@ -0,0 +1,6 @@ +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +from .execute_articutool_trajectory import ( + ExecuteArticutoolTrajectory, +) diff --git a/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py b/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py new file mode 100644 index 00000000..aded93b5 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py @@ -0,0 +1,402 @@ +# -*- coding: utf-8 -*- +# (Add appropriate Copyright/License if desired) + +""" +Defines the ExecuteArticutoolTrajectory behavior, which sends a planned +trajectory to the Articutool's FollowJointTrajectory action server, +handling controller switching. +""" + +# Standard imports +from typing import Union, Optional, Dict, Any, List +from enum import Enum + +# Third-party imports +import rclpy +from rclpy.action import ActionClient +from rclpy.action.client import ClientGoalHandle, GoalStatus +from rclpy.node import Node +from rclpy.executors import Future # For type hints + +from control_msgs.action import FollowJointTrajectory +from trajectory_msgs.msg import JointTrajectory # Input type +# from builtin_interfaces.msg import Duration # Used internally by action goal +# from sensor_msgs.msg import JointState # Not directly needed + +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status + +# Local imports (adjust paths as needed) +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior +# Assuming ControllerSwitcher is importable and works with a passed node +from articutool_control.controller_switcher import ControllerSwitcher + + +# Define constants for action status reporting (optional but clearer) +class ActionExecutionStatus(Enum): + IDLE = "IDLE" + SWITCHING_CONTROLLERS = "SWITCHING_CONTROLLERS" + SENDING_GOAL = "SENDING_GOAL" + WAITING_FOR_ACCEPTANCE = "WAITING_FOR_ACCEPTANCE" + EXECUTING = "EXECUTING" + WAITING_FOR_RESULT = "WAITING_FOR_RESULT" # If execution status isn't monitored closely + GOAL_REJECTED = "GOAL_REJECTED" + GOAL_CANCELLED = "GOAL_CANCELLED" + SUCCEEDED = "SUCCEEDED" + ABORTED = "ABORTED" # From server + FAILED = "FAILED" # General failure + + +class ExecuteArticutoolTrajectory(BlackboardBehavior): + """ + Executes a JointTrajectory on the Articutool using the + FollowJointTrajectory action server. Handles controller switching before + sending the goal. Returns RUNNING while executing, SUCCESS on completion, + FAILURE on error, rejection, or abortion. + """ + + # --- Constants --- + DEFAULT_ACTION_SERVER = "/articutool/joint_trajectory_controller/follow_joint_trajectory" + DEFAULT_CONTROLLER_TO_ACTIVATE = ["joint_trajectory_controller"] # Expects list + DEFAULT_CONTROLLERS_TO_DEACTIVATE = ["velocity_controller"] # Expects list + + def __init__(self, name: str, ns: str = "/", **kwargs): + # Pass kwargs to parent if BlackboardBehavior supports it + super().__init__(name=name, ns=ns, **kwargs) + # Internal state variables + self._action_client: Optional[ActionClient] = None + self.controller_switcher: Optional[ControllerSwitcher] = None + self._send_goal_future: Optional[Future] = None + self._get_result_future: Optional[Future] = None + self._goal_handle: Optional[ClientGoalHandle] = None + self._current_status: ActionExecutionStatus = ActionExecutionStatus.IDLE + self._result_code: Optional[int] = None # Store the final result code + + def blackboard_inputs( + self, + trajectory: Union[BlackboardKey, JointTrajectory], + controllers_to_activate: Union[BlackboardKey, List[str]] = DEFAULT_CONTROLLER_TO_ACTIVATE, + controllers_to_deactivate: Union[BlackboardKey, List[str]] = DEFAULT_CONTROLLERS_TO_DEACTIVATE, + action_server_name: Union[BlackboardKey, str] = DEFAULT_ACTION_SERVER, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + trajectory: The trajectory_msgs/JointTrajectory message to execute. + controllers_to_activate: List of controller names to activate before sending. + controllers_to_deactivate: List of controller names to deactivate before sending. + action_server_name: Name of the FollowJointTrajectory action server. + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + action_goal_accepted: Optional[BlackboardKey] = None, # -> bool + action_result_code: Optional[BlackboardKey] = None, # -> int (FollowJointTrajectory.Result.error_code) + action_status: Optional[BlackboardKey] = None, # -> str (from ActionExecutionStatus enum) + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + action_goal_accepted: True if the action server accepted the goal request. + action_result_code: The final error_code from the FollowJointTrajectory result. + action_status: The current status of the action call (e.g., "IDLE", "EXECUTING", "SUCCEEDED"). + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + """Get node, create action client and controller switcher.""" + # pylint: disable=attribute-defined-outside-init + try: + self.node: Node = kwargs['node'] + except KeyError as e: + self.logger.error(f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}") + return # Cannot function without node + + try: + try: + action_server_name = self.blackboard_get("action_server_name") + if not isinstance(action_server_name, str) or not action_server_name: + self.logger.warning(f"[{self.name}] Blackboard 'action_server_name' invalid, using default.") + action_server_name = self.DEFAULT_ACTION_SERVER + except KeyError: + self.logger.info(f"[{self.name}] Input 'action_server_name' not found, using default: {self.DEFAULT_ACTION_SERVER}") + action_server_name = self.DEFAULT_ACTION_SERVER + + self._action_client = ActionClient( + self.node, + FollowJointTrajectory, + action_server_name + ) + # Check server availability briefly during setup + timeout_sec = 1.0 + if not self._action_client.wait_for_server(timeout_sec=timeout_sec): + self.logger.error(f"[{self.name}] Action server '{action_server_name}' not available after {timeout_sec}s during setup.") + self._action_client = None # Mark as unavailable + return # Setup fails + + # Instantiate Controller Switcher (pass node if its __init__ requires it) + try: + # Assuming ControllerSwitcher might take node as arg + self.controller_switcher = ControllerSwitcher(node=self.node) + except TypeError: # If ControllerSwitcher() doesn't take node + self.controller_switcher = ControllerSwitcher() + + + self.logger.info(f"[{self.name}] Setup complete. Action client created for '{action_server_name}'.") + + except Exception as e: + self.logger.error(f"[{self.name}] Failed during setup: {e}") + self._action_client = None + self.controller_switcher = None + + + @override + def initialise(self) -> None: + """Reset action client state.""" + self.logger.debug(f"[{self.name}] Initialising.") + self._send_goal_future = None + self._get_result_future = None + self._goal_handle = None + self._current_status = ActionExecutionStatus.IDLE + self._result_code = None # Reset result code + # Clear blackboard outputs + self.blackboard_set("action_goal_accepted", False) + self.blackboard_set("action_result_code", self._result_code) # Set to None or initial value + self.blackboard_set("action_status", self._current_status.value) + + + @override + def update(self) -> Status: + """Manage the action client state machine.""" + # Update blackboard status at start of tick + self.blackboard_set("action_status", self._current_status.value) + + if self._action_client is None or self.controller_switcher is None: + self.logger.error(f"[{self.name}] Action client or controller switcher not available. Setup failed?") + return Status.FAILURE # Setup must have failed + + # --- State 1: IDLE -> Try sending goal --- + if self._current_status == ActionExecutionStatus.IDLE: + try: + trajectory: JointTrajectory = self.blackboard_get("trajectory") + controllers_to_activate: List[str] = self.blackboard_get("controllers_to_activate") + controllers_to_deactivate: List[str] = self.blackboard_get("controllers_to_deactivate") + + # Validate inputs + if not isinstance(trajectory, JointTrajectory): + self.logger.error(f"[{self.name}] Input 'trajectory' is not a JointTrajectory message.") + return Status.FAILURE + if not trajectory.points: + self.logger.warning(f"[{self.name}] Input 'trajectory' has no points. Succeeding trivially.") + self._current_status = ActionExecutionStatus.SUCCEEDED + self.blackboard_set("action_status", self._current_status.value) + self.blackboard_set("action_result_code", FollowJointTrajectory.Result.SUCCESSFUL) + return Status.SUCCESS + if not isinstance(controllers_to_activate, list) or not isinstance(controllers_to_deactivate, list): + self.logger.error(f"[{self.name}] Controller inputs must be lists.") + return Status.FAILURE + + + # 1a. Switch Controllers + self._current_status = ActionExecutionStatus.SWITCHING_CONTROLLERS + self.blackboard_set("action_status", self._current_status.value) + self.logger.info(f"[{self.name}] Requesting controller switch: start={controllers_to_activate}, stop={controllers_to_deactivate}") + try: + # Assuming switch_controllers blocks or returns success/failure quickly + self.controller_switcher.switch_controllers( + activate_controllers=controllers_to_activate, + deactivate_controllers=controllers_to_deactivate, + ) + except Exception as sw_e: + self.logger.error(f"[{self.name}] Error during controller switch: {sw_e}") + self._current_status = ActionExecutionStatus.FAILED + self.blackboard_set("action_status", self._current_status.value) + return Status.FAILURE + + + # 1b. Construct Goal Message + goal_msg = FollowJointTrajectory.Goal() + # Important: Make sure the trajectory has correct joint names for the action server + goal_msg.trajectory = trajectory + # Optional: Add goal time tolerance, path tolerance if needed + # goal_msg.goal_time_tolerance = Duration(sec=1, nanosec=0).to_msg() + + + # 1c. Send Goal Asynchronously + self.logger.info(f"[{self.name}] Sending trajectory goal to action server...") + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._current_status = ActionExecutionStatus.SENDING_GOAL + self.blackboard_set("action_status", self._current_status.value) + return Status.RUNNING + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error: {e}") + return Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Error preparing to send goal: {e}") + return Status.FAILURE + + # --- State 2: Waiting for Goal Acceptance --- + if self._current_status == ActionExecutionStatus.SENDING_GOAL: + if self._send_goal_future is None: return self._handle_invalid_state("SENDING_GOAL future None") # Should not happen + + if self._send_goal_future.done(): + try: + goal_handle: ClientGoalHandle = self._send_goal_future.result() + except Exception as e: + self.logger.error(f"[{self.name}] Exception getting goal handle from future: {e}") + return self._handle_failure("Exception getting goal handle") + + self._send_goal_future = None # Clear future + + if not goal_handle.accepted: + self.logger.warning(f"[{self.name}] Goal rejected by action server.") + self.blackboard_set("action_goal_accepted", False) + return self._handle_failure("Goal rejected", ActionExecutionStatus.GOAL_REJECTED) + else: + # Convert UUID numpy array to hex string for logging + try: + # Access the uuid field (numpy array of uint8) + uuid_array = goal_handle.goal_id.uuid + # Convert the numpy array to standard Python bytes + uuid_bytes = bytes(uuid_array) + # Convert bytes to a hex string + uuid_hex = uuid_bytes.hex() + except Exception as log_e: + # Fallback in case something goes wrong with conversion + self.logger.warning(f"[{self.name}] Could not format goal ID UUID for logging: {log_e}") + uuid_hex = "[Error Formatting UUID]" + + self.logger.info(f"[{self.name}] Goal accepted by action server (ID: {uuid_hex}).") + self._goal_handle = goal_handle + self.blackboard_set("action_goal_accepted", True) + self._get_result_future = self._goal_handle.get_result_async() + self._current_status = ActionExecutionStatus.EXECUTING # Or WAITING_FOR_RESULT if preferred + self.blackboard_set("action_status", self._current_status.value) + return Status.RUNNING + else: + # Future not done yet + return Status.RUNNING + + # --- State 3: Waiting for Result / Monitoring Execution --- + if self._current_status == ActionExecutionStatus.EXECUTING or self._current_status == ActionExecutionStatus.WAITING_FOR_RESULT : + if self._get_result_future is None or self._goal_handle is None: + return self._handle_invalid_state("EXECUTING/WAITING future or handle None") + + # Check goal handle status + status = self._goal_handle.status + if status == GoalStatus.STATUS_EXECUTING: + self._current_status = ActionExecutionStatus.EXECUTING # Ensure state is correct + # Optional: Process feedback if needed + # self.logger.debug(f"[{self.name}] Goal executing...") + elif status == GoalStatus.STATUS_ABORTED: + self.logger.warning(f"[{self.name}] Goal aborted by server.") + # Result future should complete soon, wait for it + self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT # Move to check result + elif status == GoalStatus.STATUS_CANCELED: + self.logger.warning(f"[{self.name}] Goal canceled.") + self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT + elif status == GoalStatus.STATUS_SUCCEEDED: + self.logger.info(f"[{self.name}] Goal status reported SUCCEEDED.") + self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT + + # Now check if the result future is done + if self._get_result_future.done(): + try: + result_wrapper = self._get_result_future.result() + action_result = result_wrapper.result + self._result_code = action_result.error_code + result_string = action_result.error_string or "None" + self.logger.info(f"[{self.name}] Action result received: Code={self._result_code}, Msg='{result_string}'") + self.blackboard_set("action_result_code", self._result_code) + + # Final state determination + if self._result_code == FollowJointTrajectory.Result.SUCCESSFUL: + return self._handle_success() + elif status == GoalStatus.STATUS_CANCELED: # Check status again if needed + return self._handle_failure("Goal Canceled", ActionExecutionStatus.GOAL_CANCELLED) + else: # Any other error code implies failure + return self._handle_failure(f"Action Failed: Code={self._result_code}, Msg='{result_string}'", ActionExecutionStatus.ABORTED if status == GoalStatus.STATUS_ABORTED else ActionExecutionStatus.FAILED) + + except Exception as e: + self.logger.error(f"[{self.name}] Exception getting result from future: {e}") + return self._handle_failure("Exception getting result") + else: + # Result future not done yet, goal still active (or transitioning) + self.blackboard_set("action_status", self._current_status.value) + return Status.RUNNING + + # --- Handle Terminal States (Should be reached via returns above) --- + if self._current_status == ActionExecutionStatus.SUCCEEDED: return Status.SUCCESS + if self._current_status in [ActionExecutionStatus.FAILED, ActionExecutionStatus.GOAL_REJECTED, ActionExecutionStatus.GOAL_CANCELLED, ActionExecutionStatus.ABORTED]: return Status.FAILURE + + # Fallback if state is somehow invalid + return self._handle_invalid_state(f"Unhandled status {self._current_status}") + + + @override + def terminate(self, new_status: Status) -> None: + """Cancel active goal if behavior is terminated unexpectedly.""" + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") + # If terminated INTERNALLY (SUCCESS/FAILURE), goal is already finished or failed. + # If terminated EXTERNALLY (INVALID), cancel the goal if it's active. + if new_status == Status.INVALID and self._goal_handle is not None: + status = self._goal_handle.status + if status == GoalStatus.STATUS_ACCEPTED or status == GoalStatus.STATUS_EXECUTING: + self.logger.warning(f"[{self.name}] Behavior terminated externally while action goal was active (Status: {GoalStatus(status).name}). Requesting cancellation.") + cancel_future = self._goal_handle.cancel_goal_async() + # Optional: Spin briefly/add callback to ensure cancel sent? Usually not needed. + # else: + # self.logger.debug(f"[{self.name}] Goal handle exists but not in active state ({GoalStatus(status).name}) on termination.") + # else: + # self.logger.debug(f"[{self.name}] No active goal handle or clean termination, no cancellation needed.") + + # Reset internal state variables regardless + self._send_goal_future = None + self._get_result_future = None + self._goal_handle = None + # Don't reset self._current_status here, it might be SUCCEEDED/FAILED from last update + # self._current_status = ActionExecutionStatus.IDLE # Resetting might hide final state + + # --- Helper methods for state transitions --- + def _handle_success(self) -> Status: + self._current_status = ActionExecutionStatus.SUCCEEDED + self.blackboard_set("action_status", self._current_status.value) + self._goal_handle = None + self._get_result_future = None + return Status.SUCCESS + + def _handle_failure(self, reason: str, final_status: ActionExecutionStatus = ActionExecutionStatus.FAILED) -> Status: + self.logger.error(f"[{self.name}] Failure: {reason}") + self._current_status = final_status + self.blackboard_set("action_status", self._current_status.value) + # Set result code if available and not already set failure code + if self._result_code is not None and self._result_code != FollowJointTrajectory.Result.SUCCESSFUL: + self.blackboard_set("action_result_code", self._result_code) + elif self._current_status == ActionExecutionStatus.GOAL_REJECTED: + # Use a convention for rejected? + self.blackboard_set("action_result_code", -998) # Example arbitrary code + else: # General failure + self.blackboard_set("action_result_code", -999) # Example arbitrary code + + self._goal_handle = None + self._get_result_future = None + self._send_goal_future = None # Ensure this is cleared too + return Status.FAILURE + + def _handle_invalid_state(self, message: str) -> Status: + self.logger.error(f"[{self.name}] Reached invalid state: {message}") + return self._handle_failure(f"Invalid internal state: {message}") From 5432513b9fdf46e65376441a91294713ca1a287f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Apr 2025 12:14:27 -0700 Subject: [PATCH 029/238] Execute Articutool trajectory in move above behavior --- .../ada_feeding/trees/acquire_food_tree.py | 15 ++++++++++++++- 1 file changed, 14 insertions(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index f460a818..b10bcf51 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -50,6 +50,7 @@ from ada_feeding.behaviors.state import GetJointStates, ExtractJointsFromState from ada_feeding.behaviors.ros.msgs import StampPoseFromPose from ada_feeding.behaviors.ros.tf import ApplyTransform +from ada_feeding.behaviors.articutool.execute_articutool_trajectory import ExecuteArticutoolTrajectory from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( pre_moveto_config, @@ -862,7 +863,7 @@ def move_above_plan( ], ), MoveIt2Execute( - name="MoveAbove", + name="MoveAboveJacoArm", ns=name, inputs={ "trajectory": BlackboardKey( @@ -872,6 +873,18 @@ def move_above_plan( }, outputs={}, ), + ExecuteArticutoolTrajectory( + name="MoveAboveArticutool", + ns=name, + inputs={ + "trajectory": BlackboardKey("move_above_articutool_trajectory"), + }, + outputs={ + "action_goal_accepted": BlackboardKey("tool_goal_accepted"), + "action_result_code": BlackboardKey("tool_exec_result_code"), + "action_status": BlackboardKey("tool_action_status"), + } + ), # # If Anything goes wrong, reset FT to safe levels # scoped_behavior( # name="SafeFTPreempt", From 136ddff207db715a095f5e10a98e5a1f155728eb Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Apr 2025 16:41:14 -0700 Subject: [PATCH 030/238] Pre-compute move above plan for Jaco arm and Articutool --- .../ada_feeding/trees/acquire_food_tree.py | 918 +++++++++--------- 1 file changed, 470 insertions(+), 448 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index b10bcf51..b6089166 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -589,24 +589,7 @@ def move_above_plan( "constraints": BlackboardKey("move_above_articutool_constraints"), } ), - # ### Move Above Food - # MoveIt2PoseConstraint( - # name="MoveAbovePose", - # ns=name, - # inputs={ - # "pose": BlackboardKey("move_above_pose"), - # "frame_id": "food", - # "tolerance_orientation": [ - # 0.01, - # 0.01, - # 0.01, - # ], # x, y, z rotvec - # "parameterization": 1, - # }, - # outputs={ - # "constraints": BlackboardKey("goal_constraints"), - # }, - # ), + ### Move Above Food py_trees.decorators.Timeout( name="MoveAboveJacoArmPlanTimeout", # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such @@ -649,116 +632,155 @@ def move_above_plan( }, ), ), - # # Convert Pose to PoseStamped using the defined frame_id - # StampPoseFromPose( - # name="StampMoveIntoPose", - # ns=name, - # inputs={ - # "input_pose": BlackboardKey("move_into_pose"), - # "frame_id": "food" - # }, - # outputs={ - # "output_pose_stamped": BlackboardKey("move_into_pose_stamped_food_frame") - # } - # ), - # - # # Use ApplyTransform to transform into the MoveIt Planning Frame - # ApplyTransform( - # name="TransformPoseToIKFrame", - # ns=name, - # inputs={ - # "stamped_msg": BlackboardKey("move_into_pose_stamped_food_frame"), - # "target_frame": "j2n6s200_link_base", - # }, - # outputs={ - # "transformed_msg": BlackboardKey("move_into_pose_stamped_base_frame") - # } - # ), - # - # # Compute IK for the target pose using the full jaco_arm_with_articutool planning group - # MoveIt2ComputeIK( - # name="ComputeJacoArmWithArticutoolIK", - # ns=name, - # inputs={ - # "target_pose": BlackboardKey("move_into_pose_stamped_base_frame"), - # "group_name": "jaco_arm_with_articutool", - # # "start_joint_state": BlackboardKey("current_joint_positions"), - # }, - # outputs={ - # "ik_solution_joint_state": BlackboardKey("move_into_ik_solution"), - # "success": BlackboardKey("move_into_ik_success"), - # } - # ), - # ExtractJointsFromState( - # name="ExtractJacoArmJoints", - # ns=name, - # inputs={ - # "source_joint_state": BlackboardKey("move_into_ik_solution"), - # "target_joint_names": [ - # "j2n6s200_joint_1", - # "j2n6s200_joint_2", - # "j2n6s200_joint_3", - # "j2n6s200_joint_4", - # "j2n6s200_joint_5", - # "j2n6s200_joint_6", - # ] - # - # }, - # outputs={ - # "output_joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), - # "output_joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), - # "success": BlackboardKey("extract_jaco_arm_joints_success") - # } - # ), - # MoveIt2JointConstraint( - # name="SetJacoArmJointConstraint", - # ns=name, - # inputs={ - # "joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), - # "joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), - # "tolerance": 0.001, - # "constraints": None, - # }, - # outputs={ - # "constraints": BlackboardKey("move_into_jaco_arm_constraints"), - # } - # ), - # ### Test MoveIntoFood - # MoveIt2PoseConstraint( - # name="MoveIntoPose", - # ns=name, - # inputs={ - # "pose": BlackboardKey("move_into_pose"), - # "frame_id": "food", - # }, - # outputs={ - # "constraints": BlackboardKey("goal_constraints"), - # }, - # ), - # py_trees.decorators.Timeout( - # name="MoveIntoPlanTimeout", - # # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such - # duration=10.0 * self.allowed_planning_time_for_move_into, - # child=MoveIt2Plan( - # name="MoveIntoPlan", - # ns=name, - # inputs={ - # "goal_constraints": BlackboardKey("move_into_jaco_arm_constraints"), - # "max_velocity_scale": self.max_velocity_scaling_move_into, - # "max_acceleration_scale": self.max_acceleration_scaling_move_into, - # "cartesian": True, - # "cartesian_max_step": 0.001, - # "cartesian_fraction_threshold": 0.92, - # "start_joint_state": BlackboardKey("test_into_joints"), - # "max_path_len_joint": max_path_len_joint, - # "allowed_planning_time": self.allowed_planning_time_for_move_into, - # "group_name": "jaco_arm", - # }, - # outputs={ - # "trajectory": BlackboardKey("move_into_trajectory") - # }, - # ), - # ), + # Convert Pose to PoseStamped using the defined frame_id + StampPoseFromPose( + name="StampMoveIntoPose", + ns=name, + inputs={ + "input_pose": BlackboardKey("move_into_pose"), + "frame_id": "food" + }, + outputs={ + "output_pose_stamped": BlackboardKey("move_into_pose_stamped_food_frame") + } + ), + + # Use ApplyTransform to transform into the MoveIt Planning Frame + ApplyTransform( + name="TransformPoseToIKFrame", + ns=name, + inputs={ + "stamped_msg": BlackboardKey("move_into_pose_stamped_food_frame"), + "target_frame": "j2n6s200_link_base", + }, + outputs={ + "transformed_msg": BlackboardKey("move_into_pose_stamped_base_frame") + } + ), + + # Compute IK for the target pose using the full jaco_arm_with_articutool planning group + MoveIt2ComputeIK( + name="ComputeJacoArmWithArticutoolIK", + ns=name, + inputs={ + "target_pose": BlackboardKey("move_into_pose_stamped_base_frame"), + "group_name": "jaco_arm_with_articutool", + # "start_joint_state": BlackboardKey("current_joint_positions"), + }, + outputs={ + "ik_solution_joint_state": BlackboardKey("move_into_ik_solution"), + "success": BlackboardKey("move_into_ik_success"), + } + ), + ExtractJointsFromState( + name="ExtractJacoArmJoints", + ns=name, + inputs={ + "source_joint_state": BlackboardKey("move_into_ik_solution"), + "target_joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ] + + }, + outputs={ + "output_joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), + "output_joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), + "success": BlackboardKey("extract_jaco_arm_joints_success") + } + ), + ExtractJointsFromState( + name="ExtractArticutoolJoints", + ns=name, + inputs={ + "source_joint_state": BlackboardKey("move_into_ik_solution"), + "target_joint_names": [ + "atool_joint1", + "atool_joint2", + ] + + }, + outputs={ + "output_joint_names": BlackboardKey("move_into_articutool_joint_names"), + "output_joint_positions": BlackboardKey("move_into_articutool_joint_positions"), + "success": BlackboardKey("extract_articutool_joints_success") + } + ), + MoveIt2JointConstraint( + name="SetJacoArmJointConstraint", + ns=name, + inputs={ + "joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), + "joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), + "tolerance": 0.001, + "constraints": None, + }, + outputs={ + "constraints": BlackboardKey("move_into_jaco_arm_constraints"), + } + ), + MoveIt2JointConstraint( + name="SetArticutoolJointConstraint", + ns=name, + inputs={ + "joint_positions": BlackboardKey("move_into_articutool_joint_positions"), + "joint_names": BlackboardKey("move_into_articutool_joint_names"), + "tolerance": 0.001, + "constraints": None, + }, + outputs={ + "constraints": BlackboardKey("move_into_articutool_constraints"), + } + ), + ### Test MoveIntoFood + py_trees.decorators.Timeout( + name="MoveIntoJacoArmPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 * self.allowed_planning_time_for_move_into, + child=MoveIt2Plan( + name="MoveIntoJacoArmPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey("move_into_jaco_arm_constraints"), + "max_velocity_scale": self.max_velocity_scaling_move_into, + "max_acceleration_scale": self.max_acceleration_scaling_move_into, + "cartesian": True, + "cartesian_max_step": 0.001, + "cartesian_fraction_threshold": 0.92, + "start_joint_state": BlackboardKey("test_into_joints"), + "max_path_len_joint": max_path_len_joint, + "allowed_planning_time": self.allowed_planning_time_for_move_into, + "group_name": "jaco_arm", + }, + outputs={ + "trajectory": BlackboardKey("move_into_jaco_arm_trajectory") + }, + ), + ), + py_trees.decorators.Timeout( + name="MoveIntoArticutoolPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 * self.allowed_planning_time_for_move_above, + child=MoveIt2Plan( + name="MoveIntoArticutoolPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey("move_into_articutool_constraints"), + "max_velocity_scale": self.max_velocity_scaling_move_above, + "max_acceleration_scale": self.max_acceleration_scaling_move_above, + "allowed_planning_time": self.allowed_planning_time_for_move_above, + "max_path_len_joint": max_path_len_joint, + "group_name": "articutool", + }, + outputs={ + "trajectory": BlackboardKey("move_into_articutool_trajectory"), + }, + ), + ), ], ) @@ -885,326 +907,326 @@ def move_above_plan( "action_status": BlackboardKey("tool_action_status"), } ), - # # If Anything goes wrong, reset FT to safe levels - # scoped_behavior( - # name="SafeFTPreempt", - # # Set Approach F/T Thresh - # 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")] - # ), - # response_checks=[ - # py_trees.common.ComparisonExpression( - # variable=Blackboard.separator.join( - # [ - # name, - # BlackboardKey( - # "ft_response" - # ), - # ] - # ), - # value=SetParameters.Response(), # Unused - # operator=set_parameter_response_all_success, - # ) - # ], - # ), - # ], - # ), - # post_behavior=py_trees.composites.Sequence( - # name=name, - # memory=True, - # children=[ - # pre_moveto_config( - # name="PostAcquireFTSet", re_tare=False - # ), - # ], - # ), - # on_preempt_timeout=5.0, - # # Starts a new Sequence w/ Memory internally - # workers=[ - # ### Move Into Food - # MoveIt2PoseConstraint( - # name="MoveIntoPose", - # ns=name, - # inputs={ - # "pose": BlackboardKey("move_into_pose"), - # "frame_id": "food", - # }, - # outputs={ - # "constraints": BlackboardKey( - # "goal_constraints" - # ), - # }, - # ), - # # If this fails - # # Auto-fallback to precomputed MoveInto - # # From move_above_plan() - # py_trees.decorators.FailureIsSuccess( - # name="MoveIntoPlanFallbackPrecomputed", - # child=py_trees.decorators.Timeout( - # name="MoveIntoPlanTimeout", - # # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such - # duration=10.0 - # * self.allowed_planning_time_for_move_into, - # child=MoveIt2Plan( - # name="MoveIntoPlan", - # ns=name, - # inputs={ - # "goal_constraints": BlackboardKey( - # "goal_constraints" - # ), - # "max_velocity_scale": self.max_velocity_scaling_move_into, - # "max_acceleration_scale": self.max_acceleration_scaling_move_into, - # "cartesian": True, - # "cartesian_max_step": 0.001, - # "cartesian_fraction_threshold": 0.92, - # "max_path_len_joint": max_path_len_joint, - # "allowed_planning_time": self.allowed_planning_time_for_move_into, - # }, - # outputs={ - # "trajectory": BlackboardKey( - # "move_into_trajectory" - # ) - # }, - # ), - # ), - # ), - # # MoveInto expect F/T failure - # py_trees.decorators.FailureIsSuccess( - # name="MoveIntoExecuteSucceed", - # child=MoveIt2Execute( - # name="MoveInto", - # ns=name, - # inputs={ - # "trajectory": BlackboardKey( - # "move_into_trajectory" - # ) - # }, - # outputs={}, - # ), - # ), - # ### 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, - # servo_controller_name="jaco_arm_cartesian_controller", - # start_moveit_servo=False, - # ) - # .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_cartesian_controller_parameters", - # ), - # StopServoTree( - # self._node, - # servo_controller_name="jaco_arm_cartesian_controller", - # stop_moveit_servo=False, - # ) - # .create_tree(name="StopServoScoped") - # .root, - # ], - # ), - # on_preempt_timeout=5.0, - # # Starts a new Sequence w/ Memory internally - # workers=[ - # py_trees.composites.Selector( - # name="InFoodErrorSelector", - # memory=True, - # children=[ - # py_trees.composites.Sequence( - # name="InFoodGraspExtract", - # memory=True, - # children=[ - # ### Grasp - # retry_call_ros_service( - # name="GraspFTThresh", - # service_type=SetParameters, - # service_name="~/set_cartesian_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, - # ) - # ], - # ), - # 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" - # ), - # "pub_topic": "~/cartesian_twist_cmds", - # "servo_status_sub_topic": None, - # }, - # ), # Auto Zero-Twist on terminate() - # ### Extraction - # ComputeActionTwist( - # name="ComputeExtract", - # 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_cartesian_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" - # ), - # "pub_topic": "~/cartesian_twist_cmds", - # "servo_status_sub_topic": None, - # }, - # ), # Auto Zero-Twist on terminate() - # ft_thresh_satisfied( - # name="CheckFTForkOffPlate" - # ), - # ], # End InFoodGraspExtract.children - # ), # End InFoodGraspExtract - # recovery_tree, - # ], # End InFoodErrorSelector.children - # ), # End InFoodErrorSelector - # ], # End MoveIt2Servo.workers - # ), # End MoveIt2Servo - # ], # End SafeFTPreempt.workers - # ), # End SafeFTPreempt + # If Anything goes wrong, reset FT to safe levels + scoped_behavior( + name="SafeFTPreempt", + # Set Approach F/T Thresh + 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")] + ), + response_checks=[ + py_trees.common.ComparisonExpression( + variable=Blackboard.separator.join( + [ + name, + BlackboardKey( + "ft_response" + ), + ] + ), + value=SetParameters.Response(), # Unused + operator=set_parameter_response_all_success, + ) + ], + ), + ], + ), + post_behavior=py_trees.composites.Sequence( + name=name, + memory=True, + children=[ + pre_moveto_config( + name="PostAcquireFTSet", re_tare=False + ), + ], + ), + on_preempt_timeout=5.0, + # Starts a new Sequence w/ Memory internally + workers=[ + ### Move Into Food + MoveIt2PoseConstraint( + name="MoveIntoPose", + ns=name, + inputs={ + "pose": BlackboardKey("move_into_pose"), + "frame_id": "food", + }, + outputs={ + "constraints": BlackboardKey( + "goal_constraints" + ), + }, + ), + # If this fails + # Auto-fallback to precomputed MoveInto + # From move_above_plan() + py_trees.decorators.FailureIsSuccess( + name="MoveIntoPlanFallbackPrecomputed", + child=py_trees.decorators.Timeout( + name="MoveIntoPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 + * self.allowed_planning_time_for_move_into, + child=MoveIt2Plan( + name="MoveIntoPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey( + "goal_constraints" + ), + "max_velocity_scale": self.max_velocity_scaling_move_into, + "max_acceleration_scale": self.max_acceleration_scaling_move_into, + "cartesian": True, + "cartesian_max_step": 0.001, + "cartesian_fraction_threshold": 0.92, + "max_path_len_joint": max_path_len_joint, + "allowed_planning_time": self.allowed_planning_time_for_move_into, + }, + outputs={ + "trajectory": BlackboardKey( + "move_into_trajectory" + ) + }, + ), + ), + ), + # MoveInto expect F/T failure + py_trees.decorators.FailureIsSuccess( + name="MoveIntoExecuteSucceed", + child=MoveIt2Execute( + name="MoveInto", + ns=name, + inputs={ + "trajectory": BlackboardKey( + "move_into_trajectory" + ) + }, + outputs={}, + ), + ), + ### 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, + servo_controller_name="jaco_arm_cartesian_controller", + start_moveit_servo=False, + ) + .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_cartesian_controller_parameters", + ), + StopServoTree( + self._node, + servo_controller_name="jaco_arm_cartesian_controller", + stop_moveit_servo=False, + ) + .create_tree(name="StopServoScoped") + .root, + ], + ), + on_preempt_timeout=5.0, + # Starts a new Sequence w/ Memory internally + workers=[ + py_trees.composites.Selector( + name="InFoodErrorSelector", + memory=True, + children=[ + py_trees.composites.Sequence( + name="InFoodGraspExtract", + memory=True, + children=[ + ### Grasp + retry_call_ros_service( + name="GraspFTThresh", + service_type=SetParameters, + service_name="~/set_cartesian_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, + ) + ], + ), + 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" + ), + "pub_topic": "~/cartesian_twist_cmds", + "servo_status_sub_topic": None, + }, + ), # Auto Zero-Twist on terminate() + ### Extraction + ComputeActionTwist( + name="ComputeExtract", + 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_cartesian_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" + ), + "pub_topic": "~/cartesian_twist_cmds", + "servo_status_sub_topic": None, + }, + ), # Auto Zero-Twist on terminate() + ft_thresh_satisfied( + name="CheckFTForkOffPlate" + ), + ], # End InFoodGraspExtract.children + ), # End InFoodGraspExtract + recovery_tree, + ], # End InFoodErrorSelector.children + ), # End InFoodErrorSelector + ], # End MoveIt2Servo.workers + ), # End MoveIt2Servo + ], # End SafeFTPreempt.workers + ), # End SafeFTPreempt ], # End OctomapAndTableCollision.workers ), # OctomapAndTableCollision ] From a411bf2316856cc1317e28cef6fec21e8e6c59f3 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Apr 2025 19:30:00 -0700 Subject: [PATCH 031/238] (WIP) Switch back to the previous pose constraint for the move into configuration. We cannot simply define a non-cartesian plan since the motion of the tool_tip frame in cartesian space will not be linear. TODO: Combine the test_into configurations to use as the start_joint_state for the cartesian plan --- .../ada_feeding/trees/acquire_food_tree.py | 915 +++++++++--------- 1 file changed, 484 insertions(+), 431 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index b6089166..80e817da 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -607,7 +607,7 @@ def move_above_plan( }, outputs={ "trajectory": BlackboardKey("move_above_jaco_arm_trajectory"), - "end_joint_state": BlackboardKey("test_into_joints"), + "end_joint_state": BlackboardKey("test_into_jaco_arm_joints"), }, ), ), @@ -628,156 +628,69 @@ def move_above_plan( }, outputs={ "trajectory": BlackboardKey("move_above_articutool_trajectory"), - "end_joint_state": BlackboardKey("test_into_joints"), + "end_joint_state": BlackboardKey("test_into_articutool_joints"), }, ), ), - # Convert Pose to PoseStamped using the defined frame_id - StampPoseFromPose( - name="StampMoveIntoPose", - ns=name, - inputs={ - "input_pose": BlackboardKey("move_into_pose"), - "frame_id": "food" - }, - outputs={ - "output_pose_stamped": BlackboardKey("move_into_pose_stamped_food_frame") - } - ), - - # Use ApplyTransform to transform into the MoveIt Planning Frame - ApplyTransform( - name="TransformPoseToIKFrame", - ns=name, - inputs={ - "stamped_msg": BlackboardKey("move_into_pose_stamped_food_frame"), - "target_frame": "j2n6s200_link_base", - }, - outputs={ - "transformed_msg": BlackboardKey("move_into_pose_stamped_base_frame") - } - ), - - # Compute IK for the target pose using the full jaco_arm_with_articutool planning group - MoveIt2ComputeIK( - name="ComputeJacoArmWithArticutoolIK", + ### Test MoveIntoFood + MoveIt2PoseConstraint( + name="MoveIntoJacoArmWithArticutoolPose", ns=name, inputs={ - "target_pose": BlackboardKey("move_into_pose_stamped_base_frame"), - "group_name": "jaco_arm_with_articutool", - # "start_joint_state": BlackboardKey("current_joint_positions"), + "pose": BlackboardKey("move_into_pose"), + "frame_id": "food", }, outputs={ - "ik_solution_joint_state": BlackboardKey("move_into_ik_solution"), - "success": BlackboardKey("move_into_ik_success"), - } - ), - ExtractJointsFromState( - name="ExtractJacoArmJoints", - ns=name, - inputs={ - "source_joint_state": BlackboardKey("move_into_ik_solution"), - "target_joint_names": [ - "j2n6s200_joint_1", - "j2n6s200_joint_2", - "j2n6s200_joint_3", - "j2n6s200_joint_4", - "j2n6s200_joint_5", - "j2n6s200_joint_6", - ] - + "constraints": BlackboardKey("move_into_goal_constraints"), }, - outputs={ - "output_joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), - "output_joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), - "success": BlackboardKey("extract_jaco_arm_joints_success") - } ), - ExtractJointsFromState( - name="ExtractArticutoolJoints", + GetJointStates( + name="GetArticutoolJoints", + node=self._node, ns=name, inputs={ - "source_joint_state": BlackboardKey("move_into_ik_solution"), - "target_joint_names": [ - "atool_joint1", - "atool_joint2", - ] - + "joint_names": ["atool_joint1", "atool_joint2"], }, outputs={ - "output_joint_names": BlackboardKey("move_into_articutool_joint_names"), - "output_joint_positions": BlackboardKey("move_into_articutool_joint_positions"), - "success": BlackboardKey("extract_articutool_joints_success") + "joint_names": BlackboardKey("articutool_joint_names"), + "joint_positions": BlackboardKey("articutool_joint_positions"), } ), MoveIt2JointConstraint( name="SetJacoArmJointConstraint", ns=name, inputs={ - "joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), - "joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), + "joint_positions": BlackboardKey("articutool_joint_positions"), + "joint_names": BlackboardKey("articutool_joint_names"), "tolerance": 0.001, "constraints": None, }, outputs={ - "constraints": BlackboardKey("move_into_jaco_arm_constraints"), + "constraints": BlackboardKey("move_into_path_constraints"), } ), - MoveIt2JointConstraint( - name="SetArticutoolJointConstraint", - ns=name, - inputs={ - "joint_positions": BlackboardKey("move_into_articutool_joint_positions"), - "joint_names": BlackboardKey("move_into_articutool_joint_names"), - "tolerance": 0.001, - "constraints": None, - }, - outputs={ - "constraints": BlackboardKey("move_into_articutool_constraints"), - } - ), - ### Test MoveIntoFood py_trees.decorators.Timeout( - name="MoveIntoJacoArmPlanTimeout", + name="MoveIntoJacoArmWithArticutoolPlanTimeout", # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such duration=10.0 * self.allowed_planning_time_for_move_into, child=MoveIt2Plan( - name="MoveIntoJacoArmPlan", + name="MoveIntoJacoArmWithArticutoolPlan", ns=name, inputs={ - "goal_constraints": BlackboardKey("move_into_jaco_arm_constraints"), + "goal_constraints": BlackboardKey("move_into_goal_constraints"), + "path_constraints": BlackboardKey("move_into_path_constraints"), "max_velocity_scale": self.max_velocity_scaling_move_into, "max_acceleration_scale": self.max_acceleration_scaling_move_into, "cartesian": True, "cartesian_max_step": 0.001, "cartesian_fraction_threshold": 0.92, - "start_joint_state": BlackboardKey("test_into_joints"), + # "start_joint_state": BlackboardKey("test_into_jaco_arm_with_articutool_joints"), "max_path_len_joint": max_path_len_joint, "allowed_planning_time": self.allowed_planning_time_for_move_into, - "group_name": "jaco_arm", - }, - outputs={ - "trajectory": BlackboardKey("move_into_jaco_arm_trajectory") - }, - ), - ), - py_trees.decorators.Timeout( - name="MoveIntoArticutoolPlanTimeout", - # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such - duration=10.0 * self.allowed_planning_time_for_move_above, - child=MoveIt2Plan( - name="MoveIntoArticutoolPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey("move_into_articutool_constraints"), - "max_velocity_scale": self.max_velocity_scaling_move_above, - "max_acceleration_scale": self.max_acceleration_scaling_move_above, - "allowed_planning_time": self.allowed_planning_time_for_move_above, - "max_path_len_joint": max_path_len_joint, - "group_name": "articutool", + "group_name": "jaco_arm_with_articutool", }, outputs={ - "trajectory": BlackboardKey("move_into_articutool_trajectory"), + "trajectory": BlackboardKey("move_into_jaco_arm_with_articutool_trajectory") }, ), ), @@ -908,329 +821,469 @@ def move_above_plan( } ), # If Anything goes wrong, reset FT to safe levels - scoped_behavior( - name="SafeFTPreempt", - # Set Approach F/T Thresh - 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")] - ), - response_checks=[ - py_trees.common.ComparisonExpression( - variable=Blackboard.separator.join( - [ - name, - BlackboardKey( - "ft_response" - ), - ] - ), - value=SetParameters.Response(), # Unused - operator=set_parameter_response_all_success, - ) - ], - ), - ], - ), - post_behavior=py_trees.composites.Sequence( - name=name, - memory=True, - children=[ - pre_moveto_config( - name="PostAcquireFTSet", re_tare=False - ), - ], - ), - on_preempt_timeout=5.0, - # Starts a new Sequence w/ Memory internally - workers=[ - ### Move Into Food - MoveIt2PoseConstraint( - name="MoveIntoPose", - ns=name, - inputs={ - "pose": BlackboardKey("move_into_pose"), - "frame_id": "food", - }, - outputs={ - "constraints": BlackboardKey( - "goal_constraints" - ), - }, - ), - # If this fails - # Auto-fallback to precomputed MoveInto - # From move_above_plan() - py_trees.decorators.FailureIsSuccess( - name="MoveIntoPlanFallbackPrecomputed", - child=py_trees.decorators.Timeout( - name="MoveIntoPlanTimeout", - # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such - duration=10.0 - * self.allowed_planning_time_for_move_into, - child=MoveIt2Plan( - name="MoveIntoPlan", - ns=name, - inputs={ - "goal_constraints": BlackboardKey( - "goal_constraints" - ), - "max_velocity_scale": self.max_velocity_scaling_move_into, - "max_acceleration_scale": self.max_acceleration_scaling_move_into, - "cartesian": True, - "cartesian_max_step": 0.001, - "cartesian_fraction_threshold": 0.92, - "max_path_len_joint": max_path_len_joint, - "allowed_planning_time": self.allowed_planning_time_for_move_into, - }, - outputs={ - "trajectory": BlackboardKey( - "move_into_trajectory" - ) - }, - ), - ), - ), - # MoveInto expect F/T failure - py_trees.decorators.FailureIsSuccess( - name="MoveIntoExecuteSucceed", - child=MoveIt2Execute( - name="MoveInto", - ns=name, - inputs={ - "trajectory": BlackboardKey( - "move_into_trajectory" - ) - }, - outputs={}, - ), - ), - ### 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, - servo_controller_name="jaco_arm_cartesian_controller", - start_moveit_servo=False, - ) - .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_cartesian_controller_parameters", - ), - StopServoTree( - self._node, - servo_controller_name="jaco_arm_cartesian_controller", - stop_moveit_servo=False, - ) - .create_tree(name="StopServoScoped") - .root, - ], - ), - on_preempt_timeout=5.0, - # Starts a new Sequence w/ Memory internally - workers=[ - py_trees.composites.Selector( - name="InFoodErrorSelector", - memory=True, - children=[ - py_trees.composites.Sequence( - name="InFoodGraspExtract", - memory=True, - children=[ - ### Grasp - retry_call_ros_service( - name="GraspFTThresh", - service_type=SetParameters, - service_name="~/set_cartesian_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, - ) - ], - ), - 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" - ), - "pub_topic": "~/cartesian_twist_cmds", - "servo_status_sub_topic": None, - }, - ), # Auto Zero-Twist on terminate() - ### Extraction - ComputeActionTwist( - name="ComputeExtract", - 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_cartesian_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" - ), - "pub_topic": "~/cartesian_twist_cmds", - "servo_status_sub_topic": None, - }, - ), # Auto Zero-Twist on terminate() - ft_thresh_satisfied( - name="CheckFTForkOffPlate" - ), - ], # End InFoodGraspExtract.children - ), # End InFoodGraspExtract - recovery_tree, - ], # End InFoodErrorSelector.children - ), # End InFoodErrorSelector - ], # End MoveIt2Servo.workers - ), # End MoveIt2Servo - ], # End SafeFTPreempt.workers - ), # End SafeFTPreempt + # scoped_behavior( + # name="SafeFTPreempt", + # # Set Approach F/T Thresh + # 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")] + # ), + # response_checks=[ + # py_trees.common.ComparisonExpression( + # variable=Blackboard.separator.join( + # [ + # name, + # BlackboardKey( + # "ft_response" + # ), + # ] + # ), + # value=SetParameters.Response(), # Unused + # operator=set_parameter_response_all_success, + # ) + # ], + # ), + # ], + # ), + # post_behavior=py_trees.composites.Sequence( + # name=name, + # memory=True, + # children=[ + # pre_moveto_config( + # name="PostAcquireFTSet", re_tare=False + # ), + # ], + # ), + # on_preempt_timeout=5.0, + # # Starts a new Sequence w/ Memory internally + # workers=[ + # ### Move Into Food + # # Convert Pose to PoseStamped using the defined frame_id + # StampPoseFromPose( + # name="StampMoveIntoPose", + # ns=name, + # inputs={ + # "input_pose": BlackboardKey("move_into_pose"), + # "frame_id": "food" + # }, + # outputs={ + # "output_pose_stamped": BlackboardKey("move_into_pose_stamped_food_frame") + # } + # ), + # + # # Use ApplyTransform to transform into the MoveIt Planning Frame + # ApplyTransform( + # name="TransformPoseToIKFrame", + # ns=name, + # inputs={ + # "stamped_msg": BlackboardKey("move_into_pose_stamped_food_frame"), + # "target_frame": "j2n6s200_link_base", + # }, + # outputs={ + # "transformed_msg": BlackboardKey("move_into_pose_stamped_base_frame") + # } + # ), + # + # # Compute IK for the target pose using the full jaco_arm_with_articutool planning group + # MoveIt2ComputeIK( + # name="ComputeJacoArmWithArticutoolIK", + # ns=name, + # inputs={ + # "target_pose": BlackboardKey("move_into_pose_stamped_base_frame"), + # "group_name": "jaco_arm_with_articutool", + # # "start_joint_state": BlackboardKey("current_joint_positions"), + # }, + # outputs={ + # "ik_solution_joint_state": BlackboardKey("move_into_ik_solution"), + # "success": BlackboardKey("move_into_ik_success"), + # } + # ), + # ExtractJointsFromState( + # name="ExtractJacoArmJoints", + # ns=name, + # inputs={ + # "source_joint_state": BlackboardKey("move_into_ik_solution"), + # "target_joint_names": [ + # "j2n6s200_joint_1", + # "j2n6s200_joint_2", + # "j2n6s200_joint_3", + # "j2n6s200_joint_4", + # "j2n6s200_joint_5", + # "j2n6s200_joint_6", + # ] + # + # }, + # outputs={ + # "output_joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), + # "output_joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), + # "success": BlackboardKey("extract_jaco_arm_joints_success") + # } + # ), + # ExtractJointsFromState( + # name="ExtractArticutoolJoints", + # ns=name, + # inputs={ + # "source_joint_state": BlackboardKey("move_into_ik_solution"), + # "target_joint_names": [ + # "atool_joint1", + # "atool_joint2", + # ] + # + # }, + # outputs={ + # "output_joint_names": BlackboardKey("move_into_articutool_joint_names"), + # "output_joint_positions": BlackboardKey("move_into_articutool_joint_positions"), + # "success": BlackboardKey("extract_articutool_joints_success") + # } + # ), + # MoveIt2JointConstraint( + # name="SetJacoArmJointConstraint", + # ns=name, + # inputs={ + # "joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), + # "joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), + # "tolerance": 0.001, + # "constraints": None, + # }, + # outputs={ + # "constraints": BlackboardKey("move_into_jaco_arm_constraints"), + # } + # ), + # MoveIt2JointConstraint( + # name="SetArticutoolJointConstraint", + # ns=name, + # inputs={ + # "joint_positions": BlackboardKey("move_into_articutool_joint_positions"), + # "joint_names": BlackboardKey("move_into_articutool_joint_names"), + # "tolerance": 0.001, + # "constraints": None, + # }, + # outputs={ + # "constraints": BlackboardKey("move_into_articutool_constraints"), + # } + # ), + # # If this fails + # # Auto-fallback to precomputed MoveInto + # # From move_above_plan() + # py_trees.decorators.FailureIsSuccess( + # name="MoveIntoJacoArmPlanFallbackPrecomputed", + # child=py_trees.decorators.Timeout( + # name="MoveIntoJacoArmPlanTimeout", + # # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + # duration=10.0 * self.allowed_planning_time_for_move_into, + # child=MoveIt2Plan( + # name="MoveIntoJacoArmPlan", + # ns=name, + # inputs={ + # "goal_constraints": BlackboardKey("move_into_jaco_arm_constraints"), + # "max_velocity_scale": self.max_velocity_scaling_move_into, + # "max_acceleration_scale": self.max_acceleration_scaling_move_into, + # "cartesian": True, + # "cartesian_max_step": 0.001, + # "cartesian_fraction_threshold": 0.92, + # "start_joint_state": BlackboardKey("test_into_jaco_arm_joints"), + # "max_path_len_joint": max_path_len_joint, + # "allowed_planning_time": self.allowed_planning_time_for_move_into, + # "group_name": "jaco_arm", + # }, + # outputs={ + # "trajectory": BlackboardKey("move_into_jaco_arm_trajectory") + # }, + # ), + # ), + # ), + # py_trees.decorators.FailureIsSuccess( + # name="MoveIntoArticutoolPlanFallbackPrecomputed", + # child=py_trees.decorators.Timeout( + # name="MoveIntoArticutoolPlanTimeout", + # # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + # duration=10.0 * self.allowed_planning_time_for_move_into, + # child=MoveIt2Plan( + # name="MoveIntoArticutoolPlan", + # ns=name, + # inputs={ + # "goal_constraints": BlackboardKey("move_into_articutool_constraints"), + # "max_velocity_scale": self.max_velocity_scaling_move_into, + # "max_acceleration_scale": self.max_acceleration_scaling_move_into, + # "cartesian": True, + # "cartesian_max_step": 0.001, + # "cartesian_fraction_threshold": 0.92, + # "start_joint_state": BlackboardKey("test_into_articutool_joints"), + # "max_path_len_joint": max_path_len_joint, + # "allowed_planning_time": self.allowed_planning_time_for_move_into, + # "group_name": "articutool", + # }, + # outputs={ + # "trajectory": BlackboardKey("move_into_articutool_trajectory") + # }, + # ), + # ), + # ), + # # MoveInto expect F/T failure + # ExecuteArticutoolTrajectory( + # name="MoveAboveArticutool", + # ns=name, + # inputs={ + # "trajectory": BlackboardKey("move_into_articutool_trajectory"), + # }, + # outputs={ + # "action_goal_accepted": BlackboardKey("tool_goal_accepted"), + # "action_result_code": BlackboardKey("tool_exec_result_code"), + # "action_status": BlackboardKey("tool_action_status"), + # } + # ), + # # py_trees.decorators.FailureIsSuccess( + # # name="MoveIntoArticutoolExecuteSucceed", + # # child=MoveIt2Execute( + # # name="MoveIntoArticutool", + # # ns=name, + # # inputs={ + # # "trajectory": BlackboardKey( + # # "move_into_articutool_trajectory" + # # ) + # # }, + # # outputs={}, + # # ), + # # ), + # py_trees.decorators.FailureIsSuccess( + # name="MoveIntoJacoArmExecuteSucceed", + # child=MoveIt2Execute( + # name="MoveIntoJacoArm", + # ns=name, + # inputs={ + # "trajectory": BlackboardKey( + # "move_into_jaco_arm_trajectory" + # ) + # }, + # outputs={}, + # ), + # ), + # ### 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, + # servo_controller_name="jaco_arm_cartesian_controller", + # start_moveit_servo=False, + # ) + # .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_cartesian_controller_parameters", + # ), + # StopServoTree( + # self._node, + # servo_controller_name="jaco_arm_cartesian_controller", + # stop_moveit_servo=False, + # ) + # .create_tree(name="StopServoScoped") + # .root, + # ], + # ), + # on_preempt_timeout=5.0, + # # Starts a new Sequence w/ Memory internally + # workers=[ + # py_trees.composites.Selector( + # name="InFoodErrorSelector", + # memory=True, + # children=[ + # py_trees.composites.Sequence( + # name="InFoodGraspExtract", + # memory=True, + # children=[ + # ### Grasp + # retry_call_ros_service( + # name="GraspFTThresh", + # service_type=SetParameters, + # service_name="~/set_cartesian_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, + # ) + # ], + # ), + # 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" + # ), + # "pub_topic": "~/cartesian_twist_cmds", + # "servo_status_sub_topic": None, + # }, + # ), # Auto Zero-Twist on terminate() + # ### Extraction + # ComputeActionTwist( + # name="ComputeExtract", + # 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_cartesian_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" + # ), + # "pub_topic": "~/cartesian_twist_cmds", + # "servo_status_sub_topic": None, + # }, + # ), # Auto Zero-Twist on terminate() + # ft_thresh_satisfied( + # name="CheckFTForkOffPlate" + # ), + # ], # End InFoodGraspExtract.children + # ), # End InFoodGraspExtract + # recovery_tree, + # ], # End InFoodErrorSelector.children + # ), # End InFoodErrorSelector + # ], # End MoveIt2Servo.workers + # ), # End MoveIt2Servo + # ], # End SafeFTPreempt.workers + # ), # End SafeFTPreempt ], # End OctomapAndTableCollision.workers ), # OctomapAndTableCollision ] - + resting_position_behaviors, # End Success.workers + # + resting_position_behaviors, # End Success.workers ), # End Success # TableCollision ], # End root_seq.children ) # End root_seq From 5fc1c91b771777b6da86040619d75a6f2a2c8c5a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Apr 2025 19:54:13 -0700 Subject: [PATCH 032/238] Combine Jaco arm and Articutool joint states to pass as start joint state for Cartesian move into plan --- .../ada_feeding/behaviors/state/__init__.py | 3 + .../behaviors/state/combine_joint_states.py | 166 ++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 25 ++- 3 files changed, 192 insertions(+), 2 deletions(-) create mode 100644 ada_feeding/ada_feeding/behaviors/state/combine_joint_states.py diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py index f88485ec..3735f30a 100644 --- a/ada_feeding/ada_feeding/behaviors/state/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -10,3 +10,6 @@ from .extract_joints_from_state import ( ExtractJointsFromState, ) +from .combine_joint_states import ( + CombineJointStates, +) diff --git a/ada_feeding/ada_feeding/behaviors/state/combine_joint_states.py b/ada_feeding/ada_feeding/behaviors/state/combine_joint_states.py new file mode 100644 index 00000000..49b68268 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/combine_joint_states.py @@ -0,0 +1,166 @@ +# -*- coding: utf-8 -*- +# (Add appropriate Copyright/License if desired) + +""" +Defines the CombineJointStates behavior, which merges two JointState +messages into a single one based on a provided full list of joint names. +""" + +# Standard imports +from typing import Union, Optional, Dict, Any, List + +# Third-party imports +from sensor_msgs.msg import JointState +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status +import rclpy.node +import rclpy.time + +# Local imports (adjust paths as needed) +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class CombineJointStates(BlackboardBehavior): + """ + Merges two input JointState messages (e.g., from separate arm and tool plans) + into a single output JointState message, ordered according to a provided + list of all joint names for the combined system. + """ + + def blackboard_inputs( + self, + joint_state_1: Union[BlackboardKey, JointState], + joint_state_2: Union[BlackboardKey, JointState], + full_joint_names: Union[BlackboardKey, List[str]], + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + joint_state_1: BlackboardKey resolving to the first JointState message. + joint_state_2: BlackboardKey resolving to the second JointState message. + full_joint_names: BlackboardKey resolving to the List[str] containing all + joint names for the combined state, in the desired order. + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + combined_joint_state: Optional[BlackboardKey], # -> Optional[JointState] + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + combined_joint_state: BlackboardKey where the resulting merged JointState + message will be written. + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + """Gets the ROS2 node from the arguments passed by the tree runner.""" + # pylint: disable=attribute-defined-outside-init + try: + self.node: rclpy.node.Node = kwargs['node'] + except KeyError as e: + self.logger.error(f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}") + self.node = None + + # initialise is not strictly needed, but good practice + @override + def initialise(self) -> None: + """Optionally log initialization.""" + self.logger.debug(f"[{self.name}] Initializing.") + # Clear previous output + self.blackboard_set("combined_joint_state", None) + + + @override + def update(self) -> Status: + """ + Reads input JointStates, merges them based on full_joint_names, + and writes the combined JointState. + """ + if self.node is None: + self.logger.error(f"[{self.name}] Node object not available. Setup likely failed.") + return Status.FAILURE + + try: + # Read inputs + state1: JointState = self.blackboard_get("joint_state_1") + state2: JointState = self.blackboard_get("joint_state_2") + full_names: List[str] = self.blackboard_get("full_joint_names") + + # --- Input Validation --- + if not isinstance(state1, JointState): + self.logger.error(f"[{self.name}] Input 'joint_state_1' is not a JointState message (is type: {type(state1)}).") + return Status.FAILURE + if not isinstance(state2, JointState): + self.logger.error(f"[{self.name}] Input 'joint_state_2' is not a JointState message (is type: {type(state2)}).") + return Status.FAILURE + if not isinstance(full_names, list) or not all(isinstance(n, str) for n in full_names): + self.logger.error(f"[{self.name}] Input 'full_joint_names' must be a List[str].") + return Status.FAILURE + if not full_names: + self.logger.error(f"[{self.name}] Input 'full_joint_names' cannot be empty.") + return Status.FAILURE + # --- End Validation --- + + + # --- Merge Logic --- + # Create a dictionary to hold combined positions, giving priority to state2 if names overlap + combined_positions_dict = {} + for name, pos in zip(state1.name, state1.position): + combined_positions_dict[name] = pos + for name, pos in zip(state2.name, state2.position): + combined_positions_dict[name] = pos # Overwrites if name was in state1 + + # Create the output JointState + output_state = JointState() + output_state.header.stamp = self.node.get_clock().now().to_msg() # Use current time + output_state.name = full_names # Assign the full ordered list of names + output_state.position = [] # Initialize empty list + + # Populate positions in the order specified by full_names + all_found = True + for name in full_names: + if name in combined_positions_dict: + output_state.position.append(combined_positions_dict[name]) + else: + self.logger.error(f"[{self.name}] Joint '{name}' from 'full_joint_names' not found in either input JointState.") + all_found = False + break # Stop processing + + if not all_found: + return Status.FAILURE + + # Basic check for consistency + if len(output_state.name) != len(output_state.position): + self.logger.error(f"[{self.name}] Mismatch between output names ({len(output_state.name)}) and positions ({len(output_state.position)}). Logic error?") + return Status.FAILURE + # --- End Merge Logic --- + + + # Write the successful result to the blackboard + self.blackboard_set("combined_joint_state", output_state) + self.logger.info(f"[{self.name}] Successfully combined joint states for {len(output_state.name)} joints.") + return Status.SUCCESS + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error: {e}") + return Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Unexpected error combining joint states: {e}", exc_info=True) + return Status.FAILURE diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 80e817da..f58cbcf0 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -47,7 +47,7 @@ ServoMove, ToggleCollisionObject, ) -from ada_feeding.behaviors.state import GetJointStates, ExtractJointsFromState +from ada_feeding.behaviors.state import GetJointStates, ExtractJointsFromState, CombineJointStates from ada_feeding.behaviors.ros.msgs import StampPoseFromPose from ada_feeding.behaviors.ros.tf import ApplyTransform from ada_feeding.behaviors.articutool.execute_articutool_trajectory import ExecuteArticutoolTrajectory @@ -633,6 +633,27 @@ def move_above_plan( ), ), ### Test MoveIntoFood + CombineJointStates( + name="CombineJacoArmAndArticutoolJoints", + ns=name, + inputs={ + "joint_state_1": BlackboardKey("test_into_jaco_arm_joints"), + "joint_state_2": BlackboardKey("test_into_articutool_joints"), + "full_joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + "atool_joint1", + "atool_joint2", + ], + }, + outputs={ + "combined_joint_state": BlackboardKey("test_into_joints"), + } + ), MoveIt2PoseConstraint( name="MoveIntoJacoArmWithArticutoolPose", ns=name, @@ -684,7 +705,7 @@ def move_above_plan( "cartesian": True, "cartesian_max_step": 0.001, "cartesian_fraction_threshold": 0.92, - # "start_joint_state": BlackboardKey("test_into_jaco_arm_with_articutool_joints"), + "start_joint_state": BlackboardKey("test_into_joints"), "max_path_len_joint": max_path_len_joint, "allowed_planning_time": self.allowed_planning_time_for_move_into, "group_name": "jaco_arm_with_articutool", From 7d91e50e28ec0ce70e7de64fabd1a09617c35d08 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Apr 2025 20:17:38 -0700 Subject: [PATCH 033/238] Use test into Articutool joint state to constrain Cartesian move into plan instead of current since this joint state corresponds to how the Articutool WOULD be configured AFTER the move above motion --- .../ada_feeding/trees/acquire_food_tree.py | 17 +++++++++-------- 1 file changed, 9 insertions(+), 8 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index f58cbcf0..2c43e29e 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -665,24 +665,25 @@ def move_above_plan( "constraints": BlackboardKey("move_into_goal_constraints"), }, ), - GetJointStates( - name="GetArticutoolJoints", - node=self._node, + ExtractJointsFromState( + name="ExtractTestIntoArticutoolJoints", ns=name, inputs={ - "joint_names": ["atool_joint1", "atool_joint2"], + "source_joint_state": BlackboardKey("test_into_articutool_joints"), + "target_joint_names": ["atool_joint1", "atool_joint2"], }, outputs={ - "joint_names": BlackboardKey("articutool_joint_names"), - "joint_positions": BlackboardKey("articutool_joint_positions"), + "output_joint_names": BlackboardKey("test_into_articutool_joint_names"), + "output_joint_positions": BlackboardKey("test_into_articutool_joint_positions"), + "success": None, } ), MoveIt2JointConstraint( name="SetJacoArmJointConstraint", ns=name, inputs={ - "joint_positions": BlackboardKey("articutool_joint_positions"), - "joint_names": BlackboardKey("articutool_joint_names"), + "joint_positions": BlackboardKey("test_into_articutool_joint_positions"), + "joint_names": BlackboardKey("test_into_articutool_joint_names"), "tolerance": 0.001, "constraints": None, }, From f9eb4137e232acb3face30c62916302797f80410 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Apr 2025 20:51:11 -0700 Subject: [PATCH 034/238] Disable move into path constraints temporarily. Holding the Articutool joints static leads to planning failures in the Cartesian motion. --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 2c43e29e..a520c0c2 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -679,7 +679,7 @@ def move_above_plan( } ), MoveIt2JointConstraint( - name="SetJacoArmJointConstraint", + name="SetArticutoolPathConstraint", ns=name, inputs={ "joint_positions": BlackboardKey("test_into_articutool_joint_positions"), @@ -700,7 +700,7 @@ def move_above_plan( ns=name, inputs={ "goal_constraints": BlackboardKey("move_into_goal_constraints"), - "path_constraints": BlackboardKey("move_into_path_constraints"), + # "path_constraints": BlackboardKey("move_into_path_constraints"), "max_velocity_scale": self.max_velocity_scaling_move_into, "max_acceleration_scale": self.max_acceleration_scaling_move_into, "cartesian": True, From 3c08e2b283cb673142bb03e5c97a9a16946e4733 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Apr 2025 17:13:16 -0700 Subject: [PATCH 035/238] Add MoveIt2ComputeFK behavior --- .../ada_feeding/behaviors/moveit2/__init__.py | 1 + .../behaviors/moveit2/moveit2_compute_fk.py | 206 ++++++++++++++++++ 2 files changed, 207 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_fk.py diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py b/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py index 5ad01c39..a64572d0 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/__init__.py @@ -15,6 +15,7 @@ MoveIt2PoseConstraint, ) from .moveit2_compute_ik import MoveIt2ComputeIK +from .moveit2_compute_fk import MoveIt2ComputeFK from .servo_move import ServoMove # Modifying the planning scene diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_fk.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_fk.py new file mode 100644 index 00000000..ce590cd0 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_fk.py @@ -0,0 +1,206 @@ +# -*- coding: utf-8 -*- +# (Add appropriate Copyright/License if desired) + +""" +This module defines the MoveIt2ComputeFK behavior, which uses the +ada_feeding MoveIt2 wrapper to compute Forward Kinematics. +""" + +# Standard imports +from typing import Union, Optional, Dict, Any, List, Tuple +from threading import Lock + +# Third-party imports +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import JointState +from moveit_msgs.msg import MoveItErrorCodes +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status +import rclpy.node + +# Local imports +from ada_feeding.helpers import BlackboardKey, get_moveit2_object +from ada_feeding.behaviors import BlackboardBehavior + +from pymoveit2 import MoveIt2 + + +class MoveIt2ComputeFK(BlackboardBehavior): + """ + Computes Forward Kinematics (FK) using the MoveIt2 interface wrapper + for a given joint state and specified links. + + If joint_state is not provided, uses the current state known by the MoveIt2 object. + If fk_link_names is not provided, computes FK for the default end-effector link + associated with the MoveIt2 object's group. + """ + + def blackboard_inputs( + self, + group_name: Union[BlackboardKey, str], + joint_state: Optional[Union[BlackboardKey, JointState, List[float]]] = None, + fk_link_names: Optional[Union[BlackboardKey, List[str]]] = None, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + group_name: The MoveIt2 planning group name. Used during setup to retrieve + the correct MoveIt2 object instance. + joint_state: Optional state (JointState or List[float]) for FK calculation. + If None or key not present, uses current state from MoveIt2 object. + fk_link_names: Optional list of link names. If None or key not present, uses + the default end-effector link from the MoveIt2 object. + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + fk_poses: Optional[BlackboardKey], # -> Optional[Union[PoseStamped, List[PoseStamped]]] + success: Optional[BlackboardKey], # -> bool + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + fk_poses: The resulting PoseStamped (if single link/default) or List[PoseStamped] + (if multiple fk_link_names requested). None on failure. + success: Boolean flag indicating if FK computation was successful. + """ + # pylint: disable=unused-argument, duplicate-code + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + """Get the ROS2 node and acquire the MoveIt2 object for the specified group.""" + # pylint: disable=attribute-defined-outside-init + self.node: rclpy.node.Node = kwargs['node'] + self.moveit2_obj: Optional[MoveIt2] = None + self.moveit2_lock: Optional[Lock] = None + self.fk_group_name: Optional[str] = None + + try: + self.fk_group_name = self.blackboard_get("group_name") + if not isinstance(self.fk_group_name, str) or not self.fk_group_name: + raise ValueError("group_name must be a non-empty string") + except (KeyError, ValueError) as e: + self.logger.error(f"[{self.name}] Invalid or missing input 'group_name': {e}") + return + + try: + self.moveit2_obj, self.moveit2_lock = get_moveit2_object( + blackboard=self.blackboard, + group_name=self.fk_group_name, + node=self.node, + ) + if self.moveit2_obj is None or self.moveit2_lock is None: + raise RuntimeError("get_moveit2_object returned None") + self.logger.info(f"[{self.name}] Successfully obtained MoveIt2 object for FK group '{self.fk_group_name}'") + except Exception as e: + self.logger.error(f"[{self.name}] Failed to get MoveIt2 object for FK group '{self.fk_group_name}': {e}") + self.moveit2_obj = None + self.moveit2_lock = None + + + @override + def initialise(self) -> None: + """Reset status variables if needed.""" + self.logger.debug(f"[{self.name}] Initialising FK computation.") + # Clear previous results from blackboard + self.blackboard_set("fk_poses", None) + self.blackboard_set("success", False) + + + @override + def update(self) -> Status: + """Perform the FK computation using the synchronous compute_fk method.""" + # Check if setup was successful + if self.moveit2_obj is None or self.moveit2_lock is None: + self.logger.error(f"[{self.name}] MoveIt2 object not initialized. Setup likely failed.") + return Status.FAILURE + + # Check if MoveIt2 is currently locked by another behavior + # Note: FK is usually read-only, lock might be optional depending on wrapper implementation + # but using it ensures consistency if wrapper reads internal state like current joints. + if self.moveit2_lock.locked(): + self.logger.debug(f"[{self.name}] MoveIt2 is locked, waiting.") + return Status.RUNNING + + # Acquire lock and perform FK computation + with self.moveit2_lock: + try: + # Read optional inputs using try-except to handle missing keys gracefully + joint_state_input = None + try: + joint_state_input = self.blackboard_get("joint_state") + # Optional validation + if joint_state_input is not None and not isinstance(joint_state_input, (JointState, list)): + self.logger.warning(f"[{self.name}] Input 'joint_state' is not JointState or List, ignoring.") + joint_state_input = None + except KeyError: + self.logger.debug(f"[{self.name}] Optional input 'joint_state' not found, using MoveIt2 object's current state.") + pass # Keep default None -> wrapper uses current state + + fk_link_names_input = None + try: + fk_link_names_input = self.blackboard_get("fk_link_names") + if fk_link_names_input is not None and not (isinstance(fk_link_names_input, list) and all(isinstance(n, str) for n in fk_link_names_input)): + self.logger.warning(f"[{self.name}] Input 'fk_link_names' is not List[str], ignoring.") + fk_link_names_input = None + except KeyError: + self.logger.debug(f"[{self.name}] Optional input 'fk_link_names' not found, using MoveIt2 object's default end-effector.") + pass # Keep default None -> wrapper uses default EE link + + # Call the synchronous compute_fk method + self.logger.info(f"[{self.name}] Computing FK for group '{self.fk_group_name}' " + f"with links: {fk_link_names_input or 'default EE'}.") + + result_poses: Optional[Union[PoseStamped, List[PoseStamped]]] = self.moveit2_obj.compute_fk( + joint_state=joint_state_input, + fk_link_names=fk_link_names_input, + ) + + # Determine success based on whether a result was returned + fk_success = result_poses is not None + + # Write results to blackboard + self.blackboard_set("success", fk_success) + self.blackboard_set("fk_poses", result_poses) + + if fk_success: + num_poses = len(result_poses) if isinstance(result_poses, list) else 1 + self.logger.info(f"[{self.name}] FK computation successful ({num_poses} pose(s) found).") + return Status.SUCCESS + else: + # The compute_fk wrapper likely logs the specific error code/reason + self.logger.warning(f"[{self.name}] FK computation failed (see MoveIt2 wrapper logs).") + return Status.FAILURE + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error during FK update: {e}") + self.blackboard_set("success", False) + self.blackboard_set("fk_poses", None) + return Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Unexpected error during FK computation: {e}", exc_info=True) + self.blackboard_set("success", False) + self.blackboard_set("fk_poses", None) + return Status.FAILURE + + # Should not be reached if lock logic is correct + return Status.FAILURE + + + @override + def terminate(self, new_status: Status) -> None: + """Log termination status.""" + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") From 54df371e32b56b8ffa42265a2c48f0a266be885b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Apr 2025 17:42:54 -0700 Subject: [PATCH 036/238] Run pre-commit --- .../execute_articutool_trajectory.py | 356 +++++++++++------- .../behaviors/moveit2/moveit2_compute_fk.py | 98 +++-- .../behaviors/moveit2/moveit2_compute_ik.py | 68 ++-- .../behaviors/moveit2/moveit2_constraints.py | 78 ++-- ada_feeding/ada_feeding/behaviors/ros/msgs.py | 36 +- ada_feeding/ada_feeding/behaviors/ros/tf.py | 2 +- .../behaviors/state/combine_joint_states.py | 77 ++-- .../state/extract_joints_from_state.py | 55 ++- .../behaviors/state/get_joint_states.py | 19 +- ada_feeding/ada_feeding/helpers.py | 4 +- .../ada_feeding/trees/acquire_food_tree.py | 197 +++++++--- ...configuration_with_wheelchair_wall_tree.py | 2 +- 12 files changed, 654 insertions(+), 338 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py b/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py index aded93b5..480c4ac4 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py @@ -1,4 +1,7 @@ # -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # (Add appropriate Copyright/License if desired) """ @@ -16,10 +19,11 @@ from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle, GoalStatus from rclpy.node import Node -from rclpy.executors import Future # For type hints +from rclpy.executors import Future # For type hints from control_msgs.action import FollowJointTrajectory -from trajectory_msgs.msg import JointTrajectory # Input type +from trajectory_msgs.msg import JointTrajectory # Input type + # from builtin_interfaces.msg import Duration # Used internally by action goal # from sensor_msgs.msg import JointState # Not directly needed @@ -31,6 +35,7 @@ # Local imports (adjust paths as needed) from ada_feeding.helpers import BlackboardKey from ada_feeding.behaviors import BlackboardBehavior + # Assuming ControllerSwitcher is importable and works with a passed node from articutool_control.controller_switcher import ControllerSwitcher @@ -42,12 +47,14 @@ class ActionExecutionStatus(Enum): SENDING_GOAL = "SENDING_GOAL" WAITING_FOR_ACCEPTANCE = "WAITING_FOR_ACCEPTANCE" EXECUTING = "EXECUTING" - WAITING_FOR_RESULT = "WAITING_FOR_RESULT" # If execution status isn't monitored closely + WAITING_FOR_RESULT = ( + "WAITING_FOR_RESULT" # If execution status isn't monitored closely + ) GOAL_REJECTED = "GOAL_REJECTED" GOAL_CANCELLED = "GOAL_CANCELLED" SUCCEEDED = "SUCCEEDED" - ABORTED = "ABORTED" # From server - FAILED = "FAILED" # General failure + ABORTED = "ABORTED" # From server + FAILED = "FAILED" # General failure class ExecuteArticutoolTrajectory(BlackboardBehavior): @@ -59,9 +66,11 @@ class ExecuteArticutoolTrajectory(BlackboardBehavior): """ # --- Constants --- - DEFAULT_ACTION_SERVER = "/articutool/joint_trajectory_controller/follow_joint_trajectory" - DEFAULT_CONTROLLER_TO_ACTIVATE = ["joint_trajectory_controller"] # Expects list - DEFAULT_CONTROLLERS_TO_DEACTIVATE = ["velocity_controller"] # Expects list + DEFAULT_ACTION_SERVER = ( + "/articutool/joint_trajectory_controller/follow_joint_trajectory" + ) + DEFAULT_CONTROLLER_TO_ACTIVATE = ["joint_trajectory_controller"] # Expects list + DEFAULT_CONTROLLERS_TO_DEACTIVATE = ["velocity_controller"] # Expects list def __init__(self, name: str, ns: str = "/", **kwargs): # Pass kwargs to parent if BlackboardBehavior supports it @@ -73,13 +82,17 @@ def __init__(self, name: str, ns: str = "/", **kwargs): self._get_result_future: Optional[Future] = None self._goal_handle: Optional[ClientGoalHandle] = None self._current_status: ActionExecutionStatus = ActionExecutionStatus.IDLE - self._result_code: Optional[int] = None # Store the final result code + self._result_code: Optional[int] = None # Store the final result code def blackboard_inputs( self, trajectory: Union[BlackboardKey, JointTrajectory], - controllers_to_activate: Union[BlackboardKey, List[str]] = DEFAULT_CONTROLLER_TO_ACTIVATE, - controllers_to_deactivate: Union[BlackboardKey, List[str]] = DEFAULT_CONTROLLERS_TO_DEACTIVATE, + controllers_to_activate: Union[ + BlackboardKey, List[str] + ] = DEFAULT_CONTROLLER_TO_ACTIVATE, + controllers_to_deactivate: Union[ + BlackboardKey, List[str] + ] = DEFAULT_CONTROLLERS_TO_DEACTIVATE, action_server_name: Union[BlackboardKey, str] = DEFAULT_ACTION_SERVER, ) -> None: """ @@ -98,9 +111,13 @@ def blackboard_inputs( def blackboard_outputs( self, - action_goal_accepted: Optional[BlackboardKey] = None, # -> bool - action_result_code: Optional[BlackboardKey] = None, # -> int (FollowJointTrajectory.Result.error_code) - action_status: Optional[BlackboardKey] = None, # -> str (from ActionExecutionStatus enum) + action_goal_accepted: Optional[BlackboardKey] = None, # -> bool + action_result_code: Optional[ + BlackboardKey + ] = None, # -> int (FollowJointTrajectory.Result.error_code) + action_status: Optional[ + BlackboardKey + ] = None, # -> str (from ActionExecutionStatus enum) ) -> None: """ Blackboard Outputs @@ -120,49 +137,55 @@ def setup(self, **kwargs): """Get node, create action client and controller switcher.""" # pylint: disable=attribute-defined-outside-init try: - self.node: Node = kwargs['node'] + self.node: Node = kwargs["node"] except KeyError as e: - self.logger.error(f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}") - return # Cannot function without node + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + return # Cannot function without node try: try: action_server_name = self.blackboard_get("action_server_name") if not isinstance(action_server_name, str) or not action_server_name: - self.logger.warning(f"[{self.name}] Blackboard 'action_server_name' invalid, using default.") - action_server_name = self.DEFAULT_ACTION_SERVER + self.logger.warning( + f"[{self.name}] Blackboard 'action_server_name' invalid, using default." + ) + action_server_name = self.DEFAULT_ACTION_SERVER except KeyError: - self.logger.info(f"[{self.name}] Input 'action_server_name' not found, using default: {self.DEFAULT_ACTION_SERVER}") + self.logger.info( + f"[{self.name}] Input 'action_server_name' not found, using default: {self.DEFAULT_ACTION_SERVER}" + ) action_server_name = self.DEFAULT_ACTION_SERVER self._action_client = ActionClient( - self.node, - FollowJointTrajectory, - action_server_name + self.node, FollowJointTrajectory, action_server_name ) # Check server availability briefly during setup timeout_sec = 1.0 if not self._action_client.wait_for_server(timeout_sec=timeout_sec): - self.logger.error(f"[{self.name}] Action server '{action_server_name}' not available after {timeout_sec}s during setup.") - self._action_client = None # Mark as unavailable - return # Setup fails + self.logger.error( + f"[{self.name}] Action server '{action_server_name}' not available after {timeout_sec}s during setup." + ) + self._action_client = None # Mark as unavailable + return # Setup fails # Instantiate Controller Switcher (pass node if its __init__ requires it) try: - # Assuming ControllerSwitcher might take node as arg - self.controller_switcher = ControllerSwitcher(node=self.node) - except TypeError: # If ControllerSwitcher() doesn't take node - self.controller_switcher = ControllerSwitcher() - + # Assuming ControllerSwitcher might take node as arg + self.controller_switcher = ControllerSwitcher(node=self.node) + except TypeError: # If ControllerSwitcher() doesn't take node + self.controller_switcher = ControllerSwitcher() - self.logger.info(f"[{self.name}] Setup complete. Action client created for '{action_server_name}'.") + self.logger.info( + f"[{self.name}] Setup complete. Action client created for '{action_server_name}'." + ) except Exception as e: self.logger.error(f"[{self.name}] Failed during setup: {e}") self._action_client = None self.controller_switcher = None - @override def initialise(self) -> None: """Reset action client state.""" @@ -171,13 +194,14 @@ def initialise(self) -> None: self._get_result_future = None self._goal_handle = None self._current_status = ActionExecutionStatus.IDLE - self._result_code = None # Reset result code + self._result_code = None # Reset result code # Clear blackboard outputs self.blackboard_set("action_goal_accepted", False) - self.blackboard_set("action_result_code", self._result_code) # Set to None or initial value + self.blackboard_set( + "action_result_code", self._result_code + ) # Set to None or initial value self.blackboard_set("action_status", self._current_status.value) - @override def update(self) -> Status: """Manage the action client state machine.""" @@ -185,48 +209,64 @@ def update(self) -> Status: self.blackboard_set("action_status", self._current_status.value) if self._action_client is None or self.controller_switcher is None: - self.logger.error(f"[{self.name}] Action client or controller switcher not available. Setup failed?") - return Status.FAILURE # Setup must have failed + self.logger.error( + f"[{self.name}] Action client or controller switcher not available. Setup failed?" + ) + return Status.FAILURE # Setup must have failed # --- State 1: IDLE -> Try sending goal --- if self._current_status == ActionExecutionStatus.IDLE: try: trajectory: JointTrajectory = self.blackboard_get("trajectory") - controllers_to_activate: List[str] = self.blackboard_get("controllers_to_activate") - controllers_to_deactivate: List[str] = self.blackboard_get("controllers_to_deactivate") + controllers_to_activate: List[str] = self.blackboard_get( + "controllers_to_activate" + ) + controllers_to_deactivate: List[str] = self.blackboard_get( + "controllers_to_deactivate" + ) # Validate inputs if not isinstance(trajectory, JointTrajectory): - self.logger.error(f"[{self.name}] Input 'trajectory' is not a JointTrajectory message.") + self.logger.error( + f"[{self.name}] Input 'trajectory' is not a JointTrajectory message." + ) return Status.FAILURE if not trajectory.points: - self.logger.warning(f"[{self.name}] Input 'trajectory' has no points. Succeeding trivially.") + self.logger.warning( + f"[{self.name}] Input 'trajectory' has no points. Succeeding trivially." + ) self._current_status = ActionExecutionStatus.SUCCEEDED self.blackboard_set("action_status", self._current_status.value) - self.blackboard_set("action_result_code", FollowJointTrajectory.Result.SUCCESSFUL) + self.blackboard_set( + "action_result_code", FollowJointTrajectory.Result.SUCCESSFUL + ) return Status.SUCCESS - if not isinstance(controllers_to_activate, list) or not isinstance(controllers_to_deactivate, list): - self.logger.error(f"[{self.name}] Controller inputs must be lists.") - return Status.FAILURE - + if not isinstance(controllers_to_activate, list) or not isinstance( + controllers_to_deactivate, list + ): + self.logger.error(f"[{self.name}] Controller inputs must be lists.") + return Status.FAILURE # 1a. Switch Controllers self._current_status = ActionExecutionStatus.SWITCHING_CONTROLLERS self.blackboard_set("action_status", self._current_status.value) - self.logger.info(f"[{self.name}] Requesting controller switch: start={controllers_to_activate}, stop={controllers_to_deactivate}") + self.logger.info( + f"[{self.name}] Requesting controller switch: start={controllers_to_activate}, stop={controllers_to_deactivate}" + ) try: # Assuming switch_controllers blocks or returns success/failure quickly self.controller_switcher.switch_controllers( - activate_controllers=controllers_to_activate, - deactivate_controllers=controllers_to_deactivate, + activate_controllers=controllers_to_activate, + deactivate_controllers=controllers_to_deactivate, ) except Exception as sw_e: - self.logger.error(f"[{self.name}] Error during controller switch: {sw_e}") + self.logger.error( + f"[{self.name}] Error during controller switch: {sw_e}" + ) self._current_status = ActionExecutionStatus.FAILED self.blackboard_set("action_status", self._current_status.value) return Status.FAILURE - # 1b. Construct Goal Message goal_msg = FollowJointTrajectory.Goal() # Important: Make sure the trajectory has correct joint names for the action server @@ -234,9 +274,10 @@ def update(self) -> Status: # Optional: Add goal time tolerance, path tolerance if needed # goal_msg.goal_time_tolerance = Duration(sec=1, nanosec=0).to_msg() - # 1c. Send Goal Asynchronously - self.logger.info(f"[{self.name}] Sending trajectory goal to action server...") + self.logger.info( + f"[{self.name}] Sending trajectory goal to action server..." + ) self._send_goal_future = self._action_client.send_goal_async(goal_msg) self._current_status = ActionExecutionStatus.SENDING_GOAL self.blackboard_set("action_status", self._current_status.value) @@ -251,22 +292,31 @@ def update(self) -> Status: # --- State 2: Waiting for Goal Acceptance --- if self._current_status == ActionExecutionStatus.SENDING_GOAL: - if self._send_goal_future is None: return self._handle_invalid_state("SENDING_GOAL future None") # Should not happen + if self._send_goal_future is None: + return self._handle_invalid_state( + "SENDING_GOAL future None" + ) # Should not happen if self._send_goal_future.done(): - try: - goal_handle: ClientGoalHandle = self._send_goal_future.result() - except Exception as e: - self.logger.error(f"[{self.name}] Exception getting goal handle from future: {e}") - return self._handle_failure("Exception getting goal handle") - - self._send_goal_future = None # Clear future - - if not goal_handle.accepted: - self.logger.warning(f"[{self.name}] Goal rejected by action server.") - self.blackboard_set("action_goal_accepted", False) - return self._handle_failure("Goal rejected", ActionExecutionStatus.GOAL_REJECTED) - else: + try: + goal_handle: ClientGoalHandle = self._send_goal_future.result() + except Exception as e: + self.logger.error( + f"[{self.name}] Exception getting goal handle from future: {e}" + ) + return self._handle_failure("Exception getting goal handle") + + self._send_goal_future = None # Clear future + + if not goal_handle.accepted: + self.logger.warning( + f"[{self.name}] Goal rejected by action server." + ) + self.blackboard_set("action_goal_accepted", False) + return self._handle_failure( + "Goal rejected", ActionExecutionStatus.GOAL_REJECTED + ) + else: # Convert UUID numpy array to hex string for logging try: # Access the uuid field (numpy array of uint8) @@ -277,14 +327,20 @@ def update(self) -> Status: uuid_hex = uuid_bytes.hex() except Exception as log_e: # Fallback in case something goes wrong with conversion - self.logger.warning(f"[{self.name}] Could not format goal ID UUID for logging: {log_e}") + self.logger.warning( + f"[{self.name}] Could not format goal ID UUID for logging: {log_e}" + ) uuid_hex = "[Error Formatting UUID]" - self.logger.info(f"[{self.name}] Goal accepted by action server (ID: {uuid_hex}).") + self.logger.info( + f"[{self.name}] Goal accepted by action server (ID: {uuid_hex})." + ) self._goal_handle = goal_handle self.blackboard_set("action_goal_accepted", True) self._get_result_future = self._goal_handle.get_result_async() - self._current_status = ActionExecutionStatus.EXECUTING # Or WAITING_FOR_RESULT if preferred + self._current_status = ( + ActionExecutionStatus.EXECUTING + ) # Or WAITING_FOR_RESULT if preferred self.blackboard_set("action_status", self._current_status.value) return Status.RUNNING else: @@ -292,61 +348,89 @@ def update(self) -> Status: return Status.RUNNING # --- State 3: Waiting for Result / Monitoring Execution --- - if self._current_status == ActionExecutionStatus.EXECUTING or self._current_status == ActionExecutionStatus.WAITING_FOR_RESULT : - if self._get_result_future is None or self._goal_handle is None: - return self._handle_invalid_state("EXECUTING/WAITING future or handle None") - - # Check goal handle status - status = self._goal_handle.status - if status == GoalStatus.STATUS_EXECUTING: - self._current_status = ActionExecutionStatus.EXECUTING # Ensure state is correct - # Optional: Process feedback if needed - # self.logger.debug(f"[{self.name}] Goal executing...") - elif status == GoalStatus.STATUS_ABORTED: - self.logger.warning(f"[{self.name}] Goal aborted by server.") - # Result future should complete soon, wait for it - self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT # Move to check result - elif status == GoalStatus.STATUS_CANCELED: - self.logger.warning(f"[{self.name}] Goal canceled.") - self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT - elif status == GoalStatus.STATUS_SUCCEEDED: - self.logger.info(f"[{self.name}] Goal status reported SUCCEEDED.") - self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT - - # Now check if the result future is done - if self._get_result_future.done(): - try: - result_wrapper = self._get_result_future.result() - action_result = result_wrapper.result - self._result_code = action_result.error_code - result_string = action_result.error_string or "None" - self.logger.info(f"[{self.name}] Action result received: Code={self._result_code}, Msg='{result_string}'") - self.blackboard_set("action_result_code", self._result_code) - - # Final state determination - if self._result_code == FollowJointTrajectory.Result.SUCCESSFUL: - return self._handle_success() - elif status == GoalStatus.STATUS_CANCELED: # Check status again if needed - return self._handle_failure("Goal Canceled", ActionExecutionStatus.GOAL_CANCELLED) - else: # Any other error code implies failure - return self._handle_failure(f"Action Failed: Code={self._result_code}, Msg='{result_string}'", ActionExecutionStatus.ABORTED if status == GoalStatus.STATUS_ABORTED else ActionExecutionStatus.FAILED) - - except Exception as e: - self.logger.error(f"[{self.name}] Exception getting result from future: {e}") - return self._handle_failure("Exception getting result") - else: + if ( + self._current_status == ActionExecutionStatus.EXECUTING + or self._current_status == ActionExecutionStatus.WAITING_FOR_RESULT + ): + if self._get_result_future is None or self._goal_handle is None: + return self._handle_invalid_state( + "EXECUTING/WAITING future or handle None" + ) + + # Check goal handle status + status = self._goal_handle.status + if status == GoalStatus.STATUS_EXECUTING: + self._current_status = ( + ActionExecutionStatus.EXECUTING + ) # Ensure state is correct + # Optional: Process feedback if needed + # self.logger.debug(f"[{self.name}] Goal executing...") + elif status == GoalStatus.STATUS_ABORTED: + self.logger.warning(f"[{self.name}] Goal aborted by server.") + # Result future should complete soon, wait for it + self._current_status = ( + ActionExecutionStatus.WAITING_FOR_RESULT + ) # Move to check result + elif status == GoalStatus.STATUS_CANCELED: + self.logger.warning(f"[{self.name}] Goal canceled.") + self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT + elif status == GoalStatus.STATUS_SUCCEEDED: + self.logger.info(f"[{self.name}] Goal status reported SUCCEEDED.") + self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT + + # Now check if the result future is done + if self._get_result_future.done(): + try: + result_wrapper = self._get_result_future.result() + action_result = result_wrapper.result + self._result_code = action_result.error_code + result_string = action_result.error_string or "None" + self.logger.info( + f"[{self.name}] Action result received: Code={self._result_code}, Msg='{result_string}'" + ) + self.blackboard_set("action_result_code", self._result_code) + + # Final state determination + if self._result_code == FollowJointTrajectory.Result.SUCCESSFUL: + return self._handle_success() + elif ( + status == GoalStatus.STATUS_CANCELED + ): # Check status again if needed + return self._handle_failure( + "Goal Canceled", ActionExecutionStatus.GOAL_CANCELLED + ) + else: # Any other error code implies failure + return self._handle_failure( + f"Action Failed: Code={self._result_code}, Msg='{result_string}'", + ActionExecutionStatus.ABORTED + if status == GoalStatus.STATUS_ABORTED + else ActionExecutionStatus.FAILED, + ) + + except Exception as e: + self.logger.error( + f"[{self.name}] Exception getting result from future: {e}" + ) + return self._handle_failure("Exception getting result") + else: # Result future not done yet, goal still active (or transitioning) self.blackboard_set("action_status", self._current_status.value) return Status.RUNNING # --- Handle Terminal States (Should be reached via returns above) --- - if self._current_status == ActionExecutionStatus.SUCCEEDED: return Status.SUCCESS - if self._current_status in [ActionExecutionStatus.FAILED, ActionExecutionStatus.GOAL_REJECTED, ActionExecutionStatus.GOAL_CANCELLED, ActionExecutionStatus.ABORTED]: return Status.FAILURE + if self._current_status == ActionExecutionStatus.SUCCEEDED: + return Status.SUCCESS + if self._current_status in [ + ActionExecutionStatus.FAILED, + ActionExecutionStatus.GOAL_REJECTED, + ActionExecutionStatus.GOAL_CANCELLED, + ActionExecutionStatus.ABORTED, + ]: + return Status.FAILURE # Fallback if state is somehow invalid return self._handle_invalid_state(f"Unhandled status {self._current_status}") - @override def terminate(self, new_status: Status) -> None: """Cancel active goal if behavior is terminated unexpectedly.""" @@ -355,14 +439,19 @@ def terminate(self, new_status: Status) -> None: # If terminated EXTERNALLY (INVALID), cancel the goal if it's active. if new_status == Status.INVALID and self._goal_handle is not None: status = self._goal_handle.status - if status == GoalStatus.STATUS_ACCEPTED or status == GoalStatus.STATUS_EXECUTING: - self.logger.warning(f"[{self.name}] Behavior terminated externally while action goal was active (Status: {GoalStatus(status).name}). Requesting cancellation.") - cancel_future = self._goal_handle.cancel_goal_async() - # Optional: Spin briefly/add callback to ensure cancel sent? Usually not needed. + if ( + status == GoalStatus.STATUS_ACCEPTED + or status == GoalStatus.STATUS_EXECUTING + ): + self.logger.warning( + f"[{self.name}] Behavior terminated externally while action goal was active (Status: {GoalStatus(status).name}). Requesting cancellation." + ) + cancel_future = self._goal_handle.cancel_goal_async() + # Optional: Spin briefly/add callback to ensure cancel sent? Usually not needed. # else: # self.logger.debug(f"[{self.name}] Goal handle exists but not in active state ({GoalStatus(status).name}) on termination.") # else: - # self.logger.debug(f"[{self.name}] No active goal handle or clean termination, no cancellation needed.") + # self.logger.debug(f"[{self.name}] No active goal handle or clean termination, no cancellation needed.") # Reset internal state variables regardless self._send_goal_future = None @@ -379,22 +468,29 @@ def _handle_success(self) -> Status: self._get_result_future = None return Status.SUCCESS - def _handle_failure(self, reason: str, final_status: ActionExecutionStatus = ActionExecutionStatus.FAILED) -> Status: + def _handle_failure( + self, + reason: str, + final_status: ActionExecutionStatus = ActionExecutionStatus.FAILED, + ) -> Status: self.logger.error(f"[{self.name}] Failure: {reason}") self._current_status = final_status self.blackboard_set("action_status", self._current_status.value) # Set result code if available and not already set failure code - if self._result_code is not None and self._result_code != FollowJointTrajectory.Result.SUCCESSFUL: - self.blackboard_set("action_result_code", self._result_code) + if ( + self._result_code is not None + and self._result_code != FollowJointTrajectory.Result.SUCCESSFUL + ): + self.blackboard_set("action_result_code", self._result_code) elif self._current_status == ActionExecutionStatus.GOAL_REJECTED: - # Use a convention for rejected? - self.blackboard_set("action_result_code", -998) # Example arbitrary code - else: # General failure - self.blackboard_set("action_result_code", -999) # Example arbitrary code + # Use a convention for rejected? + self.blackboard_set("action_result_code", -998) # Example arbitrary code + else: # General failure + self.blackboard_set("action_result_code", -999) # Example arbitrary code self._goal_handle = None self._get_result_future = None - self._send_goal_future = None # Ensure this is cleared too + self._send_goal_future = None # Ensure this is cleared too return Status.FAILURE def _handle_invalid_state(self, message: str) -> Status: diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_fk.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_fk.py index ce590cd0..216b5555 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_fk.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_fk.py @@ -1,4 +1,7 @@ # -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # (Add appropriate Copyright/License if desired) """ @@ -62,8 +65,10 @@ def blackboard_inputs( def blackboard_outputs( self, - fk_poses: Optional[BlackboardKey], # -> Optional[Union[PoseStamped, List[PoseStamped]]] - success: Optional[BlackboardKey], # -> bool + fk_poses: Optional[ + BlackboardKey + ], # -> Optional[Union[PoseStamped, List[PoseStamped]]] + success: Optional[BlackboardKey], # -> bool ) -> None: """ Blackboard Outputs @@ -83,7 +88,7 @@ def blackboard_outputs( def setup(self, **kwargs): """Get the ROS2 node and acquire the MoveIt2 object for the specified group.""" # pylint: disable=attribute-defined-outside-init - self.node: rclpy.node.Node = kwargs['node'] + self.node: rclpy.node.Node = kwargs["node"] self.moveit2_obj: Optional[MoveIt2] = None self.moveit2_lock: Optional[Lock] = None self.fk_group_name: Optional[str] = None @@ -91,9 +96,11 @@ def setup(self, **kwargs): try: self.fk_group_name = self.blackboard_get("group_name") if not isinstance(self.fk_group_name, str) or not self.fk_group_name: - raise ValueError("group_name must be a non-empty string") + raise ValueError("group_name must be a non-empty string") except (KeyError, ValueError) as e: - self.logger.error(f"[{self.name}] Invalid or missing input 'group_name': {e}") + self.logger.error( + f"[{self.name}] Invalid or missing input 'group_name': {e}" + ) return try: @@ -103,14 +110,17 @@ def setup(self, **kwargs): node=self.node, ) if self.moveit2_obj is None or self.moveit2_lock is None: - raise RuntimeError("get_moveit2_object returned None") - self.logger.info(f"[{self.name}] Successfully obtained MoveIt2 object for FK group '{self.fk_group_name}'") + raise RuntimeError("get_moveit2_object returned None") + self.logger.info( + f"[{self.name}] Successfully obtained MoveIt2 object for FK group '{self.fk_group_name}'" + ) except Exception as e: - self.logger.error(f"[{self.name}] Failed to get MoveIt2 object for FK group '{self.fk_group_name}': {e}") + self.logger.error( + f"[{self.name}] Failed to get MoveIt2 object for FK group '{self.fk_group_name}': {e}" + ) self.moveit2_obj = None self.moveit2_lock = None - @override def initialise(self) -> None: """Reset status variables if needed.""" @@ -119,13 +129,14 @@ def initialise(self) -> None: self.blackboard_set("fk_poses", None) self.blackboard_set("success", False) - @override def update(self) -> Status: """Perform the FK computation using the synchronous compute_fk method.""" # Check if setup was successful if self.moveit2_obj is None or self.moveit2_lock is None: - self.logger.error(f"[{self.name}] MoveIt2 object not initialized. Setup likely failed.") + self.logger.error( + f"[{self.name}] MoveIt2 object not initialized. Setup likely failed." + ) return Status.FAILURE # Check if MoveIt2 is currently locked by another behavior @@ -143,28 +154,45 @@ def update(self) -> Status: try: joint_state_input = self.blackboard_get("joint_state") # Optional validation - if joint_state_input is not None and not isinstance(joint_state_input, (JointState, list)): - self.logger.warning(f"[{self.name}] Input 'joint_state' is not JointState or List, ignoring.") - joint_state_input = None + if joint_state_input is not None and not isinstance( + joint_state_input, (JointState, list) + ): + self.logger.warning( + f"[{self.name}] Input 'joint_state' is not JointState or List, ignoring." + ) + joint_state_input = None except KeyError: - self.logger.debug(f"[{self.name}] Optional input 'joint_state' not found, using MoveIt2 object's current state.") - pass # Keep default None -> wrapper uses current state + self.logger.debug( + f"[{self.name}] Optional input 'joint_state' not found, using MoveIt2 object's current state." + ) + pass # Keep default None -> wrapper uses current state fk_link_names_input = None try: fk_link_names_input = self.blackboard_get("fk_link_names") - if fk_link_names_input is not None and not (isinstance(fk_link_names_input, list) and all(isinstance(n, str) for n in fk_link_names_input)): - self.logger.warning(f"[{self.name}] Input 'fk_link_names' is not List[str], ignoring.") - fk_link_names_input = None + if fk_link_names_input is not None and not ( + isinstance(fk_link_names_input, list) + and all(isinstance(n, str) for n in fk_link_names_input) + ): + self.logger.warning( + f"[{self.name}] Input 'fk_link_names' is not List[str], ignoring." + ) + fk_link_names_input = None except KeyError: - self.logger.debug(f"[{self.name}] Optional input 'fk_link_names' not found, using MoveIt2 object's default end-effector.") - pass # Keep default None -> wrapper uses default EE link + self.logger.debug( + f"[{self.name}] Optional input 'fk_link_names' not found, using MoveIt2 object's default end-effector." + ) + pass # Keep default None -> wrapper uses default EE link # Call the synchronous compute_fk method - self.logger.info(f"[{self.name}] Computing FK for group '{self.fk_group_name}' " - f"with links: {fk_link_names_input or 'default EE'}.") + self.logger.info( + f"[{self.name}] Computing FK for group '{self.fk_group_name}' " + f"with links: {fk_link_names_input or 'default EE'}." + ) - result_poses: Optional[Union[PoseStamped, List[PoseStamped]]] = self.moveit2_obj.compute_fk( + result_poses: Optional[ + Union[PoseStamped, List[PoseStamped]] + ] = self.moveit2_obj.compute_fk( joint_state=joint_state_input, fk_link_names=fk_link_names_input, ) @@ -177,21 +205,32 @@ def update(self) -> Status: self.blackboard_set("fk_poses", result_poses) if fk_success: - num_poses = len(result_poses) if isinstance(result_poses, list) else 1 - self.logger.info(f"[{self.name}] FK computation successful ({num_poses} pose(s) found).") + num_poses = ( + len(result_poses) if isinstance(result_poses, list) else 1 + ) + self.logger.info( + f"[{self.name}] FK computation successful ({num_poses} pose(s) found)." + ) return Status.SUCCESS else: # The compute_fk wrapper likely logs the specific error code/reason - self.logger.warning(f"[{self.name}] FK computation failed (see MoveIt2 wrapper logs).") + self.logger.warning( + f"[{self.name}] FK computation failed (see MoveIt2 wrapper logs)." + ) return Status.FAILURE except KeyError as e: - self.logger.error(f"[{self.name}] Blackboard key error during FK update: {e}") + self.logger.error( + f"[{self.name}] Blackboard key error during FK update: {e}" + ) self.blackboard_set("success", False) self.blackboard_set("fk_poses", None) return Status.FAILURE except Exception as e: - self.logger.error(f"[{self.name}] Unexpected error during FK computation: {e}", exc_info=True) + self.logger.error( + f"[{self.name}] Unexpected error during FK computation: {e}", + exc_info=True, + ) self.blackboard_set("success", False) self.blackboard_set("fk_poses", None) return Status.FAILURE @@ -199,7 +238,6 @@ def update(self) -> Status: # Should not be reached if lock logic is correct return Status.FAILURE - @override def terminate(self, new_status: Status) -> None: """Log termination status.""" diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py index 5648f6bd..d45435cc 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py @@ -14,7 +14,10 @@ # Third-party imports from geometry_msgs.msg import PoseStamped, Point, Quaternion from sensor_msgs.msg import JointState -from moveit_msgs.msg import Constraints, MoveItErrorCodes # Added MoveItErrorCodes potentially used by wrapper +from moveit_msgs.msg import ( + Constraints, + MoveItErrorCodes, +) # Added MoveItErrorCodes potentially used by wrapper from overrides import override import py_trees import py_trees.blackboard @@ -69,8 +72,8 @@ def blackboard_inputs( def blackboard_outputs( self, - ik_solution_joint_state: Optional[BlackboardKey], # -> Optional[JointState] - success: Optional[BlackboardKey], # -> bool + ik_solution_joint_state: Optional[BlackboardKey], # -> Optional[JointState] + success: Optional[BlackboardKey], # -> bool ) -> None: """ Blackboard Outputs @@ -98,9 +101,11 @@ def setup(self, **kwargs): try: self.ik_group_name = self.blackboard_get("group_name") if not isinstance(self.ik_group_name, str) or not self.ik_group_name: - raise ValueError("group_name must be a non-empty string") + raise ValueError("group_name must be a non-empty string") except (KeyError, ValueError) as e: - self.logger.error(f"[{self.name}] Invalid or missing input 'group_name': {e}") + self.logger.error( + f"[{self.name}] Invalid or missing input 'group_name': {e}" + ) return # Get the MoveIt2 object using the specific group name @@ -111,11 +116,17 @@ def setup(self, **kwargs): node=self.node, ) if self.moveit2_obj is None or self.moveit2_lock is None: - # Ensure setup fails completely if objects aren't retrieved - raise RuntimeError("get_moveit2_object returned None for MoveIt2 object or lock") - self.logger.info(f"[{self.name}] Successfully obtained MoveIt2 object for IK group '{self.ik_group_name}'") + # Ensure setup fails completely if objects aren't retrieved + raise RuntimeError( + "get_moveit2_object returned None for MoveIt2 object or lock" + ) + self.logger.info( + f"[{self.name}] Successfully obtained MoveIt2 object for IK group '{self.ik_group_name}'" + ) except Exception as e: - self.logger.error(f"[{self.name}] Failed to get MoveIt2 object for IK group '{self.ik_group_name}': {e}") + self.logger.error( + f"[{self.name}] Failed to get MoveIt2 object for IK group '{self.ik_group_name}': {e}" + ) self.moveit2_obj = None self.moveit2_lock = None @@ -126,13 +137,14 @@ def initialise(self) -> None: self.blackboard_set("success", False) self.blackboard_set("ik_solution_joint_state", None) - @override def update(self) -> py_trees.common.Status: """Perform the IK computation using the synchronous compute_ik method.""" # Check if setup was successful if self.moveit2_obj is None or self.moveit2_lock is None: - self.logger.error(f"[{self.name}] MoveIt2 object not initialized. Setup likely failed.") + self.logger.error( + f"[{self.name}] MoveIt2 object not initialized. Setup likely failed." + ) return py_trees.common.Status.FAILURE # Check if MoveIt2 is currently locked by another behavior @@ -149,23 +161,31 @@ def update(self) -> py_trees.common.Status: try: start_state_seed = self.blackboard_get("start_joint_state") except KeyError: - self.logger.debug(f"[{self.name}] Optional input 'start_joint_state' not found, using None.") + self.logger.debug( + f"[{self.name}] Optional input 'start_joint_state' not found, using None." + ) pass ik_constraints = None try: ik_constraints = self.blackboard_get("ik_constraints") except KeyError: - self.logger.debug(f"[{self.name}] Optional input 'ik_constraints' not found, using None.") + self.logger.debug( + f"[{self.name}] Optional input 'ik_constraints' not found, using None." + ) pass # Validate target_pose_stamped type if not isinstance(target_pose_stamped, PoseStamped): - self.logger.error(f"[{self.name}] Input 'target_pose' is not a PoseStamped message.") - return py_trees.common.Status.FAILURE + self.logger.error( + f"[{self.name}] Input 'target_pose' is not a PoseStamped message." + ) + return py_trees.common.Status.FAILURE # --- Call the synchronous compute_ik method provided in the API --- # It takes position and orientation separately. - self.logger.info(f"[{self.name}] Computing IK for group '{self.ik_group_name}' targeting pose in frame '{target_pose_stamped.header.frame_id}'...") + self.logger.info( + f"[{self.name}] Computing IK for group '{self.ik_group_name}' targeting pose in frame '{target_pose_stamped.header.frame_id}'..." + ) # The MoveIt service internally uses the frame_id from the PoseStamped in its request, # even though the wrapper function takes position/orientation separately. @@ -174,7 +194,7 @@ def update(self) -> py_trees.common.Status: position=target_pose_stamped.pose.position, quat_xyzw=target_pose_stamped.pose.orientation, start_joint_state=start_state_seed, - constraints=ik_constraints + constraints=ik_constraints, ) # Determine success based on whether a result was returned @@ -188,20 +208,26 @@ def update(self) -> py_trees.common.Status: self.logger.info(f"[{self.name}] IK computation successful.") return py_trees.common.Status.SUCCESS else: - self.logger.warning(f"[{self.name}] IK computation failed (see MoveIt2 wrapper logs for error code).") + self.logger.warning( + f"[{self.name}] IK computation failed (see MoveIt2 wrapper logs for error code)." + ) return py_trees.common.Status.FAILURE except KeyError as e: - self.logger.error(f"[{self.name}] Blackboard key error during IK update: {e}") + self.logger.error( + f"[{self.name}] Blackboard key error during IK update: {e}" + ) return py_trees.common.Status.FAILURE except Exception as e: - self.logger.error(f"[{self.name}] Unexpected error during IK computation: {e}", exc_info=True) + self.logger.error( + f"[{self.name}] Unexpected error during IK computation: {e}", + exc_info=True, + ) return py_trees.common.Status.FAILURE # This line should not be reachable if the lock logic is correct return py_trees.common.Status.FAILURE - @override def terminate(self, new_status: py_trees.common.Status) -> None: """Log termination status.""" diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py index a2db3a3e..12a05999 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_constraints.py @@ -107,10 +107,12 @@ def update(self) -> py_trees.common.Status: "joint_names", "tolerance", "weight", - "constraints" + "constraints", ]: if not self.blackboard_exists(argument): - self.logger.error(f"MoveIt2JointConstraint: Missing input argument: {argument}") + self.logger.error( + f"MoveIt2JointConstraint: Missing input argument: {argument}" + ) return py_trees.common.Status.FAILURE # # Check if joint positions are valid (not None or empty) @@ -242,15 +244,17 @@ def update(self) -> py_trees.common.Status: # Docstring copied from @override for argument in [ - "offset", - "frame_id", - "target_link", - "tolerance", - "weight", - "constraints", + "offset", + "frame_id", + "target_link", + "tolerance", + "weight", + "constraints", ]: if not self.blackboard_exists(argument): - self.logger.error(f"MoveIt2PositionOffsetConstraint: Missing input argument: {argument}") + self.logger.error( + f"MoveIt2PositionOffsetConstraint: Missing input argument: {argument}" + ) self.logger.error(str(self.blackboard)) return py_trees.common.Status.FAILURE @@ -396,15 +400,17 @@ def update(self) -> py_trees.common.Status: # Docstring copied from @override for argument in [ - "position", - "frame_id", - "target_link", - "tolerance", - "weight", - "constraints", + "position", + "frame_id", + "target_link", + "tolerance", + "weight", + "constraints", ]: if not self.blackboard_exists(argument): - self.logger.error(f"MoveIt2PositionConstraint: Missing input argument: {argument}") + self.logger.error( + f"MoveIt2PositionConstraint: Missing input argument: {argument}" + ) self.logger.error(str(self.blackboard)) return py_trees.common.Status.FAILURE @@ -520,16 +526,18 @@ def update(self) -> py_trees.common.Status: # Docstring copied from @override for argument in [ - "quat_xyzw", - "frame_id", - "target_link", - "tolerance", - "weight", - "constraints", - "parameterization", + "quat_xyzw", + "frame_id", + "target_link", + "tolerance", + "weight", + "constraints", + "parameterization", ]: if not self.blackboard_exists(argument): - self.logger.error(f"MoveIt2OrientationConstraint: Missing input argument: {argument}") + self.logger.error( + f"MoveIt2OrientationConstraint: Missing input argument: {argument}" + ) self.logger.error(str(self.blackboard)) return py_trees.common.Status.FAILURE @@ -646,18 +654,20 @@ def update(self) -> py_trees.common.Status: # Docstring copied from @override for argument in [ - "pose", - "frame_id", - "target_link", - "tolerance_position", - "weight_position", - "tolerance_orientation", - "weight_orientation", - "constraints", - "parameterization", + "pose", + "frame_id", + "target_link", + "tolerance_position", + "weight_position", + "tolerance_orientation", + "weight_orientation", + "constraints", + "parameterization", ]: if not self.blackboard_exists(argument): - self.logger.error(f"MoveIt2PoseConstraint: Missing input argument: {argument}") + self.logger.error( + f"MoveIt2PoseConstraint: Missing input argument: {argument}" + ) self.logger.error(str(self.blackboard)) return py_trees.common.Status.FAILURE diff --git a/ada_feeding/ada_feeding/behaviors/ros/msgs.py b/ada_feeding/ada_feeding/behaviors/ros/msgs.py index ea4f9587..b85c23ff 100644 --- a/ada_feeding/ada_feeding/behaviors/ros/msgs.py +++ b/ada_feeding/ada_feeding/behaviors/ros/msgs.py @@ -38,6 +38,7 @@ import py_trees.blackboard from py_trees.common import Access, Status + class UpdateTimestamp(BlackboardBehavior): """ Adds a custom timestamp (or current timestamp) @@ -399,6 +400,7 @@ def update(self) -> py_trees.common.Status: self.blackboard_set("twist_stamped", twist_stamped) return py_trees.common.Status.SUCCESS + # -*- coding: utf-8 -*- # (Add appropriate Copyright/License if desired) @@ -408,6 +410,7 @@ def update(self) -> py_trees.common.Status: message by adding a header with a specified frame_id and the current timestamp. """ + class StampPoseFromPose(BlackboardBehavior): """ Takes a geometry_msgs/Pose from the blackboard, adds a header @@ -441,7 +444,7 @@ def blackboard_inputs( def blackboard_outputs( self, - output_pose_stamped: Optional[BlackboardKey], # -> Optional[PoseStamped] + output_pose_stamped: Optional[BlackboardKey], # -> Optional[PoseStamped] ) -> None: """ Blackboard Outputs @@ -461,9 +464,11 @@ def setup(self, **kwargs): """Gets the ROS2 node from the arguments passed by the tree runner.""" # pylint: disable=attribute-defined-outside-init try: - self.node: rclpy.node.Node = kwargs['node'] + self.node: rclpy.node.Node = kwargs["node"] except KeyError as e: - self.logger.error(f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}") + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) self.node = None @override @@ -471,7 +476,6 @@ def initialise(self) -> None: """Optionally log initialization.""" self.logger.debug(f"[{self.name}] Initializing.") - @override def update(self) -> Status: """ @@ -480,21 +484,27 @@ def update(self) -> Status: Returns SUCCESS if successful, FAILURE otherwise. """ # Verify setup provided the node needed for timestamping - if not hasattr(self, 'node') or self.node is None: - self.logger.error(f"[{self.name}] Node object not available. Setup likely failed.") - return Status.FAILURE + if not hasattr(self, "node") or self.node is None: + self.logger.error( + f"[{self.name}] Node object not available. Setup likely failed." + ) + return Status.FAILURE try: pose_in = self.blackboard_get("input_pose") frame_id_str = self.blackboard_get("frame_id") if not isinstance(pose_in, Pose): - self.logger.error(f"[{self.name}] Input 'input_pose' is not a geometry_msgs/Pose (is type: {type(pose_in)}).") - return Status.FAILURE + self.logger.error( + f"[{self.name}] Input 'input_pose' is not a geometry_msgs/Pose (is type: {type(pose_in)})." + ) + return Status.FAILURE if not isinstance(frame_id_str, str) or not frame_id_str: - self.logger.error(f"[{self.name}] Input 'frame_id' must be a non-empty string (is: '{frame_id_str}').") - return Status.FAILURE + self.logger.error( + f"[{self.name}] Input 'frame_id' must be a non-empty string (is: '{frame_id_str}')." + ) + return Status.FAILURE pose_stamped_out = PoseStamped() pose_stamped_out.header.frame_id = frame_id_str @@ -502,7 +512,9 @@ def update(self) -> Status: pose_stamped_out.pose = pose_in self.blackboard_set("output_pose_stamped", pose_stamped_out) - self.logger.info(f"[{self.name}] Successfully stamped Pose into frame '{frame_id_str}'.") + self.logger.info( + f"[{self.name}] Successfully stamped Pose into frame '{frame_id_str}'." + ) return Status.SUCCESS except KeyError as e: diff --git a/ada_feeding/ada_feeding/behaviors/ros/tf.py b/ada_feeding/ada_feeding/behaviors/ros/tf.py index 605a02bc..332eb242 100644 --- a/ada_feeding/ada_feeding/behaviors/ros/tf.py +++ b/ada_feeding/ada_feeding/behaviors/ros/tf.py @@ -348,8 +348,8 @@ def update(self) -> py_trees.common.Status: transformed_msg = None with self.tf_lock: if target_frame is not None: - import copy + msg_to_transform = copy.deepcopy(stamped_msg) # Set stamp to zero to request latest transform msg_to_transform.header.stamp = Time(seconds=0, nanoseconds=0).to_msg() diff --git a/ada_feeding/ada_feeding/behaviors/state/combine_joint_states.py b/ada_feeding/ada_feeding/behaviors/state/combine_joint_states.py index 49b68268..c1368ab7 100644 --- a/ada_feeding/ada_feeding/behaviors/state/combine_joint_states.py +++ b/ada_feeding/ada_feeding/behaviors/state/combine_joint_states.py @@ -1,4 +1,7 @@ # -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + # (Add appropriate Copyright/License if desired) """ @@ -53,7 +56,7 @@ def blackboard_inputs( def blackboard_outputs( self, - combined_joint_state: Optional[BlackboardKey], # -> Optional[JointState] + combined_joint_state: Optional[BlackboardKey], # -> Optional[JointState] ) -> None: """ Blackboard Outputs @@ -73,9 +76,11 @@ def setup(self, **kwargs): """Gets the ROS2 node from the arguments passed by the tree runner.""" # pylint: disable=attribute-defined-outside-init try: - self.node: rclpy.node.Node = kwargs['node'] + self.node: rclpy.node.Node = kwargs["node"] except KeyError as e: - self.logger.error(f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}") + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) self.node = None # initialise is not strictly needed, but good practice @@ -86,7 +91,6 @@ def initialise(self) -> None: # Clear previous output self.blackboard_set("combined_joint_state", None) - @override def update(self) -> Status: """ @@ -94,7 +98,9 @@ def update(self) -> Status: and writes the combined JointState. """ if self.node is None: - self.logger.error(f"[{self.name}] Node object not available. Setup likely failed.") + self.logger.error( + f"[{self.name}] Node object not available. Setup likely failed." + ) return Status.FAILURE try: @@ -105,33 +111,44 @@ def update(self) -> Status: # --- Input Validation --- if not isinstance(state1, JointState): - self.logger.error(f"[{self.name}] Input 'joint_state_1' is not a JointState message (is type: {type(state1)}).") - return Status.FAILURE + self.logger.error( + f"[{self.name}] Input 'joint_state_1' is not a JointState message (is type: {type(state1)})." + ) + return Status.FAILURE if not isinstance(state2, JointState): - self.logger.error(f"[{self.name}] Input 'joint_state_2' is not a JointState message (is type: {type(state2)}).") - return Status.FAILURE - if not isinstance(full_names, list) or not all(isinstance(n, str) for n in full_names): - self.logger.error(f"[{self.name}] Input 'full_joint_names' must be a List[str].") - return Status.FAILURE + self.logger.error( + f"[{self.name}] Input 'joint_state_2' is not a JointState message (is type: {type(state2)})." + ) + return Status.FAILURE + if not isinstance(full_names, list) or not all( + isinstance(n, str) for n in full_names + ): + self.logger.error( + f"[{self.name}] Input 'full_joint_names' must be a List[str]." + ) + return Status.FAILURE if not full_names: - self.logger.error(f"[{self.name}] Input 'full_joint_names' cannot be empty.") - return Status.FAILURE + self.logger.error( + f"[{self.name}] Input 'full_joint_names' cannot be empty." + ) + return Status.FAILURE # --- End Validation --- - # --- Merge Logic --- # Create a dictionary to hold combined positions, giving priority to state2 if names overlap combined_positions_dict = {} for name, pos in zip(state1.name, state1.position): combined_positions_dict[name] = pos for name, pos in zip(state2.name, state2.position): - combined_positions_dict[name] = pos # Overwrites if name was in state1 + combined_positions_dict[name] = pos # Overwrites if name was in state1 # Create the output JointState output_state = JointState() - output_state.header.stamp = self.node.get_clock().now().to_msg() # Use current time - output_state.name = full_names # Assign the full ordered list of names - output_state.position = [] # Initialize empty list + output_state.header.stamp = ( + self.node.get_clock().now().to_msg() + ) # Use current time + output_state.name = full_names # Assign the full ordered list of names + output_state.position = [] # Initialize empty list # Populate positions in the order specified by full_names all_found = True @@ -139,28 +156,36 @@ def update(self) -> Status: if name in combined_positions_dict: output_state.position.append(combined_positions_dict[name]) else: - self.logger.error(f"[{self.name}] Joint '{name}' from 'full_joint_names' not found in either input JointState.") + self.logger.error( + f"[{self.name}] Joint '{name}' from 'full_joint_names' not found in either input JointState." + ) all_found = False - break # Stop processing + break # Stop processing if not all_found: return Status.FAILURE # Basic check for consistency if len(output_state.name) != len(output_state.position): - self.logger.error(f"[{self.name}] Mismatch between output names ({len(output_state.name)}) and positions ({len(output_state.position)}). Logic error?") - return Status.FAILURE + self.logger.error( + f"[{self.name}] Mismatch between output names ({len(output_state.name)}) and positions ({len(output_state.position)}). Logic error?" + ) + return Status.FAILURE # --- End Merge Logic --- - # Write the successful result to the blackboard self.blackboard_set("combined_joint_state", output_state) - self.logger.info(f"[{self.name}] Successfully combined joint states for {len(output_state.name)} joints.") + self.logger.info( + f"[{self.name}] Successfully combined joint states for {len(output_state.name)} joints." + ) return Status.SUCCESS except KeyError as e: self.logger.error(f"[{self.name}] Blackboard key error: {e}") return Status.FAILURE except Exception as e: - self.logger.error(f"[{self.name}] Unexpected error combining joint states: {e}", exc_info=True) + self.logger.error( + f"[{self.name}] Unexpected error combining joint states: {e}", + exc_info=True, + ) return Status.FAILURE diff --git a/ada_feeding/ada_feeding/behaviors/state/extract_joints_from_state.py b/ada_feeding/ada_feeding/behaviors/state/extract_joints_from_state.py index 3ae02dac..019c1e52 100644 --- a/ada_feeding/ada_feeding/behaviors/state/extract_joints_from_state.py +++ b/ada_feeding/ada_feeding/behaviors/state/extract_joints_from_state.py @@ -1,5 +1,8 @@ # -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + """ This module defines the ExtractJointsFromState behavior, which extracts specific joint names and their corresponding positions from a JointState message. @@ -50,9 +53,9 @@ def blackboard_inputs( def blackboard_outputs( self, - output_joint_names: Optional[BlackboardKey], # -> Optional[List[str]] - output_joint_positions: Optional[BlackboardKey], # -> Optional[List[float]] - success: Optional[BlackboardKey], # -> bool (Optional success flag) + output_joint_names: Optional[BlackboardKey], # -> Optional[List[str]] + output_joint_positions: Optional[BlackboardKey], # -> Optional[List[float]] + success: Optional[BlackboardKey], # -> bool (Optional success flag) ) -> None: """ Blackboard Outputs @@ -79,7 +82,6 @@ def initialise(self) -> None: self.blackboard_set("output_joint_positions", None) self.blackboard_set("success", False) - @override def update(self) -> Status: """ @@ -91,22 +93,32 @@ def update(self) -> Status: target_names: List[str] = self.blackboard_get("target_joint_names") if not isinstance(source_state, JointState): - self.logger.error(f"[{self.name}] Input 'source_joint_state' is not a JointState message (is type: {type(source_state)}).") - return Status.FAILURE + self.logger.error( + f"[{self.name}] Input 'source_joint_state' is not a JointState message (is type: {type(source_state)})." + ) + return Status.FAILURE - if not isinstance(target_names, list) or not all(isinstance(n, str) for n in target_names): - self.logger.error(f"[{self.name}] Input 'target_joint_names' must be a List[str].") - return Status.FAILURE + if not isinstance(target_names, list) or not all( + isinstance(n, str) for n in target_names + ): + self.logger.error( + f"[{self.name}] Input 'target_joint_names' must be a List[str]." + ) + return Status.FAILURE if not target_names: - self.logger.warning(f"[{self.name}] Input 'target_joint_names' is empty. Nothing to extract.") - self.blackboard_set("output_joint_names", []) - self.blackboard_set("output_joint_positions", []) - self.blackboard_set("success", True) - return Status.SUCCESS + self.logger.warning( + f"[{self.name}] Input 'target_joint_names' is empty. Nothing to extract." + ) + self.blackboard_set("output_joint_names", []) + self.blackboard_set("output_joint_positions", []) + self.blackboard_set("success", True) + return Status.SUCCESS # Create a lookup dictionary for efficient access - source_positions_dict = {name: pos for name, pos in zip(source_state.name, source_state.position)} + source_positions_dict = { + name: pos for name, pos in zip(source_state.name, source_state.position) + } extracted_names = [] extracted_positions = [] @@ -117,7 +129,9 @@ def update(self) -> Status: extracted_names.append(name) extracted_positions.append(source_positions_dict[name]) else: - self.logger.error(f"[{self.name}] Target joint '{name}' not found in source_joint_state names: {list(source_positions_dict.keys())}") + self.logger.error( + f"[{self.name}] Target joint '{name}' not found in source_joint_state names: {list(source_positions_dict.keys())}" + ) all_found = False break @@ -125,7 +139,9 @@ def update(self) -> Status: self.blackboard_set("output_joint_names", extracted_names) self.blackboard_set("output_joint_positions", extracted_positions) self.blackboard_set("success", True) - self.logger.info(f"[{self.name}] Successfully extracted {len(extracted_names)} joints.") + self.logger.info( + f"[{self.name}] Successfully extracted {len(extracted_names)} joints." + ) return Status.SUCCESS else: self.blackboard_set("output_joint_names", None) @@ -138,6 +154,9 @@ def update(self) -> Status: self.blackboard_set("success", False) return Status.FAILURE except Exception as e: - self.logger.error(f"[{self.name}] Unexpected error during joint extraction: {e}", exc_info=True) + self.logger.error( + f"[{self.name}] Unexpected error during joint extraction: {e}", + exc_info=True, + ) self.blackboard_set("success", False) return Status.FAILURE diff --git a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py index 6ba469bf..c8c60989 100644 --- a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py +++ b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py @@ -1,3 +1,6 @@ +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + from rclpy.node import Node from sensor_msgs.msg import JointState import py_trees @@ -6,9 +9,16 @@ from ada_feeding.behaviors import BlackboardBehavior from ada_feeding.helpers import BlackboardKey + class GetJointStates(BlackboardBehavior): - def __init__(self, name: str, node=None, ns:str="/", inputs: Optional[Dict[str, Union[BlackboardKey, Any]]] = None, outputs: Optional[Dict[str, Optional[BlackboardKey]]] = None, -): + def __init__( + self, + name: str, + node=None, + ns: str = "/", + inputs: Optional[Dict[str, Union[BlackboardKey, Any]]] = None, + outputs: Optional[Dict[str, Optional[BlackboardKey]]] = None, + ): super().__init__(name, ns=ns, inputs=inputs, outputs=outputs) self.ns = ns self.node = node @@ -68,7 +78,10 @@ def joint_states_callback(self, msg: JointState): def update(self): # Check if we have the joint states we need - if all(name in self.latest_joint_states for name in self.blackboard_get("joint_names")): + if all( + name in self.latest_joint_states + for name in self.blackboard_get("joint_names") + ): joint_names = list(self.latest_joint_states.keys()) joint_positions = list(self.latest_joint_states.values()) self.node.get_logger().info(f"Received all joint states") diff --git a/ada_feeding/ada_feeding/helpers.py b/ada_feeding/ada_feeding/helpers.py index f7caf4e2..4a832312 100644 --- a/ada_feeding/ada_feeding/helpers.py +++ b/ada_feeding/ada_feeding/helpers.py @@ -387,7 +387,9 @@ def get_moveit2_object( blackboard.set(moveit2_blackboard_key, moveit2) blackboard.set(moveit2_lock_blackboard_key, lock) - node.get_logger().info(f"Successfully created and stored MoveIt2 object and lock for group '{group_name}'.") + node.get_logger().info( + f"Successfully created and stored MoveIt2 object and lock for group '{group_name}'." + ) return moveit2, lock diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index a520c0c2..57342db6 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -47,10 +47,16 @@ ServoMove, ToggleCollisionObject, ) -from ada_feeding.behaviors.state import GetJointStates, ExtractJointsFromState, CombineJointStates +from ada_feeding.behaviors.state import ( + GetJointStates, + ExtractJointsFromState, + CombineJointStates, +) from ada_feeding.behaviors.ros.msgs import StampPoseFromPose from ada_feeding.behaviors.ros.tf import ApplyTransform -from ada_feeding.behaviors.articutool.execute_articutool_trajectory import ExecuteArticutoolTrajectory +from ada_feeding.behaviors.articutool.execute_articutool_trajectory import ( + ExecuteArticutoolTrajectory, +) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( pre_moveto_config, @@ -479,57 +485,68 @@ def move_above_plan( ] }, outputs={ - "joint_positions": BlackboardKey("current_joint_positions"), - "joint_names": BlackboardKey("current_joint_names") - } + "joint_positions": BlackboardKey( + "current_joint_positions" + ), + "joint_names": BlackboardKey("current_joint_names"), + }, ), ), - # Convert Pose to PoseStamped using the defined frame_id StampPoseFromPose( name="StampMoveAbovePose", ns=name, inputs={ "input_pose": BlackboardKey("move_above_pose"), - "frame_id": "food" + "frame_id": "food", }, outputs={ - "output_pose_stamped": BlackboardKey("move_above_pose_stamped_food_frame") - } + "output_pose_stamped": BlackboardKey( + "move_above_pose_stamped_food_frame" + ) + }, ), - # Use ApplyTransform to transform into the MoveIt Planning Frame ApplyTransform( name="TransformPoseToIKFrame", ns=name, inputs={ - "stamped_msg": BlackboardKey("move_above_pose_stamped_food_frame"), + "stamped_msg": BlackboardKey( + "move_above_pose_stamped_food_frame" + ), "target_frame": "j2n6s200_link_base", }, outputs={ - "transformed_msg": BlackboardKey("move_above_pose_stamped_base_frame") - } + "transformed_msg": BlackboardKey( + "move_above_pose_stamped_base_frame" + ) + }, ), - # Compute IK for the target pose using the full jaco_arm_with_articutool planning group MoveIt2ComputeIK( name="ComputeJacoArmWithArticutoolIK", ns=name, inputs={ - "target_pose": BlackboardKey("move_above_pose_stamped_base_frame"), + "target_pose": BlackboardKey( + "move_above_pose_stamped_base_frame" + ), "group_name": "jaco_arm_with_articutool", # "start_joint_state": BlackboardKey("current_joint_positions"), }, outputs={ - "ik_solution_joint_state": BlackboardKey("move_above_ik_solution"), + "ik_solution_joint_state": BlackboardKey( + "move_above_ik_solution" + ), "success": BlackboardKey("move_above_ik_success"), - } + }, ), ExtractJointsFromState( name="ExtractJacoArmJoints", ns=name, inputs={ - "source_joint_state": BlackboardKey("move_above_ik_solution"), + "source_joint_state": BlackboardKey( + "move_above_ik_solution" + ), "target_joint_names": [ "j2n6s200_joint_1", "j2n6s200_joint_2", @@ -537,57 +554,79 @@ def move_above_plan( "j2n6s200_joint_4", "j2n6s200_joint_5", "j2n6s200_joint_6", - ] - + ], }, outputs={ - "output_joint_names": BlackboardKey("move_above_jaco_arm_joint_names"), - "output_joint_positions": BlackboardKey("move_above_jaco_arm_joint_positions"), - "success": BlackboardKey("extract_jaco_arm_joints_success") - } + "output_joint_names": BlackboardKey( + "move_above_jaco_arm_joint_names" + ), + "output_joint_positions": BlackboardKey( + "move_above_jaco_arm_joint_positions" + ), + "success": BlackboardKey("extract_jaco_arm_joints_success"), + }, ), ExtractJointsFromState( name="ExtractArticutoolJoints", ns=name, inputs={ - "source_joint_state": BlackboardKey("move_above_ik_solution"), + "source_joint_state": BlackboardKey( + "move_above_ik_solution" + ), "target_joint_names": [ "atool_joint1", "atool_joint2", - ] - + ], }, outputs={ - "output_joint_names": BlackboardKey("move_above_articutool_joint_names"), - "output_joint_positions": BlackboardKey("move_above_articutool_joint_positions"), - "success": BlackboardKey("extract_articutool_joints_success") - } + "output_joint_names": BlackboardKey( + "move_above_articutool_joint_names" + ), + "output_joint_positions": BlackboardKey( + "move_above_articutool_joint_positions" + ), + "success": BlackboardKey( + "extract_articutool_joints_success" + ), + }, ), MoveIt2JointConstraint( name="SetJacoArmJointConstraint", ns=name, inputs={ - "joint_positions": BlackboardKey("move_above_jaco_arm_joint_positions"), - "joint_names": BlackboardKey("move_above_jaco_arm_joint_names"), + "joint_positions": BlackboardKey( + "move_above_jaco_arm_joint_positions" + ), + "joint_names": BlackboardKey( + "move_above_jaco_arm_joint_names" + ), "tolerance": 0.001, "constraints": None, }, outputs={ - "constraints": BlackboardKey("move_above_jaco_arm_constraints"), - } + "constraints": BlackboardKey( + "move_above_jaco_arm_constraints" + ), + }, ), MoveIt2JointConstraint( name="SetArticutoolJointConstraint", ns=name, inputs={ - "joint_positions": BlackboardKey("move_above_articutool_joint_positions"), - "joint_names": BlackboardKey("move_above_articutool_joint_names"), + "joint_positions": BlackboardKey( + "move_above_articutool_joint_positions" + ), + "joint_names": BlackboardKey( + "move_above_articutool_joint_names" + ), "tolerance": 0.001, "constraints": None, }, outputs={ - "constraints": BlackboardKey("move_above_articutool_constraints"), - } + "constraints": BlackboardKey( + "move_above_articutool_constraints" + ), + }, ), ### Move Above Food py_trees.decorators.Timeout( @@ -598,7 +637,9 @@ def move_above_plan( name="MoveAboveJacoArmPlan", ns=name, inputs={ - "goal_constraints": BlackboardKey("move_above_jaco_arm_constraints"), + "goal_constraints": BlackboardKey( + "move_above_jaco_arm_constraints" + ), "max_velocity_scale": self.max_velocity_scaling_move_above, "max_acceleration_scale": self.max_acceleration_scaling_move_above, "allowed_planning_time": self.allowed_planning_time_for_move_above, @@ -606,8 +647,12 @@ def move_above_plan( "group_name": "jaco_arm", }, outputs={ - "trajectory": BlackboardKey("move_above_jaco_arm_trajectory"), - "end_joint_state": BlackboardKey("test_into_jaco_arm_joints"), + "trajectory": BlackboardKey( + "move_above_jaco_arm_trajectory" + ), + "end_joint_state": BlackboardKey( + "test_into_jaco_arm_joints" + ), }, ), ), @@ -619,7 +664,9 @@ def move_above_plan( name="MoveAboveArticutoolPlan", ns=name, inputs={ - "goal_constraints": BlackboardKey("move_above_articutool_constraints"), + "goal_constraints": BlackboardKey( + "move_above_articutool_constraints" + ), "max_velocity_scale": self.max_velocity_scaling_move_above, "max_acceleration_scale": self.max_acceleration_scaling_move_above, "allowed_planning_time": self.allowed_planning_time_for_move_above, @@ -627,8 +674,12 @@ def move_above_plan( "group_name": "articutool", }, outputs={ - "trajectory": BlackboardKey("move_above_articutool_trajectory"), - "end_joint_state": BlackboardKey("test_into_articutool_joints"), + "trajectory": BlackboardKey( + "move_above_articutool_trajectory" + ), + "end_joint_state": BlackboardKey( + "test_into_articutool_joints" + ), }, ), ), @@ -638,7 +689,9 @@ def move_above_plan( ns=name, inputs={ "joint_state_1": BlackboardKey("test_into_jaco_arm_joints"), - "joint_state_2": BlackboardKey("test_into_articutool_joints"), + "joint_state_2": BlackboardKey( + "test_into_articutool_joints" + ), "full_joint_names": [ "j2n6s200_joint_1", "j2n6s200_joint_2", @@ -652,7 +705,7 @@ def move_above_plan( }, outputs={ "combined_joint_state": BlackboardKey("test_into_joints"), - } + }, ), MoveIt2PoseConstraint( name="MoveIntoJacoArmWithArticutoolPose", @@ -669,27 +722,37 @@ def move_above_plan( name="ExtractTestIntoArticutoolJoints", ns=name, inputs={ - "source_joint_state": BlackboardKey("test_into_articutool_joints"), + "source_joint_state": BlackboardKey( + "test_into_articutool_joints" + ), "target_joint_names": ["atool_joint1", "atool_joint2"], }, outputs={ - "output_joint_names": BlackboardKey("test_into_articutool_joint_names"), - "output_joint_positions": BlackboardKey("test_into_articutool_joint_positions"), + "output_joint_names": BlackboardKey( + "test_into_articutool_joint_names" + ), + "output_joint_positions": BlackboardKey( + "test_into_articutool_joint_positions" + ), "success": None, - } + }, ), MoveIt2JointConstraint( name="SetArticutoolPathConstraint", ns=name, inputs={ - "joint_positions": BlackboardKey("test_into_articutool_joint_positions"), - "joint_names": BlackboardKey("test_into_articutool_joint_names"), + "joint_positions": BlackboardKey( + "test_into_articutool_joint_positions" + ), + "joint_names": BlackboardKey( + "test_into_articutool_joint_names" + ), "tolerance": 0.001, "constraints": None, }, outputs={ "constraints": BlackboardKey("move_into_path_constraints"), - } + }, ), py_trees.decorators.Timeout( name="MoveIntoJacoArmWithArticutoolPlanTimeout", @@ -699,7 +762,9 @@ def move_above_plan( name="MoveIntoJacoArmWithArticutoolPlan", ns=name, inputs={ - "goal_constraints": BlackboardKey("move_into_goal_constraints"), + "goal_constraints": BlackboardKey( + "move_into_goal_constraints" + ), # "path_constraints": BlackboardKey("move_into_path_constraints"), "max_velocity_scale": self.max_velocity_scaling_move_into, "max_acceleration_scale": self.max_acceleration_scaling_move_into, @@ -712,7 +777,9 @@ def move_above_plan( "group_name": "jaco_arm_with_articutool", }, outputs={ - "trajectory": BlackboardKey("move_into_jaco_arm_with_articutool_trajectory") + "trajectory": BlackboardKey( + "move_into_jaco_arm_with_articutool_trajectory" + ) }, ), ), @@ -834,13 +901,21 @@ def move_above_plan( name="MoveAboveArticutool", ns=name, inputs={ - "trajectory": BlackboardKey("move_above_articutool_trajectory"), + "trajectory": BlackboardKey( + "move_above_articutool_trajectory" + ), }, outputs={ - "action_goal_accepted": BlackboardKey("tool_goal_accepted"), - "action_result_code": BlackboardKey("tool_exec_result_code"), - "action_status": BlackboardKey("tool_action_status"), - } + "action_goal_accepted": BlackboardKey( + "tool_goal_accepted" + ), + "action_result_code": BlackboardKey( + "tool_exec_result_code" + ), + "action_status": BlackboardKey( + "tool_action_status" + ), + }, ), # If Anything goes wrong, reset FT to safe levels # scoped_behavior( 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 7f1855af..718ad98c 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 @@ -192,7 +192,7 @@ def create_tree( "allowed_planning_time": self.allowed_planning_time, "max_velocity_scale": self.max_velocity_scaling_factor, "ignore_violated_path_constraints": True, - "group_name": "jaco_arm" + "group_name": "jaco_arm", }, outputs={"trajectory": BlackboardKey("trajectory")}, ), From e295e6a64843ac2caf0f29f9a8149e3aec08b182 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Apr 2025 21:06:46 -0700 Subject: [PATCH 037/238] Implement behavior to extract a particular pose from a list of poses by link name --- .../state/extract_pose_from_poses_by_link.py | 179 ++++++++++++++++++ 1 file changed, 179 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/state/extract_pose_from_poses_by_link.py diff --git a/ada_feeding/ada_feeding/behaviors/state/extract_pose_from_poses_by_link.py b/ada_feeding/ada_feeding/behaviors/state/extract_pose_from_poses_by_link.py new file mode 100644 index 00000000..a4c4ee4a --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/extract_pose_from_poses_by_link.py @@ -0,0 +1,179 @@ +# -*- coding: utf-8 -*- +# (Add appropriate Copyright/License if desired) + +""" +Defines the ExtractPoseFromPosesByLink behavior, which finds and extracts +a specific PoseStamped from a list based on its associated link name. +""" + +# Standard imports +from typing import Union, Optional, Dict, Any, List + +# Third-party imports +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import JointState +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status +import rclpy.node + +# Local imports +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class ExtractPoseFromPosesByLink(BlackboardBehavior): + """ + Extracts a specific PoseStamped message from blackboard data which could be + either a single PoseStamped or a list of PoseStamped messages. + + If the input is a list, it requires a corresponding list of link names (which + should be the same list passed to the FK behavior that produced the pose list) + and a target link name to identify which pose to extract. + + If the input is a single PoseStamped, it checks if the target link name matches + expectations (either the single link name provided to FK or the default EE). + """ + + def blackboard_inputs( + self, + fk_poses: Union[BlackboardKey, PoseStamped, List[PoseStamped]], + target_link_name: Union[BlackboardKey, str], + requested_link_names: Optional[Union[BlackboardKey, List[str]]] = None, + default_ee_link_name: Optional[Union[BlackboardKey, str]] = None, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + fk_poses: BlackboardKey resolving to the output of MoveIt2ComputeFK + (either PoseStamped or List[PoseStamped]). + requested_link_names: BlackboardKey resolving to the List[str] that was passed + to MoveIt2ComputeFK's fk_link_names input. This is + REQUIRED if fk_poses is a List. Ignored if fk_poses + is a single PoseStamped. + target_link_name: BlackboardKey resolving to the string name of the link whose + pose should be extracted. + default_ee_link_name: Optional: BlackboardKey resolving to the default EE link name. + Used for validation when fk_poses is a single PoseStamped + and requested_link_names was empty/None. + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key not in ["self", "kwargs"]} + ) + + def blackboard_outputs( + self, + extracted_pose: Optional[BlackboardKey], # -> Optional[PoseStamped] + success: Optional[BlackboardKey], # -> bool + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + extracted_pose: BlackboardKey where the single extracted PoseStamped will be written. + success: Boolean flag indicating successful extraction. + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key not in ["self", "kwargs"]} + ) + + @override + def initialise(self) -> None: + """Clear output keys.""" + self.logger.debug(f"[{self.name}] Initializing.") + self.blackboard_set("extracted_pose", None) + self.blackboard_set("success", False) + + @override + def update(self) -> Status: + """Extract the pose corresponding to the target link name.""" + try: + fk_data = self.blackboard_get("fk_poses") + target_link = self.blackboard_get("target_link_name") + req_link_names = self.blackboard_get("requested_link_names") + default_ee = self.blackboard_get("default_ee_link_name") + + extracted_pose_result: Optional[PoseStamped] = None + found = False + + # Case 1: FK result is a single PoseStamped + if isinstance(fk_data, PoseStamped): + self.logger.debug(f"[{self.name}] FK result is single PoseStamped.") + # Check if the target link matches expectations + if req_link_names is None or (isinstance(req_link_names, list) and len(req_link_names) == 0): + # FK was likely called for the default EE + if default_ee is not None and target_link == default_ee: + extracted_pose_result = fk_data + found = True + elif default_ee is None: + self.logger.warning(f"[{self.name}] Input fk_poses is single PoseStamped, but default_ee_link_name not provided for validation.") + # Assume it's correct if only one link could have been requested + extracted_pose_result = fk_data + found = True + else: + self.logger.error(f"[{self.name}] Target link '{target_link}' does not match default EE '{default_ee}' for single FK result.") + elif isinstance(req_link_names, list) and len(req_link_names) == 1: + # FK was called for one specific link + if target_link == req_link_names[0]: + extracted_pose_result = fk_data + found = True + else: + self.logger.error(f"[{self.name}] Target link '{target_link}' does not match requested link '{req_link_names[0]}' for single FK result.") + else: + self.logger.error(f"[{self.name}] Input fk_poses is single PoseStamped, but requested_link_names is ambiguous: {req_link_names}") + + + # Case 2: FK result is a List of PoseStamped + elif isinstance(fk_data, list): + self.logger.debug(f"[{self.name}] FK result is a list.") + if not isinstance(req_link_names, list) or len(fk_data) != len(req_link_names): + self.logger.error(f"[{self.name}] 'fk_poses' is a list, but 'requested_link_names' is missing, not a list, or lengths mismatch (Poses: {len(fk_data)}, Names: {len(req_link_names or [])}).") + return self._handle_failure() + + # Find the index matching the target link name + try: + target_index = req_link_names.index(target_link) + if isinstance(fk_data[target_index], PoseStamped): + extracted_pose_result = fk_data[target_index] + found = True + else: + self.logger.error(f"[{self.name}] Data at index {target_index} for link '{target_link}' is not a PoseStamped.") + except ValueError: + self.logger.error(f"[{self.name}] Target link '{target_link}' not found in requested_link_names: {req_link_names}") + except IndexError: + self.logger.error(f"[{self.name}] Index mismatch error after finding target link '{target_link}'.") + + # Case 3: FK result is None or unexpected type + else: + self.logger.error(f"[{self.name}] Input 'fk_poses' is None or unexpected type ({type(fk_data)}). FK likely failed.") + return self._handle_failure() + + + # Write results and return status + if found and extracted_pose_result is not None: + self.blackboard_set("extracted_pose", extracted_pose_result) + self.blackboard_set("success", True) + self.logger.info(f"[{self.name}] Successfully extracted pose for link '{target_link}'.") + return Status.SUCCESS + else: + # Error logged within logic above + return self._handle_failure() + + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error: {e}") + return self._handle_failure() + except Exception as e: + self.logger.error(f"[{self.name}] Unexpected error extracting pose: {e}") + return self._handle_failure() + + + def _handle_failure(self) -> Status: + """Helper to set outputs on failure.""" + self.blackboard_set("extracted_pose", None) + self.blackboard_set("success", False) + return Status.FAILURE From 0e3e73567f327d08369ca20b8f1edac9596b9e92 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Apr 2025 22:40:56 -0700 Subject: [PATCH 038/238] (WIP) Working towards optimization-based solution where we find a good move above configuration that increases the likelihood of a successful move into motion assuming the Articutool joints are static --- .../ada_feeding/behaviors/state/__init__.py | 3 + .../ada_feeding/trees/acquire_food_tree.py | 95 +++++++++++++------ 2 files changed, 70 insertions(+), 28 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py index 3735f30a..9ef05956 100644 --- a/ada_feeding/ada_feeding/behaviors/state/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -13,3 +13,6 @@ from .combine_joint_states import ( CombineJointStates, ) +from .extract_pose_from_poses_by_link import ( + ExtractPoseFromPosesByLink, +) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 57342db6..11520055 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -44,6 +44,7 @@ MoveIt2Plan, MoveIt2Execute, MoveIt2ComputeIK, + MoveIt2ComputeFK, ServoMove, ToggleCollisionObject, ) @@ -51,6 +52,7 @@ GetJointStates, ExtractJointsFromState, CombineJointStates, + ExtractPoseFromPosesByLink, ) from ada_feeding.behaviors.ros.msgs import StampPoseFromPose from ada_feeding.behaviors.ros.tf import ApplyTransform @@ -707,65 +709,102 @@ def move_above_plan( "combined_joint_state": BlackboardKey("test_into_joints"), }, ), - MoveIt2PoseConstraint( - name="MoveIntoJacoArmWithArticutoolPose", + # Convert Pose to PoseStamped using the defined frame_id + StampPoseFromPose( + name="StampMoveIntoPose", ns=name, inputs={ - "pose": BlackboardKey("move_into_pose"), + "input_pose": BlackboardKey("move_into_pose"), "frame_id": "food", }, outputs={ - "constraints": BlackboardKey("move_into_goal_constraints"), + "output_pose_stamped": BlackboardKey( + "move_into_pose_stamped_food_frame" + ) }, ), - ExtractJointsFromState( - name="ExtractTestIntoArticutoolJoints", + # Use ApplyTransform to transform into the MoveIt Planning Frame + ApplyTransform( + name="TransformPoseToIKFrame", ns=name, inputs={ - "source_joint_state": BlackboardKey( - "test_into_articutool_joints" + "stamped_msg": BlackboardKey( + "move_into_pose_stamped_food_frame" ), - "target_joint_names": ["atool_joint1", "atool_joint2"], + "target_frame": "j2n6s200_link_base", }, outputs={ - "output_joint_names": BlackboardKey( - "test_into_articutool_joint_names" + "transformed_msg": BlackboardKey( + "move_into_pose_stamped_base_frame" + ) + }, + ), + # Compute IK for the target pose using the full jaco_arm_with_articutool planning group + MoveIt2ComputeIK( + name="ComputeJacoArmWithArticutoolIK", + ns=name, + inputs={ + "target_pose": BlackboardKey( + "move_into_pose_stamped_base_frame" ), - "output_joint_positions": BlackboardKey( - "test_into_articutool_joint_positions" + "group_name": "jaco_arm_with_articutool", + # "start_joint_state": BlackboardKey("current_joint_positions"), + }, + outputs={ + "ik_solution_joint_state": BlackboardKey( + "move_into_ik_solution" ), + "success": BlackboardKey("move_into_ik_success"), + }, + ), + MoveIt2ComputeFK( + name="ComputeMoveIntoJacoArmEEPose", + ns=name, + inputs={ + "group_name": "jaco_arm_with_articutool", + "joint_state": BlackboardKey("move_into_ik_solution"), + "fk_link_names": ["j2n6s200_end_effector"], + }, + outputs={ + "fk_poses": BlackboardKey("move_into_jaco_arm_ee_poses"), "success": None, + } + ), + ExtractPoseFromPosesByLink( + name="GetJacoArmEEPose", + ns=name, + inputs={ + "fk_poses": BlackboardKey("move_into_jaco_arm_ee_poses"), + "target_link_name": "j2n6s200_end_effector", + "requested_link_names": ["j2n6s200_end_effector"], }, + outputs={ + "extracted_pose": BlackboardKey("move_into_jaco_arm_ee_pose"), + "success": None, + } ), - MoveIt2JointConstraint( - name="SetArticutoolPathConstraint", + MoveIt2PoseConstraint( + name="MoveIntoJacoArmEEPoseConstraint", ns=name, inputs={ - "joint_positions": BlackboardKey( - "test_into_articutool_joint_positions" - ), - "joint_names": BlackboardKey( - "test_into_articutool_joint_names" - ), - "tolerance": 0.001, - "constraints": None, + "pose": BlackboardKey("move_into_jaco_arm_ee_pose"), + "frame_id": "j2n6s200_link_base", }, outputs={ - "constraints": BlackboardKey("move_into_path_constraints"), + "constraints": BlackboardKey("move_into_goal_constraints"), }, ), py_trees.decorators.Timeout( - name="MoveIntoJacoArmWithArticutoolPlanTimeout", + name="MoveIntoJacoArmEEPlanTimeout", # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such duration=10.0 * self.allowed_planning_time_for_move_into, child=MoveIt2Plan( - name="MoveIntoJacoArmWithArticutoolPlan", + name="MoveIntoJacoArmEEPlan", ns=name, inputs={ "goal_constraints": BlackboardKey( "move_into_goal_constraints" ), - # "path_constraints": BlackboardKey("move_into_path_constraints"), "max_velocity_scale": self.max_velocity_scaling_move_into, "max_acceleration_scale": self.max_acceleration_scaling_move_into, "cartesian": True, @@ -774,7 +813,7 @@ def move_above_plan( "start_joint_state": BlackboardKey("test_into_joints"), "max_path_len_joint": max_path_len_joint, "allowed_planning_time": self.allowed_planning_time_for_move_into, - "group_name": "jaco_arm_with_articutool", + "group_name": "jaco_arm", }, outputs={ "trajectory": BlackboardKey( From bce01095069593c381e6326b09093142219108f2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 21 Apr 2025 15:46:14 -0700 Subject: [PATCH 039/238] Modify MoveIt2ComputeIK to accept MoveIt2_X_Constraints, and apply an orientation constraint to the Move Above IK solution to keep the wrist from rotating in a way that would prevent the Articutool joints from being able to keep the tool tip moving in a linear motion during the Move Into motion --- .../behaviors/moveit2/moveit2_compute_ik.py | 44 ++++++++++++++++--- .../ada_feeding/trees/acquire_food_tree.py | 15 +++++++ 2 files changed, 54 insertions(+), 5 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py index d45435cc..c66bc3b8 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py @@ -33,6 +33,7 @@ from ada_feeding.behaviors import BlackboardBehavior from pymoveit2 import MoveIt2 +from .moveit2_plan import MoveIt2ConstraintType class MoveIt2ComputeIK(BlackboardBehavior): @@ -51,7 +52,7 @@ def blackboard_inputs( target_pose: Union[BlackboardKey, PoseStamped], group_name: Union[BlackboardKey, str], start_joint_state: Optional[Union[BlackboardKey, JointState]] = None, - ik_constraints: Optional[Union[BlackboardKey, Constraints]] = None, + constraints: Optional[Union[BlackboardKey, List[Tuple[MoveIt2ConstraintType, Dict[str, Any]]]]] = None, ) -> None: """ Blackboard Inputs @@ -165,12 +166,12 @@ def update(self) -> py_trees.common.Status: f"[{self.name}] Optional input 'start_joint_state' not found, using None." ) pass - ik_constraints = None + constraints = None try: - ik_constraints = self.blackboard_get("ik_constraints") + constraints = self.blackboard_get("constraints") except KeyError: self.logger.debug( - f"[{self.name}] Optional input 'ik_constraints' not found, using None." + f"[{self.name}] Optional input 'constraints' not found, using None." ) pass @@ -181,6 +182,39 @@ def update(self) -> py_trees.common.Status: ) return py_trees.common.Status.FAILURE + ik_constraints_msg : Optional[Constraints] = None + if isinstance(constraints, list) and constraints: + ik_constraints_msg = Constraints() + ik_constraints_msg.name = "ik_constraints_from_list" + self.logger.debug(f"[{self.name}] Processing {len(constraints)} constraint specifications for IK.") + for constraint_type, constraint_kwargs in constraints: + try: + if constraint_type == MoveIt2ConstraintType.JOINT: + # Assumes wrapper has create_joint_constraint(**kwargs) -> JointConstraint + constraint_obj = self.moveit2_obj.create_joint_constraint(**constraint_kwargs) + if constraint_obj: ik_constraints_msg.joint_constraints.append(constraint_obj) + elif constraint_type == MoveIt2ConstraintType.POSITION: + # Assumes wrapper has create_position_constraint(**kwargs) -> PositionConstraint + constraint_obj = self.moveit2_obj.create_position_constraint(**constraint_kwargs) + if constraint_obj: ik_constraints_msg.position_constraints.append(constraint_obj) + elif constraint_type == MoveIt2ConstraintType.ORIENTATION: + # Assumes wrapper has create_orientation_constraint(**kwargs) -> OrientationConstraint + constraint_obj = self.moveit2_obj.create_orientation_constraint(**constraint_kwargs) + if constraint_obj: ik_constraints_msg.orientation_constraints.append(constraint_obj) + else: + self.logger.warning(f"[{self.name}] Unknown constraint type '{constraint_type}' in list.") + + except AttributeError as ae: + self.logger.error(f"[{self.name}] MoveIt2 wrapper missing 'create_X_constraint' method for type {constraint_type}? Error: {ae}") + return py_trees.common.Status.FAILURE + except Exception as e_constr: + self.logger.error(f"[{self.name}] Error processing constraint {constraint_type} with args {constraint_kwargs}: {e_constr}") + return py_trees.common.Status.FAILURE + # Optional: Write generated message to blackboard for debugging + # self.blackboard_set("generated_ik_constraints_msg", ik_constraints_msg) + elif constraints is not None: + self.logger.warning(f"[{self.name}] Input 'constraints' is not a list, ignoring. Type: {type(constraints)}") + # --- Call the synchronous compute_ik method provided in the API --- # It takes position and orientation separately. self.logger.info( @@ -194,7 +228,7 @@ def update(self) -> py_trees.common.Status: position=target_pose_stamped.pose.position, quat_xyzw=target_pose_stamped.pose.orientation, start_joint_state=start_state_seed, - constraints=ik_constraints, + constraints=ik_constraints_msg, ) # Determine success based on whether a result was returned diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 11520055..b759c3ac 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -524,6 +524,20 @@ def move_above_plan( ) }, ), + MoveIt2OrientationConstraint( + name="DefineIKWristRollConstraint", + ns=name, + inputs={ + "target_link": "j2n6s200_end_effector", + "frame_id": "j2n6s200_link_base", + "quat_xyzw": [0.0, 0.0, 0.0, 1.0], + "tolerance": [2*np.pi, 2*np.pi, 0.5], + "constraints": None, + }, + outputs={ + "constraints": BlackboardKey("wrist_constraints"), + } + ), # Compute IK for the target pose using the full jaco_arm_with_articutool planning group MoveIt2ComputeIK( name="ComputeJacoArmWithArticutoolIK", @@ -534,6 +548,7 @@ def move_above_plan( ), "group_name": "jaco_arm_with_articutool", # "start_joint_state": BlackboardKey("current_joint_positions"), + "constraints": BlackboardKey("wrist_constraints"), }, outputs={ "ik_solution_joint_state": BlackboardKey( From 6b82d539b8e55b1332d30e7060d04102dd439265 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 21 Apr 2025 15:53:03 -0700 Subject: [PATCH 040/238] Increase planning time for Move Above --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index b759c3ac..ef9c126b 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -107,7 +107,7 @@ def __init__( max_velocity_scaling_to_resting_configuration: Optional[float] = 0.8, max_acceleration_scaling_to_resting_configuration: Optional[float] = 0.8, pickle_goal_path: Optional[str] = None, - allowed_planning_time_for_move_above: float = 0.5, + allowed_planning_time_for_move_above: float = 1.0, allowed_planning_time_for_move_into: float = 0.5, allowed_planning_time_to_resting_configuration: float = 0.5, allowed_planning_time_for_recovery: float = 0.5, From 38931eeeba73ee85fed14f27f3854d1b83c59c8b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 21 Apr 2025 19:04:15 -0700 Subject: [PATCH 041/238] Temporarily disable max_path_len for j2n6s200_joint_1 and j2n6s200_joint_2 --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index ef9c126b..06961e8e 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -176,8 +176,8 @@ def create_tree( # The max amount that each joint can move for any computed plan. Intended # to reduce swivels. max_path_len_joint = { - "j2n6s200_joint_1": np.pi * 5.0 / 6.0, - "j2n6s200_joint_2": np.pi / 2.0, + # "j2n6s200_joint_1": np.pi * 5.0 / 6.0, + # "j2n6s200_joint_2": np.pi / 2.0, } # Get the base lin to publish servo commands in From ba8e3b3a00f015a4ef2e5b2832ec21964eedefde Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 24 Apr 2025 15:18:03 -0700 Subject: [PATCH 042/238] Re-enable move into motion, but don't replan beforehand --- .../ada_feeding/trees/acquire_food_tree.py | 734 +++++++----------- 1 file changed, 274 insertions(+), 460 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 06961e8e..448deb0f 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -832,7 +832,7 @@ def move_above_plan( }, outputs={ "trajectory": BlackboardKey( - "move_into_jaco_arm_with_articutool_trajectory" + "move_into_jaco_arm_trajectory" ) }, ), @@ -972,465 +972,279 @@ def move_above_plan( }, ), # If Anything goes wrong, reset FT to safe levels - # scoped_behavior( - # name="SafeFTPreempt", - # # Set Approach F/T Thresh - # 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")] - # ), - # response_checks=[ - # py_trees.common.ComparisonExpression( - # variable=Blackboard.separator.join( - # [ - # name, - # BlackboardKey( - # "ft_response" - # ), - # ] - # ), - # value=SetParameters.Response(), # Unused - # operator=set_parameter_response_all_success, - # ) - # ], - # ), - # ], - # ), - # post_behavior=py_trees.composites.Sequence( - # name=name, - # memory=True, - # children=[ - # pre_moveto_config( - # name="PostAcquireFTSet", re_tare=False - # ), - # ], - # ), - # on_preempt_timeout=5.0, - # # Starts a new Sequence w/ Memory internally - # workers=[ - # ### Move Into Food - # # Convert Pose to PoseStamped using the defined frame_id - # StampPoseFromPose( - # name="StampMoveIntoPose", - # ns=name, - # inputs={ - # "input_pose": BlackboardKey("move_into_pose"), - # "frame_id": "food" - # }, - # outputs={ - # "output_pose_stamped": BlackboardKey("move_into_pose_stamped_food_frame") - # } - # ), - # - # # Use ApplyTransform to transform into the MoveIt Planning Frame - # ApplyTransform( - # name="TransformPoseToIKFrame", - # ns=name, - # inputs={ - # "stamped_msg": BlackboardKey("move_into_pose_stamped_food_frame"), - # "target_frame": "j2n6s200_link_base", - # }, - # outputs={ - # "transformed_msg": BlackboardKey("move_into_pose_stamped_base_frame") - # } - # ), - # - # # Compute IK for the target pose using the full jaco_arm_with_articutool planning group - # MoveIt2ComputeIK( - # name="ComputeJacoArmWithArticutoolIK", - # ns=name, - # inputs={ - # "target_pose": BlackboardKey("move_into_pose_stamped_base_frame"), - # "group_name": "jaco_arm_with_articutool", - # # "start_joint_state": BlackboardKey("current_joint_positions"), - # }, - # outputs={ - # "ik_solution_joint_state": BlackboardKey("move_into_ik_solution"), - # "success": BlackboardKey("move_into_ik_success"), - # } - # ), - # ExtractJointsFromState( - # name="ExtractJacoArmJoints", - # ns=name, - # inputs={ - # "source_joint_state": BlackboardKey("move_into_ik_solution"), - # "target_joint_names": [ - # "j2n6s200_joint_1", - # "j2n6s200_joint_2", - # "j2n6s200_joint_3", - # "j2n6s200_joint_4", - # "j2n6s200_joint_5", - # "j2n6s200_joint_6", - # ] - # - # }, - # outputs={ - # "output_joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), - # "output_joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), - # "success": BlackboardKey("extract_jaco_arm_joints_success") - # } - # ), - # ExtractJointsFromState( - # name="ExtractArticutoolJoints", - # ns=name, - # inputs={ - # "source_joint_state": BlackboardKey("move_into_ik_solution"), - # "target_joint_names": [ - # "atool_joint1", - # "atool_joint2", - # ] - # - # }, - # outputs={ - # "output_joint_names": BlackboardKey("move_into_articutool_joint_names"), - # "output_joint_positions": BlackboardKey("move_into_articutool_joint_positions"), - # "success": BlackboardKey("extract_articutool_joints_success") - # } - # ), - # MoveIt2JointConstraint( - # name="SetJacoArmJointConstraint", - # ns=name, - # inputs={ - # "joint_positions": BlackboardKey("move_into_jaco_arm_joint_positions"), - # "joint_names": BlackboardKey("move_into_jaco_arm_joint_names"), - # "tolerance": 0.001, - # "constraints": None, - # }, - # outputs={ - # "constraints": BlackboardKey("move_into_jaco_arm_constraints"), - # } - # ), - # MoveIt2JointConstraint( - # name="SetArticutoolJointConstraint", - # ns=name, - # inputs={ - # "joint_positions": BlackboardKey("move_into_articutool_joint_positions"), - # "joint_names": BlackboardKey("move_into_articutool_joint_names"), - # "tolerance": 0.001, - # "constraints": None, - # }, - # outputs={ - # "constraints": BlackboardKey("move_into_articutool_constraints"), - # } - # ), - # # If this fails - # # Auto-fallback to precomputed MoveInto - # # From move_above_plan() - # py_trees.decorators.FailureIsSuccess( - # name="MoveIntoJacoArmPlanFallbackPrecomputed", - # child=py_trees.decorators.Timeout( - # name="MoveIntoJacoArmPlanTimeout", - # # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such - # duration=10.0 * self.allowed_planning_time_for_move_into, - # child=MoveIt2Plan( - # name="MoveIntoJacoArmPlan", - # ns=name, - # inputs={ - # "goal_constraints": BlackboardKey("move_into_jaco_arm_constraints"), - # "max_velocity_scale": self.max_velocity_scaling_move_into, - # "max_acceleration_scale": self.max_acceleration_scaling_move_into, - # "cartesian": True, - # "cartesian_max_step": 0.001, - # "cartesian_fraction_threshold": 0.92, - # "start_joint_state": BlackboardKey("test_into_jaco_arm_joints"), - # "max_path_len_joint": max_path_len_joint, - # "allowed_planning_time": self.allowed_planning_time_for_move_into, - # "group_name": "jaco_arm", - # }, - # outputs={ - # "trajectory": BlackboardKey("move_into_jaco_arm_trajectory") - # }, - # ), - # ), - # ), - # py_trees.decorators.FailureIsSuccess( - # name="MoveIntoArticutoolPlanFallbackPrecomputed", - # child=py_trees.decorators.Timeout( - # name="MoveIntoArticutoolPlanTimeout", - # # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such - # duration=10.0 * self.allowed_planning_time_for_move_into, - # child=MoveIt2Plan( - # name="MoveIntoArticutoolPlan", - # ns=name, - # inputs={ - # "goal_constraints": BlackboardKey("move_into_articutool_constraints"), - # "max_velocity_scale": self.max_velocity_scaling_move_into, - # "max_acceleration_scale": self.max_acceleration_scaling_move_into, - # "cartesian": True, - # "cartesian_max_step": 0.001, - # "cartesian_fraction_threshold": 0.92, - # "start_joint_state": BlackboardKey("test_into_articutool_joints"), - # "max_path_len_joint": max_path_len_joint, - # "allowed_planning_time": self.allowed_planning_time_for_move_into, - # "group_name": "articutool", - # }, - # outputs={ - # "trajectory": BlackboardKey("move_into_articutool_trajectory") - # }, - # ), - # ), - # ), - # # MoveInto expect F/T failure - # ExecuteArticutoolTrajectory( - # name="MoveAboveArticutool", - # ns=name, - # inputs={ - # "trajectory": BlackboardKey("move_into_articutool_trajectory"), - # }, - # outputs={ - # "action_goal_accepted": BlackboardKey("tool_goal_accepted"), - # "action_result_code": BlackboardKey("tool_exec_result_code"), - # "action_status": BlackboardKey("tool_action_status"), - # } - # ), - # # py_trees.decorators.FailureIsSuccess( - # # name="MoveIntoArticutoolExecuteSucceed", - # # child=MoveIt2Execute( - # # name="MoveIntoArticutool", - # # ns=name, - # # inputs={ - # # "trajectory": BlackboardKey( - # # "move_into_articutool_trajectory" - # # ) - # # }, - # # outputs={}, - # # ), - # # ), - # py_trees.decorators.FailureIsSuccess( - # name="MoveIntoJacoArmExecuteSucceed", - # child=MoveIt2Execute( - # name="MoveIntoJacoArm", - # ns=name, - # inputs={ - # "trajectory": BlackboardKey( - # "move_into_jaco_arm_trajectory" - # ) - # }, - # outputs={}, - # ), - # ), - # ### 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, - # servo_controller_name="jaco_arm_cartesian_controller", - # start_moveit_servo=False, - # ) - # .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_cartesian_controller_parameters", - # ), - # StopServoTree( - # self._node, - # servo_controller_name="jaco_arm_cartesian_controller", - # stop_moveit_servo=False, - # ) - # .create_tree(name="StopServoScoped") - # .root, - # ], - # ), - # on_preempt_timeout=5.0, - # # Starts a new Sequence w/ Memory internally - # workers=[ - # py_trees.composites.Selector( - # name="InFoodErrorSelector", - # memory=True, - # children=[ - # py_trees.composites.Sequence( - # name="InFoodGraspExtract", - # memory=True, - # children=[ - # ### Grasp - # retry_call_ros_service( - # name="GraspFTThresh", - # service_type=SetParameters, - # service_name="~/set_cartesian_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, - # ) - # ], - # ), - # 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" - # ), - # "pub_topic": "~/cartesian_twist_cmds", - # "servo_status_sub_topic": None, - # }, - # ), # Auto Zero-Twist on terminate() - # ### Extraction - # ComputeActionTwist( - # name="ComputeExtract", - # 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_cartesian_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" - # ), - # "pub_topic": "~/cartesian_twist_cmds", - # "servo_status_sub_topic": None, - # }, - # ), # Auto Zero-Twist on terminate() - # ft_thresh_satisfied( - # name="CheckFTForkOffPlate" - # ), - # ], # End InFoodGraspExtract.children - # ), # End InFoodGraspExtract - # recovery_tree, - # ], # End InFoodErrorSelector.children - # ), # End InFoodErrorSelector - # ], # End MoveIt2Servo.workers - # ), # End MoveIt2Servo - # ], # End SafeFTPreempt.workers - # ), # End SafeFTPreempt + scoped_behavior( + name="SafeFTPreempt", + # Set Approach F/T Thresh + 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")] + ), + response_checks=[ + py_trees.common.ComparisonExpression( + variable=Blackboard.separator.join( + [ + name, + BlackboardKey( + "ft_response" + ), + ] + ), + value=SetParameters.Response(), # Unused + operator=set_parameter_response_all_success, + ) + ], + ), + ], + ), + post_behavior=py_trees.composites.Sequence( + name=name, + memory=True, + children=[ + pre_moveto_config( + name="PostAcquireFTSet", re_tare=False + ), + ], + ), + on_preempt_timeout=5.0, + # Starts a new Sequence w/ Memory internally + workers=[ + ### Move Into Food + # MoveInto expect F/T failure + py_trees.decorators.FailureIsSuccess( + name="MoveIntoJacoArmExecuteSucceed", + child=MoveIt2Execute( + name="MoveIntoJacoArm", + ns=name, + inputs={ + "trajectory": BlackboardKey( + "move_into_jaco_arm_trajectory" + ) + }, + outputs={}, + ), + ), + ### 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, + # servo_controller_name="jaco_arm_cartesian_controller", + # start_moveit_servo=False, + # ) + # .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_cartesian_controller_parameters", + # ), + # StopServoTree( + # self._node, + # servo_controller_name="jaco_arm_cartesian_controller", + # stop_moveit_servo=False, + # ) + # .create_tree(name="StopServoScoped") + # .root, + # ], + # ), + # on_preempt_timeout=5.0, + # # Starts a new Sequence w/ Memory internally + # workers=[ + # py_trees.composites.Selector( + # name="InFoodErrorSelector", + # memory=True, + # children=[ + # py_trees.composites.Sequence( + # name="InFoodGraspExtract", + # memory=True, + # children=[ + # ### Grasp + # retry_call_ros_service( + # name="GraspFTThresh", + # service_type=SetParameters, + # service_name="~/set_cartesian_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, + # ) + # ], + # ), + # 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" + # ), + # "pub_topic": "~/cartesian_twist_cmds", + # "servo_status_sub_topic": None, + # }, + # ), # Auto Zero-Twist on terminate() + # ### Extraction + # ComputeActionTwist( + # name="ComputeExtract", + # 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_cartesian_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" + # ), + # "pub_topic": "~/cartesian_twist_cmds", + # "servo_status_sub_topic": None, + # }, + # ), # Auto Zero-Twist on terminate() + # ft_thresh_satisfied( + # name="CheckFTForkOffPlate" + # ), + # ], # End InFoodGraspExtract.children + # ), # End InFoodGraspExtract + # recovery_tree, + # ], # End InFoodErrorSelector.children + # ), # End InFoodErrorSelector + # ], # End MoveIt2Servo.workers + # ), # End MoveIt2Servo + ], # End SafeFTPreempt.workers + ), # End SafeFTPreempt ], # End OctomapAndTableCollision.workers ), # OctomapAndTableCollision ] From a0bd4cf0cd5a851a8631c7ba331cbdc614cac87f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 24 Apr 2025 16:06:34 -0700 Subject: [PATCH 043/238] Add behavior to call the SetOrientationControl service on the Articutool. Currently, we pass the entire PoseStamped in the base frame and extract the orientation to command, but in the future we should probably just pass the explicit orientation in the world frame we wish to achieve --- .../behaviors/articutool/__init__.py | 3 + .../call_set_orientation_control.py | 200 ++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 14 ++ 3 files changed, 217 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py diff --git a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py index 09033ed1..21ade188 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py @@ -4,3 +4,6 @@ from .execute_articutool_trajectory import ( ExecuteArticutoolTrajectory, ) +from .call_set_orientation_control import ( + CallSetOrientationControl, +) diff --git a/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py new file mode 100644 index 00000000..15751680 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py @@ -0,0 +1,200 @@ +# -*- coding: utf-8 -*- +# (Add appropriate Copyright/License if desired) + +""" +Defines the CallSetOrientationControl behavior, which calls the service +to enable/disable the Articutool orientation controller. +""" + +import rclpy +from rclpy.node import Node +from rclpy.executors import Future # For type hints +from rclpy.duration import Duration +from rclpy.executors import ExternalShutdownException +from typing import Union, Optional, Dict, Any, List, Tuple # Added Tuple just in case + +from geometry_msgs.msg import PoseStamped, Quaternion # Need Quaternion if creating default PoseStamped +# Import your service definition (adjust path as needed) +from articutool_interfaces.srv import SetOrientationControl + +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status + +# Local imports (adjust paths as needed) +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class CallSetOrientationControl(BlackboardBehavior): + """ + Calls the SetOrientationControl service to enable/disable the + Articutool's orientation controller and optionally set its target pose/orientation. + + Returns SUCCESS if the service call completes and the service response indicates success. + Returns FAILURE if the service is unavailable, the call fails, or the service response indicates failure. + Returns RUNNING while waiting for the service response. + """ + + DEFAULT_SERVICE_NAME = "/articutool/set_orientation_control" + DEFAULT_WAIT_TIMEOUT_SEC = 1.0 + + def blackboard_inputs( + self, + enable: Union[BlackboardKey, bool], + target_pose: Union[BlackboardKey, PoseStamped], + service_name: Union[BlackboardKey, str] = DEFAULT_SERVICE_NAME, + wait_for_server_timeout_sec: Union[BlackboardKey, float] = DEFAULT_WAIT_TIMEOUT_SEC, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + enable: True to enable orientation control, False to disable. + target_pose: The PoseStamped message. The service uses pose.orientation as the + target and header.frame_id as its reference frame, transforming + it internally to the controller's reference frame. Position is ignored. + Provide a valid PoseStamped even when enable=False (can be default). + service_name: Name of the SetOrientationControl service. + wait_for_server_timeout_sec: Max time (sec) to wait for server in initial check. + Use 0.0 or negative to skip check (not recommended). + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + service_call_succeeded: Optional[BlackboardKey] = None, # -> bool (Did call complete?) + service_response_success: Optional[BlackboardKey] = None, # -> bool (Payload 'success' field) + service_response_message: Optional[BlackboardKey] = None, # -> str (Payload 'message' field) + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + service_call_succeeded: True if the service call finished without client-side errors/timeouts. + service_response_success: The boolean 'success' field from the service response payload. + service_response_message: The string 'message' field from the service response payload. + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def setup(self, **kwargs): + """Get the node and create the service client.""" + # pylint: disable=attribute-defined-outside-init + self.node: Node = kwargs['node'] + self.client = None # Initialize client to None + try: + self.service_name = self.blackboard_get("service_name") + self.wait_timeout = self.blackboard_get("wait_for_server_timeout_sec") + self.client = self.node.create_client(SetOrientationControl, self.service_name) + self.logger.info(f"[{self.name}] Service client created for '{self.service_name}'") + except KeyError as e: + self.logger.error(f"[{self.name}] Missing blackboard key during setup: {e}") + except Exception as e: + self.logger.error(f"[{self.name}] Failed to create service client: {e}") + + def initialise(self) -> None: + """Reset the future.""" + # pylint: disable=attribute-defined-outside-init + self.service_future: Optional[Future] = None + self.logger.debug(f"[{self.name}] Initializing service call state.") + # Clear blackboard outputs + self.blackboard_set("service_call_succeeded", False) + self.blackboard_set("service_response_success", False) + self.blackboard_set("service_response_message", "") + + def update(self) -> Status: + """Manage the asynchronous service call state machine.""" + if self.client is None: + self.logger.error(f"[{self.name}] Service client not available. Setup failed?") + return Status.FAILURE + + # --- State 1: Ready to Call --- + if self.service_future is None: + # Check server availability (only if timeout > 0) + wait_timeout = self.wait_timeout # Use timeout from setup + if wait_timeout >= 0.0: + if not self.client.wait_for_service(timeout_sec=wait_timeout): + self.logger.error(f"[{self.name}] Service '{self.service_name}' not available after {wait_timeout}s timeout.") + return Status.FAILURE # Service unavailable is a failure + + try: + # Read inputs from blackboard + enable_flag = self.blackboard_get("enable") + target_pose_stamped = self.blackboard_get("target_pose") + + # Input validation + if not isinstance(enable_flag, bool): + self.logger.error(f"[{self.name}] Input 'enable' type mismatch: expected bool, got {type(enable_flag)}.") + return Status.FAILURE + if not isinstance(target_pose_stamped, PoseStamped): + self.logger.error(f"[{self.name}] Input 'target_pose' type mismatch: expected PoseStamped, got {type(target_pose_stamped)}.") + return Status.FAILURE + + # Create request + req = SetOrientationControl.Request() + req.enable = enable_flag + req.target_orientation = target_pose_stamped.pose.orientation + + # Call service asynchronously + self.logger.info(f"[{self.name}] Calling service '{self.service_name}' (enable={req.enable})...") + self.service_future = self.client.call_async(req) + return Status.RUNNING # Waiting for response + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error preparing service call: {e}") + return Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Error preparing service call: {e}") + return Status.FAILURE + + # --- State 2: Waiting for Response --- + elif not self.service_future.done(): + self.logger.debug(f"[{self.name}] Waiting for service response...") + return Status.RUNNING + + # --- State 3: Processing Response --- + else: + self.logger.debug(f"[{self.name}] Service call future done.") + response = None + try: + response = self.service_future.result() # Get the response payload + self.blackboard_set("service_call_succeeded", True) # Call itself completed + + if response is not None: + self.blackboard_set("service_response_success", response.success) + self.blackboard_set("service_response_message", response.message) + self.logger.info(f"[{self.name}] Service response: success={response.success}, msg='{response.message}'") + final_status = Status.SUCCESS if response.success else Status.FAILURE + else: + # Should not happen if future.result() didn't raise exception + self.logger.error(f"[{self.name}] Service call future done but result is None.") + self.blackboard_set("service_response_success", False) + final_status = Status.FAILURE + + except Exception as e: + # Error during call execution on server or getting result + self.logger.error(f"[{self.name}] Service call failed with exception: {e}") + self.blackboard_set("service_call_succeeded", False) # Call failed + self.blackboard_set("service_response_success", False) + self.blackboard_set("service_response_message", f"Exception: {e}") + final_status = Status.FAILURE + + # Reset future regardless of outcome for next tick + self.service_future = None + return final_status + + def terminate(self, new_status: Status) -> None: + """Log termination status and clear future if needed.""" + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") + # If terminated while waiting for response, clear the future + if self.service_future is not None and not self.service_future.done(): + self.logger.warning(f"[{self.name}] Service call terminated while waiting for response.") + # We can't easily cancel a service call future, just abandon it + self.service_future = None diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 448deb0f..56923949 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -59,6 +59,9 @@ from ada_feeding.behaviors.articutool.execute_articutool_trajectory import ( ExecuteArticutoolTrajectory, ) +from ada_feeding.behaviors.articutool.call_set_orientation_control import ( + CallSetOrientationControl, +) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( pre_moveto_config, @@ -1027,6 +1030,17 @@ def move_above_plan( # Starts a new Sequence w/ Memory internally workers=[ ### Move Into Food + CallSetOrientationControl( + name="SetArticutoolOrientation", + ns=name, + inputs={ + "enable": True, + "target_pose": BlackboardKey("move_into_pose_stamped_base_frame"), + }, + outputs={ + + }, + ), # MoveInto expect F/T failure py_trees.decorators.FailureIsSuccess( name="MoveIntoJacoArmExecuteSucceed", From c1c9e6e744777ee8167e19d858b106c497d57f37 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 24 Apr 2025 17:08:50 -0700 Subject: [PATCH 044/238] Add behavior to switch the Articutool's controller, and activate the velocity controller before the move into motion --- .../behaviors/articutool/__init__.py | 3 + .../switch_articutool_controllers.py | 229 ++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 15 ++ 3 files changed, 247 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/articutool/switch_articutool_controllers.py diff --git a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py index 21ade188..17af1b81 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py @@ -7,3 +7,6 @@ from .call_set_orientation_control import ( CallSetOrientationControl, ) +from .switch_articutool_controllers import ( + SwitchArticutoolControllers, +) diff --git a/ada_feeding/ada_feeding/behaviors/articutool/switch_articutool_controllers.py b/ada_feeding/ada_feeding/behaviors/articutool/switch_articutool_controllers.py new file mode 100644 index 00000000..8ddb5066 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/articutool/switch_articutool_controllers.py @@ -0,0 +1,229 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# (Add appropriate Copyright/License if desired) + +""" +Defines the SwitchArticutoolControllers behavior, which calls the standard +controller_manager service to switch controllers for the Articutool. +""" + +import rclpy +from rclpy.node import Node +from rclpy.executors import Future # For type hints +from rclpy.duration import Duration + +# Import the standard controller manager service definition +from controller_manager_msgs.srv import SwitchController + +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status + +# Import base class and helpers (Adjust path as needed) +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + +# Typing imports +from typing import Union, Optional, Dict, Any, List + + +class SwitchArticutoolControllers(BlackboardBehavior): + """ + Calls the /articutool/controller_manager/switch_controller service + to activate and deactivate specified controllers. + + Returns SUCCESS if the service call completes and the response indicates success (ok=True). + Returns FAILURE if the service is unavailable, the call fails, or the response indicates failure. + Returns RUNNING while waiting for the service response. + """ + + # Define default values used if not provided via blackboard inputs during instantiation + DEFAULT_SERVICE_NAME = "/articutool/controller_manager/switch_controller" + DEFAULT_WAIT_TIMEOUT_SEC = 2.0 # Timeout for wait_for_service + DEFAULT_STRICTNESS = SwitchController.Request.BEST_EFFORT # Default: 1 (STRICT=2) + + def blackboard_inputs( + self, + controllers_to_activate: Union[BlackboardKey, List[str]], + controllers_to_deactivate: Union[BlackboardKey, List[str]], + strictness: Union[BlackboardKey, int] = DEFAULT_STRICTNESS, + service_name: Union[BlackboardKey, str] = DEFAULT_SERVICE_NAME, + wait_for_server_timeout_sec: Union[BlackboardKey, float] = DEFAULT_WAIT_TIMEOUT_SEC, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + controllers_to_activate: List of controller names to activate (start). + controllers_to_deactivate: List of controller names to deactivate (stop). + strictness: Strictness level for the switch operation. Options are + SwitchController.Request.BEST_EFFORT (1) or + SwitchController.Request.STRICT (2). + service_name: Name of the controller manager's switch_controller service. + wait_for_server_timeout_sec: Max time (sec) to wait for server in initial check. + Use 0.0 or negative to skip check. + """ + # Use the base class method to register keys/defaults + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + switch_call_succeeded: Optional[BlackboardKey] = None, # -> bool (Did call complete?) + switch_response_ok: Optional[BlackboardKey] = None, # -> bool (Payload 'ok' field) + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + switch_call_succeeded: True if the service call finished without client-side errors. + switch_response_ok: The boolean 'ok' field from the SwitchController response payload, + indicating if the controller manager performed the switch successfully. + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + """Get node and create the service client.""" + # pylint: disable=attribute-defined-outside-init + self.node: Node = kwargs['node'] + self.client: Optional[rclpy.client.Client] = None + try: + # Read configuration parameters using the blackboard helper method + self.service_name = self.blackboard_get("service_name") + self.wait_timeout = self.blackboard_get("wait_for_server_timeout_sec") + + # Create client for the standard controller manager service + self.client = self.node.create_client(SwitchController, self.service_name) + self.logger.info(f"[{self.name}] Service client created for '{self.service_name}'") + + except KeyError as e: + self.logger.error(f"[{self.name}] Missing blackboard key during setup: {e}") + self.client = None # Ensure client is None if setup fails + except Exception as e: + self.logger.error(f"[{self.name}] Failed to create service client: {e}") + self.client = None # Ensure client is None if setup fails + + + @override + def initialise(self) -> None: + """Reset the future before potentially calling the service.""" + # pylint: disable=attribute-defined-outside-init + self.service_future: Optional[Future] = None + self.logger.debug(f"[{self.name}] Initializing controller switch call state.") + # Clear outputs to default non-success state + self.blackboard_set("switch_call_succeeded", False) + self.blackboard_set("switch_response_ok", False) + + + @override + def update(self) -> Status: + """Manage the asynchronous service call state machine.""" + if self.client is None: + self.logger.error(f"[{self.name}] Service client not available. Setup failed?") + return Status.FAILURE + + # --- State 1: Ready to Call --- + if self.service_future is None: + # Check server availability (only if timeout >= 0) + wait_timeout = self.wait_timeout + if wait_timeout >= 0.0: + if not self.client.wait_for_service(timeout_sec=wait_timeout): + self.logger.error(f"[{self.name}] Service '{self.service_name}' not available after {wait_timeout}s timeout.") + return Status.FAILURE # Service unavailable is a failure + + try: + # Read inputs from blackboard + activate_list = self.blackboard_get("controllers_to_activate") + deactivate_list = self.blackboard_get("controllers_to_deactivate") + strict_level = self.blackboard_get("strictness") + + # Basic Type Validation + if not isinstance(activate_list, list) or not all(isinstance(s, str) for s in activate_list): + self.logger.error(f"[{self.name}] Input 'controllers_to_activate' must be a List[str].") + return Status.FAILURE + if not isinstance(deactivate_list, list) or not all(isinstance(s, str) for s in deactivate_list): + self.logger.error(f"[{self.name}] Input 'controllers_to_deactivate' must be a List[str].") + return Status.FAILURE + if not isinstance(strict_level, int): + self.logger.error(f"[{self.name}] Input 'strictness' must be an integer.") + return Status.FAILURE + if strict_level not in [SwitchController.Request.BEST_EFFORT, SwitchController.Request.STRICT]: + self.logger.warning(f"[{self.name}] Invalid strictness value {strict_level}, using BEST_EFFORT.") + strict_level = SwitchController.Request.BEST_EFFORT + + # Create request + req = SwitchController.Request() + req.activate_controllers = activate_list + req.deactivate_controllers = deactivate_list + req.strictness = strict_level + req.activate_asap = False # Usually False for planned switches + req.timeout = Duration(seconds=5.0).to_msg() # Server-side timeout within CM + + # Call service async + self.logger.info(f"[{self.name}] Calling switch_controller: " + f"Activate={req.activate_controllers}, Deactivate={req.deactivate_controllers}, Strictness={req.strictness}") + self.service_future = self.client.call_async(req) + return Status.RUNNING # Waiting for response + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error: {e}") + return Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Error preparing service call: {e}") + return Status.FAILURE + + # --- State 2: Waiting for Response --- + elif not self.service_future.done(): + self.logger.debug(f"[{self.name}] Waiting for switch_controller response...") + return Status.RUNNING + + # --- State 3: Processing Response --- + else: + self.logger.info(f"[{self.name}] switch_controller call finished.") + response = None + call_succeeded = False # Did the call itself complete without client-side error? + response_ok = False # Did the service response payload indicate success? + try: + response = self.service_future.result() # Get the response payload + call_succeeded = True # If we get here, the call itself completed + + if response is not None: + response_ok = response.ok # Check the 'ok' field in the response + self.logger.info(f"[{self.name}] switch_controller response: ok={response_ok}") + else: + self.logger.error(f"[{self.name}] Service call future done but result is None.") + response_ok = False + + except Exception as e: + # Error during call execution on server or getting result + self.logger.error(f"[{self.name}] Service call failed with exception: {e}") + call_succeeded = False + response_ok = False + + # Write outputs + self.blackboard_set("switch_call_succeeded", call_succeeded) + self.blackboard_set("switch_response_ok", response_ok) + + # Reset future + self.service_future = None + + # Return status based on response 'ok' field + return Status.SUCCESS if response_ok else Status.FAILURE + + + @override + def terminate(self, new_status: Status) -> None: + """Log termination status and clear future if needed.""" + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") + # If terminated while waiting for response, clear the future + if self.service_future is not None and not self.service_future.done(): + self.logger.warning(f"[{self.name}] Terminated while waiting for switch_controller response.") + # We can't easily cancel a service call future, just abandon it + self.service_future = None diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 56923949..3d3c43ba 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -62,6 +62,9 @@ from ada_feeding.behaviors.articutool.call_set_orientation_control import ( CallSetOrientationControl, ) +from ada_feeding.behaviors.articutool.switch_articutool_controllers import ( + SwitchArticutoolControllers, +) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( pre_moveto_config, @@ -1030,6 +1033,18 @@ def move_above_plan( # Starts a new Sequence w/ Memory internally workers=[ ### Move Into Food + SwitchArticutoolControllers( + name="SwitchArticutoolToVelocity", + ns=name, + inputs={ + "controllers_to_activate": ["velocity_controller"], + "controllers_to_deactivate": ["joint_trajectory_controller"], + }, + outputs={ + "switch_call_succeeded": None, + "switch_response_ok": None, + } + ), CallSetOrientationControl( name="SetArticutoolOrientation", ns=name, From c2079e254a79f2429bbf90ef6b1b4c749d2850f7 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 24 Apr 2025 17:34:11 -0700 Subject: [PATCH 045/238] Re-enable grasp and extract twists, we may need to revisit how these are defined since the Cartesian controller is now relative to the Jaco end-effector, not the fork tip --- .../ada_feeding/trees/acquire_food_tree.py | 402 +++++++++--------- 1 file changed, 201 insertions(+), 201 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 3d3c43ba..eebe253a 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1071,207 +1071,207 @@ def move_above_plan( ), ), ### 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, - # servo_controller_name="jaco_arm_cartesian_controller", - # start_moveit_servo=False, - # ) - # .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_cartesian_controller_parameters", - # ), - # StopServoTree( - # self._node, - # servo_controller_name="jaco_arm_cartesian_controller", - # stop_moveit_servo=False, - # ) - # .create_tree(name="StopServoScoped") - # .root, - # ], - # ), - # on_preempt_timeout=5.0, - # # Starts a new Sequence w/ Memory internally - # workers=[ - # py_trees.composites.Selector( - # name="InFoodErrorSelector", - # memory=True, - # children=[ - # py_trees.composites.Sequence( - # name="InFoodGraspExtract", - # memory=True, - # children=[ - # ### Grasp - # retry_call_ros_service( - # name="GraspFTThresh", - # service_type=SetParameters, - # service_name="~/set_cartesian_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, - # ) - # ], - # ), - # 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" - # ), - # "pub_topic": "~/cartesian_twist_cmds", - # "servo_status_sub_topic": None, - # }, - # ), # Auto Zero-Twist on terminate() - # ### Extraction - # ComputeActionTwist( - # name="ComputeExtract", - # 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_cartesian_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" - # ), - # "pub_topic": "~/cartesian_twist_cmds", - # "servo_status_sub_topic": None, - # }, - # ), # Auto Zero-Twist on terminate() - # ft_thresh_satisfied( - # name="CheckFTForkOffPlate" - # ), - # ], # End InFoodGraspExtract.children - # ), # End InFoodGraspExtract - # recovery_tree, - # ], # End InFoodErrorSelector.children - # ), # End InFoodErrorSelector - # ], # End MoveIt2Servo.workers - # ), # End MoveIt2Servo + scoped_behavior( + name="MoveIt2Servo", + # Set Approach F/T Thresh + pre_behavior=py_trees.composites.Sequence( + name=name, + memory=True, + children=[ + StartServoTree( + self._node, + servo_controller_name="jaco_arm_cartesian_controller", + start_moveit_servo=False, + ) + .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_cartesian_controller_parameters", + ), + StopServoTree( + self._node, + servo_controller_name="jaco_arm_cartesian_controller", + stop_moveit_servo=False, + ) + .create_tree(name="StopServoScoped") + .root, + ], + ), + on_preempt_timeout=5.0, + # Starts a new Sequence w/ Memory internally + workers=[ + py_trees.composites.Selector( + name="InFoodErrorSelector", + memory=True, + children=[ + py_trees.composites.Sequence( + name="InFoodGraspExtract", + memory=True, + children=[ + ### Grasp + retry_call_ros_service( + name="GraspFTThresh", + service_type=SetParameters, + service_name="~/set_cartesian_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, + ) + ], + ), + 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" + ), + "pub_topic": "~/cartesian_twist_cmds", + "servo_status_sub_topic": None, + }, + ), # Auto Zero-Twist on terminate() + ### Extraction + ComputeActionTwist( + name="ComputeExtract", + 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_cartesian_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" + ), + "pub_topic": "~/cartesian_twist_cmds", + "servo_status_sub_topic": None, + }, + ), # Auto Zero-Twist on terminate() + ft_thresh_satisfied( + name="CheckFTForkOffPlate" + ), + ], # End InFoodGraspExtract.children + ), # End InFoodGraspExtract + recovery_tree, + ], # End InFoodErrorSelector.children + ), # End InFoodErrorSelector + ], # End MoveIt2Servo.workers + ), # End MoveIt2Servo ], # End SafeFTPreempt.workers ), # End SafeFTPreempt ], # End OctomapAndTableCollision.workers From 2795dddd3aeaf1076258267f7fad643e663c097b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 24 Apr 2025 17:58:27 -0700 Subject: [PATCH 046/238] Restore resting position behavior. Add behavior to extract components of a Pose or PoseStamped object, including position, orientation, and header information --- .../ada_feeding/behaviors/state/__init__.py | 3 + .../state/extract_pose_components.py | 152 ++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 16 +- 3 files changed, 170 insertions(+), 1 deletion(-) create mode 100644 ada_feeding/ada_feeding/behaviors/state/extract_pose_components.py diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py index 9ef05956..86571439 100644 --- a/ada_feeding/ada_feeding/behaviors/state/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -16,3 +16,6 @@ from .extract_pose_from_poses_by_link import ( ExtractPoseFromPosesByLink, ) +from .extract_pose_components import ( + ExtractPoseComponents, +) diff --git a/ada_feeding/ada_feeding/behaviors/state/extract_pose_components.py b/ada_feeding/ada_feeding/behaviors/state/extract_pose_components.py new file mode 100644 index 00000000..b8f61954 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/extract_pose_components.py @@ -0,0 +1,152 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# (Add appropriate Copyright/License if desired) + +""" +Defines the ExtractPoseComponents behavior, which separates Pose/PoseStamped +into its constituent parts. +""" + +import rclpy +from rclpy.node import Node +from geometry_msgs.msg import Pose, PoseStamped, Point, Quaternion +from std_msgs.msg import Header # For outputting header if PoseStamped +from typing import Union, Optional, Dict, Any + +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status + +# Local imports (adjust paths as needed) +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class ExtractPoseComponents(BlackboardBehavior): + """ + Extracts Position and Orientation components from an input Pose or PoseStamped + object stored on the blackboard. Optionally outputs the Header if the input + is PoseStamped. + + Returns SUCCESS if extraction is possible, FAILURE otherwise. + """ + + def blackboard_inputs( + self, + input_pose_object: Union[BlackboardKey, Pose, PoseStamped], + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + input_pose_object: The blackboard key resolving to the Pose or PoseStamped message + from which to extract components. + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + output_position: Optional[BlackboardKey] = None, # -> Optional[Point] + output_orientation: Optional[BlackboardKey] = None, # -> Optional[Quaternion] + output_header: Optional[BlackboardKey] = None, # -> Optional[Header] + success: Optional[BlackboardKey] = None, # -> bool + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + output_position: Blackboard key to write the extracted geometry_msgs/Point. + output_orientation: Blackboard key to write the extracted geometry_msgs/Quaternion. + output_header: Optional key to write the std_msgs/Header if input was PoseStamped. + If input was Pose, this key will be set to None. + success: Optional key to write boolean success flag (True if extraction succeeded). + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs): + """Get the node if needed (e.g., for logging).""" + # pylint: disable=attribute-defined-outside-init + try: + # Node access is useful for logging via self.logger + self.node: Node = kwargs['node'] + except KeyError: + # Fallback logger if node isn't passed (though BlackboardBehavior likely handles this) + self.logger.warning(f"[{self.name}] Node not explicitly provided via setup kwargs.") + self.node = None + + @override + def initialise(self) -> None: + """Clear output keys on the blackboard before execution.""" + self.logger.debug(f"[{self.name}] Initializing and clearing outputs.") + # Use blackboard_set helper which handles key existence checks based on blackboard_outputs registration + self.blackboard_set("output_position", None) + self.blackboard_set("output_orientation", None) + self.blackboard_set("output_header", None) + self.blackboard_set("success", False) + + + @override + def update(self) -> Status: + """Read input, extract components based on type, write outputs.""" + extracted_pos: Optional[Point] = None + extracted_ori: Optional[Quaternion] = None + extracted_header: Optional[Header] = None + op_success = False # Assume failure initially + + try: + # Read the input object from the blackboard + pose_obj = self.blackboard_get("input_pose_object") + + # Check the type and extract components + if isinstance(pose_obj, PoseStamped): + self.logger.debug(f"[{self.name}] Input is PoseStamped. Extracting pose and header.") + extracted_pos = pose_obj.pose.position + extracted_ori = pose_obj.pose.orientation + extracted_header = pose_obj.header # Get the header + op_success = True + elif isinstance(pose_obj, Pose): + self.logger.debug(f"[{self.name}] Input is Pose. Extracting components.") + extracted_pos = pose_obj.position + extracted_ori = pose_obj.orientation + extracted_header = None # No header available for Pose type + op_success = True + else: + # Log error if input is neither Pose nor PoseStamped + self.logger.error(f"[{self.name}] Input 'input_pose_object' is not Pose or PoseStamped (type: {type(pose_obj)}). Cannot extract components.") + op_success = False + + except KeyError as e: + # Error if the input blackboard key doesn't exist + self.logger.error(f"[{self.name}] Blackboard key error reading input: {e}") + op_success = False + except AttributeError as e: + # Error if the object exists but doesn't have expected fields (e.g., None value) + self.logger.error(f"[{self.name}] Attribute error accessing pose components (is input object None?): {e}") + op_success = False + except Exception as e: + # Catch any other unexpected errors + self.logger.error(f"[{self.name}] Unexpected error extracting pose components: {e}") + op_success = False + + # Write results (or None on failure) to blackboard output keys + # blackboard_set handles checking if the output key was defined + self.blackboard_set("output_position", extracted_pos) + self.blackboard_set("output_orientation", extracted_ori) + self.blackboard_set("output_header", extracted_header) + self.blackboard_set("success", op_success) + + # Return SUCCESS or FAILURE based on whether extraction worked + return Status.SUCCESS if op_success else Status.FAILURE + + @override + def terminate(self, new_status: Status) -> None: + """Log termination.""" + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index eebe253a..a3b32188 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -53,6 +53,7 @@ ExtractJointsFromState, CombineJointStates, ExtractPoseFromPosesByLink, + ExtractPoseComponents, ) from ada_feeding.behaviors.ros.msgs import StampPoseFromPose from ada_feeding.behaviors.ros.tf import ApplyTransform @@ -1045,6 +1046,19 @@ def move_above_plan( "switch_response_ok": None, } ), + ExtractPoseComponents( + name="GetMoveIntoOrientation", + ns=name, + inputs={ + "input_pose_object": BlackboardKey("move_into_pose_stamped_base_frame"), + }, + outputs={ + "output_position": BlackboardKey("move_into_position"), + "output_orientation": BlackboardKey("move_into_orientation"), + "output_header": BlackboardKey("move_into_header"), + "success": None, + } + ), CallSetOrientationControl( name="SetArticutoolOrientation", ns=name, @@ -1277,7 +1291,7 @@ def move_above_plan( ], # End OctomapAndTableCollision.workers ), # OctomapAndTableCollision ] - # + resting_position_behaviors, # End Success.workers + + resting_position_behaviors, # End Success.workers ), # End Success # TableCollision ], # End root_seq.children ) # End root_seq From d525e4bd503aa07772682cd35e788ece471c9696 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 24 Apr 2025 18:13:28 -0700 Subject: [PATCH 047/238] Change CallSetOrientationControl to accept a quaternion (object, tuple, list) instead of a Pose object --- .../call_set_orientation_control.py | 40 ++++++++++++++----- .../ada_feeding/trees/acquire_food_tree.py | 2 +- 2 files changed, 31 insertions(+), 11 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py index 15751680..0d2358de 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py @@ -21,6 +21,7 @@ import py_trees import py_trees.blackboard from py_trees.common import Access, Status +import numpy as np # Local imports (adjust paths as needed) from ada_feeding.helpers import BlackboardKey @@ -43,7 +44,7 @@ class CallSetOrientationControl(BlackboardBehavior): def blackboard_inputs( self, enable: Union[BlackboardKey, bool], - target_pose: Union[BlackboardKey, PoseStamped], + quat_xyzw: Union[BlackboardKey, PoseStamped], service_name: Union[BlackboardKey, str] = DEFAULT_SERVICE_NAME, wait_for_server_timeout_sec: Union[BlackboardKey, float] = DEFAULT_WAIT_TIMEOUT_SEC, ) -> None: @@ -53,10 +54,9 @@ def blackboard_inputs( Parameters ---------- enable: True to enable orientation control, False to disable. - target_pose: The PoseStamped message. The service uses pose.orientation as the - target and header.frame_id as its reference frame, transforming - it internally to the controller's reference frame. Position is ignored. - Provide a valid PoseStamped even when enable=False (can be default). + quat_xyzw: The target orientation, provided as a geometry_msgs/Quaternion + message or an [x, y, z, w] list/tuple. Must be relative + to the frame expected by the orientation controller node. service_name: Name of the SetOrientationControl service. wait_for_server_timeout_sec: Max time (sec) to wait for server in initial check. Use 0.0 or negative to skip check (not recommended). @@ -127,20 +127,40 @@ def update(self) -> Status: try: # Read inputs from blackboard enable_flag = self.blackboard_get("enable") - target_pose_stamped = self.blackboard_get("target_pose") + quat_xyzw = self.blackboard_get("quat_xyzw") # Input validation if not isinstance(enable_flag, bool): self.logger.error(f"[{self.name}] Input 'enable' type mismatch: expected bool, got {type(enable_flag)}.") return Status.FAILURE - if not isinstance(target_pose_stamped, PoseStamped): - self.logger.error(f"[{self.name}] Input 'target_pose' type mismatch: expected PoseStamped, got {type(target_pose_stamped)}.") - return Status.FAILURE + + target_quat_msg = Quaternion() + if isinstance(quat_xyzw, Quaternion): + target_quat_msg = quat_xyzw + self.logger.debug(f"[{self.name}] Using Quaternion message input.") + elif isinstance(quat_xyzw, (list, tuple)) and len(quat_xyzw) == 4: + self.logger.debug(f"[{self.name}] Converting list/tuple input to Quaternion.") + try: + target_quat_msg.x = float(quat_xyzw[0]) + target_quat_msg.y = float(quat_xyzw[1]) + target_quat_msg.z = float(quat_xyzw[2]) + target_quat_msg.w = float(quat_xyzw[3]) + norm = np.linalg.norm([target_quat_msg.x, target_quat_msg.y, target_quat_msg.z, target_quat_msg.w]) + if not np.isclose(norm, 1.0, atol=0.01): self.logger.warning(f"Input quat norm is {norm:.3f}") + except (ValueError, TypeError, IndexError) as e: + self.logger.error(f"Could not convert list/tuple {quat_xyzw} to Quaternion: {e}"); return Status.FAILURE + else: + if enable_flag: # Target is mandatory if enabling + self.logger.error(f"[{self.name}] Input 'target_orientation_input' invalid type {type(quat_xyzw)} when enable=True.") + return Status.FAILURE + else: # Use default identity if disabling + self.logger.debug(f"[{self.name}] Using default identity quaternion because enable=False.") + target_quat_msg = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) # Create request req = SetOrientationControl.Request() req.enable = enable_flag - req.target_orientation = target_pose_stamped.pose.orientation + req.target_orientation = target_quat_msg # Call service asynchronously self.logger.info(f"[{self.name}] Calling service '{self.service_name}' (enable={req.enable})...") diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index a3b32188..39675a53 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1064,7 +1064,7 @@ def move_above_plan( ns=name, inputs={ "enable": True, - "target_pose": BlackboardKey("move_into_pose_stamped_base_frame"), + "quat_xyzw": BlackboardKey("move_into_orientation"), }, outputs={ From 9a1a41b6ee5b6de7063064ed1b1a615ce728e5bb Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 29 Apr 2025 17:03:57 -0700 Subject: [PATCH 048/238] After bite acquisition, and before moving to the resting configuration, we home the Articutool to its zero configuration and enable orientation control to keep the tool tip in an orientation that will reduce the likelihood of dropping food --- .../ada_feeding/trees/acquire_food_tree.py | 76 +++++++++++++++++++ 1 file changed, 76 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 39675a53..dc4e5cad 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -213,6 +213,82 @@ def create_tree( # Default fail if service is down wait_for_server_timeout_sec=0.0, ), + MoveIt2JointConstraint( + name="HomeArticutoolConstraint", + ns=name, + inputs={ + "joint_positions": [0.0, 0.0], + "joint_names": ["atool_joint1", "atool_joint2"], + }, + outputs={ + "constraints": BlackboardKey("goal_constraints"), + }, + ), + py_trees.decorators.Timeout( + name="HomeArticutoolPlanTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 + * self.allowed_planning_time_to_resting_configuration, + child=MoveIt2Plan( + name="HomeArticutoolPlan", + ns=name, + inputs={ + "goal_constraints": BlackboardKey( + "goal_constraints" + ), + "max_velocity_scale": self.max_velocity_scaling_to_resting_configuration, + "max_acceleration_scale": self.max_acceleration_scaling_to_resting_configuration, + "allowed_planning_time": self.allowed_planning_time_to_resting_configuration, + "group_name": "articutool", + }, + outputs={ + "trajectory": BlackboardKey("home_articutool_trajectory") + }, + ), + ), + ExecuteArticutoolTrajectory( + name="HomeArticutool", + ns=name, + inputs={ + "trajectory": BlackboardKey( + "home_articutool_trajectory" + ), + }, + outputs={ + "action_goal_accepted": BlackboardKey( + "tool_goal_accepted" + ), + "action_result_code": BlackboardKey( + "tool_exec_result_code" + ), + "action_status": BlackboardKey( + "tool_action_status" + ), + }, + ), + SwitchArticutoolControllers( + name="SwitchArticutoolToVelocity", + ns=name, + inputs={ + "controllers_to_activate": ["velocity_controller"], + "controllers_to_deactivate": ["joint_trajectory_controller"], + }, + outputs={ + "switch_call_succeeded": None, + "switch_response_ok": None, + } + ), + CallSetOrientationControl( + name="SetArticutoolOrientation", + ns=name, + inputs={ + "enable": True, + "quat_xyzw": [0.5, 0.5, 0.5, 0.5], + }, + outputs={ + + }, + ), MoveIt2JointConstraint( name="RestingConstraint", ns=name, From 08c4e6ba6f2549554272e3648bbdc12a34ec6cdf Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 29 Apr 2025 18:33:09 -0700 Subject: [PATCH 049/238] Disable Articutool orientation control after completing a MoveTo action --- ...ove_to_configuration_with_ft_thresholds_tree.py | 14 ++++++++++++++ ...e_to_configuration_with_wheelchair_wall_tree.py | 14 ++++++++++++++ 2 files changed, 28 insertions(+) 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 06e2ccec..5e7d2426 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 @@ -23,6 +23,9 @@ MoveIt2JointConstraint, ) from ada_feeding.behaviors.state import GetJointStates +from ada_feeding.behaviors.articutool.call_set_orientation_control import ( + CallSetOrientationControl, +) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import pre_moveto_config, scoped_behavior from ada_feeding.idioms.bite_transfer import get_toggle_watchdog_listener_behavior @@ -203,6 +206,17 @@ def create_tree( }, outputs={}, ), + CallSetOrientationControl( + name="SetArticutoolOrientation", + ns=name, + inputs={ + "enable": False, + "quat_xyzw": None, + }, + outputs={ + + }, + ), ], ) 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 718ad98c..d3425b93 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 @@ -28,6 +28,9 @@ MoveIt2OrientationConstraint, ) from ada_feeding.behaviors.state import GetJointStates +from ada_feeding.behaviors.articutool.call_set_orientation_control import ( + CallSetOrientationControl, +) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import pre_moveto_config, scoped_behavior from ada_feeding.idioms.bite_transfer import ( @@ -207,6 +210,17 @@ def create_tree( }, outputs={}, ), + CallSetOrientationControl( + name="SetArticutoolOrientation", + ns=name, + inputs={ + "enable": False, + "quat_xyzw": None, + }, + outputs={ + + }, + ), ], ), ], From 11d7289ea76eae8d96a576ff4cf2929a596e564c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 30 Apr 2025 21:24:05 -0700 Subject: [PATCH 050/238] Reinstate max_path_len_joint for Jaco joint 1 and 2, but increase limit for joint 1 since that seems to increase chance of planning success with Articutool --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index dc4e5cad..3ecaca89 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -185,6 +185,8 @@ def create_tree( max_path_len_joint = { # "j2n6s200_joint_1": np.pi * 5.0 / 6.0, # "j2n6s200_joint_2": np.pi / 2.0, + "j2n6s200_joint_1": np.pi, + "j2n6s200_joint_2": np.pi / 2.0, } # Get the base lin to publish servo commands in From eb337ac0f3f65ed18e966adbe6211faad3683904 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 1 May 2025 09:34:22 -0700 Subject: [PATCH 051/238] Adjust Jaco joint 2 limits for planning --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 3ecaca89..1fad9605 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -186,7 +186,7 @@ def create_tree( # "j2n6s200_joint_1": np.pi * 5.0 / 6.0, # "j2n6s200_joint_2": np.pi / 2.0, "j2n6s200_joint_1": np.pi, - "j2n6s200_joint_2": np.pi / 2.0, + "j2n6s200_joint_2": np.pi / 1.5, } # Get the base lin to publish servo commands in From 6f92d04d6354a2dd198a2baa8bcb340068b394f5 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 1 May 2025 09:50:20 -0700 Subject: [PATCH 052/238] Specify jaco_arm group when planning recovery motion --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 1fad9605..1eb4b222 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -448,6 +448,7 @@ def create_tree( "cartesian_max_step": 0.001, "cartesian_fraction_threshold": 0.92, "allowed_planning_time": self.allowed_planning_time_for_recovery, + "group_name": "jaco_arm", }, outputs={ "trajectory": BlackboardKey( @@ -462,7 +463,8 @@ def create_tree( inputs={ "trajectory": BlackboardKey( "recovery_trajectory" - ) + ), + "group_name": "jaco_arm", }, outputs={}, ), From 1908fab51eba9a30d4992ea0ebfedd0e7d6fca91 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 1 May 2025 11:59:46 -0700 Subject: [PATCH 053/238] Remove exc_info --- ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py | 1 - 1 file changed, 1 deletion(-) diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py index c66bc3b8..70c04a56 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py @@ -255,7 +255,6 @@ def update(self) -> py_trees.common.Status: except Exception as e: self.logger.error( f"[{self.name}] Unexpected error during IK computation: {e}", - exc_info=True, ) return py_trees.common.Status.FAILURE From 6b5e5695ef99b705d38f66bd70d75f0bfdad8ae0 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 1 May 2025 12:11:20 -0700 Subject: [PATCH 054/238] Return aggregate JointState from GetJointStates behavior, and use current joint state when computing IK solution for move above --- .../behaviors/state/get_joint_states.py | 11 ++++ .../ada_feeding/trees/acquire_food_tree.py | 59 ++++++++++--------- 2 files changed, 41 insertions(+), 29 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py index c8c60989..7ab6e04a 100644 --- a/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py +++ b/ada_feeding/ada_feeding/behaviors/state/get_joint_states.py @@ -45,6 +45,7 @@ def blackboard_inputs( def blackboard_outputs( self, + joint_state: Union[BlackboardKey, JointState] = None, joint_positions: Union[BlackboardKey, List[float]] = None, joint_names: Union[BlackboardKey, Optional[List[str]]] = None, ) -> None: @@ -84,9 +85,19 @@ def update(self): ): joint_names = list(self.latest_joint_states.keys()) joint_positions = list(self.latest_joint_states.values()) + + # Create the JointState message object + joint_state_msg = JointState() + joint_state_msg.header.stamp = self.node.get_clock().now().to_msg() + + # Add frame_id if known/relevant, e.g., joint_state_msg.header.frame_id = "base_link" + joint_state_msg.name = joint_names + joint_state_msg.position = joint_positions + self.node.get_logger().info(f"Received all joint states") self.node.get_logger().info(f"Joint Names: {joint_names}") self.node.get_logger().info(f"Joint Positions: {joint_positions}") + self.blackboard_set("joint_state", joint_state_msg) self.blackboard_set("joint_names", joint_names) self.blackboard_set("joint_positions", joint_positions) return py_trees.common.Status.SUCCESS diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 1eb4b222..1b4e6cf4 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -553,34 +553,6 @@ def move_above_plan( ), # Re-Tare FT Sensor and default to 4N threshold pre_moveto_config(name="PreAcquireFTTare"), - # Get Current Joint State (for IK Seed) - py_trees.decorators.Timeout( - name="GetCurrentState", - duration=1.0, - child=GetJointStates( - name="GetArticutoolJoints", - ns=name, - node=self._node, - inputs={ - "joint_names": [ - "j2n6s200_joint_1", - "j2n6s200_joint_2", - "j2n6s200_joint_3", - "j2n6s200_joint_4", - "j2n6s200_joint_5", - "j2n6s200_joint_6", - "atool_joint1", - "atool_joint2", - ] - }, - outputs={ - "joint_positions": BlackboardKey( - "current_joint_positions" - ), - "joint_names": BlackboardKey("current_joint_names"), - }, - ), - ), # Convert Pose to PoseStamped using the defined frame_id StampPoseFromPose( name="StampMoveAbovePose", @@ -625,6 +597,35 @@ def move_above_plan( "constraints": BlackboardKey("wrist_constraints"), } ), + # Get Current Joint State (for IK Seed) + py_trees.decorators.Timeout( + name="GetCurrentState", + duration=1.0, + child=GetJointStates( + name="GetArticutoolJoints", + ns=name, + node=self._node, + inputs={ + "joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + "atool_joint1", + "atool_joint2", + ] + }, + outputs={ + "joint_state": BlackboardKey("current_joint_state"), + "joint_positions": BlackboardKey( + "current_joint_positions" + ), + "joint_names": BlackboardKey("current_joint_names"), + }, + ), + ), # Compute IK for the target pose using the full jaco_arm_with_articutool planning group MoveIt2ComputeIK( name="ComputeJacoArmWithArticutoolIK", @@ -634,7 +635,7 @@ def move_above_plan( "move_above_pose_stamped_base_frame" ), "group_name": "jaco_arm_with_articutool", - # "start_joint_state": BlackboardKey("current_joint_positions"), + "start_joint_state": BlackboardKey("current_joint_state"), "constraints": BlackboardKey("wrist_constraints"), }, outputs={ From 1f7cd6af24a86d430fadd5903a9ce2e0258e9d61 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 10 May 2025 11:34:11 -0700 Subject: [PATCH 055/238] Revert planner_benchmark default MoveIt2 object to jaco_arm --- ada_feeding/scripts/planner_benchmark.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ada_feeding/scripts/planner_benchmark.py b/ada_feeding/scripts/planner_benchmark.py index 8cb269c3..47c54d67 100755 --- a/ada_feeding/scripts/planner_benchmark.py +++ b/ada_feeding/scripts/planner_benchmark.py @@ -330,10 +330,10 @@ def main(out_dir: Optional[str]): callback_group = ReentrantCallbackGroup() moveit2 = MoveIt2( node=node, - joint_names=kinova.joint_names() + ["atool_joint1", "atool_joint2"], + joint_names=kinova.joint_names(), base_link_name=kinova.base_link_name(), - end_effector_name="tool_tip", - group_name="jaco_arm_with_articutool", + end_effector_name="j2n6s200_end_effector", + group_name="jaco_arm", callback_group=callback_group, ) From 026146b0ffcfb2c1cf6a9c53045bd868fbfa70e9 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 10 May 2025 18:03:58 -0700 Subject: [PATCH 056/238] Add planning benchmark for orientation constrained motions between pre-defined configurations --- ada_feeding/CMakeLists.txt | 1 + ...ientation_constrained_planner_benchmark.py | 818 ++++++++++++++++++ 2 files changed, 819 insertions(+) create mode 100755 ada_feeding/scripts/orientation_constrained_planner_benchmark.py diff --git a/ada_feeding/CMakeLists.txt b/ada_feeding/CMakeLists.txt index 8c603ed3..4287b9ff 100644 --- a/ada_feeding/CMakeLists.txt +++ b/ada_feeding/CMakeLists.txt @@ -37,6 +37,7 @@ install(PROGRAMS scripts/dummy_ft_sensor.py scripts/joint_state_latency.py scripts/planner_benchmark.py + scripts/orientation_constrained_planner_benchmark.py scripts/robot_state_service.py scripts/save_image.py DESTINATION lib/${PROJECT_NAME} diff --git a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py new file mode 100755 index 00000000..08d80435 --- /dev/null +++ b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py @@ -0,0 +1,818 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script is used to benchmark planner performance for reaching +pre-defined joint configurations while enforcing a path-wide orientation +constraint on the end-effector. +""" + +# Standard imports +from collections import defaultdict, namedtuple +from datetime import datetime +import os +import sys +import time +from threading import Thread, Lock +from typing import Optional, List, Dict, Tuple, Any + +# import yaml # Not strictly needed if hardcoding configs, but good for future + +# Third-party imports +import numpy as np +from pymoveit2 import MoveIt2 +from pymoveit2.robots import kinova +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from rclpy.duration import Duration +from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory +from geometry_msgs.msg import Quaternion, PoseStamped +import moveit_msgs.msg +from sensor_msgs.msg import JointState +from scipy.spatial.transform import Rotation as R + +# --- Logging and Path Length Utility --- +_LOGGER_INSTANCE = None + + +def _get_logger(): + global _LOGGER_INSTANCE + if _LOGGER_INSTANCE is None: + if rclpy.ok() and rclpy.utilities.get_default_context().ok(): + _LOGGER_INSTANCE = rclpy.logging.get_logger("planner_benchmark_script") + else: + + class PrintLogger: + def info(self, msg): + print(f"INFO: {msg}") + + def warn(self, msg): + print(f"WARN: {msg}") + + def error(self, msg): + print(f"ERROR: {msg}") + + def debug(self, msg): + print(f"DEBUG: {msg}") + + _LOGGER_INSTANCE = PrintLogger() + return _LOGGER_INSTANCE + + +try: + from ada_feeding.behaviors.moveit2.moveit2_plan import ( + MoveIt2Plan as AdaMoveIt2PlanUtil, + ) + + GET_PATH_LEN_METHOD = AdaMoveIt2PlanUtil.get_path_len +except ImportError: + _get_logger().warn( + "Could not import AdaMoveIt2PlanUtil.get_path_len. Using fallback." + ) + + def fallback_get_path_len( + trajectory: JointTrajectory, exclude_j6_name: Optional[str] = "j2n6s200_joint_6" + ) -> Tuple[Optional[float], Optional[Dict[str, float]]]: + if not trajectory or not trajectory.points or len(trajectory.points) < 1: + return None, None + if len(trajectory.points) < 2: + return 0.0, {name: 0.0 for name in trajectory.joint_names} + + total_len_l2_no_j6 = 0.0 + joint_lens = {name: 0.0 for name in trajectory.joint_names} + + j6_idx = -1 + if exclude_j6_name and exclude_j6_name in trajectory.joint_names: + try: + j6_idx = trajectory.joint_names.index(exclude_j6_name) + except ValueError: + _get_logger().warn( + f"Joint {exclude_j6_name} for exclusion not found in trajectory joint_names." + ) + j6_idx = -1 + + prev_positions = np.array(trajectory.points[0].positions) + for point_idx in range(1, len(trajectory.points)): + point = trajectory.points[point_idx] + curr_positions = np.array(point.positions) + if len(curr_positions) != len(prev_positions): + _get_logger().warn( + f"Path length calc: Mismatch in joint count at point {point_idx}. Skipping." + ) + prev_positions = curr_positions + continue + + diff = np.abs(curr_positions - prev_positions) + diff = np.minimum(diff, 2 * np.pi - diff) + + current_segment_geom_dist_sq_no_j6 = 0.0 + for i in range(len(diff)): + joint_lens[trajectory.joint_names[i]] += diff[i] + if i != j6_idx: + current_segment_geom_dist_sq_no_j6 += diff[i] ** 2 + + total_len_l2_no_j6 += np.sqrt(current_segment_geom_dist_sq_no_j6) + prev_positions = curr_positions + return total_len_l2_no_j6, joint_lens + + GET_PATH_LEN_METHOD = fallback_get_path_len + +BenchmarkCondition = namedtuple("BenchmarkCondition", ["name", "joint_values"]) +PlanResult = namedtuple( + "PlanResult", + [ + "trajectory", + "elapsed_time", + "path_length", + "joint_path_lengths", + "max_roll_deviation", + "success", + ], +) + + +class PlannerBenchmark: + NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ = (0.5, 0.5, 0.5, 0.5) + NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS = (np.pi * 1.99, np.pi * 1.99, np.pi * 1.0) + NO_ROLL_CONSTRAINT_WEIGHT = 1.0 + NO_ROLL_PARAMETERIZATION = 0 + + def __init__( + self, + node: Node, + moveit2_interface: MoveIt2, + # target_configs_yaml_path: str, # Removing YAML path for now + hardcoded_target_configs: Dict[str, List[float]], # New argument + planners_to_test: List[str], + initial_joint_config: List[float], + planning_group: str, + end_effector_link: str, + base_link: str, + joint_names_for_group: List[str], + planning_timeout_sec: float = 10.0, + ): + self.node = node + self.logger = self.node.get_logger() + self.moveit2_interface = moveit2_interface + self.planners_to_test = planners_to_test + self.initial_joint_config = initial_joint_config + self.planning_group = planning_group + self.end_effector_link = end_effector_link + self.base_link = base_link + self.joint_names_for_group = joint_names_for_group + self.planning_timeout_sec = planning_timeout_sec + + self.target_configurations = self._process_hardcoded_configurations( + hardcoded_target_configs + ) + self.num_conditions = len(self.target_configurations) + + self.results: Dict[str, Dict[str, PlanResult]] = defaultdict(dict) + self.rate = self.node.create_rate(10) + + self.no_roll_path_constraint_kwargs = self._get_no_roll_constraint_kwargs() + + self.logger.info( + f"Benchmark initialized for group '{self.planning_group}' and EE '{self.end_effector_link}'." + ) + self.logger.info( + f"Testing {self.num_conditions} target configurations with planners: {self.planners_to_test}" + ) + self.logger.info( + f"Using 'no roll' constraint: Target Quat (xyzw)={self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ}, Tol(xyz_abs)={self.NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS}" + ) + + def _process_hardcoded_configurations( + self, hardcoded_configs: Dict[str, List[float]] + ) -> List[BenchmarkCondition]: + configs = [] + self.logger.info(f"Processing hardcoded target configurations...") + num_expected_joints = len(self.joint_names_for_group) + for name, values in hardcoded_configs.items(): + if isinstance(values, list) and len(values) == num_expected_joints: + configs.append(BenchmarkCondition(name, values)) + self.logger.info( + f" Loaded '{name}' with {num_expected_joints} joints." + ) + else: + self.logger.warn( + f" Skipping hardcoded config '{name}'. Expected {num_expected_joints} joint values, got {len(values) if isinstance(values, list) else type(values)}." + ) + + if not configs: + self.logger.error( + f"No valid target configurations provided from hardcoded list. Exiting." + ) + sys.exit(1) + return configs + + def _get_no_roll_constraint_kwargs(self) -> Dict[str, Any]: + # Returns kwargs for self.moveit2_interface.set_path_orientation_constraint + return { + "quat_xyzw": Quaternion( + x=self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ[0], + y=self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ[1], + z=self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ[2], + w=self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ[3], + ), + "frame_id": self.base_link, + "target_link": self.end_effector_link, + "tolerance": self.NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS, # Tuple (x,y,z) + "weight": self.NO_ROLL_CONSTRAINT_WEIGHT, + "parameterization": self.NO_ROLL_PARAMETERIZATION, # As used in ada_feeding OrientationConstraint + } + + def _calculate_max_roll_deviation( + self, trajectory: Optional[JointTrajectory] + ) -> Optional[float]: + if trajectory is None or not trajectory.points: + return None + + max_abs_roll_deviation = 0.0 + + for point_idx, point in enumerate(trajectory.points): + if len(point.positions) != len(self.joint_names_for_group): + self.logger.warn( + f"FK Calc: Mismatch in joint count at point {point_idx}. Expected {len(self.joint_names_for_group)}, got {len(point.positions)}. Skipping." + ) + continue + + joint_positions_list = list(point.positions) + + try: + result_poses: Optional[List[PoseStamped]] = ( + self.moveit2_interface.compute_fk( + joint_state=joint_positions_list, + fk_link_names=[self.end_effector_link], + ) + ) + + if ( + not result_poses + or not isinstance(result_poses, list) + or not result_poses[0] + ): + self.logger.warn( + f"FK returned None or invalid format for point {point_idx}." + ) + continue + + pose_stamped_ee = result_poses[0] + + if pose_stamped_ee.header.frame_id.lstrip("/") != self.base_link.lstrip( + "/" + ): + self.logger.warn( + f"FK pose for {self.end_effector_link} is in frame '{pose_stamped_ee.header.frame_id}', " + f"expected '{self.base_link}'. Ensure consistency or implement TF." + ) + + q = pose_stamped_ee.pose.orientation + orientation_scipy = R.from_quat([q.x, q.y, q.z, q.w]) + + # Target "no roll" orientation is identity w.r.t base_link. + # Tolerances NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS are (tol_ee_x, tol_ee_y, tol_ee_z_roll) + # applied to the EE frame's axes relative to this identity orientation. + # Euler angles of the EE frame w.r.t. base_link directly give deviations. + # If NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS[2] (index 2) is for roll around Z-axis of EE (when aligned): + euler_angles_xyz = orientation_scipy.as_euler("xyz", degrees=False) + current_z_axis_rotation_of_ee_wrt_base = euler_angles_xyz[2] + + deviation = current_z_axis_rotation_of_ee_wrt_base + while deviation > np.pi: + deviation -= 2 * np.pi + while deviation < -np.pi: + deviation += 2 * np.pi + + abs_deviation = abs(deviation) + max_abs_roll_deviation = max(max_abs_roll_deviation, abs_deviation) + + except Exception as e: + self.logger.error( + f"Error during FK for roll deviation at point {point_idx}: {e}", + ) + return float("inf") + return max_abs_roll_deviation + + def _get_path_length_stats( + self, trajectory: Optional[JointTrajectory] + ) -> Tuple[Optional[float], Optional[Dict[str, float]]]: + if trajectory is None or not trajectory.points: + return None, None + j6_name = ( + "j2n6s200_joint_6" + if "j2n6s200_joint_6" in self.joint_names_for_group + else None + ) + return GET_PATH_LEN_METHOD(trajectory) + + def plan_to_target_configuration( + self, + goal_joints: List[float], + planner_id_str: str, + path_constraints_kwargs_to_apply: Optional[Dict[str, Any]] = None, + ) -> Tuple[Optional[JointTrajectory], float]: + self.logger.info( + f" Attempting to plan with: {planner_id_str} to {goal_joints}" + ) + + # 1. Set planner_id and pipeline_id on self.moveit2_interface + if planner_id_str.lower() == "chomp": + self.moveit2_interface.planning_pipeline_id = "chomp" + self.moveit2_interface.planner_id = "chomp" + elif planner_id_str.lower() == "stomp": + self.moveit2_interface.planning_pipeline_id = "stomp" + self.moveit2_interface.planner_id = "" + else: # OMPL + self.moveit2_interface.planning_pipeline_id = "ompl" + self.moveit2_interface.planner_id = planner_id_str + + self.moveit2_interface.allowed_planning_time = self.planning_timeout_sec + + # 2. Clear previous constraints and set new ones + self.moveit2_interface.clear_goal_constraints() + self.moveit2_interface.clear_path_constraints() + + # --- Set the Joint Space Goal --- + try: + self.moveit2_interface.set_joint_goal( + joint_positions=goal_joints, + joint_names=self.joint_names_for_group, + tolerance=0.01, # Example tolerance, adjust as needed + weight=1.0, # Example weight, adjust as needed + ) + self.logger.info(f" Set joint goal for {planner_id_str}.") + except Exception as e: + self.logger.error(f" Failed to set joint goal: {e}") + return None, 0.0 + + # 3. Apply the path_constraints (if any) + if path_constraints_kwargs_to_apply: + try: + self.moveit2_interface.set_path_orientation_constraint( + **path_constraints_kwargs_to_apply + ) + self.logger.info( + f" Applied 'no roll' path constraint for {planner_id_str}." + ) + except Exception as e: + self.logger.error(f" Failed to set path orientation constraint: {e}") + # Optionally, you might decide to not proceed if constraint setting fails + # return None, 0.0 + + # 4. Call the general planning method + start_time_ros = self.node.get_clock().now() + + # plan_async() plans to the goals and with the path constraints previously set. + # start_joint_state=None means plan from the current state known to MoveIt2. + future = self.moveit2_interface.plan_async(start_joint_state=None) + + joint_trajectory_for_analysis: Optional[JointTrajectory] = None + + # 5. Handle asynchronous result and timeout + timeout_duration_rclpy = Duration( + seconds=self.planning_timeout_sec + 2.0 + ) # Add a small buffer + wait_start_time_rclpy = self.node.get_clock().now() + + while rclpy.ok() and not future.done(): + elapsed_wait = self.node.get_clock().now() - wait_start_time_rclpy + if elapsed_wait >= timeout_duration_rclpy: + self.logger.warn( + f" Planner {planner_id_str} timed out after {elapsed_wait.nanoseconds / 1e9:.2f}s while waiting for future.done()." + ) + if ( + hasattr(future, "cancel") + and callable(future.cancel) + and not future.cancelled() + ): + future.cancel() + break + self.rate.sleep() + + if future.done() and not future.cancelled(): + try: + plan_result_srv_response = future.result() + if ( + plan_result_srv_response.motion_plan_response.error_code.val + == moveit_msgs.msg.MoveItErrorCodes.SUCCESS + ): + # Extract the JointTrajectory from the RobotTrajectory + trajectory_msg = ( + plan_result_srv_response.motion_plan_response.trajectory + ) + if ( + trajectory_msg and trajectory_msg.joint_trajectory.points + ): # Check if it has points + joint_trajectory_for_analysis = trajectory_msg.joint_trajectory + self.logger.info(f" Planner {planner_id_str} succeeded.") + else: + self.logger.warn( + f" Planner {planner_id_str} succeeded but returned an empty joint_trajectory." + ) + else: + self.logger.warn( + f" Planner {planner_id_str} failed with MoveItErrorCode: {plan_result_srv_response.motion_plan_response.error_code.val}" + ) + except Exception as e: + self.logger.error( + f" Exception while getting plan result for {planner_id_str}: {e}", + ) + elif future.cancelled(): + self.logger.warn( + f" Planning for {planner_id_str} was cancelled (likely due to timeout)." + ) + + elapsed_time = (self.node.get_clock().now() - start_time_ros).nanoseconds / 1e9 + + # 6. Clear path constraints (and goal constraints, though new goals will override) + self.moveit2_interface.clear_path_constraints() + self.moveit2_interface.clear_goal_constraints() # Good practice + + return joint_trajectory_for_analysis, elapsed_time + + def move_to_initial_config(self): + self.logger.info( + f"Moving to initial configuration: {self.initial_joint_config}" + ) + + original_planner = self.moveit2_interface.planner_id + original_pipeline = self.moveit2_interface.pipeline_id + original_timeout = self.moveit2_interface.allowed_planning_time + + self.moveit2_interface.planning_pipeline_id = "ompl" + self.moveit2_interface.planner_id = "RRTConnectkConfigDefault" + self.moveit2_interface.allowed_planning_time = 10.0 + self.moveit2_interface.clear_goal_constraints() + self.moveit2_interface.clear_path_constraints() + + try: + self.logger.info( + f" Commanding move to configuration with {len(self.initial_joint_config)} joints for group '{self.planning_group}'" + ) # Use self.planning_group + self.moveit2_interface.move_to_configuration( + joint_positions=self.initial_joint_config, + joint_names=self.joint_names_for_group, + ) + self.moveit2_interface.wait_until_executed() + self.logger.info("Reached initial configuration.") + except Exception as e: + self.logger.error(f"Failed to move to initial configuration: {e}") + raise + finally: + self.moveit2_interface.planner_id = original_planner + self.moveit2_interface.pipeline_id = original_pipeline + self.moveit2_interface.allowed_planning_time = original_timeout + + def run_benchmark_condition( + self, condition: BenchmarkCondition + ): # Renamed from run_condition + self.logger.info(f"--- Testing Configuration: {condition.name} ---") + self.logger.info(f" Target Joints: {condition.joint_values}") + + for planner_id_str in self.planners_to_test: + trajectory, elapsed_time = self.plan_to_target_configuration( + goal_joints=condition.joint_values, + planner_id_str=planner_id_str, + path_constraints_kwargs_to_apply=self.no_roll_path_constraint_kwargs, + ) + + success = trajectory is not None and bool(trajectory.points) + path_len_total, joint_path_lens_map = self._get_path_length_stats( + trajectory + ) + max_roll_dev = self._calculate_max_roll_deviation(trajectory) + + if condition.name not in self.results: + self.results[condition.name] = {} + + self.results[condition.name][planner_id_str] = PlanResult( + trajectory, + elapsed_time, + path_len_total, + joint_path_lens_map, + max_roll_dev, + success, + ) + if success: + self.logger.info( + f" Planner: {planner_id_str} | Success: True | Time: {elapsed_time:.3f}s " + f"| PathLen: {path_len_total if path_len_total is not None else 'N/A':.3f} " + f"| MaxRollDev: {max_roll_dev if max_roll_dev is not None else 'N/A':.3f}" + ) + else: + self.logger.warn( + f" Planner: {planner_id_str} | Success: False | Time: {elapsed_time:.3f}s" + ) + + def run_all_tests(self, log_summary_every_n_conditions: Optional[int] = None): + self.move_to_initial_config() + + for i, condition in enumerate(self.target_configurations): + self.logger.info( + f"=== Running Condition {i + 1}/{self.num_conditions}: {condition.name} ===" + ) + self.run_benchmark_condition(condition) + + if ( + log_summary_every_n_conditions + and (i + 1) % log_summary_every_n_conditions == 0 + ): + self.log_summary_results() + + self.logger.info("=== Benchmark Run Completed ===") + + def get_csv_header(self) -> List[str]: + header = [f"start_{name}" for name in self.joint_names_for_group] + header.extend( + [ + "goal_config_name", + *[f"goal_{name}" for name in self.joint_names_for_group], + "planner_id", + "elapsed_time_s", + "success", + "path_length_total", + "max_roll_deviation_rad", + ] + ) + header.extend([f"path_length_{name}" for name in self.joint_names_for_group]) + return header + + def write_results_to_csv(self, filename: str): + self.logger.info(f"Writing results to {filename}") + start_config_for_csv = self.initial_joint_config + + with open(filename, "w", newline="") as f: + import csv + + csv_writer = csv.writer(f) + header = self.get_csv_header() + csv_writer.writerow(header) + + for target_condition in self.target_configurations: + config_name = target_condition.name + if config_name in self.results: + planner_runs = self.results[config_name] + for planner_id_str in self.planners_to_test: + if planner_id_str in planner_runs: + result = planner_runs[planner_id_str] + row = [] + row.extend(map(str, start_config_for_csv)) + row.append(str(target_condition.name)) + row.extend(map(str, target_condition.joint_values)) + row.append(str(planner_id_str)) + row.append(f"{result.elapsed_time:.4f}") + row.append(str(1 if result.success else 0)) + row.append( + f"{result.path_length:.4f}" + if result.path_length is not None + else "" + ) + row.append( + f"{result.max_roll_deviation:.4f}" + if result.max_roll_deviation is not None + else "" + ) + + if result.joint_path_lengths: + for joint_name in self.joint_names_for_group: + length_val = result.joint_path_lengths.get( + joint_name + ) + row.append( + f"{length_val:.4f}" + if length_val is not None + else "" + ) + else: + row.extend([""] * len(self.joint_names_for_group)) + csv_writer.writerow(row) + else: + self.logger.warn( + f"No result for planner '{planner_id_str}' in condition '{config_name}'. Skipping CSV row." + ) + else: + self.logger.warn( + f"No results found for configuration: {config_name} in self.results. Skipping CSV rows for this condition." + ) + + self.logger.info(f"Results successfully written to {filename}") + + def log_summary_results(self): + self.logger.info("--- BENCHMARK SUMMARY ---") + for planner_id_str in self.planners_to_test: + results_for_planner = [ + run_results[planner_id_str] + for run_results in self.results.values() + if planner_id_str in run_results + ] + + total_plans = len(results_for_planner) + if total_plans == 0: + self.logger.info(f"--- Planner: {planner_id_str} ---") + self.logger.info(" No plans attempted or recorded for this planner.") + continue + + successful_plans = sum(1 for r in results_for_planner if r.success) + total_planning_time = sum(r.elapsed_time for r in results_for_planner) + + path_lengths_list = [ + r.path_length + for r in results_for_planner + if r.success and r.path_length is not None + ] + roll_deviations_list = [ + r.max_roll_deviation + for r in results_for_planner + if r.success and r.max_roll_deviation is not None + ] + + self.logger.info(f"--- Planner: {planner_id_str} ---") + success_rate = ( + (successful_plans / total_plans) * 100 if total_plans > 0 else 0 + ) + avg_planning_time_all = ( + total_planning_time / total_plans if total_plans > 0 else 0 + ) + + successful_planning_times = [ + r.elapsed_time for r in results_for_planner if r.success + ] + avg_planning_time_succ = ( + sum(successful_planning_times) / len(successful_planning_times) + if successful_planning_times + else 0 + ) + + self.logger.info( + f" Success Rate: {success_rate:.2f}% ({successful_plans}/{total_plans})" + ) + self.logger.info( + f" Avg. Planning Time (all attempts): {avg_planning_time_all:.3f}s" + ) + if successful_planning_times: + self.logger.info( + f" Avg. Planning Time (successful attempts): {avg_planning_time_succ:.3f}s" + ) + + if path_lengths_list: + self.logger.info( + f" Path Lengths (successful plans): {PlannerBenchmark._summary_stats_str(path_lengths_list)}" + ) + else: + self.logger.info(" Path Lengths (successful plans): N/A") + + valid_roll_deviations = [ + r + for r in roll_deviations_list + if r is not None and not (isinstance(r, float) and np.isinf(r)) + ] + if valid_roll_deviations: + self.logger.info( + f" Max Roll Deviations (successful plans): {PlannerBenchmark._summary_stats_str(valid_roll_deviations)}" + ) + else: + self.logger.info( + " Max Roll Deviations (successful plans): N/A (or all FK failed/no valid deviations)" + ) + self.logger.info("-------------------------") + + @staticmethod + def _summary_stats_str(data: List[float]) -> str: + if not data: + return "N/A" + valid_data = [x for x in data if isinstance(x, (int, float)) and np.isfinite(x)] + if not valid_data: + return "N/A (no valid data points)" + return ( + f"Mean {np.mean(valid_data):.3f}, Median {np.median(valid_data):.3f}, " + f"Min {np.min(valid_data):.3f}, Max {np.max(valid_data):.3f}, Std {np.std(valid_data):.3f}, " + f"Count {len(valid_data)}" + ) + + +def main(output_dir_arg: Optional[str]): + rclpy.init() + node = Node("planner_benchmark_joint_config_constrained") + + # Set global logger instance once node is created + global _LOGGER_INSTANCE + _LOGGER_INSTANCE = node.get_logger() + + executor = rclpy.executors.MultiThreadedExecutor(2) + executor.add_node(node) + executor_thread = Thread(target=executor.spin, daemon=True, args=()) + executor_thread.start() + + _LOGGER_INSTANCE.info( + "Benchmark node spinning. Waiting for services (approx 5s)..." + ) + time.sleep(5.0) + + # --- Hardcoded Target Configurations --- + # (Values taken from your YAML structure for j2n6s200 - 6 joints) + hardcoded_targets = { + "MoveAbovePlate": [ + -2.4538579336877304, + 3.07974419938212, + 1.8320725365979, + 4.096143890468605, + -2.003422584820525, + -3.2123560395465063, + ], + "RestingAcquireFood": [-1.94672, 2.51268, 0.35653, -4.76501, 5.99991, 4.99555], + "MoveToRestingPosition": [ + -1.94672, + 2.51268, + 0.35653, + -4.76501, + 5.99991, + 4.99555, + ], # Same as AcquireFood resting + "MoveToStagingConfiguration": [ + -2.32526, + 4.456298, + 4.16769, + 1.53262, + -2.18359, + -2.19525, + ], # "Shorter" version from your YAML + "MoveToStowLocation": [-1.52101, 2.60098, 0.32811, -4.00012, 0.22831, 3.87886], + } + # --- End Hardcoded --- + + planners = ["RRTConnectkConfigDefault", "RRTstarkConfigDefault", "chomp"] + initial_config = [ + -2.4538579336877304, + 3.07974419938212, + 1.8320725365979, + 4.096143890468605, + -2.003422584820525, + -3.2123560395465063, + ] + planning_group_name = "jaco_arm" + ee_link_name = "j2n6s200_end_effector" + base_link_name = kinova.base_link_name() + group_joint_names = kinova.joint_names() + planning_timeout = 15.0 + + callback_group = ReentrantCallbackGroup() + moveit2_interface = MoveIt2( + node=node, + joint_names=group_joint_names, + base_link_name=base_link_name, + end_effector_name=ee_link_name, + group_name=planning_group_name, + callback_group=callback_group, + ) + # Set defaults on the interface object + moveit2_interface.allowed_planning_time = planning_timeout + moveit2_interface.max_velocity_scaling_factor = 0.5 + moveit2_interface.max_acceleration_scaling_factor = 0.5 + _LOGGER_INSTANCE.info( + f"MoveIt2 interface initialized for benchmark with group: '{planning_group_name}'" + ) + + benchmark_runner = PlannerBenchmark( + node=node, + moveit2_interface=moveit2_interface, + # target_configs_yaml_path=target_configs_yaml, # Using hardcoded now + hardcoded_target_configs=hardcoded_targets, + planners_to_test=planners, + initial_joint_config=initial_config, + planning_group=planning_group_name, + end_effector_link=ee_link_name, + base_link=base_link_name, + joint_names_for_group=group_joint_names, + planning_timeout_sec=planning_timeout, + ) + + try: + benchmark_runner.run_all_tests(log_summary_every_n_conditions=1) + + output_dir_to_use = output_dir_arg if output_dir_arg else "." + if not os.path.exists(output_dir_to_use): + os.makedirs(output_dir_to_use) + _LOGGER_INSTANCE.info(f"Created output directory: {output_dir_to_use}") + + csv_filename = os.path.join( + output_dir_to_use, + datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + + "_joint_config_constrained_benchmark.csv", + ) + benchmark_runner.write_results_to_csv(csv_filename) + benchmark_runner.log_summary_results() + + except Exception as e: + _LOGGER_INSTANCE.error(f"Benchmark run failed: {e}") + finally: + _LOGGER_INSTANCE.info("Shutting down benchmark node.") + rclpy.shutdown() + executor_thread.join() + + +if __name__ == "__main__": + out_dir_arg = os.path.expanduser(sys.argv[1]) if len(sys.argv) > 1 else None + main(out_dir_arg) From 5f0fc24725d7a4b5b9cdab2332fa51a070a79ab4 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 11 May 2025 10:31:04 -0700 Subject: [PATCH 057/238] Modify orientation constrained planner benchmark to also save trajectories --- ...ientation_constrained_planner_benchmark.py | 233 ++++++++++++------ 1 file changed, 154 insertions(+), 79 deletions(-) diff --git a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py index 08d80435..9ce500d1 100755 --- a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py +++ b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py @@ -5,7 +5,7 @@ """ This script is used to benchmark planner performance for reaching pre-defined joint configurations while enforcing a path-wide orientation -constraint on the end-effector. +constraint on the end-effector. It now also saves successful trajectories. """ # Standard imports @@ -16,8 +16,7 @@ import time from threading import Thread, Lock from typing import Optional, List, Dict, Tuple, Any - -# import yaml # Not strictly needed if hardcoding configs, but good for future +import json # Third-party imports import numpy as np @@ -32,6 +31,7 @@ import moveit_msgs.msg from sensor_msgs.msg import JointState from scipy.spatial.transform import Rotation as R +from rosidl_runtime_py import message_to_ordereddict # --- Logging and Path Length Utility --- _LOGGER_INSTANCE = None @@ -129,13 +129,23 @@ def fallback_get_path_len( "joint_path_lengths", "max_roll_deviation", "success", + "trajectory_filename", ], ) class PlannerBenchmark: - NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ = (0.5, 0.5, 0.5, 0.5) - NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS = (np.pi * 1.99, np.pi * 1.99, np.pi * 1.0) + NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ = ( + 0.5, + 0.5, + 0.5, + 0.5, + ) + NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS = ( + np.pi * 1.99, + np.pi * 1.99, + np.deg2rad(90.0), + ) NO_ROLL_CONSTRAINT_WEIGHT = 1.0 NO_ROLL_PARAMETERIZATION = 0 @@ -143,8 +153,7 @@ def __init__( self, node: Node, moveit2_interface: MoveIt2, - # target_configs_yaml_path: str, # Removing YAML path for now - hardcoded_target_configs: Dict[str, List[float]], # New argument + hardcoded_target_configs: Dict[str, List[float]], planners_to_test: List[str], initial_joint_config: List[float], planning_group: str, @@ -152,6 +161,7 @@ def __init__( base_link: str, joint_names_for_group: List[str], planning_timeout_sec: float = 10.0, + trajectory_save_dir: Optional[str] = None, ): self.node = node self.logger = self.node.get_logger() @@ -174,6 +184,13 @@ def __init__( self.no_roll_path_constraint_kwargs = self._get_no_roll_constraint_kwargs() + self.trajectory_save_dir = trajectory_save_dir + if self.trajectory_save_dir: + os.makedirs(self.trajectory_save_dir, exist_ok=True) + self.logger.info( + f"Successful trajectories will be saved in: {self.trajectory_save_dir}" + ) + self.logger.info( f"Benchmark initialized for group '{self.planning_group}' and EE '{self.end_effector_link}'." ) @@ -209,7 +226,6 @@ def _process_hardcoded_configurations( return configs def _get_no_roll_constraint_kwargs(self) -> Dict[str, Any]: - # Returns kwargs for self.moveit2_interface.set_path_orientation_constraint return { "quat_xyzw": Quaternion( x=self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ[0], @@ -219,9 +235,9 @@ def _get_no_roll_constraint_kwargs(self) -> Dict[str, Any]: ), "frame_id": self.base_link, "target_link": self.end_effector_link, - "tolerance": self.NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS, # Tuple (x,y,z) + "tolerance": self.NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS, "weight": self.NO_ROLL_CONSTRAINT_WEIGHT, - "parameterization": self.NO_ROLL_PARAMETERIZATION, # As used in ada_feeding OrientationConstraint + "parameterization": self.NO_ROLL_PARAMETERIZATION, } def _calculate_max_roll_deviation( @@ -270,24 +286,50 @@ def _calculate_max_roll_deviation( ) q = pose_stamped_ee.pose.orientation - orientation_scipy = R.from_quat([q.x, q.y, q.z, q.w]) - # Target "no roll" orientation is identity w.r.t base_link. - # Tolerances NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS are (tol_ee_x, tol_ee_y, tol_ee_z_roll) - # applied to the EE frame's axes relative to this identity orientation. - # Euler angles of the EE frame w.r.t. base_link directly give deviations. - # If NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS[2] (index 2) is for roll around Z-axis of EE (when aligned): - euler_angles_xyz = orientation_scipy.as_euler("xyz", degrees=False) - current_z_axis_rotation_of_ee_wrt_base = euler_angles_xyz[2] + # Convert the actual EE orientation to SciPy Rotation + actual_ee_orientation_scipy = R.from_quat([q.x, q.y, q.z, q.w]) - deviation = current_z_axis_rotation_of_ee_wrt_base - while deviation > np.pi: - deviation -= 2 * np.pi - while deviation < -np.pi: - deviation += 2 * np.pi + # Convert the target "no roll" orientation (defined by NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ) + # to SciPy Rotation. This quaternion is already w.r.t. base_link. + target_quat_params = self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ + target_ee_orientation_scipy = R.from_quat(target_quat_params) - abs_deviation = abs(deviation) - max_abs_roll_deviation = max(max_abs_roll_deviation, abs_deviation) + # Calculate the rotational difference: R_diff = R_target_inv * R_actual + # This R_diff represents the rotation needed to get from target to actual. + # Its Euler angles (especially around the Z-axis of the *target* frame) + # can indicate roll deviation. + diff_rotation = ( + target_ee_orientation_scipy.inv() * actual_ee_orientation_scipy + ) + + # Get Euler angles of this difference. The Z-angle of this difference, + # if using 'xyz' for example, would represent the roll *after* aligning X and Y. + # Choose an Euler sequence that makes sense for your definition of "roll". + # 'zyx' is common, where the first angle (z) is yaw, second (y) is pitch, third (x) is roll. + # If your constraint is primarily about the EE's Z-axis pointing, then 'xyz' might be more intuitive + # where the last angle (z) is the rotation around the new Z axis. + # For this 'no roll' constraint, we are interested in rotation around the target EE's Z-axis. + # Let's use the target frame's Z-axis as the roll axis. + # Euler angles of R_diff in target frame's basis: + euler_angles_of_diff_in_target_basis = diff_rotation.as_euler( + "xyz", degrees=False + ) # or 'ZYX', etc. + + # The roll deviation would be the rotation around the axis that corresponds to "roll" + # in your chosen Euler sequence. For 'xyz', euler_angles_of_diff_in_target_basis[2] is about the new Z. + # This should correspond to the Z-axis of the `NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ` + current_roll_deviation = euler_angles_of_diff_in_target_basis[2] + + # Normalize to [-pi, pi] + while current_roll_deviation > np.pi: + current_roll_deviation -= 2 * np.pi + while current_roll_deviation < -np.pi: + current_roll_deviation += 2 * np.pi + + max_abs_roll_deviation = max( + max_abs_roll_deviation, abs(current_roll_deviation) + ) except Exception as e: self.logger.error( @@ -308,6 +350,32 @@ def _get_path_length_stats( ) return GET_PATH_LEN_METHOD(trajectory) + def _save_trajectory_to_file( + self, trajectory: JointTrajectory, condition_name: str, planner_id_str: str + ) -> Optional[str]: + if not self.trajectory_save_dir: + return None + + try: + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S-%f") + # Sanitize condition_name and planner_id_str for filename + safe_condition_name = condition_name.replace(" ", "_").replace("/", "_") + safe_planner_id_str = planner_id_str.replace(" ", "_").replace("/", "_") + + filename = f"{safe_condition_name}_{safe_planner_id_str}_{timestamp}.json" + filepath = os.path.join(self.trajectory_save_dir, filename) + + traj_dict = message_to_ordereddict(trajectory) + with open(filepath, "w") as f: + json.dump(traj_dict, f, indent=2) + self.logger.info(f" Successfully saved trajectory to: {filepath}") + return filename + except Exception as e: + self.logger.error( + f" Failed to save trajectory for {condition_name} ({planner_id_str}): {e}", + ) + return None + def plan_to_target_configuration( self, goal_joints: List[float], @@ -318,7 +386,6 @@ def plan_to_target_configuration( f" Attempting to plan with: {planner_id_str} to {goal_joints}" ) - # 1. Set planner_id and pipeline_id on self.moveit2_interface if planner_id_str.lower() == "chomp": self.moveit2_interface.planning_pipeline_id = "chomp" self.moveit2_interface.planner_id = "chomp" @@ -331,24 +398,21 @@ def plan_to_target_configuration( self.moveit2_interface.allowed_planning_time = self.planning_timeout_sec - # 2. Clear previous constraints and set new ones self.moveit2_interface.clear_goal_constraints() self.moveit2_interface.clear_path_constraints() - # --- Set the Joint Space Goal --- try: self.moveit2_interface.set_joint_goal( joint_positions=goal_joints, joint_names=self.joint_names_for_group, - tolerance=0.01, # Example tolerance, adjust as needed - weight=1.0, # Example weight, adjust as needed + tolerance=0.01, + weight=1.0, ) self.logger.info(f" Set joint goal for {planner_id_str}.") except Exception as e: self.logger.error(f" Failed to set joint goal: {e}") return None, 0.0 - # 3. Apply the path_constraints (if any) if path_constraints_kwargs_to_apply: try: self.moveit2_interface.set_path_orientation_constraint( @@ -359,22 +423,11 @@ def plan_to_target_configuration( ) except Exception as e: self.logger.error(f" Failed to set path orientation constraint: {e}") - # Optionally, you might decide to not proceed if constraint setting fails - # return None, 0.0 - # 4. Call the general planning method start_time_ros = self.node.get_clock().now() - - # plan_async() plans to the goals and with the path constraints previously set. - # start_joint_state=None means plan from the current state known to MoveIt2. future = self.moveit2_interface.plan_async(start_joint_state=None) - joint_trajectory_for_analysis: Optional[JointTrajectory] = None - - # 5. Handle asynchronous result and timeout - timeout_duration_rclpy = Duration( - seconds=self.planning_timeout_sec + 2.0 - ) # Add a small buffer + timeout_duration_rclpy = Duration(seconds=self.planning_timeout_sec + 2.0) wait_start_time_rclpy = self.node.get_clock().now() while rclpy.ok() and not future.done(): @@ -399,13 +452,10 @@ def plan_to_target_configuration( plan_result_srv_response.motion_plan_response.error_code.val == moveit_msgs.msg.MoveItErrorCodes.SUCCESS ): - # Extract the JointTrajectory from the RobotTrajectory trajectory_msg = ( plan_result_srv_response.motion_plan_response.trajectory ) - if ( - trajectory_msg and trajectory_msg.joint_trajectory.points - ): # Check if it has points + if trajectory_msg and trajectory_msg.joint_trajectory.points: joint_trajectory_for_analysis = trajectory_msg.joint_trajectory self.logger.info(f" Planner {planner_id_str} succeeded.") else: @@ -426,11 +476,8 @@ def plan_to_target_configuration( ) elapsed_time = (self.node.get_clock().now() - start_time_ros).nanoseconds / 1e9 - - # 6. Clear path constraints (and goal constraints, though new goals will override) self.moveit2_interface.clear_path_constraints() - self.moveit2_interface.clear_goal_constraints() # Good practice - + self.moveit2_interface.clear_goal_constraints() return joint_trajectory_for_analysis, elapsed_time def move_to_initial_config(self): @@ -451,7 +498,7 @@ def move_to_initial_config(self): try: self.logger.info( f" Commanding move to configuration with {len(self.initial_joint_config)} joints for group '{self.planning_group}'" - ) # Use self.planning_group + ) self.moveit2_interface.move_to_configuration( joint_positions=self.initial_joint_config, joint_names=self.joint_names_for_group, @@ -463,48 +510,57 @@ def move_to_initial_config(self): raise finally: self.moveit2_interface.planner_id = original_planner - self.moveit2_interface.pipeline_id = original_pipeline + self.moveit2_interface.planning_pipeline_id = original_pipeline self.moveit2_interface.allowed_planning_time = original_timeout - def run_benchmark_condition( - self, condition: BenchmarkCondition - ): # Renamed from run_condition + def run_benchmark_condition(self, condition: BenchmarkCondition): self.logger.info(f"--- Testing Configuration: {condition.name} ---") self.logger.info(f" Target Joints: {condition.joint_values}") for planner_id_str in self.planners_to_test: - trajectory, elapsed_time = self.plan_to_target_configuration( + joint_trajectory, elapsed_time = self.plan_to_target_configuration( goal_joints=condition.joint_values, planner_id_str=planner_id_str, path_constraints_kwargs_to_apply=self.no_roll_path_constraint_kwargs, ) - success = trajectory is not None and bool(trajectory.points) + success = joint_trajectory is not None and bool(joint_trajectory.points) path_len_total, joint_path_lens_map = self._get_path_length_stats( - trajectory + joint_trajectory ) - max_roll_dev = self._calculate_max_roll_deviation(trajectory) + max_roll_dev = self._calculate_max_roll_deviation(joint_trajectory) + + trajectory_filename = None + if success and self.trajectory_save_dir: + trajectory_filename = self._save_trajectory_to_file( + joint_trajectory, condition.name, planner_id_str + ) if condition.name not in self.results: self.results[condition.name] = {} self.results[condition.name][planner_id_str] = PlanResult( - trajectory, + joint_trajectory, elapsed_time, path_len_total, joint_path_lens_map, max_roll_dev, success, + trajectory_filename, + ) + + log_msg_suffix = ( + f"| TrajFile: {trajectory_filename}" if trajectory_filename else "" ) if success: self.logger.info( f" Planner: {planner_id_str} | Success: True | Time: {elapsed_time:.3f}s " f"| PathLen: {path_len_total if path_len_total is not None else 'N/A':.3f} " - f"| MaxRollDev: {max_roll_dev if max_roll_dev is not None else 'N/A':.3f}" + f"| MaxRollDev: {max_roll_dev if max_roll_dev is not None else 'N/A':.3f} rad {log_msg_suffix}" ) else: self.logger.warn( - f" Planner: {planner_id_str} | Success: False | Time: {elapsed_time:.3f}s" + f" Planner: {planner_id_str} | Success: False | Time: {elapsed_time:.3f}s {log_msg_suffix}" ) def run_all_tests(self, log_summary_every_n_conditions: Optional[int] = None): @@ -535,6 +591,7 @@ def get_csv_header(self) -> List[str]: "success", "path_length_total", "max_roll_deviation_rad", + "trajectory_filename", ] ) header.extend([f"path_length_{name}" for name in self.joint_names_for_group]) @@ -575,6 +632,11 @@ def write_results_to_csv(self, filename: str): if result.max_roll_deviation is not None else "" ) + row.append( + result.trajectory_filename + if result.trajectory_filename + else "" + ) if result.joint_path_lengths: for joint_name in self.joint_names_for_group: @@ -671,11 +733,11 @@ def log_summary_results(self): ] if valid_roll_deviations: self.logger.info( - f" Max Roll Deviations (successful plans): {PlannerBenchmark._summary_stats_str(valid_roll_deviations)}" + f" Max Roll Deviations (rad, successful plans): {PlannerBenchmark._summary_stats_str(valid_roll_deviations)}" ) else: self.logger.info( - " Max Roll Deviations (successful plans): N/A (or all FK failed/no valid deviations)" + " Max Roll Deviations (rad, successful plans): N/A (or all FK failed/no valid deviations)" ) self.logger.info("-------------------------") @@ -697,7 +759,6 @@ def main(output_dir_arg: Optional[str]): rclpy.init() node = Node("planner_benchmark_joint_config_constrained") - # Set global logger instance once node is created global _LOGGER_INSTANCE _LOGGER_INSTANCE = node.get_logger() @@ -711,8 +772,6 @@ def main(output_dir_arg: Optional[str]): ) time.sleep(5.0) - # --- Hardcoded Target Configurations --- - # (Values taken from your YAML structure for j2n6s200 - 6 joints) hardcoded_targets = { "MoveAbovePlate": [ -2.4538579336877304, @@ -730,7 +789,7 @@ def main(output_dir_arg: Optional[str]): -4.76501, 5.99991, 4.99555, - ], # Same as AcquireFood resting + ], "MoveToStagingConfiguration": [ -2.32526, 4.456298, @@ -738,12 +797,16 @@ def main(output_dir_arg: Optional[str]): 1.53262, -2.18359, -2.19525, - ], # "Shorter" version from your YAML + ], "MoveToStowLocation": [-1.52101, 2.60098, 0.32811, -4.00012, 0.22831, 3.87886], } - # --- End Hardcoded --- + _LOGGER_INSTANCE.info("Using hardcoded target configurations.") - planners = ["RRTConnectkConfigDefault", "RRTstarkConfigDefault", "chomp"] + planners = [ + "RRTConnectkConfigDefault", + "RRTstarkConfigDefault", + "CHOMP", + ] initial_config = [ -2.4538579336877304, 3.07974419938212, @@ -758,6 +821,15 @@ def main(output_dir_arg: Optional[str]): group_joint_names = kinova.joint_names() planning_timeout = 15.0 + # --- Trajectory Save Directory --- + base_output_dir = output_dir_arg if output_dir_arg else "." + # Create the base output directory here if it's specified and doesn't exist + if output_dir_arg and not os.path.exists(base_output_dir): + os.makedirs(base_output_dir) + _LOGGER_INSTANCE.info(f"Created base output directory: {base_output_dir}") + + trajectory_save_location = os.path.join(base_output_dir, "saved_trajectories") + callback_group = ReentrantCallbackGroup() moveit2_interface = MoveIt2( node=node, @@ -767,7 +839,6 @@ def main(output_dir_arg: Optional[str]): group_name=planning_group_name, callback_group=callback_group, ) - # Set defaults on the interface object moveit2_interface.allowed_planning_time = planning_timeout moveit2_interface.max_velocity_scaling_factor = 0.5 moveit2_interface.max_acceleration_scaling_factor = 0.5 @@ -778,7 +849,6 @@ def main(output_dir_arg: Optional[str]): benchmark_runner = PlannerBenchmark( node=node, moveit2_interface=moveit2_interface, - # target_configs_yaml_path=target_configs_yaml, # Using hardcoded now hardcoded_target_configs=hardcoded_targets, planners_to_test=planners, initial_joint_config=initial_config, @@ -787,18 +857,23 @@ def main(output_dir_arg: Optional[str]): base_link=base_link_name, joint_names_for_group=group_joint_names, planning_timeout_sec=planning_timeout, + trajectory_save_dir=trajectory_save_location, ) try: benchmark_runner.run_all_tests(log_summary_every_n_conditions=1) - output_dir_to_use = output_dir_arg if output_dir_arg else "." - if not os.path.exists(output_dir_to_use): - os.makedirs(output_dir_to_use) - _LOGGER_INSTANCE.info(f"Created output directory: {output_dir_to_use}") + # Ensure base_output_dir exists before writing CSV + if not os.path.exists(base_output_dir) and base_output_dir != ".": + os.makedirs(base_output_dir) + _LOGGER_INSTANCE.info( + f"Created output directory for CSV: {base_output_dir}" + ) + elif base_output_dir == "." and not os.path.exists(base_output_dir): + os.makedirs(base_output_dir) csv_filename = os.path.join( - output_dir_to_use, + base_output_dir, datetime.now().strftime("%Y-%m-%d_%H-%M-%S") + "_joint_config_constrained_benchmark.csv", ) From 5d04ce3a055b6219043cd257d021fe0534e74fb7 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 11 May 2025 10:31:38 -0700 Subject: [PATCH 058/238] WIP Add script to visualize saved trajectories with Pinocchio and Meshcat --- ada_feeding/CMakeLists.txt | 1 + ada_feeding/scripts/visualize_trajectory.py | 333 ++++++++++++++++++++ 2 files changed, 334 insertions(+) create mode 100755 ada_feeding/scripts/visualize_trajectory.py diff --git a/ada_feeding/CMakeLists.txt b/ada_feeding/CMakeLists.txt index 4287b9ff..54233f23 100644 --- a/ada_feeding/CMakeLists.txt +++ b/ada_feeding/CMakeLists.txt @@ -38,6 +38,7 @@ install(PROGRAMS scripts/joint_state_latency.py scripts/planner_benchmark.py scripts/orientation_constrained_planner_benchmark.py + scripts/visualize_trajectory.py scripts/robot_state_service.py scripts/save_image.py DESTINATION lib/${PROJECT_NAME} diff --git a/ada_feeding/scripts/visualize_trajectory.py b/ada_feeding/scripts/visualize_trajectory.py new file mode 100755 index 00000000..5c00266f --- /dev/null +++ b/ada_feeding/scripts/visualize_trajectory.py @@ -0,0 +1,333 @@ +#!/usr/bin/env python3 + +import pinocchio as pin +import pinocchio.visualize # Import the submodule +import meshcat +import numpy as np +import json +import subprocess +import tempfile +import os +import argparse +import time +import sys + +from typing import Optional + + +def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str]: + """Converts a Xacro file to a URDF XML string using ros2 run xacro.""" + if not os.path.exists(xacro_filename): + logger_func(f"Error: Xacro file not found at {xacro_filename}") + return None + + logger_func(f"Processing Xacro file: {xacro_filename}") + try: + # Try to find a ROS 2 distribution to use in error messages if needed + ros_distro = os.environ.get("ROS_DISTRO", "your_ros2_distro") + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", xacro_filename], + check=True, + capture_output=True, + text=True, + ) + urdf_xml_string = process.stdout + logger_func("XACRO processing successful.") + return urdf_xml_string + except FileNotFoundError as e: + logger_func(f"Fatal: Command 'ros2 run xacro ...' failed. Is xacro installed ") + return None + except subprocess.CalledProcessError as e: + logger_func( + f"Fatal: XACRO processing command failed with exit code {e.returncode}." + ) + logger_func(f"XACRO stdout:\n{e.stdout}") + logger_func(f"XACRO stderr:\n{e.stderr}") + return None + except Exception as e: + logger_func(f"An unexpected error occurred during XACRO processing: {e}") + return None + + +def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=print): + """Loads a Pinocchio model from a URDF XML string via a temporary file.""" + temp_urdf_file = None + try: + with tempfile.NamedTemporaryFile( + mode="w", suffix=".urdf", delete=False + ) as temp_file: + temp_urdf_path = temp_file.name + temp_file.write(urdf_xml_string) + + logger_func(f"Generated temporary URDF file: {temp_urdf_path}") + + # For Pinocchio to find meshes, especially if using package://, + # it might need directories to search. + # Example: model = pin.buildModelFromUrdf(temp_urdf_path, package_dirs=['/path/to/your_ros_ws/src/your_robot_description_pkg']) + # If your meshes are relative to the URDF, and the URDF is generated from Xacro, + # ensure the Xacro resolves paths correctly or provide absolute paths in Xacro, + # or explore pin.loadUrdfModel with package_dirs. + # For now, we assume meshes are findable or simple geometries are used. + + model = pin.buildModelFromUrdf(temp_urdf_path) + # Pinocchio automatically creates collision and visual models from the URDF + collision_model = pin.buildGeomFromUrdf( + model, temp_urdf_path, pin.GeometryType.COLLISION + ) + visual_model = pin.buildGeomFromUrdf( + model, temp_urdf_path, pin.GeometryType.VISUAL + ) + + data = model.createData() + collision_data = collision_model.createData() # if you need collision checks + visual_data = ( + visual_model.createData() + ) # if you need to update visual geoms (less common for just display) + + logger_func( + f"Pinocchio model loaded successfully. Name: {model.name}, Nq: {model.nq}, Nv: {model.nv}" + ) + logger_func(f"Number of joints in model: {model.njoints}") # Includes universe + # for jname in model.names: print(f" Joint in model: {jname}") + + return model, collision_model, visual_model, data + except Exception as e: + logger_func(f"Error loading Pinocchio model: {e}") + return None, None, None, None + finally: + if temp_urdf_file and os.path.exists(temp_urdf_path): + os.remove(temp_urdf_path) + logger_func(f"Cleaned up temporary URDF file: {temp_urdf_path}") + + +def load_trajectory_from_json(filepath: str, logger_func=print): + """Loads a JointTrajectory from a JSON file.""" + try: + with open(filepath, "r") as f: + traj_data = json.load(f) + logger_func(f"Trajectory loaded from {filepath}") + return traj_data + except FileNotFoundError: + logger_func(f"Error: Trajectory file not found at {filepath}") + except json.JSONDecodeError: + logger_func(f"Error: Could not decode JSON from {filepath}") + except Exception as e: + logger_func(f"An unexpected error occurred loading trajectory: {e}") + return None + + +def main(xacro_file: str, trajectory_file: str): + print("--- Pinocchio + MeshCat Trajectory Visualizer ---") + + # 1. Convert Xacro to URDF String + urdf_string = xacro_to_urdf_string(xacro_file) + if not urdf_string: + print("Exiting due to Xacro processing failure.") + return + + # 2. Load Pinocchio Model + load_result = load_pinocchio_model_from_urdf_string(urdf_string) + if not load_result or not load_result[0]: # Check if model is loaded + print("Exiting due to Pinocchio model loading failure.") + return + model, collision_model, visual_model, data = load_result + + # 3. Initialize MeshCat Visualizer + print("Initializing MeshCat viewer... Waiting for connection.") + try: + # You can specify a zmq_url if you have a specific MeshCat server running + # visualizer = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000") + visualizer = ( + meshcat.Visualizer() + ) # Starts its own server or connects to existing + print(f"MeshCat viewer URL: {visualizer.url()}") + + # Create Pinocchio's MeshCat visualizer interface + # Note: pin.visualize.MeshcatVisualizer is the correct class + pin_viz = pin.visualize.MeshcatVisualizer(model, collision_model, visual_model) + pin_viz.initViewer( + viewer=visualizer, open=True + ) # Pass the meshcat.Visualizer instance + pin_viz.loadViewerModel( + rootNodeName="pinocchio_robot" + ) # Give it a name in the scene tree + print("MeshCat viewer initialized and model loaded.") + + except Exception as e: + print(f"Error initializing MeshCat or Pinocchio visualizer: {e}") + print("Please ensure MeshCat is installed and accessible.") + return + + # 4. Load Trajectory + trajectory_data = load_trajectory_from_json(trajectory_file) + if ( + not trajectory_data + or "joint_names" not in trajectory_data + or "points" not in trajectory_data + ): + print("Failed to load valid trajectory data. Exiting.") + return + + traj_joint_names = trajectory_data["joint_names"] + traj_points = trajectory_data["points"] + + if not traj_points: + print("Trajectory contains no points. Exiting.") + return + + # 5. Map Trajectory Joint Names to Pinocchio Model's q indices + # Pinocchio's q includes all DoFs, including the root joint if it's a free flyer. + # For a fixed-base manipulator, model.nq usually matches the number of actuated joints. + # For a floating base, the first 7 elements of q are [tx,ty,tz, qx,qy,qz,qw] for the base. + + # Get the neutral configuration (often zeros for fixed base, or identity for floating base part) + q = pin.neutral(model) + print(f"Neutral configuration q (size {model.nq}): {q.T}") + + # Create a list of indices in q that correspond to the trajectory joint names + # and their order. + q_indices_for_traj_joints = [] + missing_joints_in_model = [] + + for name_in_traj in traj_joint_names: + if model.existJointName(name_in_traj): + joint_id = model.getJointId(name_in_traj) + joint_obj = model.joints[joint_id] + + # idx_q is the starting index in q for this joint's configuration + # nq is the number of parameters for this joint's configuration (1 for revolute/prismatic) + if joint_obj.nq == 1: # Assuming all trajectory joints are 1-DoF + q_indices_for_traj_joints.append(joint_obj.idx_q) + else: + print( + f"Warning: Joint '{name_in_traj}' in model has {joint_obj.nq} DoFs. Trajectory assumes 1-DoF. Skipping this joint." + ) + q_indices_for_traj_joints.append(None) # Placeholder + else: + missing_joints_in_model.append(name_in_traj) + q_indices_for_traj_joints.append(None) # Placeholder + + if missing_joints_in_model: + print( + f"Warning: The following joints from the trajectory were NOT found in the Pinocchio model: {missing_joints_in_model}" + ) + + if not any(idx is not None for idx in q_indices_for_traj_joints): + print( + "Error: None of the trajectory joints could be mapped to the Pinocchio model. Exiting." + ) + return + + print(f"Mapped q-indices for trajectory joints: {q_indices_for_traj_joints}") + + # Display initial pose (neutral or first trajectory point) + if traj_points: + initial_positions = traj_points[0]["positions"] + for i, q_idx in enumerate(q_indices_for_traj_joints): + if q_idx is not None and i < len(initial_positions): + q[q_idx] = initial_positions[i] + pin_viz.display(q) + + # --- Visualization Loop --- + current_point_idx = 0 + num_trajectory_points = len(traj_points) + print("\n--- Trajectory Control ---") + print("Open the MeshCat URL in your browser if it didn't open automatically.") + + running = True + while running: + print(f"\nPoint: {current_point_idx + 1}/{num_trajectory_points}") + print("Commands: [n]ext, [p]rev, [f]irst, [l]ast, [g ], [a]nimate, [q]uit") + + try: + user_input = input("Enter command: ").strip().lower() + + if user_input == "n": + if current_point_idx < num_trajectory_points - 1: + current_point_idx += 1 + else: + print("Already at the last point.") + elif user_input == "p": + if current_point_idx > 0: + current_point_idx -= 1 + else: + print("Already at the first point.") + elif user_input == "f": + current_point_idx = 0 + elif user_input == "l": + current_point_idx = num_trajectory_points - 1 + elif user_input.startswith("g "): + try: + target_idx = int(user_input.split(" ")[1]) - 1 + if 0 <= target_idx < num_trajectory_points: + current_point_idx = target_idx + else: + print(f"Point number out of range (1-{num_trajectory_points}).") + except (IndexError, ValueError): + print("Invalid format. Use 'g '.") + elif user_input == "a": + print("Animating trajectory... Press Ctrl+C to stop animation.") + start_anim_idx = current_point_idx + try: + for i in range(start_anim_idx, num_trajectory_points): + current_point_idx = i + positions_to_set = traj_points[current_point_idx]["positions"] + temp_q = q.copy() # Start from the full neutral/previous q + for k, q_idx in enumerate(q_indices_for_traj_joints): + if q_idx is not None and k < len(positions_to_set): + temp_q[q_idx] = positions_to_set[k] + pin_viz.display(temp_q) + print( + f" Displaying point {current_point_idx + 1}/{num_trajectory_points}", + end="\r", + flush=True, + ) + time.sleep(0.05) # Animation speed + q[:] = temp_q[:] # Update main q to last animated state + print("\nAnimation finished. ") + except KeyboardInterrupt: + q[:] = temp_q[:] # Update main q to where animation stopped + print("\nAnimation stopped. ") + elif user_input == "q": + running = False + print("Quitting.") + break + else: + if user_input: + print("Invalid command.") + + # Update q for the current point_idx and display + if traj_points and 0 <= current_point_idx < num_trajectory_points: + positions_to_set = traj_points[current_point_idx]["positions"] + # q = pin.neutral(model) # Reset q to neutral before applying current point for fixed-base + # Or, update incrementally from previous q for smoother transitions if some joints are not in traj + for i, q_idx in enumerate(q_indices_for_traj_joints): + if q_idx is not None and i < len(positions_to_set): + q[q_idx] = positions_to_set[i] + pin_viz.display(q) + + except EOFError: # Handle Ctrl+D + print("\nEOF received, quitting.") + running = False + except KeyboardInterrupt: + print("\nInterrupted, quitting.") + running = False + except Exception as e: + print(f"An error occurred in the loop: {e}") + # Decide if you want to break or continue + # running = False + + print("Visualizer finished.") + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Visualize a saved JointTrajectory using Pinocchio and MeshCat." + ) + parser.add_argument("xacro_file", type=str, help="Path to the robot.xacro file.") + parser.add_argument( + "trajectory_file", type=str, help="Path to the .json trajectory file." + ) + + args = parser.parse_args() + main(args.xacro_file, args.trajectory_file) From 6bc2ebf2702300df6db01448763595a86f760d0a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 11 May 2025 10:39:46 -0700 Subject: [PATCH 059/238] Modify trajectory visualization script to handle different joint types, e.g. continuous --- ada_feeding/scripts/visualize_trajectory.py | 314 ++++++++++++-------- 1 file changed, 194 insertions(+), 120 deletions(-) diff --git a/ada_feeding/scripts/visualize_trajectory.py b/ada_feeding/scripts/visualize_trajectory.py index 5c00266f..2e0db490 100755 --- a/ada_feeding/scripts/visualize_trajectory.py +++ b/ada_feeding/scripts/visualize_trajectory.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import pinocchio as pin -import pinocchio.visualize # Import the submodule +import pinocchio.visualize import meshcat import numpy as np import json @@ -23,7 +23,6 @@ def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str logger_func(f"Processing Xacro file: {xacro_filename}") try: - # Try to find a ROS 2 distribution to use in error messages if needed ros_distro = os.environ.get("ROS_DISTRO", "your_ros2_distro") process = subprocess.run( ["ros2", "run", "xacro", "xacro", xacro_filename], @@ -51,7 +50,7 @@ def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=print): """Loads a Pinocchio model from a URDF XML string via a temporary file.""" - temp_urdf_file = None + temp_urdf_path = "" # Initialize to ensure it's defined in finally try: with tempfile.NamedTemporaryFile( mode="w", suffix=".urdf", delete=False @@ -61,47 +60,56 @@ def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=prin logger_func(f"Generated temporary URDF file: {temp_urdf_path}") - # For Pinocchio to find meshes, especially if using package://, - # it might need directories to search. - # Example: model = pin.buildModelFromUrdf(temp_urdf_path, package_dirs=['/path/to/your_ros_ws/src/your_robot_description_pkg']) - # If your meshes are relative to the URDF, and the URDF is generated from Xacro, - # ensure the Xacro resolves paths correctly or provide absolute paths in Xacro, - # or explore pin.loadUrdfModel with package_dirs. - # For now, we assume meshes are findable or simple geometries are used. - - model = pin.buildModelFromUrdf(temp_urdf_path) - # Pinocchio automatically creates collision and visual models from the URDF - collision_model = pin.buildGeomFromUrdf( - model, temp_urdf_path, pin.GeometryType.COLLISION - ) - visual_model = pin.buildGeomFromUrdf( - model, temp_urdf_path, pin.GeometryType.VISUAL - ) + # Attempt to guess package directories if ROS_PACKAGE_PATH is set + package_dirs = [] + ros_package_path = os.environ.get("ROS_PACKAGE_PATH") + if ros_package_path: + package_dirs = [p for p in ros_package_path.split(":") if os.path.isdir(p)] + + if package_dirs: + logger_func(f"Using package_dirs for Pinocchio: {package_dirs}") + model = pin.buildModelFromUrdf(temp_urdf_path, package_dirs=package_dirs) + collision_model = pin.buildGeomFromUrdf( + model, + temp_urdf_path, + pin.GeometryType.COLLISION, + package_dirs=package_dirs, + ) + visual_model = pin.buildGeomFromUrdf( + model, + temp_urdf_path, + pin.GeometryType.VISUAL, + package_dirs=package_dirs, + ) + else: + logger_func( + "Warning: ROS_PACKAGE_PATH not found or empty. Mesh loading might fail if using package:// paths without it." + ) + model = pin.buildModelFromUrdf(temp_urdf_path) + collision_model = pin.buildGeomFromUrdf( + model, temp_urdf_path, pin.GeometryType.COLLISION + ) + visual_model = pin.buildGeomFromUrdf( + model, temp_urdf_path, pin.GeometryType.VISUAL + ) data = model.createData() - collision_data = collision_model.createData() # if you need collision checks - visual_data = ( - visual_model.createData() - ) # if you need to update visual geoms (less common for just display) - logger_func( - f"Pinocchio model loaded successfully. Name: {model.name}, Nq: {model.nq}, Nv: {model.nv}" + f"Pinocchio model loaded. Name: {model.name}, Nq: {model.nq}, Nv: {model.nv}, NJoints: {model.njoints}" ) - logger_func(f"Number of joints in model: {model.njoints}") # Includes universe - # for jname in model.names: print(f" Joint in model: {jname}") - return model, collision_model, visual_model, data except Exception as e: logger_func(f"Error loading Pinocchio model: {e}") return None, None, None, None finally: - if temp_urdf_file and os.path.exists(temp_urdf_path): + if temp_urdf_path and os.path.exists(temp_urdf_path): + # For debugging URDF, comment out the next line + # logger_func(f"Temporary URDF file for debugging: {temp_urdf_path}") os.remove(temp_urdf_path) - logger_func(f"Cleaned up temporary URDF file: {temp_urdf_path}") + # logger_func(f"Cleaned up temporary URDF file: {temp_urdf_path}") def load_trajectory_from_json(filepath: str, logger_func=print): - """Loads a JointTrajectory from a JSON file.""" try: with open(filepath, "r") as f: traj_data = json.load(f) @@ -119,46 +127,69 @@ def load_trajectory_from_json(filepath: str, logger_func=print): def main(xacro_file: str, trajectory_file: str): print("--- Pinocchio + MeshCat Trajectory Visualizer ---") - # 1. Convert Xacro to URDF String urdf_string = xacro_to_urdf_string(xacro_file) if not urdf_string: print("Exiting due to Xacro processing failure.") return - # 2. Load Pinocchio Model load_result = load_pinocchio_model_from_urdf_string(urdf_string) - if not load_result or not load_result[0]: # Check if model is loaded + if not load_result or not load_result[0]: print("Exiting due to Pinocchio model loading failure.") return model, collision_model, visual_model, data = load_result - # 3. Initialize MeshCat Visualizer + if model: + print("\n--- Pinocchio Model Joint Details (from model.joints) ---") + for i in range(model.njoints): + joint_obj = model.joints[i] + joint_name_in_model = model.names[i] + print( + f"IdxInModel {i}: Name='{joint_name_in_model}', PinocchioShortName='{joint_obj.shortname()}', " + f"idx_q={joint_obj.idx_q}, nq={joint_obj.nq}, " + f"idx_v={joint_obj.idx_v}, nv={joint_obj.nv}, " + f"idInModel={joint_obj.id}" + ) + type_name = type(joint_obj).__name__ + # More specific checks for common types that might have nq > 1 + if isinstance(joint_obj, pin.JointModelFreeFlyer): + type_name = "JointModelFreeFlyer (nq=7,nv=6)" + elif isinstance(joint_obj, pin.JointModelSphericalZYX): + type_name = "JointModelSphericalZYX (nq=3,nv=3)" + elif isinstance(joint_obj, pin.JointModelSpherical): + type_name = "JointModelSpherical (nq=4,nv=3)" + elif isinstance(joint_obj, pin.JointModelPlanar): + type_name = "JointModelPlanar (nq=3,nv=3)" + elif isinstance(joint_obj, pin.JointModelTranslation): + type_name = "JointModelTranslation (nq=3,nv=3)" + # Check for unaligned revolute joints (often used for continuous) + elif ( + "Rx" in type_name + or "Ry" in type_name + or "Rz" in type_name + or "RevoluteUnaligned" in type_name + ): + if joint_obj.nq == 2 and joint_obj.nv == 1: + type_name += " (Likely Continuous cos/sin)" + print(f" Type: {type_name}") + print("-------------------------------------------------------\n") + print("Initializing MeshCat viewer... Waiting for connection.") try: - # You can specify a zmq_url if you have a specific MeshCat server running - # visualizer = meshcat.Visualizer(zmq_url="tcp://127.0.0.1:6000") - visualizer = ( - meshcat.Visualizer() - ) # Starts its own server or connects to existing + visualizer = meshcat.Visualizer() print(f"MeshCat viewer URL: {visualizer.url()}") - - # Create Pinocchio's MeshCat visualizer interface - # Note: pin.visualize.MeshcatVisualizer is the correct class pin_viz = pin.visualize.MeshcatVisualizer(model, collision_model, visual_model) - pin_viz.initViewer( - viewer=visualizer, open=True - ) # Pass the meshcat.Visualizer instance + pin_viz.initViewer(viewer=visualizer, open=True) pin_viz.loadViewerModel( - rootNodeName="pinocchio_robot" - ) # Give it a name in the scene tree + rootNodeName=model.name if model.name else "pinocchio_robot" + ) print("MeshCat viewer initialized and model loaded.") - except Exception as e: print(f"Error initializing MeshCat or Pinocchio visualizer: {e}") - print("Please ensure MeshCat is installed and accessible.") + print( + "Please ensure MeshCat is installed and accessible (e.g., `pip install meshcat`)." + ) return - # 4. Load Trajectory trajectory_data = load_trajectory_from_json(trajectory_file) if ( not trajectory_data @@ -170,69 +201,88 @@ def main(xacro_file: str, trajectory_file: str): traj_joint_names = trajectory_data["joint_names"] traj_points = trajectory_data["points"] - if not traj_points: print("Trajectory contains no points. Exiting.") return - # 5. Map Trajectory Joint Names to Pinocchio Model's q indices - # Pinocchio's q includes all DoFs, including the root joint if it's a free flyer. - # For a fixed-base manipulator, model.nq usually matches the number of actuated joints. - # For a floating base, the first 7 elements of q are [tx,ty,tz, qx,qy,qz,qw] for the base. - - # Get the neutral configuration (often zeros for fixed base, or identity for floating base part) q = pin.neutral(model) - print(f"Neutral configuration q (size {model.nq}): {q.T}") + print( + f"Neutral configuration q (size {model.nq}): {q.T if model.nq > 0 else 'N/A (nq=0)'}" + ) - # Create a list of indices in q that correspond to the trajectory joint names - # and their order. - q_indices_for_traj_joints = [] + mapped_joint_info_for_trajectory = [] missing_joints_in_model = [] for name_in_traj in traj_joint_names: if model.existJointName(name_in_traj): joint_id = model.getJointId(name_in_traj) joint_obj = model.joints[joint_id] - - # idx_q is the starting index in q for this joint's configuration - # nq is the number of parameters for this joint's configuration (1 for revolute/prismatic) - if joint_obj.nq == 1: # Assuming all trajectory joints are 1-DoF - q_indices_for_traj_joints.append(joint_obj.idx_q) + if joint_obj.nq > 0: + mapped_joint_info_for_trajectory.append( + { + "name": name_in_traj, + "q_idx_start": joint_obj.idx_q, + "nq": joint_obj.nq, + "nv": joint_obj.nv, + } + ) else: print( - f"Warning: Joint '{name_in_traj}' in model has {joint_obj.nq} DoFs. Trajectory assumes 1-DoF. Skipping this joint." + f"Info: Joint '{name_in_traj}' from trajectory is type '{joint_obj.shortname()}' (nq=0) in model. Will not be actuated." ) - q_indices_for_traj_joints.append(None) # Placeholder + mapped_joint_info_for_trajectory.append(None) else: missing_joints_in_model.append(name_in_traj) - q_indices_for_traj_joints.append(None) # Placeholder + mapped_joint_info_for_trajectory.append(None) if missing_joints_in_model: print( - f"Warning: The following joints from the trajectory were NOT found in the Pinocchio model: {missing_joints_in_model}" + f"Warning: Trajectory joints NOT found in Pinocchio model: {missing_joints_in_model}" ) - if not any(idx is not None for idx in q_indices_for_traj_joints): + valid_mappings = [ + info for info in mapped_joint_info_for_trajectory if info is not None + ] + if not valid_mappings: print( - "Error: None of the trajectory joints could be mapped to the Pinocchio model. Exiting." + "Error: None of the trajectory joints could be mapped to actuated joints in Pinocchio model. Exiting." ) return - print(f"Mapped q-indices for trajectory joints: {q_indices_for_traj_joints}") + print("\nMapped info for trajectory joints:") + for i, info in enumerate(mapped_joint_info_for_trajectory): + if info: + print( + f" Traj Joint {i} ('{info['name']}'): Maps to q[{info['q_idx_start']}-" + f"{info['q_idx_start'] + info['nq'] - 1}], ModelJoint_nq={info['nq']}, ModelJoint_nv={info['nv']}" + ) + else: + print( + f" Traj Joint {i} ('{traj_joint_names[i]}'): Not mapped or not actuated." + ) - # Display initial pose (neutral or first trajectory point) + # Initial display + current_q_display = q.copy() if traj_points: initial_positions = traj_points[0]["positions"] - for i, q_idx in enumerate(q_indices_for_traj_joints): - if q_idx is not None and i < len(initial_positions): - q[q_idx] = initial_positions[i] - pin_viz.display(q) + for k, mapping_info in enumerate(mapped_joint_info_for_trajectory): + if mapping_info and k < len(initial_positions): + theta_traj = initial_positions[k] + q_idx_start = mapping_info["q_idx_start"] + joint_nq = mapping_info["nq"] + joint_nv = mapping_info["nv"] + if joint_nq == 1: # Standard revolute/prismatic (includes nv=0 or nv=1) + current_q_display[q_idx_start] = theta_traj + elif joint_nq == 2 and joint_nv == 1: # Continuous (cos/sin) + current_q_display[q_idx_start] = np.cos(theta_traj) + current_q_display[q_idx_start + 1] = np.sin(theta_traj) + pin_viz.display(current_q_display) + q[:] = current_q_display[:] # Update main q - # --- Visualization Loop --- current_point_idx = 0 num_trajectory_points = len(traj_points) print("\n--- Trajectory Control ---") - print("Open the MeshCat URL in your browser if it didn't open automatically.") + print("Open the MeshCat URL in your browser.") running = True while running: @@ -241,17 +291,20 @@ def main(xacro_file: str, trajectory_file: str): try: user_input = input("Enter command: ").strip().lower() + new_q_to_display = ( + q.copy() + ) # Start with current q, only update relevant parts - if user_input == "n": - if current_point_idx < num_trajectory_points - 1: - current_point_idx += 1 - else: - print("Already at the last point.") + if user_input == "q": + running = False + print("Quitting.") + break + elif user_input == "n": + current_point_idx = min( + current_point_idx + 1, num_trajectory_points - 1 + ) elif user_input == "p": - if current_point_idx > 0: - current_point_idx -= 1 - else: - print("Already at the first point.") + current_point_idx = max(current_point_idx - 1, 0) elif user_input == "f": current_point_idx = 0 elif user_input == "l": @@ -263,50 +316,71 @@ def main(xacro_file: str, trajectory_file: str): current_point_idx = target_idx else: print(f"Point number out of range (1-{num_trajectory_points}).") - except (IndexError, ValueError): + continue + except: print("Invalid format. Use 'g '.") + continue elif user_input == "a": print("Animating trajectory... Press Ctrl+C to stop animation.") start_anim_idx = current_point_idx + anim_q = q.copy() try: for i in range(start_anim_idx, num_trajectory_points): current_point_idx = i positions_to_set = traj_points[current_point_idx]["positions"] - temp_q = q.copy() # Start from the full neutral/previous q - for k, q_idx in enumerate(q_indices_for_traj_joints): - if q_idx is not None and k < len(positions_to_set): - temp_q[q_idx] = positions_to_set[k] - pin_viz.display(temp_q) + for k, mapping_info in enumerate( + mapped_joint_info_for_trajectory + ): + if mapping_info and k < len(positions_to_set): + theta_traj = positions_to_set[k] + q_idx_start = mapping_info["q_idx_start"] + joint_nq = mapping_info["nq"] + joint_nv = mapping_info["nv"] + if joint_nq == 1: + anim_q[q_idx_start] = theta_traj + elif joint_nq == 2 and joint_nv == 1: + anim_q[q_idx_start] = np.cos(theta_traj) + anim_q[q_idx_start + 1] = np.sin(theta_traj) + pin_viz.display(anim_q) print( f" Displaying point {current_point_idx + 1}/{num_trajectory_points}", end="\r", flush=True, ) - time.sleep(0.05) # Animation speed - q[:] = temp_q[:] # Update main q to last animated state + time.sleep(0.05) print("\nAnimation finished. ") except KeyboardInterrupt: - q[:] = temp_q[:] # Update main q to where animation stopped print("\nAnimation stopped. ") - elif user_input == "q": - running = False - print("Quitting.") - break + new_q_to_display[:] = anim_q[:] # Update with last animated state + q[:] = new_q_to_display[:] + pin_viz.display(q) # Ensure final state is displayed once + continue # Skip re-applying single point after animation + elif user_input: + print("Invalid command.") + continue else: - if user_input: - print("Invalid command.") + continue # Empty input, re-prompt - # Update q for the current point_idx and display + # Update new_q_to_display for the current_point_idx if traj_points and 0 <= current_point_idx < num_trajectory_points: positions_to_set = traj_points[current_point_idx]["positions"] - # q = pin.neutral(model) # Reset q to neutral before applying current point for fixed-base - # Or, update incrementally from previous q for smoother transitions if some joints are not in traj - for i, q_idx in enumerate(q_indices_for_traj_joints): - if q_idx is not None and i < len(positions_to_set): - q[q_idx] = positions_to_set[i] - pin_viz.display(q) - - except EOFError: # Handle Ctrl+D + for k, mapping_info in enumerate(mapped_joint_info_for_trajectory): + if mapping_info and k < len(positions_to_set): + theta_traj = positions_to_set[k] + q_idx_start = mapping_info["q_idx_start"] + joint_nq = mapping_info["nq"] + joint_nv = mapping_info["nv"] + if joint_nq == 1: + new_q_to_display[q_idx_start] = theta_traj + elif joint_nq == 2 and joint_nv == 1: + new_q_to_display[q_idx_start] = np.cos(theta_traj) + new_q_to_display[q_idx_start + 1] = np.sin(theta_traj) + # else: # Other nq/nv cases are not set + + pin_viz.display(new_q_to_display) + q[:] = new_q_to_display[:] # Persist the displayed q + + except EOFError: print("\nEOF received, quitting.") running = False except KeyboardInterrupt: @@ -314,20 +388,20 @@ def main(xacro_file: str, trajectory_file: str): running = False except Exception as e: print(f"An error occurred in the loop: {e}") - # Decide if you want to break or continue - # running = False - print("Visualizer finished.") if __name__ == "__main__": parser = argparse.ArgumentParser( - description="Visualize a saved JointTrajectory using Pinocchio and MeshCat." + description="Visualize saved JointTrajectory using Pinocchio and MeshCat." + ) + parser.add_argument( + "xacro_file", + type=str, + help="Path to the robot XACRO file (that instantiates the robot).", ) - parser.add_argument("xacro_file", type=str, help="Path to the robot.xacro file.") parser.add_argument( "trajectory_file", type=str, help="Path to the .json trajectory file." ) - args = parser.parse_args() main(args.xacro_file, args.trajectory_file) From 10fdeb3257125feb47007fac6a458c5b9186e2f2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 11 May 2025 13:11:22 -0700 Subject: [PATCH 060/238] Modify benchmark to test constrained planning from each configuration to each other configuration, which is more representative of what happens in practice --- ...ientation_constrained_planner_benchmark.py | 411 ++++++++++-------- 1 file changed, 236 insertions(+), 175 deletions(-) diff --git a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py index 9ce500d1..3067ca38 100755 --- a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py +++ b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py @@ -5,7 +5,8 @@ """ This script is used to benchmark planner performance for reaching pre-defined joint configurations while enforcing a path-wide orientation -constraint on the end-effector. It now also saves successful trajectories. +constraint on the end-effector. It now also saves successful trajectories +and tests planning between all pairs of defined configurations. """ # Standard imports @@ -17,6 +18,7 @@ from threading import Thread, Lock from typing import Optional, List, Dict, Tuple, Any import json +import itertools # Third-party imports import numpy as np @@ -119,7 +121,11 @@ def fallback_get_path_len( GET_PATH_LEN_METHOD = fallback_get_path_len -BenchmarkCondition = namedtuple("BenchmarkCondition", ["name", "joint_values"]) +# Represents a single named joint configuration +BenchmarkNamedConfig = namedtuple("BenchmarkNamedConfig", ["name", "joint_values"]) +# Represents a planning problem from a start to a goal configuration +PlanningTask = namedtuple("PlanningTask", ["start_config", "goal_config"]) + PlanResult = namedtuple( "PlanResult", [ @@ -135,12 +141,7 @@ def fallback_get_path_len( class PlannerBenchmark: - NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ = ( - 0.5, - 0.5, - 0.5, - 0.5, - ) + NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ = (0.5, 0.5, 0.5, 0.5) NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS = ( np.pi * 1.99, np.pi * 1.99, @@ -174,13 +175,22 @@ def __init__( self.joint_names_for_group = joint_names_for_group self.planning_timeout_sec = planning_timeout_sec - self.target_configurations = self._process_hardcoded_configurations( + # Process the hardcoded configurations into a list of BenchmarkNamedConfig + self.all_named_configurations = self._process_hardcoded_configurations( hardcoded_target_configs ) - self.num_conditions = len(self.target_configurations) - self.results: Dict[str, Dict[str, PlanResult]] = defaultdict(dict) - self.rate = self.node.create_rate(10) + # Generate all planning tasks (pairs of start_config, goal_config) + self.planning_tasks = self._generate_planning_tasks( + self.all_named_configurations + ) + self.num_tasks = len(self.planning_tasks) + + # Results stored as: self.results[(start_name, goal_name)][planner_id] + self.results: Dict[Tuple[str, str], Dict[str, PlanResult]] = defaultdict( + lambda: defaultdict(dict) + ) + self.rate = self.node.create_rate(10) # For plan_async timeout loop self.no_roll_path_constraint_kwargs = self._get_no_roll_constraint_kwargs() @@ -195,36 +205,50 @@ def __init__( f"Benchmark initialized for group '{self.planning_group}' and EE '{self.end_effector_link}'." ) self.logger.info( - f"Testing {self.num_conditions} target configurations with planners: {self.planners_to_test}" + f"Generated {self.num_tasks} planning tasks from {len(self.all_named_configurations)} unique configurations." ) + self.logger.info(f"Testing with planners: {self.planners_to_test}") self.logger.info( f"Using 'no roll' constraint: Target Quat (xyzw)={self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ}, Tol(xyz_abs)={self.NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS}" ) def _process_hardcoded_configurations( self, hardcoded_configs: Dict[str, List[float]] - ) -> List[BenchmarkCondition]: + ) -> List[BenchmarkNamedConfig]: configs = [] - self.logger.info(f"Processing hardcoded target configurations...") + self.logger.info(f"Processing hardcoded named configurations...") num_expected_joints = len(self.joint_names_for_group) for name, values in hardcoded_configs.items(): if isinstance(values, list) and len(values) == num_expected_joints: - configs.append(BenchmarkCondition(name, values)) + configs.append(BenchmarkNamedConfig(name, values)) self.logger.info( - f" Loaded '{name}' with {num_expected_joints} joints." + f" Loaded named config '{name}' with {num_expected_joints} joints." ) else: self.logger.warn( f" Skipping hardcoded config '{name}'. Expected {num_expected_joints} joint values, got {len(values) if isinstance(values, list) else type(values)}." ) - if not configs: + if not configs or len(configs) < 2: self.logger.error( - f"No valid target configurations provided from hardcoded list. Exiting." + f"Not enough valid named configurations provided (found {len(configs)}, need at least 2 for pair-wise tasks). Exiting." ) sys.exit(1) return configs + def _generate_planning_tasks( + self, named_configs: List[BenchmarkNamedConfig] + ) -> List[PlanningTask]: + tasks = [] + for start_config in named_configs: + for goal_config in named_configs: + if start_config.name != goal_config.name: + tasks.append(PlanningTask(start_config, goal_config)) + self.logger.info( + f"Generated {len(tasks)} planning tasks (all pairs, start != goal)." + ) + return tasks + def _get_no_roll_constraint_kwargs(self) -> Dict[str, Any]: return { "quat_xyzw": Quaternion( @@ -245,18 +269,14 @@ def _calculate_max_roll_deviation( ) -> Optional[float]: if trajectory is None or not trajectory.points: return None - max_abs_roll_deviation = 0.0 - for point_idx, point in enumerate(trajectory.points): if len(point.positions) != len(self.joint_names_for_group): self.logger.warn( f"FK Calc: Mismatch in joint count at point {point_idx}. Expected {len(self.joint_names_for_group)}, got {len(point.positions)}. Skipping." ) continue - joint_positions_list = list(point.positions) - try: result_poses: Optional[List[PoseStamped]] = ( self.moveit2_interface.compute_fk( @@ -264,7 +284,6 @@ def _calculate_max_roll_deviation( fk_link_names=[self.end_effector_link], ) ) - if ( not result_poses or not isinstance(result_poses, list) @@ -274,9 +293,7 @@ def _calculate_max_roll_deviation( f"FK returned None or invalid format for point {point_idx}." ) continue - pose_stamped_ee = result_poses[0] - if pose_stamped_ee.header.frame_id.lstrip("/") != self.base_link.lstrip( "/" ): @@ -284,53 +301,24 @@ def _calculate_max_roll_deviation( f"FK pose for {self.end_effector_link} is in frame '{pose_stamped_ee.header.frame_id}', " f"expected '{self.base_link}'. Ensure consistency or implement TF." ) - q = pose_stamped_ee.pose.orientation - - # Convert the actual EE orientation to SciPy Rotation actual_ee_orientation_scipy = R.from_quat([q.x, q.y, q.z, q.w]) - - # Convert the target "no roll" orientation (defined by NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ) - # to SciPy Rotation. This quaternion is already w.r.t. base_link. target_quat_params = self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ target_ee_orientation_scipy = R.from_quat(target_quat_params) - - # Calculate the rotational difference: R_diff = R_target_inv * R_actual - # This R_diff represents the rotation needed to get from target to actual. - # Its Euler angles (especially around the Z-axis of the *target* frame) - # can indicate roll deviation. diff_rotation = ( target_ee_orientation_scipy.inv() * actual_ee_orientation_scipy ) - - # Get Euler angles of this difference. The Z-angle of this difference, - # if using 'xyz' for example, would represent the roll *after* aligning X and Y. - # Choose an Euler sequence that makes sense for your definition of "roll". - # 'zyx' is common, where the first angle (z) is yaw, second (y) is pitch, third (x) is roll. - # If your constraint is primarily about the EE's Z-axis pointing, then 'xyz' might be more intuitive - # where the last angle (z) is the rotation around the new Z axis. - # For this 'no roll' constraint, we are interested in rotation around the target EE's Z-axis. - # Let's use the target frame's Z-axis as the roll axis. - # Euler angles of R_diff in target frame's basis: euler_angles_of_diff_in_target_basis = diff_rotation.as_euler( "xyz", degrees=False - ) # or 'ZYX', etc. - - # The roll deviation would be the rotation around the axis that corresponds to "roll" - # in your chosen Euler sequence. For 'xyz', euler_angles_of_diff_in_target_basis[2] is about the new Z. - # This should correspond to the Z-axis of the `NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ` + ) current_roll_deviation = euler_angles_of_diff_in_target_basis[2] - - # Normalize to [-pi, pi] while current_roll_deviation > np.pi: current_roll_deviation -= 2 * np.pi while current_roll_deviation < -np.pi: current_roll_deviation += 2 * np.pi - max_abs_roll_deviation = max( max_abs_roll_deviation, abs(current_roll_deviation) ) - except Exception as e: self.logger.error( f"Error during FK for roll deviation at point {point_idx}: {e}", @@ -351,20 +339,21 @@ def _get_path_length_stats( return GET_PATH_LEN_METHOD(trajectory) def _save_trajectory_to_file( - self, trajectory: JointTrajectory, condition_name: str, planner_id_str: str + self, + trajectory: JointTrajectory, + start_config_name: str, + goal_config_name: str, + planner_id_str: str, ) -> Optional[str]: if not self.trajectory_save_dir: return None - try: timestamp = datetime.now().strftime("%Y%m%d-%H%M%S-%f") - # Sanitize condition_name and planner_id_str for filename - safe_condition_name = condition_name.replace(" ", "_").replace("/", "_") + safe_start_name = start_config_name.replace(" ", "_").replace("/", "_") + safe_goal_name = goal_config_name.replace(" ", "_").replace("/", "_") safe_planner_id_str = planner_id_str.replace(" ", "_").replace("/", "_") - - filename = f"{safe_condition_name}_{safe_planner_id_str}_{timestamp}.json" + filename = f"{safe_start_name}_to_{safe_goal_name}_{safe_planner_id_str}_{timestamp}.json" filepath = os.path.join(self.trajectory_save_dir, filename) - traj_dict = message_to_ordereddict(trajectory) with open(filepath, "w") as f: json.dump(traj_dict, f, indent=2) @@ -372,7 +361,7 @@ def _save_trajectory_to_file( return filename except Exception as e: self.logger.error( - f" Failed to save trajectory for {condition_name} ({planner_id_str}): {e}", + f" Failed to save trajectory for {start_config_name} to {goal_config_name} ({planner_id_str}): {e}", ) return None @@ -383,24 +372,20 @@ def plan_to_target_configuration( path_constraints_kwargs_to_apply: Optional[Dict[str, Any]] = None, ) -> Tuple[Optional[JointTrajectory], float]: self.logger.info( - f" Attempting to plan with: {planner_id_str} to {goal_joints}" + f" Attempting to plan with: {planner_id_str} to GOAL: {goal_joints}" ) - if planner_id_str.lower() == "chomp": self.moveit2_interface.planning_pipeline_id = "chomp" self.moveit2_interface.planner_id = "chomp" elif planner_id_str.lower() == "stomp": self.moveit2_interface.planning_pipeline_id = "stomp" self.moveit2_interface.planner_id = "" - else: # OMPL + else: self.moveit2_interface.planning_pipeline_id = "ompl" self.moveit2_interface.planner_id = planner_id_str - self.moveit2_interface.allowed_planning_time = self.planning_timeout_sec - self.moveit2_interface.clear_goal_constraints() self.moveit2_interface.clear_path_constraints() - try: self.moveit2_interface.set_joint_goal( joint_positions=goal_joints, @@ -412,7 +397,6 @@ def plan_to_target_configuration( except Exception as e: self.logger.error(f" Failed to set joint goal: {e}") return None, 0.0 - if path_constraints_kwargs_to_apply: try: self.moveit2_interface.set_path_orientation_constraint( @@ -423,13 +407,13 @@ def plan_to_target_configuration( ) except Exception as e: self.logger.error(f" Failed to set path orientation constraint: {e}") - start_time_ros = self.node.get_clock().now() - future = self.moveit2_interface.plan_async(start_joint_state=None) + future = self.moveit2_interface.plan_async( + start_joint_state=None + ) # None means plan from current joint_trajectory_for_analysis: Optional[JointTrajectory] = None timeout_duration_rclpy = Duration(seconds=self.planning_timeout_sec + 2.0) wait_start_time_rclpy = self.node.get_clock().now() - while rclpy.ok() and not future.done(): elapsed_wait = self.node.get_clock().now() - wait_start_time_rclpy if elapsed_wait >= timeout_duration_rclpy: @@ -444,7 +428,6 @@ def plan_to_target_configuration( future.cancel() break self.rate.sleep() - if future.done() and not future.cancelled(): try: plan_result_srv_response = future.result() @@ -468,58 +451,104 @@ def plan_to_target_configuration( ) except Exception as e: self.logger.error( - f" Exception while getting plan result for {planner_id_str}: {e}", + f" Exception while getting plan result for {planner_id_str}: {e}" ) elif future.cancelled(): self.logger.warn( f" Planning for {planner_id_str} was cancelled (likely due to timeout)." ) - elapsed_time = (self.node.get_clock().now() - start_time_ros).nanoseconds / 1e9 self.moveit2_interface.clear_path_constraints() self.moveit2_interface.clear_goal_constraints() return joint_trajectory_for_analysis, elapsed_time - def move_to_initial_config(self): + def _move_to_config_blocking( + self, + target_config_name: str, + target_joints: List[float], + move_timeout_sec: float = 20.0, + ): + """Moves the robot to a specified joint configuration and waits.""" self.logger.info( - f"Moving to initial configuration: {self.initial_joint_config}" + f"Attempting to move to configuration: '{target_config_name}' {target_joints}" ) original_planner = self.moveit2_interface.planner_id original_pipeline = self.moveit2_interface.pipeline_id original_timeout = self.moveit2_interface.allowed_planning_time + # Use a reliable planner for setup moves self.moveit2_interface.planning_pipeline_id = "ompl" self.moveit2_interface.planner_id = "RRTConnectkConfigDefault" - self.moveit2_interface.allowed_planning_time = 10.0 + self.moveit2_interface.allowed_planning_time = max( + 5.0, self.planning_timeout_sec / 2.0 + ) + self.moveit2_interface.clear_goal_constraints() self.moveit2_interface.clear_path_constraints() try: self.logger.info( - f" Commanding move to configuration with {len(self.initial_joint_config)} joints for group '{self.planning_group}'" + f" Commanding move to '{target_config_name}' for group '{self.planning_group}'" ) + # PyMoveIt2's move_to_configuration blocks until motion is complete or failed self.moveit2_interface.move_to_configuration( - joint_positions=self.initial_joint_config, + joint_positions=target_joints, joint_names=self.joint_names_for_group, + tolerance=0.01, ) - self.moveit2_interface.wait_until_executed() - self.logger.info("Reached initial configuration.") + # wait_until_executed might be part of move_to_configuration or needs to be called if plan_exec is used. + # For move_to_configuration, it typically blocks. + # We can add a small sleep or check current joint state if needed for more robustness. + time.sleep(1.0) + + # Verification + current_joints = self.moveit2_interface.get_joint_positions() + if current_joints: # TODO: Replace with actual current joint state getter + if not np.allclose(current_joints, target_joints, atol=0.05): + self.logger.warn( + f" Post-move verification: Current joints {current_joints} not close enough to target {target_joints} for '{target_config_name}'." + ) + else: + self.logger.info( + f" Successfully moved to configuration: '{target_config_name}'." + ) + else: + self.logger.info( + f" Move command to '{target_config_name}' sent. Assuming success without immediate verification." + ) + except Exception as e: - self.logger.error(f"Failed to move to initial configuration: {e}") - raise + self.logger.error( + f"Failed to move to configuration '{target_config_name}': {e}" + ) + # This is a critical failure for the benchmark structure if a start state can't be reached. + # Depending on desired behavior, could raise, or just log and subsequent plans might fail from wrong start. + raise RuntimeError( + f"Critical failure: Could not move to start configuration '{target_config_name}'." + ) from e finally: self.moveit2_interface.planner_id = original_planner self.moveit2_interface.planning_pipeline_id = original_pipeline self.moveit2_interface.allowed_planning_time = original_timeout - def run_benchmark_condition(self, condition: BenchmarkCondition): - self.logger.info(f"--- Testing Configuration: {condition.name} ---") - self.logger.info(f" Target Joints: {condition.joint_values}") + def move_to_initial_config(self): + self._move_to_config_blocking("InitialScriptSetup", self.initial_joint_config) + + def run_benchmark_planning_task(self, task: PlanningTask): + start_name = task.start_config.name + goal_name = task.goal_config.name + goal_joints = task.goal_config.joint_values + + self.logger.info(f"--- Testing Task: FROM '{start_name}' TO '{goal_name}' ---") + self.logger.info(f" Goal Joints: {goal_joints}") + + task_key = (start_name, goal_name) for planner_id_str in self.planners_to_test: + # Current state is assumed to be task.start_config.joint_values due to prior _move_to_config_blocking joint_trajectory, elapsed_time = self.plan_to_target_configuration( - goal_joints=condition.joint_values, + goal_joints=goal_joints, planner_id_str=planner_id_str, path_constraints_kwargs_to_apply=self.no_roll_path_constraint_kwargs, ) @@ -533,13 +562,10 @@ def run_benchmark_condition(self, condition: BenchmarkCondition): trajectory_filename = None if success and self.trajectory_save_dir: trajectory_filename = self._save_trajectory_to_file( - joint_trajectory, condition.name, planner_id_str + joint_trajectory, start_name, goal_name, planner_id_str ) - if condition.name not in self.results: - self.results[condition.name] = {} - - self.results[condition.name][planner_id_str] = PlanResult( + self.results[task_key][planner_id_str] = PlanResult( joint_trajectory, elapsed_time, path_len_total, @@ -563,29 +589,60 @@ def run_benchmark_condition(self, condition: BenchmarkCondition): f" Planner: {planner_id_str} | Success: False | Time: {elapsed_time:.3f}s {log_msg_suffix}" ) - def run_all_tests(self, log_summary_every_n_conditions: Optional[int] = None): + def run_all_tests(self, log_summary_every_n_tasks: Optional[int] = None): + self.logger.info( + "=== Starting Benchmark: Moving to Initial Script Configuration ===" + ) self.move_to_initial_config() - for i, condition in enumerate(self.target_configurations): + for i, planning_task in enumerate(self.planning_tasks): self.logger.info( - f"=== Running Condition {i + 1}/{self.num_conditions}: {condition.name} ===" + f"\n=== Running Planning Task {i + 1}/{self.num_tasks}: " + f"FROM '{planning_task.start_config.name}' TO '{planning_task.goal_config.name}' ===" ) - self.run_benchmark_condition(condition) - if ( - log_summary_every_n_conditions - and (i + 1) % log_summary_every_n_conditions == 0 - ): + # Move to the start configuration for this specific task + try: + self._move_to_config_blocking( + planning_task.start_config.name, + planning_task.start_config.joint_values, + ) + except RuntimeError as e: + self.logger.error( + f"Skipping task FROM '{planning_task.start_config.name}' TO '{planning_task.goal_config.name}' due to failure to reach start state: {e}" + ) + # Optionally, record this failure in results for this task pair + task_key = ( + planning_task.start_config.name, + planning_task.goal_config.name, + ) + for planner_id_str in self.planners_to_test: + self.results[task_key][planner_id_str] = PlanResult( + None, + 0.0, + None, + None, + None, + False, + f"ERROR_MOVE_TO_START_FAILED:{e}", + ) + continue + + # Now run the actual benchmarked planning for this task + self.run_benchmark_planning_task(planning_task) + + if log_summary_every_n_tasks and (i + 1) % log_summary_every_n_tasks == 0: self.log_summary_results() self.logger.info("=== Benchmark Run Completed ===") def get_csv_header(self) -> List[str]: - header = [f"start_{name}" for name in self.joint_names_for_group] + header = ["start_config_name"] + header.extend([f"start_{name}" for name in self.joint_names_for_group]) + header.extend(["goal_config_name"]) + header.extend([f"goal_{name}" for name in self.joint_names_for_group]) header.extend( [ - "goal_config_name", - *[f"goal_{name}" for name in self.joint_names_for_group], "planner_id", "elapsed_time_s", "success", @@ -599,7 +656,6 @@ def get_csv_header(self) -> List[str]: def write_results_to_csv(self, filename: str): self.logger.info(f"Writing results to {filename}") - start_config_for_csv = self.initial_joint_config with open(filename, "w", newline="") as f: import csv @@ -608,17 +664,39 @@ def write_results_to_csv(self, filename: str): header = self.get_csv_header() csv_writer.writerow(header) - for target_condition in self.target_configurations: - config_name = target_condition.name - if config_name in self.results: - planner_runs = self.results[config_name] + # Iterate through the planning tasks to maintain order if desired, + # or directly through self.results.keys() + for task_key in sorted(self.results.keys()): + start_name, goal_name = task_key + + # Find the original BenchmarkNamedConfig objects for start and goal + # This is a bit inefficient but ensures data integrity if names are unique + start_cfg_obj = next( + (c for c in self.all_named_configurations if c.name == start_name), + None, + ) + goal_cfg_obj = next( + (c for c in self.all_named_configurations if c.name == goal_name), + None, + ) + + if not start_cfg_obj or not goal_cfg_obj: + self.logger.warn( + f"Could not find original config objects for task key {task_key}. Skipping CSV rows for this task." + ) + continue + + if task_key in self.results: + planner_runs_for_task = self.results[task_key] for planner_id_str in self.planners_to_test: - if planner_id_str in planner_runs: - result = planner_runs[planner_id_str] + if planner_id_str in planner_runs_for_task: + result = planner_runs_for_task[planner_id_str] row = [] - row.extend(map(str, start_config_for_csv)) - row.append(str(target_condition.name)) - row.extend(map(str, target_condition.joint_values)) + row.append(str(start_cfg_obj.name)) + row.extend(map(str, start_cfg_obj.joint_values)) + row.append(str(goal_cfg_obj.name)) + row.extend(map(str, goal_cfg_obj.joint_values)) + row.append(str(planner_id_str)) row.append(f"{result.elapsed_time:.4f}") row.append(str(1 if result.success else 0)) @@ -639,9 +717,9 @@ def write_results_to_csv(self, filename: str): ) if result.joint_path_lengths: - for joint_name in self.joint_names_for_group: + for joint_name_csv in self.joint_names_for_group: length_val = result.joint_path_lengths.get( - joint_name + joint_name_csv ) row.append( f"{length_val:.4f}" @@ -653,33 +731,27 @@ def write_results_to_csv(self, filename: str): csv_writer.writerow(row) else: self.logger.warn( - f"No result for planner '{planner_id_str}' in condition '{config_name}'. Skipping CSV row." + f"No result for planner '{planner_id_str}' in task '{start_name}' to '{goal_name}'. Skipping CSV row." ) - else: - self.logger.warn( - f"No results found for configuration: {config_name} in self.results. Skipping CSV rows for this condition." - ) self.logger.info(f"Results successfully written to {filename}") def log_summary_results(self): - self.logger.info("--- BENCHMARK SUMMARY ---") + self.logger.info("--- BENCHMARK SUMMARY (ALL TASKS) ---") for planner_id_str in self.planners_to_test: - results_for_planner = [ - run_results[planner_id_str] - for run_results in self.results.values() - if planner_id_str in run_results - ] + # Collect all results for this planner across all (start, goal) pairs + results_for_planner = [] + for task_results_dict in self.results.values(): + if planner_id_str in task_results_dict: + results_for_planner.append(task_results_dict[planner_id_str]) total_plans = len(results_for_planner) if total_plans == 0: self.logger.info(f"--- Planner: {planner_id_str} ---") self.logger.info(" No plans attempted or recorded for this planner.") continue - successful_plans = sum(1 for r in results_for_planner if r.success) total_planning_time = sum(r.elapsed_time for r in results_for_planner) - path_lengths_list = [ r.path_length for r in results_for_planner @@ -690,7 +762,6 @@ def log_summary_results(self): for r in results_for_planner if r.success and r.max_roll_deviation is not None ] - self.logger.info(f"--- Planner: {planner_id_str} ---") success_rate = ( (successful_plans / total_plans) * 100 if total_plans > 0 else 0 @@ -698,7 +769,6 @@ def log_summary_results(self): avg_planning_time_all = ( total_planning_time / total_plans if total_plans > 0 else 0 ) - successful_planning_times = [ r.elapsed_time for r in results_for_planner if r.success ] @@ -707,7 +777,6 @@ def log_summary_results(self): if successful_planning_times else 0 ) - self.logger.info( f" Success Rate: {success_rate:.2f}% ({successful_plans}/{total_plans})" ) @@ -718,14 +787,12 @@ def log_summary_results(self): self.logger.info( f" Avg. Planning Time (successful attempts): {avg_planning_time_succ:.3f}s" ) - if path_lengths_list: self.logger.info( f" Path Lengths (successful plans): {PlannerBenchmark._summary_stats_str(path_lengths_list)}" ) else: self.logger.info(" Path Lengths (successful plans): N/A") - valid_roll_deviations = [ r for r in roll_deviations_list @@ -757,22 +824,20 @@ def _summary_stats_str(data: List[float]) -> str: def main(output_dir_arg: Optional[str]): rclpy.init() - node = Node("planner_benchmark_joint_config_constrained") - + node = Node("planner_benchmark_pairwise_constrained") global _LOGGER_INSTANCE _LOGGER_INSTANCE = node.get_logger() - executor = rclpy.executors.MultiThreadedExecutor(2) executor.add_node(node) executor_thread = Thread(target=executor.spin, daemon=True, args=()) executor_thread.start() - _LOGGER_INSTANCE.info( "Benchmark node spinning. Waiting for services (approx 5s)..." ) time.sleep(5.0) - hardcoded_targets = { + # --- Hardcoded Named Configurations (used as pool for start/goal states) --- + hardcoded_configs = { "MoveAbovePlate": [ -2.4538579336877304, 3.07974419938212, @@ -782,15 +847,7 @@ def main(output_dir_arg: Optional[str]): -3.2123560395465063, ], "RestingAcquireFood": [-1.94672, 2.51268, 0.35653, -4.76501, 5.99991, 4.99555], - "MoveToRestingPosition": [ - -1.94672, - 2.51268, - 0.35653, - -4.76501, - 5.99991, - 4.99555, - ], - "MoveToStagingConfiguration": [ + "StagingConfig": [ -2.32526, 4.456298, 4.16769, @@ -798,37 +855,40 @@ def main(output_dir_arg: Optional[str]): -2.18359, -2.19525, ], - "MoveToStowLocation": [-1.52101, 2.60098, 0.32811, -4.00012, 0.22831, 3.87886], + "StowLocation": [ + -1.52101, + 2.60098, + 0.32811, + -4.00012, + 0.22831, + 3.87886, + ], } - _LOGGER_INSTANCE.info("Using hardcoded target configurations.") - - planners = [ - "RRTConnectkConfigDefault", - "RRTstarkConfigDefault", - "CHOMP", - ] - initial_config = [ - -2.4538579336877304, - 3.07974419938212, - 1.8320725365979, - 4.096143890468605, - -2.003422584820525, - -3.2123560395465063, - ] + _LOGGER_INSTANCE.info( + f"Using {len(hardcoded_configs)} hardcoded named configurations for generating planning tasks." + ) + + planners = ["RRTConnectkConfigDefault", "RRTstarkConfigDefault", "CHOMP"] + + # This initial_config is for the VERY FIRST robot position before any tasks begin. + # It could be one of the hardcoded_configs or something else. + # For simplicity, let's make it the first one from hardcoded_configs if available. + # Or use the one previously defined. + initial_script_setup_config = list(hardcoded_configs.values())[0] + planning_group_name = "jaco_arm" ee_link_name = "j2n6s200_end_effector" base_link_name = kinova.base_link_name() group_joint_names = kinova.joint_names() planning_timeout = 15.0 - # --- Trajectory Save Directory --- base_output_dir = output_dir_arg if output_dir_arg else "." - # Create the base output directory here if it's specified and doesn't exist if output_dir_arg and not os.path.exists(base_output_dir): os.makedirs(base_output_dir) _LOGGER_INSTANCE.info(f"Created base output directory: {base_output_dir}") - - trajectory_save_location = os.path.join(base_output_dir, "saved_trajectories") + trajectory_save_location = os.path.join( + base_output_dir, "saved_trajectories_pairwise" + ) callback_group = ReentrantCallbackGroup() moveit2_interface = MoveIt2( @@ -849,9 +909,9 @@ def main(output_dir_arg: Optional[str]): benchmark_runner = PlannerBenchmark( node=node, moveit2_interface=moveit2_interface, - hardcoded_target_configs=hardcoded_targets, + hardcoded_target_configs=hardcoded_configs, planners_to_test=planners, - initial_joint_config=initial_config, + initial_joint_config=initial_script_setup_config, planning_group=planning_group_name, end_effector_link=ee_link_name, base_link=base_link_name, @@ -861,9 +921,10 @@ def main(output_dir_arg: Optional[str]): ) try: - benchmark_runner.run_all_tests(log_summary_every_n_conditions=1) + num_total_tasks = benchmark_runner.num_tasks + log_interval = max(1, num_total_tasks // 5) + benchmark_runner.run_all_tests(log_summary_every_n_tasks=log_interval) - # Ensure base_output_dir exists before writing CSV if not os.path.exists(base_output_dir) and base_output_dir != ".": os.makedirs(base_output_dir) _LOGGER_INSTANCE.info( @@ -875,7 +936,7 @@ def main(output_dir_arg: Optional[str]): csv_filename = os.path.join( base_output_dir, datetime.now().strftime("%Y-%m-%d_%H-%M-%S") - + "_joint_config_constrained_benchmark.csv", + + "_pairwise_constrained_benchmark.csv", ) benchmark_runner.write_results_to_csv(csv_filename) benchmark_runner.log_summary_results() From bd70f75daa854cd4b17c603f3a8bb08400187d2f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 11 May 2025 14:28:51 -0700 Subject: [PATCH 061/238] Address issue where continuous joints reported joint states that were off by 2pi and thus were not recognized as equivalent when verifying the joint states after a move to one of the testing configurations --- ...ientation_constrained_planner_benchmark.py | 372 ++++++++++++------ 1 file changed, 245 insertions(+), 127 deletions(-) diff --git a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py index 3067ca38..272fd6ef 100755 --- a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py +++ b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py @@ -6,7 +6,8 @@ This script is used to benchmark planner performance for reaching pre-defined joint configurations while enforcing a path-wide orientation constraint on the end-effector. It now also saves successful trajectories -and tests planning between all pairs of defined configurations. +and tests planning between all pairs of defined configurations by physically +moving to start states. """ # Standard imports @@ -81,10 +82,8 @@ def fallback_get_path_len( return None, None if len(trajectory.points) < 2: return 0.0, {name: 0.0 for name in trajectory.joint_names} - total_len_l2_no_j6 = 0.0 joint_lens = {name: 0.0 for name in trajectory.joint_names} - j6_idx = -1 if exclude_j6_name and exclude_j6_name in trajectory.joint_names: try: @@ -94,7 +93,6 @@ def fallback_get_path_len( f"Joint {exclude_j6_name} for exclusion not found in trajectory joint_names." ) j6_idx = -1 - prev_positions = np.array(trajectory.points[0].positions) for point_idx in range(1, len(trajectory.points)): point = trajectory.points[point_idx] @@ -105,27 +103,21 @@ def fallback_get_path_len( ) prev_positions = curr_positions continue - diff = np.abs(curr_positions - prev_positions) diff = np.minimum(diff, 2 * np.pi - diff) - current_segment_geom_dist_sq_no_j6 = 0.0 for i in range(len(diff)): joint_lens[trajectory.joint_names[i]] += diff[i] if i != j6_idx: current_segment_geom_dist_sq_no_j6 += diff[i] ** 2 - total_len_l2_no_j6 += np.sqrt(current_segment_geom_dist_sq_no_j6) prev_positions = curr_positions return total_len_l2_no_j6, joint_lens GET_PATH_LEN_METHOD = fallback_get_path_len -# Represents a single named joint configuration BenchmarkNamedConfig = namedtuple("BenchmarkNamedConfig", ["name", "joint_values"]) -# Represents a planning problem from a start to a goal configuration PlanningTask = namedtuple("PlanningTask", ["start_config", "goal_config"]) - PlanResult = namedtuple( "PlanResult", [ @@ -163,6 +155,8 @@ def __init__( joint_names_for_group: List[str], planning_timeout_sec: float = 10.0, trajectory_save_dir: Optional[str] = None, + joint_state_topic: str = "/joint_states", + continuous_joint_indices: Optional[List[int]] = None, ): self.node = node self.logger = self.node.get_logger() @@ -174,26 +168,42 @@ def __init__( self.base_link = base_link self.joint_names_for_group = joint_names_for_group self.planning_timeout_sec = planning_timeout_sec + self.joint_state_topic_name = joint_state_topic + + if continuous_joint_indices is None: + self.continuous_joint_indices = [] + default_continuous = [ + "j2n6s200_joint_1", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ] + for i, name in enumerate(self.joint_names_for_group): + if name in default_continuous: + self.continuous_joint_indices.append(i) + self.logger.info( + f"Auto-detected continuous joint indices (0-based for group): {self.continuous_joint_indices} for joints named {default_continuous} within {self.joint_names_for_group}" + ) + else: + self.continuous_joint_indices = continuous_joint_indices + self.logger.info( + f"Using provided continuous joint indices: {self.continuous_joint_indices}" + ) - # Process the hardcoded configurations into a list of BenchmarkNamedConfig self.all_named_configurations = self._process_hardcoded_configurations( hardcoded_target_configs ) - - # Generate all planning tasks (pairs of start_config, goal_config) self.planning_tasks = self._generate_planning_tasks( self.all_named_configurations ) self.num_tasks = len(self.planning_tasks) - # Results stored as: self.results[(start_name, goal_name)][planner_id] self.results: Dict[Tuple[str, str], Dict[str, PlanResult]] = defaultdict( lambda: defaultdict(dict) ) - self.rate = self.node.create_rate(10) # For plan_async timeout loop + self.rate = self.node.create_rate(10) self.no_roll_path_constraint_kwargs = self._get_no_roll_constraint_kwargs() - self.trajectory_save_dir = trajectory_save_dir if self.trajectory_save_dir: os.makedirs(self.trajectory_save_dir, exist_ok=True) @@ -201,6 +211,16 @@ def __init__( f"Successful trajectories will be saved in: {self.trajectory_save_dir}" ) + self._latest_joint_state_msg: Optional[JointState] = None + self._joint_state_lock = Lock() + self.joint_state_sub = self.node.create_subscription( + JointState, self.joint_state_topic_name, self._joint_state_callback, 10 + ) + self.logger.info( + f"Subscribed to '{self.joint_state_topic_name}' for current joint state information." + ) + time.sleep(0.5) + self.logger.info( f"Benchmark initialized for group '{self.planning_group}' and EE '{self.end_effector_link}'." ) @@ -212,6 +232,71 @@ def __init__( f"Using 'no roll' constraint: Target Quat (xyzw)={self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ}, Tol(xyz_abs)={self.NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS}" ) + def _joint_state_callback(self, msg: JointState): + # Check if the message contains all the joints relevant to the planning group + # This helps filter out partial messages (e.g., only 'robot_tilt') + if not self.joint_names_for_group: # Should not happen if initialized correctly + return + + # Create a set of names in the message for efficient lookup + msg_joint_names_set = set(msg.name) + + # Check if all required joint names are present in the message + all_required_joints_present = True + for req_joint_name in self.joint_names_for_group: + if req_joint_name not in msg_joint_names_set: + all_required_joints_present = False + break + + if all_required_joints_present: + with self._joint_state_lock: + self._latest_joint_state_msg = msg + + def get_current_joint_positions( + self, timeout_sec: float = 1.0 + ) -> Optional[Dict[str, float]]: + """ + Gets the current joint positions for the planning group by reading the latest JointState message. + Returns a dictionary mapping joint name to position, or None if timeout or error. + """ + start_time = self.node.get_clock().now() + latest_msg_to_process: Optional[JointState] = None + + while rclpy.ok() and (self.node.get_clock().now() - start_time) < Duration( + seconds=timeout_sec + ): + with self._joint_state_lock: + if self._latest_joint_state_msg is not None: + latest_msg_to_process = self._latest_joint_state_msg + break + self.node.get_clock().sleep_for(Duration(seconds=0.02)) + + if latest_msg_to_process: + current_positions_dict: Dict[str, float] = {} + name_to_pos_map = { + name: latest_msg_to_process.position[i] + for i, name in enumerate(latest_msg_to_process.name) + if i < len(latest_msg_to_process.position) + } + + all_planning_group_joints_found = True + for req_name in self.joint_names_for_group: + if req_name in name_to_pos_map: + current_positions_dict[req_name] = name_to_pos_map[req_name] + else: + all_planning_group_joints_found = False + break + + if all_planning_group_joints_found: + return current_positions_dict + else: + return None + else: + self.logger.warn( + f"Timed out or no relevant JointState message received on '{self.joint_state_topic_name}' within {timeout_sec}s for get_current_joint_positions." + ) + return None + def _process_hardcoded_configurations( self, hardcoded_configs: Dict[str, List[float]] ) -> List[BenchmarkNamedConfig]: @@ -228,7 +313,6 @@ def _process_hardcoded_configurations( self.logger.warn( f" Skipping hardcoded config '{name}'. Expected {num_expected_joints} joint values, got {len(values) if isinstance(values, list) else type(values)}." ) - if not configs or len(configs) < 2: self.logger.error( f"Not enough valid named configurations provided (found {len(configs)}, need at least 2 for pair-wise tasks). Exiting." @@ -298,11 +382,12 @@ def _calculate_max_roll_deviation( "/" ): self.logger.warn( - f"FK pose for {self.end_effector_link} is in frame '{pose_stamped_ee.header.frame_id}', " - f"expected '{self.base_link}'. Ensure consistency or implement TF." + f"FK pose for {self.end_effector_link} is in frame '{pose_stamped_ee.header.frame_id}', expected '{self.base_link}'." ) - q = pose_stamped_ee.pose.orientation - actual_ee_orientation_scipy = R.from_quat([q.x, q.y, q.z, q.w]) + q_msg = pose_stamped_ee.pose.orientation + actual_ee_orientation_scipy = R.from_quat( + [q_msg.x, q_msg.y, q_msg.z, q_msg.w] + ) target_quat_params = self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ target_ee_orientation_scipy = R.from_quat(target_quat_params) diff_rotation = ( @@ -321,7 +406,7 @@ def _calculate_max_roll_deviation( ) except Exception as e: self.logger.error( - f"Error during FK for roll deviation at point {point_idx}: {e}", + f"Error during FK for roll deviation at point {point_idx}: {e}" ) return float("inf") return max_abs_roll_deviation @@ -361,7 +446,7 @@ def _save_trajectory_to_file( return filename except Exception as e: self.logger.error( - f" Failed to save trajectory for {start_config_name} to {goal_config_name} ({planner_id_str}): {e}", + f" Failed to save trajectory for {start_config_name} to {goal_config_name} ({planner_id_str}): {e}" ) return None @@ -375,13 +460,13 @@ def plan_to_target_configuration( f" Attempting to plan with: {planner_id_str} to GOAL: {goal_joints}" ) if planner_id_str.lower() == "chomp": - self.moveit2_interface.planning_pipeline_id = "chomp" + self.moveit2_interface.pipeline_id = "chomp" self.moveit2_interface.planner_id = "chomp" elif planner_id_str.lower() == "stomp": - self.moveit2_interface.planning_pipeline_id = "stomp" + self.moveit2_interface.pipeline_id = "stomp" self.moveit2_interface.planner_id = "" else: - self.moveit2_interface.planning_pipeline_id = "ompl" + self.moveit2_interface.pipeline_id = "ompl" self.moveit2_interface.planner_id = planner_id_str self.moveit2_interface.allowed_planning_time = self.planning_timeout_sec self.moveit2_interface.clear_goal_constraints() @@ -408,9 +493,7 @@ def plan_to_target_configuration( except Exception as e: self.logger.error(f" Failed to set path orientation constraint: {e}") start_time_ros = self.node.get_clock().now() - future = self.moveit2_interface.plan_async( - start_joint_state=None - ) # None means plan from current + future = self.moveit2_interface.plan_async(start_joint_state=None) joint_trajectory_for_analysis: Optional[JointTrajectory] = None timeout_duration_rclpy = Duration(seconds=self.planning_timeout_sec + 2.0) wait_start_time_rclpy = self.node.get_clock().now() @@ -462,26 +545,39 @@ def plan_to_target_configuration( self.moveit2_interface.clear_goal_constraints() return joint_trajectory_for_analysis, elapsed_time + def _normalize_angle(self, angle: float) -> float: + """Normalize an angle to the range [-pi, pi].""" + return (angle + np.pi) % (2 * np.pi) - np.pi + + def _are_angles_close( + self, angle1: float, angle2: float, tolerance: float, is_continuous: bool + ) -> bool: + """Checks if two angles are close, handling wrap-around for continuous joints.""" + if is_continuous: + diff = self._normalize_angle(angle1 - angle2) + return abs(diff) <= tolerance + else: + return abs(angle1 - angle2) <= tolerance + def _move_to_config_blocking( self, target_config_name: str, target_joints: List[float], - move_timeout_sec: float = 20.0, + verification_timeout_sec: float = 10.0, + verification_poll_interval_sec: float = 1.0, + verification_tolerance: float = 0.05, ): - """Moves the robot to a specified joint configuration and waits.""" self.logger.info( - f"Attempting to move to configuration: '{target_config_name}' {target_joints}" + f"Attempting to move to configuration: '{target_config_name}' target values: {np.round(target_joints, 4).tolist()}" ) - original_planner = self.moveit2_interface.planner_id original_pipeline = self.moveit2_interface.pipeline_id original_timeout = self.moveit2_interface.allowed_planning_time - # Use a reliable planner for setup moves - self.moveit2_interface.planning_pipeline_id = "ompl" + self.moveit2_interface.pipeline_id = "ompl" self.moveit2_interface.planner_id = "RRTConnectkConfigDefault" self.moveit2_interface.allowed_planning_time = max( - 5.0, self.planning_timeout_sec / 2.0 + 10.0, self.planning_timeout_sec ) self.moveit2_interface.clear_goal_constraints() @@ -489,47 +585,112 @@ def _move_to_config_blocking( try: self.logger.info( - f" Commanding move to '{target_config_name}' for group '{self.planning_group}'" + f" Commanding move to '{target_config_name}' for group '{self.planning_group}'." ) - # PyMoveIt2's move_to_configuration blocks until motion is complete or failed self.moveit2_interface.move_to_configuration( joint_positions=target_joints, joint_names=self.joint_names_for_group, tolerance=0.01, ) - # wait_until_executed might be part of move_to_configuration or needs to be called if plan_exec is used. - # For move_to_configuration, it typically blocks. - # We can add a small sleep or check current joint state if needed for more robustness. - time.sleep(1.0) - - # Verification - current_joints = self.moveit2_interface.get_joint_positions() - if current_joints: # TODO: Replace with actual current joint state getter - if not np.allclose(current_joints, target_joints, atol=0.05): - self.logger.warn( - f" Post-move verification: Current joints {current_joints} not close enough to target {target_joints} for '{target_config_name}'." + self.logger.info( + f" Move_to_configuration for '{target_config_name}' call returned. Now verifying final settled state (timeout: {verification_timeout_sec}s)." + ) + + verification_loop_start_time = self.node.get_clock().now() + achieved_target = False + last_known_joints_list_ordered_str = "N/A" + + for attempt in range( + int(verification_timeout_sec / verification_poll_interval_sec) + 2 + ): + current_joint_positions_dict = self.get_current_joint_positions( + timeout_sec=0.5 + ) + + if current_joint_positions_dict: + current_joints_list_for_comparison = [] + all_names_found_in_current = True + for name in self.joint_names_for_group: + if name in current_joint_positions_dict: + current_joints_list_for_comparison.append( + current_joint_positions_dict[name] + ) + else: + all_names_found_in_current = False + break + + last_known_joints_list_ordered_str = ( + str(np.round(current_joints_list_for_comparison, 4).tolist()) + if all_names_found_in_current + else "Partial state" ) - else: - self.logger.info( - f" Successfully moved to configuration: '{target_config_name}'." + + if all_names_found_in_current: + # Perform comparison joint by joint, handling continuous joints + all_joints_within_tolerance = True + for j_idx, (target_val, actual_val) in enumerate( + zip(target_joints, current_joints_list_for_comparison) + ): + is_continuous = j_idx in self.continuous_joint_indices + if not self._are_angles_close( + target_val, + actual_val, + verification_tolerance, + is_continuous, + ): + all_joints_within_tolerance = False + self.logger.info( + f" Verification attempt {attempt + 1} for '{target_config_name}': Joint '{self.joint_names_for_group[j_idx]}' (idx {j_idx}, cont: {is_continuous}) out of tolerance. Target: {target_val:.4f}, Actual: {actual_val:.4f}, Diff: {self._normalize_angle(target_val - actual_val):.4f}" + ) + break # No need to check other joints if one fails + + if all_joints_within_tolerance: + self.logger.info( + f" Successfully verified robot reached configuration: '{target_config_name}' on attempt {attempt + 1}." + ) + self.logger.info( + f" Final Target: {np.round(target_joints, 4).tolist()}" + ) + self.logger.info( + f" Final Actual: {np.round(current_joints_list_for_comparison, 4).tolist()}" + ) + achieved_target = True + break + + if not rclpy.ok() or ( + self.node.get_clock().now() - verification_loop_start_time + ) >= Duration(seconds=verification_timeout_sec): + if not achieved_target: + self.logger.warn( + f" Verification timeout for '{target_config_name}' after {verification_timeout_sec}s." + ) + break + + if not achieved_target: + self.node.get_clock().sleep_for( + Duration(seconds=verification_poll_interval_sec) ) - else: - self.logger.info( - f" Move command to '{target_config_name}' sent. Assuming success without immediate verification." + + if not achieved_target: + self.logger.error( + f" Failed to verify robot reached '{target_config_name}' (target: {np.round(target_joints, 4).tolist()}) " + f"within {verification_timeout_sec}s verification timeout. " + f"Last known state from /joint_states: {last_known_joints_list_ordered_str}" + ) + raise RuntimeError( + f"Failed to reach and verify start configuration '{target_config_name}'." ) except Exception as e: self.logger.error( - f"Failed to move to configuration '{target_config_name}': {e}" + f"Failed during _move_to_config_blocking for '{target_config_name}': {type(e).__name__} - {e}" ) - # This is a critical failure for the benchmark structure if a start state can't be reached. - # Depending on desired behavior, could raise, or just log and subsequent plans might fail from wrong start. raise RuntimeError( f"Critical failure: Could not move to start configuration '{target_config_name}'." ) from e finally: self.moveit2_interface.planner_id = original_planner - self.moveit2_interface.planning_pipeline_id = original_pipeline + self.moveit2_interface.pipeline_id = original_pipeline self.moveit2_interface.allowed_planning_time = original_timeout def move_to_initial_config(self): @@ -539,32 +700,25 @@ def run_benchmark_planning_task(self, task: PlanningTask): start_name = task.start_config.name goal_name = task.goal_config.name goal_joints = task.goal_config.joint_values - self.logger.info(f"--- Testing Task: FROM '{start_name}' TO '{goal_name}' ---") self.logger.info(f" Goal Joints: {goal_joints}") - task_key = (start_name, goal_name) - for planner_id_str in self.planners_to_test: - # Current state is assumed to be task.start_config.joint_values due to prior _move_to_config_blocking joint_trajectory, elapsed_time = self.plan_to_target_configuration( goal_joints=goal_joints, planner_id_str=planner_id_str, path_constraints_kwargs_to_apply=self.no_roll_path_constraint_kwargs, ) - success = joint_trajectory is not None and bool(joint_trajectory.points) path_len_total, joint_path_lens_map = self._get_path_length_stats( joint_trajectory ) max_roll_dev = self._calculate_max_roll_deviation(joint_trajectory) - trajectory_filename = None if success and self.trajectory_save_dir: trajectory_filename = self._save_trajectory_to_file( joint_trajectory, start_name, goal_name, planner_id_str ) - self.results[task_key][planner_id_str] = PlanResult( joint_trajectory, elapsed_time, @@ -574,15 +728,12 @@ def run_benchmark_planning_task(self, task: PlanningTask): success, trajectory_filename, ) - log_msg_suffix = ( f"| TrajFile: {trajectory_filename}" if trajectory_filename else "" ) if success: self.logger.info( - f" Planner: {planner_id_str} | Success: True | Time: {elapsed_time:.3f}s " - f"| PathLen: {path_len_total if path_len_total is not None else 'N/A':.3f} " - f"| MaxRollDev: {max_roll_dev if max_roll_dev is not None else 'N/A':.3f} rad {log_msg_suffix}" + f" Planner: {planner_id_str} | Success: True | Time: {elapsed_time:.3f}s | PathLen: {path_len_total if path_len_total is not None else 'N/A':.3f} | MaxRollDev: {max_roll_dev if max_roll_dev is not None else 'N/A':.3f} rad {log_msg_suffix}" ) else: self.logger.warn( @@ -593,15 +744,20 @@ def run_all_tests(self, log_summary_every_n_tasks: Optional[int] = None): self.logger.info( "=== Starting Benchmark: Moving to Initial Script Configuration ===" ) - self.move_to_initial_config() + try: + self.move_to_initial_config() + except RuntimeError as e: + self.logger.error( + f"CRITICAL FAILURE: Could not move to initial script setup configuration. Benchmark aborted. Error: {e}" + ) + # Record this major failure if needed, though the script will likely exit or not proceed. + return # Abort benchmark if initial setup fails for i, planning_task in enumerate(self.planning_tasks): self.logger.info( f"\n=== Running Planning Task {i + 1}/{self.num_tasks}: " f"FROM '{planning_task.start_config.name}' TO '{planning_task.goal_config.name}' ===" ) - - # Move to the start configuration for this specific task try: self._move_to_config_blocking( planning_task.start_config.name, @@ -611,7 +767,6 @@ def run_all_tests(self, log_summary_every_n_tasks: Optional[int] = None): self.logger.error( f"Skipping task FROM '{planning_task.start_config.name}' TO '{planning_task.goal_config.name}' due to failure to reach start state: {e}" ) - # Optionally, record this failure in results for this task pair task_key = ( planning_task.start_config.name, planning_task.goal_config.name, @@ -628,12 +783,10 @@ def run_all_tests(self, log_summary_every_n_tasks: Optional[int] = None): ) continue - # Now run the actual benchmarked planning for this task self.run_benchmark_planning_task(planning_task) if log_summary_every_n_tasks and (i + 1) % log_summary_every_n_tasks == 0: self.log_summary_results() - self.logger.info("=== Benchmark Run Completed ===") def get_csv_header(self) -> List[str]: @@ -656,21 +809,14 @@ def get_csv_header(self) -> List[str]: def write_results_to_csv(self, filename: str): self.logger.info(f"Writing results to {filename}") - with open(filename, "w", newline="") as f: import csv csv_writer = csv.writer(f) header = self.get_csv_header() csv_writer.writerow(header) - - # Iterate through the planning tasks to maintain order if desired, - # or directly through self.results.keys() for task_key in sorted(self.results.keys()): start_name, goal_name = task_key - - # Find the original BenchmarkNamedConfig objects for start and goal - # This is a bit inefficient but ensures data integrity if names are unique start_cfg_obj = next( (c for c in self.all_named_configurations if c.name == start_name), None, @@ -679,13 +825,11 @@ def write_results_to_csv(self, filename: str): (c for c in self.all_named_configurations if c.name == goal_name), None, ) - if not start_cfg_obj or not goal_cfg_obj: self.logger.warn( f"Could not find original config objects for task key {task_key}. Skipping CSV rows for this task." ) continue - if task_key in self.results: planner_runs_for_task = self.results[task_key] for planner_id_str in self.planners_to_test: @@ -696,7 +840,6 @@ def write_results_to_csv(self, filename: str): row.extend(map(str, start_cfg_obj.joint_values)) row.append(str(goal_cfg_obj.name)) row.extend(map(str, goal_cfg_obj.joint_values)) - row.append(str(planner_id_str)) row.append(f"{result.elapsed_time:.4f}") row.append(str(1 if result.success else 0)) @@ -715,7 +858,6 @@ def write_results_to_csv(self, filename: str): if result.trajectory_filename else "" ) - if result.joint_path_lengths: for joint_name_csv in self.joint_names_for_group: length_val = result.joint_path_lengths.get( @@ -733,22 +875,20 @@ def write_results_to_csv(self, filename: str): self.logger.warn( f"No result for planner '{planner_id_str}' in task '{start_name}' to '{goal_name}'. Skipping CSV row." ) - self.logger.info(f"Results successfully written to {filename}") def log_summary_results(self): self.logger.info("--- BENCHMARK SUMMARY (ALL TASKS) ---") for planner_id_str in self.planners_to_test: - # Collect all results for this planner across all (start, goal) pairs results_for_planner = [] for task_results_dict in self.results.values(): if planner_id_str in task_results_dict: results_for_planner.append(task_results_dict[planner_id_str]) - total_plans = len(results_for_planner) if total_plans == 0: - self.logger.info(f"--- Planner: {planner_id_str} ---") - self.logger.info(" No plans attempted or recorded for this planner.") + self.logger.info( + f"--- Planner: {planner_id_str} ---\n No plans attempted or recorded for this planner." + ) continue successful_plans = sum(1 for r in results_for_planner if r.success) total_planning_time = sum(r.elapsed_time for r in results_for_planner) @@ -815,16 +955,12 @@ def _summary_stats_str(data: List[float]) -> str: valid_data = [x for x in data if isinstance(x, (int, float)) and np.isfinite(x)] if not valid_data: return "N/A (no valid data points)" - return ( - f"Mean {np.mean(valid_data):.3f}, Median {np.median(valid_data):.3f}, " - f"Min {np.min(valid_data):.3f}, Max {np.max(valid_data):.3f}, Std {np.std(valid_data):.3f}, " - f"Count {len(valid_data)}" - ) + return f"Mean {np.mean(valid_data):.3f}, Median {np.median(valid_data):.3f}, Min {np.min(valid_data):.3f}, Max {np.max(valid_data):.3f}, Std {np.std(valid_data):.3f}, Count {len(valid_data)}" def main(output_dir_arg: Optional[str]): rclpy.init() - node = Node("planner_benchmark_pairwise_constrained") + node = Node("planner_benchmark_pairwise_physical_moves") global _LOGGER_INSTANCE _LOGGER_INSTANCE = node.get_logger() executor = rclpy.executors.MultiThreadedExecutor(2) @@ -836,7 +972,6 @@ def main(output_dir_arg: Optional[str]): ) time.sleep(5.0) - # --- Hardcoded Named Configurations (used as pool for start/goal states) --- hardcoded_configs = { "MoveAbovePlate": [ -2.4538579336877304, @@ -847,34 +982,19 @@ def main(output_dir_arg: Optional[str]): -3.2123560395465063, ], "RestingAcquireFood": [-1.94672, 2.51268, 0.35653, -4.76501, 5.99991, 4.99555], - "StagingConfig": [ - -2.32526, - 4.456298, - 4.16769, - 1.53262, - -2.18359, - -2.19525, - ], - "StowLocation": [ - -1.52101, - 2.60098, - 0.32811, - -4.00012, - 0.22831, - 3.87886, - ], + "StagingConfig": [-2.32526, 4.456298, 4.16769, 1.53262, -2.18359, -2.19525], + "StowLocation": [-1.52101, 2.60098, 0.32811, -4.00012, 0.22831, 3.87886], } _LOGGER_INSTANCE.info( f"Using {len(hardcoded_configs)} hardcoded named configurations for generating planning tasks." ) planners = ["RRTConnectkConfigDefault", "RRTstarkConfigDefault", "CHOMP"] - - # This initial_config is for the VERY FIRST robot position before any tasks begin. - # It could be one of the hardcoded_configs or something else. - # For simplicity, let's make it the first one from hardcoded_configs if available. - # Or use the one previously defined. - initial_script_setup_config = list(hardcoded_configs.values())[0] + initial_script_setup_config = ( + list(hardcoded_configs.values())[0] + if hardcoded_configs + else [0.0] * len(kinova.joint_names()) + ) planning_group_name = "jaco_arm" ee_link_name = "j2n6s200_end_effector" @@ -887,7 +1007,7 @@ def main(output_dir_arg: Optional[str]): os.makedirs(base_output_dir) _LOGGER_INSTANCE.info(f"Created base output directory: {base_output_dir}") trajectory_save_location = os.path.join( - base_output_dir, "saved_trajectories_pairwise" + base_output_dir, "saved_trajectories_pairwise_physical" ) callback_group = ReentrantCallbackGroup() @@ -918,13 +1038,13 @@ def main(output_dir_arg: Optional[str]): joint_names_for_group=group_joint_names, planning_timeout_sec=planning_timeout, trajectory_save_dir=trajectory_save_location, + joint_state_topic="/joint_states", ) try: num_total_tasks = benchmark_runner.num_tasks - log_interval = max(1, num_total_tasks // 5) + log_interval = max(1, num_total_tasks // 5 if num_total_tasks > 0 else 1) benchmark_runner.run_all_tests(log_summary_every_n_tasks=log_interval) - if not os.path.exists(base_output_dir) and base_output_dir != ".": os.makedirs(base_output_dir) _LOGGER_INSTANCE.info( @@ -932,15 +1052,13 @@ def main(output_dir_arg: Optional[str]): ) elif base_output_dir == "." and not os.path.exists(base_output_dir): os.makedirs(base_output_dir) - csv_filename = os.path.join( base_output_dir, datetime.now().strftime("%Y-%m-%d_%H-%M-%S") - + "_pairwise_constrained_benchmark.csv", + + "_pairwise_physical_benchmark.csv", ) benchmark_runner.write_results_to_csv(csv_filename) benchmark_runner.log_summary_results() - except Exception as e: _LOGGER_INSTANCE.error(f"Benchmark run failed: {e}") finally: From 88c8dae017a48c862bd6a9f0ba0a68643e1b5b76 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 11 May 2025 15:36:42 -0700 Subject: [PATCH 062/238] Enable multiple planning attempts for unconstrained motions to nominal testing configurations --- ...ientation_constrained_planner_benchmark.py | 208 ++++++++++++------ 1 file changed, 142 insertions(+), 66 deletions(-) diff --git a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py index 272fd6ef..1958f1ee 100755 --- a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py +++ b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py @@ -566,6 +566,7 @@ def _move_to_config_blocking( verification_timeout_sec: float = 10.0, verification_poll_interval_sec: float = 1.0, verification_tolerance: float = 0.05, + max_move_attempts: int = 3, ): self.logger.info( f"Attempting to move to configuration: '{target_config_name}' target values: {np.round(target_joints, 4).tolist()}" @@ -574,59 +575,87 @@ def _move_to_config_blocking( original_pipeline = self.moveit2_interface.pipeline_id original_timeout = self.moveit2_interface.allowed_planning_time + # Use a reliable planner for unconstrained moves self.moveit2_interface.pipeline_id = "ompl" - self.moveit2_interface.planner_id = "RRTConnectkConfigDefault" + # Consider making this configurable if needed, or even a list of planners to try + unconstrained_move_planner_id = "RRTConnectkConfigDefault" + self.moveit2_interface.planner_id = unconstrained_move_planner_id + # Ensure a reasonable timeout for these critical setup moves self.moveit2_interface.allowed_planning_time = max( 10.0, self.planning_timeout_sec ) - self.moveit2_interface.clear_goal_constraints() - self.moveit2_interface.clear_path_constraints() + achieved_target = False + last_known_joints_list_ordered_str = "N/A" + last_exception_detail = "No attempts made or error prior to first attempt." - try: + for attempt in range(max_move_attempts): self.logger.info( - f" Commanding move to '{target_config_name}' for group '{self.planning_group}'." - ) - self.moveit2_interface.move_to_configuration( - joint_positions=target_joints, - joint_names=self.joint_names_for_group, - tolerance=0.01, - ) - self.logger.info( - f" Move_to_configuration for '{target_config_name}' call returned. Now verifying final settled state (timeout: {verification_timeout_sec}s)." + f"Move attempt {attempt + 1}/{max_move_attempts} to '{target_config_name}' using planner '{unconstrained_move_planner_id}'." ) + try: + self.moveit2_interface.clear_goal_constraints() + self.moveit2_interface.clear_path_constraints() + + # Command the move + self.logger.info( + f" Commanding move_to_configuration for '{target_config_name}' (attempt {attempt + 1})." + ) + # move_to_configuration can raise an exception if planning fails or execution is controller-aborted. + # It is a blocking call. + self.moveit2_interface.move_to_configuration( + joint_positions=target_joints, + joint_names=self.joint_names_for_group, + tolerance=0.01, + ) + self.logger.info( + f" Move_to_configuration call for '{target_config_name}' (attempt {attempt + 1}) completed by MoveIt2. " + f"Now verifying final settled state (timeout: {verification_timeout_sec}s)." + ) - verification_loop_start_time = self.node.get_clock().now() - achieved_target = False - last_known_joints_list_ordered_str = "N/A" + # Verification loop + verification_loop_start_time = self.node.get_clock().now() + achieved_target_this_attempt = False - for attempt in range( - int(verification_timeout_sec / verification_poll_interval_sec) + 2 - ): - current_joint_positions_dict = self.get_current_joint_positions( - timeout_sec=0.5 + num_verification_checks = max( + 2, + int(verification_timeout_sec / verification_poll_interval_sec) + 1, ) - if current_joint_positions_dict: - current_joints_list_for_comparison = [] - all_names_found_in_current = True - for name in self.joint_names_for_group: - if name in current_joint_positions_dict: - current_joints_list_for_comparison.append( - current_joint_positions_dict[name] + for verify_idx in range(num_verification_checks): + current_joint_positions_dict = self.get_current_joint_positions( + timeout_sec=0.5 + ) + + if current_joint_positions_dict: + current_joints_list_for_comparison = [] + all_names_found_in_current = True + for name in self.joint_names_for_group: + if name in current_joint_positions_dict: + current_joints_list_for_comparison.append( + current_joint_positions_dict[name] + ) + else: + all_names_found_in_current = False + self.logger.warn( + f" Verification (attempt {attempt + 1}, check {verify_idx + 1}): Missing joint '{name}' in current joint state." + ) + break + + if not all_names_found_in_current: + last_known_joints_list_ordered_str = ( + "Partial joint state received" ) - else: - all_names_found_in_current = False - break + # Give a small pause and try next verification check + self.node.get_clock().sleep_for( + Duration(seconds=verification_poll_interval_sec) + ) + continue - last_known_joints_list_ordered_str = ( - str(np.round(current_joints_list_for_comparison, 4).tolist()) - if all_names_found_in_current - else "Partial state" - ) + last_known_joints_list_ordered_str = str( + np.round(current_joints_list_for_comparison, 4).tolist() + ) - if all_names_found_in_current: - # Perform comparison joint by joint, handling continuous joints all_joints_within_tolerance = True for j_idx, (target_val, actual_val) in enumerate( zip(target_joints, current_joints_list_for_comparison) @@ -639,14 +668,20 @@ def _move_to_config_blocking( is_continuous, ): all_joints_within_tolerance = False - self.logger.info( - f" Verification attempt {attempt + 1} for '{target_config_name}': Joint '{self.joint_names_for_group[j_idx]}' (idx {j_idx}, cont: {is_continuous}) out of tolerance. Target: {target_val:.4f}, Actual: {actual_val:.4f}, Diff: {self._normalize_angle(target_val - actual_val):.4f}" - ) - break # No need to check other joints if one fails + # Log less frequently during verification to avoid spam, but ensure important info is logged + if ( + verify_idx % 5 == 0 + or verify_idx == num_verification_checks - 1 + or verification_timeout_sec < 2.0 + ): + self.logger.info( + f" Verification (attempt {attempt + 1}, check {verify_idx + 1}) for '{target_config_name}': Joint '{self.joint_names_for_group[j_idx]}' (idx {j_idx}, cont: {is_continuous}) out of tolerance. Target: {target_val:.4f}, Actual: {actual_val:.4f}, NormDiff: {self._normalize_angle(target_val - actual_val):.4f}, RawDiff: {(target_val - actual_val):.4f}" + ) + break if all_joints_within_tolerance: self.logger.info( - f" Successfully verified robot reached configuration: '{target_config_name}' on attempt {attempt + 1}." + f" Successfully verified robot reached configuration: '{target_config_name}' on move attempt {attempt + 1}, verification check {verify_idx + 1}." ) self.logger.info( f" Final Target: {np.round(target_joints, 4).tolist()}" @@ -655,43 +690,84 @@ def _move_to_config_blocking( f" Final Actual: {np.round(current_joints_list_for_comparison, 4).tolist()}" ) achieved_target = True + achieved_target_this_attempt = True break - - if not rclpy.ok() or ( - self.node.get_clock().now() - verification_loop_start_time - ) >= Duration(seconds=verification_timeout_sec): - if not achieved_target: + else: + last_known_joints_list_ordered_str = ( + "No joint states received for this check." + ) self.logger.warn( - f" Verification timeout for '{target_config_name}' after {verification_timeout_sec}s." + f" Verification (attempt {attempt + 1}, check {verify_idx + 1}): No relevant JointState message received." ) + + # Check for overall verification timeout for this attempt + if not rclpy.ok() or ( + self.node.get_clock().now() - verification_loop_start_time + ) >= Duration(seconds=verification_timeout_sec): + if not achieved_target_this_attempt: + self.logger.warn( + f" Verification timeout for '{target_config_name}' on move attempt {attempt + 1} after {verification_timeout_sec}s of polling." + ) + break + + if not achieved_target_this_attempt: + self.node.get_clock().sleep_for( + Duration(seconds=verification_poll_interval_sec) + ) + + if achieved_target: break - if not achieved_target: - self.node.get_clock().sleep_for( - Duration(seconds=verification_poll_interval_sec) + # If verification failed for this attempt (either by joints out of tolerance or timeout) + if not achieved_target_this_attempt: + self.logger.warn( + f" Move attempt {attempt + 1} to '{target_config_name}' seemed to complete motion, but verification failed. " + f"Last known state during verification: {last_known_joints_list_ordered_str}" ) + last_exception_detail = f"Verification failed after move_to_configuration call. Last state: {last_known_joints_list_ordered_str}" - if not achieved_target: + except Exception as e: + self.logger.warn( + f" Move attempt {attempt + 1}/{max_move_attempts} to '{target_config_name}' failed directly during move_to_configuration. Error: {type(e).__name__} - {e}" + ) + last_exception_detail = ( + f"Error in move_to_configuration: {type(e).__name__} - {e}" + ) + + if achieved_target: + break + + # If this was the last attempt and still not achieved + if attempt == max_move_attempts - 1: self.logger.error( - f" Failed to verify robot reached '{target_config_name}' (target: {np.round(target_joints, 4).tolist()}) " - f"within {verification_timeout_sec}s verification timeout. " - f"Last known state from /joint_states: {last_known_joints_list_ordered_str}" + f"All {max_move_attempts} attempts to move to '{target_config_name}' have been exhausted and failed." ) - raise RuntimeError( - f"Failed to reach and verify start configuration '{target_config_name}'." + # The final error will be raised outside the loop if achieved_target is still False + elif not achieved_target: + self.logger.info( + f" Pausing briefly before retry for '{target_config_name}'." ) + self.node.get_clock().sleep_for(Duration(seconds=1.5)) - except Exception as e: + # Restore original MoveIt2 settings AFTER all attempts + self.moveit2_interface.planner_id = original_planner + self.moveit2_interface.pipeline_id = original_pipeline + self.moveit2_interface.allowed_planning_time = original_timeout + + if not achieved_target: self.logger.error( - f"Failed during _move_to_config_blocking for '{target_config_name}': {type(e).__name__} - {e}" + f"CRITICAL: Failed to move AND verify robot reached '{target_config_name}' (target: {np.round(target_joints, 4).tolist()}) " + f"after {max_move_attempts} attempts. " + f"Last known state from /joint_states (during last verification): {last_known_joints_list_ordered_str}. " + f"Detail of last failure: {last_exception_detail}" ) raise RuntimeError( - f"Critical failure: Could not move to start configuration '{target_config_name}'." - ) from e - finally: - self.moveit2_interface.planner_id = original_planner - self.moveit2_interface.pipeline_id = original_pipeline - self.moveit2_interface.allowed_planning_time = original_timeout + f"Failed to reach and verify start configuration '{target_config_name}' after {max_move_attempts} attempts. Last failure detail: {last_exception_detail}" + ) + # If loop finished and achieved_target is True, then the move was successful. + self.logger.info( + f"Successfully moved to and verified configuration '{target_config_name}'." + ) def move_to_initial_config(self): self._move_to_config_blocking("InitialScriptSetup", self.initial_joint_config) From 64c29553bf66024cd451568563895378e69d0eaf Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 13 May 2025 17:06:55 -0700 Subject: [PATCH 063/238] Add metrics for Articutool feasibility, and add a script to analyze the results of the benchmark --- ada_feeding/CMakeLists.txt | 1 + ...ientation_constrained_planner_benchmark.py | 1187 ++++++++++------- .../scripts/planner_benchmark_analysis.py | 388 ++++++ ada_feeding/scripts/visualize_trajectory.py | 391 +++--- 4 files changed, 1344 insertions(+), 623 deletions(-) create mode 100755 ada_feeding/scripts/planner_benchmark_analysis.py diff --git a/ada_feeding/CMakeLists.txt b/ada_feeding/CMakeLists.txt index 54233f23..d5ff44e9 100644 --- a/ada_feeding/CMakeLists.txt +++ b/ada_feeding/CMakeLists.txt @@ -37,6 +37,7 @@ install(PROGRAMS scripts/dummy_ft_sensor.py scripts/joint_state_latency.py scripts/planner_benchmark.py + scripts/planner_benchmark_analysis.py scripts/orientation_constrained_planner_benchmark.py scripts/visualize_trajectory.py scripts/robot_state_service.py diff --git a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py index 1958f1ee..8bc3ebf2 100755 --- a/ada_feeding/scripts/orientation_constrained_planner_benchmark.py +++ b/ada_feeding/scripts/orientation_constrained_planner_benchmark.py @@ -4,10 +4,12 @@ """ This script is used to benchmark planner performance for reaching -pre-defined joint configurations while enforcing a path-wide orientation -constraint on the end-effector. It now also saves successful trajectories -and tests planning between all pairs of defined configurations by physically -moving to start states. +pre-defined joint configurations. It has been updated to include: +- Kinematic feasibility checks for a 2-DOF Articutool attached to the end-effector. +- Saving of Articutool's per-waypoint IK solutions (pitch, roll) within the + trajectory file for visualization and detailed analysis. +- Detailed metrics related to Articutool performance and joint utilization. +- Physical movement to start states for pairwise planning tasks. """ # Standard imports @@ -19,7 +21,7 @@ from threading import Thread, Lock from typing import Optional, List, Dict, Tuple, Any import json -import itertools +import math # For atan2, asin, cos, sin, pi, isclose # Third-party imports import numpy as np @@ -28,9 +30,11 @@ import rclpy from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node -from rclpy.duration import Duration +from rclpy.duration import ( + Duration as RCLPYDuration, +) # Alias to avoid conflict with local Duration from trajectory_msgs.msg import JointTrajectoryPoint, JointTrajectory -from geometry_msgs.msg import Quaternion, PoseStamped +from geometry_msgs.msg import Quaternion, PoseStamped, Pose import moveit_msgs.msg from sensor_msgs.msg import JointState from scipy.spatial.transform import Rotation as R @@ -89,9 +93,6 @@ def fallback_get_path_len( try: j6_idx = trajectory.joint_names.index(exclude_j6_name) except ValueError: - _get_logger().warn( - f"Joint {exclude_j6_name} for exclusion not found in trajectory joint_names." - ) j6_idx = -1 prev_positions = np.array(trajectory.points[0].positions) for point_idx in range(1, len(trajectory.points)): @@ -99,7 +100,7 @@ def fallback_get_path_len( curr_positions = np.array(point.positions) if len(curr_positions) != len(prev_positions): _get_logger().warn( - f"Path length calc: Mismatch in joint count at point {point_idx}. Skipping." + f"Path length calc: Mismatch joint count at point {point_idx}. Skipping." ) prev_positions = curr_positions continue @@ -118,16 +119,44 @@ def fallback_get_path_len( BenchmarkNamedConfig = namedtuple("BenchmarkNamedConfig", ["name", "joint_values"]) PlanningTask = namedtuple("PlanningTask", ["start_config", "goal_config"]) + +ArticutoolWaypointSolution = namedtuple( + "ArticutoolWaypointSolution", + [ + "waypoint_feasible", # bool: Is Articutool feasible at this specific waypoint? + "pitch_solution_rad", # Optional[float]: Solved pitch angle (theta_p) + "roll_solution_rad", # Optional[float]: Solved roll angle (theta_r) + ], +) + +ArticutoolMetrics = namedtuple( + "ArticutoolMetrics", + [ + "path_feasible", + "min_pitch_rad", + "max_pitch_rad", + "avg_pitch_abs_rad", + "pitch_range_used_percent", + "min_roll_rad", + "max_roll_rad", + "avg_roll_abs_rad", + "roll_range_used_percent", + "num_infeasible_points", + ], +) + PlanResult = namedtuple( "PlanResult", [ - "trajectory", + "trajectory", # Original JointTrajectory from planner "elapsed_time", "path_length", "joint_path_lengths", - "max_roll_deviation", - "success", - "trajectory_filename", + "max_jaco_hand_roll_deviation", + "jaco_plan_success", + "trajectory_filename", # Filename of the *enhanced* JSON trajectory + "articutool_metrics", # Aggregate metrics for the Articutool over the path + "articutool_solutions_per_waypoint", # List[ArticutoolWaypointSolution] ], ) @@ -141,6 +170,11 @@ class PlannerBenchmark: ) NO_ROLL_CONSTRAINT_WEIGHT = 1.0 NO_ROLL_PARAMETERIZATION = 0 + R_JACO_HAND_TO_ATOOL_BASE_SCIPY = R.from_euler("z", np.pi / 2) + ARTICUTOOL_PITCH_LIMITS_RAD = (-np.pi / 2, np.pi / 2) + ARTICUTOOL_ROLL_LIMITS_RAD = (-np.pi, np.pi) + WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) + EPSILON = 1e-6 def __init__( self, @@ -157,9 +191,10 @@ def __init__( trajectory_save_dir: Optional[str] = None, joint_state_topic: str = "/joint_states", continuous_joint_indices: Optional[List[int]] = None, + use_naive_jaco_hand_constraint: bool = True, ): self.node = node - self.logger = self.node.get_logger() + self.logger = _get_logger() # Use the global logger self.moveit2_interface = moveit2_interface self.planners_to_test = planners_to_test self.initial_joint_config = initial_joint_config @@ -169,6 +204,7 @@ def __init__( self.joint_names_for_group = joint_names_for_group self.planning_timeout_sec = planning_timeout_sec self.joint_state_topic_name = joint_state_topic + self.use_naive_jaco_hand_constraint = use_naive_jaco_hand_constraint if continuous_joint_indices is None: self.continuous_joint_indices = [] @@ -182,7 +218,7 @@ def __init__( if name in default_continuous: self.continuous_joint_indices.append(i) self.logger.info( - f"Auto-detected continuous joint indices (0-based for group): {self.continuous_joint_indices} for joints named {default_continuous} within {self.joint_names_for_group}" + f"Auto-detected continuous joint indices: {self.continuous_joint_indices}" ) else: self.continuous_joint_indices = continuous_joint_indices @@ -197,13 +233,13 @@ def __init__( self.all_named_configurations ) self.num_tasks = len(self.planning_tasks) - self.results: Dict[Tuple[str, str], Dict[str, PlanResult]] = defaultdict( lambda: defaultdict(dict) ) self.rate = self.node.create_rate(10) - - self.no_roll_path_constraint_kwargs = self._get_no_roll_constraint_kwargs() + self.naive_jaco_hand_constraint_kwargs = ( + self._get_naive_jaco_hand_constraint_kwargs() + ) self.trajectory_save_dir = trajectory_save_dir if self.trajectory_save_dir: os.makedirs(self.trajectory_save_dir, exist_ok=True) @@ -217,10 +253,9 @@ def __init__( JointState, self.joint_state_topic_name, self._joint_state_callback, 10 ) self.logger.info( - f"Subscribed to '{self.joint_state_topic_name}' for current joint state information." + f"Subscribed to '{self.joint_state_topic_name}' for current joint state." ) time.sleep(0.5) - self.logger.info( f"Benchmark initialized for group '{self.planning_group}' and EE '{self.end_effector_link}'." ) @@ -228,26 +263,30 @@ def __init__( f"Generated {self.num_tasks} planning tasks from {len(self.all_named_configurations)} unique configurations." ) self.logger.info(f"Testing with planners: {self.planners_to_test}") + if self.use_naive_jaco_hand_constraint: + self.logger.info( + f"Using NAIVE Jaco hand constraint: Target Quat (xyzw)={self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ}, Tol(xyz_abs)={self.NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS}" + ) + else: + self.logger.info( + "NOT using naive Jaco hand orientation constraint during planning." + ) + self.logger.info( + f"Articutool Pitch Limits (theta_p): {self.ARTICUTOOL_PITCH_LIMITS_RAD} rad" + ) self.logger.info( - f"Using 'no roll' constraint: Target Quat (xyzw)={self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ}, Tol(xyz_abs)={self.NO_ROLL_CONSTRAINT_TOLERANCE_XYZ_ABS}" + f"Articutool Roll Limits (theta_r): {self.ARTICUTOOL_ROLL_LIMITS_RAD} rad" ) def _joint_state_callback(self, msg: JointState): - # Check if the message contains all the joints relevant to the planning group - # This helps filter out partial messages (e.g., only 'robot_tilt') - if not self.joint_names_for_group: # Should not happen if initialized correctly + # ... (implementation from previous version) ... + if not self.joint_names_for_group: return - - # Create a set of names in the message for efficient lookup msg_joint_names_set = set(msg.name) - - # Check if all required joint names are present in the message - all_required_joints_present = True - for req_joint_name in self.joint_names_for_group: - if req_joint_name not in msg_joint_names_set: - all_required_joints_present = False - break - + all_required_joints_present = all( + req_joint_name in msg_joint_names_set + for req_joint_name in self.joint_names_for_group + ) if all_required_joints_present: with self._joint_state_lock: self._latest_joint_state_msg = msg @@ -255,21 +294,17 @@ def _joint_state_callback(self, msg: JointState): def get_current_joint_positions( self, timeout_sec: float = 1.0 ) -> Optional[Dict[str, float]]: - """ - Gets the current joint positions for the planning group by reading the latest JointState message. - Returns a dictionary mapping joint name to position, or None if timeout or error. - """ + # ... (implementation from previous version) ... start_time = self.node.get_clock().now() latest_msg_to_process: Optional[JointState] = None - - while rclpy.ok() and (self.node.get_clock().now() - start_time) < Duration( + while rclpy.ok() and (self.node.get_clock().now() - start_time) < RCLPYDuration( seconds=timeout_sec ): with self._joint_state_lock: if self._latest_joint_state_msg is not None: latest_msg_to_process = self._latest_joint_state_msg break - self.node.get_clock().sleep_for(Duration(seconds=0.02)) + self.node.get_clock().sleep_for(RCLPYDuration(seconds=0.02)) if latest_msg_to_process: current_positions_dict: Dict[str, float] = {} @@ -278,7 +313,6 @@ def get_current_joint_positions( for i, name in enumerate(latest_msg_to_process.name) if i < len(latest_msg_to_process.position) } - all_planning_group_joints_found = True for req_name in self.joint_names_for_group: if req_name in name_to_pos_map: @@ -286,36 +320,30 @@ def get_current_joint_positions( else: all_planning_group_joints_found = False break - if all_planning_group_joints_found: return current_positions_dict - else: - return None - else: - self.logger.warn( - f"Timed out or no relevant JointState message received on '{self.joint_state_topic_name}' within {timeout_sec}s for get_current_joint_positions." - ) - return None + self.logger.warn( + f"Timed out or no relevant JointState msg on '{self.joint_state_topic_name}' for get_current_joint_positions." + ) + return None def _process_hardcoded_configurations( self, hardcoded_configs: Dict[str, List[float]] ) -> List[BenchmarkNamedConfig]: + # ... (implementation from previous version) ... configs = [] self.logger.info(f"Processing hardcoded named configurations...") num_expected_joints = len(self.joint_names_for_group) for name, values in hardcoded_configs.items(): if isinstance(values, list) and len(values) == num_expected_joints: configs.append(BenchmarkNamedConfig(name, values)) - self.logger.info( - f" Loaded named config '{name}' with {num_expected_joints} joints." - ) else: self.logger.warn( - f" Skipping hardcoded config '{name}'. Expected {num_expected_joints} joint values, got {len(values) if isinstance(values, list) else type(values)}." + f" Skipping hardcoded config '{name}'. Expected {num_expected_joints}, got {len(values) if isinstance(values, list) else type(values)}." ) if not configs or len(configs) < 2: self.logger.error( - f"Not enough valid named configurations provided (found {len(configs)}, need at least 2 for pair-wise tasks). Exiting." + f"Not enough valid named configurations provided (found {len(configs)}, need at least 2). Exiting." ) sys.exit(1) return configs @@ -323,6 +351,7 @@ def _process_hardcoded_configurations( def _generate_planning_tasks( self, named_configs: List[BenchmarkNamedConfig] ) -> List[PlanningTask]: + # ... (implementation from previous version) ... tasks = [] for start_config in named_configs: for goal_config in named_configs: @@ -333,7 +362,8 @@ def _generate_planning_tasks( ) return tasks - def _get_no_roll_constraint_kwargs(self) -> Dict[str, Any]: + def _get_naive_jaco_hand_constraint_kwargs(self) -> Dict[str, Any]: + # ... (implementation from previous version) ... return { "quat_xyzw": Quaternion( x=self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ[0], @@ -348,17 +378,192 @@ def _get_no_roll_constraint_kwargs(self) -> Dict[str, Any]: "parameterization": self.NO_ROLL_PARAMETERIZATION, } - def _calculate_max_roll_deviation( + def _normalize_angle(self, angle: float) -> float: + return (angle + np.pi) % (2 * np.pi) - np.pi + + def _solve_articutool_ik( + self, target_vector_in_jaco_hand_frame: np.ndarray + ) -> List[Tuple[float, float]]: + # ... (implementation from previous version, using math module) ... + vx, vy, vz = target_vector_in_jaco_hand_frame + solutions: List[Tuple[float, float]] = [] + if not (-1.0 - self.EPSILON <= -vx <= 1.0 + self.EPSILON): + return [] + asin_arg = np.clip(-vx, -1.0, 1.0) + theta_r_cand1 = math.asin(asin_arg) + theta_r_cand2 = self._normalize_angle(math.pi - theta_r_cand1) + candidate_thetas_r = [theta_r_cand1] + if not math.isclose(theta_r_cand1, theta_r_cand2, abs_tol=self.EPSILON): + candidate_thetas_r.append(theta_r_cand2) + + for theta_r in candidate_thetas_r: + cos_theta_r = math.cos(theta_r) + theta_p_sol: float = 0.0 + if math.isclose(cos_theta_r, 0.0, abs_tol=self.EPSILON): + if ( + math.isclose(abs(vx), 1.0, abs_tol=self.EPSILON) + and math.isclose(vy, 0.0, abs_tol=self.EPSILON) + and math.isclose(vz, 0.0, abs_tol=self.EPSILON) + ): + solutions.append((theta_p_sol, theta_r)) + else: + theta_p_sol = math.atan2(vz / cos_theta_r, vy / cos_theta_r) + solutions.append((theta_p_sol, theta_r)) + return solutions + + def _check_articutool_feasibility_at_waypoint( + self, R_world_jaco_hand: R + ) -> ArticutoolWaypointSolution: + target_vector_in_jaco_hand = R_world_jaco_hand.inv().apply(self.WORLD_UP_VECTOR) + ik_solutions = self._solve_articutool_ik(target_vector_in_jaco_hand) + valid_solutions_in_limits = [] + for theta_p, theta_r in ik_solutions: + theta_p_norm = self._normalize_angle(theta_p) + theta_r_norm = self._normalize_angle(theta_r) + if ( + self.ARTICUTOOL_PITCH_LIMITS_RAD[0] - self.EPSILON + <= theta_p_norm + <= self.ARTICUTOOL_PITCH_LIMITS_RAD[1] + self.EPSILON + and self.ARTICUTOOL_ROLL_LIMITS_RAD[0] - self.EPSILON + <= theta_r_norm + <= self.ARTICUTOOL_ROLL_LIMITS_RAD[1] + self.EPSILON + ): + valid_solutions_in_limits.append((theta_p_norm, theta_r_norm)) + if not valid_solutions_in_limits: + return ArticutoolWaypointSolution(False, None, None) + best_sol = min(valid_solutions_in_limits, key=lambda s: s[0] ** 2 + s[1] ** 2) + return ArticutoolWaypointSolution(True, best_sol[0], best_sol[1]) + + def _analyze_trajectory_for_articutool( + self, trajectory: Optional[JointTrajectory] + ) -> Tuple[ArticutoolMetrics, List[ArticutoolWaypointSolution]]: + # Default return values + default_metrics = ArticutoolMetrics(False, *([np.nan] * 8), 0) + default_solutions_per_wp: List[ArticutoolWaypointSolution] = [] + + if not trajectory or not trajectory.points: + return default_metrics, default_solutions_per_wp + + required_pitches_rad: List[float] = [] + required_rolls_rad: List[float] = [] + path_is_articutool_feasible = True + num_infeasible_wps = 0 + per_waypoint_solutions: List[ArticutoolWaypointSolution] = [] + + for point_idx, point in enumerate(trajectory.points): + current_wp_solution = ArticutoolWaypointSolution(False, None, None) + if len(point.positions) != len(self.joint_names_for_group): + self.logger.warn( + f"Articutool Metrics: Mismatch joint count at point {point_idx}." + ) + path_is_articutool_feasible = False + num_infeasible_wps += 1 + else: + jaco_joint_positions = list(point.positions) + try: + fk_results: Optional[List[PoseStamped]] = ( + self.moveit2_interface.compute_fk( + joint_state=jaco_joint_positions, + fk_link_names=[self.end_effector_link], + ) + ) + if not fk_results or not fk_results[0]: + self.logger.warn( + f"Articutool Metrics: FK failed for Jaco hand at point {point_idx}." + ) + path_is_articutool_feasible = False + num_infeasible_wps += 1 + else: + jaco_hand_pose_msg: Pose = fk_results[0].pose + R_world_jaco_hand = R.from_quat( + [ + jaco_hand_pose_msg.orientation.x, + jaco_hand_pose_msg.orientation.y, + jaco_hand_pose_msg.orientation.z, + jaco_hand_pose_msg.orientation.w, + ] + ) + current_wp_solution = ( + self._check_articutool_feasibility_at_waypoint( + R_world_jaco_hand + ) + ) + if not current_wp_solution.waypoint_feasible: + path_is_articutool_feasible = False + num_infeasible_wps += 1 + else: + if current_wp_solution.pitch_solution_rad is not None: + required_pitches_rad.append( + current_wp_solution.pitch_solution_rad + ) + if current_wp_solution.roll_solution_rad is not None: + required_rolls_rad.append( + current_wp_solution.roll_solution_rad + ) + except Exception as e: + self.logger.error( + f"Articutool Metrics: Error at point {point_idx}: {e}" + ) + path_is_articutool_feasible = False + num_infeasible_wps += 1 + per_waypoint_solutions.append(current_wp_solution) + + p_min_lim, p_max_lim = self.ARTICUTOOL_PITCH_LIMITS_RAD + r_min_lim, r_max_lim = self.ARTICUTOOL_ROLL_LIMITS_RAD + pitch_joint_range = p_max_lim - p_min_lim + roll_joint_range = r_max_lim - r_min_lim + min_p_val, max_p_val, avg_abs_p_val, pitch_range_used_val = [np.nan] * 4 + if required_pitches_rad: + min_p_val, max_p_val = ( + np.min(required_pitches_rad), + np.max(required_pitches_rad), + ) + avg_abs_p_val = np.mean(np.abs(required_pitches_rad)) + pitch_range_used_val = ( + ((max_p_val - min_p_val) / pitch_joint_range * 100) + if pitch_joint_range > self.EPSILON + else 0.0 + ) + min_r_val, max_r_val, avg_abs_r_val, roll_range_used_val = [np.nan] * 4 + if required_rolls_rad: + min_r_val, max_r_val = ( + np.min(required_rolls_rad), + np.max(required_rolls_rad), + ) + avg_abs_r_val = np.mean(np.abs(required_rolls_rad)) + roll_range_used_val = ( + ((max_r_val - min_r_val) / roll_joint_range * 100) + if roll_joint_range > self.EPSILON + else 0.0 + ) + + aggregate_metrics = ArticutoolMetrics( + path_is_articutool_feasible, + min_p_val, + max_p_val, + avg_abs_p_val, + pitch_range_used_val, + min_r_val, + max_r_val, + avg_abs_r_val, + roll_range_used_val, + num_infeasible_wps, + ) + return aggregate_metrics, per_waypoint_solutions + + def _calculate_max_jaco_hand_roll_deviation( self, trajectory: Optional[JointTrajectory] ) -> Optional[float]: + # ... (implementation from previous version, ensure it uses self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ) ... if trajectory is None or not trajectory.points: return None max_abs_roll_deviation = 0.0 + target_quat_xyzw_array = np.array( + self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ + ) + target_ee_orientation_scipy = R.from_quat(target_quat_xyzw_array) for point_idx, point in enumerate(trajectory.points): if len(point.positions) != len(self.joint_names_for_group): - self.logger.warn( - f"FK Calc: Mismatch in joint count at point {point_idx}. Expected {len(self.joint_names_for_group)}, got {len(point.positions)}. Skipping." - ) continue joint_positions_list = list(point.positions) try: @@ -368,45 +573,23 @@ def _calculate_max_roll_deviation( fk_link_names=[self.end_effector_link], ) ) - if ( - not result_poses - or not isinstance(result_poses, list) - or not result_poses[0] - ): - self.logger.warn( - f"FK returned None or invalid format for point {point_idx}." - ) + if not result_poses or not result_poses[0]: continue - pose_stamped_ee = result_poses[0] - if pose_stamped_ee.header.frame_id.lstrip("/") != self.base_link.lstrip( - "/" - ): - self.logger.warn( - f"FK pose for {self.end_effector_link} is in frame '{pose_stamped_ee.header.frame_id}', expected '{self.base_link}'." - ) - q_msg = pose_stamped_ee.pose.orientation + q_msg = result_poses[0].pose.orientation actual_ee_orientation_scipy = R.from_quat( [q_msg.x, q_msg.y, q_msg.z, q_msg.w] ) - target_quat_params = self.NO_ROLL_CONSTRAINT_TARGET_QUATERNION_XYWZ - target_ee_orientation_scipy = R.from_quat(target_quat_params) diff_rotation = ( target_ee_orientation_scipy.inv() * actual_ee_orientation_scipy ) - euler_angles_of_diff_in_target_basis = diff_rotation.as_euler( - "xyz", degrees=False - ) - current_roll_deviation = euler_angles_of_diff_in_target_basis[2] - while current_roll_deviation > np.pi: - current_roll_deviation -= 2 * np.pi - while current_roll_deviation < -np.pi: - current_roll_deviation += 2 * np.pi + euler_angles_of_diff = diff_rotation.as_euler("xyz", degrees=False) + current_roll_deviation = self._normalize_angle(euler_angles_of_diff[2]) max_abs_roll_deviation = max( max_abs_roll_deviation, abs(current_roll_deviation) ) except Exception as e: self.logger.error( - f"Error during FK for roll deviation at point {point_idx}: {e}" + f"Jaco Hand Roll Dev FK: Error at point {point_idx}: {e}" ) return float("inf") return max_abs_roll_deviation @@ -414,18 +597,17 @@ def _calculate_max_roll_deviation( def _get_path_length_stats( self, trajectory: Optional[JointTrajectory] ) -> Tuple[Optional[float], Optional[Dict[str, float]]]: + # ... (implementation from previous version) ... if trajectory is None or not trajectory.points: return None, None - j6_name = ( - "j2n6s200_joint_6" - if "j2n6s200_joint_6" in self.joint_names_for_group - else None - ) return GET_PATH_LEN_METHOD(trajectory) def _save_trajectory_to_file( self, - trajectory: JointTrajectory, + original_trajectory: JointTrajectory, # Original Jaco trajectory + articutool_solutions_per_wp: List[ + ArticutoolWaypointSolution + ], # Per-waypoint solutions start_config_name: str, goal_config_name: str, planner_id_str: str, @@ -437,16 +619,66 @@ def _save_trajectory_to_file( safe_start_name = start_config_name.replace(" ", "_").replace("/", "_") safe_goal_name = goal_config_name.replace(" ", "_").replace("/", "_") safe_planner_id_str = planner_id_str.replace(" ", "_").replace("/", "_") - filename = f"{safe_start_name}_to_{safe_goal_name}_{safe_planner_id_str}_{timestamp}.json" + filename = f"{safe_start_name}_to_{safe_goal_name}_{safe_planner_id_str}_{timestamp}_enhanced.json" filepath = os.path.join(self.trajectory_save_dir, filename) - traj_dict = message_to_ordereddict(trajectory) + + enhanced_trajectory_data: Dict[str, Any] = { + "jaco_joint_names": list(original_trajectory.joint_names), + "waypoints": [], + } + + num_jaco_points = len(original_trajectory.points) + num_articutool_solutions = len(articutool_solutions_per_wp) + + if num_jaco_points != num_articutool_solutions: + self.logger.warn( + f"Mismatch between Jaco trajectory points ({num_jaco_points}) and " + f"Articutool solutions ({num_articutool_solutions}) for {filename}. " + f"Saving only up to the minimum length." + ) + + min_len = min(num_jaco_points, num_articutool_solutions) + + for i in range(min_len): + jaco_point = original_trajectory.points[i] + at_solution = articutool_solutions_per_wp[i] + + waypoint_data = { + "time_from_start_sec": jaco_point.time_from_start.sec + + jaco_point.time_from_start.nanosec * 1e-9, + "jaco_positions_rad": list(jaco_point.positions), + "jaco_velocities_rad_per_sec": ( + list(jaco_point.velocities) if jaco_point.velocities else [] + ), + "jaco_accelerations_rad_per_sec2": ( + list(jaco_point.accelerations) + if jaco_point.accelerations + else [] + ), + "articutool_waypoint_feasible": at_solution.waypoint_feasible, + "articutool_pitch_solution_rad": ( + at_solution.pitch_solution_rad + if at_solution.pitch_solution_rad is not None + else None + ), # Ensure None is JSON null + "articutool_roll_solution_rad": ( + at_solution.roll_solution_rad + if at_solution.roll_solution_rad is not None + else None + ), + } + enhanced_trajectory_data["waypoints"].append(waypoint_data) + with open(filepath, "w") as f: - json.dump(traj_dict, f, indent=2) - self.logger.info(f" Successfully saved trajectory to: {filepath}") + json.dump(enhanced_trajectory_data, f, indent=2) + self.logger.info( + f" Successfully saved ENHANCED trajectory to: {filepath}" + ) return filename except Exception as e: self.logger.error( - f" Failed to save trajectory for {start_config_name} to {goal_config_name} ({planner_id_str}): {e}" + f" Failed to save ENHANCED trajectory for {start_config_name} to {goal_config_name} ({planner_id_str}): {e}", + exc_info=True, ) return None @@ -456,8 +688,9 @@ def plan_to_target_configuration( planner_id_str: str, path_constraints_kwargs_to_apply: Optional[Dict[str, Any]] = None, ) -> Tuple[Optional[JointTrajectory], float]: + # ... (implementation from previous version, ensure self.rate.sleep() is time.sleep() or handled by executor) ... self.logger.info( - f" Attempting to plan with: {planner_id_str} to GOAL: {goal_joints}" + f" Attempting to plan with: {planner_id_str} to GOAL: {np.round(goal_joints, 3).tolist()}" ) if planner_id_str.lower() == "chomp": self.moveit2_interface.pipeline_id = "chomp" @@ -478,7 +711,6 @@ def plan_to_target_configuration( tolerance=0.01, weight=1.0, ) - self.logger.info(f" Set joint goal for {planner_id_str}.") except Exception as e: self.logger.error(f" Failed to set joint goal: {e}") return None, 0.0 @@ -487,21 +719,18 @@ def plan_to_target_configuration( self.moveit2_interface.set_path_orientation_constraint( **path_constraints_kwargs_to_apply ) - self.logger.info( - f" Applied 'no roll' path constraint for {planner_id_str}." - ) except Exception as e: self.logger.error(f" Failed to set path orientation constraint: {e}") start_time_ros = self.node.get_clock().now() future = self.moveit2_interface.plan_async(start_joint_state=None) joint_trajectory_for_analysis: Optional[JointTrajectory] = None - timeout_duration_rclpy = Duration(seconds=self.planning_timeout_sec + 2.0) + timeout_duration_rclpy = RCLPYDuration(seconds=self.planning_timeout_sec + 5.0) wait_start_time_rclpy = self.node.get_clock().now() while rclpy.ok() and not future.done(): elapsed_wait = self.node.get_clock().now() - wait_start_time_rclpy if elapsed_wait >= timeout_duration_rclpy: self.logger.warn( - f" Planner {planner_id_str} timed out after {elapsed_wait.nanoseconds / 1e9:.2f}s while waiting for future.done()." + f" Planner {planner_id_str} timed out after {elapsed_wait.nanoseconds / 1e9:.2f}s waiting for future." ) if ( hasattr(future, "cancel") @@ -510,7 +739,7 @@ def plan_to_target_configuration( ): future.cancel() break - self.rate.sleep() + time.sleep(0.05) # Yield for other ROS processing if future.done() and not future.cancelled(): try: plan_result_srv_response = future.result() @@ -518,15 +747,13 @@ def plan_to_target_configuration( plan_result_srv_response.motion_plan_response.error_code.val == moveit_msgs.msg.MoveItErrorCodes.SUCCESS ): - trajectory_msg = ( - plan_result_srv_response.motion_plan_response.trajectory - ) - if trajectory_msg and trajectory_msg.joint_trajectory.points: - joint_trajectory_for_analysis = trajectory_msg.joint_trajectory + traj_msg = plan_result_srv_response.motion_plan_response.trajectory + if traj_msg and traj_msg.joint_trajectory.points: + joint_trajectory_for_analysis = traj_msg.joint_trajectory self.logger.info(f" Planner {planner_id_str} succeeded.") else: self.logger.warn( - f" Planner {planner_id_str} succeeded but returned an empty joint_trajectory." + f" Planner {planner_id_str} succeeded but returned empty trajectory." ) else: self.logger.warn( @@ -534,25 +761,21 @@ def plan_to_target_configuration( ) except Exception as e: self.logger.error( - f" Exception while getting plan result for {planner_id_str}: {e}" + f" Exception getting plan result for {planner_id_str}: {e}" ) elif future.cancelled(): - self.logger.warn( - f" Planning for {planner_id_str} was cancelled (likely due to timeout)." - ) - elapsed_time = (self.node.get_clock().now() - start_time_ros).nanoseconds / 1e9 + self.logger.warn(f" Planning for {planner_id_str} was cancelled.") + elapsed_time_sec = ( + self.node.get_clock().now() - start_time_ros + ).nanoseconds / 1e9 self.moveit2_interface.clear_path_constraints() self.moveit2_interface.clear_goal_constraints() - return joint_trajectory_for_analysis, elapsed_time - - def _normalize_angle(self, angle: float) -> float: - """Normalize an angle to the range [-pi, pi].""" - return (angle + np.pi) % (2 * np.pi) - np.pi + return joint_trajectory_for_analysis, elapsed_time_sec def _are_angles_close( self, angle1: float, angle2: float, tolerance: float, is_continuous: bool ) -> bool: - """Checks if two angles are close, handling wrap-around for continuous joints.""" + # ... (implementation from previous version) ... if is_continuous: diff = self._normalize_angle(angle1 - angle2) return abs(diff) <= tolerance @@ -568,203 +791,125 @@ def _move_to_config_blocking( verification_tolerance: float = 0.05, max_move_attempts: int = 3, ): + # ... (implementation from previous version) ... self.logger.info( - f"Attempting to move to configuration: '{target_config_name}' target values: {np.round(target_joints, 4).tolist()}" + f"Attempting to move to config: '{target_config_name}' target: {np.round(target_joints, 4).tolist()}" + ) + original_planner, original_pipeline, original_timeout = ( + self.moveit2_interface.planner_id, + self.moveit2_interface.pipeline_id, + self.moveit2_interface.allowed_planning_time, ) - original_planner = self.moveit2_interface.planner_id - original_pipeline = self.moveit2_interface.pipeline_id - original_timeout = self.moveit2_interface.allowed_planning_time - - # Use a reliable planner for unconstrained moves self.moveit2_interface.pipeline_id = "ompl" - # Consider making this configurable if needed, or even a list of planners to try - unconstrained_move_planner_id = "RRTConnectkConfigDefault" - self.moveit2_interface.planner_id = unconstrained_move_planner_id - # Ensure a reasonable timeout for these critical setup moves + self.moveit2_interface.planner_id = "RRTConnectkConfigDefault" self.moveit2_interface.allowed_planning_time = max( 10.0, self.planning_timeout_sec ) - achieved_target = False - last_known_joints_list_ordered_str = "N/A" - last_exception_detail = "No attempts made or error prior to first attempt." - + last_known_joints_str = "N/A" + last_exception_detail = "No attempts." for attempt in range(max_move_attempts): self.logger.info( - f"Move attempt {attempt + 1}/{max_move_attempts} to '{target_config_name}' using planner '{unconstrained_move_planner_id}'." + f"Move attempt {attempt + 1}/{max_move_attempts} to '{target_config_name}'." ) try: self.moveit2_interface.clear_goal_constraints() self.moveit2_interface.clear_path_constraints() - - # Command the move - self.logger.info( - f" Commanding move_to_configuration for '{target_config_name}' (attempt {attempt + 1})." - ) - # move_to_configuration can raise an exception if planning fails or execution is controller-aborted. - # It is a blocking call. self.moveit2_interface.move_to_configuration( joint_positions=target_joints, joint_names=self.joint_names_for_group, tolerance=0.01, ) self.logger.info( - f" Move_to_configuration call for '{target_config_name}' (attempt {attempt + 1}) completed by MoveIt2. " - f"Now verifying final settled state (timeout: {verification_timeout_sec}s)." + f" Move_to_configuration call for '{target_config_name}' completed. Verifying..." ) - # Verification loop - verification_loop_start_time = self.node.get_clock().now() - achieved_target_this_attempt = False - - num_verification_checks = max( - 2, + verif_start_time = self.node.get_clock().now() + achieved_this_attempt = False + num_verif_checks = max( + 3, int(verification_timeout_sec / verification_poll_interval_sec) + 1, ) - - for verify_idx in range(num_verification_checks): - current_joint_positions_dict = self.get_current_joint_positions( + for v_idx in range(num_verif_checks): + current_joints_dict = self.get_current_joint_positions( timeout_sec=0.5 ) - - if current_joint_positions_dict: - current_joints_list_for_comparison = [] - all_names_found_in_current = True - for name in self.joint_names_for_group: - if name in current_joint_positions_dict: - current_joints_list_for_comparison.append( - current_joint_positions_dict[name] - ) - else: - all_names_found_in_current = False - self.logger.warn( - f" Verification (attempt {attempt + 1}, check {verify_idx + 1}): Missing joint '{name}' in current joint state." - ) - break - - if not all_names_found_in_current: - last_known_joints_list_ordered_str = ( - "Partial joint state received" - ) - # Give a small pause and try next verification check + if current_joints_dict: + current_joints_list = [ + current_joints_dict[name] + for name in self.joint_names_for_group + if name in current_joints_dict + ] + if len(current_joints_list) != len(self.joint_names_for_group): + last_known_joints_str = "Partial state." self.node.get_clock().sleep_for( - Duration(seconds=verification_poll_interval_sec) + RCLPYDuration(seconds=verification_poll_interval_sec) ) continue - - last_known_joints_list_ordered_str = str( - np.round(current_joints_list_for_comparison, 4).tolist() + last_known_joints_str = str( + np.round(current_joints_list, 4).tolist() ) - - all_joints_within_tolerance = True - for j_idx, (target_val, actual_val) in enumerate( - zip(target_joints, current_joints_list_for_comparison) + all_close = True + for j_idx, (tgt, act) in enumerate( + zip(target_joints, current_joints_list) ): - is_continuous = j_idx in self.continuous_joint_indices if not self._are_angles_close( - target_val, - actual_val, + tgt, + act, verification_tolerance, - is_continuous, + j_idx in self.continuous_joint_indices, ): - all_joints_within_tolerance = False - # Log less frequently during verification to avoid spam, but ensure important info is logged - if ( - verify_idx % 5 == 0 - or verify_idx == num_verification_checks - 1 - or verification_timeout_sec < 2.0 - ): - self.logger.info( - f" Verification (attempt {attempt + 1}, check {verify_idx + 1}) for '{target_config_name}': Joint '{self.joint_names_for_group[j_idx]}' (idx {j_idx}, cont: {is_continuous}) out of tolerance. Target: {target_val:.4f}, Actual: {actual_val:.4f}, NormDiff: {self._normalize_angle(target_val - actual_val):.4f}, RawDiff: {(target_val - actual_val):.4f}" - ) + all_close = False break - - if all_joints_within_tolerance: - self.logger.info( - f" Successfully verified robot reached configuration: '{target_config_name}' on move attempt {attempt + 1}, verification check {verify_idx + 1}." - ) + if all_close: self.logger.info( - f" Final Target: {np.round(target_joints, 4).tolist()}" - ) - self.logger.info( - f" Final Actual: {np.round(current_joints_list_for_comparison, 4).tolist()}" + f" Successfully verified robot reached '{target_config_name}'." ) achieved_target = True - achieved_target_this_attempt = True + achieved_this_attempt = True break else: - last_known_joints_list_ordered_str = ( - "No joint states received for this check." - ) - self.logger.warn( - f" Verification (attempt {attempt + 1}, check {verify_idx + 1}): No relevant JointState message received." - ) - - # Check for overall verification timeout for this attempt - if not rclpy.ok() or ( - self.node.get_clock().now() - verification_loop_start_time - ) >= Duration(seconds=verification_timeout_sec): - if not achieved_target_this_attempt: + last_known_joints_str = "No joint state." + if ( + self.node.get_clock().now() - verif_start_time + ) >= RCLPYDuration(seconds=verification_timeout_sec): + if not achieved_this_attempt: self.logger.warn( - f" Verification timeout for '{target_config_name}' on move attempt {attempt + 1} after {verification_timeout_sec}s of polling." + f" Verification timeout for '{target_config_name}'." ) - break - - if not achieved_target_this_attempt: + break + if not achieved_this_attempt: self.node.get_clock().sleep_for( - Duration(seconds=verification_poll_interval_sec) + RCLPYDuration(seconds=verification_poll_interval_sec) ) - if achieved_target: break - - # If verification failed for this attempt (either by joints out of tolerance or timeout) - if not achieved_target_this_attempt: - self.logger.warn( - f" Move attempt {attempt + 1} to '{target_config_name}' seemed to complete motion, but verification failed. " - f"Last known state during verification: {last_known_joints_list_ordered_str}" + if not achieved_this_attempt: + last_exception_detail = ( + f"Verification failed. Last state: {last_known_joints_str}" ) - last_exception_detail = f"Verification failed after move_to_configuration call. Last state: {last_known_joints_list_ordered_str}" - except Exception as e: self.logger.warn( - f" Move attempt {attempt + 1}/{max_move_attempts} to '{target_config_name}' failed directly during move_to_configuration. Error: {type(e).__name__} - {e}" + f" Move attempt {attempt + 1} to '{target_config_name}' failed during move: {e}" ) - last_exception_detail = ( - f"Error in move_to_configuration: {type(e).__name__} - {e}" - ) - + last_exception_detail = f"Error in move: {e}" if achieved_target: break - - # If this was the last attempt and still not achieved - if attempt == max_move_attempts - 1: - self.logger.error( - f"All {max_move_attempts} attempts to move to '{target_config_name}' have been exhausted and failed." - ) - # The final error will be raised outside the loop if achieved_target is still False - elif not achieved_target: - self.logger.info( - f" Pausing briefly before retry for '{target_config_name}'." - ) - self.node.get_clock().sleep_for(Duration(seconds=1.5)) - - # Restore original MoveIt2 settings AFTER all attempts - self.moveit2_interface.planner_id = original_planner - self.moveit2_interface.pipeline_id = original_pipeline - self.moveit2_interface.allowed_planning_time = original_timeout - + if attempt < max_move_attempts - 1: + self.node.get_clock().sleep_for(RCLPYDuration(seconds=1.5)) + ( + self.moveit2_interface.planner_id, + self.moveit2_interface.pipeline_id, + self.moveit2_interface.allowed_planning_time, + ) = ( + original_planner, + original_pipeline, + original_timeout, + ) if not achieved_target: - self.logger.error( - f"CRITICAL: Failed to move AND verify robot reached '{target_config_name}' (target: {np.round(target_joints, 4).tolist()}) " - f"after {max_move_attempts} attempts. " - f"Last known state from /joint_states (during last verification): {last_known_joints_list_ordered_str}. " - f"Detail of last failure: {last_exception_detail}" - ) - raise RuntimeError( - f"Failed to reach and verify start configuration '{target_config_name}' after {max_move_attempts} attempts. Last failure detail: {last_exception_detail}" - ) - # If loop finished and achieved_target is True, then the move was successful. + final_error_msg = f"CRITICAL: Failed to move AND verify robot at '{target_config_name}' after {max_move_attempts} attempts. Last state: {last_known_joints_str}. Detail: {last_exception_detail}" + self.logger.error(final_error_msg) + raise RuntimeError(final_error_msg) self.logger.info( f"Successfully moved to and verified configuration '{target_config_name}'." ) @@ -773,50 +918,124 @@ def move_to_initial_config(self): self._move_to_config_blocking("InitialScriptSetup", self.initial_joint_config) def run_benchmark_planning_task(self, task: PlanningTask): - start_name = task.start_config.name - goal_name = task.goal_config.name - goal_joints = task.goal_config.joint_values + start_name, goal_name, goal_joints = ( + task.start_config.name, + task.goal_config.name, + task.goal_config.joint_values, + ) self.logger.info(f"--- Testing Task: FROM '{start_name}' TO '{goal_name}' ---") - self.logger.info(f" Goal Joints: {goal_joints}") + self.logger.info(f" Goal Joints: {np.round(goal_joints, 3).tolist()}") task_key = (start_name, goal_name) + for planner_id_str in self.planners_to_test: - joint_trajectory, elapsed_time = self.plan_to_target_configuration( - goal_joints=goal_joints, - planner_id_str=planner_id_str, - path_constraints_kwargs_to_apply=self.no_roll_path_constraint_kwargs, + path_constraints_to_apply = ( + self.naive_jaco_hand_constraint_kwargs + if self.use_naive_jaco_hand_constraint + else None + ) + + jaco_trajectory, elapsed_time = self.plan_to_target_configuration( + goal_joints, planner_id_str, path_constraints_to_apply + ) + + jaco_plan_success = jaco_trajectory is not None and bool( + jaco_trajectory.points ) - success = joint_trajectory is not None and bool(joint_trajectory.points) path_len_total, joint_path_lens_map = self._get_path_length_stats( - joint_trajectory + jaco_trajectory ) - max_roll_dev = self._calculate_max_roll_deviation(joint_trajectory) + max_jaco_roll_dev = self._calculate_max_jaco_hand_roll_deviation( + jaco_trajectory + ) + + aggregate_at_metrics: ArticutoolMetrics + per_wp_at_solutions: List[ArticutoolWaypointSolution] + + if jaco_plan_success: + aggregate_at_metrics, per_wp_at_solutions = ( + self._analyze_trajectory_for_articutool(jaco_trajectory) + ) + else: + num_potential_wps = len( + goal_joints + ) # A rough estimate if trajectory is None + aggregate_at_metrics = ArticutoolMetrics( + False, *([np.nan] * 8), num_potential_wps + ) + per_wp_at_solutions = [ + ArticutoolWaypointSolution(False, None, None) + ] * num_potential_wps + trajectory_filename = None - if success and self.trajectory_save_dir: + if jaco_plan_success and self.trajectory_save_dir: trajectory_filename = self._save_trajectory_to_file( - joint_trajectory, start_name, goal_name, planner_id_str + jaco_trajectory, + per_wp_at_solutions, + start_name, + goal_name, + planner_id_str, ) + self.results[task_key][planner_id_str] = PlanResult( - joint_trajectory, + jaco_trajectory, elapsed_time, path_len_total, joint_path_lens_map, - max_roll_dev, - success, + max_jaco_roll_dev, + jaco_plan_success, trajectory_filename, + aggregate_at_metrics, + per_wp_at_solutions, # Storing per-wp solutions in PlanResult + ) + + # Pre-format strings for logging to avoid f-string errors with None/NaN + path_len_str = ( + f"{path_len_total:.3f}" + if path_len_total is not None + and not np.isnan(path_len_total) + and not np.isinf(path_len_total) + else "N/A" + ) + jaco_roll_dev_str = ( + f"{max_jaco_roll_dev:.3f}" + if max_jaco_roll_dev is not None + and not np.isnan(max_jaco_roll_dev) + and not np.isinf(max_jaco_roll_dev) + else "N/A" + ) + + at_feasible_str = str(aggregate_at_metrics.path_feasible) + at_infeasible_pts_str = str(aggregate_at_metrics.num_infeasible_points) + at_p_range_str = ( + f"{aggregate_at_metrics.pitch_range_used_percent:.1f}" + if not np.isnan(aggregate_at_metrics.pitch_range_used_percent) + else "N/A" + ) + at_r_range_str = ( + f"{aggregate_at_metrics.roll_range_used_percent:.1f}" + if not np.isnan(aggregate_at_metrics.roll_range_used_percent) + else "N/A" ) - log_msg_suffix = ( + + articutool_log = ( + f"| Articutool Feasible: {at_feasible_str} (Infeasible Pts: {at_infeasible_pts_str}, " + f"P-Range%: {at_p_range_str}, R-Range%: {at_r_range_str})" + ) + log_suffix = ( f"| TrajFile: {trajectory_filename}" if trajectory_filename else "" ) - if success: + + if jaco_plan_success: self.logger.info( - f" Planner: {planner_id_str} | Success: True | Time: {elapsed_time:.3f}s | PathLen: {path_len_total if path_len_total is not None else 'N/A':.3f} | MaxRollDev: {max_roll_dev if max_roll_dev is not None else 'N/A':.3f} rad {log_msg_suffix}" + f" Planner: {planner_id_str} | Jaco Success: True | Time: {elapsed_time:.3f}s | PathLen: {path_len_str} | JacoHandRollDev: {jaco_roll_dev_str} rad {articutool_log} {log_suffix}" ) else: self.logger.warn( - f" Planner: {planner_id_str} | Success: False | Time: {elapsed_time:.3f}s {log_msg_suffix}" + f" Planner: {planner_id_str} | Jaco Success: False | Time: {elapsed_time:.3f}s {log_suffix}" ) def run_all_tests(self, log_summary_every_n_tasks: Optional[int] = None): + # ... (implementation from previous version, ensure default ArticutoolMetrics and empty list for per_wp_solutions on failure) ... self.logger.info( "=== Starting Benchmark: Moving to Initial Script Configuration ===" ) @@ -824,15 +1043,13 @@ def run_all_tests(self, log_summary_every_n_tasks: Optional[int] = None): self.move_to_initial_config() except RuntimeError as e: self.logger.error( - f"CRITICAL FAILURE: Could not move to initial script setup configuration. Benchmark aborted. Error: {e}" + f"CRITICAL FAILURE: Could not move to initial config. Benchmark aborted. Error: {e}" ) - # Record this major failure if needed, though the script will likely exit or not proceed. - return # Abort benchmark if initial setup fails + return for i, planning_task in enumerate(self.planning_tasks): self.logger.info( - f"\n=== Running Planning Task {i + 1}/{self.num_tasks}: " - f"FROM '{planning_task.start_config.name}' TO '{planning_task.goal_config.name}' ===" + f"\n=== Running Planning Task {i + 1}/{self.num_tasks}: FROM '{planning_task.start_config.name}' TO '{planning_task.goal_config.name}' ===" ) try: self._move_to_config_blocking( @@ -847,50 +1064,61 @@ def run_all_tests(self, log_summary_every_n_tasks: Optional[int] = None): planning_task.start_config.name, planning_task.goal_config.name, ) - for planner_id_str in self.planners_to_test: - self.results[task_key][planner_id_str] = PlanResult( + default_at_metrics = ArticutoolMetrics(False, *([np.nan] * 8), 0) + empty_at_solutions: List[ArticutoolWaypointSolution] = [] + for planner_id_str_fail in self.planners_to_test: + self.results[task_key][planner_id_str_fail] = PlanResult( None, 0.0, None, None, None, False, - f"ERROR_MOVE_TO_START_FAILED:{e}", + f"ERROR_MOVE_TO_START_FAILED:{str(e)[:50]}", + default_at_metrics, + empty_at_solutions, ) continue - self.run_benchmark_planning_task(planning_task) - if log_summary_every_n_tasks and (i + 1) % log_summary_every_n_tasks == 0: self.log_summary_results() self.logger.info("=== Benchmark Run Completed ===") def get_csv_header(self) -> List[str]: - header = ["start_config_name"] - header.extend([f"start_{name}" for name in self.joint_names_for_group]) - header.extend(["goal_config_name"]) - header.extend([f"goal_{name}" for name in self.joint_names_for_group]) - header.extend( - [ - "planner_id", - "elapsed_time_s", - "success", - "path_length_total", - "max_roll_deviation_rad", - "trajectory_filename", - ] - ) - header.extend([f"path_length_{name}" for name in self.joint_names_for_group]) + # ... (implementation from previous version, ensure names match PlanResult) ... + header = [ + "start_config_name", + *[f"start_{name}" for name in self.joint_names_for_group], + "goal_config_name", + *[f"goal_{name}" for name in self.joint_names_for_group], + "planner_id", + "elapsed_time_s", + "jaco_plan_success", + "path_length_total", + "max_jaco_hand_roll_deviation_rad", + "trajectory_filename", + "articutool_path_feasible", + "articutool_num_infeasible_points", + "articutool_min_pitch_rad", + "articutool_max_pitch_rad", + "articutool_avg_pitch_abs_rad", + "articutool_pitch_range_used_percent", + "articutool_min_roll_rad", + "articutool_max_roll_rad", + "articutool_avg_roll_abs_rad", + "articutool_roll_range_used_percent", + *[f"path_length_{name}" for name in self.joint_names_for_group], + ] return header def write_results_to_csv(self, filename: str): + # ... (implementation from previous version, ensure all fields from PlanResult.articutool_metrics are written) ... self.logger.info(f"Writing results to {filename}") with open(filename, "w", newline="") as f: import csv csv_writer = csv.writer(f) - header = self.get_csv_header() - csv_writer.writerow(header) + csv_writer.writerow(self.get_csv_header()) for task_key in sorted(self.results.keys()): start_name, goal_name = task_key start_cfg_obj = next( @@ -902,146 +1130,177 @@ def write_results_to_csv(self, filename: str): None, ) if not start_cfg_obj or not goal_cfg_obj: - self.logger.warn( - f"Could not find original config objects for task key {task_key}. Skipping CSV rows for this task." - ) continue if task_key in self.results: - planner_runs_for_task = self.results[task_key] for planner_id_str in self.planners_to_test: - if planner_id_str in planner_runs_for_task: - result = planner_runs_for_task[planner_id_str] - row = [] - row.append(str(start_cfg_obj.name)) - row.extend(map(str, start_cfg_obj.joint_values)) - row.append(str(goal_cfg_obj.name)) - row.extend(map(str, goal_cfg_obj.joint_values)) - row.append(str(planner_id_str)) - row.append(f"{result.elapsed_time:.4f}") - row.append(str(1 if result.success else 0)) - row.append( - f"{result.path_length:.4f}" - if result.path_length is not None - else "" - ) - row.append( - f"{result.max_roll_deviation:.4f}" - if result.max_roll_deviation is not None - else "" - ) - row.append( - result.trajectory_filename - if result.trajectory_filename - else "" - ) + if planner_id_str in self.results[task_key]: + result = self.results[task_key][planner_id_str] + row = [ + start_cfg_obj.name, + *map(str, start_cfg_obj.joint_values), + goal_cfg_obj.name, + *map(str, goal_cfg_obj.joint_values), + planner_id_str, + f"{result.elapsed_time:.4f}", + str(1 if result.jaco_plan_success else 0), + ( + f"{result.path_length:.4f}" + if result.path_length is not None + else "" + ), + ( + f"{result.max_jaco_hand_roll_deviation:.4f}" + if result.max_jaco_hand_roll_deviation is not None + else "" + ), + ( + result.trajectory_filename + if result.trajectory_filename + else "" + ), + ] + atm = result.articutool_metrics + if atm: + row.extend( + [ + str(1 if atm.path_feasible else 0), + str(atm.num_infeasible_points), + ( + f"{atm.min_pitch_rad:.4f}" + if not np.isnan(atm.min_pitch_rad) + else "" + ), + ( + f"{atm.max_pitch_rad:.4f}" + if not np.isnan(atm.max_pitch_rad) + else "" + ), + ( + f"{atm.avg_pitch_abs_rad:.4f}" + if not np.isnan(atm.avg_pitch_abs_rad) + else "" + ), + ( + f"{atm.pitch_range_used_percent:.2f}" + if not np.isnan( + atm.pitch_range_used_percent + ) + else "" + ), + ( + f"{atm.min_roll_rad:.4f}" + if not np.isnan(atm.min_roll_rad) + else "" + ), + ( + f"{atm.max_roll_rad:.4f}" + if not np.isnan(atm.max_roll_rad) + else "" + ), + ( + f"{atm.avg_roll_abs_rad:.4f}" + if not np.isnan(atm.avg_roll_abs_rad) + else "" + ), + ( + f"{atm.roll_range_used_percent:.2f}" + if not np.isnan(atm.roll_range_used_percent) + else "" + ), + ] + ) + else: + row.extend(["ERROR_NO_AT_METRICS"] * 10) if result.joint_path_lengths: - for joint_name_csv in self.joint_names_for_group: - length_val = result.joint_path_lengths.get( - joint_name_csv - ) - row.append( - f"{length_val:.4f}" - if length_val is not None - else "" - ) + row.extend( + [ + ( + f"{result.joint_path_lengths.get(name, ''):.4f}" + if result.joint_path_lengths.get(name) + is not None + else "" + ) + for name in self.joint_names_for_group + ] + ) else: row.extend([""] * len(self.joint_names_for_group)) csv_writer.writerow(row) - else: - self.logger.warn( - f"No result for planner '{planner_id_str}' in task '{start_name}' to '{goal_name}'. Skipping CSV row." - ) self.logger.info(f"Results successfully written to {filename}") def log_summary_results(self): - self.logger.info("--- BENCHMARK SUMMARY (ALL TASKS) ---") + # ... (implementation from previous version, ensure it uses updated PlanResult fields) ... + self.logger.info("--- BENCHMARK SUMMARY (ALL COMPLETED TASKS) ---") for planner_id_str in self.planners_to_test: - results_for_planner = [] - for task_results_dict in self.results.values(): - if planner_id_str in task_results_dict: - results_for_planner.append(task_results_dict[planner_id_str]) - total_plans = len(results_for_planner) - if total_plans == 0: + results_for_planner = [ + res[planner_id_str] + for res in self.results.values() + if planner_id_str in res + ] + if not results_for_planner: self.logger.info( - f"--- Planner: {planner_id_str} ---\n No plans attempted or recorded for this planner." + f"--- Planner: {planner_id_str} ---\n No tasks recorded." ) continue - successful_plans = sum(1 for r in results_for_planner if r.success) - total_planning_time = sum(r.elapsed_time for r in results_for_planner) - path_lengths_list = [ - r.path_length - for r in results_for_planner - if r.success and r.path_length is not None - ] - roll_deviations_list = [ - r.max_roll_deviation - for r in results_for_planner - if r.success and r.max_roll_deviation is not None - ] - self.logger.info(f"--- Planner: {planner_id_str} ---") - success_rate = ( - (successful_plans / total_plans) * 100 if total_plans > 0 else 0 - ) - avg_planning_time_all = ( - total_planning_time / total_plans if total_plans > 0 else 0 + + successful_jaco_plans = sum( + 1 for r in results_for_planner if r.jaco_plan_success ) - successful_planning_times = [ - r.elapsed_time for r in results_for_planner if r.success - ] - avg_planning_time_succ = ( - sum(successful_planning_times) / len(successful_planning_times) - if successful_planning_times - else 0 + total_tasks = len(results_for_planner) + jaco_success_rate = ( + (successful_jaco_plans / total_tasks * 100) if total_tasks > 0 else 0 ) + + self.logger.info(f"--- Planner: {planner_id_str} ---") self.logger.info( - f" Success Rate: {success_rate:.2f}% ({successful_plans}/{total_plans})" + f" Jaco Plan Success Rate: {jaco_success_rate:.2f}% ({successful_jaco_plans}/{total_tasks})" ) - self.logger.info( - f" Avg. Planning Time (all attempts): {avg_planning_time_all:.3f}s" + # ... (other Jaco metrics as before) ... + + articutool_feasible_paths_count = sum( + 1 + for r in results_for_planner + if r.jaco_plan_success + and r.articutool_metrics + and r.articutool_metrics.path_feasible ) - if successful_planning_times: - self.logger.info( - f" Avg. Planning Time (successful attempts): {avg_planning_time_succ:.3f}s" - ) - if path_lengths_list: + if successful_jaco_plans > 0: + articutool_path_success_rate = ( + articutool_feasible_paths_count / successful_jaco_plans + ) * 100 self.logger.info( - f" Path Lengths (successful plans): {PlannerBenchmark._summary_stats_str(path_lengths_list)}" - ) - else: - self.logger.info(" Path Lengths (successful plans): N/A") - valid_roll_deviations = [ - r - for r in roll_deviations_list - if r is not None and not (isinstance(r, float) and np.isinf(r)) - ] - if valid_roll_deviations: - self.logger.info( - f" Max Roll Deviations (rad, successful plans): {PlannerBenchmark._summary_stats_str(valid_roll_deviations)}" + f" Articutool Path Feasibility Rate (of Jaco succ. plans): {articutool_path_success_rate:.2f}% ({articutool_feasible_paths_count}/{successful_jaco_plans})" ) else: self.logger.info( - " Max Roll Deviations (rad, successful plans): N/A (or all FK failed/no valid deviations)" + " Articutool Path Feasibility Rate: N/A (no Jaco successful plans)" ) self.logger.info("-------------------------") @staticmethod def _summary_stats_str(data: List[float]) -> str: + # ... (implementation from previous version) ... if not data: return "N/A" valid_data = [x for x in data if isinstance(x, (int, float)) and np.isfinite(x)] if not valid_data: - return "N/A (no valid data points)" - return f"Mean {np.mean(valid_data):.3f}, Median {np.median(valid_data):.3f}, Min {np.min(valid_data):.3f}, Max {np.max(valid_data):.3f}, Std {np.std(valid_data):.3f}, Count {len(valid_data)}" + return "N/A (no valid numeric data)" + return ( + f"Mean {np.mean(valid_data):.3f}, Median {np.median(valid_data):.3f}, " + f"Min {np.min(valid_data):.3f}, Max {np.max(valid_data):.3f}, " + f"Std {np.std(valid_data):.3f}, Count {len(valid_data)}" + ) -def main(output_dir_arg: Optional[str]): +def main(output_dir_arg: Optional[str], use_naive_constraint_arg: bool): + # ... (implementation from previous version, ensure PlannerBenchmark is instantiated correctly) ... rclpy.init() node = Node("planner_benchmark_pairwise_physical_moves") global _LOGGER_INSTANCE _LOGGER_INSTANCE = node.get_logger() - executor = rclpy.executors.MultiThreadedExecutor(2) + executor = rclpy.executors.MultiThreadedExecutor(num_threads=2) executor.add_node(node) - executor_thread = Thread(target=executor.spin, daemon=True, args=()) + executor_thread = Thread(target=executor.spin, daemon=True) executor_thread.start() _LOGGER_INSTANCE.info( "Benchmark node spinning. Waiting for services (approx 5s)..." @@ -1049,42 +1308,36 @@ def main(output_dir_arg: Optional[str]): time.sleep(5.0) hardcoded_configs = { - "MoveAbovePlate": [ - -2.4538579336877304, - 3.07974419938212, - 1.8320725365979, - 4.096143890468605, - -2.003422584820525, - -3.2123560395465063, - ], - "RestingAcquireFood": [-1.94672, 2.51268, 0.35653, -4.76501, 5.99991, 4.99555], - "StagingConfig": [-2.32526, 4.456298, 4.16769, 1.53262, -2.18359, -2.19525], - "StowLocation": [-1.52101, 2.60098, 0.32811, -4.00012, 0.22831, 3.87886], + "MoveAbovePlate": [-2.4538, 3.0797, 1.8320, 4.0961, -2.0034, -3.2123], + "RestingAcquireFood": [-1.9467, 2.5126, 0.3565, -4.7650, 5.9999, 4.9955], + "StagingConfig": [-2.3252, 4.4562, 4.1676, 1.5326, -2.1835, -2.1952], + "StowLocation": [-1.5210, 2.6009, 0.3281, -4.0001, 0.2283, 3.8788], } - _LOGGER_INSTANCE.info( - f"Using {len(hardcoded_configs)} hardcoded named configurations for generating planning tasks." - ) - planners = ["RRTConnectkConfigDefault", "RRTstarkConfigDefault", "CHOMP"] initial_script_setup_config = ( list(hardcoded_configs.values())[0] if hardcoded_configs else [0.0] * len(kinova.joint_names()) ) - - planning_group_name = "jaco_arm" - ee_link_name = "j2n6s200_end_effector" - base_link_name = kinova.base_link_name() - group_joint_names = kinova.joint_names() + planning_group_name, ee_link_name, base_link_name, group_joint_names = ( + "jaco_arm", + kinova.end_effector_name(), + kinova.base_link_name(), + kinova.joint_names(), + ) planning_timeout = 15.0 - - base_output_dir = output_dir_arg if output_dir_arg else "." - if output_dir_arg and not os.path.exists(base_output_dir): + base_output_dir = ( + output_dir_arg + if output_dir_arg + else os.path.join(os.getcwd(), "benchmark_results") + ) + if not os.path.exists(base_output_dir): os.makedirs(base_output_dir) - _LOGGER_INSTANCE.info(f"Created base output directory: {base_output_dir}") trajectory_save_location = os.path.join( - base_output_dir, "saved_trajectories_pairwise_physical" - ) + base_output_dir, "saved_trajectories_enhanced" + ) # New subdir + if not os.path.exists(trajectory_save_location): + os.makedirs(trajectory_save_location) callback_group = ReentrantCallbackGroup() moveit2_interface = MoveIt2( @@ -1098,9 +1351,6 @@ def main(output_dir_arg: Optional[str]): moveit2_interface.allowed_planning_time = planning_timeout moveit2_interface.max_velocity_scaling_factor = 0.5 moveit2_interface.max_acceleration_scaling_factor = 0.5 - _LOGGER_INSTANCE.info( - f"MoveIt2 interface initialized for benchmark with group: '{planning_group_name}'" - ) benchmark_runner = PlannerBenchmark( node=node, @@ -1115,34 +1365,53 @@ def main(output_dir_arg: Optional[str]): planning_timeout_sec=planning_timeout, trajectory_save_dir=trajectory_save_location, joint_state_topic="/joint_states", + use_naive_jaco_hand_constraint=use_naive_constraint_arg, ) - try: num_total_tasks = benchmark_runner.num_tasks log_interval = max(1, num_total_tasks // 5 if num_total_tasks > 0 else 1) benchmark_runner.run_all_tests(log_summary_every_n_tasks=log_interval) - if not os.path.exists(base_output_dir) and base_output_dir != ".": - os.makedirs(base_output_dir) - _LOGGER_INSTANCE.info( - f"Created output directory for CSV: {base_output_dir}" - ) - elif base_output_dir == "." and not os.path.exists(base_output_dir): + if not os.path.exists(base_output_dir): os.makedirs(base_output_dir) + constraint_label = ( + "naive_constraint" if use_naive_constraint_arg else "no_hand_constraint" + ) csv_filename = os.path.join( base_output_dir, - datetime.now().strftime("%Y-%m-%d_%H-%M-%S") - + "_pairwise_physical_benchmark.csv", + datetime.now().strftime("%Y%m%d-%H%M%S") + + f"_benchmark_{constraint_label}.csv", ) benchmark_runner.write_results_to_csv(csv_filename) benchmark_runner.log_summary_results() except Exception as e: - _LOGGER_INSTANCE.error(f"Benchmark run failed: {e}") + _LOGGER_INSTANCE.error(f"Benchmark run failed: {e}", exc_info=True) finally: _LOGGER_INSTANCE.info("Shutting down benchmark node.") rclpy.shutdown() executor_thread.join() + _LOGGER_INSTANCE.info("Executor joined. Script finished.") if __name__ == "__main__": - out_dir_arg = os.path.expanduser(sys.argv[1]) if len(sys.argv) > 1 else None - main(out_dir_arg) + default_out_dir = os.path.join(os.getcwd(), "planner_benchmark_output_v3") + out_dir_arg = default_out_dir + use_naive_arg_str = "true" # Default + + if len(sys.argv) == 2: # Only one arg + if sys.argv[1].lower() in ["true", "use_naive", "naive"]: + use_naive_arg_str = "true" + elif sys.argv[1].lower() in ["false", "no_constraint", "none"]: + use_naive_arg_str = "false" + else: + out_dir_arg = os.path.expanduser(sys.argv[1]) # Assume it's output dir + elif len(sys.argv) > 2: # Two or more args + out_dir_arg = os.path.expanduser(sys.argv[1]) + if sys.argv[2].lower() in ["true", "use_naive", "naive"]: + use_naive_arg_str = "true" + elif sys.argv[2].lower() in ["false", "no_constraint", "none"]: + use_naive_arg_str = "false" + + use_naive_constraint_bool = use_naive_arg_str == "true" + _get_logger().info(f"Output directory set to: {out_dir_arg}") + _get_logger().info(f"Using naive Jaco hand constraint: {use_naive_constraint_bool}") + main(out_dir_arg, use_naive_constraint_bool) diff --git a/ada_feeding/scripts/planner_benchmark_analysis.py b/ada_feeding/scripts/planner_benchmark_analysis.py new file mode 100755 index 00000000..76ada504 --- /dev/null +++ b/ada_feeding/scripts/planner_benchmark_analysis.py @@ -0,0 +1,388 @@ +#!/usr/bin/env python3 + +""" +Analyzes the CSV output from the PlannerBenchmark script. +Generates summary statistics and comparative plots for different planners. +""" + +import pandas as pd +import numpy as np +import matplotlib.pyplot as plt +import seaborn as sns +import argparse +import os +from typing import List, Dict, Any + +# Define expected numeric columns that might need conversion and could contain NaN-like strings +NUMERIC_COLUMNS = [ + "elapsed_time_s", + "jaco_plan_success", + "path_length_total", + "max_jaco_hand_roll_deviation_rad", + "articutool_path_feasible", + "articutool_num_infeasible_points", + "articutool_min_pitch_rad", + "articutool_max_pitch_rad", + "articutool_avg_pitch_abs_rad", + "articutool_pitch_range_used_percent", + "articutool_min_roll_rad", + "articutool_max_roll_rad", + "articutool_avg_roll_abs_rad", + "articutool_roll_range_used_percent", +] + + +def load_and_preprocess_data(csv_filepath: str) -> pd.DataFrame: + """Loads data from CSV and preprocesses it.""" + print(f"Loading data from: {csv_filepath}") + try: + df = pd.read_csv(csv_filepath) + except FileNotFoundError: + print(f"Error: CSV file not found at {csv_filepath}") + raise + except Exception as e: + print(f"Error loading CSV: {e}") + raise + + print(f"Loaded {len(df)} rows.") + + # Identify path_length_joint columns dynamically + path_length_joint_cols = [ + col for col in df.columns if col.startswith("path_length_j") + ] + all_numeric_cols = NUMERIC_COLUMNS + path_length_joint_cols + + for col in all_numeric_cols: + if col in df.columns: + # Replace common non-numeric placeholders with NaN before attempting conversion + df[col] = df[col].replace( + ["N/A", "nan", "inf", "-inf", "ERROR", "ERROR_NO_AT_METRICS", ""], + np.nan, + ) + # Attempt to convert to numeric, coercing errors to NaN + df[col] = pd.to_numeric(df[col], errors="coerce") + else: + print( + f"Warning: Expected numeric column '{col}' not found in CSV. Skipping." + ) + + # Ensure boolean-like columns are 0 or 1 (or NaN) + for bool_col in ["jaco_plan_success", "articutool_path_feasible"]: + if bool_col in df.columns: + # After to_numeric, valid values should be 0.0, 1.0, or NaN. + # We can keep them as float or convert to Int64 (which supports NaN) + # For calculations like mean (success rate), float is fine. + pass + + print("Data preprocessing complete.") + return df + + +def calculate_summary_stats(df: pd.DataFrame) -> Dict[str, Dict[str, Any]]: + """Calculates summary statistics for each planner.""" + summary: Dict[str, Dict[str, Any]] = {} + planners = df["planner_id"].unique() + + for planner in planners: + planner_df = df[ + df["planner_id"] == planner + ].copy() # Use .copy() to avoid SettingWithCopyWarning + total_tasks = len(planner_df) + if total_tasks == 0: + summary[planner] = {"total_tasks": 0} + continue + + # Jaco Plan Success + jaco_successful_plans_df = planner_df[planner_df["jaco_plan_success"] == 1] + num_jaco_successful = len(jaco_successful_plans_df) + jaco_success_rate = ( + (num_jaco_successful / total_tasks) * 100 if total_tasks > 0 else 0 + ) + + # Articutool Path Feasibility (of Jaco successful plans) + articutool_feasible_df = pd.DataFrame() # Ensure it's defined + num_articutool_feasible = 0 + articutool_feasibility_rate = 0.0 # Default + if num_jaco_successful > 0: + articutool_feasible_df = jaco_successful_plans_df[ + jaco_successful_plans_df["articutool_path_feasible"] == 1 + ] + num_articutool_feasible = len(articutool_feasible_df) + articutool_feasibility_rate = ( + (num_articutool_feasible / num_jaco_successful) * 100 + if num_jaco_successful > 0 + else 0 + ) + + # Overall Success (Jaco success AND Articutool feasible) + overall_success_rate = ( + (num_articutool_feasible / total_tasks) * 100 if total_tasks > 0 else 0 + ) + + # Planning Time + avg_time_all = planner_df["elapsed_time_s"].mean() + avg_time_jaco_succ = ( + jaco_successful_plans_df["elapsed_time_s"].mean() + if num_jaco_successful > 0 + else np.nan + ) + avg_time_overall_succ = ( + articutool_feasible_df["elapsed_time_s"].mean() + if num_articutool_feasible > 0 + else np.nan + ) + + # Path Length + avg_path_len_jaco_succ = ( + jaco_successful_plans_df["path_length_total"].mean() + if num_jaco_successful > 0 + else np.nan + ) + + # Max Jaco Hand Roll Deviation + avg_jaco_roll_dev_jaco_succ = ( + jaco_successful_plans_df["max_jaco_hand_roll_deviation_rad"].mean() + if num_jaco_successful > 0 + else np.nan + ) + + # Articutool Metrics (for overall successful paths) + avg_at_pitch_range_overall_succ = ( + articutool_feasible_df["articutool_pitch_range_used_percent"].mean() + if num_articutool_feasible > 0 + else np.nan + ) + avg_at_roll_range_overall_succ = ( + articutool_feasible_df["articutool_roll_range_used_percent"].mean() + if num_articutool_feasible > 0 + else np.nan + ) + avg_at_infeasible_pts_overall_succ = ( + articutool_feasible_df["articutool_num_infeasible_points"].mean() + if num_articutool_feasible > 0 + else np.nan + ) + + summary[planner] = { + "total_tasks": total_tasks, + "num_jaco_successful": num_jaco_successful, + "jaco_success_rate_percent": jaco_success_rate, + "num_articutool_feasible_of_jaco_succ": num_articutool_feasible, # Corrected key + "articutool_feasibility_rate_percent": articutool_feasibility_rate, + "num_overall_successful": num_articutool_feasible, # Same as above, but for clarity in overall rate + "overall_success_rate_percent": overall_success_rate, + "avg_time_all_s": avg_time_all, + "avg_time_jaco_succ_s": avg_time_jaco_succ, + "avg_time_overall_succ_s": avg_time_overall_succ, + "avg_path_len_jaco_succ": avg_path_len_jaco_succ, + "avg_jaco_roll_dev_jaco_succ_rad": avg_jaco_roll_dev_jaco_succ, + "avg_at_pitch_range_overall_succ_percent": avg_at_pitch_range_overall_succ, + "avg_at_roll_range_overall_succ_percent": avg_at_roll_range_overall_succ, + "avg_at_infeasible_pts_overall_succ": avg_at_infeasible_pts_overall_succ, + } + return summary + + +def print_summary_report(summary_stats: Dict[str, Dict[str, Any]]): + """Prints a formatted summary report to the console.""" + print("\n--- Benchmark Analysis Report ---") + for planner, stats in summary_stats.items(): + print(f"\nPlanner: {planner} (Total Tasks: {stats.get('total_tasks', 0)})") + if stats.get("total_tasks", 0) == 0: + print(" No data for this planner.") + continue + + print( + f" Jaco Plan Success Rate: {stats.get('jaco_success_rate_percent', 0):.2f}% ({stats.get('num_jaco_successful', 0)}/{stats.get('total_tasks', 0)})" + ) + if stats.get("num_jaco_successful", 0) > 0: + print( + f" Articutool Feasibility (of Jaco succ.): {stats.get('articutool_feasibility_rate_percent', 0):.2f}% ({stats.get('num_articutool_feasible_of_jaco_succ', 0)}/{stats.get('num_jaco_successful', 0)})" + ) + else: + print( + " Articutool Feasibility (of Jaco succ.): N/A (No Jaco successful plans)" + ) + print( + f" Overall Task Success Rate: {stats.get('overall_success_rate_percent', 0):.2f}% ({stats.get('num_overall_successful', 0)}/{stats.get('total_tasks', 0)})" + ) + + print( + f" Avg. Planning Time (all attempts): {stats.get('avg_time_all_s', np.nan):.3f} s" + ) + print( + f" Avg. Planning Time (Jaco successful): {stats.get('avg_time_jaco_succ_s', np.nan):.3f} s" + ) + print( + f" Avg. Planning Time (Overall successful): {stats.get('avg_time_overall_succ_s', np.nan):.3f} s" + ) + + print( + f" Avg. Path Length (Jaco successful): {stats.get('avg_path_len_jaco_succ', np.nan):.3f}" + ) + print( + f" Avg. Max Jaco Hand Roll Dev (Jaco successful): {stats.get('avg_jaco_roll_dev_jaco_succ_rad', np.nan):.3f} rad" + ) + + if stats.get("num_overall_successful", 0) > 0: + print( + f" Avg. Articutool Pitch Range Used (Overall successful): {stats.get('avg_at_pitch_range_overall_succ_percent', np.nan):.2f}%" + ) + print( + f" Avg. Articutool Roll Range Used (Overall successful): {stats.get('avg_at_roll_range_overall_succ_percent', np.nan):.2f}%" + ) + print( + f" Avg. Articutool Infeasible Points (Overall successful): {stats.get('avg_at_infeasible_pts_overall_succ', np.nan):.2f}" + ) + else: + print(" Articutool Performance Metrics (Overall successful): N/A") + print("-----------------------------") + + +def generate_plots(df: pd.DataFrame, output_dir: str): + """Generates and saves comparative plots.""" + if not os.path.exists(output_dir): + os.makedirs(output_dir) + print(f"Created output directory for plots: {output_dir}") + + sns.set_theme(style="whitegrid") + planners = df["planner_id"].unique() + + # --- Success Rate Plots --- + success_data = [] + for planner in planners: + planner_df = df[df["planner_id"] == planner] + total = len(planner_df) + if total == 0: + continue + jaco_succ = planner_df["jaco_plan_success"].sum() + + # For Articutool feasibility, only consider Jaco successful plans + jaco_succ_df = planner_df[planner_df["jaco_plan_success"] == 1] + at_feasible_of_jaco_succ = ( + jaco_succ_df["articutool_path_feasible"].sum() + if not jaco_succ_df.empty + else 0 + ) + + success_data.append( + { + "planner": planner, + "type": "Jaco Plan Success", + "rate": (jaco_succ / total) * 100 if total else 0, + } + ) + success_data.append( + { + "planner": planner, + "type": "Articutool Feasible (of Jaco Succ.)", + "rate": ( + (at_feasible_of_jaco_succ / jaco_succ) * 100 if jaco_succ else 0 + ), + } + ) + success_data.append( + { + "planner": planner, + "type": "Overall Success", + "rate": (at_feasible_of_jaco_succ / total) * 100 if total else 0, + } + ) + + success_df = pd.DataFrame(success_data) + + plt.figure(figsize=(12, 7)) + sns.barplot(x="planner", y="rate", hue="type", data=success_df, palette="viridis") + plt.title("Planner Success Rates") + plt.ylabel("Success Rate (%)") + plt.xlabel("Planner ID") + plt.xticks(rotation=45, ha="right") + plt.legend(title="Success Type") + plt.tight_layout() + plt.savefig(os.path.join(output_dir, "success_rates.png")) + plt.close() + print(f"Saved plot: success_rates.png to {output_dir}") + + # --- Distribution Plots (Box Plots) --- + # Filter for Jaco successful plans for these metrics + jaco_successful_df = df[df["jaco_plan_success"] == 1].copy() + # Filter for overall successful plans for Articutool specific metrics + overall_successful_df = jaco_successful_df[ + jaco_successful_df["articutool_path_feasible"] == 1 + ].copy() + + plot_metrics = [ + ("elapsed_time_s", "Planning Time (Jaco Successful) (s)", jaco_successful_df), + ("path_length_total", "Path Length (Jaco Successful)", jaco_successful_df), + ( + "max_jaco_hand_roll_deviation_rad", + "Max Jaco Hand Roll Dev (Jaco Successful) (rad)", + jaco_successful_df, + ), + ( + "articutool_pitch_range_used_percent", + "Articutool Pitch Range Used (Overall Successful) (%)", + overall_successful_df, + ), + ( + "articutool_roll_range_used_percent", + "Articutool Roll Range Used (Overall Successful) (%)", + overall_successful_df, + ), + ] + + for metric_col, title, data_subset in plot_metrics: + if ( + metric_col not in data_subset.columns + or data_subset[metric_col].isnull().all() + ): + print(f"Skipping plot for '{title}': column missing or all NaN.") + continue + if data_subset.empty: + print(f"Skipping plot for '{title}': no data after filtering.") + continue + + plt.figure(figsize=(10, 6)) + sns.boxplot(x="planner_id", y=metric_col, data=data_subset, palette="pastel") + plt.title(title) + plt.ylabel(metric_col.replace("_", " ").title()) + plt.xlabel("Planner ID") + plt.xticks(rotation=45, ha="right") + plt.tight_layout() + plt.savefig(os.path.join(output_dir, f"{metric_col}_distribution.png")) + plt.close() + print(f"Saved plot: {metric_col}_distribution.png to {output_dir}") + + print("Plot generation complete.") + + +def main(): + parser = argparse.ArgumentParser( + description="Analyze Planner Benchmark CSV results." + ) + parser.add_argument( + "csv_filepath", type=str, help="Path to the benchmark CSV file." + ) + parser.add_argument( + "--output_dir", + type=str, + default="benchmark_analysis_plots", + help="Directory to save generated plots (default: benchmark_analysis_plots).", + ) + args = parser.parse_args() + + try: + data_df = load_and_preprocess_data(args.csv_filepath) + summary_stats = calculate_summary_stats(data_df) + print_summary_report(summary_stats) + generate_plots(data_df, args.output_dir) + print("\nAnalysis complete.") + except Exception as e: + print(f"An error occurred during analysis: {e}") + import traceback + + traceback.print_exc() + + +if __name__ == "__main__": + main() diff --git a/ada_feeding/scripts/visualize_trajectory.py b/ada_feeding/scripts/visualize_trajectory.py index 2e0db490..a7c7892d 100755 --- a/ada_feeding/scripts/visualize_trajectory.py +++ b/ada_feeding/scripts/visualize_trajectory.py @@ -1,7 +1,7 @@ #!/usr/bin/env python3 import pinocchio as pin -import pinocchio.visualize +import pinocchio.visualize # Ensure this is imported for MeshcatVisualizer import meshcat import numpy as np import json @@ -12,7 +12,12 @@ import time import sys -from typing import Optional +from typing import Optional, Dict, Any, List # Added List + +# Define Articutool joint names as they appear in the Pinocchio model +# (Verify these names from the script's "Pinocchio Model Joint Details" printout) +ARTICUTOOL_PITCH_JOINT_NAME = "atool_joint1" +ARTICUTOOL_ROLL_JOINT_NAME = "atool_joint2" def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str]: @@ -23,7 +28,7 @@ def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str logger_func(f"Processing Xacro file: {xacro_filename}") try: - ros_distro = os.environ.get("ROS_DISTRO", "your_ros2_distro") + # Attempt to find ros2 executable, assuming it's in PATH process = subprocess.run( ["ros2", "run", "xacro", "xacro", xacro_filename], check=True, @@ -33,8 +38,10 @@ def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str urdf_xml_string = process.stdout logger_func("XACRO processing successful.") return urdf_xml_string - except FileNotFoundError as e: - logger_func(f"Fatal: Command 'ros2 run xacro ...' failed. Is xacro installed ") + except FileNotFoundError: + logger_func( + "Fatal: Command 'ros2' not found. Make sure ROS 2 environment is sourced." + ) return None except subprocess.CalledProcessError as e: logger_func( @@ -50,40 +57,68 @@ def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=print): """Loads a Pinocchio model from a URDF XML string via a temporary file.""" - temp_urdf_path = "" # Initialize to ensure it's defined in finally + temp_urdf_path = "" try: with tempfile.NamedTemporaryFile( mode="w", suffix=".urdf", delete=False ) as temp_file: temp_urdf_path = temp_file.name temp_file.write(urdf_xml_string) - logger_func(f"Generated temporary URDF file: {temp_urdf_path}") - # Attempt to guess package directories if ROS_PACKAGE_PATH is set package_dirs = [] ros_package_path = os.environ.get("ROS_PACKAGE_PATH") if ros_package_path: - package_dirs = [p for p in ros_package_path.split(":") if os.path.isdir(p)] + package_dirs = [ + p for p in ros_package_path.split(os.pathsep) if os.path.isdir(p) + ] # Use os.pathsep + + # Add common workspace paths if specific package_dirs are not found or to supplement + # This is a heuristic and might need adjustment based on your workspace structure + common_workspace_paths = [ + os.path.expanduser("~/ros2_ws/src"), + os.path.expanduser("~/workspace/src"), + # Add other common workspace locations if necessary + ] + for wsp in common_workspace_paths: + if os.path.isdir(wsp) and wsp not in package_dirs: + # Pinocchio expects a list of directories containing packages, not the package directories themselves + # So, if wsp is like '/home/user/ros2_ws/src', it's correct. + package_dirs.append(wsp) + + # If the current working directory's parent might be a workspace 'src' + # This helps if running from within a package. + current_ws_src_path = os.path.abspath( + os.path.join(os.getcwd(), "..", "..", "src") + ) + if ( + os.path.isdir(current_ws_src_path) + and current_ws_src_path not in package_dirs + ): + package_dirs.append(current_ws_src_path) if package_dirs: - logger_func(f"Using package_dirs for Pinocchio: {package_dirs}") - model = pin.buildModelFromUrdf(temp_urdf_path, package_dirs=package_dirs) + logger_func( + f"Using package_dirs for Pinocchio: {list(set(package_dirs))}" + ) # Show unique dirs + model = pin.buildModelFromUrdf( + temp_urdf_path, package_dirs=list(set(package_dirs)) + ) collision_model = pin.buildGeomFromUrdf( model, temp_urdf_path, pin.GeometryType.COLLISION, - package_dirs=package_dirs, + package_dirs=list(set(package_dirs)), ) visual_model = pin.buildGeomFromUrdf( model, temp_urdf_path, pin.GeometryType.VISUAL, - package_dirs=package_dirs, + package_dirs=list(set(package_dirs)), ) else: logger_func( - "Warning: ROS_PACKAGE_PATH not found or empty. Mesh loading might fail if using package:// paths without it." + "Warning: ROS_PACKAGE_PATH not found or empty. Mesh loading might fail if using package:// paths." ) model = pin.buildModelFromUrdf(temp_urdf_path) collision_model = pin.buildGeomFromUrdf( @@ -103,17 +138,24 @@ def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=prin return None, None, None, None finally: if temp_urdf_path and os.path.exists(temp_urdf_path): - # For debugging URDF, comment out the next line - # logger_func(f"Temporary URDF file for debugging: {temp_urdf_path}") + # logger_func(f"Temporary URDF file for debugging: {temp_urdf_path}") # Uncomment to keep file os.remove(temp_urdf_path) - # logger_func(f"Cleaned up temporary URDF file: {temp_urdf_path}") -def load_trajectory_from_json(filepath: str, logger_func=print): +def load_enhanced_trajectory_from_json( + filepath: str, logger_func=print +) -> Optional[Dict[str, Any]]: + """Loads the enhanced trajectory data from a JSON file.""" try: with open(filepath, "r") as f: traj_data = json.load(f) - logger_func(f"Trajectory loaded from {filepath}") + logger_func(f"Enhanced trajectory loaded from {filepath}") + # Basic validation for the new structure + if "jaco_joint_names" not in traj_data or "waypoints" not in traj_data: + logger_func( + "Error: Loaded JSON is missing 'jaco_joint_names' or 'waypoints'." + ) + return None return traj_data except FileNotFoundError: logger_func(f"Error: Trajectory file not found at {filepath}") @@ -124,8 +166,26 @@ def load_trajectory_from_json(filepath: str, logger_func=print): return None +def get_pinocchio_joint_info( + model: pin.Model, joint_name: str +) -> Optional[Dict[str, Any]]: + """Gets q_idx_start, nq, and nv for a named joint in the Pinocchio model.""" + if model.existJointName(joint_name): + joint_id = model.getJointId(joint_name) + joint_obj = model.joints[joint_id] + if joint_obj.nq > 0: # Only consider actuated joints + return { + "name": joint_name, + "q_idx_start": joint_obj.idx_q, + "nq": joint_obj.nq, + "nv": joint_obj.nv, + "id": joint_id, + } + return None + + def main(xacro_file: str, trajectory_file: str): - print("--- Pinocchio + MeshCat Trajectory Visualizer ---") + print("--- Pinocchio + MeshCat Trajectory Visualizer (Enhanced) ---") urdf_string = xacro_to_urdf_string(xacro_file) if not urdf_string: @@ -139,161 +199,192 @@ def main(xacro_file: str, trajectory_file: str): model, collision_model, visual_model, data = load_result if model: - print("\n--- Pinocchio Model Joint Details (from model.joints) ---") - for i in range(model.njoints): - joint_obj = model.joints[i] + print("\n--- Pinocchio Model Joint Details ---") + for i in range(1, model.njoints): # Start from 1 to skip universe joint_name_in_model = model.names[i] + joint_obj = model.joints[i] print( - f"IdxInModel {i}: Name='{joint_name_in_model}', PinocchioShortName='{joint_obj.shortname()}', " - f"idx_q={joint_obj.idx_q}, nq={joint_obj.nq}, " - f"idx_v={joint_obj.idx_v}, nv={joint_obj.nv}, " - f"idInModel={joint_obj.id}" + f"IdxInModel {i}: Name='{joint_name_in_model}', PinocchioType='{joint_obj.shortname()}', " + f"idx_q={joint_obj.idx_q if joint_obj.idx_q >= 0 else 'N/A'}, nq={joint_obj.nq}, " + f"idx_v={joint_obj.idx_v if joint_obj.idx_v >= 0 else 'N/A'}, nv={joint_obj.nv}" ) - type_name = type(joint_obj).__name__ - # More specific checks for common types that might have nq > 1 - if isinstance(joint_obj, pin.JointModelFreeFlyer): - type_name = "JointModelFreeFlyer (nq=7,nv=6)" - elif isinstance(joint_obj, pin.JointModelSphericalZYX): - type_name = "JointModelSphericalZYX (nq=3,nv=3)" - elif isinstance(joint_obj, pin.JointModelSpherical): - type_name = "JointModelSpherical (nq=4,nv=3)" - elif isinstance(joint_obj, pin.JointModelPlanar): - type_name = "JointModelPlanar (nq=3,nv=3)" - elif isinstance(joint_obj, pin.JointModelTranslation): - type_name = "JointModelTranslation (nq=3,nv=3)" - # Check for unaligned revolute joints (often used for continuous) - elif ( - "Rx" in type_name - or "Ry" in type_name - or "Rz" in type_name - or "RevoluteUnaligned" in type_name - ): - if joint_obj.nq == 2 and joint_obj.nv == 1: - type_name += " (Likely Continuous cos/sin)" - print(f" Type: {type_name}") - print("-------------------------------------------------------\n") + print("-----------------------------------\n") print("Initializing MeshCat viewer... Waiting for connection.") try: - visualizer = meshcat.Visualizer() + visualizer = ( + meshcat.Visualizer().open() + ) # open=True is default if not passed to initViewer + # visualizer.wait() # Wait for browser to connect print(f"MeshCat viewer URL: {visualizer.url()}") pin_viz = pin.visualize.MeshcatVisualizer(model, collision_model, visual_model) - pin_viz.initViewer(viewer=visualizer, open=True) + pin_viz.initViewer(viewer=visualizer) # Already opened pin_viz.loadViewerModel( rootNodeName=model.name if model.name else "pinocchio_robot" ) print("MeshCat viewer initialized and model loaded.") except Exception as e: print(f"Error initializing MeshCat or Pinocchio visualizer: {e}") - print( - "Please ensure MeshCat is installed and accessible (e.g., `pip install meshcat`)." - ) return - trajectory_data = load_trajectory_from_json(trajectory_file) - if ( - not trajectory_data - or "joint_names" not in trajectory_data - or "points" not in trajectory_data - ): + trajectory_data = load_enhanced_trajectory_from_json(trajectory_file) + if not trajectory_data: print("Failed to load valid trajectory data. Exiting.") return - traj_joint_names = trajectory_data["joint_names"] - traj_points = trajectory_data["points"] - if not traj_points: - print("Trajectory contains no points. Exiting.") + jaco_joint_names_from_traj = trajectory_data["jaco_joint_names"] + waypoints_data = trajectory_data["waypoints"] + if not waypoints_data: + print("Trajectory contains no waypoints. Exiting.") return q = pin.neutral(model) print( - f"Neutral configuration q (size {model.nq}): {q.T if model.nq > 0 else 'N/A (nq=0)'}" + f"Neutral configuration q (size {model.nq}): {q.T if model.nq > 0 else 'N/A'}" ) - mapped_joint_info_for_trajectory = [] - missing_joints_in_model = [] - - for name_in_traj in traj_joint_names: - if model.existJointName(name_in_traj): - joint_id = model.getJointId(name_in_traj) - joint_obj = model.joints[joint_id] - if joint_obj.nq > 0: - mapped_joint_info_for_trajectory.append( - { - "name": name_in_traj, - "q_idx_start": joint_obj.idx_q, - "nq": joint_obj.nq, - "nv": joint_obj.nv, - } - ) - else: - print( - f"Info: Joint '{name_in_traj}' from trajectory is type '{joint_obj.shortname()}' (nq=0) in model. Will not be actuated." - ) - mapped_joint_info_for_trajectory.append(None) - else: - missing_joints_in_model.append(name_in_traj) - mapped_joint_info_for_trajectory.append(None) + # Map Jaco joint names from trajectory to Pinocchio model indices + jaco_joint_mappings: List[Optional[Dict[str, Any]]] = [] + for name_in_traj in jaco_joint_names_from_traj: + jaco_joint_mappings.append(get_pinocchio_joint_info(model, name_in_traj)) - if missing_joints_in_model: + if any(m is None for m in jaco_joint_mappings): print( - f"Warning: Trajectory joints NOT found in Pinocchio model: {missing_joints_in_model}" + "Warning: Some Jaco joints from trajectory not found or not actuated in model:" ) + for i, name in enumerate(jaco_joint_names_from_traj): + if jaco_joint_mappings[i] is None: + print(f" - {name}") - valid_mappings = [ - info for info in mapped_joint_info_for_trajectory if info is not None - ] - if not valid_mappings: + # Get Pinocchio info for Articutool joints + articutool_pitch_joint_info = get_pinocchio_joint_info( + model, ARTICUTOOL_PITCH_JOINT_NAME + ) + articutool_roll_joint_info = get_pinocchio_joint_info( + model, ARTICUTOOL_ROLL_JOINT_NAME + ) + + if not articutool_pitch_joint_info: + print( + f"Error: Articutool pitch joint '{ARTICUTOOL_PITCH_JOINT_NAME}' not found or not actuated in model. Exiting." + ) + return + if not articutool_roll_joint_info: print( - "Error: None of the trajectory joints could be mapped to actuated joints in Pinocchio model. Exiting." + f"Error: Articutool roll joint '{ARTICUTOOL_ROLL_JOINT_NAME}' not found or not actuated in model. Exiting." ) return - print("\nMapped info for trajectory joints:") - for i, info in enumerate(mapped_joint_info_for_trajectory): - if info: - print( - f" Traj Joint {i} ('{info['name']}'): Maps to q[{info['q_idx_start']}-" - f"{info['q_idx_start'] + info['nq'] - 1}], ModelJoint_nq={info['nq']}, ModelJoint_nv={info['nv']}" - ) + print("\nArticutool Joint Mappings:") + print( + f" Pitch ('{ARTICUTOOL_PITCH_JOINT_NAME}'): Maps to q[{articutool_pitch_joint_info['q_idx_start']}], nq={articutool_pitch_joint_info['nq']}" + ) + print( + f" Roll ('{ARTICUTOOL_ROLL_JOINT_NAME}'): Maps to q[{articutool_roll_joint_info['q_idx_start']}], nq={articutool_roll_joint_info['nq']}" + ) + + def set_q_from_waypoint(q_vector: np.ndarray, waypoint: Dict[str, Any]): + """Updates q_vector with Jaco and Articutool positions from a waypoint.""" + # Set Jaco joints + jaco_positions = waypoint.get("jaco_positions_rad", []) + for k, mapping_info in enumerate(jaco_joint_mappings): + if mapping_info and k < len(jaco_positions): + theta_traj = jaco_positions[k] + q_idx_start, nq, nv = ( + mapping_info["q_idx_start"], + mapping_info["nq"], + mapping_info["nv"], + ) + if nq == 1: + q_vector[q_idx_start] = theta_traj + elif nq == 2 and nv == 1: # Continuous (cos/sin) + q_vector[q_idx_start] = np.cos(theta_traj) + q_vector[q_idx_start + 1] = np.sin(theta_traj) + + # Set Articutool joints + if waypoint.get("articutool_waypoint_feasible", False): + pitch_sol = waypoint.get("articutool_pitch_solution_rad") + roll_sol = waypoint.get("articutool_roll_solution_rad") + + if pitch_sol is not None and articutool_pitch_joint_info: + q_idx = articutool_pitch_joint_info["q_idx_start"] + # Assuming nq=1 for these revolute joints + if articutool_pitch_joint_info["nq"] == 1: + q_vector[q_idx] = pitch_sol + elif ( + articutool_pitch_joint_info["nq"] == 2 + and articutool_pitch_joint_info["nv"] == 1 + ): # cos/sin + q_vector[q_idx] = np.cos(pitch_sol) + q_vector[q_idx + 1] = np.sin(pitch_sol) + + if roll_sol is not None and articutool_roll_joint_info: + q_idx = articutool_roll_joint_info["q_idx_start"] + if articutool_roll_joint_info["nq"] == 1: + q_vector[q_idx] = roll_sol + elif ( + articutool_roll_joint_info["nq"] == 2 + and articutool_roll_joint_info["nv"] == 1 + ): # cos/sin + q_vector[q_idx] = np.cos(roll_sol) + q_vector[q_idx + 1] = np.sin(roll_sol) else: - print( - f" Traj Joint {i} ('{traj_joint_names[i]}'): Not mapped or not actuated." - ) + # Articutool not feasible at this waypoint. + # Optionally, set Articutool joints to a default/neutral pose (e.g., 0.0) + # This provides a visual cue. If not set, they retain previous values. + if articutool_pitch_joint_info: + q_idx = articutool_pitch_joint_info["q_idx_start"] + if articutool_pitch_joint_info["nq"] == 1: + q_vector[q_idx] = 0.0 + elif ( + articutool_pitch_joint_info["nq"] == 2 + and articutool_pitch_joint_info["nv"] == 1 + ): + q_vector[q_idx] = np.cos(0.0) + q_vector[q_idx + 1] = np.sin(0.0) + if articutool_roll_joint_info: + q_idx = articutool_roll_joint_info["q_idx_start"] + if articutool_roll_joint_info["nq"] == 1: + q_vector[q_idx] = 0.0 + elif ( + articutool_roll_joint_info["nq"] == 2 + and articutool_roll_joint_info["nv"] == 1 + ): + q_vector[q_idx] = np.cos(0.0) + q_vector[q_idx + 1] = np.sin(0.0) + # print(f" Articutool not feasible at this waypoint. Setting its joints to 0.") # Initial display - current_q_display = q.copy() - if traj_points: - initial_positions = traj_points[0]["positions"] - for k, mapping_info in enumerate(mapped_joint_info_for_trajectory): - if mapping_info and k < len(initial_positions): - theta_traj = initial_positions[k] - q_idx_start = mapping_info["q_idx_start"] - joint_nq = mapping_info["nq"] - joint_nv = mapping_info["nv"] - if joint_nq == 1: # Standard revolute/prismatic (includes nv=0 or nv=1) - current_q_display[q_idx_start] = theta_traj - elif joint_nq == 2 and joint_nv == 1: # Continuous (cos/sin) - current_q_display[q_idx_start] = np.cos(theta_traj) - current_q_display[q_idx_start + 1] = np.sin(theta_traj) + current_q_display = q.copy() # Start with neutral + if waypoints_data: + set_q_from_waypoint(current_q_display, waypoints_data[0]) pin_viz.display(current_q_display) q[:] = current_q_display[:] # Update main q current_point_idx = 0 - num_trajectory_points = len(traj_points) + num_trajectory_points = len(waypoints_data) print("\n--- Trajectory Control ---") print("Open the MeshCat URL in your browser.") running = True while running: - print(f"\nPoint: {current_point_idx + 1}/{num_trajectory_points}") + wp_info = waypoints_data[current_point_idx] + at_feasible = wp_info.get("articutool_waypoint_feasible", False) + at_pitch = wp_info.get("articutool_pitch_solution_rad", "N/A") + at_roll = wp_info.get("articutool_roll_solution_rad", "N/A") + if isinstance(at_pitch, float): + at_pitch = f"{at_pitch:.3f}" + if isinstance(at_roll, float): + at_roll = f"{at_roll:.3f}" + + print( + f"\nPoint: {current_point_idx + 1}/{num_trajectory_points} | Articutool Feasible: {at_feasible} (P: {at_pitch}, R: {at_roll})" + ) print("Commands: [n]ext, [p]rev, [f]irst, [l]ast, [g ], [a]nimate, [q]uit") try: user_input = input("Enter command: ").strip().lower() - new_q_to_display = ( - q.copy() - ) # Start with current q, only update relevant parts + new_q_to_display = q.copy() if user_input == "q": running = False @@ -327,58 +418,30 @@ def main(xacro_file: str, trajectory_file: str): try: for i in range(start_anim_idx, num_trajectory_points): current_point_idx = i - positions_to_set = traj_points[current_point_idx]["positions"] - for k, mapping_info in enumerate( - mapped_joint_info_for_trajectory - ): - if mapping_info and k < len(positions_to_set): - theta_traj = positions_to_set[k] - q_idx_start = mapping_info["q_idx_start"] - joint_nq = mapping_info["nq"] - joint_nv = mapping_info["nv"] - if joint_nq == 1: - anim_q[q_idx_start] = theta_traj - elif joint_nq == 2 and joint_nv == 1: - anim_q[q_idx_start] = np.cos(theta_traj) - anim_q[q_idx_start + 1] = np.sin(theta_traj) + set_q_from_waypoint(anim_q, waypoints_data[current_point_idx]) pin_viz.display(anim_q) print( f" Displaying point {current_point_idx + 1}/{num_trajectory_points}", end="\r", flush=True, ) - time.sleep(0.05) + time.sleep(0.05) # Animation speed print("\nAnimation finished. ") except KeyboardInterrupt: print("\nAnimation stopped. ") - new_q_to_display[:] = anim_q[:] # Update with last animated state + new_q_to_display[:] = anim_q[:] q[:] = new_q_to_display[:] - pin_viz.display(q) # Ensure final state is displayed once - continue # Skip re-applying single point after animation + pin_viz.display(q) + continue elif user_input: print("Invalid command.") continue else: - continue # Empty input, re-prompt - - # Update new_q_to_display for the current_point_idx - if traj_points and 0 <= current_point_idx < num_trajectory_points: - positions_to_set = traj_points[current_point_idx]["positions"] - for k, mapping_info in enumerate(mapped_joint_info_for_trajectory): - if mapping_info and k < len(positions_to_set): - theta_traj = positions_to_set[k] - q_idx_start = mapping_info["q_idx_start"] - joint_nq = mapping_info["nq"] - joint_nv = mapping_info["nv"] - if joint_nq == 1: - new_q_to_display[q_idx_start] = theta_traj - elif joint_nq == 2 and joint_nv == 1: - new_q_to_display[q_idx_start] = np.cos(theta_traj) - new_q_to_display[q_idx_start + 1] = np.sin(theta_traj) - # else: # Other nq/nv cases are not set + continue + set_q_from_waypoint(new_q_to_display, waypoints_data[current_point_idx]) pin_viz.display(new_q_to_display) - q[:] = new_q_to_display[:] # Persist the displayed q + q[:] = new_q_to_display[:] except EOFError: print("\nEOF received, quitting.") @@ -393,7 +456,7 @@ def main(xacro_file: str, trajectory_file: str): if __name__ == "__main__": parser = argparse.ArgumentParser( - description="Visualize saved JointTrajectory using Pinocchio and MeshCat." + description="Visualize enhanced JointTrajectory using Pinocchio and MeshCat." ) parser.add_argument( "xacro_file", @@ -401,7 +464,7 @@ def main(xacro_file: str, trajectory_file: str): help="Path to the robot XACRO file (that instantiates the robot).", ) parser.add_argument( - "trajectory_file", type=str, help="Path to the .json trajectory file." + "trajectory_file", type=str, help="Path to the .json ENHANCED trajectory file." ) args = parser.parse_args() main(args.xacro_file, args.trajectory_file) From 4624a51d68576db3df7bb612cdbcfdce5c26f7ef Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 15 May 2025 08:42:53 -0700 Subject: [PATCH 064/238] Remove wrist constraints for MoveAbove IK solution --- .../ada_feeding/trees/acquire_food_tree.py | 78 +++++++++---------- 1 file changed, 39 insertions(+), 39 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 1b4e6cf4..9211f625 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -244,7 +244,9 @@ def create_tree( "group_name": "articutool", }, outputs={ - "trajectory": BlackboardKey("home_articutool_trajectory") + "trajectory": BlackboardKey( + "home_articutool_trajectory" + ) }, ), ), @@ -263,9 +265,7 @@ def create_tree( "action_result_code": BlackboardKey( "tool_exec_result_code" ), - "action_status": BlackboardKey( - "tool_action_status" - ), + "action_status": BlackboardKey("tool_action_status"), }, ), SwitchArticutoolControllers( @@ -273,12 +273,14 @@ def create_tree( ns=name, inputs={ "controllers_to_activate": ["velocity_controller"], - "controllers_to_deactivate": ["joint_trajectory_controller"], + "controllers_to_deactivate": [ + "joint_trajectory_controller" + ], }, outputs={ "switch_call_succeeded": None, "switch_response_ok": None, - } + }, ), CallSetOrientationControl( name="SetArticutoolOrientation", @@ -287,9 +289,7 @@ def create_tree( "enable": True, "quat_xyzw": [0.5, 0.5, 0.5, 0.5], }, - outputs={ - - }, + outputs={}, ), MoveIt2JointConstraint( name="RestingConstraint", @@ -583,20 +583,6 @@ def move_above_plan( ) }, ), - MoveIt2OrientationConstraint( - name="DefineIKWristRollConstraint", - ns=name, - inputs={ - "target_link": "j2n6s200_end_effector", - "frame_id": "j2n6s200_link_base", - "quat_xyzw": [0.0, 0.0, 0.0, 1.0], - "tolerance": [2*np.pi, 2*np.pi, 0.5], - "constraints": None, - }, - outputs={ - "constraints": BlackboardKey("wrist_constraints"), - } - ), # Get Current Joint State (for IK Seed) py_trees.decorators.Timeout( name="GetCurrentState", @@ -636,7 +622,7 @@ def move_above_plan( ), "group_name": "jaco_arm_with_articutool", "start_joint_state": BlackboardKey("current_joint_state"), - "constraints": BlackboardKey("wrist_constraints"), + "constraints": None, }, outputs={ "ik_solution_joint_state": BlackboardKey( @@ -871,7 +857,7 @@ def move_above_plan( outputs={ "fk_poses": BlackboardKey("move_into_jaco_arm_ee_poses"), "success": None, - } + }, ), ExtractPoseFromPosesByLink( name="GetJacoArmEEPose", @@ -882,9 +868,11 @@ def move_above_plan( "requested_link_names": ["j2n6s200_end_effector"], }, outputs={ - "extracted_pose": BlackboardKey("move_into_jaco_arm_ee_pose"), + "extracted_pose": BlackboardKey( + "move_into_jaco_arm_ee_pose" + ), "success": None, - } + }, ), MoveIt2PoseConstraint( name="MoveIntoJacoArmEEPoseConstraint", @@ -1119,37 +1107,49 @@ def move_above_plan( name="SwitchArticutoolToVelocity", ns=name, inputs={ - "controllers_to_activate": ["velocity_controller"], - "controllers_to_deactivate": ["joint_trajectory_controller"], + "controllers_to_activate": [ + "velocity_controller" + ], + "controllers_to_deactivate": [ + "joint_trajectory_controller" + ], }, outputs={ "switch_call_succeeded": None, "switch_response_ok": None, - } + }, ), ExtractPoseComponents( name="GetMoveIntoOrientation", ns=name, inputs={ - "input_pose_object": BlackboardKey("move_into_pose_stamped_base_frame"), + "input_pose_object": BlackboardKey( + "move_into_pose_stamped_base_frame" + ), }, outputs={ - "output_position": BlackboardKey("move_into_position"), - "output_orientation": BlackboardKey("move_into_orientation"), - "output_header": BlackboardKey("move_into_header"), + "output_position": BlackboardKey( + "move_into_position" + ), + "output_orientation": BlackboardKey( + "move_into_orientation" + ), + "output_header": BlackboardKey( + "move_into_header" + ), "success": None, - } + }, ), CallSetOrientationControl( name="SetArticutoolOrientation", ns=name, inputs={ "enable": True, - "quat_xyzw": BlackboardKey("move_into_orientation"), - }, - outputs={ - + "quat_xyzw": BlackboardKey( + "move_into_orientation" + ), }, + outputs={}, ), # MoveInto expect F/T failure py_trees.decorators.FailureIsSuccess( From c5db4a164f73aaf6f21a1bad629caa3047bb68db Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 15 May 2025 15:52:13 -0700 Subject: [PATCH 065/238] Add behavior to measure the manipulability of the Jaco arm along a particular direction defined by two nominal poses. We use this to ensure that the Jaco arm has sufficient manipulability to perform a Cartesian motion in the direction from the MoveAbove pose to the MoveInto pose --- .../ada_feeding/behaviors/state/__init__.py | 4 + .../check_jaco_directional_manipulability.py | 485 ++++++++++++++++++ 2 files changed, 489 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py index 86571439..97e5f21e 100644 --- a/ada_feeding/ada_feeding/behaviors/state/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -4,6 +4,7 @@ """ This package contains custom py_tree behaviors for interacting with ROS. """ + from .get_joint_states import ( GetJointStates, ) @@ -19,3 +20,6 @@ from .extract_pose_components import ( ExtractPoseComponents, ) +from .check_jaco_directional_manipulability import ( + CheckJacoDirectionalManipulability, +) diff --git a/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py b/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py new file mode 100644 index 00000000..6fc9bdad --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py @@ -0,0 +1,485 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +Defines the CheckJacoDirectionalManipulability behavior. +This behavior evaluates if the Jaco arm, at a given configuration, +has sufficient translational manipulability for its end-effector to move +along a specified Cartesian direction (derived from desired tool tip motion). +""" + +# Standard imports +import os +import subprocess +import tempfile +import math # Added for cos/sin +from typing import Union, Optional, List, Dict, Any + +# Third-party imports +from geometry_msgs.msg import PoseStamped +from sensor_msgs.msg import JointState +import numpy as np +from overrides import override +import pinocchio as pin +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status +import rclpy.node +import rclpy.time + +# Local imports +from ada_feeding.helpers import BlackboardKey # Assuming this is your helper +from ada_feeding.behaviors import BlackboardBehavior # Assuming this is your base class + + +class CheckJacoDirectionalManipulability(BlackboardBehavior): + """ + Checks the Jaco arm's directional translational manipulability. + The manipulability is assessed for the Jaco end-effector at the current + "Move Above" configuration, for moving in a direction derived from the + desired tool tip motion (from tool_tip_move_above_pose to tool_tip_move_into_pose). + """ + + def blackboard_inputs( + self, + current_full_robot_joint_state_MA: Union[ + BlackboardKey, JointState + ], # Full state for Pinocchio FK/Jacobian + tool_tip_move_above_pose_world: Union[BlackboardKey, PoseStamped], + tool_tip_move_into_pose_world: Union[BlackboardKey, PoseStamped], + jaco_joint_names: Union[BlackboardKey, List[str]], + jaco_end_effector_link_name: Union[ + BlackboardKey, str + ], # Link whose manipulability is checked + directional_manipulability_threshold: Union[BlackboardKey, float], + urdf_file_path: Union[BlackboardKey, str], + ) -> None: + """ + Blackboard Inputs + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + jaco_directional_manipulability_score: Optional[ + BlackboardKey + ], # -> Optional[float] + jaco_is_manipulable_for_direction: Optional[BlackboardKey], # -> Optional[bool] + ) -> None: + """ + Blackboard Outputs + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def __init__(self, name: str, **kwargs): + super().__init__(name=name, **kwargs) + self.pin_model: Optional[pin.Model] = None + self.pin_data: Optional[pin.Data] = None + self.jaco_pin_vel_indices: List[int] = [] + self.jaco_ee_frame_id: Optional[int] = None + self.node: Optional[rclpy.node.Node] = None + self._urdf_loaded_successfully = False + + @override + def setup(self, **kwargs): + """Loads the Pinocchio model and gets joint/frame IDs.""" + try: + self.node = kwargs["node"] + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + return + + if self._urdf_loaded_successfully: # Avoid reloading if already done + self.logger.debug(f"[{self.name}] Pinocchio model already loaded.") + return + + urdf_path_str: str = self.blackboard_get("urdf_file_path") + if not urdf_path_str: + self.logger.error( + f"[{self.name}] 'urdf_file_path' not provided or is empty." + ) + return + + resolved_urdf_path = self._resolve_package_path(urdf_path_str) + if not resolved_urdf_path: + return # Error already logged by _resolve_package_path + + processed_urdf_path = resolved_urdf_path + temp_urdf_file_name = None # Store name for cleanup + + try: + if resolved_urdf_path.endswith(".xacro"): + self.logger.info( + f"[{self.name}] Processing XACRO file: {resolved_urdf_path}" + ) + with tempfile.NamedTemporaryFile( + mode="w", suffix=".urdf", delete=False + ) as temp_file: + temp_urdf_file_name = temp_file.name + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", resolved_urdf_path], + check=True, + capture_output=True, + text=True, + ) + temp_file.write(process.stdout) + processed_urdf_path = temp_urdf_file_name + + self.logger.info(f"[{self.name}] Loading URDF from: {processed_urdf_path}") + self.pin_model = pin.buildModelFromUrdf(processed_urdf_path) + self.pin_data = self.pin_model.createData() + self.logger.info( + f"[{self.name}] Pinocchio model loaded: {self.pin_model.name}, Nq={self.pin_model.nq}, Nv={self.pin_model.nv}" + ) + + jaco_joint_names_list: List[str] = self.blackboard_get("jaco_joint_names") + if not jaco_joint_names_list: + self.logger.error( + f"[{self.name}] 'jaco_joint_names' list is empty or not provided." + ) + self.pin_model = None # Invalidate model + return + + self.jaco_pin_vel_indices = ( + [] + ) # Reset for safety if setup is called multiple times + for joint_name in jaco_joint_names_list: + if self.pin_model.existJointName(joint_name): + joint_id = self.pin_model.getJointId(joint_name) + if ( + self.pin_model.joints[joint_id].nv == 1 + ): # Check if this joint is 1 DoF in velocity space + self.jaco_pin_vel_indices.append( + self.pin_model.joints[joint_id].idx_v + ) + else: + # This error should ideally not be hit for standard Jaco joints if Pinocchio parses them as revolute. + self.logger.error( + f"[{self.name}] Jaco joint '{joint_name}' (Pinocchio type: {self.pin_model.joints[joint_id].shortname()}) has nv={self.pin_model.joints[joint_id].nv} != 1. Not supported for simple vel_indices list." + ) + self.pin_model = None + return # Invalidate model and return + else: + self.logger.error( + f"[{self.name}] Jaco joint '{joint_name}' not found in Pinocchio model." + ) + self.pin_model = None # Invalidate model + return + + if not self.jaco_pin_vel_indices: + self.logger.error( + f"[{self.name}] No Jaco joint velocity indices could be determined." + ) + self.pin_model = None + return + self.logger.info( + f"[{self.name}] Jaco velocity indices in Pinocchio model: {self.jaco_pin_vel_indices}" + ) + + jaco_ee_link: str = self.blackboard_get("jaco_end_effector_link_name") + if not jaco_ee_link: + self.logger.error( + f"[{self.name}] 'jaco_end_effector_link_name' not provided." + ) + self.pin_model = None # Invalidate model + return + if self.pin_model.existFrame(jaco_ee_link): + self.jaco_ee_frame_id = self.pin_model.getFrameId(jaco_ee_link) + else: + self.logger.error( + f"[{self.name}] Jaco EE link '{jaco_ee_link}' not found in Pinocchio model." + ) + self.pin_model = None # Invalidate model + return + + self._urdf_loaded_successfully = True + + except FileNotFoundError: + self.logger.error( + f"[{self.name}] URDF/XACRO file not found at: {resolved_urdf_path}" + ) + self.pin_model = None + except subprocess.CalledProcessError as e: + self.logger.error( + f"[{self.name}] XACRO processing failed: {e}\nOutput:\n{e.stderr}" + ) + self.pin_model = None + except Exception as e: + self.logger.error(f"[{self.name}] Failed to load Pinocchio model: {e}") + self.pin_model = None + finally: + if ( + temp_urdf_file_name + and os.path.exists(temp_urdf_file_name) + and processed_urdf_path == temp_urdf_file_name + ): + try: + os.unlink(processed_urdf_path) + except OSError as e_unlink: + self.logger.error( + f"[{self.name}] Failed to delete temp URDF file {processed_urdf_path}: {e_unlink}" + ) + + def _resolve_package_path(self, path_str: str) -> Optional[str]: + """Resolves package:// paths to absolute paths.""" + if path_str.startswith("package://"): + try: + parts = path_str.split("package://", 1)[1].split("/", 1) + package_name = parts[0] + relative_path = parts[1] if len(parts) > 1 else "" + + from ament_index_python.packages import get_package_share_directory + + package_share_directory = get_package_share_directory(package_name) + return os.path.join(package_share_directory, relative_path) + except Exception as e: + self.logger.error( + f"[{self.name}] Could not resolve package path '{path_str}': {e}" + ) + return None + return path_str + + @override + def initialise(self) -> None: + """Clear previous outputs.""" + self.logger.debug(f"[{self.name}] Initializing.") + self.blackboard_set("jaco_directional_manipulability_score", None) + self.blackboard_set("jaco_is_manipulable_for_direction", None) + + @override + def update(self) -> Status: + if not self.node: + self.feedback_message = "Node not initialized" + return Status.FAILURE + if ( + not self._urdf_loaded_successfully + or not self.pin_model + or not self.pin_data + or self.jaco_ee_frame_id is None + or not self.jaco_pin_vel_indices + ): + self.feedback_message = ( + "Pinocchio model/data not loaded or required IDs not found" + ) + self.logger.error( + f"[{self.name}] {self.feedback_message} during update. Setup likely failed." + ) + return Status.FAILURE + + try: + current_q_robot_state: JointState = self.blackboard_get( + "current_full_robot_joint_state_MA" + ) + tip_pose_MA_world: PoseStamped = self.blackboard_get( + "tool_tip_move_above_pose_world" + ) + tip_pose_MI_world: PoseStamped = self.blackboard_get( + "tool_tip_move_into_pose_world" + ) + threshold: float = self.blackboard_get( + "directional_manipulability_threshold" + ) + + if not all( + isinstance(p, PoseStamped) + for p in [tip_pose_MA_world, tip_pose_MI_world] + ): + self.feedback_message = "Input tool tip poses are not PoseStamped." + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + # --- Populate Pinocchio's q vector (q_MA) --- + q_MA = pin.neutral(self.pin_model) + + num_updated_joints_in_q = 0 + for i, name_from_js in enumerate(current_q_robot_state.name): + if self.pin_model.existJointName(name_from_js): + joint_id_pin = self.pin_model.getJointId(name_from_js) + joint_obj_pin = self.pin_model.joints[joint_id_pin] + pin_joint_name = self.pin_model.names[joint_id_pin] + + theta = current_q_robot_state.position[i] + + if ( + joint_obj_pin.nq == 2 and joint_obj_pin.nv == 1 + ): # Typical for revolute joints in Pinocchio + # Expects [cos(theta), sin(theta)] + q_MA[joint_obj_pin.idx_q] = math.cos(theta) + q_MA[joint_obj_pin.idx_q + 1] = math.sin(theta) + num_updated_joints_in_q += ( + 1 # Counts as one "joint" from JointState + ) + self.logger.debug( + f"[{self.name}] Updated revolute joint '{pin_joint_name}' (nq=2) in q_MA with [cos({theta:.3f}), sin({theta:.3f})]" + ) + elif ( + joint_obj_pin.nq == 1 and joint_obj_pin.nv == 1 + ): # E.g. Prismatic joint + q_MA[joint_obj_pin.idx_q] = theta + num_updated_joints_in_q += 1 + self.logger.debug( + f"[{self.name}] Updated prismatic/nq=1 joint '{pin_joint_name}' in q_MA with {theta:.3f}" + ) + elif joint_obj_pin.nq > 1: # Other complex joints, e.g. FreeFlyer + if ( + pin_joint_name == self.pin_model.names[1] + and joint_obj_pin.shortname() == "JointModelFreeFlyer" + ): + if (i + 7) <= len( + current_q_robot_state.position + ): # Check if enough values are present + q_MA[joint_obj_pin.idx_q : joint_obj_pin.idx_q + 7] = ( + current_q_robot_state.position[i : i + 7] + ) + num_updated_joints_in_q += 1 # Counts as one "entry" from JointState perspective + self.logger.debug( + f"[{self.name}] Updated FreeFlyer base joint '{pin_joint_name}' in q_MA." + ) + else: + self.logger.warn( + f"[{self.name}] FreeFlyer base joint '{pin_joint_name}' found, but not enough values in JointState (from index {i}) to populate q (expected 7). Using neutral for base." + ) + else: + self.logger.warn( + f"[{self.name}] Joint '{name_from_js}' (Pinocchio name: '{pin_joint_name}') has nq={joint_obj_pin.nq}, nv={joint_obj_pin.nv}. " + f"Unhandled complex joint type in q_MA population. It will remain neutral if not FreeFlyer." + ) + else: + self.logger.warn( + f"[{self.name}] Joint '{name_from_js}' from input JointState not found in Pinocchio model. Skipping." + ) + + if num_updated_joints_in_q == 0: + self.logger.error( + f"[{self.name}] No joints in Pinocchio's q were updated from input JointState. Check joint names and URDF consistency." + ) + return Status.FAILURE + + jaco_joint_names_bb: List[str] = self.blackboard_get("jaco_joint_names") + log_jaco_q_values = [] + for name in jaco_joint_names_bb: + if self.pin_model.existJointName(name): + jid = self.pin_model.getJointId(name) + j_obj = self.pin_model.joints[jid] + if j_obj.nq == 1: + log_jaco_q_values.append(f"{name}: {q_MA[j_obj.idx_q]:.3f}") + elif j_obj.nq == 2: + log_jaco_q_values.append( + f"{name}: [{q_MA[j_obj.idx_q]:.3f}, {q_MA[j_obj.idx_q + 1]:.3f}]" + ) + self.logger.info( + f"[{self.name}] Populated q_MA for Jaco joints: {{{', '.join(log_jaco_q_values)}}}" + ) + # --- End q_MA Population --- + + pin.forwardKinematics(self.pin_model, self.pin_data, q_MA) + pin.updateFramePlacements(self.pin_model, self.pin_data) + + J_jacoEE_world_full = pin.computeFrameJacobian( + self.pin_model, + self.pin_data, + q_MA, + self.jaco_ee_frame_id, + pin.ReferenceFrame.WORLD, + ) + + J_jacoEE_world_jaco = J_jacoEE_world_full[:, self.jaco_pin_vel_indices] + J_jacoEE_world_jaco_trans = J_jacoEE_world_jaco[:3, :] + self.logger.info( + f"[{self.name}] Jaco EE Trans Jacobian (J_jacoEE_world_jaco_trans):\n{J_jacoEE_world_jaco_trans}" + ) + + pos_MA_tip = np.array( + [ + tip_pose_MA_world.pose.position.x, + tip_pose_MA_world.pose.position.y, + tip_pose_MA_world.pose.position.z, + ] + ) + pos_MI_tip = np.array( + [ + tip_pose_MI_world.pose.position.x, + tip_pose_MI_world.pose.position.y, + tip_pose_MI_world.pose.position.z, + ] + ) + + task_direction_vec = pos_MI_tip - pos_MA_tip + norm_task_direction = np.linalg.norm(task_direction_vec) + self.logger.info( + f"[{self.name}] Tool Tip MA Pose: {pos_MA_tip}, MI Pose: {pos_MI_tip}" + ) + self.logger.info( + f"[{self.name}] Task Direction Vector (tool tip): {task_direction_vec}, Norm: {norm_task_direction}" + ) + + if norm_task_direction < 1e-6: + self.feedback_message = "Task direction vector (tool tip) is near zero." + self.logger.warn(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("jaco_directional_manipulability_score", 0.0) + self.blackboard_set("jaco_is_manipulable_for_direction", False) + return Status.FAILURE + + unit_task_direction_tip = task_direction_vec / norm_task_direction + unit_task_direction_jacoEE = unit_task_direction_tip + self.logger.info( + f"[{self.name}] Unit Task Direction (Jaco EE): {unit_task_direction_jacoEE}" + ) + + JJT_trans_jacoEE = J_jacoEE_world_jaco_trans @ J_jacoEE_world_jaco_trans.T + self.logger.info( + f"[{self.name}] JJT_trans_jacoEE (3x3 matrix):\n{JJT_trans_jacoEE}" + ) + det_JJT = np.linalg.det(JJT_trans_jacoEE) + self.logger.info( + f"[{self.name}] Determinant of JJT_trans_jacoEE: {det_JJT:.6e}" + ) + + m_d_squared = ( + unit_task_direction_jacoEE.T + @ JJT_trans_jacoEE + @ unit_task_direction_jacoEE + ) + self.logger.info( + f"[{self.name}] m_d_squared (before sqrt): {m_d_squared:.6e}" + ) + manipulability_score = np.sqrt(max(0.0, m_d_squared)) + + self.logger.info( + f"[{self.name}] Jaco EE directional manipulability (tool tip direction): {manipulability_score:.4f}" + ) + self.blackboard_set( + "jaco_directional_manipulability_score", manipulability_score + ) + + is_manipulable = manipulability_score >= threshold + self.blackboard_set("jaco_is_manipulable_for_direction", is_manipulable) + + if is_manipulable: + self.feedback_message = f"Jaco EE is manipulable for task direction (Score: {manipulability_score:.4f} >= Threshold: {threshold:.4f})" + self.logger.info(f"[{self.name}] {self.feedback_message}") + return Status.SUCCESS + else: + self.feedback_message = f"Jaco EE NOT manipulable for task direction (Score: {manipulability_score:.4f} < Threshold: {threshold:.4f})" + self.logger.warn(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + except KeyError as e: + self.feedback_message = f"Blackboard key error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + except Exception as e: + self.feedback_message = f"Unexpected error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + @override + def terminate(self, new_status: Status) -> None: + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") From f900d263f4c7f611bc81619157d87812600b28bf Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 17 May 2025 10:15:20 -0700 Subject: [PATCH 066/238] Disable orientation control service call for now, we'll find another place to activate it more selectively and intentionally --- ...ove_to_configuration_with_ft_thresholds_tree.py | 14 -------------- ...e_to_configuration_with_wheelchair_wall_tree.py | 14 -------------- 2 files changed, 28 deletions(-) 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 5e7d2426..06e2ccec 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 @@ -23,9 +23,6 @@ MoveIt2JointConstraint, ) from ada_feeding.behaviors.state import GetJointStates -from ada_feeding.behaviors.articutool.call_set_orientation_control import ( - CallSetOrientationControl, -) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import pre_moveto_config, scoped_behavior from ada_feeding.idioms.bite_transfer import get_toggle_watchdog_listener_behavior @@ -206,17 +203,6 @@ def create_tree( }, outputs={}, ), - CallSetOrientationControl( - name="SetArticutoolOrientation", - ns=name, - inputs={ - "enable": False, - "quat_xyzw": None, - }, - outputs={ - - }, - ), ], ) 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 d3425b93..718ad98c 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 @@ -28,9 +28,6 @@ MoveIt2OrientationConstraint, ) from ada_feeding.behaviors.state import GetJointStates -from ada_feeding.behaviors.articutool.call_set_orientation_control import ( - CallSetOrientationControl, -) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import pre_moveto_config, scoped_behavior from ada_feeding.idioms.bite_transfer import ( @@ -210,17 +207,6 @@ def create_tree( }, outputs={}, ), - CallSetOrientationControl( - name="SetArticutoolOrientation", - ns=name, - inputs={ - "enable": False, - "quat_xyzw": None, - }, - outputs={ - - }, - ), ], ), ], From d43491ddbf3fa39b56ffab4a1f594c72eb3fc830 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 10:20:45 -0700 Subject: [PATCH 067/238] Separate move_above_plan into three distinct sequences, pre_acquisition_sequence which handles perception, action selection, and calculates target poses, move_above_sequence which plans the motion to the MoveAbove configuration and computes a manipulability metric, and the move_into_sequence plans the Cartesian motion to the MoveInto configuration --- .../ada_feeding/trees/acquire_food_tree.py | 979 +++++++++--------- 1 file changed, 510 insertions(+), 469 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 9211f625..038da980 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -54,6 +54,7 @@ CombineJointStates, ExtractPoseFromPosesByLink, ExtractPoseComponents, + CheckJacoDirectionalManipulability, ) from ada_feeding.behaviors.ros.msgs import StampPoseFromPose from ada_feeding.behaviors.ros.tf import ApplyTransform @@ -475,12 +476,12 @@ def create_tree( ], # End RecoverySequence.children ) # End RecoverySequence - def move_above_plan( + def pre_acquisition_sequence( flip_food_frame: bool = False, action: Optional[BlackboardKey] = None, ) -> py_trees.behaviour.Behaviour: return py_trees.composites.Sequence( - name="MoveAbovePlanningSeq", + name="PreAcquisitionSequence", memory=True, children=[ # Compute Food Frame @@ -541,8 +542,12 @@ def move_above_plan( # Default approach_frame_id = "approach" }, outputs={ - "move_above_pose": BlackboardKey("move_above_pose"), - "move_into_pose": BlackboardKey("move_into_pose"), + "move_above_pose": BlackboardKey( + "move_above_pose_food_frame" + ), + "move_into_pose": BlackboardKey( + "move_into_pose_food_frame" + ), "approach_thresh": BlackboardKey("approach_thresh"), "grasp_thresh": BlackboardKey("grasp_thresh"), "ext_thresh": BlackboardKey("ext_thresh"), @@ -553,90 +558,151 @@ def move_above_plan( ), # Re-Tare FT Sensor and default to 4N threshold pre_moveto_config(name="PreAcquireFTTare"), - # Convert Pose to PoseStamped using the defined frame_id + # --- Prepare MoveAbove Pose for IK --- StampPoseFromPose( - name="StampMoveAbovePose", + name="StampMoveAbovePoseFood", ns=name, inputs={ - "input_pose": BlackboardKey("move_above_pose"), + "input_pose": BlackboardKey("move_above_pose_food_frame"), "frame_id": "food", }, outputs={ "output_pose_stamped": BlackboardKey( - "move_above_pose_stamped_food_frame" + "move_above_pose_stamped_food" ) }, ), - # Use ApplyTransform to transform into the MoveIt Planning Frame ApplyTransform( - name="TransformPoseToIKFrame", + name="TransformMoveAbovePoseToWorld", ns=name, inputs={ "stamped_msg": BlackboardKey( - "move_above_pose_stamped_food_frame" + "move_above_pose_stamped_food" ), "target_frame": "j2n6s200_link_base", }, outputs={ "transformed_msg": BlackboardKey( - "move_above_pose_stamped_base_frame" + "tool_tip_move_above_pose_world" ) }, ), - # Get Current Joint State (for IK Seed) - py_trees.decorators.Timeout( - name="GetCurrentState", - duration=1.0, - child=GetJointStates( - name="GetArticutoolJoints", - ns=name, - node=self._node, - inputs={ - "joint_names": [ - "j2n6s200_joint_1", - "j2n6s200_joint_2", - "j2n6s200_joint_3", - "j2n6s200_joint_4", - "j2n6s200_joint_5", - "j2n6s200_joint_6", - "atool_joint1", - "atool_joint2", - ] - }, - outputs={ - "joint_state": BlackboardKey("current_joint_state"), - "joint_positions": BlackboardKey( - "current_joint_positions" - ), - "joint_names": BlackboardKey("current_joint_names"), - }, - ), + StampPoseFromPose( + name="StampMoveIntoPoseFood", + ns=name, + inputs={ + "input_pose": BlackboardKey("move_into_pose_food_frame"), + "frame_id": "food", + }, + outputs={ + "output_pose_stamped": BlackboardKey( + "move_into_pose_stamped_food" + ) + }, + ), + ApplyTransform( + name="TransformMoveIntoPoseToWorld", + ns=name, + inputs={ + "stamped_msg": BlackboardKey("move_into_pose_stamped_food"), + "target_frame": "j2n6s200_link_base", + }, + outputs={ + "transformed_msg": BlackboardKey( + "tool_tip_move_into_pose_world" + ) + }, + ), + ], + ) + + def move_above_sequence() -> py_trees.behaviour.Behaviour: + return py_trees.composites.Sequence( + name="MoveAboveSequence", + memory=True, + children=[ + GetJointStates( + name="GetFullCurrentJointStateForIKSeed", + ns=name, + node=self._node, + inputs={ + "joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + "atool_joint1", + "atool_joint2", + ] + }, + outputs={ + "joint_state": BlackboardKey( + "current_full_joint_state_for_ik" + ) + }, ), - # Compute IK for the target pose using the full jaco_arm_with_articutool planning group MoveIt2ComputeIK( - name="ComputeJacoArmWithArticutoolIK", + name="ComputeIKForMoveAbove", ns=name, inputs={ "target_pose": BlackboardKey( - "move_above_pose_stamped_base_frame" + "tool_tip_move_above_pose_world" ), "group_name": "jaco_arm_with_articutool", - "start_joint_state": BlackboardKey("current_joint_state"), - "constraints": None, + "start_joint_state": BlackboardKey( + "current_full_joint_state_for_ik" + ), }, outputs={ "ik_solution_joint_state": BlackboardKey( - "move_above_ik_solution" + "move_above_ik_solution_8dof" ), "success": BlackboardKey("move_above_ik_success"), }, ), + # Check Jaco Directional Manipulability + CheckJacoDirectionalManipulability( + name="CheckJacoManipulabilityForMoveInto", + ns=name, + inputs={ + "current_full_robot_joint_state_MA": BlackboardKey( + "move_above_ik_solution_8dof" + ), + "tool_tip_move_above_pose_world": BlackboardKey( + "tool_tip_move_above_pose_world" + ), + "tool_tip_move_into_pose_world": BlackboardKey( + "tool_tip_move_into_pose_world" + ), + "jaco_joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ], + "jaco_end_effector_link_name": "j2n6s200_end_effector", + "directional_manipulability_threshold": 0.01, + "urdf_file_path": "package://ada_moveit/config/ada.urdf.xacro", + }, + outputs={ + "jaco_directional_manipulability_score": BlackboardKey( + "jaco_manip_score" + ), + "jaco_is_manipulable_for_direction": BlackboardKey( + "jaco_can_move_into" + ), + }, + ), ExtractJointsFromState( - name="ExtractJacoArmJoints", + name="ExtractJacoArmJointsForMoveAbove", ns=name, inputs={ "source_joint_state": BlackboardKey( - "move_above_ik_solution" + "move_above_ik_solution_8dof" ), "target_joint_names": [ "j2n6s200_joint_1", @@ -654,20 +720,17 @@ def move_above_plan( "output_joint_positions": BlackboardKey( "move_above_jaco_arm_joint_positions" ), - "success": BlackboardKey("extract_jaco_arm_joints_success"), + "success": None, }, ), ExtractJointsFromState( - name="ExtractArticutoolJoints", + name="ExtractArticutoolJointsForMoveAbove", ns=name, inputs={ "source_joint_state": BlackboardKey( - "move_above_ik_solution" + "move_above_ik_solution_8dof" ), - "target_joint_names": [ - "atool_joint1", - "atool_joint2", - ], + "target_joint_names": ["atool_joint1", "atool_joint2"], }, outputs={ "output_joint_names": BlackboardKey( @@ -676,13 +739,11 @@ def move_above_plan( "output_joint_positions": BlackboardKey( "move_above_articutool_joint_positions" ), - "success": BlackboardKey( - "extract_articutool_joints_success" - ), + "success": None, }, ), MoveIt2JointConstraint( - name="SetJacoArmJointConstraint", + name="SetJacoArmJointConstraintForMoveAbove", ns=name, inputs={ "joint_positions": BlackboardKey( @@ -691,17 +752,15 @@ def move_above_plan( "joint_names": BlackboardKey( "move_above_jaco_arm_joint_names" ), - "tolerance": 0.001, - "constraints": None, }, outputs={ "constraints": BlackboardKey( "move_above_jaco_arm_constraints" - ), + ) }, ), MoveIt2JointConstraint( - name="SetArticutoolJointConstraint", + name="SetArticutoolJointConstraintForMoveAbove", ns=name, inputs={ "joint_positions": BlackboardKey( @@ -710,19 +769,15 @@ def move_above_plan( "joint_names": BlackboardKey( "move_above_articutool_joint_names" ), - "tolerance": 0.001, - "constraints": None, }, outputs={ "constraints": BlackboardKey( "move_above_articutool_constraints" - ), + ) }, ), - ### Move Above Food py_trees.decorators.Timeout( name="MoveAboveJacoArmPlanTimeout", - # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such duration=10.0 * self.allowed_planning_time_for_move_above, child=MoveIt2Plan( name="MoveAboveJacoArmPlan", @@ -734,7 +789,6 @@ def move_above_plan( "max_velocity_scale": self.max_velocity_scaling_move_above, "max_acceleration_scale": self.max_acceleration_scaling_move_above, "allowed_planning_time": self.allowed_planning_time_for_move_above, - "max_path_len_joint": max_path_len_joint, "group_name": "jaco_arm", }, outputs={ @@ -742,14 +796,13 @@ def move_above_plan( "move_above_jaco_arm_trajectory" ), "end_joint_state": BlackboardKey( - "test_into_jaco_arm_joints" + "move_above_jaco_arm_end_joint_state" ), }, ), ), py_trees.decorators.Timeout( name="MoveAboveArticutoolPlanTimeout", - # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such duration=10.0 * self.allowed_planning_time_for_move_above, child=MoveIt2Plan( name="MoveAboveArticutoolPlan", @@ -761,7 +814,6 @@ def move_above_plan( "max_velocity_scale": self.max_velocity_scaling_move_above, "max_acceleration_scale": self.max_acceleration_scaling_move_above, "allowed_planning_time": self.allowed_planning_time_for_move_above, - "max_path_len_joint": max_path_len_joint, "group_name": "articutool", }, outputs={ @@ -769,19 +821,20 @@ def move_above_plan( "move_above_articutool_trajectory" ), "end_joint_state": BlackboardKey( - "test_into_articutool_joints" + "move_above_articutool_end_joint_state" ), }, ), ), - ### Test MoveIntoFood CombineJointStates( name="CombineJacoArmAndArticutoolJoints", ns=name, inputs={ - "joint_state_1": BlackboardKey("test_into_jaco_arm_joints"), + "joint_state_1": BlackboardKey( + "move_above_jaco_arm_end_joint_state" + ), "joint_state_2": BlackboardKey( - "test_into_articutool_joints" + "move_above_articutool_end_joint_state" ), "full_joint_names": [ "j2n6s200_joint_1", @@ -795,53 +848,35 @@ def move_above_plan( ], }, outputs={ - "combined_joint_state": BlackboardKey("test_into_joints"), - }, - ), - # Convert Pose to PoseStamped using the defined frame_id - StampPoseFromPose( - name="StampMoveIntoPose", - ns=name, - inputs={ - "input_pose": BlackboardKey("move_into_pose"), - "frame_id": "food", - }, - outputs={ - "output_pose_stamped": BlackboardKey( - "move_into_pose_stamped_food_frame" - ) - }, - ), - # Use ApplyTransform to transform into the MoveIt Planning Frame - ApplyTransform( - name="TransformPoseToIKFrame", - ns=name, - inputs={ - "stamped_msg": BlackboardKey( - "move_into_pose_stamped_food_frame" + "combined_joint_state": BlackboardKey( + "move_above_end_joint_state" ), - "target_frame": "j2n6s200_link_base", - }, - outputs={ - "transformed_msg": BlackboardKey( - "move_into_pose_stamped_base_frame" - ) }, ), + ], + ) + + def move_into_sequence() -> py_trees.behaviour.Behaviour: + return py_trees.composites.Sequence( + name="MoveIntoSequence", + memory=True, + children=[ # Compute IK for the target pose using the full jaco_arm_with_articutool planning group MoveIt2ComputeIK( - name="ComputeJacoArmWithArticutoolIK", + name="ComputeIKForMoveInto", ns=name, inputs={ "target_pose": BlackboardKey( - "move_into_pose_stamped_base_frame" + "tool_tip_move_into_pose_world" ), "group_name": "jaco_arm_with_articutool", - # "start_joint_state": BlackboardKey("current_joint_positions"), + "start_joint_state": BlackboardKey( + "move_above_end_joint_state" + ), }, outputs={ "ik_solution_joint_state": BlackboardKey( - "move_into_ik_solution" + "move_into_ik_solution_8dof" ), "success": BlackboardKey("move_into_ik_success"), }, @@ -851,7 +886,7 @@ def move_above_plan( ns=name, inputs={ "group_name": "jaco_arm_with_articutool", - "joint_state": BlackboardKey("move_into_ik_solution"), + "joint_state": BlackboardKey("move_into_ik_solution_8dof"), "fk_link_names": ["j2n6s200_end_effector"], }, outputs={ @@ -901,7 +936,9 @@ def move_above_plan( "cartesian": True, "cartesian_max_step": 0.001, "cartesian_fraction_threshold": 0.92, - "start_joint_state": BlackboardKey("test_into_joints"), + "start_joint_state": BlackboardKey( + "move_above_jaco_arm_end_joint_state" + ), "max_path_len_joint": max_path_len_joint, "allowed_planning_time": self.allowed_planning_time_for_move_into, "group_name": "jaco_arm", @@ -1012,367 +1049,371 @@ def move_above_plan( name="BackupFlipFoodFrameSel", memory=True, children=[ - move_above_plan(True), - move_above_plan(False, BlackboardKey("action")), - ], - ), - MoveIt2Execute( - name="MoveAboveJacoArm", - ns=name, - inputs={ - "trajectory": BlackboardKey( - "move_above_jaco_arm_trajectory" + pre_acquisition_sequence(True), + pre_acquisition_sequence( + False, BlackboardKey("action") ), - "group_name": "jaco_arm", - }, - outputs={}, - ), - ExecuteArticutoolTrajectory( - name="MoveAboveArticutool", - ns=name, - inputs={ - "trajectory": BlackboardKey( - "move_above_articutool_trajectory" - ), - }, - outputs={ - "action_goal_accepted": BlackboardKey( - "tool_goal_accepted" - ), - "action_result_code": BlackboardKey( - "tool_exec_result_code" - ), - "action_status": BlackboardKey( - "tool_action_status" - ), - }, + ], ), - # If Anything goes wrong, reset FT to safe levels - scoped_behavior( - name="SafeFTPreempt", - # Set Approach F/T Thresh - 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")] - ), - response_checks=[ - py_trees.common.ComparisonExpression( - variable=Blackboard.separator.join( - [ - name, - BlackboardKey( - "ft_response" - ), - ] - ), - value=SetParameters.Response(), # Unused - operator=set_parameter_response_all_success, - ) - ], - ), - ], - ), - post_behavior=py_trees.composites.Sequence( - name=name, - memory=True, - children=[ - pre_moveto_config( - name="PostAcquireFTSet", re_tare=False - ), - ], - ), - on_preempt_timeout=5.0, - # Starts a new Sequence w/ Memory internally - workers=[ - ### Move Into Food - SwitchArticutoolControllers( - name="SwitchArticutoolToVelocity", - ns=name, - inputs={ - "controllers_to_activate": [ - "velocity_controller" - ], - "controllers_to_deactivate": [ - "joint_trajectory_controller" - ], - }, - outputs={ - "switch_call_succeeded": None, - "switch_response_ok": None, - }, - ), - ExtractPoseComponents( - name="GetMoveIntoOrientation", - ns=name, - inputs={ - "input_pose_object": BlackboardKey( - "move_into_pose_stamped_base_frame" - ), - }, - outputs={ - "output_position": BlackboardKey( - "move_into_position" - ), - "output_orientation": BlackboardKey( - "move_into_orientation" - ), - "output_header": BlackboardKey( - "move_into_header" - ), - "success": None, - }, - ), - CallSetOrientationControl( - name="SetArticutoolOrientation", - ns=name, - inputs={ - "enable": True, - "quat_xyzw": BlackboardKey( - "move_into_orientation" - ), - }, - outputs={}, - ), - # MoveInto expect F/T failure - py_trees.decorators.FailureIsSuccess( - name="MoveIntoJacoArmExecuteSucceed", - child=MoveIt2Execute( - name="MoveIntoJacoArm", - ns=name, - inputs={ - "trajectory": BlackboardKey( - "move_into_jaco_arm_trajectory" - ) - }, - outputs={}, - ), - ), - ### 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, - servo_controller_name="jaco_arm_cartesian_controller", - start_moveit_servo=False, - ) - .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_cartesian_controller_parameters", - ), - StopServoTree( - self._node, - servo_controller_name="jaco_arm_cartesian_controller", - stop_moveit_servo=False, - ) - .create_tree(name="StopServoScoped") - .root, - ], - ), - on_preempt_timeout=5.0, - # Starts a new Sequence w/ Memory internally - workers=[ - py_trees.composites.Selector( - name="InFoodErrorSelector", - memory=True, - children=[ - py_trees.composites.Sequence( - name="InFoodGraspExtract", - memory=True, - children=[ - ### Grasp - retry_call_ros_service( - name="GraspFTThresh", - service_type=SetParameters, - service_name="~/set_cartesian_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, - ) - ], - ), - 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" - ), - "pub_topic": "~/cartesian_twist_cmds", - "servo_status_sub_topic": None, - }, - ), # Auto Zero-Twist on terminate() - ### Extraction - ComputeActionTwist( - name="ComputeExtract", - 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_cartesian_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" - ), - "pub_topic": "~/cartesian_twist_cmds", - "servo_status_sub_topic": None, - }, - ), # Auto Zero-Twist on terminate() - ft_thresh_satisfied( - name="CheckFTForkOffPlate" - ), - ], # End InFoodGraspExtract.children - ), # End InFoodGraspExtract - recovery_tree, - ], # End InFoodErrorSelector.children - ), # End InFoodErrorSelector - ], # End MoveIt2Servo.workers - ), # End MoveIt2Servo - ], # End SafeFTPreempt.workers - ), # End SafeFTPreempt + move_above_sequence(), + move_into_sequence(), + # MoveIt2Execute( + # name="MoveAboveJacoArm", + # ns=name, + # inputs={ + # "trajectory": BlackboardKey( + # "move_above_jaco_arm_trajectory" + # ), + # "group_name": "jaco_arm", + # }, + # outputs={}, + # ), + # ExecuteArticutoolTrajectory( + # name="MoveAboveArticutool", + # ns=name, + # inputs={ + # "trajectory": BlackboardKey( + # "move_above_articutool_trajectory" + # ), + # }, + # outputs={ + # "action_goal_accepted": BlackboardKey( + # "tool_goal_accepted" + # ), + # "action_result_code": BlackboardKey( + # "tool_exec_result_code" + # ), + # "action_status": BlackboardKey( + # "tool_action_status" + # ), + # }, + # ), + # # If Anything goes wrong, reset FT to safe levels + # scoped_behavior( + # name="SafeFTPreempt", + # # Set Approach F/T Thresh + # 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")] + # ), + # response_checks=[ + # py_trees.common.ComparisonExpression( + # variable=Blackboard.separator.join( + # [ + # name, + # BlackboardKey( + # "ft_response" + # ), + # ] + # ), + # value=SetParameters.Response(), # Unused + # operator=set_parameter_response_all_success, + # ) + # ], + # ), + # ], + # ), + # post_behavior=py_trees.composites.Sequence( + # name=name, + # memory=True, + # children=[ + # pre_moveto_config( + # name="PostAcquireFTSet", re_tare=False + # ), + # ], + # ), + # on_preempt_timeout=5.0, + # # Starts a new Sequence w/ Memory internally + # workers=[ + # ### Move Into Food + # SwitchArticutoolControllers( + # name="SwitchArticutoolToVelocity", + # ns=name, + # inputs={ + # "controllers_to_activate": [ + # "velocity_controller" + # ], + # "controllers_to_deactivate": [ + # "joint_trajectory_controller" + # ], + # }, + # outputs={ + # "switch_call_succeeded": None, + # "switch_response_ok": None, + # }, + # ), + # ExtractPoseComponents( + # name="GetMoveIntoOrientation", + # ns=name, + # inputs={ + # "input_pose_object": BlackboardKey( + # "move_into_pose_stamped_base_frame" + # ), + # }, + # outputs={ + # "output_position": BlackboardKey( + # "move_into_position" + # ), + # "output_orientation": BlackboardKey( + # "move_into_orientation" + # ), + # "output_header": BlackboardKey( + # "move_into_header" + # ), + # "success": None, + # }, + # ), + # CallSetOrientationControl( + # name="SetArticutoolOrientation", + # ns=name, + # inputs={ + # "enable": True, + # "quat_xyzw": BlackboardKey( + # "move_into_orientation" + # ), + # }, + # outputs={}, + # ), + # # MoveInto expect F/T failure + # py_trees.decorators.FailureIsSuccess( + # name="MoveIntoJacoArmExecuteSucceed", + # child=MoveIt2Execute( + # name="MoveIntoJacoArm", + # ns=name, + # inputs={ + # "trajectory": BlackboardKey( + # "move_into_jaco_arm_trajectory" + # ) + # }, + # outputs={}, + # ), + # ), + # ### 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, + # servo_controller_name="jaco_arm_cartesian_controller", + # start_moveit_servo=False, + # ) + # .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_cartesian_controller_parameters", + # ), + # StopServoTree( + # self._node, + # servo_controller_name="jaco_arm_cartesian_controller", + # stop_moveit_servo=False, + # ) + # .create_tree(name="StopServoScoped") + # .root, + # ], + # ), + # on_preempt_timeout=5.0, + # # Starts a new Sequence w/ Memory internally + # workers=[ + # py_trees.composites.Selector( + # name="InFoodErrorSelector", + # memory=True, + # children=[ + # py_trees.composites.Sequence( + # name="InFoodGraspExtract", + # memory=True, + # children=[ + # ### Grasp + # retry_call_ros_service( + # name="GraspFTThresh", + # service_type=SetParameters, + # service_name="~/set_cartesian_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, + # ) + # ], + # ), + # 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" + # ), + # "pub_topic": "~/cartesian_twist_cmds", + # "servo_status_sub_topic": None, + # }, + # ), # Auto Zero-Twist on terminate() + # ### Extraction + # ComputeActionTwist( + # name="ComputeExtract", + # 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_cartesian_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" + # ), + # "pub_topic": "~/cartesian_twist_cmds", + # "servo_status_sub_topic": None, + # }, + # ), # Auto Zero-Twist on terminate() + # ft_thresh_satisfied( + # name="CheckFTForkOffPlate" + # ), + # ], # End InFoodGraspExtract.children + # ), # End InFoodGraspExtract + # recovery_tree, + # ], # End InFoodErrorSelector.children + # ), # End InFoodErrorSelector + # ], # End MoveIt2Servo.workers + # ), # End MoveIt2Servo + # ], # End SafeFTPreempt.workers + # ), # End SafeFTPreempt ], # End OctomapAndTableCollision.workers ), # OctomapAndTableCollision - ] - + resting_position_behaviors, # End Success.workers + ], + # + resting_position_behaviors, # End Success.workers ), # End Success # TableCollision ], # End root_seq.children ) # End root_seq From 6930f5a1b1ea60504880cfdbdc12b479e8ae7dac Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 11:06:38 -0700 Subject: [PATCH 068/238] Add behavior to load Pinocchio model of full Jaco arm and Articutool for kinematic computations --- .../behaviors/state/load_pinocchio_model.py | 314 ++++++++++++++++++ 1 file changed, 314 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/state/load_pinocchio_model.py diff --git a/ada_feeding/ada_feeding/behaviors/state/load_pinocchio_model.py b/ada_feeding/ada_feeding/behaviors/state/load_pinocchio_model.py new file mode 100644 index 00000000..9d7924e7 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/load_pinocchio_model.py @@ -0,0 +1,314 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +Defines the LoadPinocchioModel behavior, which loads the robot model +using Pinocchio and places the model, data, and key kinematic information +onto the blackboard for use by other behaviors. +""" + +# Standard imports +import os +import subprocess +import tempfile +from typing import Union, Optional, List, Dict, Any + +# Third-party imports +import pinocchio as pin +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status +import rclpy.node + +# Local imports +from ada_feeding.helpers import BlackboardKey # Assuming this is your helper +from ada_feeding.behaviors import BlackboardBehavior # Assuming this is your base class + + +class LoadPinocchioModel(BlackboardBehavior): + """ + Loads the robot's kinematic model using Pinocchio from a URDF/XACRO file. + It writes the Pinocchio model, data, and optionally pre-identified + joint velocity indices and frame IDs to the blackboard. + This behavior should typically run once at the start of a sequence + requiring Pinocchio-based calculations. It will only attempt to load + the model once per instantiation unless explicitly reset or re-set up. + """ + + def blackboard_inputs( + self, + urdf_file_path: Union[BlackboardKey, str], + jaco_joint_names: Optional[Union[BlackboardKey, List[str]]] = None, + articutool_joint_names: Optional[Union[BlackboardKey, List[str]]] = None, + jaco_end_effector_link_name: Optional[Union[BlackboardKey, str]] = None, + tool_tip_link_name: Optional[Union[BlackboardKey, str]] = None, + # Add other relevant link names if needed, e.g., articutool_base_link + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + urdf_file_path: Path to the robot's URDF or XACRO file. + jaco_joint_names: Optional. List of Jaco arm joint names. If provided, their + velocity indices in the Pinocchio model will be stored. + articutool_joint_names: Optional. List of Articutool joint names. If provided, + their velocity indices will be stored. + jaco_end_effector_link_name: Optional. Name of the Jaco end-effector link. + If provided, its frame ID will be stored. + tool_tip_link_name: Optional. Name of the tool tip link. If provided, + its frame ID will be stored. + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + pinocchio_model: Optional[BlackboardKey], # -> Optional[pin.Model] + pinocchio_data: Optional[BlackboardKey], # -> Optional[pin.Data] + jaco_vel_indices_pin: Optional[BlackboardKey] = None, # -> Optional[List[int]] + articutool_vel_indices_pin: Optional[ + BlackboardKey + ] = None, # -> Optional[List[int]] + jaco_ee_frame_id_pin: Optional[BlackboardKey] = None, # -> Optional[int] + tool_tip_frame_id_pin: Optional[BlackboardKey] = None, # -> Optional[int] + # Add other relevant frame IDs or indices as outputs if needed + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + pinocchio_model: The loaded Pinocchio model object. + pinocchio_data: The Pinocchio data object associated with the model. + jaco_vel_indices_pin: List of velocity indices for Jaco joints in the model. + articutool_vel_indices_pin: List of velocity indices for Articutool joints. + jaco_ee_frame_id_pin: Frame ID for the Jaco end-effector link. + tool_tip_frame_id_pin: Frame ID for the tool tip link. + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def __init__(self, name: str, **kwargs): + super().__init__(name=name, **kwargs) + self.node: Optional[rclpy.node.Node] = None + self._model_loaded_successfully = False # Flag to prevent re-loading + + @override + def setup(self, **kwargs): + """Gets the ROS2 node from arguments.""" + try: + self.node = kwargs["node"] + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + # No return here, update will handle node being None + + def _resolve_package_path(self, path_str: str) -> Optional[str]: + """Resolves package:// paths to absolute paths.""" + if path_str.startswith("package://"): + try: + parts = path_str.split("package://", 1)[1].split("/", 1) + package_name = parts[0] + relative_path = parts[1] if len(parts) > 1 else "" + from ament_index_python.packages import get_package_share_directory + + package_share_directory = get_package_share_directory(package_name) + return os.path.join(package_share_directory, relative_path) + except Exception as e: + self.logger.error( + f"[{self.name}] Could not resolve package path '{path_str}': {e}" + ) + return None + return path_str + + def _get_vel_indices_for_joints( + self, model: pin.Model, joint_names: List[str], group_name_log: str + ) -> Optional[List[int]]: + """Helper to get velocity indices for a list of joint names.""" + vel_indices = [] + for joint_name in joint_names: + if model.existJointName(joint_name): + joint_id = model.getJointId(joint_name) + if model.joints[joint_id].nv == 1: # Assuming 1 DoF in velocity space + vel_indices.append(model.joints[joint_id].idx_v) + else: + self.logger.error( + f"[{self.name}] {group_name_log} joint '{joint_name}' has nv={model.joints[joint_id].nv} != 1. Cannot directly get single velocity index." + ) + return None + else: + self.logger.error( + f"[{self.name}] {group_name_log} joint '{joint_name}' not found in Pinocchio model." + ) + return None + if ( + not vel_indices and joint_names + ): # If names were provided but no indices found + self.logger.error( + f"[{self.name}] No velocity indices determined for {group_name_log} joints: {joint_names}" + ) + return None + return vel_indices + + @override + def update(self) -> Status: + """ + Loads the Pinocchio model if not already loaded and writes outputs to blackboard. + Returns SUCCESS if model is loaded (or was already loaded), FAILURE otherwise. + """ + if not self.node: + self.feedback_message = "Node not initialized in setup." + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + if self._model_loaded_successfully: + self.feedback_message = "Pinocchio model already loaded and on blackboard." + self.logger.debug(f"[{self.name}] {self.feedback_message}") + return Status.SUCCESS # Already done, no need to reload + + try: + urdf_path_str: str = self.blackboard_get("urdf_file_path") + if not urdf_path_str: + self.feedback_message = "'urdf_file_path' not provided or is empty." + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + resolved_urdf_path = self._resolve_package_path(urdf_path_str) + if not resolved_urdf_path: + self.feedback_message = f"Failed to resolve URDF path: {urdf_path_str}" + # _resolve_package_path already logged the error + return Status.FAILURE + + processed_urdf_path = resolved_urdf_path + temp_urdf_file_name = None + + if resolved_urdf_path.endswith(".xacro"): + self.logger.info( + f"[{self.name}] Processing XACRO file: {resolved_urdf_path}" + ) + with tempfile.NamedTemporaryFile( + mode="w", suffix=".urdf", delete=False + ) as temp_file: + temp_urdf_file_name = temp_file.name + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", resolved_urdf_path], + check=True, + capture_output=True, + text=True, + ) + temp_file.write(process.stdout) + processed_urdf_path = temp_urdf_file_name + + self.logger.info(f"[{self.name}] Loading URDF from: {processed_urdf_path}") + # Important: If your URDF has a floating base (e.g. for a mobile manipulator), + # you might need to load it with pin.buildModelFromUrdf(..., pin.JointModelFreeFlyer()) + # or ensure the URDF correctly specifies the root joint. + # For a fixed-base manipulator like Jaco on a table, this is usually fine. + model = pin.buildModelFromUrdf(processed_urdf_path) + data = model.createData() + self.logger.info( + f"[{self.name}] Pinocchio model loaded: {model.name}, Nq={model.nq}, Nv={model.nv}" + ) + + self.blackboard_set("pinocchio_model", model) + self.blackboard_set("pinocchio_data", data) + + # Optionally get and store Jaco joint velocity indices + jaco_joint_names_list = self.blackboard_try_get("jaco_joint_names") + if jaco_joint_names_list: + jaco_vel_indices = self._get_vel_indices_for_joints( + model, jaco_joint_names_list, "Jaco" + ) + if jaco_vel_indices is not None: + self.blackboard_set("jaco_vel_indices_pin", jaco_vel_indices) + self.logger.info( + f"[{self.name}] Stored Jaco velocity indices: {jaco_vel_indices}" + ) + # else: return Status.FAILURE # Fail if requested but not found + + # Optionally get and store Articutool joint velocity indices + articutool_joint_names_list = self.blackboard_try_get( + "articutool_joint_names" + ) + if articutool_joint_names_list: + articutool_vel_indices = self._get_vel_indices_for_joints( + model, articutool_joint_names_list, "Articutool" + ) + if articutool_vel_indices is not None: + self.blackboard_set( + "articutool_vel_indices_pin", articutool_vel_indices + ) + self.logger.info( + f"[{self.name}] Stored Articutool velocity indices: {articutool_vel_indices}" + ) + # else: return Status.FAILURE + + # Optionally get and store Jaco EE frame ID + jaco_ee_link_name_str = self.blackboard_try_get( + "jaco_end_effector_link_name" + ) + if jaco_ee_link_name_str: + if model.existFrame(jaco_ee_link_name_str): + jaco_ee_id = model.getFrameId(jaco_ee_link_name_str) + self.blackboard_set("jaco_ee_frame_id_pin", jaco_ee_id) + self.logger.info( + f"[{self.name}] Stored Jaco EE frame ID for '{jaco_ee_link_name_str}': {jaco_ee_id}" + ) + else: + self.logger.warn( + f"[{self.name}] Jaco EE link '{jaco_ee_link_name_str}' not found in Pinocchio model." + ) + # Not failing here, as it's optional + + # Optionally get and store tool tip frame ID + tool_tip_link_name_str = self.blackboard_try_get("tool_tip_link_name") + if tool_tip_link_name_str: + if model.existFrame(tool_tip_link_name_str): + tool_tip_id = model.getFrameId(tool_tip_link_name_str) + self.blackboard_set("tool_tip_frame_id_pin", tool_tip_id) + self.logger.info( + f"[{self.name}] Stored tool tip frame ID for '{tool_tip_link_name_str}': {tool_tip_id}" + ) + else: + self.logger.warn( + f"[{self.name}] Tool tip link '{tool_tip_link_name_str}' not found in Pinocchio model." + ) + + self._model_loaded_successfully = True + self.feedback_message = "Pinocchio model loaded successfully." + self.logger.info(f"[{self.name}] {self.feedback_message}") + return Status.SUCCESS + + except FileNotFoundError as e: + self.feedback_message = f"URDF/XACRO file not found: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + except subprocess.CalledProcessError as e: + self.feedback_message = f"XACRO processing failed: {e}\nOutput:\n{e.stderr}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + except KeyError as e: # For blackboard_get if a required key is missing + self.feedback_message = ( + f"Blackboard key error during setup/input reading: {e}" + ) + self.logger.error(f"[{self.name}] {self.feedback_message}") + except Exception as e: + self.feedback_message = f"Failed to load Pinocchio model: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + finally: + if temp_urdf_file_name and os.path.exists(temp_urdf_file_name): + try: + os.unlink(temp_urdf_file_name) + except OSError as e_unlink: + self.logger.error( + f"[{self.name}] Failed to delete temp URDF file: {e_unlink}" + ) + + return Status.FAILURE # Reached if any exception occurred + + @override + def terminate(self, new_status: Status) -> None: + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") From d3244e21646eb46d88d5773de5b17a8e2751cc5f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 11:07:37 -0700 Subject: [PATCH 069/238] Add behavior to check the kinematic feasibility of the Articutool achieving a desired orientation throughout a given trajectory --- ...articutool_path_orientation_feasibility.py | 423 ++++++++++++++++++ 1 file changed, 423 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py diff --git a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py new file mode 100644 index 00000000..a54966b5 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py @@ -0,0 +1,423 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +Defines the CheckArticutoolPathOrientationFeasibility behavior. +This behavior checks if the Articutool can maintain a desired world orientation +for its tool tip throughout a given Cartesian trajectory of the Jaco end-effector, +using an analytical IK adapted from benchmark scripts. +""" + +# Standard imports +import os +import math +from typing import Union, Optional, List, Tuple + +# Third-party imports +from geometry_msgs.msg import PoseStamped, Quaternion as QuaternionMsg, Pose +from moveit_msgs.msg import RobotTrajectory +import numpy as np +from overrides import override +from scipy.spatial.transform import Rotation # For R.from_quat and R.apply +import py_trees +import py_trees.blackboard +from py_trees.common import Status +import rclpy.node + +# Local imports +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class CheckArticutoolPathOrientationFeasibility(BlackboardBehavior): + """ + Checks if the Articutool can maintain a desired world orientation for its + tool tip throughout a given Cartesian trajectory of the Jaco end-effector. + It uses an analytical IK adapted from the orientation_constrained_planner_benchmark.py script. + """ + + # Constants adapted from benchmark for IK logic + EPSILON = 1e-6 + + def blackboard_inputs( + self, + jaco_ee_cartesian_trajectory: Union[BlackboardKey, RobotTrajectory], + desired_tool_tip_world_orientation: Union[BlackboardKey, QuaternionMsg], + articutool_joint_names: Union[ + BlackboardKey, List[str] + ], # Expected: ["pitch_joint", "roll_joint"] order + articutool_pitch_limits_rad: Union[ + BlackboardKey, Tuple[float, float] + ], # (min, max) + articutool_roll_limits_rad: Union[ + BlackboardKey, Tuple[float, float] + ], # (min, max) + num_trajectory_points_to_check: Union[BlackboardKey, int] = 10, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + jaco_ee_cartesian_trajectory: RobotTrajectory for the Jaco end-effector's Cartesian path. + Expected to contain poses in multi_dof_joint_trajectory. + desired_tool_tip_world_orientation: QuaternionMsg for the target world orientation of the tool_tip. + articutool_joint_names: List of Articutool joint names, e.g., ["atool_joint1", "atool_joint2"]. + The order should match how limits are provided and how IK solutions are interpreted. + articutool_pitch_limits_rad: Tuple (min_pitch, max_pitch) in radians for the first Articutool joint. + articutool_roll_limits_rad: Tuple (min_roll, max_roll) in radians for the second Articutool joint. + num_trajectory_points_to_check: Number of points to sample and check along the trajectory. + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + articutool_is_orientation_feasible: Optional[ + BlackboardKey + ], # -> Optional[bool] + # Optional outputs for debugging: + # first_failing_articutool_config: Optional[BlackboardKey], # -> Optional[List[float]] + # first_failing_jaco_ee_pose_idx: Optional[BlackboardKey], # -> Optional[int] + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + articutool_is_orientation_feasible: Boolean, True if Articutool can maintain orientation. + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def __init__(self, name: str, **kwargs): + super().__init__(name=name, **kwargs) + self.node: Optional[rclpy.node.Node] = None + # Store limits internally after setup for easier access + self._pitch_limits: Optional[Tuple[float, float]] = None + self._roll_limits: Optional[Tuple[float, float]] = None + + @override + def setup(self, **kwargs): + """Gets the ROS2 node and retrieves fixed parameters like joint limits.""" + try: + self.node = kwargs["node"] + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + return # Cannot proceed + + # Retrieve and store joint limits once during setup + try: + self._pitch_limits = self.blackboard_get("articutool_pitch_limits_rad") + self._roll_limits = self.blackboard_get("articutool_roll_limits_rad") + if not ( + isinstance(self._pitch_limits, tuple) + and len(self._pitch_limits) == 2 + and isinstance(self._roll_limits, tuple) + and len(self._roll_limits) == 2 + ): + self.logger.error( + f"[{self.name}] Articutool joint limits are not valid tuples of size 2." + ) + self._pitch_limits = None # Invalidate + self._roll_limits = None # Invalidate + except KeyError as e: + self.logger.error( + f"[{self.name}] Blackboard key error for joint limits: {e}" + ) + + def _normalize_angle(self, angle: float) -> float: + """Normalizes an angle to the range [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + + def _solve_articutool_ik_benchmark_style( + self, target_y_axis_in_jaco_hand_frame: np.ndarray + ) -> List[Tuple[float, float]]: + """ + Analytical IK solver for the Articutool, adapted from the benchmark script. + It solves for (theta_p, theta_r) to align the Articutool's effective + pointing Y-axis with the target_y_axis_in_jaco_hand_frame. + + The benchmark's IK implies the Articutool's pointing Y-axis in its base frame is: + [-sin(theta_r), cos(theta_p)cos(theta_r), sin(theta_p)cos(theta_r)] + + Parameters + ---------- + target_y_axis_in_jaco_hand_frame : np.ndarray + A 3D vector representing the desired orientation of the Articutool's + pointing Y-axis, expressed in the Jaco hand frame. + + Returns + ------- + List[Tuple[float, float]] + A list of (theta_p, theta_r) solutions in radians. + """ + vx, vy, vz = target_y_axis_in_jaco_hand_frame + solutions: List[Tuple[float, float]] = [] + + # From benchmark: asin_arg = np.clip(-vx, -1.0, 1.0) + # This means: target_x_in_jaco_hand = -sin(theta_r) + if not (-1.0 - self.EPSILON <= -vx <= 1.0 + self.EPSILON): + self.logger.debug( + f"[{self.name}] IK: -vx ({vx:.4f}) out of range for asin." + ) + return [] + + asin_arg_for_theta_r = np.clip(-vx, -1.0, 1.0) + theta_r_sol1 = math.asin(asin_arg_for_theta_r) + theta_r_sol2 = self._normalize_angle( + math.pi - theta_r_sol1 + ) # Second solution for asin + + candidate_thetas_r = [theta_r_sol1] + if not math.isclose(theta_r_sol1, theta_r_sol2, abs_tol=self.EPSILON): + candidate_thetas_r.append(theta_r_sol2) + + # self.logger.debug(f"IK: Target Y in JacoHand: [{vx:.3f}, {vy:.3f}, {vz:.3f}]") + # self.logger.debug(f"IK: Candidate theta_r values: {[round(r, 3) for r in candidate_thetas_r]}") + + for theta_r in candidate_thetas_r: + cos_theta_r = math.cos(theta_r) + + # From benchmark: theta_p_sol = math.atan2(vz / cos_theta_r, vy / cos_theta_r) + # This means: target_y_in_jaco_hand = cos(theta_p)cos(theta_r) + # target_z_in_jaco_hand = sin(theta_p)cos(theta_r) + if math.isclose(cos_theta_r, 0.0, abs_tol=self.EPSILON): + # If cos(theta_r) is 0, then sin(theta_r) is +/-1. + # This means -vx should be +/-1 (target_x_in_jaco_hand is +/-1). + # Also, target_y_in_jaco_hand and target_z_in_jaco_hand must be 0. + if math.isclose(vy, 0.0, abs_tol=self.EPSILON) and math.isclose( + vz, 0.0, abs_tol=self.EPSILON + ): + # theta_p is indeterminate (or can be chosen freely, e.g., 0) + # This configuration means the tool is pointing purely along Jaco hand's X-axis. + theta_p_sol = 0.0 # Choose a default, like 0 + solutions.append((theta_p_sol, theta_r)) + # self.logger.debug(f"IK: cos(theta_r) is zero, vy, vz also zero. Added solution ({theta_p_sol:.3f}, {theta_r:.3f})") + # else: + # self.logger.debug(f"IK: cos(theta_r) is zero, but vy or vz is non-zero. Infeasible for this theta_r.") + continue # Move to next theta_r candidate if any + + # Normal case: cos(theta_r) is not zero + # theta_p = atan2( (sin(theta_p)cos(theta_r)) , (cos(theta_p)cos(theta_r)) ) + # theta_p = atan2( vz , vy ) + theta_p_sol = math.atan2(vz, vy) + solutions.append( + (self._normalize_angle(theta_p_sol), self._normalize_angle(theta_r)) + ) + # self.logger.debug(f"IK: For theta_r={theta_r:.3f}, got theta_p={theta_p_sol:.3f}. Added solution.") + + return solutions + + @override + def initialise(self) -> None: + self.logger.debug(f"[{self.name}] Initializing.") + self.blackboard_set("articutool_is_orientation_feasible", None) + # self.blackboard_set("first_failing_articutool_config", None) + # self.blackboard_set("first_failing_jaco_ee_pose_idx", None) + + @override + def update(self) -> Status: + if not self.node: + self.feedback_message = "Node not initialized" + return Status.FAILURE + if self._pitch_limits is None or self._roll_limits is None: + self.feedback_message = "Articutool joint limits not set up correctly." + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + try: + trajectory: RobotTrajectory = self.blackboard_get( + "jaco_ee_cartesian_trajectory" + ) + desired_orientation_msg: QuaternionMsg = self.blackboard_get( + "desired_tool_tip_world_orientation" + ) + num_points_to_check: int = self.blackboard_get( + "num_trajectory_points_to_check" + ) + + if not isinstance( + trajectory, RobotTrajectory + ): # No points check needed if not RobotTrajectory + self.feedback_message = ( + "Input 'jaco_ee_cartesian_trajectory' is not a RobotTrajectory." + ) + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + if not isinstance(desired_orientation_msg, QuaternionMsg): + self.feedback_message = ( + "Input 'desired_tool_tip_world_orientation' is not a Quaternion." + ) + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + # Check if trajectory has points + # We expect poses in multi_dof_joint_trajectory for Cartesian paths + if not trajectory.multi_dof_joint_trajectory.points: + if trajectory.joint_trajectory.points: + self.feedback_message = "Trajectory has joint points but no multi_dof (Cartesian) points. FK needed but not implemented here." + self.logger.error(f"[{self.name}] {self.feedback_message}") + else: + self.feedback_message = "Input Jaco EE trajectory is empty." + self.logger.warn( + f"[{self.name}] {self.feedback_message} Considering feasible as no points to fail." + ) + # If no points, it's vacuously feasible, or could be failure depending on requirements. + # Let's say it's feasible if no points to check. + self.blackboard_set("articutool_is_orientation_feasible", True) + return Status.SUCCESS + + R_World_TipTarget = Rotation.from_quat( + [ + desired_orientation_msg.x, + desired_orientation_msg.y, + desired_orientation_msg.z, + desired_orientation_msg.w, + ] + ) + # This is the Y-axis of the desired tool tip orientation, expressed in World frame. + # This is the vector that the Articutool's "pointing Y-axis" should align with in the world. + y_axis_TipTarget_InWorld = R_World_TipTarget.apply( + np.array([0.0, 1.0, 0.0]) + ) + + jaco_ee_poses_from_traj: List[Pose] = [] + for mjt_point in trajectory.multi_dof_joint_trajectory.points: + if mjt_point.transforms: + transform = mjt_point.transforms[ + 0 + ] # Assuming first transform is the EE + pose = Pose() + pose.position.x = transform.translation.x + pose.position.y = transform.translation.y + pose.position.z = transform.translation.z + pose.orientation = transform.rotation + jaco_ee_poses_from_traj.append(pose) + + if not jaco_ee_poses_from_traj: + self.feedback_message = ( + "No poses extracted from multi_dof_joint_trajectory." + ) + self.logger.warn(f"[{self.name}] {self.feedback_message}") + self.blackboard_set( + "articutool_is_orientation_feasible", True + ) # Vacuously true + return Status.SUCCESS + + indices_to_check = [] + if len(jaco_ee_poses_from_traj) == 1: + indices_to_check = [0] + elif num_points_to_check >= len( + jaco_ee_poses_from_traj + ): # Check all if fewer than num_points + indices_to_check = list(range(len(jaco_ee_poses_from_traj))) + elif num_points_to_check > 0: + indices_to_check = np.linspace( + 0, len(jaco_ee_poses_from_traj) - 1, num_points_to_check, dtype=int + ) + + if ( + not indices_to_check and num_points_to_check > 0 + ): # Should not happen if jaco_ee_poses_from_traj is not empty + self.logger.warn( + f"[{self.name}] No indices to check despite having trajectory points and num_points_to_check > 0." + ) + self.blackboard_set("articutool_is_orientation_feasible", True) + return Status.SUCCESS + + for point_idx_in_indices, actual_traj_idx in enumerate(indices_to_check): + jaco_ee_pose_msg: Pose = jaco_ee_poses_from_traj[actual_traj_idx] + + R_World_JacoEE = Rotation.from_quat( + [ + jaco_ee_pose_msg.orientation.x, + jaco_ee_pose_msg.orientation.y, + jaco_ee_pose_msg.orientation.z, + jaco_ee_pose_msg.orientation.w, + ] + ) + + # Transform the desired tool tip Y-axis (in World) into the Jaco EE (Articutool Base) frame + target_y_axis_for_ik_in_jaco_hand = R_World_JacoEE.inv().apply( + y_axis_TipTarget_InWorld + ) + + # Normalize for robustness + norm_target_y = np.linalg.norm(target_y_axis_for_ik_in_jaco_hand) + if norm_target_y < self.EPSILON: + self.feedback_message = f"Target Y-axis for IK became zero vector at trajectory point {actual_traj_idx}." + self.logger.error(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_is_orientation_feasible", False) + return Status.FAILURE + target_y_axis_for_ik_in_jaco_hand /= norm_target_y + + ik_solutions = self._solve_articutool_ik_benchmark_style( + target_y_axis_for_ik_in_jaco_hand + ) + + found_valid_solution_for_waypoint = False + best_solution_this_waypoint = None + + for theta_p_sol, theta_r_sol in ik_solutions: + # Normalize angles before checking limits + theta_p_norm = self._normalize_angle(theta_p_sol) + theta_r_norm = self._normalize_angle(theta_r_sol) + + if ( + self._pitch_limits[0] - self.EPSILON + <= theta_p_norm + <= self._pitch_limits[1] + self.EPSILON + and self._roll_limits[0] - self.EPSILON + <= theta_r_norm + <= self._roll_limits[1] + self.EPSILON + ): + found_valid_solution_for_waypoint = True + best_solution_this_waypoint = ( + theta_p_norm, + theta_r_norm, + ) # Take the first valid one + break + + if not found_valid_solution_for_waypoint: + self.feedback_message = ( + f"Articutool IK failed or solution out of limits at trajectory point index {actual_traj_idx}. " + f"Target Y in JacoHand: {np.round(target_y_axis_for_ik_in_jaco_hand, 3)}. " + f"Raw IK solutions: {[(round(s[0], 3), round(s[1], 3)) for s in ik_solutions]}" + ) + self.logger.warn(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_is_orientation_feasible", False) + # self.blackboard_set("first_failing_jaco_ee_pose_idx", actual_traj_idx) + return Status.FAILURE + + self.logger.debug( + f"[{self.name}] Pt {actual_traj_idx}: JacoEE Ori (quat xyzw): " + f"[{jaco_ee_pose_msg.orientation.x:.2f}, ..., {jaco_ee_pose_msg.orientation.w:.2f}], " + f"Target Y_JacoHand: {np.round(target_y_axis_for_ik_in_jaco_hand, 2)}, " + f"Articutool q_sol: [{best_solution_this_waypoint[0]:.3f}, {best_solution_this_waypoint[1]:.3f}]" + ) + + self.feedback_message = ( + "Articutool can maintain orientation throughout Jaco trajectory." + ) + self.logger.info(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_is_orientation_feasible", True) + return Status.SUCCESS + + except KeyError as e: + self.feedback_message = f"Blackboard key error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + except Exception as e: + self.feedback_message = f"Unexpected error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + @override + def terminate(self, new_status: Status) -> None: + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") From 99e689369d66d083101d529611f54278c1fa8a23 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 11:07:53 -0700 Subject: [PATCH 070/238] Add initial imports --- ada_feeding/ada_feeding/behaviors/state/__init__.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py index 97e5f21e..7a616714 100644 --- a/ada_feeding/ada_feeding/behaviors/state/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -23,3 +23,9 @@ from .check_jaco_directional_manipulability import ( CheckJacoDirectionalManipulability, ) +from .check_articutool_path_orientation_feasibility import ( + CheckArticutoolPathOrientationFeasibility, +) +from .load_pinocchio_model import ( + LoadPinocchioModel, +) From bfad643e62056173f64208d39dc244e1aaa393fe Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 11:10:53 -0700 Subject: [PATCH 071/238] (WIP) Incorporate Articutool kinematic feasibility check. Currently, the feasibility check expects a RobotTrajectory, but we give it a JointTrajectory from MoveIt2 plan. --- .../ada_feeding/trees/acquire_food_tree.py | 70 +++++++++++++++++-- 1 file changed, 64 insertions(+), 6 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 038da980..631b7734 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -55,6 +55,7 @@ ExtractPoseFromPosesByLink, ExtractPoseComponents, CheckJacoDirectionalManipulability, + CheckArticutoolPathOrientationFeasibility, ) from ada_feeding.behaviors.ros.msgs import StampPoseFromPose from ada_feeding.behaviors.ros.tf import ApplyTransform @@ -882,25 +883,28 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: }, ), MoveIt2ComputeFK( - name="ComputeMoveIntoJacoArmEEPose", + name="ComputeMoveIntoPoses", ns=name, inputs={ "group_name": "jaco_arm_with_articutool", "joint_state": BlackboardKey("move_into_ik_solution_8dof"), - "fk_link_names": ["j2n6s200_end_effector"], + "fk_link_names": ["j2n6s200_end_effector", "tool_tip"], }, outputs={ - "fk_poses": BlackboardKey("move_into_jaco_arm_ee_poses"), + "fk_poses": BlackboardKey("move_into_poses"), "success": None, }, ), ExtractPoseFromPosesByLink( - name="GetJacoArmEEPose", + name="GetMoveIntoJacoArmEEPose", ns=name, inputs={ - "fk_poses": BlackboardKey("move_into_jaco_arm_ee_poses"), + "fk_poses": BlackboardKey("move_into_poses"), "target_link_name": "j2n6s200_end_effector", - "requested_link_names": ["j2n6s200_end_effector"], + "requested_link_names": [ + "j2n6s200_end_effector", + "tool_tip", + ], }, outputs={ "extracted_pose": BlackboardKey( @@ -909,6 +913,22 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: "success": None, }, ), + ExtractPoseFromPosesByLink( + name="GetMoveIntoToolTipPose", + ns=name, + inputs={ + "fk_poses": BlackboardKey("move_into_poses"), + "target_link_name": "tool_tip", + "requested_link_names": [ + "j2n6s200_end_effector", + "tool_tip", + ], + }, + outputs={ + "extracted_pose": BlackboardKey("move_into_tool_tip_pose"), + "success": None, + }, + ), MoveIt2PoseConstraint( name="MoveIntoJacoArmEEPoseConstraint", ns=name, @@ -950,6 +970,44 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: }, ), ), + ExtractPoseComponents( + name="ExtractMoveIntoPoseComponents", + ns=name, + inputs={ + "input_pose_object": BlackboardKey( + "move_into_tool_tip_pose" + ), + }, + outputs={ + "output_position": None, + "output_orientation": BlackboardKey( + "move_into_tool_tip_orientation" + ), + "output_header": None, + "success": None, + }, + ), + CheckArticutoolPathOrientationFeasibility( + name="CheckArticutoolFeasibilityForMoveInto", + ns=name, + inputs={ + "jaco_ee_cartesian_trajectory": BlackboardKey( + "move_into_jaco_arm_trajectory" + ), + "desired_tool_tip_world_orientation": BlackboardKey( + "move_into_tool_tip_orientation" + ), + "articutool_joint_names": ["atool_joint1", "atool_joint2"], + "articutool_pitch_limits_rad": (-np.pi / 2, np.pi / 2), + "articutool_roll_limits_rad": (-np.pi, np.pi), + "num_trajectory_points_to_check": 20, + }, + outputs={ + "articutool_is_orientation_feasible": BlackboardKey( + "articutool_can_maintain_scoop_angle" + ) + }, + ), ], ) From 8e1a4930a0fd973cb0b53a61c8fc04f6b61e2186 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 11:28:08 -0700 Subject: [PATCH 072/238] Bug fixes in load_pinocchio_model.py --- .../behaviors/state/load_pinocchio_model.py | 13 +++++-------- 1 file changed, 5 insertions(+), 8 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/load_pinocchio_model.py b/ada_feeding/ada_feeding/behaviors/state/load_pinocchio_model.py index 9d7924e7..72c439f7 100644 --- a/ada_feeding/ada_feeding/behaviors/state/load_pinocchio_model.py +++ b/ada_feeding/ada_feeding/behaviors/state/load_pinocchio_model.py @@ -13,6 +13,7 @@ import subprocess import tempfile from typing import Union, Optional, List, Dict, Any +from overrides import override # Third-party imports import pinocchio as pin @@ -219,7 +220,7 @@ def update(self) -> Status: self.blackboard_set("pinocchio_data", data) # Optionally get and store Jaco joint velocity indices - jaco_joint_names_list = self.blackboard_try_get("jaco_joint_names") + jaco_joint_names_list = self.blackboard_get("jaco_joint_names") if jaco_joint_names_list: jaco_vel_indices = self._get_vel_indices_for_joints( model, jaco_joint_names_list, "Jaco" @@ -232,9 +233,7 @@ def update(self) -> Status: # else: return Status.FAILURE # Fail if requested but not found # Optionally get and store Articutool joint velocity indices - articutool_joint_names_list = self.blackboard_try_get( - "articutool_joint_names" - ) + articutool_joint_names_list = self.blackboard_get("articutool_joint_names") if articutool_joint_names_list: articutool_vel_indices = self._get_vel_indices_for_joints( model, articutool_joint_names_list, "Articutool" @@ -249,9 +248,7 @@ def update(self) -> Status: # else: return Status.FAILURE # Optionally get and store Jaco EE frame ID - jaco_ee_link_name_str = self.blackboard_try_get( - "jaco_end_effector_link_name" - ) + jaco_ee_link_name_str = self.blackboard_get("jaco_end_effector_link_name") if jaco_ee_link_name_str: if model.existFrame(jaco_ee_link_name_str): jaco_ee_id = model.getFrameId(jaco_ee_link_name_str) @@ -266,7 +263,7 @@ def update(self) -> Status: # Not failing here, as it's optional # Optionally get and store tool tip frame ID - tool_tip_link_name_str = self.blackboard_try_get("tool_tip_link_name") + tool_tip_link_name_str = self.blackboard_get("tool_tip_link_name") if tool_tip_link_name_str: if model.existFrame(tool_tip_link_name_str): tool_tip_id = model.getFrameId(tool_tip_link_name_str) From 88da136a1d45d161872c6107fda471cb0e98a5e2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 11:28:50 -0700 Subject: [PATCH 073/238] Integrate behavior to load pinocchio model for re-use in other behaviors that need a Pinocchio model for kinematic computation --- .../ada_feeding/trees/acquire_food_tree.py | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 631b7734..f4ea2067 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -56,6 +56,7 @@ ExtractPoseComponents, CheckJacoDirectionalManipulability, CheckArticutoolPathOrientationFeasibility, + LoadPinocchioModel, ) from ada_feeding.behaviors.ros.msgs import StampPoseFromPose from ada_feeding.behaviors.ros.tf import ApplyTransform @@ -614,6 +615,40 @@ def pre_acquisition_sequence( ) }, ), + LoadPinocchioModel( + name="LoadPinocchioModel", + ns=name, + inputs={ + "urdf_file_path": "package://ada_moveit/config/ada.urdf.xacro", + "jaco_joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ], + "articutool_joint_names": ["atool_joint1", "atool_joint2"], + "jaco_end_effector_link_name": "j2n6s200_end_effector", + "tool_tip_link_name": "tool_tip", + }, + outputs={ + "pinocchio_model": BlackboardKey("pinocchio_model"), + "pinocchio_data": BlackboardKey("pinocchio_data"), + "jaco_vel_indices_pin": BlackboardKey( + "jaco_vel_indices_pin" + ), + "articutool_vel_indices_pin": BlackboardKey( + "articutool_vel_indices_pin" + ), + "jaco_ee_frame_id_pin": BlackboardKey( + "jaco_ee_frame_id_pin" + ), + "tool_tip_frame_id_pin": BlackboardKey( + "tool_tip_frame_id_pin" + ), + }, + ), ], ) From 56210017e09b2e723654be0215d6c08eacc00756 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 12:13:36 -0700 Subject: [PATCH 074/238] Update CheckJacoDirectionalManipulability to use the pre-loaded Pinocchio model instead of instantiating one internally --- .../check_jaco_directional_manipulability.py | 375 ++++++------------ .../ada_feeding/trees/acquire_food_tree.py | 18 +- 2 files changed, 139 insertions(+), 254 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py b/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py index 6fc9bdad..e8237743 100644 --- a/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py +++ b/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py @@ -6,14 +6,12 @@ Defines the CheckJacoDirectionalManipulability behavior. This behavior evaluates if the Jaco arm, at a given configuration, has sufficient translational manipulability for its end-effector to move -along a specified Cartesian direction (derived from desired tool tip motion). +along a specified Cartesian direction. It uses a pre-loaded Pinocchio model +from the blackboard. """ # Standard imports -import os -import subprocess -import tempfile -import math # Added for cos/sin +import math from typing import Union, Optional, List, Dict, Any # Third-party imports @@ -26,37 +24,46 @@ import py_trees.blackboard from py_trees.common import Access, Status import rclpy.node -import rclpy.time # Local imports -from ada_feeding.helpers import BlackboardKey # Assuming this is your helper -from ada_feeding.behaviors import BlackboardBehavior # Assuming this is your base class +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior class CheckJacoDirectionalManipulability(BlackboardBehavior): """ - Checks the Jaco arm's directional translational manipulability. - The manipulability is assessed for the Jaco end-effector at the current - "Move Above" configuration, for moving in a direction derived from the - desired tool tip motion (from tool_tip_move_above_pose to tool_tip_move_into_pose). + Checks Jaco arm's directional translational manipulability using a pre-loaded + Pinocchio model and pre-calculated joint/frame IDs from the blackboard. """ def blackboard_inputs( self, - current_full_robot_joint_state_MA: Union[ - BlackboardKey, JointState - ], # Full state for Pinocchio FK/Jacobian + pinocchio_model: Union[BlackboardKey, pin.Model], + pinocchio_data: Union[BlackboardKey, pin.Data], + jaco_vel_indices_pin: Union[ + BlackboardKey, List[int] + ], # Pre-calculated velocity indices for Jaco joints + jaco_ee_frame_id_pin: Union[ + BlackboardKey, int + ], # Pre-calculated frame ID for Jaco EE + current_full_robot_joint_state_MA: Union[BlackboardKey, JointState], tool_tip_move_above_pose_world: Union[BlackboardKey, PoseStamped], tool_tip_move_into_pose_world: Union[BlackboardKey, PoseStamped], - jaco_joint_names: Union[BlackboardKey, List[str]], - jaco_end_effector_link_name: Union[ - BlackboardKey, str - ], # Link whose manipulability is checked directional_manipulability_threshold: Union[BlackboardKey, float], - urdf_file_path: Union[BlackboardKey, str], ) -> None: """ Blackboard Inputs + + Parameters + ---------- + pinocchio_model: Pinocchio model object (from blackboard). + pinocchio_data: Pinocchio data object (from blackboard). + jaco_vel_indices_pin: List of Pinocchio velocity indices for Jaco joints (from blackboard). + jaco_ee_frame_id_pin: Pinocchio frame ID for the Jaco end-effector (from blackboard). + current_full_robot_joint_state_MA: JointState of the full robot at "Move Above". + tool_tip_move_above_pose_world: PoseStamped of tool tip at "Move Above" in world. + tool_tip_move_into_pose_world: PoseStamped of tool tip at "Move Into" in world. + directional_manipulability_threshold: Minimum acceptable score. """ super().blackboard_inputs( **{key: value for key, value in locals().items() if key != "self"} @@ -64,13 +71,12 @@ def blackboard_inputs( def blackboard_outputs( self, - jaco_directional_manipulability_score: Optional[ - BlackboardKey - ], # -> Optional[float] - jaco_is_manipulable_for_direction: Optional[BlackboardKey], # -> Optional[bool] + jaco_directional_manipulability_score: Optional[BlackboardKey], + jaco_is_manipulable_for_direction: Optional[BlackboardKey], ) -> None: """ Blackboard Outputs + (Same as before) """ super().blackboard_outputs( **{key: value for key, value in locals().items() if key != "self"} @@ -78,198 +84,107 @@ def blackboard_outputs( def __init__(self, name: str, **kwargs): super().__init__(name=name, **kwargs) - self.pin_model: Optional[pin.Model] = None - self.pin_data: Optional[pin.Data] = None - self.jaco_pin_vel_indices: List[int] = [] - self.jaco_ee_frame_id: Optional[int] = None self.node: Optional[rclpy.node.Node] = None - self._urdf_loaded_successfully = False + # Pinocchio objects will be read from blackboard in update/initialise + self._pin_model: Optional[pin.Model] = None + self._pin_data: Optional[pin.Data] = None + self._jaco_vel_indices: Optional[List[int]] = None + self._jaco_ee_frame_id: Optional[int] = None @override def setup(self, **kwargs): - """Loads the Pinocchio model and gets joint/frame IDs.""" + """Gets the ROS2 node from arguments.""" try: self.node = kwargs["node"] except KeyError as e: self.logger.error( f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" ) - return - - if self._urdf_loaded_successfully: # Avoid reloading if already done - self.logger.debug(f"[{self.name}] Pinocchio model already loaded.") - return - - urdf_path_str: str = self.blackboard_get("urdf_file_path") - if not urdf_path_str: - self.logger.error( - f"[{self.name}] 'urdf_file_path' not provided or is empty." - ) - return + # Node is critical, update will fail if it's None - resolved_urdf_path = self._resolve_package_path(urdf_path_str) - if not resolved_urdf_path: - return # Error already logged by _resolve_package_path - - processed_urdf_path = resolved_urdf_path - temp_urdf_file_name = None # Store name for cleanup + @override + def initialise(self) -> None: + """ + Reads Pinocchio model, data, and pre-calculated IDs from the blackboard. + Clears previous outputs. + """ + self.logger.debug( + f"[{self.name}] Initializing and attempting to read Pinocchio data from blackboard." + ) + self.blackboard_set("jaco_directional_manipulability_score", None) + self.blackboard_set("jaco_is_manipulable_for_direction", None) try: - if resolved_urdf_path.endswith(".xacro"): - self.logger.info( - f"[{self.name}] Processing XACRO file: {resolved_urdf_path}" - ) - with tempfile.NamedTemporaryFile( - mode="w", suffix=".urdf", delete=False - ) as temp_file: - temp_urdf_file_name = temp_file.name - process = subprocess.run( - ["ros2", "run", "xacro", "xacro", resolved_urdf_path], - check=True, - capture_output=True, - text=True, - ) - temp_file.write(process.stdout) - processed_urdf_path = temp_urdf_file_name + self._pin_model = self.blackboard_get("pinocchio_model") + self._pin_data = self.blackboard_get("pinocchio_data") + self._jaco_vel_indices = self.blackboard_get("jaco_vel_indices_pin") + self._jaco_ee_frame_id = self.blackboard_get("jaco_ee_frame_id_pin") - self.logger.info(f"[{self.name}] Loading URDF from: {processed_urdf_path}") - self.pin_model = pin.buildModelFromUrdf(processed_urdf_path) - self.pin_data = self.pin_model.createData() - self.logger.info( - f"[{self.name}] Pinocchio model loaded: {self.pin_model.name}, Nq={self.pin_model.nq}, Nv={self.pin_model.nv}" - ) - - jaco_joint_names_list: List[str] = self.blackboard_get("jaco_joint_names") - if not jaco_joint_names_list: + if not isinstance(self._pin_model, pin.Model): self.logger.error( - f"[{self.name}] 'jaco_joint_names' list is empty or not provided." + f"[{self.name}] 'pinocchio_model' from blackboard is not a valid Pinocchio Model." ) - self.pin_model = None # Invalidate model - return - - self.jaco_pin_vel_indices = ( - [] - ) # Reset for safety if setup is called multiple times - for joint_name in jaco_joint_names_list: - if self.pin_model.existJointName(joint_name): - joint_id = self.pin_model.getJointId(joint_name) - if ( - self.pin_model.joints[joint_id].nv == 1 - ): # Check if this joint is 1 DoF in velocity space - self.jaco_pin_vel_indices.append( - self.pin_model.joints[joint_id].idx_v - ) - else: - # This error should ideally not be hit for standard Jaco joints if Pinocchio parses them as revolute. - self.logger.error( - f"[{self.name}] Jaco joint '{joint_name}' (Pinocchio type: {self.pin_model.joints[joint_id].shortname()}) has nv={self.pin_model.joints[joint_id].nv} != 1. Not supported for simple vel_indices list." - ) - self.pin_model = None - return # Invalidate model and return - else: - self.logger.error( - f"[{self.name}] Jaco joint '{joint_name}' not found in Pinocchio model." - ) - self.pin_model = None # Invalidate model - return - - if not self.jaco_pin_vel_indices: + self._pin_model = None # Invalidate + if not isinstance(self._pin_data, pin.Data): self.logger.error( - f"[{self.name}] No Jaco joint velocity indices could be determined." + f"[{self.name}] 'pinocchio_data' from blackboard is not a valid Pinocchio Data." ) - self.pin_model = None - return - self.logger.info( - f"[{self.name}] Jaco velocity indices in Pinocchio model: {self.jaco_pin_vel_indices}" - ) - - jaco_ee_link: str = self.blackboard_get("jaco_end_effector_link_name") - if not jaco_ee_link: + self._pin_data = None # Invalidate + if not isinstance(self._jaco_vel_indices, list) or not all( + isinstance(i, int) for i in self._jaco_vel_indices + ): self.logger.error( - f"[{self.name}] 'jaco_end_effector_link_name' not provided." + f"[{self.name}] 'jaco_vel_indices_pin' from blackboard is not a valid List[int]." ) - self.pin_model = None # Invalidate model - return - if self.pin_model.existFrame(jaco_ee_link): - self.jaco_ee_frame_id = self.pin_model.getFrameId(jaco_ee_link) - else: + self._jaco_vel_indices = None # Invalidate + if not isinstance(self._jaco_ee_frame_id, int): self.logger.error( - f"[{self.name}] Jaco EE link '{jaco_ee_link}' not found in Pinocchio model." + f"[{self.name}] 'jaco_ee_frame_id_pin' from blackboard is not a valid int." ) - self.pin_model = None # Invalidate model - return - - self._urdf_loaded_successfully = True + self._jaco_ee_frame_id = None # Invalidate - except FileNotFoundError: + except KeyError as e: self.logger.error( - f"[{self.name}] URDF/XACRO file not found at: {resolved_urdf_path}" + f"[{self.name}] Failed to get required Pinocchio info from blackboard: {e}. Ensure LoadPinocchioModel ran successfully." ) - self.pin_model = None - except subprocess.CalledProcessError as e: + self._pin_model = None # Ensure invalidated on error + self._pin_data = None + self._jaco_vel_indices = None + self._jaco_ee_frame_id = None + + if ( + self._pin_model + and self._pin_data + and self._jaco_vel_indices + and self._jaco_ee_frame_id is not None + ): + self.logger.debug( + f"[{self.name}] Successfully retrieved Pinocchio model, data, and Jaco IDs from blackboard." + ) + else: self.logger.error( - f"[{self.name}] XACRO processing failed: {e}\nOutput:\n{e.stderr}" + f"[{self.name}] Failed to properly initialize with Pinocchio data from blackboard." ) - self.pin_model = None - except Exception as e: - self.logger.error(f"[{self.name}] Failed to load Pinocchio model: {e}") - self.pin_model = None - finally: - if ( - temp_urdf_file_name - and os.path.exists(temp_urdf_file_name) - and processed_urdf_path == temp_urdf_file_name - ): - try: - os.unlink(processed_urdf_path) - except OSError as e_unlink: - self.logger.error( - f"[{self.name}] Failed to delete temp URDF file {processed_urdf_path}: {e_unlink}" - ) - - def _resolve_package_path(self, path_str: str) -> Optional[str]: - """Resolves package:// paths to absolute paths.""" - if path_str.startswith("package://"): - try: - parts = path_str.split("package://", 1)[1].split("/", 1) - package_name = parts[0] - relative_path = parts[1] if len(parts) > 1 else "" - - from ament_index_python.packages import get_package_share_directory - - package_share_directory = get_package_share_directory(package_name) - return os.path.join(package_share_directory, relative_path) - except Exception as e: - self.logger.error( - f"[{self.name}] Could not resolve package path '{path_str}': {e}" - ) - return None - return path_str - - @override - def initialise(self) -> None: - """Clear previous outputs.""" - self.logger.debug(f"[{self.name}] Initializing.") - self.blackboard_set("jaco_directional_manipulability_score", None) - self.blackboard_set("jaco_is_manipulable_for_direction", None) @override def update(self) -> Status: if not self.node: - self.feedback_message = "Node not initialized" + self.feedback_message = "Node not initialized in setup." + self.logger.error(f"[{self.name}] {self.feedback_message}") return Status.FAILURE + + # Check if Pinocchio objects were successfully retrieved in initialise if ( - not self._urdf_loaded_successfully - or not self.pin_model - or not self.pin_data - or self.jaco_ee_frame_id is None - or not self.jaco_pin_vel_indices + not self._pin_model + or not self._pin_data + or self._jaco_ee_frame_id is None + or not self._jaco_vel_indices ): self.feedback_message = ( - "Pinocchio model/data not loaded or required IDs not found" + "Pinocchio model/data/IDs not available from blackboard." ) self.logger.error( - f"[{self.name}] {self.feedback_message} during update. Setup likely failed." + f"[{self.name}] {self.feedback_message} Ensure LoadPinocchioModel ran successfully and populated the blackboard." ) return Status.FAILURE @@ -296,60 +211,41 @@ def update(self) -> Status: return Status.FAILURE # --- Populate Pinocchio's q vector (q_MA) --- - q_MA = pin.neutral(self.pin_model) + q_MA = pin.neutral(self._pin_model) # Use the model from blackboard num_updated_joints_in_q = 0 for i, name_from_js in enumerate(current_q_robot_state.name): - if self.pin_model.existJointName(name_from_js): - joint_id_pin = self.pin_model.getJointId(name_from_js) - joint_obj_pin = self.pin_model.joints[joint_id_pin] - pin_joint_name = self.pin_model.names[joint_id_pin] + if self._pin_model.existJointName(name_from_js): + joint_id_pin = self._pin_model.getJointId(name_from_js) + joint_obj_pin = self._pin_model.joints[joint_id_pin] + pin_joint_name = self._pin_model.names[joint_id_pin] theta = current_q_robot_state.position[i] - if ( - joint_obj_pin.nq == 2 and joint_obj_pin.nv == 1 - ): # Typical for revolute joints in Pinocchio - # Expects [cos(theta), sin(theta)] + if joint_obj_pin.nq == 2 and joint_obj_pin.nv == 1: q_MA[joint_obj_pin.idx_q] = math.cos(theta) q_MA[joint_obj_pin.idx_q + 1] = math.sin(theta) - num_updated_joints_in_q += ( - 1 # Counts as one "joint" from JointState - ) - self.logger.debug( - f"[{self.name}] Updated revolute joint '{pin_joint_name}' (nq=2) in q_MA with [cos({theta:.3f}), sin({theta:.3f})]" - ) - elif ( - joint_obj_pin.nq == 1 and joint_obj_pin.nv == 1 - ): # E.g. Prismatic joint + num_updated_joints_in_q += 1 + elif joint_obj_pin.nq == 1 and joint_obj_pin.nv == 1: q_MA[joint_obj_pin.idx_q] = theta num_updated_joints_in_q += 1 - self.logger.debug( - f"[{self.name}] Updated prismatic/nq=1 joint '{pin_joint_name}' in q_MA with {theta:.3f}" - ) - elif joint_obj_pin.nq > 1: # Other complex joints, e.g. FreeFlyer + elif joint_obj_pin.nq > 1: if ( - pin_joint_name == self.pin_model.names[1] + pin_joint_name == self._pin_model.names[1] and joint_obj_pin.shortname() == "JointModelFreeFlyer" ): - if (i + 7) <= len( - current_q_robot_state.position - ): # Check if enough values are present + if (i + 7) <= len(current_q_robot_state.position): q_MA[joint_obj_pin.idx_q : joint_obj_pin.idx_q + 7] = ( current_q_robot_state.position[i : i + 7] ) - num_updated_joints_in_q += 1 # Counts as one "entry" from JointState perspective - self.logger.debug( - f"[{self.name}] Updated FreeFlyer base joint '{pin_joint_name}' in q_MA." - ) + num_updated_joints_in_q += 1 else: self.logger.warn( f"[{self.name}] FreeFlyer base joint '{pin_joint_name}' found, but not enough values in JointState (from index {i}) to populate q (expected 7). Using neutral for base." ) else: self.logger.warn( - f"[{self.name}] Joint '{name_from_js}' (Pinocchio name: '{pin_joint_name}') has nq={joint_obj_pin.nq}, nv={joint_obj_pin.nv}. " - f"Unhandled complex joint type in q_MA population. It will remain neutral if not FreeFlyer." + f"[{self.name}] Joint '{name_from_js}' (Pinocchio name: '{pin_joint_name}') has nq={joint_obj_pin.nq}, nv={joint_obj_pin.nv}. Unhandled complex joint type." ) else: self.logger.warn( @@ -358,39 +254,46 @@ def update(self) -> Status: if num_updated_joints_in_q == 0: self.logger.error( - f"[{self.name}] No joints in Pinocchio's q were updated from input JointState. Check joint names and URDF consistency." + f"[{self.name}] No joints in Pinocchio's q were updated from input JointState." ) return Status.FAILURE - jaco_joint_names_bb: List[str] = self.blackboard_get("jaco_joint_names") + jaco_joint_names_bb: List[str] = self.blackboard_get( + "jaco_joint_names" + ) # For logging log_jaco_q_values = [] - for name in jaco_joint_names_bb: - if self.pin_model.existJointName(name): - jid = self.pin_model.getJointId(name) - j_obj = self.pin_model.joints[jid] + for ( + name_j + ) in ( + jaco_joint_names_bb + ): # Use 'name_j' to avoid conflict with outer 'name' + if self._pin_model.existJointName(name_j): + jid = self._pin_model.getJointId(name_j) + j_obj = self._pin_model.joints[jid] if j_obj.nq == 1: - log_jaco_q_values.append(f"{name}: {q_MA[j_obj.idx_q]:.3f}") + log_jaco_q_values.append(f"{name_j}: {q_MA[j_obj.idx_q]:.3f}") elif j_obj.nq == 2: log_jaco_q_values.append( - f"{name}: [{q_MA[j_obj.idx_q]:.3f}, {q_MA[j_obj.idx_q + 1]:.3f}]" + f"{name_j}: [{q_MA[j_obj.idx_q]:.3f}, {q_MA[j_obj.idx_q + 1]:.3f}]" ) self.logger.info( f"[{self.name}] Populated q_MA for Jaco joints: {{{', '.join(log_jaco_q_values)}}}" ) - # --- End q_MA Population --- - pin.forwardKinematics(self.pin_model, self.pin_data, q_MA) - pin.updateFramePlacements(self.pin_model, self.pin_data) + pin.forwardKinematics(self._pin_model, self._pin_data, q_MA) + pin.updateFramePlacements(self._pin_model, self._pin_data) J_jacoEE_world_full = pin.computeFrameJacobian( - self.pin_model, - self.pin_data, + self._pin_model, + self._pin_data, q_MA, - self.jaco_ee_frame_id, + self._jaco_ee_frame_id, pin.ReferenceFrame.WORLD, ) - J_jacoEE_world_jaco = J_jacoEE_world_full[:, self.jaco_pin_vel_indices] + J_jacoEE_world_jaco = J_jacoEE_world_full[ + :, self._jaco_vel_indices + ] # Use pre-calculated indices J_jacoEE_world_jaco_trans = J_jacoEE_world_jaco[:3, :] self.logger.info( f"[{self.name}] Jaco EE Trans Jacobian (J_jacoEE_world_jaco_trans):\n{J_jacoEE_world_jaco_trans}" @@ -413,12 +316,6 @@ def update(self) -> Status: task_direction_vec = pos_MI_tip - pos_MA_tip norm_task_direction = np.linalg.norm(task_direction_vec) - self.logger.info( - f"[{self.name}] Tool Tip MA Pose: {pos_MA_tip}, MI Pose: {pos_MI_tip}" - ) - self.logger.info( - f"[{self.name}] Task Direction Vector (tool tip): {task_direction_vec}, Norm: {norm_task_direction}" - ) if norm_task_direction < 1e-6: self.feedback_message = "Task direction vector (tool tip) is near zero." @@ -427,18 +324,11 @@ def update(self) -> Status: self.blackboard_set("jaco_is_manipulable_for_direction", False) return Status.FAILURE - unit_task_direction_tip = task_direction_vec / norm_task_direction - unit_task_direction_jacoEE = unit_task_direction_tip - self.logger.info( - f"[{self.name}] Unit Task Direction (Jaco EE): {unit_task_direction_jacoEE}" - ) + unit_task_direction_jacoEE = task_direction_vec / norm_task_direction JJT_trans_jacoEE = J_jacoEE_world_jaco_trans @ J_jacoEE_world_jaco_trans.T - self.logger.info( - f"[{self.name}] JJT_trans_jacoEE (3x3 matrix):\n{JJT_trans_jacoEE}" - ) det_JJT = np.linalg.det(JJT_trans_jacoEE) - self.logger.info( + self.logger.debug( f"[{self.name}] Determinant of JJT_trans_jacoEE: {det_JJT:.6e}" ) @@ -447,9 +337,6 @@ def update(self) -> Status: @ JJT_trans_jacoEE @ unit_task_direction_jacoEE ) - self.logger.info( - f"[{self.name}] m_d_squared (before sqrt): {m_d_squared:.6e}" - ) manipulability_score = np.sqrt(max(0.0, m_d_squared)) self.logger.info( diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index f4ea2067..0558a619 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -703,6 +703,14 @@ def move_above_sequence() -> py_trees.behaviour.Behaviour: name="CheckJacoManipulabilityForMoveInto", ns=name, inputs={ + "pinocchio_model": BlackboardKey("pinocchio_model"), + "pinocchio_data": BlackboardKey("pinocchio_data"), + "jaco_vel_indices_pin": BlackboardKey( + "jaco_vel_indices_pin" + ), + "jaco_ee_frame_id_pin": BlackboardKey( + "jaco_ee_frame_id_pin" + ), "current_full_robot_joint_state_MA": BlackboardKey( "move_above_ik_solution_8dof" ), @@ -712,17 +720,7 @@ def move_above_sequence() -> py_trees.behaviour.Behaviour: "tool_tip_move_into_pose_world": BlackboardKey( "tool_tip_move_into_pose_world" ), - "jaco_joint_names": [ - "j2n6s200_joint_1", - "j2n6s200_joint_2", - "j2n6s200_joint_3", - "j2n6s200_joint_4", - "j2n6s200_joint_5", - "j2n6s200_joint_6", - ], - "jaco_end_effector_link_name": "j2n6s200_end_effector", "directional_manipulability_threshold": 0.01, - "urdf_file_path": "package://ada_moveit/config/ada.urdf.xacro", }, outputs={ "jaco_directional_manipulability_score": BlackboardKey( From 234671b5bf6fc4cb7043e24a524f956f602f1235 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 12:19:21 -0700 Subject: [PATCH 075/238] Remove logs because I don't want to pass the joint names as an input --- .../check_jaco_directional_manipulability.py | 22 ------------------- 1 file changed, 22 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py b/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py index e8237743..f60e29e5 100644 --- a/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py +++ b/ada_feeding/ada_feeding/behaviors/state/check_jaco_directional_manipulability.py @@ -258,28 +258,6 @@ def update(self) -> Status: ) return Status.FAILURE - jaco_joint_names_bb: List[str] = self.blackboard_get( - "jaco_joint_names" - ) # For logging - log_jaco_q_values = [] - for ( - name_j - ) in ( - jaco_joint_names_bb - ): # Use 'name_j' to avoid conflict with outer 'name' - if self._pin_model.existJointName(name_j): - jid = self._pin_model.getJointId(name_j) - j_obj = self._pin_model.joints[jid] - if j_obj.nq == 1: - log_jaco_q_values.append(f"{name_j}: {q_MA[j_obj.idx_q]:.3f}") - elif j_obj.nq == 2: - log_jaco_q_values.append( - f"{name_j}: [{q_MA[j_obj.idx_q]:.3f}, {q_MA[j_obj.idx_q + 1]:.3f}]" - ) - self.logger.info( - f"[{self.name}] Populated q_MA for Jaco joints: {{{', '.join(log_jaco_q_values)}}}" - ) - pin.forwardKinematics(self._pin_model, self._pin_data, q_MA) pin.updateFramePlacements(self._pin_model, self._pin_data) From 7a5223568d8999eb34cdbf06e6f11fbd8039c30c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 18 May 2025 13:20:04 -0700 Subject: [PATCH 076/238] Update CheckArticutoolPathOrientationFeasibility behavior to use pre-loaded Pinocchio model --- ...articutool_path_orientation_feasibility.py | 487 ++++++++++-------- .../ada_feeding/trees/acquire_food_tree.py | 20 +- 2 files changed, 284 insertions(+), 223 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py index a54966b5..e97a309d 100644 --- a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py +++ b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py @@ -5,25 +5,36 @@ """ Defines the CheckArticutoolPathOrientationFeasibility behavior. This behavior checks if the Articutool can maintain a desired world orientation -for its tool tip throughout a given Cartesian trajectory of the Jaco end-effector, -using an analytical IK adapted from benchmark scripts. +for its tool tip throughout a given trajectory of the Jaco arm. +It uses Pinocchio for FK if the Jaco trajectory is in joint space, +and an analytical IK for the Articutool. """ # Standard imports -import os import math from typing import Union, Optional, List, Tuple # Third-party imports -from geometry_msgs.msg import PoseStamped, Quaternion as QuaternionMsg, Pose +from geometry_msgs.msg import ( + PoseStamped, + Quaternion as QuaternionMsg, + Pose, + TransformStamped, +) from moveit_msgs.msg import RobotTrajectory +from trajectory_msgs.msg import ( + JointTrajectoryPoint, + JointTrajectory, + MultiDOFJointTrajectoryPoint, +) import numpy as np from overrides import override -from scipy.spatial.transform import Rotation # For R.from_quat and R.apply +from scipy.spatial.transform import Rotation import py_trees import py_trees.blackboard from py_trees.common import Status import rclpy.node +import pinocchio as pin # Local imports from ada_feeding.helpers import BlackboardKey @@ -32,63 +43,45 @@ class CheckArticutoolPathOrientationFeasibility(BlackboardBehavior): """ - Checks if the Articutool can maintain a desired world orientation for its - tool tip throughout a given Cartesian trajectory of the Jaco end-effector. - It uses an analytical IK adapted from the orientation_constrained_planner_benchmark.py script. + Checks Articutool's ability to maintain tool tip orientation during Jaco trajectory. + Uses Pinocchio for FK if Jaco trajectory is joint-space. """ - # Constants adapted from benchmark for IK logic EPSILON = 1e-6 def blackboard_inputs( self, - jaco_ee_cartesian_trajectory: Union[BlackboardKey, RobotTrajectory], - desired_tool_tip_world_orientation: Union[BlackboardKey, QuaternionMsg], - articutool_joint_names: Union[ + pinocchio_model: Union[BlackboardKey, pin.Model], + pinocchio_data: Union[BlackboardKey, pin.Data], + # Jaco info for FK if needed + jaco_joint_names_pin: Union[ + BlackboardKey, List[str] + ], # Names as in Pinocchio model for q mapping + jaco_ee_frame_id_pin: Union[ + BlackboardKey, int + ], # Pinocchio frame ID for Jaco EE + # Articutool info for its own q mapping if full robot q is constructed + articutool_joint_names_pin: Union[ BlackboardKey, List[str] - ], # Expected: ["pitch_joint", "roll_joint"] order - articutool_pitch_limits_rad: Union[ - BlackboardKey, Tuple[float, float] - ], # (min, max) - articutool_roll_limits_rad: Union[ - BlackboardKey, Tuple[float, float] - ], # (min, max) + ], # Names as in Pinocchio model + # Trajectory and target + jaco_trajectory: Union[ + BlackboardKey, RobotTrajectory, JointTrajectory + ], # Can be full RobotTrajectory or just JointTrajectory + desired_tool_tip_world_orientation: Union[BlackboardKey, QuaternionMsg], + # Articutool specific limits for its IK + articutool_pitch_limits_rad: Union[BlackboardKey, Tuple[float, float]], + articutool_roll_limits_rad: Union[BlackboardKey, Tuple[float, float]], num_trajectory_points_to_check: Union[BlackboardKey, int] = 10, ) -> None: - """ - Blackboard Inputs - - Parameters - ---------- - jaco_ee_cartesian_trajectory: RobotTrajectory for the Jaco end-effector's Cartesian path. - Expected to contain poses in multi_dof_joint_trajectory. - desired_tool_tip_world_orientation: QuaternionMsg for the target world orientation of the tool_tip. - articutool_joint_names: List of Articutool joint names, e.g., ["atool_joint1", "atool_joint2"]. - The order should match how limits are provided and how IK solutions are interpreted. - articutool_pitch_limits_rad: Tuple (min_pitch, max_pitch) in radians for the first Articutool joint. - articutool_roll_limits_rad: Tuple (min_roll, max_roll) in radians for the second Articutool joint. - num_trajectory_points_to_check: Number of points to sample and check along the trajectory. - """ super().blackboard_inputs( **{key: value for key, value in locals().items() if key != "self"} ) def blackboard_outputs( self, - articutool_is_orientation_feasible: Optional[ - BlackboardKey - ], # -> Optional[bool] - # Optional outputs for debugging: - # first_failing_articutool_config: Optional[BlackboardKey], # -> Optional[List[float]] - # first_failing_jaco_ee_pose_idx: Optional[BlackboardKey], # -> Optional[int] + articutool_is_orientation_feasible: Optional[BlackboardKey], ) -> None: - """ - Blackboard Outputs - - Parameters - ---------- - articutool_is_orientation_feasible: Boolean, True if Articutool can maintain orientation. - """ super().blackboard_outputs( **{key: value for key, value in locals().items() if key != "self"} ) @@ -96,144 +89,122 @@ def blackboard_outputs( def __init__(self, name: str, **kwargs): super().__init__(name=name, **kwargs) self.node: Optional[rclpy.node.Node] = None - # Store limits internally after setup for easier access - self._pitch_limits: Optional[Tuple[float, float]] = None - self._roll_limits: Optional[Tuple[float, float]] = None + self._pin_model: Optional[pin.Model] = None + self._pin_data: Optional[pin.Data] = None + self._jaco_joint_names_pin: Optional[List[str]] = None + self._jaco_ee_frame_id_pin: Optional[int] = None + self._articutool_joint_names_pin: Optional[List[str]] = None + self._pitch_limits_rad: Optional[Tuple[float, float]] = None + self._roll_limits_rad: Optional[Tuple[float, float]] = None + self._pinocchio_ready = False @override def setup(self, **kwargs): - """Gets the ROS2 node and retrieves fixed parameters like joint limits.""" + """Gets the ROS2 node.""" try: self.node = kwargs["node"] except KeyError as e: self.logger.error( f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" ) - return # Cannot proceed - # Retrieve and store joint limits once during setup + def _get_pinocchio_essentials_from_blackboard(self) -> bool: + """Reads Pinocchio model, data, and relevant IDs/names from blackboard.""" + if self._pinocchio_ready: # Avoid re-reading if already successful + return True try: - self._pitch_limits = self.blackboard_get("articutool_pitch_limits_rad") - self._roll_limits = self.blackboard_get("articutool_roll_limits_rad") - if not ( - isinstance(self._pitch_limits, tuple) - and len(self._pitch_limits) == 2 - and isinstance(self._roll_limits, tuple) - and len(self._roll_limits) == 2 + self._pin_model = self.blackboard_get("pinocchio_model") + self._pin_data = self.blackboard_get("pinocchio_data") + self._jaco_joint_names_pin = self.blackboard_get("jaco_joint_names_pin") + self._jaco_ee_frame_id_pin = self.blackboard_get("jaco_ee_frame_id_pin") + self._articutool_joint_names_pin = self.blackboard_get( + "articutool_joint_names_pin" + ) + self._pitch_limits_rad = self.blackboard_get("articutool_pitch_limits_rad") + self._roll_limits_rad = self.blackboard_get("articutool_roll_limits_rad") + + if ( + not isinstance(self._pin_model, pin.Model) + or not isinstance(self._pin_data, pin.Data) + or not isinstance(self._jaco_joint_names_pin, list) + or not isinstance(self._jaco_ee_frame_id_pin, int) + or not isinstance(self._articutool_joint_names_pin, list) + or not ( + isinstance(self._pitch_limits_rad, tuple) + and len(self._pitch_limits_rad) == 2 + ) + or not ( + isinstance(self._roll_limits_rad, tuple) + and len(self._roll_limits_rad) == 2 + ) ): self.logger.error( - f"[{self.name}] Articutool joint limits are not valid tuples of size 2." + f"[{self.name}] One or more Pinocchio-related inputs from blackboard are invalid type/format." ) - self._pitch_limits = None # Invalidate - self._roll_limits = None # Invalidate + return False + self._pinocchio_ready = True + return True except KeyError as e: self.logger.error( - f"[{self.name}] Blackboard key error for joint limits: {e}" + f"[{self.name}] Failed to get Pinocchio info from blackboard: {e}. Ensure LoadPinocchioModel ran." ) + return False def _normalize_angle(self, angle: float) -> float: - """Normalizes an angle to the range [-pi, pi].""" return (angle + math.pi) % (2 * math.pi) - math.pi def _solve_articutool_ik_benchmark_style( self, target_y_axis_in_jaco_hand_frame: np.ndarray ) -> List[Tuple[float, float]]: - """ - Analytical IK solver for the Articutool, adapted from the benchmark script. - It solves for (theta_p, theta_r) to align the Articutool's effective - pointing Y-axis with the target_y_axis_in_jaco_hand_frame. - - The benchmark's IK implies the Articutool's pointing Y-axis in its base frame is: - [-sin(theta_r), cos(theta_p)cos(theta_r), sin(theta_p)cos(theta_r)] - - Parameters - ---------- - target_y_axis_in_jaco_hand_frame : np.ndarray - A 3D vector representing the desired orientation of the Articutool's - pointing Y-axis, expressed in the Jaco hand frame. - - Returns - ------- - List[Tuple[float, float]] - A list of (theta_p, theta_r) solutions in radians. - """ vx, vy, vz = target_y_axis_in_jaco_hand_frame solutions: List[Tuple[float, float]] = [] - - # From benchmark: asin_arg = np.clip(-vx, -1.0, 1.0) - # This means: target_x_in_jaco_hand = -sin(theta_r) - if not (-1.0 - self.EPSILON <= -vx <= 1.0 + self.EPSILON): - self.logger.debug( - f"[{self.name}] IK: -vx ({vx:.4f}) out of range for asin." - ) + asin_arg_for_tr = -vx + if not (-1.0 - self.EPSILON <= asin_arg_for_tr <= 1.0 + self.EPSILON): return [] - - asin_arg_for_theta_r = np.clip(-vx, -1.0, 1.0) - theta_r_sol1 = math.asin(asin_arg_for_theta_r) - theta_r_sol2 = self._normalize_angle( - math.pi - theta_r_sol1 - ) # Second solution for asin - + asin_arg_for_tr_clipped = np.clip(asin_arg_for_tr, -1.0, 1.0) + theta_r_sol1 = math.asin(asin_arg_for_tr_clipped) + theta_r_sol2 = self._normalize_angle(math.pi - theta_r_sol1) candidate_thetas_r = [theta_r_sol1] if not math.isclose(theta_r_sol1, theta_r_sol2, abs_tol=self.EPSILON): candidate_thetas_r.append(theta_r_sol2) - - # self.logger.debug(f"IK: Target Y in JacoHand: [{vx:.3f}, {vy:.3f}, {vz:.3f}]") - # self.logger.debug(f"IK: Candidate theta_r values: {[round(r, 3) for r in candidate_thetas_r]}") - for theta_r in candidate_thetas_r: cos_theta_r = math.cos(theta_r) - - # From benchmark: theta_p_sol = math.atan2(vz / cos_theta_r, vy / cos_theta_r) - # This means: target_y_in_jaco_hand = cos(theta_p)cos(theta_r) - # target_z_in_jaco_hand = sin(theta_p)cos(theta_r) if math.isclose(cos_theta_r, 0.0, abs_tol=self.EPSILON): - # If cos(theta_r) is 0, then sin(theta_r) is +/-1. - # This means -vx should be +/-1 (target_x_in_jaco_hand is +/-1). - # Also, target_y_in_jaco_hand and target_z_in_jaco_hand must be 0. if math.isclose(vy, 0.0, abs_tol=self.EPSILON) and math.isclose( vz, 0.0, abs_tol=self.EPSILON ): - # theta_p is indeterminate (or can be chosen freely, e.g., 0) - # This configuration means the tool is pointing purely along Jaco hand's X-axis. - theta_p_sol = 0.0 # Choose a default, like 0 - solutions.append((theta_p_sol, theta_r)) - # self.logger.debug(f"IK: cos(theta_r) is zero, vy, vz also zero. Added solution ({theta_p_sol:.3f}, {theta_r:.3f})") - # else: - # self.logger.debug(f"IK: cos(theta_r) is zero, but vy or vz is non-zero. Infeasible for this theta_r.") - continue # Move to next theta_r candidate if any - - # Normal case: cos(theta_r) is not zero - # theta_p = atan2( (sin(theta_p)cos(theta_r)) , (cos(theta_p)cos(theta_r)) ) - # theta_p = atan2( vz , vy ) + solutions.append((0.0, self._normalize_angle(theta_r))) + continue theta_p_sol = math.atan2(vz, vy) solutions.append( (self._normalize_angle(theta_p_sol), self._normalize_angle(theta_r)) ) - # self.logger.debug(f"IK: For theta_r={theta_r:.3f}, got theta_p={theta_p_sol:.3f}. Added solution.") - return solutions @override def initialise(self) -> None: self.logger.debug(f"[{self.name}] Initializing.") self.blackboard_set("articutool_is_orientation_feasible", None) - # self.blackboard_set("first_failing_articutool_config", None) - # self.blackboard_set("first_failing_jaco_ee_pose_idx", None) + self._pinocchio_ready = False @override def update(self) -> Status: if not self.node: self.feedback_message = "Node not initialized" return Status.FAILURE - if self._pitch_limits is None or self._roll_limits is None: - self.feedback_message = "Articutool joint limits not set up correctly." - self.logger.error(f"[{self.name}] {self.feedback_message}") - return Status.FAILURE + if ( + not self._pinocchio_ready + ): # Attempt to load Pinocchio essentials if not already done + if not self._get_pinocchio_essentials_from_blackboard(): + self.feedback_message = ( + "Failed to get Pinocchio essentials from blackboard." + ) + return Status.FAILURE + # Now, self._pin_model, self._pin_data etc. should be populated if _get_pinocchio_essentials_from_blackboard was successful try: - trajectory: RobotTrajectory = self.blackboard_get( - "jaco_ee_cartesian_trajectory" + trajectory_input: Union[RobotTrajectory, JointTrajectory] = ( + self.blackboard_get("jaco_trajectory") ) desired_orientation_msg: QuaternionMsg = self.blackboard_get( "desired_tool_tip_world_orientation" @@ -242,37 +213,13 @@ def update(self) -> Status: "num_trajectory_points_to_check" ) - if not isinstance( - trajectory, RobotTrajectory - ): # No points check needed if not RobotTrajectory - self.feedback_message = ( - "Input 'jaco_ee_cartesian_trajectory' is not a RobotTrajectory." - ) - self.logger.error(f"[{self.name}] {self.feedback_message}") - return Status.FAILURE if not isinstance(desired_orientation_msg, QuaternionMsg): self.feedback_message = ( - "Input 'desired_tool_tip_world_orientation' is not a Quaternion." + "Input 'desired_tool_tip_world_orientation' is not Quaternion." ) self.logger.error(f"[{self.name}] {self.feedback_message}") return Status.FAILURE - # Check if trajectory has points - # We expect poses in multi_dof_joint_trajectory for Cartesian paths - if not trajectory.multi_dof_joint_trajectory.points: - if trajectory.joint_trajectory.points: - self.feedback_message = "Trajectory has joint points but no multi_dof (Cartesian) points. FK needed but not implemented here." - self.logger.error(f"[{self.name}] {self.feedback_message}") - else: - self.feedback_message = "Input Jaco EE trajectory is empty." - self.logger.warn( - f"[{self.name}] {self.feedback_message} Considering feasible as no points to fail." - ) - # If no points, it's vacuously feasible, or could be failure depending on requirements. - # Let's say it's feasible if no points to check. - self.blackboard_set("articutool_is_orientation_feasible", True) - return Status.SUCCESS - R_World_TipTarget = Rotation.from_quat( [ desired_orientation_msg.x, @@ -281,59 +228,173 @@ def update(self) -> Status: desired_orientation_msg.w, ] ) - # This is the Y-axis of the desired tool tip orientation, expressed in World frame. - # This is the vector that the Articutool's "pointing Y-axis" should align with in the world. y_axis_TipTarget_InWorld = R_World_TipTarget.apply( np.array([0.0, 1.0, 0.0]) ) - jaco_ee_poses_from_traj: List[Pose] = [] - for mjt_point in trajectory.multi_dof_joint_trajectory.points: - if mjt_point.transforms: - transform = mjt_point.transforms[ - 0 - ] # Assuming first transform is the EE + jaco_ee_world_poses: List[Pose] = [] + + if isinstance(trajectory_input, RobotTrajectory): + if trajectory_input.multi_dof_joint_trajectory.points: + self.logger.debug( + f"[{self.name}] Using multi_dof_joint_trajectory from RobotTrajectory." + ) + for mjt_point in trajectory_input.multi_dof_joint_trajectory.points: + if mjt_point.transforms: + transform = mjt_point.transforms[0] + pose = Pose() + pose.position.x = transform.translation.x + pose.position.y = transform.translation.y + pose.position.z = transform.translation.z + pose.orientation = transform.rotation + jaco_ee_world_poses.append(pose) + elif trajectory_input.joint_trajectory.points: + self.logger.debug( + f"[{self.name}] Using joint_trajectory from RobotTrajectory. Performing FK." + ) + jt_points = trajectory_input.joint_trajectory.points + jt_joint_names = trajectory_input.joint_trajectory.joint_names + # Map jt_joint_names to the order expected by self._jaco_joint_names_pin for FK + # This part needs care if orders differ or if jt_joint_names is not just Jaco + + q_robot = pin.neutral(self._pin_model) # Full robot configuration + for point_data in jt_points: + # Populate q_robot based on point_data.positions, jt_joint_names + # and neutral/current for other joints (Articutool, virtual base) + + # Simplified: Assume jt_joint_names are ONLY Jaco joints and match _jaco_joint_names_pin order + # And Articutool joints remain at their neutral/current state from q_robot initialization + # A more robust solution would take current full robot state and update Jaco parts. + + # Map positions from trajectory to the correct Jaco joints in Pinocchio model + temp_jaco_map = { + name: point_data.positions[i] + for i, name in enumerate(jt_joint_names) + } + + for ( + j_name_pin + ) in ( + self._jaco_joint_names_pin + ): # Iterate through Pinocchio's Jaco joint names + if j_name_pin in temp_jaco_map: + joint_id = self._pin_model.getJointId(j_name_pin) + joint_obj = self._pin_model.joints[joint_id] + theta = temp_jaco_map[j_name_pin] + if joint_obj.nq == 2 and joint_obj.nv == 1: # Revolute + q_robot[joint_obj.idx_q] = math.cos(theta) + q_robot[joint_obj.idx_q + 1] = math.sin(theta) + elif ( + joint_obj.nq == 1 and joint_obj.nv == 1 + ): # Prismatic + q_robot[joint_obj.idx_q] = theta + + # Articutool joints (assume they stay at neutral or a fixed known state for FK of Jaco EE) + # For simplicity, let's assume they remain neutral as set by pin.neutral(self._pin_model) + # If Articutool moves with Jaco, its state should also be in trajectory or known. + + pin.forwardKinematics(self._pin_model, self._pin_data, q_robot) + pin.updateFramePlacements(self._pin_model, self._pin_data) + jaco_ee_transform_pin: pin.SE3 = self._pin_data.oMf[ + self._jaco_ee_frame_id_pin + ] + + pose = Pose() + pose.position.x, pose.position.y, pose.position.z = ( + jaco_ee_transform_pin.translation + ) + quat_xyzw = Rotation.from_matrix( + jaco_ee_transform_pin.rotation + ).as_quat() + ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) = quat_xyzw + jaco_ee_world_poses.append(pose) + + else: # RobotTrajectory is empty + self.logger.warn( + f"[{self.name}] Input RobotTrajectory is empty. Assuming feasible." + ) + self.blackboard_set("articutool_is_orientation_feasible", True) + return Status.SUCCESS + + elif isinstance(trajectory_input, JointTrajectory): + self.logger.debug( + f"[{self.name}] Input is JointTrajectory. Performing FK." + ) + # Similar FK logic as above for RobotTrajectory.joint_trajectory + jt_points = trajectory_input.points + jt_joint_names = trajectory_input.joint_names + q_robot = pin.neutral(self._pin_model) + for point_data in jt_points: + temp_jaco_map = { + name: point_data.positions[i] + for i, name in enumerate(jt_joint_names) + } + for j_name_pin in self._jaco_joint_names_pin: + if j_name_pin in temp_jaco_map: + joint_id = self._pin_model.getJointId(j_name_pin) + joint_obj = self._pin_model.joints[joint_id] + theta = temp_jaco_map[j_name_pin] + if joint_obj.nq == 2 and joint_obj.nv == 1: + q_robot[joint_obj.idx_q] = math.cos(theta) + q_robot[joint_obj.idx_q + 1] = math.sin(theta) + elif joint_obj.nq == 1 and joint_obj.nv == 1: + q_robot[joint_obj.idx_q] = theta + pin.forwardKinematics(self._pin_model, self._pin_data, q_robot) + pin.updateFramePlacements(self._pin_model, self._pin_data) + jaco_ee_transform_pin: pin.SE3 = self._pin_data.oMf[ + self._jaco_ee_frame_id_pin + ] pose = Pose() - pose.position.x = transform.translation.x - pose.position.y = transform.translation.y - pose.position.z = transform.translation.z - pose.orientation = transform.rotation - jaco_ee_poses_from_traj.append(pose) + pose.position.x, pose.position.y, pose.position.z = ( + jaco_ee_transform_pin.translation + ) + quat_xyzw = Rotation.from_matrix( + jaco_ee_transform_pin.rotation + ).as_quat() + ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) = quat_xyzw + jaco_ee_world_poses.append(pose) + else: + self.feedback_message = f"Input 'jaco_trajectory' is of unexpected type: {type(trajectory_input)}." + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE - if not jaco_ee_poses_from_traj: - self.feedback_message = ( - "No poses extracted from multi_dof_joint_trajectory." + if not jaco_ee_world_poses: + self.logger.warn( + f"[{self.name}] No Jaco EE poses to check. Assuming feasible." ) - self.logger.warn(f"[{self.name}] {self.feedback_message}") - self.blackboard_set( - "articutool_is_orientation_feasible", True - ) # Vacuously true + self.blackboard_set("articutool_is_orientation_feasible", True) return Status.SUCCESS indices_to_check = [] - if len(jaco_ee_poses_from_traj) == 1: + if len(jaco_ee_world_poses) == 1: indices_to_check = [0] - elif num_points_to_check >= len( - jaco_ee_poses_from_traj - ): # Check all if fewer than num_points - indices_to_check = list(range(len(jaco_ee_poses_from_traj))) + elif num_points_to_check >= len(jaco_ee_world_poses): + indices_to_check = list(range(len(jaco_ee_world_poses))) elif num_points_to_check > 0: indices_to_check = np.linspace( - 0, len(jaco_ee_poses_from_traj) - 1, num_points_to_check, dtype=int + 0, len(jaco_ee_world_poses) - 1, num_points_to_check, dtype=int ) - if ( - not indices_to_check and num_points_to_check > 0 - ): # Should not happen if jaco_ee_poses_from_traj is not empty + # Check the length of indices_to_check, as it can be a list or numpy array + if len(indices_to_check) == 0 and num_points_to_check > 0: self.logger.warn( - f"[{self.name}] No indices to check despite having trajectory points and num_points_to_check > 0." + f"[{self.name}] No indices to check (num_points_to_check={num_points_to_check}, len_poses={len(jaco_ee_world_poses)}). Assuming feasible." ) self.blackboard_set("articutool_is_orientation_feasible", True) return Status.SUCCESS - for point_idx_in_indices, actual_traj_idx in enumerate(indices_to_check): - jaco_ee_pose_msg: Pose = jaco_ee_poses_from_traj[actual_traj_idx] - + for traj_idx in indices_to_check: + jaco_ee_pose_msg: Pose = jaco_ee_world_poses[traj_idx] R_World_JacoEE = Rotation.from_quat( [ jaco_ee_pose_msg.orientation.x, @@ -342,64 +403,48 @@ def update(self) -> Status: jaco_ee_pose_msg.orientation.w, ] ) - - # Transform the desired tool tip Y-axis (in World) into the Jaco EE (Articutool Base) frame - target_y_axis_for_ik_in_jaco_hand = R_World_JacoEE.inv().apply( + target_y_for_ik_in_atool_base = R_World_JacoEE.inv().apply( y_axis_TipTarget_InWorld ) - - # Normalize for robustness - norm_target_y = np.linalg.norm(target_y_axis_for_ik_in_jaco_hand) + norm_target_y = np.linalg.norm(target_y_for_ik_in_atool_base) if norm_target_y < self.EPSILON: - self.feedback_message = f"Target Y-axis for IK became zero vector at trajectory point {actual_traj_idx}." + self.feedback_message = ( + f"Target Y-axis for IK near zero at traj point {traj_idx}." + ) self.logger.error(f"[{self.name}] {self.feedback_message}") self.blackboard_set("articutool_is_orientation_feasible", False) return Status.FAILURE - target_y_axis_for_ik_in_jaco_hand /= norm_target_y + target_y_for_ik_in_atool_base /= norm_target_y ik_solutions = self._solve_articutool_ik_benchmark_style( - target_y_axis_for_ik_in_jaco_hand + target_y_for_ik_in_atool_base ) found_valid_solution_for_waypoint = False - best_solution_this_waypoint = None - for theta_p_sol, theta_r_sol in ik_solutions: - # Normalize angles before checking limits - theta_p_norm = self._normalize_angle(theta_p_sol) - theta_r_norm = self._normalize_angle(theta_r_sol) - if ( - self._pitch_limits[0] - self.EPSILON - <= theta_p_norm - <= self._pitch_limits[1] + self.EPSILON - and self._roll_limits[0] - self.EPSILON - <= theta_r_norm - <= self._roll_limits[1] + self.EPSILON + self._pitch_limits_rad[0] - self.EPSILON + <= theta_p_sol + <= self._pitch_limits_rad[1] + self.EPSILON + and self._roll_limits_rad[0] - self.EPSILON + <= theta_r_sol + <= self._roll_limits_rad[1] + self.EPSILON ): found_valid_solution_for_waypoint = True - best_solution_this_waypoint = ( - theta_p_norm, - theta_r_norm, - ) # Take the first valid one break if not found_valid_solution_for_waypoint: self.feedback_message = ( - f"Articutool IK failed or solution out of limits at trajectory point index {actual_traj_idx}. " - f"Target Y in JacoHand: {np.round(target_y_axis_for_ik_in_jaco_hand, 3)}. " - f"Raw IK solutions: {[(round(s[0], 3), round(s[1], 3)) for s in ik_solutions]}" + f"Articutool IK failed or solution out of limits at traj point {traj_idx}. " + f"Target Y_AtoolBase: {np.round(target_y_for_ik_in_atool_base, 3)}. " + f"Raw IK solutions (tp,tr): {[(round(s[0], 3), round(s[1], 3)) for s in ik_solutions]}" ) self.logger.warn(f"[{self.name}] {self.feedback_message}") self.blackboard_set("articutool_is_orientation_feasible", False) - # self.blackboard_set("first_failing_jaco_ee_pose_idx", actual_traj_idx) return Status.FAILURE self.logger.debug( - f"[{self.name}] Pt {actual_traj_idx}: JacoEE Ori (quat xyzw): " - f"[{jaco_ee_pose_msg.orientation.x:.2f}, ..., {jaco_ee_pose_msg.orientation.w:.2f}], " - f"Target Y_JacoHand: {np.round(target_y_axis_for_ik_in_jaco_hand, 2)}, " - f"Articutool q_sol: [{best_solution_this_waypoint[0]:.3f}, {best_solution_this_waypoint[1]:.3f}]" + f"[{self.name}] Pt {traj_idx}: Articutool IK feasible." ) self.feedback_message = ( diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 0558a619..40f971e4 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1024,13 +1024,29 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: name="CheckArticutoolFeasibilityForMoveInto", ns=name, inputs={ - "jaco_ee_cartesian_trajectory": BlackboardKey( + "pinocchio_model": BlackboardKey("pinocchio_model"), + "pinocchio_data": BlackboardKey("pinocchio_data"), + "jaco_joint_names_pin": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ], + "jaco_ee_frame_id_pin": BlackboardKey( + "jaco_ee_frame_id_pin" + ), + "articutool_joint_names_pin": [ + "atool_joint1", + "atool_joint2", + ], + "jaco_trajectory": BlackboardKey( "move_into_jaco_arm_trajectory" ), "desired_tool_tip_world_orientation": BlackboardKey( "move_into_tool_tip_orientation" ), - "articutool_joint_names": ["atool_joint1", "atool_joint2"], "articutool_pitch_limits_rad": (-np.pi / 2, np.pi / 2), "articutool_roll_limits_rad": (-np.pi, np.pi), "num_trajectory_points_to_check": 20, From 5cd3cd179b8aa0ce7454a40708645f7573dea6f6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 19 May 2025 13:08:18 -0700 Subject: [PATCH 077/238] Update CallSetOrientationControl to adhere to new interface --- .../call_set_orientation_control.py | 323 ++++++++++++------ 1 file changed, 216 insertions(+), 107 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py index 0d2358de..ffedd147 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py @@ -1,20 +1,20 @@ # -*- coding: utf-8 -*- -# (Add appropriate Copyright/License if desired) +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. """ Defines the CallSetOrientationControl behavior, which calls the service -to enable/disable the Articutool orientation controller. +to set the Articutool orientation controller's mode and targets. """ import rclpy from rclpy.node import Node -from rclpy.executors import Future # For type hints -from rclpy.duration import Duration -from rclpy.executors import ExternalShutdownException -from typing import Union, Optional, Dict, Any, List, Tuple # Added Tuple just in case +from rclpy.executors import Future # For type hints -from geometry_msgs.msg import PoseStamped, Quaternion # Need Quaternion if creating default PoseStamped -# Import your service definition (adjust path as needed) +from typing import Union, Optional, Dict, Any, List, Tuple +import math + +from geometry_msgs.msg import Quaternion from articutool_interfaces.srv import SetOrientationControl from overrides import override @@ -23,15 +23,15 @@ from py_trees.common import Access, Status import numpy as np -# Local imports (adjust paths as needed) +# Local imports from ada_feeding.helpers import BlackboardKey from ada_feeding.behaviors import BlackboardBehavior class CallSetOrientationControl(BlackboardBehavior): """ - Calls the SetOrientationControl service to enable/disable the - Articutool's orientation controller and optionally set its target pose/orientation. + Calls the SetOrientationControl service to set the Articutool's + orientation control mode and corresponding targets. Returns SUCCESS if the service call completes and the service response indicates success. Returns FAILURE if the service is unavailable, the call fails, or the service response indicates failure. @@ -43,23 +43,35 @@ class CallSetOrientationControl(BlackboardBehavior): def blackboard_inputs( self, - enable: Union[BlackboardKey, bool], - quat_xyzw: Union[BlackboardKey, PoseStamped], + control_mode: Union[ + BlackboardKey, int + ], # MODE_DISABLED, MODE_LEVELING, MODE_FULL_ORIENTATION + pitch_offset_deg: Optional[ + Union[BlackboardKey, float] + ] = 0.0, # For MODE_LEVELING + roll_offset_deg: Optional[ + Union[BlackboardKey, float] + ] = 0.0, # For MODE_LEVELING + target_orientation_robot_base_quat: Optional[ + Union[BlackboardKey, Quaternion, List[float], Tuple[float, ...]] + ] = None, # For MODE_FULL_ORIENTATION service_name: Union[BlackboardKey, str] = DEFAULT_SERVICE_NAME, - wait_for_server_timeout_sec: Union[BlackboardKey, float] = DEFAULT_WAIT_TIMEOUT_SEC, + wait_for_server_timeout_sec: Union[ + BlackboardKey, float + ] = DEFAULT_WAIT_TIMEOUT_SEC, ) -> None: """ Blackboard Inputs Parameters ---------- - enable: True to enable orientation control, False to disable. - quat_xyzw: The target orientation, provided as a geometry_msgs/Quaternion - message or an [x, y, z, w] list/tuple. Must be relative - to the frame expected by the orientation controller node. + control_mode: The desired control mode (e.g., SetOrientationControl.Request.MODE_LEVELING). + pitch_offset_deg: Target pitch offset in degrees (for MODE_LEVELING). + roll_offset_deg: Target roll offset in degrees (for MODE_LEVELING). + target_orientation_robot_base_quat: Target orientation as Quaternion msg or [x,y,z,w] list/tuple + (for MODE_FULL_ORIENTATION), relative to robot_base_frame. service_name: Name of the SetOrientationControl service. - wait_for_server_timeout_sec: Max time (sec) to wait for server in initial check. - Use 0.0 or negative to skip check (not recommended). + wait_for_server_timeout_sec: Max time (sec) to wait for server. """ super().blackboard_inputs( **{key: value for key, value in locals().items() if key != "self"} @@ -67,18 +79,13 @@ def blackboard_inputs( def blackboard_outputs( self, - service_call_succeeded: Optional[BlackboardKey] = None, # -> bool (Did call complete?) - service_response_success: Optional[BlackboardKey] = None, # -> bool (Payload 'success' field) - service_response_message: Optional[BlackboardKey] = None, # -> str (Payload 'message' field) + service_call_succeeded: Optional[BlackboardKey] = None, + service_response_success: Optional[BlackboardKey] = None, + service_response_message: Optional[BlackboardKey] = None, ) -> None: """ Blackboard Outputs - - Parameters - ---------- - service_call_succeeded: True if the service call finished without client-side errors/timeouts. - service_response_success: The boolean 'success' field from the service response payload. - service_response_message: The string 'message' field from the service response payload. + (Same as before) """ super().blackboard_outputs( **{key: value for key, value in locals().items() if key != "self"} @@ -86,25 +93,37 @@ def blackboard_outputs( def setup(self, **kwargs): """Get the node and create the service client.""" - # pylint: disable=attribute-defined-outside-init - self.node: Node = kwargs['node'] - self.client = None # Initialize client to None + self.node: Optional[Node] = kwargs.get("node") # Use .get() for safety + self.client = None + if not self.node: + self.logger.error( + f"[{self.name}] Node object not provided in setup kwargs." + ) + return + try: - self.service_name = self.blackboard_get("service_name") - self.wait_timeout = self.blackboard_get("wait_for_server_timeout_sec") - self.client = self.node.create_client(SetOrientationControl, self.service_name) - self.logger.info(f"[{self.name}] Service client created for '{self.service_name}'") + # It's better to get these once in setup if they don't change per tick + # Or get them in update if they can change dynamically from the blackboard + self._service_name_str = self.blackboard_get("service_name") + self._wait_timeout_float = self.blackboard_get( + "wait_for_server_timeout_sec" + ) + + self.client = self.node.create_client( + SetOrientationControl, self._service_name_str + ) + self.logger.info( + f"[{self.name}] Service client created for '{self._service_name_str}'" + ) except KeyError as e: - self.logger.error(f"[{self.name}] Missing blackboard key during setup: {e}") + self.logger.error(f"[{self.name}] Missing blackboard key during setup: {e}") except Exception as e: self.logger.error(f"[{self.name}] Failed to create service client: {e}") def initialise(self) -> None: """Reset the future.""" - # pylint: disable=attribute-defined-outside-init self.service_future: Optional[Future] = None self.logger.debug(f"[{self.name}] Initializing service call state.") - # Clear blackboard outputs self.blackboard_set("service_call_succeeded", False) self.blackboard_set("service_response_success", False) self.blackboard_set("service_response_message", "") @@ -112,109 +131,199 @@ def initialise(self) -> None: def update(self) -> Status: """Manage the asynchronous service call state machine.""" if self.client is None: - self.logger.error(f"[{self.name}] Service client not available. Setup failed?") + self.logger.error( + f"[{self.name}] Service client not available. Setup likely failed or did not complete." + ) return Status.FAILURE # --- State 1: Ready to Call --- if self.service_future is None: - # Check server availability (only if timeout > 0) - wait_timeout = self.wait_timeout # Use timeout from setup - if wait_timeout >= 0.0: - if not self.client.wait_for_service(timeout_sec=wait_timeout): - self.logger.error(f"[{self.name}] Service '{self.service_name}' not available after {wait_timeout}s timeout.") - return Status.FAILURE # Service unavailable is a failure + if ( + self._wait_timeout_float >= 0.0 + ): # Check server availability only if timeout is non-negative + if not self.client.wait_for_service( + timeout_sec=self._wait_timeout_float + ): + self.logger.error( + f"[{self.name}] Service '{self._service_name_str}' not available after {self._wait_timeout_float}s timeout." + ) + return Status.FAILURE try: - # Read inputs from blackboard - enable_flag = self.blackboard_get("enable") - quat_xyzw = self.blackboard_get("quat_xyzw") - - # Input validation - if not isinstance(enable_flag, bool): - self.logger.error(f"[{self.name}] Input 'enable' type mismatch: expected bool, got {type(enable_flag)}.") - return Status.FAILURE - - target_quat_msg = Quaternion() - if isinstance(quat_xyzw, Quaternion): - target_quat_msg = quat_xyzw - self.logger.debug(f"[{self.name}] Using Quaternion message input.") - elif isinstance(quat_xyzw, (list, tuple)) and len(quat_xyzw) == 4: - self.logger.debug(f"[{self.name}] Converting list/tuple input to Quaternion.") - try: - target_quat_msg.x = float(quat_xyzw[0]) - target_quat_msg.y = float(quat_xyzw[1]) - target_quat_msg.z = float(quat_xyzw[2]) - target_quat_msg.w = float(quat_xyzw[3]) - norm = np.linalg.norm([target_quat_msg.x, target_quat_msg.y, target_quat_msg.z, target_quat_msg.w]) - if not np.isclose(norm, 1.0, atol=0.01): self.logger.warning(f"Input quat norm is {norm:.3f}") - except (ValueError, TypeError, IndexError) as e: - self.logger.error(f"Could not convert list/tuple {quat_xyzw} to Quaternion: {e}"); return Status.FAILURE - else: - if enable_flag: # Target is mandatory if enabling - self.logger.error(f"[{self.name}] Input 'target_orientation_input' invalid type {type(quat_xyzw)} when enable=True.") - return Status.FAILURE - else: # Use default identity if disabling - self.logger.debug(f"[{self.name}] Using default identity quaternion because enable=False.") - target_quat_msg = Quaternion(x=0.0, y=0.0, z=0.0, w=1.0) + # Read all necessary inputs from blackboard for this call + control_mode_input = self.blackboard_get("control_mode") + + # Validate control_mode + if control_mode_input not in [ + SetOrientationControl.Request.MODE_DISABLED, + SetOrientationControl.Request.MODE_LEVELING, + SetOrientationControl.Request.MODE_FULL_ORIENTATION, + ]: + self.logger.error( + f"[{self.name}] Invalid 'control_mode' input: {control_mode_input}." + ) + return Status.FAILURE - # Create request req = SetOrientationControl.Request() - req.enable = enable_flag - req.target_orientation = target_quat_msg + req.control_mode = int(control_mode_input) # Ensure it's int + + # Populate fields based on mode + if req.control_mode == SetOrientationControl.Request.MODE_LEVELING: + pitch_deg = self.blackboard_try_get( + "pitch_offset_deg", 0.0 + ) # Default to 0 if not found + roll_deg = self.blackboard_try_get( + "roll_offset_deg", 0.0 + ) # Default to 0 if not found + req.pitch_offset = float(math.radians(pitch_deg)) + req.roll_offset = float(math.radians(roll_deg)) + # Set a default for target_orientation_robot_base when not in MODE_FULL_ORIENTATION + req.target_orientation_robot_base = Quaternion( + x=0.0, y=0.0, z=0.0, w=1.0 + ) + self.logger.info( + f"[{self.name}] Setting MODE_LEVELING with pitch_offset_rad={req.pitch_offset:.3f}, roll_offset_rad={req.roll_offset:.3f}" + ) - # Call service asynchronously - self.logger.info(f"[{self.name}] Calling service '{self.service_name}' (enable={req.enable})...") + elif ( + req.control_mode + == SetOrientationControl.Request.MODE_FULL_ORIENTATION + ): + target_orient_input = self.blackboard_try_get( + "target_orientation_robot_base_quat" + ) + if target_orient_input is None: + self.logger.error( + f"[{self.name}] 'target_orientation_robot_base_quat' is required for MODE_FULL_ORIENTATION but not found or None." + ) + return Status.FAILURE + + temp_quat_msg = Quaternion() + if isinstance(target_orient_input, Quaternion): + temp_quat_msg = target_orient_input + elif ( + isinstance(target_orient_input, (list, tuple)) + and len(target_orient_input) == 4 + ): + try: + temp_quat_msg.x = float(target_orient_input[0]) + temp_quat_msg.y = float(target_orient_input[1]) + temp_quat_msg.z = float(target_orient_input[2]) + temp_quat_msg.w = float(target_orient_input[3]) + norm = np.linalg.norm( + [ + temp_quat_msg.x, + temp_quat_msg.y, + temp_quat_msg.z, + temp_quat_msg.w, + ] + ) + if not np.isclose(norm, 1.0, atol=0.01) and not np.isclose( + norm, 0.0, atol=0.01 + ): # Allow zero quat if it means "don't care" + self.logger.warning( + f"Input target_orientation_robot_base_quat norm is {norm:.3f}, not 1.0." + ) + except (ValueError, TypeError, IndexError) as e: + self.logger.error( + f"Could not convert list/tuple {target_orient_input} to Quaternion: {e}" + ) + return Status.FAILURE + else: + self.logger.error( + f"[{self.name}] Input 'target_orientation_robot_base_quat' invalid type {type(target_orient_input)} for MODE_FULL_ORIENTATION." + ) + return Status.FAILURE + req.target_orientation_robot_base = temp_quat_msg + # Set defaults for leveling offsets when not in MODE_LEVELING + req.pitch_offset = 0.0 + req.roll_offset = 0.0 + self.logger.info( + f"[{self.name}] Setting MODE_FULL_ORIENTATION with target_quat (xyzw): [{req.target_orientation_robot_base.x:.3f}, {req.target_orientation_robot_base.y:.3f}, {req.target_orientation_robot_base.z:.3f}, {req.target_orientation_robot_base.w:.3f}]" + ) + + else: # MODE_DISABLED or other + # Set defaults for all orientation fields + req.pitch_offset = 0.0 + req.roll_offset = 0.0 + req.target_orientation_robot_base = Quaternion( + x=0.0, y=0.0, z=0.0, w=1.0 + ) + self.logger.info( + f"[{self.name}] Setting control_mode to {req.control_mode} (likely DISABLED)." + ) + + self.logger.info( + f"[{self.name}] Calling service '{self._service_name_str}' with mode={req.control_mode}..." + ) self.service_future = self.client.call_async(req) - return Status.RUNNING # Waiting for response + return Status.RUNNING except KeyError as e: - self.logger.error(f"[{self.name}] Blackboard key error preparing service call: {e}") + self.logger.error( + f"[{self.name}] Blackboard key error preparing service call: {e}" + ) return Status.FAILURE except Exception as e: self.logger.error(f"[{self.name}] Error preparing service call: {e}") return Status.FAILURE # --- State 2: Waiting for Response --- - elif not self.service_future.done(): + elif ( + self.service_future and not self.service_future.done() + ): # Added check for self.service_future not None self.logger.debug(f"[{self.name}] Waiting for service response...") return Status.RUNNING # --- State 3: Processing Response --- - else: + elif self.service_future: # Ensure future exists before trying to get result self.logger.debug(f"[{self.name}] Service call future done.") - response = None + response_payload = None try: - response = self.service_future.result() # Get the response payload - self.blackboard_set("service_call_succeeded", True) # Call itself completed - - if response is not None: - self.blackboard_set("service_response_success", response.success) - self.blackboard_set("service_response_message", response.message) - self.logger.info(f"[{self.name}] Service response: success={response.success}, msg='{response.message}'") - final_status = Status.SUCCESS if response.success else Status.FAILURE + response_payload = self.service_future.result() + self.blackboard_set("service_call_succeeded", True) + + if response_payload is not None: + self.blackboard_set( + "service_response_success", response_payload.success + ) + self.blackboard_set( + "service_response_message", response_payload.message + ) + self.logger.info( + f"[{self.name}] Service response: success={response_payload.success}, msg='{response_payload.message}'" + ) + final_status = ( + Status.SUCCESS if response_payload.success else Status.FAILURE + ) else: - # Should not happen if future.result() didn't raise exception - self.logger.error(f"[{self.name}] Service call future done but result is None.") + self.logger.error( + f"[{self.name}] Service call future done but result is None." + ) self.blackboard_set("service_response_success", False) final_status = Status.FAILURE - except Exception as e: - # Error during call execution on server or getting result - self.logger.error(f"[{self.name}] Service call failed with exception: {e}") - self.blackboard_set("service_call_succeeded", False) # Call failed + self.logger.error( + f"[{self.name}] Service call failed with exception: {e}", + ) + self.blackboard_set("service_call_succeeded", False) self.blackboard_set("service_response_success", False) self.blackboard_set("service_response_message", f"Exception: {e}") final_status = Status.FAILURE - # Reset future regardless of outcome for next tick - self.service_future = None + self.service_future = None # Reset for next call return final_status + # Should not be reached if logic is correct + self.logger.warn(f"[{self.name}] Reached unexpected state in update method.") + return Status.FAILURE + def terminate(self, new_status: Status) -> None: """Log termination status and clear future if needed.""" self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") - # If terminated while waiting for response, clear the future if self.service_future is not None and not self.service_future.done(): - self.logger.warning(f"[{self.name}] Service call terminated while waiting for response.") - # We can't easily cancel a service call future, just abandon it - self.service_future = None + self.logger.warning( + f"[{self.name}] Service call terminated while waiting for response." + ) + # Future cannot be truly cancelled easily, just abandoned + self.service_future = None From 172fccd0fb10491729662c32ba5fdd75d5adc5e5 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 22 May 2025 14:55:35 -0700 Subject: [PATCH 078/238] Remove lingering call to CallSetOrientationControl --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 9 --------- 1 file changed, 9 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 40f971e4..c0e60f4a 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -285,15 +285,6 @@ def create_tree( "switch_response_ok": None, }, ), - CallSetOrientationControl( - name="SetArticutoolOrientation", - ns=name, - inputs={ - "enable": True, - "quat_xyzw": [0.5, 0.5, 0.5, 0.5], - }, - outputs={}, - ), MoveIt2JointConstraint( name="RestingConstraint", ns=name, From 94c854c0d4411f52b30f324a350323e77053fc68 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 22 May 2025 18:11:06 -0700 Subject: [PATCH 079/238] Add behavior to adjust food frame for scooping so that we don't attempt to align with the principal axes of whatever we're trying to scoop. Instead, we simply approach along the line from the robot base to the food frame if we observe from a top down view. --- .../behaviors/acquisition/__init__.py | 3 + ...st_food_frame_yaw_for_scooping_approach.py | 260 ++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 15 + 3 files changed, 278 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py b/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py index 4ab55a19..0b4fec48 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py @@ -10,3 +10,6 @@ ComputeActionConstraints, ComputeActionTwist, ) +from .adjust_food_frame_yaw_for_scooping_approach import ( + AdjustFoodFrameYawForScoopingApproach, +) diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py b/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py new file mode 100644 index 00000000..581a2f88 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py @@ -0,0 +1,260 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This module defines the AdjustFoodFrameYawForScoopingApproach behavior. +It modifies the TF of the 'food_frame' to align the approach vector +(MoveAbove to MoveInto) with the vector from the robot base to the food, +affecting only the yaw. +""" + +# Standard imports +import math +from typing import Union, Optional + +# Third-party imports +from geometry_msgs.msg import ( + Pose, + TransformStamped, + PointStamped, + Vector3 as Vector3Msg, +) +import numpy as np +from overrides import override +import py_trees +from py_trees.common import Status +import rclpy +from rclpy.node import Node +from scipy.spatial.transform import Rotation as R +import ros2_numpy + +# TF2 imports +import tf2_ros +from tf2_geometry_msgs import do_transform_point # For transforming PointStamped + +# Local imports +from ada_feeding.behaviors import BlackboardBehavior +from ada_feeding.helpers import ( + BlackboardKey, + set_static_tf, +) # Assuming set_static_tf is available + + +class AdjustFoodFrameYawForScoopingApproach(BlackboardBehavior): + """ + Adjusts the yaw of the 'food_frame' so that the local approach vector + (from MoveAbove to MoveInto poses) aligns its XY projection in the + robot_base_frame with the XY projection of the vector from the + robot_base_frame's origin to the food_frame's origin. + The pitch and roll of the original food_frame relative to the world, + and the pitch/roll of the tool approach, are preserved. + """ + + EPSILON = 1e-6 + + def blackboard_inputs( + self, + move_above_pose_food_frame: Union[ + BlackboardKey, Pose + ], # In current (old) food_frame + move_into_pose_food_frame: Union[ + BlackboardKey, Pose + ], # In current (old) food_frame + food_frame_id: Union[BlackboardKey, str] = "food", + robot_base_frame_id: Union[BlackboardKey, str] = "j2n6s200_link_base", + ) -> None: + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + # No direct blackboard outputs other than status, effect is on TF + def blackboard_outputs(self) -> None: + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def __init__(self, name: str, **kwargs): + super().__init__(name=name, **kwargs) + self.node: Optional[Node] = None + self.tf_buffer: Optional[tf2_ros.Buffer] = None + self.tf_listener: Optional[tf2_ros.TransformListener] = None + # self.static_tf_broadcaster: Optional[tf2_ros.StaticTransformBroadcaster] = None # If set_static_tf needs it + + @override + def setup(self, **kwargs): + try: + self.node = kwargs["node"] + self.tf_buffer = tf2_ros.Buffer(node=self.node) + self.tf_listener = tf2_ros.TransformListener( + self.tf_buffer, self.node, spin_thread=False + ) + # If set_static_tf requires a broadcaster passed in: + # self.static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster(self.node) + self.logger.debug(f"[{self.name}] TF Buffer and Listener initialized.") + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + except Exception as e: + self.logger.error(f"[{self.name}] Error during setup: {e}") + + @override + def update(self) -> Status: + if not self.node or not self.tf_buffer: + self.feedback_message = "Node or TF Buffer not initialized." + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + try: + ma_pose_food: Pose = self.blackboard_get("move_above_pose_food_frame") + mi_pose_food: Pose = self.blackboard_get("move_into_pose_food_frame") + food_fid: str = self.blackboard_get("food_frame_id") + robot_base_fid: str = self.blackboard_get("robot_base_frame_id") + + # 1. Get current transform T_RobotBase_FoodOld + try: + t_rb_foodold_stamped: TransformStamped = ( + self.tf_buffer.lookup_transform( + robot_base_fid, + food_fid, + rclpy.time.Time(), + rclpy.duration.Duration(seconds=1.0), + ) + ) + except ( + tf2_ros.LookupException, + tf2_ros.ConnectivityException, + tf2_ros.ExtrapolationException, + ) as e: + self.feedback_message = ( + f"TF lookup {robot_base_fid} -> {food_fid} failed: {e}" + ) + self.logger.warn(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE # Or RUNNING if you want to retry + + R_rb_foodold_scipy = R.from_quat( + [ + t_rb_foodold_stamped.transform.rotation.x, + t_rb_foodold_stamped.transform.rotation.y, + t_rb_foodold_stamped.transform.rotation.z, + t_rb_foodold_stamped.transform.rotation.w, + ] + ) + P_foodcentroid_rb_numpy = ros2_numpy.numpify( + t_rb_foodold_stamped.transform.translation + ) + + # 2. Calculate approach vector D_FoodOld (in old food frame) + P_ma_foodold_numpy = ros2_numpy.numpify(ma_pose_food.position) + P_mi_foodold_numpy = ros2_numpy.numpify(mi_pose_food.position) + D_foodold_numpy = P_mi_foodold_numpy - P_ma_foodold_numpy + + # 3. Transform D_FoodOld to D_RobotBase (in robot base frame) + D_rb_numpy = R_rb_foodold_scipy.apply(D_foodold_numpy) + + # 4. Determine desired approach direction L_RobotBase (from robot base origin to food centroid) + # This is simply P_foodcentroid_rb_numpy if robot_base_fid is the reference + L_rb_numpy = P_foodcentroid_rb_numpy + + # 5. Project D_RobotBase and L_RobotBase onto XY plane of RobotBase frame + D_xy_rb = np.array([D_rb_numpy[0], D_rb_numpy[1]]) + L_xy_rb = np.array([L_rb_numpy[0], L_rb_numpy[1]]) + + if ( + np.linalg.norm(D_xy_rb) < self.EPSILON + or np.linalg.norm(L_xy_rb) < self.EPSILON + ): + self.logger.info( + f"[{self.name}] Approach vector or base-to-food vector has zero XY projection. Skipping yaw adjustment." + ) + # No adjustment needed, or situation is ambiguous for pure yaw. + # The original food_frame TF remains. + self.feedback_message = ( + "Skipped yaw adjustment due to zero XY projection." + ) + return Status.SUCCESS # Or FAILURE if this state is problematic + + # 6. Calculate yaw correction angle delta_psi + current_approach_yaw = math.atan2(D_xy_rb[1], D_xy_rb[0]) + desired_approach_yaw = math.atan2(L_xy_rb[1], L_xy_rb[0]) + delta_psi = self._normalize_angle( + desired_approach_yaw - current_approach_yaw + ) + + self.logger.info( + f"[{self.name}] Current approach yaw (in robot_base XY): {math.degrees(current_approach_yaw):.1f} deg. " + f"Desired: {math.degrees(desired_approach_yaw):.1f} deg. Delta: {math.degrees(delta_psi):.1f} deg." + ) + + # 7. Compute new food frame orientation R_RobotBase_FoodNew + R_deltapsi_z = R.from_euler("z", delta_psi) + R_rb_foodnew_scipy = ( + R_deltapsi_z * R_rb_foodold_scipy + ) # Apply yaw correction in RobotBase frame + + # 8. Create and publish new TransformStamped for T_RobotBase_FoodNew + t_rb_foodnew_stamped = TransformStamped() + t_rb_foodnew_stamped.header.stamp = self.node.get_clock().now().to_msg() + t_rb_foodnew_stamped.header.frame_id = robot_base_fid + t_rb_foodnew_stamped.child_frame_id = food_fid + + t_rb_foodnew_stamped.transform.translation = ros2_numpy.msgify( + Vector3Msg, P_foodcentroid_rb_numpy + ) + + new_quat_xyzw = R_rb_foodnew_scipy.as_quat() + t_rb_foodnew_stamped.transform.rotation.x = new_quat_xyzw[0] + t_rb_foodnew_stamped.transform.rotation.y = new_quat_xyzw[1] + t_rb_foodnew_stamped.transform.rotation.z = new_quat_xyzw[2] + t_rb_foodnew_stamped.transform.rotation.w = new_quat_xyzw[3] + + # Use the set_static_tf helper. + # Assuming set_static_tf is: set_static_tf(transform_stamped, blackboard_client, node_instance) + # Or if it manages its own broadcaster: set_static_tf(transform_stamped, node_instance) + # For this example, I'll assume the latter simplified version. + # If it needs the blackboard, you'd pass self.blackboard + if ( + hasattr(self, "static_tf_broadcaster") + and self.static_tf_broadcaster is not None + ): + # If you were managing it directly (less likely if set_static_tf is a true helper) + self.static_tf_broadcaster.sendTransform(t_rb_foodnew_stamped) + elif "set_static_tf" in globals() or hasattr( + ada_feeding.helpers, "set_static_tf" + ): + # Call the helper, assuming it's imported or available + set_static_tf( + t_rb_foodnew_stamped, self.node + ) # Pass node, and blackboard if needed by helper + self.logger.info( + f"[{self.name}] Updated static TF for '{food_fid}' relative to '{robot_base_fid}'." + ) + else: + self.logger.error( + f"[{self.name}] 'set_static_tf' helper not available. Cannot update TF." + ) + self.feedback_message = "set_static_tf helper missing." + return Status.FAILURE + + self.feedback_message = ( + f"Food frame yaw adjusted by {math.degrees(delta_psi):.1f} deg." + ) + return Status.SUCCESS + + except KeyError as e: + self.feedback_message = f"Blackboard key error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + except Exception as e: + self.feedback_message = f"Unexpected error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + def _normalize_angle(self, angle: float) -> float: + """Normalize an angle to [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + + @override + def terminate(self, new_status: Status) -> None: + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index c0e60f4a..41988acf 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -35,6 +35,7 @@ ComputeFoodFrame, ComputeActionConstraints, ComputeActionTwist, + AdjustFoodFrameYawForScoopingApproach, ) from ada_feeding.behaviors.moveit2 import ( MoveIt2JointConstraint, @@ -640,6 +641,20 @@ def pre_acquisition_sequence( ), }, ), + AdjustFoodFrameYawForScoopingApproach( + name="AdjustFoodYawForScooping", + ns=name, + inputs={ + "move_above_pose_food_frame": BlackboardKey( + "move_above_pose_food_frame" + ), + "move_into_pose_food_frame": BlackboardKey( + "move_into_pose_food_frame" + ), + "food_frame_id": "food", + "robot_base_frame_id": "j2n6s200_link_base", + }, + ), ], ) From efe8c6a255c93225327edaf9a155dac6aea59abd Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 22 May 2025 18:38:11 -0700 Subject: [PATCH 080/238] Add a scooping action. I haven't verified this, but we can adjust it later if it doesn't work as expected --- .../config/acquisition_library.yaml | 25 +++++++++++++++++++ 1 file changed, 25 insertions(+) diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index c6494abc..105fcef8 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -262,3 +262,28 @@ actions: - -0.0486593 - -0.1055426 pre_torque: 4.0 +- ext_angular: [0.0, 0.0, 0.0] + ext_duration: 1.0 + ext_force: 50.0 + ext_linear: [0.0, 0.0, 0.08] + ext_torque: 4.0 + grasp_angular: [0.0, 0.0, 0.0] + grasp_duration: 0.0 + grasp_force: 15.0 + grasp_linear: [0.0, 0.0, 0.0] + grasp_torque: 4.0 + pre_force: 20.0 + pre_offset: # MoveInto position in food_frame + - 0.01 # X: 1cm into the food + - 0.0 # Y + - -0.005 # Z: 0.5cm down + pre_pos: # Direction for MoveAbove standoff in food_frame + - -1.0 # Purely along -X of food_frame (will be normalized & scaled) + - 0.0 + - 0.0 + pre_quat: # Tool orientation in food_frame (Pitch -20deg around Y) + - 0.0 # qx + - -0.17364818 # qy + - 0.0 # qz + - 0.98480775 # qw + pre_torque: 4.0 From dc1081af468f4d376f597c5fe530d6456d1f9b3a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 23 May 2025 08:53:00 -0700 Subject: [PATCH 081/238] Add blackboard to set_static_tf call --- .../adjust_food_frame_yaw_for_scooping_approach.py | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py b/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py index 581a2f88..d292bd57 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py @@ -224,9 +224,7 @@ def update(self) -> Status: ada_feeding.helpers, "set_static_tf" ): # Call the helper, assuming it's imported or available - set_static_tf( - t_rb_foodnew_stamped, self.node - ) # Pass node, and blackboard if needed by helper + set_static_tf(t_rb_foodnew_stamped, self.blackboard, self.node) self.logger.info( f"[{self.name}] Updated static TF for '{food_fid}' relative to '{robot_base_fid}'." ) From fdf8adaf6b22f4d2f543b78a9dae06a318c868ce Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 23 May 2025 14:13:54 -0700 Subject: [PATCH 082/238] Update visualization script to take joint state in JSON format to visualize statically --- ada_feeding/scripts/visualize_trajectory.py | 377 ++++++++++++-------- 1 file changed, 227 insertions(+), 150 deletions(-) diff --git a/ada_feeding/scripts/visualize_trajectory.py b/ada_feeding/scripts/visualize_trajectory.py index a7c7892d..d4570ace 100755 --- a/ada_feeding/scripts/visualize_trajectory.py +++ b/ada_feeding/scripts/visualize_trajectory.py @@ -1,7 +1,17 @@ #!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. +""" +This script is used to visualize robot trajectories or single joint states +using Pinocchio and MeshCat. It can load: +1. Enhanced trajectory JSON files (containing Jaco and Articutool waypoints). +2. Single joint state JSON files (representing a sensor_msgs/msg/JointState). +""" + +# Standard imports import pinocchio as pin -import pinocchio.visualize # Ensure this is imported for MeshcatVisualizer +import pinocchio.visualize import meshcat import numpy as np import json @@ -11,8 +21,7 @@ import argparse import time import sys - -from typing import Optional, Dict, Any, List # Added List +from typing import Optional, Dict, Any, List # Define Articutool joint names as they appear in the Pinocchio model # (Verify these names from the script's "Pinocchio Model Joint Details" printout) @@ -28,7 +37,6 @@ def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str logger_func(f"Processing Xacro file: {xacro_filename}") try: - # Attempt to find ros2 executable, assuming it's in PATH process = subprocess.run( ["ros2", "run", "xacro", "xacro", xacro_filename], check=True, @@ -71,54 +79,54 @@ def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=prin if ros_package_path: package_dirs = [ p for p in ros_package_path.split(os.pathsep) if os.path.isdir(p) - ] # Use os.pathsep + ] - # Add common workspace paths if specific package_dirs are not found or to supplement - # This is a heuristic and might need adjustment based on your workspace structure common_workspace_paths = [ - os.path.expanduser("~/ros2_ws/src"), + os.path.expanduser("~/ros2_ws/src"), # Common Foxy/Galactic/Humble + os.path.expanduser("~/dev_ws/src"), # Common Iron+ os.path.expanduser("~/workspace/src"), - # Add other common workspace locations if necessary ] for wsp in common_workspace_paths: if os.path.isdir(wsp) and wsp not in package_dirs: - # Pinocchio expects a list of directories containing packages, not the package directories themselves - # So, if wsp is like '/home/user/ros2_ws/src', it's correct. package_dirs.append(wsp) - # If the current working directory's parent might be a workspace 'src' - # This helps if running from within a package. - current_ws_src_path = os.path.abspath( - os.path.join(os.getcwd(), "..", "..", "src") - ) - if ( - os.path.isdir(current_ws_src_path) - and current_ws_src_path not in package_dirs - ): - package_dirs.append(current_ws_src_path) + # Attempt to find the 'src' directory of the current ROS 2 workspace + # This is a heuristic based on typical workspace layouts. + current_dir = os.getcwd() + while current_dir != os.path.dirname(current_dir): # Stop at root + if ( + os.path.basename(current_dir) == "install" + or os.path.basename(current_dir) == "build" + or os.path.basename(current_dir) == "log" + ): + ws_root = os.path.dirname(current_dir) + src_path = os.path.join(ws_root, "src") + if os.path.isdir(src_path) and src_path not in package_dirs: + package_dirs.append(src_path) + break + current_dir = os.path.dirname(current_dir) if package_dirs: - logger_func( - f"Using package_dirs for Pinocchio: {list(set(package_dirs))}" - ) # Show unique dirs + unique_package_dirs = list(set(package_dirs)) # Remove duplicates + logger_func(f"Using package_dirs for Pinocchio: {unique_package_dirs}") model = pin.buildModelFromUrdf( - temp_urdf_path, package_dirs=list(set(package_dirs)) + temp_urdf_path, package_dirs=unique_package_dirs ) collision_model = pin.buildGeomFromUrdf( model, temp_urdf_path, pin.GeometryType.COLLISION, - package_dirs=list(set(package_dirs)), + package_dirs=unique_package_dirs, ) visual_model = pin.buildGeomFromUrdf( model, temp_urdf_path, pin.GeometryType.VISUAL, - package_dirs=list(set(package_dirs)), + package_dirs=unique_package_dirs, ) else: logger_func( - "Warning: ROS_PACKAGE_PATH not found or empty. Mesh loading might fail if using package:// paths." + "Warning: ROS_PACKAGE_PATH not found or empty, and common workspace paths not found. Mesh loading might fail if using package:// paths." ) model = pin.buildModelFromUrdf(temp_urdf_path) collision_model = pin.buildGeomFromUrdf( @@ -138,7 +146,6 @@ def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=prin return None, None, None, None finally: if temp_urdf_path and os.path.exists(temp_urdf_path): - # logger_func(f"Temporary URDF file for debugging: {temp_urdf_path}") # Uncomment to keep file os.remove(temp_urdf_path) @@ -150,7 +157,6 @@ def load_enhanced_trajectory_from_json( with open(filepath, "r") as f: traj_data = json.load(f) logger_func(f"Enhanced trajectory loaded from {filepath}") - # Basic validation for the new structure if "jaco_joint_names" not in traj_data or "waypoints" not in traj_data: logger_func( "Error: Loaded JSON is missing 'jaco_joint_names' or 'waypoints'." @@ -159,11 +165,43 @@ def load_enhanced_trajectory_from_json( return traj_data except FileNotFoundError: logger_func(f"Error: Trajectory file not found at {filepath}") + return None except json.JSONDecodeError: logger_func(f"Error: Could not decode JSON from {filepath}") + return None except Exception as e: logger_func(f"An unexpected error occurred loading trajectory: {e}") - return None + return None + + +def load_joint_state_from_json( + filepath: str, logger_func=print +) -> Optional[Dict[str, Any]]: + """Loads joint state data (expected sensor_msgs/msg/JointState format) from a JSON file.""" + try: + with open(filepath, "r") as f: + joint_state_data = json.load(f) + logger_func(f"Joint state loaded from {filepath}") + if "name" not in joint_state_data or "position" not in joint_state_data: + logger_func( + "Error: Loaded JSON for joint state is missing 'name' or 'position' fields." + ) + return None + if len(joint_state_data["name"]) != len(joint_state_data["position"]): + logger_func( + "Error: Mismatch between number of names and positions in joint state JSON." + ) + return None + return joint_state_data + except FileNotFoundError: + logger_func(f"Error: Joint state file not found at {filepath}") + return None + except json.JSONDecodeError: + logger_func(f"Error: Could not decode JSON from {filepath}") + return None + except Exception as e: + logger_func(f"An unexpected error occurred loading joint state: {e}") + return None def get_pinocchio_joint_info( @@ -173,7 +211,8 @@ def get_pinocchio_joint_info( if model.existJointName(joint_name): joint_id = model.getJointId(joint_name) joint_obj = model.joints[joint_id] - if joint_obj.nq > 0: # Only consider actuated joints + # Consider only joints that contribute to the configuration vector q + if joint_obj.idx_q >= 0 and joint_obj.nq > 0: return { "name": joint_name, "q_idx_start": joint_obj.idx_q, @@ -184,10 +223,50 @@ def get_pinocchio_joint_info( return None -def main(xacro_file: str, trajectory_file: str): - print("--- Pinocchio + MeshCat Trajectory Visualizer (Enhanced) ---") +def set_q_from_joint_state_data( + q_vector: np.ndarray, + model: pin.Model, + joint_names: List[str], + joint_positions: List[float], + logger_func=print, +): + """Updates Pinocchio q_vector based on names and positions from a JointState-like structure.""" + if len(joint_names) != len(joint_positions): + logger_func( + "Error in set_q_from_joint_state_data: name and position lists have different lengths." + ) + return + + name_to_pos = {name: joint_positions[i] for i, name in enumerate(joint_names)} + + for i in range( + 1, model.njoints + ): # Iterate through Pinocchio model joints (skip universe) + joint_name_in_model = model.names[i] + joint_info = get_pinocchio_joint_info(model, joint_name_in_model) - urdf_string = xacro_to_urdf_string(xacro_file) + if joint_info and joint_name_in_model in name_to_pos: + theta_input = name_to_pos[joint_name_in_model] + q_idx_start, nq, nv = ( + joint_info["q_idx_start"], + joint_info["nq"], + joint_info["nv"], + ) + + if nq == 1: # Typically for prismatic or revolute if not using cos/sin + q_vector[q_idx_start] = theta_input + elif nq == 2 and nv == 1: # Revolute joint with cos/sin representation + q_vector[q_idx_start] = np.cos(theta_input) + q_vector[q_idx_start + 1] = np.sin(theta_input) + # Add other joint types if necessary (e.g., free flyer) + # else: logger_func(f"Joint {joint_name_in_model} has nq={nq}, nv={nv} - unhandled for direct q setting from JointState.") + # else: logger_func(f"Joint {joint_name_in_model} from model not in provided joint state or not actuated.") + + +def main(args): + print("--- Pinocchio + MeshCat Visualizer ---") + + urdf_string = xacro_to_urdf_string(args.xacro_file) if not urdf_string: print("Exiting due to Xacro processing failure.") return @@ -200,7 +279,7 @@ def main(xacro_file: str, trajectory_file: str): if model: print("\n--- Pinocchio Model Joint Details ---") - for i in range(1, model.njoints): # Start from 1 to skip universe + for i in range(1, model.njoints): joint_name_in_model = model.names[i] joint_obj = model.joints[i] print( @@ -212,13 +291,10 @@ def main(xacro_file: str, trajectory_file: str): print("Initializing MeshCat viewer... Waiting for connection.") try: - visualizer = ( - meshcat.Visualizer().open() - ) # open=True is default if not passed to initViewer - # visualizer.wait() # Wait for browser to connect + visualizer = meshcat.Visualizer().open() print(f"MeshCat viewer URL: {visualizer.url()}") pin_viz = pin.visualize.MeshcatVisualizer(model, collision_model, visual_model) - pin_viz.initViewer(viewer=visualizer) # Already opened + pin_viz.initViewer(viewer=visualizer) pin_viz.loadViewerModel( rootNodeName=model.name if model.name else "pinocchio_robot" ) @@ -227,7 +303,44 @@ def main(xacro_file: str, trajectory_file: str): print(f"Error initializing MeshCat or Pinocchio visualizer: {e}") return - trajectory_data = load_enhanced_trajectory_from_json(trajectory_file) + q = pin.neutral(model) + print( + f"Neutral configuration q (size {model.nq}): {q.T if model.nq > 0 else 'N/A'}" + ) + + if args.ik_solution_json: + print(f"\n--- Visualizing Single IK Solution from: {args.ik_solution_json} ---") + joint_state_data = load_joint_state_from_json(args.ik_solution_json) + if not joint_state_data: + print("Failed to load IK solution JSON. Exiting.") + return + + # Set q from the loaded joint state + set_q_from_joint_state_data( + q, model, joint_state_data["name"], joint_state_data["position"] + ) + + print(f"Displaying configuration from IK solution: {q.T}") + pin_viz.display(q) + try: + input("Robot displayed. Press Enter to quit.") + except KeyboardInterrupt: + print("\nQuitting by user interrupt.") + return # Exit after displaying single state + + # --- Original Trajectory Visualization Logic --- + if not args.trajectory_file: + print( + "No trajectory file or IK solution JSON provided. Displaying neutral pose." + ) + pin_viz.display(q) + try: + input("Neutral pose displayed. Press Enter to quit.") + except KeyboardInterrupt: + print("\nQuitting by user interrupt.") + return + + trajectory_data = load_enhanced_trajectory_from_json(args.trajectory_file) if not trajectory_data: print("Failed to load valid trajectory data. Exiting.") return @@ -238,25 +351,9 @@ def main(xacro_file: str, trajectory_file: str): print("Trajectory contains no waypoints. Exiting.") return - q = pin.neutral(model) - print( - f"Neutral configuration q (size {model.nq}): {q.T if model.nq > 0 else 'N/A'}" - ) - - # Map Jaco joint names from trajectory to Pinocchio model indices - jaco_joint_mappings: List[Optional[Dict[str, Any]]] = [] - for name_in_traj in jaco_joint_names_from_traj: - jaco_joint_mappings.append(get_pinocchio_joint_info(model, name_in_traj)) - - if any(m is None for m in jaco_joint_mappings): - print( - "Warning: Some Jaco joints from trajectory not found or not actuated in model:" - ) - for i, name in enumerate(jaco_joint_names_from_traj): - if jaco_joint_mappings[i] is None: - print(f" - {name}") - - # Get Pinocchio info for Articutool joints + jaco_joint_mappings: List[Optional[Dict[str, Any]]] = [ + get_pinocchio_joint_info(model, name) for name in jaco_joint_names_from_traj + ] articutool_pitch_joint_info = get_pinocchio_joint_info( model, ARTICUTOOL_PITCH_JOINT_NAME ) @@ -264,15 +361,8 @@ def main(xacro_file: str, trajectory_file: str): model, ARTICUTOOL_ROLL_JOINT_NAME ) - if not articutool_pitch_joint_info: - print( - f"Error: Articutool pitch joint '{ARTICUTOOL_PITCH_JOINT_NAME}' not found or not actuated in model. Exiting." - ) - return - if not articutool_roll_joint_info: - print( - f"Error: Articutool roll joint '{ARTICUTOOL_ROLL_JOINT_NAME}' not found or not actuated in model. Exiting." - ) + if not articutool_pitch_joint_info or not articutool_roll_joint_info: + print("Error: Articutool pitch or roll joint not found in model. Exiting.") return print("\nArticutool Joint Mappings:") @@ -283,9 +373,7 @@ def main(xacro_file: str, trajectory_file: str): f" Roll ('{ARTICUTOOL_ROLL_JOINT_NAME}'): Maps to q[{articutool_roll_joint_info['q_idx_start']}], nq={articutool_roll_joint_info['nq']}" ) - def set_q_from_waypoint(q_vector: np.ndarray, waypoint: Dict[str, Any]): - """Updates q_vector with Jaco and Articutool positions from a waypoint.""" - # Set Jaco joints + def set_q_from_enhanced_waypoint(q_vector: np.ndarray, waypoint: Dict[str, Any]): jaco_positions = waypoint.get("jaco_positions_rad", []) for k, mapping_info in enumerate(jaco_joint_mappings): if mapping_info and k < len(jaco_positions): @@ -297,75 +385,46 @@ def set_q_from_waypoint(q_vector: np.ndarray, waypoint: Dict[str, Any]): ) if nq == 1: q_vector[q_idx_start] = theta_traj - elif nq == 2 and nv == 1: # Continuous (cos/sin) - q_vector[q_idx_start] = np.cos(theta_traj) - q_vector[q_idx_start + 1] = np.sin(theta_traj) + elif nq == 2 and nv == 1: + q_vector[q_idx_start], q_vector[q_idx_start + 1] = ( + np.cos(theta_traj), + np.sin(theta_traj), + ) - # Set Articutool joints if waypoint.get("articutool_waypoint_feasible", False): - pitch_sol = waypoint.get("articutool_pitch_solution_rad") - roll_sol = waypoint.get("articutool_roll_solution_rad") - - if pitch_sol is not None and articutool_pitch_joint_info: - q_idx = articutool_pitch_joint_info["q_idx_start"] - # Assuming nq=1 for these revolute joints - if articutool_pitch_joint_info["nq"] == 1: - q_vector[q_idx] = pitch_sol - elif ( - articutool_pitch_joint_info["nq"] == 2 - and articutool_pitch_joint_info["nv"] == 1 - ): # cos/sin - q_vector[q_idx] = np.cos(pitch_sol) - q_vector[q_idx + 1] = np.sin(pitch_sol) - - if roll_sol is not None and articutool_roll_joint_info: - q_idx = articutool_roll_joint_info["q_idx_start"] - if articutool_roll_joint_info["nq"] == 1: - q_vector[q_idx] = roll_sol - elif ( - articutool_roll_joint_info["nq"] == 2 - and articutool_roll_joint_info["nv"] == 1 - ): # cos/sin - q_vector[q_idx] = np.cos(roll_sol) - q_vector[q_idx + 1] = np.sin(roll_sol) - else: - # Articutool not feasible at this waypoint. - # Optionally, set Articutool joints to a default/neutral pose (e.g., 0.0) - # This provides a visual cue. If not set, they retain previous values. - if articutool_pitch_joint_info: - q_idx = articutool_pitch_joint_info["q_idx_start"] - if articutool_pitch_joint_info["nq"] == 1: - q_vector[q_idx] = 0.0 - elif ( - articutool_pitch_joint_info["nq"] == 2 - and articutool_pitch_joint_info["nv"] == 1 - ): - q_vector[q_idx] = np.cos(0.0) - q_vector[q_idx + 1] = np.sin(0.0) - if articutool_roll_joint_info: - q_idx = articutool_roll_joint_info["q_idx_start"] - if articutool_roll_joint_info["nq"] == 1: - q_vector[q_idx] = 0.0 - elif ( - articutool_roll_joint_info["nq"] == 2 - and articutool_roll_joint_info["nv"] == 1 - ): - q_vector[q_idx] = np.cos(0.0) - q_vector[q_idx + 1] = np.sin(0.0) - # print(f" Articutool not feasible at this waypoint. Setting its joints to 0.") - - # Initial display - current_q_display = q.copy() # Start with neutral + pitch_sol, roll_sol = ( + waypoint.get("articutool_pitch_solution_rad"), + waypoint.get("articutool_roll_solution_rad"), + ) + for sol, info in [ + (pitch_sol, articutool_pitch_joint_info), + (roll_sol, articutool_roll_joint_info), + ]: + if sol is not None and info: + q_idx, nq, nv = info["q_idx_start"], info["nq"], info["nv"] + if nq == 1: + q_vector[q_idx] = sol + elif nq == 2 and nv == 1: + q_vector[q_idx], q_vector[q_idx + 1] = np.cos(sol), np.sin(sol) + else: # Articutool not feasible, set to neutral (0.0) + for info in [articutool_pitch_joint_info, articutool_roll_joint_info]: + if info: + q_idx, nq, nv = info["q_idx_start"], info["nq"], info["nv"] + if nq == 1: + q_vector[q_idx] = 0.0 + elif nq == 2 and nv == 1: + q_vector[q_idx], q_vector[q_idx + 1] = np.cos(0.0), np.sin(0.0) + + current_q_display = q.copy() if waypoints_data: - set_q_from_waypoint(current_q_display, waypoints_data[0]) + set_q_from_enhanced_waypoint(current_q_display, waypoints_data[0]) pin_viz.display(current_q_display) - q[:] = current_q_display[:] # Update main q + q[:] = current_q_display[:] current_point_idx = 0 num_trajectory_points = len(waypoints_data) print("\n--- Trajectory Control ---") print("Open the MeshCat URL in your browser.") - running = True while running: wp_info = waypoints_data[current_point_idx] @@ -376,16 +435,13 @@ def set_q_from_waypoint(q_vector: np.ndarray, waypoint: Dict[str, Any]): at_pitch = f"{at_pitch:.3f}" if isinstance(at_roll, float): at_roll = f"{at_roll:.3f}" - print( f"\nPoint: {current_point_idx + 1}/{num_trajectory_points} | Articutool Feasible: {at_feasible} (P: {at_pitch}, R: {at_roll})" ) print("Commands: [n]ext, [p]rev, [f]irst, [l]ast, [g ], [a]nimate, [q]uit") - try: user_input = input("Enter command: ").strip().lower() new_q_to_display = q.copy() - if user_input == "q": running = False print("Quitting.") @@ -418,17 +474,23 @@ def set_q_from_waypoint(q_vector: np.ndarray, waypoint: Dict[str, Any]): try: for i in range(start_anim_idx, num_trajectory_points): current_point_idx = i - set_q_from_waypoint(anim_q, waypoints_data[current_point_idx]) + set_q_from_enhanced_waypoint( + anim_q, waypoints_data[current_point_idx] + ) pin_viz.display(anim_q) print( f" Displaying point {current_point_idx + 1}/{num_trajectory_points}", end="\r", flush=True, ) - time.sleep(0.05) # Animation speed - print("\nAnimation finished. ") + time.sleep(0.05) + print( + "\nAnimation finished. " + ) except KeyboardInterrupt: - print("\nAnimation stopped. ") + print( + "\nAnimation stopped. " + ) new_q_to_display[:] = anim_q[:] q[:] = new_q_to_display[:] pin_viz.display(q) @@ -438,11 +500,11 @@ def set_q_from_waypoint(q_vector: np.ndarray, waypoint: Dict[str, Any]): continue else: continue - - set_q_from_waypoint(new_q_to_display, waypoints_data[current_point_idx]) + set_q_from_enhanced_waypoint( + new_q_to_display, waypoints_data[current_point_idx] + ) pin_viz.display(new_q_to_display) q[:] = new_q_to_display[:] - except EOFError: print("\nEOF received, quitting.") running = False @@ -456,15 +518,30 @@ def set_q_from_waypoint(q_vector: np.ndarray, waypoint: Dict[str, Any]): if __name__ == "__main__": parser = argparse.ArgumentParser( - description="Visualize enhanced JointTrajectory using Pinocchio and MeshCat." + description="Visualize robot trajectories or single IK solutions using Pinocchio and MeshCat." ) - parser.add_argument( - "xacro_file", + parser.add_argument("xacro_file", type=str, help="Path to the robot XACRO file.") + + group = parser.add_mutually_exclusive_group( + required=False + ) # Make providing one of these optional + group.add_argument( + "--trajectory_file", type=str, - help="Path to the robot XACRO file (that instantiates the robot).", + help="Path to the .json ENHANCED trajectory file.", ) - parser.add_argument( - "trajectory_file", type=str, help="Path to the .json ENHANCED trajectory file." + group.add_argument( + "--ik_solution_json", + type=str, + help="Path to a .json file representing a single sensor_msgs/msg/JointState for IK visualization.", ) + args = parser.parse_args() - main(args.xacro_file, args.trajectory_file) + + if not args.trajectory_file and not args.ik_solution_json: + print( + "Neither --trajectory_file nor --ik_solution_json provided. Will display neutral pose." + ) + # main will handle displaying neutral if both are None after model load. + + main(args) From 35ef89a6864c919007e896fc79b0586ef36e1bac Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 23 May 2025 14:14:34 -0700 Subject: [PATCH 083/238] Update scooping motion, now that I verified the orientation is correct --- .../config/acquisition_library.yaml | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index 105fcef8..af14ee3f 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -265,7 +265,7 @@ actions: - ext_angular: [0.0, 0.0, 0.0] ext_duration: 1.0 ext_force: 50.0 - ext_linear: [0.0, 0.0, 0.08] + ext_linear: [0.0, 0.0, 0.08] # Lift after scoop ext_torque: 4.0 grasp_angular: [0.0, 0.0, 0.0] grasp_duration: 0.0 @@ -273,17 +273,17 @@ actions: grasp_linear: [0.0, 0.0, 0.0] grasp_torque: 4.0 pre_force: 20.0 - pre_offset: # MoveInto position in food_frame - - 0.01 # X: 1cm into the food - - 0.0 # Y - - -0.005 # Z: 0.5cm down - pre_pos: # Direction for MoveAbove standoff in food_frame - - -1.0 # Purely along -X of food_frame (will be normalized & scaled) - - 0.0 - - 0.0 - pre_quat: # Tool orientation in food_frame (Pitch -20deg around Y) - - 0.0 # qx - - -0.17364818 # qy - - 0.0 # qz - - 0.98480775 # qw + pre_offset: # Tool tip target position IN FOOD_FRAME at end of "MoveInto" + - 0.02 # X: e.g., 2cm "into" the food along food_frame X-axis + - 0.0 # Y + - -0.01 # Z: e.g., 1cm down from food_frame origin + pre_pos: # Defines DIRECTION for "MoveAbove" standoff IN FOOD_FRAME + - -1.0 # Approach from food_frame -X (will be normalized & scaled by move_above_dist_m) + - 0.0 # Y + - 0.0 # Z (can add a small positive Z for height if needed, e.g., 0.1 or 0.2) + pre_quat: # Tool tip orientation IN FOOD_FRAME + - 0.353453 # qx + - 0.6122721 # qy + - 0.6127465 # qz + - 0.3531793 # qw pre_torque: 4.0 From e0d22263fb2d8ab897eb19d1f711c6e31b69beca Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 23 May 2025 16:07:26 -0700 Subject: [PATCH 084/238] Replace AdjustFoodFrameYawForScoopingApproach behavior with the RotateLocalApproachPoses behavior which now simply rotates the MoveAbove and MoveInto poses about the food frame's z-axis to align with the straight line from the robot's base to the food frame --- .../behaviors/acquisition/__init__.py | 4 +- ...st_food_frame_yaw_for_scooping_approach.py | 258 ----------------- .../rotate_local_approach_poses.py | 271 ++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 42 +-- 4 files changed, 298 insertions(+), 277 deletions(-) delete mode 100644 ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py create mode 100644 ada_feeding/ada_feeding/behaviors/acquisition/rotate_local_approach_poses.py diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py b/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py index 0b4fec48..1b51bdee 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py @@ -10,6 +10,6 @@ ComputeActionConstraints, ComputeActionTwist, ) -from .adjust_food_frame_yaw_for_scooping_approach import ( - AdjustFoodFrameYawForScoopingApproach, +from .rotate_local_approach_poses import ( + RotateLocalApproachPoses, ) diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py b/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py deleted file mode 100644 index d292bd57..00000000 --- a/ada_feeding/ada_feeding/behaviors/acquisition/adjust_food_frame_yaw_for_scooping_approach.py +++ /dev/null @@ -1,258 +0,0 @@ -# -*- coding: utf-8 -*- -# Copyright (c) 2024-2025, Personal Robotics Laboratory -# License: BSD 3-Clause. See LICENSE.md file in root directory. - -""" -This module defines the AdjustFoodFrameYawForScoopingApproach behavior. -It modifies the TF of the 'food_frame' to align the approach vector -(MoveAbove to MoveInto) with the vector from the robot base to the food, -affecting only the yaw. -""" - -# Standard imports -import math -from typing import Union, Optional - -# Third-party imports -from geometry_msgs.msg import ( - Pose, - TransformStamped, - PointStamped, - Vector3 as Vector3Msg, -) -import numpy as np -from overrides import override -import py_trees -from py_trees.common import Status -import rclpy -from rclpy.node import Node -from scipy.spatial.transform import Rotation as R -import ros2_numpy - -# TF2 imports -import tf2_ros -from tf2_geometry_msgs import do_transform_point # For transforming PointStamped - -# Local imports -from ada_feeding.behaviors import BlackboardBehavior -from ada_feeding.helpers import ( - BlackboardKey, - set_static_tf, -) # Assuming set_static_tf is available - - -class AdjustFoodFrameYawForScoopingApproach(BlackboardBehavior): - """ - Adjusts the yaw of the 'food_frame' so that the local approach vector - (from MoveAbove to MoveInto poses) aligns its XY projection in the - robot_base_frame with the XY projection of the vector from the - robot_base_frame's origin to the food_frame's origin. - The pitch and roll of the original food_frame relative to the world, - and the pitch/roll of the tool approach, are preserved. - """ - - EPSILON = 1e-6 - - def blackboard_inputs( - self, - move_above_pose_food_frame: Union[ - BlackboardKey, Pose - ], # In current (old) food_frame - move_into_pose_food_frame: Union[ - BlackboardKey, Pose - ], # In current (old) food_frame - food_frame_id: Union[BlackboardKey, str] = "food", - robot_base_frame_id: Union[BlackboardKey, str] = "j2n6s200_link_base", - ) -> None: - super().blackboard_inputs( - **{key: value for key, value in locals().items() if key != "self"} - ) - - # No direct blackboard outputs other than status, effect is on TF - def blackboard_outputs(self) -> None: - super().blackboard_outputs( - **{key: value for key, value in locals().items() if key != "self"} - ) - - def __init__(self, name: str, **kwargs): - super().__init__(name=name, **kwargs) - self.node: Optional[Node] = None - self.tf_buffer: Optional[tf2_ros.Buffer] = None - self.tf_listener: Optional[tf2_ros.TransformListener] = None - # self.static_tf_broadcaster: Optional[tf2_ros.StaticTransformBroadcaster] = None # If set_static_tf needs it - - @override - def setup(self, **kwargs): - try: - self.node = kwargs["node"] - self.tf_buffer = tf2_ros.Buffer(node=self.node) - self.tf_listener = tf2_ros.TransformListener( - self.tf_buffer, self.node, spin_thread=False - ) - # If set_static_tf requires a broadcaster passed in: - # self.static_tf_broadcaster = tf2_ros.StaticTransformBroadcaster(self.node) - self.logger.debug(f"[{self.name}] TF Buffer and Listener initialized.") - except KeyError as e: - self.logger.error( - f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" - ) - except Exception as e: - self.logger.error(f"[{self.name}] Error during setup: {e}") - - @override - def update(self) -> Status: - if not self.node or not self.tf_buffer: - self.feedback_message = "Node or TF Buffer not initialized." - self.logger.error(f"[{self.name}] {self.feedback_message}") - return Status.FAILURE - - try: - ma_pose_food: Pose = self.blackboard_get("move_above_pose_food_frame") - mi_pose_food: Pose = self.blackboard_get("move_into_pose_food_frame") - food_fid: str = self.blackboard_get("food_frame_id") - robot_base_fid: str = self.blackboard_get("robot_base_frame_id") - - # 1. Get current transform T_RobotBase_FoodOld - try: - t_rb_foodold_stamped: TransformStamped = ( - self.tf_buffer.lookup_transform( - robot_base_fid, - food_fid, - rclpy.time.Time(), - rclpy.duration.Duration(seconds=1.0), - ) - ) - except ( - tf2_ros.LookupException, - tf2_ros.ConnectivityException, - tf2_ros.ExtrapolationException, - ) as e: - self.feedback_message = ( - f"TF lookup {robot_base_fid} -> {food_fid} failed: {e}" - ) - self.logger.warn(f"[{self.name}] {self.feedback_message}") - return Status.FAILURE # Or RUNNING if you want to retry - - R_rb_foodold_scipy = R.from_quat( - [ - t_rb_foodold_stamped.transform.rotation.x, - t_rb_foodold_stamped.transform.rotation.y, - t_rb_foodold_stamped.transform.rotation.z, - t_rb_foodold_stamped.transform.rotation.w, - ] - ) - P_foodcentroid_rb_numpy = ros2_numpy.numpify( - t_rb_foodold_stamped.transform.translation - ) - - # 2. Calculate approach vector D_FoodOld (in old food frame) - P_ma_foodold_numpy = ros2_numpy.numpify(ma_pose_food.position) - P_mi_foodold_numpy = ros2_numpy.numpify(mi_pose_food.position) - D_foodold_numpy = P_mi_foodold_numpy - P_ma_foodold_numpy - - # 3. Transform D_FoodOld to D_RobotBase (in robot base frame) - D_rb_numpy = R_rb_foodold_scipy.apply(D_foodold_numpy) - - # 4. Determine desired approach direction L_RobotBase (from robot base origin to food centroid) - # This is simply P_foodcentroid_rb_numpy if robot_base_fid is the reference - L_rb_numpy = P_foodcentroid_rb_numpy - - # 5. Project D_RobotBase and L_RobotBase onto XY plane of RobotBase frame - D_xy_rb = np.array([D_rb_numpy[0], D_rb_numpy[1]]) - L_xy_rb = np.array([L_rb_numpy[0], L_rb_numpy[1]]) - - if ( - np.linalg.norm(D_xy_rb) < self.EPSILON - or np.linalg.norm(L_xy_rb) < self.EPSILON - ): - self.logger.info( - f"[{self.name}] Approach vector or base-to-food vector has zero XY projection. Skipping yaw adjustment." - ) - # No adjustment needed, or situation is ambiguous for pure yaw. - # The original food_frame TF remains. - self.feedback_message = ( - "Skipped yaw adjustment due to zero XY projection." - ) - return Status.SUCCESS # Or FAILURE if this state is problematic - - # 6. Calculate yaw correction angle delta_psi - current_approach_yaw = math.atan2(D_xy_rb[1], D_xy_rb[0]) - desired_approach_yaw = math.atan2(L_xy_rb[1], L_xy_rb[0]) - delta_psi = self._normalize_angle( - desired_approach_yaw - current_approach_yaw - ) - - self.logger.info( - f"[{self.name}] Current approach yaw (in robot_base XY): {math.degrees(current_approach_yaw):.1f} deg. " - f"Desired: {math.degrees(desired_approach_yaw):.1f} deg. Delta: {math.degrees(delta_psi):.1f} deg." - ) - - # 7. Compute new food frame orientation R_RobotBase_FoodNew - R_deltapsi_z = R.from_euler("z", delta_psi) - R_rb_foodnew_scipy = ( - R_deltapsi_z * R_rb_foodold_scipy - ) # Apply yaw correction in RobotBase frame - - # 8. Create and publish new TransformStamped for T_RobotBase_FoodNew - t_rb_foodnew_stamped = TransformStamped() - t_rb_foodnew_stamped.header.stamp = self.node.get_clock().now().to_msg() - t_rb_foodnew_stamped.header.frame_id = robot_base_fid - t_rb_foodnew_stamped.child_frame_id = food_fid - - t_rb_foodnew_stamped.transform.translation = ros2_numpy.msgify( - Vector3Msg, P_foodcentroid_rb_numpy - ) - - new_quat_xyzw = R_rb_foodnew_scipy.as_quat() - t_rb_foodnew_stamped.transform.rotation.x = new_quat_xyzw[0] - t_rb_foodnew_stamped.transform.rotation.y = new_quat_xyzw[1] - t_rb_foodnew_stamped.transform.rotation.z = new_quat_xyzw[2] - t_rb_foodnew_stamped.transform.rotation.w = new_quat_xyzw[3] - - # Use the set_static_tf helper. - # Assuming set_static_tf is: set_static_tf(transform_stamped, blackboard_client, node_instance) - # Or if it manages its own broadcaster: set_static_tf(transform_stamped, node_instance) - # For this example, I'll assume the latter simplified version. - # If it needs the blackboard, you'd pass self.blackboard - if ( - hasattr(self, "static_tf_broadcaster") - and self.static_tf_broadcaster is not None - ): - # If you were managing it directly (less likely if set_static_tf is a true helper) - self.static_tf_broadcaster.sendTransform(t_rb_foodnew_stamped) - elif "set_static_tf" in globals() or hasattr( - ada_feeding.helpers, "set_static_tf" - ): - # Call the helper, assuming it's imported or available - set_static_tf(t_rb_foodnew_stamped, self.blackboard, self.node) - self.logger.info( - f"[{self.name}] Updated static TF for '{food_fid}' relative to '{robot_base_fid}'." - ) - else: - self.logger.error( - f"[{self.name}] 'set_static_tf' helper not available. Cannot update TF." - ) - self.feedback_message = "set_static_tf helper missing." - return Status.FAILURE - - self.feedback_message = ( - f"Food frame yaw adjusted by {math.degrees(delta_psi):.1f} deg." - ) - return Status.SUCCESS - - except KeyError as e: - self.feedback_message = f"Blackboard key error: {e}" - self.logger.error(f"[{self.name}] {self.feedback_message}") - return Status.FAILURE - except Exception as e: - self.feedback_message = f"Unexpected error: {e}" - self.logger.error(f"[{self.name}] {self.feedback_message}") - return Status.FAILURE - - def _normalize_angle(self, angle: float) -> float: - """Normalize an angle to [-pi, pi].""" - return (angle + math.pi) % (2 * math.pi) - math.pi - - @override - def terminate(self, new_status: Status) -> None: - self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/rotate_local_approach_poses.py b/ada_feeding/ada_feeding/behaviors/acquisition/rotate_local_approach_poses.py new file mode 100644 index 00000000..c0e87b6a --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/acquisition/rotate_local_approach_poses.py @@ -0,0 +1,271 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This module defines the RotateLocalApproachPoses behavior. +It takes original MoveAbove and MoveInto poses defined in a food_frame, +and rotates them around the food_frame's Z-axis. The rotation angle +is calculated to align the food_frame's Y-axis (when projected onto the +robot_base_frame's XY plane) with the vector from the robot_base_frame's +origin to the food_frame's origin (also projected onto the robot_base_frame's XY plane). +This is intended to align the tool's functional direction (assumed to be +oriented along the food_frame's Y-axis by a pre_quat) with the +robot-to-food line. +""" + +# Standard imports +import math +from typing import Union, Optional + +# Third-party imports +from geometry_msgs.msg import Pose, Point as PointMsg, Quaternion as QuaternionMsg +import numpy as np +from overrides import override +import py_trees +from py_trees.common import Status +import rclpy +from rclpy.node import Node +from scipy.spatial.transform import Rotation as R +import ros2_numpy + +# TF2 imports +import tf2_ros + +# Local imports +from ada_feeding.behaviors import BlackboardBehavior +from ada_feeding.helpers import BlackboardKey + + +class RotateLocalApproachPoses(BlackboardBehavior): + """ + Rotates local MoveAbove and MoveInto poses around the Z-axis of their + defining food_frame to align the food_frame's Y-axis (and thus a + tool Z-axis aligned with it) with the robot-base-to-food-origin direction. + """ + + EPSILON = 1e-6 + + def blackboard_inputs( + self, + move_above_pose_in_food_frame_orig: Union[BlackboardKey, Pose], + move_into_pose_in_food_frame_orig: Union[BlackboardKey, Pose], + food_frame_id: Union[BlackboardKey, str] = "food", + robot_base_frame_id: Union[BlackboardKey, str] = "j2n6s200_link_base", + ) -> None: + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + move_above_pose_in_food_frame_rotated: Optional[BlackboardKey], + move_into_pose_in_food_frame_rotated: Optional[BlackboardKey], + ) -> None: + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def __init__(self, name: str, **kwargs): + super().__init__(name=name, **kwargs) + self.node: Optional[Node] = None + self.tf_buffer: Optional[tf2_ros.Buffer] = None + self.tf_listener: Optional[tf2_ros.TransformListener] = None + self._initialized_properly = False + + @override + def setup(self, **kwargs): + try: + self.node = kwargs["node"] + self.tf_buffer = tf2_ros.Buffer(node=self.node) + self.tf_listener = tf2_ros.TransformListener( + self.tf_buffer, self.node, spin_thread=False + ) + self.logger.debug(f"[{self.name}] TF Buffer and Listener initialized.") + self._initialized_properly = True + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + self._initialized_properly = False + except Exception as e: + self.logger.error(f"[{self.name}] Error during setup: {e}") + self._initialized_properly = False + + def _normalize_angle(self, angle: float) -> float: + """Normalize an angle to [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + + @override + def update(self) -> Status: + if not self._initialized_properly or not self.node or not self.tf_buffer: + self.feedback_message = "Behavior not properly initialized in setup." + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + try: + ma_pose_orig_food: Pose = self.blackboard_get( + "move_above_pose_in_food_frame_orig" + ) + mi_pose_orig_food: Pose = self.blackboard_get( + "move_into_pose_in_food_frame_orig" + ) + food_fid: str = self.blackboard_get("food_frame_id") + robot_base_fid: str = self.blackboard_get("robot_base_frame_id") + + # 1. Get current transform T_RobotBase_FoodOriginal + try: + t_rb_food_stamped: TransformStamped = self.tf_buffer.lookup_transform( + robot_base_fid, + food_fid, + rclpy.time.Time(), + rclpy.duration.Duration(seconds=1.0), + ) + except Exception as e: + self.feedback_message = ( + f"TF lookup {robot_base_fid} -> {food_fid} failed: {e}" + ) + self.logger.warn( + f"[{self.name}] {self.feedback_message}. Retrying if BT ticks again." + ) + return Status.RUNNING + + R_rb_food = R.from_quat( + [ + t_rb_food_stamped.transform.rotation.x, + t_rb_food_stamped.transform.rotation.y, + t_rb_food_stamped.transform.rotation.z, + t_rb_food_stamped.transform.rotation.w, + ] + ) + P_food_in_rb = ros2_numpy.numpify(t_rb_food_stamped.transform.translation) + + # 2. Calculate Desired Approach Line in Robot Base XY plane (L_xy_rb) + L_xy_rb_numpy = np.array( + [P_food_in_rb[0], P_food_in_rb[1]] + ) # Only XY components + desired_global_approach_yaw_rb: float + if np.linalg.norm(L_xy_rb_numpy) < self.EPSILON: + self.logger.info( + f"[{self.name}] Food origin too close to robot base XY origin. Desired global yaw is ill-defined, using 0." + ) + desired_global_approach_yaw_rb = 0.0 + # If desired direction is undefined, delta_psi should ideally make current X-axis align with robot_base X-axis, + # or simply make delta_psi = 0 if current_food_x_yaw_rb is also ill-defined. + # For now, if desired is ill-defined, we'll effectively try to rotate food X to robot X. + else: + L_xy_rb_norm_vec = L_xy_rb_numpy / np.linalg.norm(L_xy_rb_numpy) + desired_global_approach_yaw_rb = math.atan2( + L_xy_rb_norm_vec[1], L_xy_rb_norm_vec[0] + ) + + # 3. Calculate Current Direction of Food Frame's X-axis in Robot Base XY plane + X_axis_in_food_frame = np.array([1.0, 0.0, 0.0]) # Food Frame's X-axis + X_axis_in_rb_frame = R_rb_food.apply( + X_axis_in_food_frame + ) # Transform to robot base + X_axis_xy_in_rb_frame = np.array( + [X_axis_in_rb_frame[0], X_axis_in_rb_frame[1]] + ) + + current_food_x_yaw_rb: float + if np.linalg.norm(X_axis_xy_in_rb_frame) < self.EPSILON: + self.logger.warn( + f"[{self.name}] Food frame's X-axis has near-zero XY projection in robot base. " + f"This implies food_frame Z is nearly aligned with robot_base XY plane (e.g. food frame is 'on its side'). " + f"Cannot reliably determine current food_X_yaw. Assuming 0 adjustment needed relative to this." + ) + # If current X-axis has no defined yaw, any desired_global_approach_yaw_rb becomes the delta directly, + # but this might not be what we want. It's better to make no change if current state is ambiguous. + delta_psi_food_z = 0.0 + else: + X_axis_xy_in_rb_norm_vec = X_axis_xy_in_rb_frame / np.linalg.norm( + X_axis_xy_in_rb_frame + ) + current_food_x_yaw_rb = math.atan2( + X_axis_xy_in_rb_norm_vec[1], X_axis_xy_in_rb_norm_vec[0] + ) + + # 4. Calculate Yaw Correction delta_psi_food_z + # This is the rotation around the food_frame's Z-axis needed to align its X-axis + # (projected to robot_base XY) with the desired_global_approach_yaw_rb. + delta_psi_food_z = self._normalize_angle( + desired_global_approach_yaw_rb - current_food_x_yaw_rb + ) + self.logger.info( + f"[{self.name}] Aligning Food_X to base-food line. " + f"Current Food_X Yaw (in RB): {math.degrees(current_food_x_yaw_rb):.1f} deg. " + f"Desired Global Yaw (for Food_X in RB): {math.degrees(desired_global_approach_yaw_rb):.1f} deg. " + f"Required Food_Z Rotation: {math.degrees(delta_psi_food_z):.1f} deg." + ) + + # 5. Apply rotation to local poses (around Z-axis of food_frame) + R_correction_in_food_z = R.from_euler("z", delta_psi_food_z) + + # Rotate MoveAbove pose + P_ma_orig_food_numpy = ros2_numpy.numpify(ma_pose_orig_food.position) + R_ma_orig_food_scipy = R.from_quat( + [ + ma_pose_orig_food.orientation.x, + ma_pose_orig_food.orientation.y, + ma_pose_orig_food.orientation.z, + ma_pose_orig_food.orientation.w, + ] + ) + P_ma_rotated_food_numpy = R_correction_in_food_z.apply(P_ma_orig_food_numpy) + R_ma_rotated_food_scipy = R_correction_in_food_z * R_ma_orig_food_scipy + + ma_pose_rotated_food = Pose() + ma_pose_rotated_food.position = ros2_numpy.msgify( + PointMsg, P_ma_rotated_food_numpy + ) + ma_quat_xyzw_rotated = R_ma_rotated_food_scipy.as_quat() + ma_pose_rotated_food.orientation.x = ma_quat_xyzw_rotated[0] + ma_pose_rotated_food.orientation.y = ma_quat_xyzw_rotated[1] + ma_pose_rotated_food.orientation.z = ma_quat_xyzw_rotated[2] + ma_pose_rotated_food.orientation.w = ma_quat_xyzw_rotated[3] + self.blackboard_set( + "move_above_pose_in_food_frame_rotated", ma_pose_rotated_food + ) + + # Rotate MoveInto pose + P_mi_orig_food_numpy = ros2_numpy.numpify(mi_pose_orig_food.position) + R_mi_orig_food_scipy = R.from_quat( + [ + mi_pose_orig_food.orientation.x, + mi_pose_orig_food.orientation.y, + mi_pose_orig_food.orientation.z, + mi_pose_orig_food.orientation.w, + ] + ) # Should be same as R_ma_orig_food_scipy + P_mi_rotated_food_numpy = R_correction_in_food_z.apply(P_mi_orig_food_numpy) + R_mi_rotated_food_scipy = R_correction_in_food_z * R_mi_orig_food_scipy + + mi_pose_rotated_food = Pose() + mi_pose_rotated_food.position = ros2_numpy.msgify( + PointMsg, P_mi_rotated_food_numpy + ) + mi_quat_xyzw_rotated = R_mi_rotated_food_scipy.as_quat() + mi_pose_rotated_food.orientation.x = mi_quat_xyzw_rotated[0] + mi_pose_rotated_food.orientation.y = mi_quat_xyzw_rotated[1] + mi_pose_rotated_food.orientation.z = mi_quat_xyzw_rotated[2] + mi_pose_rotated_food.orientation.w = mi_quat_xyzw_rotated[3] + self.blackboard_set( + "move_into_pose_in_food_frame_rotated", mi_pose_rotated_food + ) + + self.feedback_message = f"Local poses rotated by {math.degrees(delta_psi_food_z):.1f} deg around food Z." + return Status.SUCCESS + + except KeyError as e: + self.feedback_message = f"Blackboard key error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + except Exception as e: + self.feedback_message = f"Unexpected error in {self.name}: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + @override + def terminate(self, new_status: Status) -> None: + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 41988acf..ee600dab 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -35,7 +35,7 @@ ComputeFoodFrame, ComputeActionConstraints, ComputeActionTwist, - AdjustFoodFrameYawForScoopingApproach, + RotateLocalApproachPoses, ) from ada_feeding.behaviors.moveit2 import ( MoveIt2JointConstraint, @@ -550,6 +550,28 @@ def pre_acquisition_sequence( }, ), ), + RotateLocalApproachPoses( + name="RotateLocalFoodPosesForApproach", + ns=name, + inputs={ + "move_above_pose_in_food_frame_orig": BlackboardKey( + "move_above_pose_food_frame" + ), + "move_into_pose_in_food_frame_orig": BlackboardKey( + "move_into_pose_food_frame" + ), + "food_frame_id": "food", + "robot_base_frame_id": "j2n6s200_link_base", + }, + outputs={ + "move_above_pose_in_food_frame_rotated": BlackboardKey( + "move_above_pose_final_local" + ), + "move_into_pose_in_food_frame_rotated": BlackboardKey( + "move_into_pose_final_local" + ), + }, + ), # Re-Tare FT Sensor and default to 4N threshold pre_moveto_config(name="PreAcquireFTTare"), # --- Prepare MoveAbove Pose for IK --- @@ -557,7 +579,7 @@ def pre_acquisition_sequence( name="StampMoveAbovePoseFood", ns=name, inputs={ - "input_pose": BlackboardKey("move_above_pose_food_frame"), + "input_pose": BlackboardKey("move_above_pose_final_local"), "frame_id": "food", }, outputs={ @@ -585,7 +607,7 @@ def pre_acquisition_sequence( name="StampMoveIntoPoseFood", ns=name, inputs={ - "input_pose": BlackboardKey("move_into_pose_food_frame"), + "input_pose": BlackboardKey("move_into_pose_final_local"), "frame_id": "food", }, outputs={ @@ -641,20 +663,6 @@ def pre_acquisition_sequence( ), }, ), - AdjustFoodFrameYawForScoopingApproach( - name="AdjustFoodYawForScooping", - ns=name, - inputs={ - "move_above_pose_food_frame": BlackboardKey( - "move_above_pose_food_frame" - ), - "move_into_pose_food_frame": BlackboardKey( - "move_into_pose_food_frame" - ), - "food_frame_id": "food", - "robot_base_frame_id": "j2n6s200_link_base", - }, - ), ], ) From d7efac95bae44e76b3d5277d23ef91a8f3799d85 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 23 May 2025 16:07:52 -0700 Subject: [PATCH 085/238] Reformat moveit2_compute_ik, and add a log to see the result --- .../behaviors/moveit2/moveit2_compute_ik.py | 64 ++++++++++++++----- 1 file changed, 49 insertions(+), 15 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py index 70c04a56..111be0b8 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py @@ -52,7 +52,9 @@ def blackboard_inputs( target_pose: Union[BlackboardKey, PoseStamped], group_name: Union[BlackboardKey, str], start_joint_state: Optional[Union[BlackboardKey, JointState]] = None, - constraints: Optional[Union[BlackboardKey, List[Tuple[MoveIt2ConstraintType, Dict[str, Any]]]]] = None, + constraints: Optional[ + Union[BlackboardKey, List[Tuple[MoveIt2ConstraintType, Dict[str, Any]]]] + ] = None, ) -> None: """ Blackboard Inputs @@ -182,38 +184,69 @@ def update(self) -> py_trees.common.Status: ) return py_trees.common.Status.FAILURE - ik_constraints_msg : Optional[Constraints] = None + ik_constraints_msg: Optional[Constraints] = None if isinstance(constraints, list) and constraints: ik_constraints_msg = Constraints() ik_constraints_msg.name = "ik_constraints_from_list" - self.logger.debug(f"[{self.name}] Processing {len(constraints)} constraint specifications for IK.") + self.logger.debug( + f"[{self.name}] Processing {len(constraints)} constraint specifications for IK." + ) for constraint_type, constraint_kwargs in constraints: try: if constraint_type == MoveIt2ConstraintType.JOINT: # Assumes wrapper has create_joint_constraint(**kwargs) -> JointConstraint - constraint_obj = self.moveit2_obj.create_joint_constraint(**constraint_kwargs) - if constraint_obj: ik_constraints_msg.joint_constraints.append(constraint_obj) + constraint_obj = ( + self.moveit2_obj.create_joint_constraint( + **constraint_kwargs + ) + ) + if constraint_obj: + ik_constraints_msg.joint_constraints.append( + constraint_obj + ) elif constraint_type == MoveIt2ConstraintType.POSITION: # Assumes wrapper has create_position_constraint(**kwargs) -> PositionConstraint - constraint_obj = self.moveit2_obj.create_position_constraint(**constraint_kwargs) - if constraint_obj: ik_constraints_msg.position_constraints.append(constraint_obj) + constraint_obj = ( + self.moveit2_obj.create_position_constraint( + **constraint_kwargs + ) + ) + if constraint_obj: + ik_constraints_msg.position_constraints.append( + constraint_obj + ) elif constraint_type == MoveIt2ConstraintType.ORIENTATION: # Assumes wrapper has create_orientation_constraint(**kwargs) -> OrientationConstraint - constraint_obj = self.moveit2_obj.create_orientation_constraint(**constraint_kwargs) - if constraint_obj: ik_constraints_msg.orientation_constraints.append(constraint_obj) + constraint_obj = ( + self.moveit2_obj.create_orientation_constraint( + **constraint_kwargs + ) + ) + if constraint_obj: + ik_constraints_msg.orientation_constraints.append( + constraint_obj + ) else: - self.logger.warning(f"[{self.name}] Unknown constraint type '{constraint_type}' in list.") + self.logger.warning( + f"[{self.name}] Unknown constraint type '{constraint_type}' in list." + ) except AttributeError as ae: - self.logger.error(f"[{self.name}] MoveIt2 wrapper missing 'create_X_constraint' method for type {constraint_type}? Error: {ae}") - return py_trees.common.Status.FAILURE + self.logger.error( + f"[{self.name}] MoveIt2 wrapper missing 'create_X_constraint' method for type {constraint_type}? Error: {ae}" + ) + return py_trees.common.Status.FAILURE except Exception as e_constr: - self.logger.error(f"[{self.name}] Error processing constraint {constraint_type} with args {constraint_kwargs}: {e_constr}") - return py_trees.common.Status.FAILURE + self.logger.error( + f"[{self.name}] Error processing constraint {constraint_type} with args {constraint_kwargs}: {e_constr}" + ) + return py_trees.common.Status.FAILURE # Optional: Write generated message to blackboard for debugging # self.blackboard_set("generated_ik_constraints_msg", ik_constraints_msg) elif constraints is not None: - self.logger.warning(f"[{self.name}] Input 'constraints' is not a list, ignoring. Type: {type(constraints)}") + self.logger.warning( + f"[{self.name}] Input 'constraints' is not a list, ignoring. Type: {type(constraints)}" + ) # --- Call the synchronous compute_ik method provided in the API --- # It takes position and orientation separately. @@ -240,6 +273,7 @@ def update(self) -> py_trees.common.Status: if ik_success: self.logger.info(f"[{self.name}] IK computation successful.") + self.logger.info(f"IK solution: {ik_result_state}") return py_trees.common.Status.SUCCESS else: self.logger.warning( From 87fd61cc001a95765860ddc28f41eec45e42f5d0 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 23 May 2025 16:47:11 -0700 Subject: [PATCH 086/238] Re-enable MoveAbove and MoveInto motions without explicit orientation control on the Articutool --- .../ada_feeding/trees/acquire_food_tree.py | 706 +++++++++--------- 1 file changed, 353 insertions(+), 353 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index ee600dab..363f458e 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1178,359 +1178,359 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: ), move_above_sequence(), move_into_sequence(), - # MoveIt2Execute( - # name="MoveAboveJacoArm", - # ns=name, - # inputs={ - # "trajectory": BlackboardKey( - # "move_above_jaco_arm_trajectory" - # ), - # "group_name": "jaco_arm", - # }, - # outputs={}, - # ), - # ExecuteArticutoolTrajectory( - # name="MoveAboveArticutool", - # ns=name, - # inputs={ - # "trajectory": BlackboardKey( - # "move_above_articutool_trajectory" - # ), - # }, - # outputs={ - # "action_goal_accepted": BlackboardKey( - # "tool_goal_accepted" - # ), - # "action_result_code": BlackboardKey( - # "tool_exec_result_code" - # ), - # "action_status": BlackboardKey( - # "tool_action_status" - # ), - # }, - # ), - # # If Anything goes wrong, reset FT to safe levels - # scoped_behavior( - # name="SafeFTPreempt", - # # Set Approach F/T Thresh - # 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")] - # ), - # response_checks=[ - # py_trees.common.ComparisonExpression( - # variable=Blackboard.separator.join( - # [ - # name, - # BlackboardKey( - # "ft_response" - # ), - # ] - # ), - # value=SetParameters.Response(), # Unused - # operator=set_parameter_response_all_success, - # ) - # ], - # ), - # ], - # ), - # post_behavior=py_trees.composites.Sequence( - # name=name, - # memory=True, - # children=[ - # pre_moveto_config( - # name="PostAcquireFTSet", re_tare=False - # ), - # ], - # ), - # on_preempt_timeout=5.0, - # # Starts a new Sequence w/ Memory internally - # workers=[ - # ### Move Into Food - # SwitchArticutoolControllers( - # name="SwitchArticutoolToVelocity", - # ns=name, - # inputs={ - # "controllers_to_activate": [ - # "velocity_controller" - # ], - # "controllers_to_deactivate": [ - # "joint_trajectory_controller" - # ], - # }, - # outputs={ - # "switch_call_succeeded": None, - # "switch_response_ok": None, - # }, - # ), - # ExtractPoseComponents( - # name="GetMoveIntoOrientation", - # ns=name, - # inputs={ - # "input_pose_object": BlackboardKey( - # "move_into_pose_stamped_base_frame" - # ), - # }, - # outputs={ - # "output_position": BlackboardKey( - # "move_into_position" - # ), - # "output_orientation": BlackboardKey( - # "move_into_orientation" - # ), - # "output_header": BlackboardKey( - # "move_into_header" - # ), - # "success": None, - # }, - # ), - # CallSetOrientationControl( - # name="SetArticutoolOrientation", - # ns=name, - # inputs={ - # "enable": True, - # "quat_xyzw": BlackboardKey( - # "move_into_orientation" - # ), - # }, - # outputs={}, - # ), - # # MoveInto expect F/T failure - # py_trees.decorators.FailureIsSuccess( - # name="MoveIntoJacoArmExecuteSucceed", - # child=MoveIt2Execute( - # name="MoveIntoJacoArm", - # ns=name, - # inputs={ - # "trajectory": BlackboardKey( - # "move_into_jaco_arm_trajectory" - # ) - # }, - # outputs={}, - # ), - # ), - # ### 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, - # servo_controller_name="jaco_arm_cartesian_controller", - # start_moveit_servo=False, - # ) - # .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_cartesian_controller_parameters", - # ), - # StopServoTree( - # self._node, - # servo_controller_name="jaco_arm_cartesian_controller", - # stop_moveit_servo=False, - # ) - # .create_tree(name="StopServoScoped") - # .root, - # ], - # ), - # on_preempt_timeout=5.0, - # # Starts a new Sequence w/ Memory internally - # workers=[ - # py_trees.composites.Selector( - # name="InFoodErrorSelector", - # memory=True, - # children=[ - # py_trees.composites.Sequence( - # name="InFoodGraspExtract", - # memory=True, - # children=[ - # ### Grasp - # retry_call_ros_service( - # name="GraspFTThresh", - # service_type=SetParameters, - # service_name="~/set_cartesian_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, - # ) - # ], - # ), - # 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" - # ), - # "pub_topic": "~/cartesian_twist_cmds", - # "servo_status_sub_topic": None, - # }, - # ), # Auto Zero-Twist on terminate() - # ### Extraction - # ComputeActionTwist( - # name="ComputeExtract", - # 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_cartesian_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" - # ), - # "pub_topic": "~/cartesian_twist_cmds", - # "servo_status_sub_topic": None, - # }, - # ), # Auto Zero-Twist on terminate() - # ft_thresh_satisfied( - # name="CheckFTForkOffPlate" - # ), - # ], # End InFoodGraspExtract.children - # ), # End InFoodGraspExtract - # recovery_tree, - # ], # End InFoodErrorSelector.children - # ), # End InFoodErrorSelector - # ], # End MoveIt2Servo.workers - # ), # End MoveIt2Servo - # ], # End SafeFTPreempt.workers - # ), # End SafeFTPreempt + MoveIt2Execute( + name="MoveAboveJacoArm", + ns=name, + inputs={ + "trajectory": BlackboardKey( + "move_above_jaco_arm_trajectory" + ), + "group_name": "jaco_arm", + }, + outputs={}, + ), + ExecuteArticutoolTrajectory( + name="MoveAboveArticutool", + ns=name, + inputs={ + "trajectory": BlackboardKey( + "move_above_articutool_trajectory" + ), + }, + outputs={ + "action_goal_accepted": BlackboardKey( + "tool_goal_accepted" + ), + "action_result_code": BlackboardKey( + "tool_exec_result_code" + ), + "action_status": BlackboardKey( + "tool_action_status" + ), + }, + ), + # If Anything goes wrong, reset FT to safe levels + scoped_behavior( + name="SafeFTPreempt", + # Set Approach F/T Thresh + 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")] + ), + response_checks=[ + py_trees.common.ComparisonExpression( + variable=Blackboard.separator.join( + [ + name, + BlackboardKey( + "ft_response" + ), + ] + ), + value=SetParameters.Response(), # Unused + operator=set_parameter_response_all_success, + ) + ], + ), + ], + ), + post_behavior=py_trees.composites.Sequence( + name=name, + memory=True, + children=[ + pre_moveto_config( + name="PostAcquireFTSet", re_tare=False + ), + ], + ), + on_preempt_timeout=5.0, + # Starts a new Sequence w/ Memory internally + workers=[ + ### Move Into Food + # SwitchArticutoolControllers( + # name="SwitchArticutoolToVelocity", + # ns=name, + # inputs={ + # "controllers_to_activate": [ + # "velocity_controller" + # ], + # "controllers_to_deactivate": [ + # "joint_trajectory_controller" + # ], + # }, + # outputs={ + # "switch_call_succeeded": None, + # "switch_response_ok": None, + # }, + # ), + # ExtractPoseComponents( + # name="GetMoveIntoOrientation", + # ns=name, + # inputs={ + # "input_pose_object": BlackboardKey( + # "move_into_pose_stamped_base_frame" + # ), + # }, + # outputs={ + # "output_position": BlackboardKey( + # "move_into_position" + # ), + # "output_orientation": BlackboardKey( + # "move_into_orientation" + # ), + # "output_header": BlackboardKey( + # "move_into_header" + # ), + # "success": None, + # }, + # ), + # CallSetOrientationControl( + # name="SetArticutoolOrientation", + # ns=name, + # inputs={ + # "enable": True, + # "quat_xyzw": BlackboardKey( + # "move_into_orientation" + # ), + # }, + # outputs={}, + # ), + # MoveInto expect F/T failure + py_trees.decorators.FailureIsSuccess( + name="MoveIntoJacoArmExecuteSucceed", + child=MoveIt2Execute( + name="MoveIntoJacoArm", + ns=name, + inputs={ + "trajectory": BlackboardKey( + "move_into_jaco_arm_trajectory" + ) + }, + outputs={}, + ), + ), + ### 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, + servo_controller_name="jaco_arm_cartesian_controller", + start_moveit_servo=False, + ) + .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_cartesian_controller_parameters", + ), + StopServoTree( + self._node, + servo_controller_name="jaco_arm_cartesian_controller", + stop_moveit_servo=False, + ) + .create_tree(name="StopServoScoped") + .root, + ], + ), + on_preempt_timeout=5.0, + # Starts a new Sequence w/ Memory internally + workers=[ + py_trees.composites.Selector( + name="InFoodErrorSelector", + memory=True, + children=[ + py_trees.composites.Sequence( + name="InFoodGraspExtract", + memory=True, + children=[ + ### Grasp + retry_call_ros_service( + name="GraspFTThresh", + service_type=SetParameters, + service_name="~/set_cartesian_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, + ) + ], + ), + 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" + ), + "pub_topic": "~/cartesian_twist_cmds", + "servo_status_sub_topic": None, + }, + ), # Auto Zero-Twist on terminate() + ### Extraction + ComputeActionTwist( + name="ComputeExtract", + 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_cartesian_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" + ), + "pub_topic": "~/cartesian_twist_cmds", + "servo_status_sub_topic": None, + }, + ), # Auto Zero-Twist on terminate() + ft_thresh_satisfied( + name="CheckFTForkOffPlate" + ), + ], # End InFoodGraspExtract.children + ), # End InFoodGraspExtract + recovery_tree, + ], # End InFoodErrorSelector.children + ), # End InFoodErrorSelector + ], # End MoveIt2Servo.workers + ), # End MoveIt2Servo + ], # End SafeFTPreempt.workers + ), # End SafeFTPreempt ], # End OctomapAndTableCollision.workers ), # OctomapAndTableCollision ], From 991f355e380d2b45bbe7ee977c8d4e1c6c7a85e4 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 23 May 2025 23:45:25 -0700 Subject: [PATCH 087/238] Add behavior to compute joint configuration for Articutool to achieve level orientation for a given Jaco EE pose, and plan/execute such a motion before going to the resting configuration --- .../behaviors/articutool/__init__.py | 1 + .../compute_articutool_leveling_joints.py | 277 ++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 134 ++++++--- 3 files changed, 378 insertions(+), 34 deletions(-) create mode 100644 ada_feeding/ada_feeding/behaviors/articutool/compute_articutool_leveling_joints.py diff --git a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py index 17af1b81..b63cd8a9 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py @@ -10,3 +10,4 @@ from .switch_articutool_controllers import ( SwitchArticutoolControllers, ) +from .compute_articutool_leveling_joints import ComputeArticutoolLevelingJoints diff --git a/ada_feeding/ada_feeding/behaviors/articutool/compute_articutool_leveling_joints.py b/ada_feeding/ada_feeding/behaviors/articutool/compute_articutool_leveling_joints.py new file mode 100644 index 00000000..e81e2b90 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/articutool/compute_articutool_leveling_joints.py @@ -0,0 +1,277 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This module defines the ComputeArticutoolLevelingJoints behavior. +Given the Jaco arm's end-effector world pose, this behavior calculates +the Articutool joint angles (pitch, roll) required to make the +Articutool's tool_tip Y-axis point upwards against gravity (world +Z). +""" + +# Standard imports +import math +from typing import Union, Optional, List, Tuple + +# Third-party imports +from geometry_msgs.msg import Pose, PoseStamped, Quaternion as QuaternionMsg +import numpy as np +from overrides import override +from scipy.spatial.transform import Rotation as R +import py_trees +from py_trees.common import Status, Access +import rclpy +from rclpy.node import Node + +# Local imports +from ada_feeding.behaviors import BlackboardBehavior # Assuming this is your base class +from ada_feeding.helpers import BlackboardKey + + +class ComputeArticutoolLevelingJoints(BlackboardBehavior): + """ + Calculates Articutool joint angles for its tool_tip Y-axis to align with World +Z. + """ + + EPSILON = 1e-6 + WORLD_Z_UP_VECTOR = np.array([0.0, 0.0, 1.0]) # Target direction in World Frame + + def blackboard_inputs( + self, + jaco_ee_world_pose: Union[ + BlackboardKey, Pose, PoseStamped + ], # Current pose of Jaco EE in world + articutool_pitch_limits_rad: Union[BlackboardKey, Tuple[float, float]] = ( + -math.pi / 2, + math.pi / 2, + ), + articutool_roll_limits_rad: Union[BlackboardKey, Tuple[float, float]] = ( + -math.pi, + math.pi, + ), + ) -> None: + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + articutool_joint_positions: Union[BlackboardKey, List[float]], + articutool_leveling_ik_found: Optional[BlackboardKey], + ) -> None: + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def __init__(self, name: str, **kwargs): + super().__init__(name=name, **kwargs) + self.node: Optional[Node] = None + self._initialized_properly = False + + @override + def setup(self, **kwargs): + try: + self.node = kwargs["node"] + self._initialized_properly = True + self.logger.debug(f"[{self.name}] Behavior initialized.") + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + self._initialized_properly = False + except Exception as e: + self.logger.error(f"[{self.name}] Error during setup: {e}") + self._initialized_properly = False + + def _normalize_angle(self, angle: float) -> float: + """Normalize an angle to [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + + def _solve_articutool_ik_for_leveling( + self, target_y_axis_in_jaco_hand_frame: np.ndarray + ) -> List[Tuple[float, float]]: + """ + Analytical IK solver for the 2-DOF Articutool. + Input: target_y_axis_in_jaco_hand_frame (3D numpy array) - This is the desired + pointing direction of the Articutool's tool_tip Y-axis, expressed in the + Jaco Hand Frame (Articutool's base frame, F_JH). + Output: List of (theta_p, theta_r) solutions in radians. + This IK is based on the one used in the benchmark and CheckArticutoolPathOrientationFeasibility. + It assumes the relationship where the input vector (vx, vy, vz) from F_JH + is related to the Articutool's FK $y_{tip}^{AB}(\theta_p, \theta_r) = (X_{AB}, Y_{AB}, Z_{AB})$ + after transformation $y_{tip}^{AB} = R_z(-\pi/2) \cdot (v_x, v_y, v_z)^T = (v_y, -v_x, v_z)^T$. + So, the IK solves for: + $\cos\theta_p \cos\theta_r = v_y$ + $\sin\theta_r = -v_x$ + $\sin\theta_p \cos\theta_r = v_z$ + """ + vx, vy, vz = ( + target_y_axis_in_jaco_hand_frame # This is the target vector in Jaco Hand Frame + ) + solutions: List[Tuple[float, float]] = [] + + # From sin(theta_r) = -vx + asin_arg_for_tr = -vx + if not (-1.0 - self.EPSILON <= asin_arg_for_tr <= 1.0 + self.EPSILON): + self.logger.debug( + f"[{self.name}] IK: asin_arg_for_tr ({asin_arg_for_tr:.4f}) out of range for -vx={-vx:.4f}." + ) + return [] # No real solution for theta_r + + asin_arg_for_tr_clipped = np.clip(asin_arg_for_tr, -1.0, 1.0) + theta_r_sol1 = math.asin(asin_arg_for_tr_clipped) + theta_r_sol2 = self._normalize_angle(math.pi - theta_r_sol1) + + candidate_thetas_r = [theta_r_sol1] + if not math.isclose(theta_r_sol1, theta_r_sol2, abs_tol=self.EPSILON): + candidate_thetas_r.append(theta_r_sol2) + + for theta_r in candidate_thetas_r: + cos_theta_r = math.cos(theta_r) + # From cos(theta_p)cos(theta_r) = vy AND sin(theta_p)cos(theta_r) = vz + # If cos(theta_r) is near zero (singularity): + if math.isclose(cos_theta_r, 0.0, abs_tol=self.EPSILON): + # Then vy and vz must also be near zero. + # If so, theta_p is indeterminate but can be chosen (e.g., 0). + # This case also means sin(theta_r) = +/-1, so -vx = +/-1. + if math.isclose(vy, 0.0, abs_tol=self.EPSILON) and math.isclose( + vz, 0.0, abs_tol=self.EPSILON + ): + # This implies target vector was (∓1, 0, 0) in Jaco Hand Frame. + # This would mean Artic Y is trying to point along Jaco Hand +/-X. + solutions.append( + (0.0, self._normalize_angle(theta_r)) + ) # Choose theta_p = 0 + # else: (vy or vz is not zero, but cos_theta_r is zero) -> No solution + continue # Skip to next theta_r if singular and conditions not met + + # Regular case: cos_theta_r is not zero + # theta_p = atan2(sin_theta_p, cos_theta_p) + # theta_p = atan2(vz / cos_theta_r, vy / cos_theta_r) which simplifies to atan2(vz, vy) + theta_p_sol = math.atan2(vz, vy) + solutions.append( + (self._normalize_angle(theta_p_sol), self._normalize_angle(theta_r)) + ) + return solutions + + @override + def update(self) -> Status: + if not self._initialized_properly: + self.feedback_message = "Behavior not properly initialized." + self.logger.error(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_leveling_ik_found", False) + return Status.FAILURE + + try: + jaco_ee_pose_input: Union[Pose, PoseStamped] = self.blackboard_get( + "jaco_ee_world_pose" + ) + pitch_limits: Tuple[float, float] = self.blackboard_get( + "articutool_pitch_limits_rad" + ) + roll_limits: Tuple[float, float] = self.blackboard_get( + "articutool_roll_limits_rad" + ) + + current_jaco_ee_pose: Pose + if isinstance(jaco_ee_pose_input, PoseStamped): + current_jaco_ee_pose = jaco_ee_pose_input.pose + elif isinstance(jaco_ee_pose_input, Pose): + current_jaco_ee_pose = jaco_ee_pose_input + else: + self.feedback_message = ( + "jaco_ee_world_pose is not a Pose or PoseStamped." + ) + self.logger.error(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_leveling_ik_found", False) + return Status.FAILURE + + # 1. Extract R_World_JacoEE from the Jaco EE's world pose + R_world_jacoee = R.from_quat( + [ + current_jaco_ee_pose.orientation.x, + current_jaco_ee_pose.orientation.y, + current_jaco_ee_pose.orientation.z, + current_jaco_ee_pose.orientation.w, + ] + ) + + # 2. Desired tool_tip Y-axis in World Frame is WORLD_Z_UP_VECTOR + # Transform this into the Jaco EE frame (F_JH) + # target_Ytip_in_JacoEE = R_JacoEE_World * WORLD_Z_UP_VECTOR + target_Ytip_in_JacoEE = R_world_jacoee.inv().apply(self.WORLD_Z_UP_VECTOR) + + self.logger.debug( + f"[{self.name}] Jaco EE R_W_EE: {R_world_jacoee.as_quat(canonical=False)}" + ) + self.logger.debug( + f"[{self.name}] Target Y_tip in JacoEE frame: {np.round(target_Ytip_in_JacoEE, 3)}" + ) + + # 3. Solve Articutool IK + ik_solutions = self._solve_articutool_ik_for_leveling(target_Ytip_in_JacoEE) + + if not ik_solutions: + self.feedback_message = "Articutool IK found no solutions for leveling." + self.logger.warn(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_leveling_ik_found", False) + return ( + Status.FAILURE + ) # Or SUCCESS if allowing no solution to mean "do nothing" + + # 4. Select a valid solution (e.g., first one within limits, or closest to current) + # For simplicity, taking the first valid one. + valid_solution_found = False + best_pitch = None + best_roll = None + + for theta_p_sol, theta_r_sol in ik_solutions: + # Normalize again just to be safe, though IK solver should do it + tp_norm = self._normalize_angle(theta_p_sol) + tr_norm = self._normalize_angle(theta_r_sol) + + if ( + pitch_limits[0] - self.EPSILON + <= tp_norm + <= pitch_limits[1] + self.EPSILON + and roll_limits[0] - self.EPSILON + <= tr_norm + <= roll_limits[1] + self.EPSILON + ): + best_pitch = tp_norm + best_roll = tr_norm + valid_solution_found = True + self.logger.info( + f"[{self.name}] Found valid Articutool leveling solution: " + f"Pitch={math.degrees(best_pitch):.1f} deg, Roll={math.degrees(best_roll):.1f} deg" + ) + self.blackboard_set( + "articutool_joint_positions", [best_pitch, best_roll] + ) + self.blackboard_set("articutool_leveling_ik_found", True) + self.feedback_message = "Articutool leveling IK solution found." + return Status.SUCCESS + + self.feedback_message = ( + "Articutool IK solutions for leveling are out of joint limits." + ) + self.logger.warn( + f"[{self.name}] {self.feedback_message} Solutions: {[(math.degrees(s[0]), math.degrees(s[1])) for s in ik_solutions]}" + ) + self.blackboard_set("articutool_leveling_ik_found", False) + return Status.FAILURE + + except KeyError as e: # For blackboard_get errors + self.feedback_message = f"Blackboard key error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_leveling_ik_found", False) + return Status.FAILURE + except Exception as e: + self.feedback_message = f"Unexpected error in {self.name}: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_leveling_ik_found", False) + return Status.FAILURE + + @override + def terminate(self, new_status: Status) -> None: + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 363f458e..bb61914f 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -61,14 +61,11 @@ ) from ada_feeding.behaviors.ros.msgs import StampPoseFromPose from ada_feeding.behaviors.ros.tf import ApplyTransform -from ada_feeding.behaviors.articutool.execute_articutool_trajectory import ( +from ada_feeding.behaviors.articutool import ( ExecuteArticutoolTrajectory, -) -from ada_feeding.behaviors.articutool.call_set_orientation_control import ( CallSetOrientationControl, -) -from ada_feeding.behaviors.articutool.switch_articutool_controllers import ( SwitchArticutoolControllers, + ComputeArticutoolLevelingJoints, ) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( @@ -219,47 +216,116 @@ def create_tree( # Default fail if service is down wait_for_server_timeout_sec=0.0, ), + GetJointStates( + name="GetJacoArmStateForLeveling", + ns=name, + node=self._node, + inputs={ + "joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ], + }, + outputs={ + "joint_state": BlackboardKey( + "current_jaco_arm_state_for_leveling_fk" + ), + "joint_positions": None, + "joint_names": None, + }, + ), + MoveIt2ComputeFK( + name="GetJacoEEPoseForLeveling", + ns=name, + inputs={ + "group_name": "jaco_arm", + "joint_state": BlackboardKey( + "current_jaco_arm_state_for_leveling_fk" + ), + "fk_link_names": ["j2n6s200_end_effector"], + }, + outputs={ + "fk_poses": BlackboardKey("current_jaco_arm_fk_poses"), + "success": None, + }, + ), + ExtractPoseFromPosesByLink( + name="ExtractJacoEEPoseForLeveling", + ns=name, + inputs={ + "fk_poses": BlackboardKey("current_jaco_arm_fk_poses"), + "target_link_name": "j2n6s200_end_effector", + "requested_link_names": ["j2n6s200_end_effector"], + }, + outputs={ + "extracted_pose": BlackboardKey( + "current_jaco_ee_world_pose_stamped" + ), + "success": None, + }, + ), + ComputeArticutoolLevelingJoints( + name="ComputeLevelingAngles", + ns=name, + inputs={ + "jaco_ee_world_pose": BlackboardKey( + "current_jaco_ee_world_pose_stamped" + ), + }, + outputs={ + "articutool_joint_positions": BlackboardKey( + "articutool_joint_positions" + ), + "articutool_leveling_ik_found": BlackboardKey( + "leveling_ik_success" + ), + }, + ), MoveIt2JointConstraint( - name="HomeArticutoolConstraint", + name="SetLevelingJointGoal", ns=name, inputs={ - "joint_positions": [0.0, 0.0], - "joint_names": ["atool_joint1", "atool_joint2"], + "joint_positions": BlackboardKey( + "articutool_joint_positions" + ), }, outputs={ - "constraints": BlackboardKey("goal_constraints"), + "constraints": BlackboardKey( + "articutool_leveling_constraints" + ) }, ), py_trees.decorators.Timeout( - name="HomeArticutoolPlanTimeout", + name="PlanToLevelArticutoolTimeout", # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such duration=10.0 * self.allowed_planning_time_to_resting_configuration, child=MoveIt2Plan( - name="HomeArticutoolPlan", + name="PlanToLevelArticutool", ns=name, inputs={ "goal_constraints": BlackboardKey( - "goal_constraints" + "articutool_leveling_constraints" ), - "max_velocity_scale": self.max_velocity_scaling_to_resting_configuration, - "max_acceleration_scale": self.max_acceleration_scaling_to_resting_configuration, - "allowed_planning_time": self.allowed_planning_time_to_resting_configuration, "group_name": "articutool", }, outputs={ "trajectory": BlackboardKey( - "home_articutool_trajectory" + "level_articutool_trajectory" ) }, ), ), ExecuteArticutoolTrajectory( - name="HomeArticutool", + name="LevelArticutool", ns=name, inputs={ "trajectory": BlackboardKey( - "home_articutool_trajectory" + "level_articutool_trajectory" ), }, outputs={ @@ -272,20 +338,20 @@ def create_tree( "action_status": BlackboardKey("tool_action_status"), }, ), - SwitchArticutoolControllers( - name="SwitchArticutoolToVelocity", - ns=name, - inputs={ - "controllers_to_activate": ["velocity_controller"], - "controllers_to_deactivate": [ - "joint_trajectory_controller" - ], - }, - outputs={ - "switch_call_succeeded": None, - "switch_response_ok": None, - }, - ), + # SwitchArticutoolControllers( + # name="SwitchArticutoolToVelocity", + # ns=name, + # inputs={ + # "controllers_to_activate": ["velocity_controller"], + # "controllers_to_deactivate": [ + # "joint_trajectory_controller" + # ], + # }, + # outputs={ + # "switch_call_succeeded": None, + # "switch_response_ok": None, + # }, + # ), MoveIt2JointConstraint( name="RestingConstraint", ns=name, @@ -1533,8 +1599,8 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: ), # End SafeFTPreempt ], # End OctomapAndTableCollision.workers ), # OctomapAndTableCollision - ], - # + resting_position_behaviors, # End Success.workers + ] + + resting_position_behaviors, # End Success.workers ), # End Success # TableCollision ], # End root_seq.children ) # End root_seq From 7ce9e4f0d7db95b0fee85a33c1c4b3b0b96c9257 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 24 May 2025 10:37:39 -0700 Subject: [PATCH 088/238] Add behavior to trigger Articutool IMU calibration --- .../behaviors/articutool/__init__.py | 1 + .../trigger_articutool_calibration.py | 275 ++++++++++++++++++ 2 files changed, 276 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/articutool/trigger_articutool_calibration.py diff --git a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py index b63cd8a9..5c2762ca 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py @@ -11,3 +11,4 @@ SwitchArticutoolControllers, ) from .compute_articutool_leveling_joints import ComputeArticutoolLevelingJoints +from .trigger_articutool_calibration import TriggerArticutoolCalibration diff --git a/ada_feeding/ada_feeding/behaviors/articutool/trigger_articutool_calibration.py b/ada_feeding/ada_feeding/behaviors/articutool/trigger_articutool_calibration.py new file mode 100644 index 00000000..6597fa0a --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/articutool/trigger_articutool_calibration.py @@ -0,0 +1,275 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +Defines the TriggerArticutoolCalibration behavior, which calls the service +to trigger the Articutool's IMU orientation calibration routine. +""" + +import rclpy +from rclpy.node import Node +from rclpy.executors import Future # For type hints +from rclpy.duration import Duration as RCLPYDuration # For service call timeout + +from typing import Union, Optional, Dict, Any +import time # For service call timeout logic + +from geometry_msgs.msg import Quaternion as QuaternionMsg +from articutool_interfaces.srv import TriggerCalibration # Service type + +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Access, Status + +# Local imports +from ada_feeding.helpers import BlackboardKey # Assuming this is your helper +from ada_feeding.behaviors import BlackboardBehavior # Assuming this is your base class + + +class TriggerArticutoolCalibration(BlackboardBehavior): + """ + Calls the TriggerCalibration service to initiate Articutool's + IMU orientation calibration. + + Returns SUCCESS if the service call completes and the service response indicates success. + Returns FAILURE if the service is unavailable, the call fails, or the service response indicates failure. + Returns RUNNING while waiting for the service response. + """ + + DEFAULT_SERVICE_NAME = ( + "/orientation_calibration_service/trigger_calibration" # Default name + ) + DEFAULT_WAIT_FOR_SERVER_TIMEOUT_SEC = 2.0 + DEFAULT_SERVICE_CALL_TIMEOUT_SEC = 10.0 # Calibration might take some time + + def blackboard_inputs( + self, + service_name: Union[BlackboardKey, str] = DEFAULT_SERVICE_NAME, + wait_for_server_timeout_sec: Union[ + BlackboardKey, float + ] = DEFAULT_WAIT_FOR_SERVER_TIMEOUT_SEC, + service_call_timeout_sec: Union[ + BlackboardKey, float + ] = DEFAULT_SERVICE_CALL_TIMEOUT_SEC, + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + service_name: Name of the TriggerCalibration service. + wait_for_server_timeout_sec: Max time (sec) to wait for server availability. + service_call_timeout_sec: Max time (sec) to wait for the service call to complete. + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + calibration_call_succeeded: Optional[ + BlackboardKey + ] = None, # bool: Did the ROS service call itself succeed? + calibration_routine_success: Optional[ + BlackboardKey + ] = None, # bool: From service response .success + calibration_message: Optional[ + BlackboardKey + ] = None, # string: From service response .message + computed_calibration_offset_quat: Optional[ + BlackboardKey + ] = None, # QuaternionMsg: From service response + ) -> None: + """ + Blackboard Outputs + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def setup(self, **kwargs): + """Get the node and create the service client.""" + self.node: Optional[Node] = kwargs.get("node") + self.client = None + self._service_name_str: str = self.DEFAULT_SERVICE_NAME + self._wait_timeout_float: float = self.DEFAULT_WAIT_FOR_SERVER_TIMEOUT_SEC + self._call_timeout_float: float = self.DEFAULT_SERVICE_CALL_TIMEOUT_SEC + self._initialized_properly = False + + if not self.node: + self.logger.error( + f"[{self.name}] Node object not provided in setup kwargs." + ) + return + + try: + # It's better to get these once in setup if they don't change per tick, + # or get them in update if they can change dynamically from the blackboard. + # For service clients, usually setup is fine. + self._service_name_str = self.blackboard_get("service_name") + self._wait_timeout_float = self.blackboard_get( + "wait_for_server_timeout_sec" + ) + self._call_timeout_float = self.blackboard_get("service_call_timeout_sec") + + self.client = self.node.create_client( + TriggerCalibration, self._service_name_str + ) + self.logger.info( + f"[{self.name}] Service client created for '{self._service_name_str}'" + ) + self._initialized_properly = True + except KeyError as e: + self.logger.error(f"[{self.name}] Missing blackboard key during setup: {e}") + except Exception as e: + self.logger.error( + f"[{self.name}] Failed to create service client during setup: {e}" + ) + + def update(self) -> Status: + """Manage the asynchronous service call state machine.""" + if not self._initialized_properly or self.client is None: + self.logger.error( + f"[{self.name}] Service client not available. Setup likely failed." + ) + self.blackboard_set( + "calibration_call_succeeded", False + ) # Use internal port name + self.blackboard_set("calibration_routine_success", False) + return Status.FAILURE + + # --- State 1: Ready to Call --- + if self.service_future is None: + if self._wait_timeout_float >= 0.0: + if not self.client.wait_for_service( + timeout_sec=self._wait_timeout_float + ): + self.logger.error( + f"[{self.name}] Service '{self._service_name_str}' not available after {self._wait_timeout_float}s timeout." + ) + self.blackboard_set("calibration_call_succeeded", False) + return Status.FAILURE + + # If wait_for_service timeout is < 0, we attempt call without waiting (not recommended for critical services) + # elif not self.client.service_is_ready(): + # self.logger.error(f"[{self.name}] Service '{self._service_name_str}' not ready (no wait).") + # self.blackboard_set("calibration_call_succeeded", False) + # return Status.FAILURE + + req = TriggerCalibration.Request() # Empty request + self.logger.info( + f"[{self.name}] Calling service '{self._service_name_str}' to trigger calibration." + ) + try: + self.service_future = self.client.call_async(req) + self.call_sent_time = time.monotonic() # Record time when call was sent + return Status.RUNNING + except Exception as e: + self.logger.error(f"[{self.name}] Error sending service request: {e}") + self.blackboard_set("calibration_call_succeeded", False) + return Status.FAILURE + + # --- State 2: Waiting for Response --- + elif self.service_future and not self.service_future.done(): + if self.call_sent_time is not None and ( + time.monotonic() - self.call_sent_time > self._call_timeout_float + ): + self.logger.error( + f"[{self.name}] Service call to '{self._service_name_str}' timed out after {self._call_timeout_float}s." + ) + # Attempt to cancel, though ROS 2 service call cancellation is not straightforward + if hasattr(self.service_future, "cancel") and callable( + self.service_future.cancel + ): + if ( + not self.service_future.cancelled() + ): # Check if it can be cancelled + if self.service_future.cancel(): # Try to cancel + self.logger.info( + f"[{self.name}] Service call future cancelled." + ) + else: + self.logger.warn( + f"[{self.name}] Failed to cancel service call future (may have already completed or cannot be cancelled)." + ) + self.service_future = None # Abandon the future + self.call_sent_time = None + self.blackboard_set("calibration_call_succeeded", False) + self.blackboard_set("calibration_message", "Service call timed out") + return Status.FAILURE + + self.logger.debug( + f"[{self.name}] Waiting for calibration service response..." + ) + return Status.RUNNING + + # --- State 3: Processing Response --- + elif self.service_future: # Future exists and is done + self.logger.debug(f"[{self.name}] Calibration service call future done.") + response_payload = None + final_status = Status.FAILURE # Default to failure + try: + response_payload = self.service_future.result() + self.blackboard_set("calibration_call_succeeded", True) + + if response_payload is not None: + self.blackboard_set( + "calibration_routine_success", response_payload.success + ) + self.blackboard_set("calibration_message", response_payload.message) + self.blackboard_set( + "computed_calibration_offset_quat", + response_payload.computed_offset_jacobase_to_filterworld, + ) + + self.logger.info( + f"[{self.name}] Calibration service response: success={response_payload.success}, msg='{response_payload.message}'" + ) + if response_payload.success: + self.logger.info( + f" Computed Offset (xyzw): [" + f"{response_payload.computed_offset_jacobase_to_filterworld.x:.4f}, " + f"{response_payload.computed_offset_jacobase_to_filterworld.y:.4f}, " + f"{response_payload.computed_offset_jacobase_to_filterworld.z:.4f}, " + f"{response_payload.computed_offset_jacobase_to_filterworld.w:.4f}]" + ) + final_status = ( + Status.SUCCESS if response_payload.success else Status.FAILURE + ) + else: + self.logger.error( + f"[{self.name}] Service call future done but result is None." + ) + self.blackboard_set("calibration_routine_success", False) + self.blackboard_set( + "calibration_message", "Service call returned None result" + ) + except Exception as e: + self.logger.error( + f"[{self.name}] Service call failed with exception: {e}" + ) + self.blackboard_set("calibration_call_succeeded", False) + self.blackboard_set("calibration_routine_success", False) + self.blackboard_set("calibration_message", f"Exception: {e}") + + self.service_future = None # Reset for next call + self.call_sent_time = None + return final_status + + # Should not be reached + self.logger.warn(f"[{self.name}] Reached unexpected state in update method.") + return Status.FAILURE + + def terminate(self, new_status: Status) -> None: + """Log termination status and clear future if needed.""" + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") + if self.service_future is not None and not self.service_future.done(): + self.logger.warning( + f"[{self.name}] Service call terminated while waiting for response." + ) + # Note: ROS 2 futures are not easily cancellable once sent if the service doesn't support it. + # We just abandon it here. + self.service_future = None + self.call_sent_time = None From 1db6413fa7dd2625db6ef917fd3e5d1e5e43f636 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 24 May 2025 11:34:08 -0700 Subject: [PATCH 089/238] Minor fixes --- .../articutool/call_set_orientation_control.py | 10 +++++----- .../articutool/trigger_articutool_calibration.py | 6 ++++++ 2 files changed, 11 insertions(+), 5 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py index ffedd147..372d458e 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py @@ -169,11 +169,11 @@ def update(self) -> Status: # Populate fields based on mode if req.control_mode == SetOrientationControl.Request.MODE_LEVELING: - pitch_deg = self.blackboard_try_get( - "pitch_offset_deg", 0.0 + pitch_deg = self.blackboard_get( + "pitch_offset_deg" ) # Default to 0 if not found - roll_deg = self.blackboard_try_get( - "roll_offset_deg", 0.0 + roll_deg = self.blackboard_get( + "roll_offset_deg" ) # Default to 0 if not found req.pitch_offset = float(math.radians(pitch_deg)) req.roll_offset = float(math.radians(roll_deg)) @@ -189,7 +189,7 @@ def update(self) -> Status: req.control_mode == SetOrientationControl.Request.MODE_FULL_ORIENTATION ): - target_orient_input = self.blackboard_try_get( + target_orient_input = self.blackboard_get( "target_orientation_robot_base_quat" ) if target_orient_input is None: diff --git a/ada_feeding/ada_feeding/behaviors/articutool/trigger_articutool_calibration.py b/ada_feeding/ada_feeding/behaviors/articutool/trigger_articutool_calibration.py index 6597fa0a..c48c9c6a 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/trigger_articutool_calibration.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/trigger_articutool_calibration.py @@ -128,6 +128,12 @@ def setup(self, **kwargs): f"[{self.name}] Failed to create service client during setup: {e}" ) + @override + def initialise(self) -> None: + self.service_future = None # Reset for a new call + self.call_sent_time = None + self.logger.debug(f"[{self.name}] Initializing service call state.") + def update(self) -> Status: """Manage the asynchronous service call state machine.""" if not self._initialized_properly or self.client is None: From 8ac9bf5e0d239fb5112c2732da331e95960cccda Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 24 May 2025 11:34:46 -0700 Subject: [PATCH 090/238] Enable MODE_FULL_ORIENATION control for Articutool before moving into food --- .../ada_feeding/trees/acquire_food_tree.py | 82 ++++++++----------- 1 file changed, 34 insertions(+), 48 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index bb61914f..443fd54d 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -66,6 +66,7 @@ CallSetOrientationControl, SwitchArticutoolControllers, ComputeArticutoolLevelingJoints, + TriggerArticutoolCalibration, ) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( @@ -1331,54 +1332,39 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: # Starts a new Sequence w/ Memory internally workers=[ ### Move Into Food - # SwitchArticutoolControllers( - # name="SwitchArticutoolToVelocity", - # ns=name, - # inputs={ - # "controllers_to_activate": [ - # "velocity_controller" - # ], - # "controllers_to_deactivate": [ - # "joint_trajectory_controller" - # ], - # }, - # outputs={ - # "switch_call_succeeded": None, - # "switch_response_ok": None, - # }, - # ), - # ExtractPoseComponents( - # name="GetMoveIntoOrientation", - # ns=name, - # inputs={ - # "input_pose_object": BlackboardKey( - # "move_into_pose_stamped_base_frame" - # ), - # }, - # outputs={ - # "output_position": BlackboardKey( - # "move_into_position" - # ), - # "output_orientation": BlackboardKey( - # "move_into_orientation" - # ), - # "output_header": BlackboardKey( - # "move_into_header" - # ), - # "success": None, - # }, - # ), - # CallSetOrientationControl( - # name="SetArticutoolOrientation", - # ns=name, - # inputs={ - # "enable": True, - # "quat_xyzw": BlackboardKey( - # "move_into_orientation" - # ), - # }, - # outputs={}, - # ), + SwitchArticutoolControllers( + name="SwitchArticutoolToVelocity", + ns=name, + inputs={ + "controllers_to_activate": [ + "velocity_controller" + ], + "controllers_to_deactivate": [ + "joint_trajectory_controller" + ], + }, + outputs={ + "switch_call_succeeded": None, + "switch_response_ok": None, + }, + ), + TriggerArticutoolCalibration( + name="TriggerArticutoolCalibration", + ns=name, + inputs={}, + outputs={}, + ), + CallSetOrientationControl( + name="SetArticutoolOrientation", + ns=name, + inputs={ + "control_mode": 2, + "target_orientation_robot_base_quat": BlackboardKey( + "move_into_tool_tip_orientation" + ), + }, + outputs={}, + ), # MoveInto expect F/T failure py_trees.decorators.FailureIsSuccess( name="MoveIntoJacoArmExecuteSucceed", From 82529194bd97266b5971b1fa549d8a255e72c7b1 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 24 May 2025 12:24:06 -0700 Subject: [PATCH 091/238] Enable MODE_LEVELING after achieving level Articutool orientation --- .../ada_feeding/trees/acquire_food_tree.py | 36 +++++++++++-------- 1 file changed, 22 insertions(+), 14 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 443fd54d..27db305b 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -339,20 +339,28 @@ def create_tree( "action_status": BlackboardKey("tool_action_status"), }, ), - # SwitchArticutoolControllers( - # name="SwitchArticutoolToVelocity", - # ns=name, - # inputs={ - # "controllers_to_activate": ["velocity_controller"], - # "controllers_to_deactivate": [ - # "joint_trajectory_controller" - # ], - # }, - # outputs={ - # "switch_call_succeeded": None, - # "switch_response_ok": None, - # }, - # ), + SwitchArticutoolControllers( + name="SwitchArticutoolToVelocity", + ns=name, + inputs={ + "controllers_to_activate": ["velocity_controller"], + "controllers_to_deactivate": [ + "joint_trajectory_controller" + ], + }, + outputs={ + "switch_call_succeeded": None, + "switch_response_ok": None, + }, + ), + CallSetOrientationControl( + name="SetArticutoolOrientation", + ns=name, + inputs={ + "control_mode": 1, + }, + outputs={}, + ), MoveIt2JointConstraint( name="RestingConstraint", ns=name, From d4e7b669909225a332e08927017c4a2caf316f14 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 27 May 2025 18:49:19 -0700 Subject: [PATCH 092/238] Abstract post-acquisition Articutool self-leveling into separate sequence --- .../ada_feeding/trees/acquire_food_tree.py | 293 +++++++++--------- 1 file changed, 149 insertions(+), 144 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 27db305b..7a25387f 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -217,150 +217,6 @@ def create_tree( # Default fail if service is down wait_for_server_timeout_sec=0.0, ), - GetJointStates( - name="GetJacoArmStateForLeveling", - ns=name, - node=self._node, - inputs={ - "joint_names": [ - "j2n6s200_joint_1", - "j2n6s200_joint_2", - "j2n6s200_joint_3", - "j2n6s200_joint_4", - "j2n6s200_joint_5", - "j2n6s200_joint_6", - ], - }, - outputs={ - "joint_state": BlackboardKey( - "current_jaco_arm_state_for_leveling_fk" - ), - "joint_positions": None, - "joint_names": None, - }, - ), - MoveIt2ComputeFK( - name="GetJacoEEPoseForLeveling", - ns=name, - inputs={ - "group_name": "jaco_arm", - "joint_state": BlackboardKey( - "current_jaco_arm_state_for_leveling_fk" - ), - "fk_link_names": ["j2n6s200_end_effector"], - }, - outputs={ - "fk_poses": BlackboardKey("current_jaco_arm_fk_poses"), - "success": None, - }, - ), - ExtractPoseFromPosesByLink( - name="ExtractJacoEEPoseForLeveling", - ns=name, - inputs={ - "fk_poses": BlackboardKey("current_jaco_arm_fk_poses"), - "target_link_name": "j2n6s200_end_effector", - "requested_link_names": ["j2n6s200_end_effector"], - }, - outputs={ - "extracted_pose": BlackboardKey( - "current_jaco_ee_world_pose_stamped" - ), - "success": None, - }, - ), - ComputeArticutoolLevelingJoints( - name="ComputeLevelingAngles", - ns=name, - inputs={ - "jaco_ee_world_pose": BlackboardKey( - "current_jaco_ee_world_pose_stamped" - ), - }, - outputs={ - "articutool_joint_positions": BlackboardKey( - "articutool_joint_positions" - ), - "articutool_leveling_ik_found": BlackboardKey( - "leveling_ik_success" - ), - }, - ), - MoveIt2JointConstraint( - name="SetLevelingJointGoal", - ns=name, - inputs={ - "joint_positions": BlackboardKey( - "articutool_joint_positions" - ), - }, - outputs={ - "constraints": BlackboardKey( - "articutool_leveling_constraints" - ) - }, - ), - py_trees.decorators.Timeout( - name="PlanToLevelArticutoolTimeout", - # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such - duration=10.0 - * self.allowed_planning_time_to_resting_configuration, - child=MoveIt2Plan( - name="PlanToLevelArticutool", - ns=name, - inputs={ - "goal_constraints": BlackboardKey( - "articutool_leveling_constraints" - ), - "group_name": "articutool", - }, - outputs={ - "trajectory": BlackboardKey( - "level_articutool_trajectory" - ) - }, - ), - ), - ExecuteArticutoolTrajectory( - name="LevelArticutool", - ns=name, - inputs={ - "trajectory": BlackboardKey( - "level_articutool_trajectory" - ), - }, - outputs={ - "action_goal_accepted": BlackboardKey( - "tool_goal_accepted" - ), - "action_result_code": BlackboardKey( - "tool_exec_result_code" - ), - "action_status": BlackboardKey("tool_action_status"), - }, - ), - SwitchArticutoolControllers( - name="SwitchArticutoolToVelocity", - ns=name, - inputs={ - "controllers_to_activate": ["velocity_controller"], - "controllers_to_deactivate": [ - "joint_trajectory_controller" - ], - }, - outputs={ - "switch_call_succeeded": None, - "switch_response_ok": None, - }, - ), - CallSetOrientationControl( - name="SetArticutoolOrientation", - ns=name, - inputs={ - "control_mode": 1, - }, - outputs={}, - ), MoveIt2JointConstraint( name="RestingConstraint", ns=name, @@ -545,6 +401,154 @@ def create_tree( ], # End RecoverySequence.children ) # End RecoverySequence + def post_acquisition_sequence() -> py_trees.behaviour.Behaviour: + return py_trees.composites.Sequence( + name="PreAcquisitionSequence", + memory=True, + children=[ + GetJointStates( + name="GetJacoArmStateForLeveling", + ns=name, + node=self._node, + inputs={ + "joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ], + }, + outputs={ + "joint_state": BlackboardKey( + "current_jaco_arm_state_for_leveling_fk" + ), + "joint_positions": None, + "joint_names": None, + }, + ), + MoveIt2ComputeFK( + name="GetJacoEEPoseForLeveling", + ns=name, + inputs={ + "group_name": "jaco_arm", + "joint_state": BlackboardKey( + "current_jaco_arm_state_for_leveling_fk" + ), + "fk_link_names": ["j2n6s200_end_effector"], + }, + outputs={ + "fk_poses": BlackboardKey("current_jaco_arm_fk_poses"), + "success": None, + }, + ), + ExtractPoseFromPosesByLink( + name="ExtractJacoEEPoseForLeveling", + ns=name, + inputs={ + "fk_poses": BlackboardKey("current_jaco_arm_fk_poses"), + "target_link_name": "j2n6s200_end_effector", + "requested_link_names": ["j2n6s200_end_effector"], + }, + outputs={ + "extracted_pose": BlackboardKey( + "current_jaco_ee_world_pose_stamped" + ), + "success": None, + }, + ), + ComputeArticutoolLevelingJoints( + name="ComputeLevelingAngles", + ns=name, + inputs={ + "jaco_ee_world_pose": BlackboardKey( + "current_jaco_ee_world_pose_stamped" + ), + }, + outputs={ + "articutool_joint_positions": BlackboardKey( + "articutool_joint_positions" + ), + "articutool_leveling_ik_found": BlackboardKey( + "leveling_ik_success" + ), + }, + ), + MoveIt2JointConstraint( + name="SetLevelingJointGoal", + ns=name, + inputs={ + "joint_positions": BlackboardKey( + "articutool_joint_positions" + ), + }, + outputs={ + "constraints": BlackboardKey( + "articutool_leveling_constraints" + ) + }, + ), + py_trees.decorators.Timeout( + name="PlanToLevelArticutoolTimeout", + # Increase allowed_planning_time to account for ROS2 overhead and MoveIt2 setup and such + duration=10.0 + * self.allowed_planning_time_to_resting_configuration, + child=MoveIt2Plan( + name="PlanToLevelArticutool", + ns=name, + inputs={ + "goal_constraints": BlackboardKey( + "articutool_leveling_constraints" + ), + "group_name": "articutool", + }, + outputs={ + "trajectory": BlackboardKey( + "level_articutool_trajectory" + ) + }, + ), + ), + ExecuteArticutoolTrajectory( + name="LevelArticutool", + ns=name, + inputs={ + "trajectory": BlackboardKey("level_articutool_trajectory"), + }, + outputs={ + "action_goal_accepted": BlackboardKey("tool_goal_accepted"), + "action_result_code": BlackboardKey( + "tool_exec_result_code" + ), + "action_status": BlackboardKey("tool_action_status"), + }, + ), + SwitchArticutoolControllers( + name="SwitchArticutoolToVelocity", + ns=name, + inputs={ + "controllers_to_activate": ["velocity_controller"], + "controllers_to_deactivate": [ + "joint_trajectory_controller" + ], + }, + outputs={ + "switch_call_succeeded": None, + "switch_response_ok": None, + }, + ), + CallSetOrientationControl( + name="SetArticutoolOrientation", + ns=name, + inputs={ + "control_mode": 1, + }, + outputs={}, + ), + ], + ) + def pre_acquisition_sequence( flip_food_frame: bool = False, action: Optional[BlackboardKey] = None, @@ -1591,6 +1595,7 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: ), # End MoveIt2Servo ], # End SafeFTPreempt.workers ), # End SafeFTPreempt + post_acquisition_sequence(), ], # End OctomapAndTableCollision.workers ), # OctomapAndTableCollision ] From 1dd8cbe1a8cc69dc1babebd8bf5ac9badba3092a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 27 May 2025 18:51:47 -0700 Subject: [PATCH 093/238] Explicitly disable orientation control post-acquisition to ensure clean transition from MODE_FULL_ORIENATION to MODE_LEVELING --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 7a25387f..39b8e40d 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -406,6 +406,14 @@ def post_acquisition_sequence() -> py_trees.behaviour.Behaviour: name="PreAcquisitionSequence", memory=True, children=[ + CallSetOrientationControl( + name="DisableArticutoolOrientation", + ns=name, + inputs={ + "control_mode": 0, + }, + outputs={}, + ), GetJointStates( name="GetJacoArmStateForLeveling", ns=name, From 8e86fee566b8ca374dc836a50e8b76210daa2396 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 27 May 2025 19:15:23 -0700 Subject: [PATCH 094/238] Execute MoveAbove trajectory for Articutool first, then Jaco arm --- .../ada_feeding/trees/acquire_food_tree.py | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 39b8e40d..1339db6b 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1265,17 +1265,6 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: ), move_above_sequence(), move_into_sequence(), - MoveIt2Execute( - name="MoveAboveJacoArm", - ns=name, - inputs={ - "trajectory": BlackboardKey( - "move_above_jaco_arm_trajectory" - ), - "group_name": "jaco_arm", - }, - outputs={}, - ), ExecuteArticutoolTrajectory( name="MoveAboveArticutool", ns=name, @@ -1296,6 +1285,17 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: ), }, ), + MoveIt2Execute( + name="MoveAboveJacoArm", + ns=name, + inputs={ + "trajectory": BlackboardKey( + "move_above_jaco_arm_trajectory" + ), + "group_name": "jaco_arm", + }, + outputs={}, + ), # If Anything goes wrong, reset FT to safe levels scoped_behavior( name="SafeFTPreempt", From 95021071af7d7e69f1fd5e658e0f0b3345a00025 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 29 May 2025 12:31:07 -0700 Subject: [PATCH 095/238] Update default service name for setting orientation control mode --- .../behaviors/articutool/call_set_orientation_control.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py index 372d458e..3c456a59 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/call_set_orientation_control.py @@ -38,7 +38,7 @@ class CallSetOrientationControl(BlackboardBehavior): Returns RUNNING while waiting for the service response. """ - DEFAULT_SERVICE_NAME = "/articutool/set_orientation_control" + DEFAULT_SERVICE_NAME = "/orientation_control/set_orientation_control_mode" DEFAULT_WAIT_TIMEOUT_SEC = 1.0 def blackboard_inputs( From 51a740ef6ec7f0061b15d3ee9b1ebafe5f4854ae Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 29 May 2025 12:31:36 -0700 Subject: [PATCH 096/238] Disable octomap --- ada_planning_scene/launch/ada_moveit_launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ada_planning_scene/launch/ada_moveit_launch.xml b/ada_planning_scene/launch/ada_moveit_launch.xml index d436250c..7de04355 100644 --- a/ada_planning_scene/launch/ada_moveit_launch.xml +++ b/ada_planning_scene/launch/ada_moveit_launch.xml @@ -18,6 +18,7 @@ + From 32a8eb71ede8de6ef2d7dbae1cb952fb059e367b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 1 Jun 2025 14:24:59 -0700 Subject: [PATCH 097/238] Disable ADA specific configurations --- start.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/start.py b/start.py index b0eb8662..e28f5f65 100755 --- a/start.py +++ b/start.py @@ -153,7 +153,7 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "`./src/feeding_web_interface/feedingwebapp`)." ) else: - print(f"# Terminating the ada_feeding demo in **{ args.sim}**") + print(f"# Terminating the ada_feeding demo in **{args.sim}**") print( "################################################################################" ) @@ -283,12 +283,12 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "ros2 launch ada_feeding_perception ada_feeding_perception.launch.py combine_perception_nodes:=true", ], "moveit": [ - "Xvfb :5 -screen 0 800x600x24 &" if not args.dev else "", - "export DISPLAY=:5" if not args.dev else "", + # "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'}", ], "feeding": [ - "sudo ./src/ada_feeding/configure_lovelace.sh", + # "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}" From dfdabae998adc992c83499539fb927d255f6e941 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 1 Jun 2025 15:09:54 -0700 Subject: [PATCH 098/238] Remove ft screen sessions since this F/T data reading and publishing will be handled by the Articutool instead --- start.py | 9 --------- 1 file changed, 9 deletions(-) diff --git a/start.py b/start.py index e28f5f65..4d16d05a 100755 --- a/start.py +++ b/start.py @@ -169,9 +169,6 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "cd ./src/feeding_web_interface/feedingwebapp", "node --env-file=.env server.js", ], - "ft": [ - "ros2 run ada_feeding dummy_ft_sensor.py", - ], "camera": [ ( "ros2 launch feeding_web_app_ros2_test feeding_web_app_dummy_nodes_launch.xml " @@ -226,9 +223,6 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "cd ./src/feeding_web_interface/feedingwebapp", "node --env-file=.env server.js", ], - "ft": [ - "ros2 run ada_feeding dummy_ft_sensor.py", - ], "perception": [ ( "ros2 launch feeding_web_app_ros2_test feeding_web_app_dummy_nodes_launch.xml " @@ -273,9 +267,6 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "camera": [ "ssh nano@nano -t './start_nano.sh'", ], - "ft": [ - "ros2 run forque_sensor_hardware forque_sensor_hardware --ros-args -p host:=ft-sensor-2", - ], "rosbridge": [ "ros2 launch rosbridge_server rosbridge_websocket_launch.xml", ], From 9861670a41e041f9542c0fbc6396e169b6a6cc7a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 1 Jun 2025 15:20:50 -0700 Subject: [PATCH 099/238] Update F/T topics --- ada_feeding/launch/ada_feeding_launch.xml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ada_feeding/launch/ada_feeding_launch.xml b/ada_feeding/launch/ada_feeding_launch.xml index 6e89a401..57363a26 100644 --- a/ada_feeding/launch/ada_feeding_launch.xml +++ b/ada_feeding/launch/ada_feeding_launch.xml @@ -19,7 +19,7 @@ - + @@ -28,8 +28,8 @@ - - + + From a84cb2423df784541d00f7e85bd783cd930dac60 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 1 Jun 2025 19:44:16 -0700 Subject: [PATCH 100/238] Compute leveling joints for the Articutool at the end of the MoveInto motion as an indicator that the leveling motion will succeed after the subsequent Grasp and Extract twist. Since the Grasp and Extract are purely Cartesian motions, this should work, but in the future we might also consider a way to compute where the Jaco EE frame will end up after these twists but before actually executing them. --- .../ada_feeding/trees/acquire_food_tree.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 1339db6b..f019f4a7 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1063,6 +1063,21 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: "success": None, }, ), + ComputeArticutoolLevelingJoints( + name="ComputeLevelingAngles", + ns=name, + inputs={ + "jaco_ee_world_pose": BlackboardKey( + "move_into_jaco_arm_ee_pose" + ), + }, + outputs={ + "articutool_joint_positions": None, + "articutool_leveling_ik_found": BlackboardKey( + "move_into_leveling_ik_success" + ), + }, + ), MoveIt2PoseConstraint( name="MoveIntoJacoArmEEPoseConstraint", ns=name, From 854a41e2acf287ea112e647d3856d17e5f36f734 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 1 Jun 2025 21:34:20 -0700 Subject: [PATCH 101/238] Add retries for move above and into planning sequences --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index f019f4a7..f558cb53 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1278,8 +1278,18 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: ), ], ), - move_above_sequence(), - move_into_sequence(), + py_trees.decorators.Retry( + name="PlanAcquisitionSequenceRetry", + num_failures=3, + child=py_trees.composites.Sequence( + name="PlanAcquisitionSequence", + memory=True, + children=[ + move_above_sequence(), + move_into_sequence(), + ], + ), + ), ExecuteArticutoolTrajectory( name="MoveAboveArticutool", ns=name, From 8001230af8244edab5ed3c801440ccc1ae750487 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 1 Jun 2025 22:20:09 -0700 Subject: [PATCH 102/238] Refactor(ExecuteArticutoolTrajectory): Simplify and improve robustness This commit refactors the ExecuteArticutoolTrajectory behavior: - Removes internal controller switching logic. This responsibility is now delegated to the dedicated SwitchArticutoolControllers behavior, simplifying ExecuteArticutoolTrajectory to focus solely on action execution. - Enhances startup robustness by deferring action server availability checks from `setup()` to a non-blocking `CHECKING_DEPENDENCIES` state in `update()`. The behavior will now wait for the action server to become available if it's not ready when the behavior first ticks. - Corrects QoS profile usage for ROS 2 Humble by explicitly defining profiles for the action client's services and subscriptions, resolving a `qos_profile_action_default` import error. - Fixes an `AttributeError` by storing and using the resolved action server name string for logging, as `ActionClient` does not have an `action_name` attribute. --- .../execute_articutool_trajectory.py | 403 ++++++++---------- .../ada_feeding/trees/acquire_food_tree.py | 28 ++ 2 files changed, 209 insertions(+), 222 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py b/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py index 480c4ac4..74c1f0e4 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/execute_articutool_trajectory.py @@ -1,13 +1,12 @@ # -*- coding: utf-8 -*- # Copyright (c) 2025, Personal Robotics Laboratory # License: BSD 3-Clause. See LICENSE.md file in root directory. - # (Add appropriate Copyright/License if desired) """ Defines the ExecuteArticutoolTrajectory behavior, which sends a planned -trajectory to the Articutool's FollowJointTrajectory action server, -handling controller switching. +trajectory to the Articutool's FollowJointTrajectory action server. +It assumes controllers are already appropriately switched by another behavior. """ # Standard imports @@ -18,15 +17,19 @@ import rclpy from rclpy.action import ActionClient from rclpy.action.client import ClientGoalHandle, GoalStatus -from rclpy.node import Node -from rclpy.executors import Future # For type hints +from rclpy.node import Node as RclpyNode # Alias for clarity +from rclpy.executors import Future +from rclpy.qos import ( + qos_profile_services_default, + QoSProfile, + ReliabilityPolicy, + DurabilityPolicy, + HistoryPolicy, +) from control_msgs.action import FollowJointTrajectory from trajectory_msgs.msg import JointTrajectory # Input type -# from builtin_interfaces.msg import Duration # Used internally by action goal -# from sensor_msgs.msg import JointState # Not directly needed - from overrides import override import py_trees import py_trees.blackboard @@ -36,20 +39,15 @@ from ada_feeding.helpers import BlackboardKey from ada_feeding.behaviors import BlackboardBehavior -# Assuming ControllerSwitcher is importable and works with a passed node -from articutool_control.controller_switcher import ControllerSwitcher - -# Define constants for action status reporting (optional but clearer) +# Define constants for action status reporting class ActionExecutionStatus(Enum): IDLE = "IDLE" - SWITCHING_CONTROLLERS = "SWITCHING_CONTROLLERS" + CHECKING_DEPENDENCIES = "CHECKING_DEPENDENCIES" SENDING_GOAL = "SENDING_GOAL" WAITING_FOR_ACCEPTANCE = "WAITING_FOR_ACCEPTANCE" EXECUTING = "EXECUTING" - WAITING_FOR_RESULT = ( - "WAITING_FOR_RESULT" # If execution status isn't monitored closely - ) + WAITING_FOR_RESULT = "WAITING_FOR_RESULT" GOAL_REJECTED = "GOAL_REJECTED" GOAL_CANCELLED = "GOAL_CANCELLED" SUCCEEDED = "SUCCEEDED" @@ -60,8 +58,12 @@ class ActionExecutionStatus(Enum): class ExecuteArticutoolTrajectory(BlackboardBehavior): """ Executes a JointTrajectory on the Articutool using the - FollowJointTrajectory action server. Handles controller switching before - sending the goal. Returns RUNNING while executing, SUCCESS on completion, + FollowJointTrajectory action server. This behavior assumes that the + necessary controllers (e.g., 'joint_trajectory_controller') have already + been activated by a separate behavior (like SwitchArticutoolControllers) + prior to this behavior being ticked. + + Returns RUNNING while executing, SUCCESS on completion, FAILURE on error, rejection, or abortion. """ @@ -69,30 +71,26 @@ class ExecuteArticutoolTrajectory(BlackboardBehavior): DEFAULT_ACTION_SERVER = ( "/articutool/joint_trajectory_controller/follow_joint_trajectory" ) - DEFAULT_CONTROLLER_TO_ACTIVATE = ["joint_trajectory_controller"] # Expects list - DEFAULT_CONTROLLERS_TO_DEACTIVATE = ["velocity_controller"] # Expects list def __init__(self, name: str, ns: str = "/", **kwargs): - # Pass kwargs to parent if BlackboardBehavior supports it super().__init__(name=name, ns=ns, **kwargs) # Internal state variables self._action_client: Optional[ActionClient] = None - self.controller_switcher: Optional[ControllerSwitcher] = None self._send_goal_future: Optional[Future] = None self._get_result_future: Optional[Future] = None self._goal_handle: Optional[ClientGoalHandle] = None self._current_status: ActionExecutionStatus = ActionExecutionStatus.IDLE self._result_code: Optional[int] = None # Store the final result code + self._action_client_initialized: bool = False + self.node: Optional[RclpyNode] = None # Store the node instance + self._action_server_name_str: str = ( + "" # To store the resolved action server name for logging + ) + def blackboard_inputs( self, trajectory: Union[BlackboardKey, JointTrajectory], - controllers_to_activate: Union[ - BlackboardKey, List[str] - ] = DEFAULT_CONTROLLER_TO_ACTIVATE, - controllers_to_deactivate: Union[ - BlackboardKey, List[str] - ] = DEFAULT_CONTROLLERS_TO_DEACTIVATE, action_server_name: Union[BlackboardKey, str] = DEFAULT_ACTION_SERVER, ) -> None: """ @@ -101,8 +99,6 @@ def blackboard_inputs( Parameters ---------- trajectory: The trajectory_msgs/JointTrajectory message to execute. - controllers_to_activate: List of controller names to activate before sending. - controllers_to_deactivate: List of controller names to deactivate before sending. action_server_name: Name of the FollowJointTrajectory action server. """ super().blackboard_inputs( @@ -134,57 +130,59 @@ def blackboard_outputs( @override def setup(self, **kwargs): - """Get node, create action client and controller switcher.""" - # pylint: disable=attribute-defined-outside-init + """Get node and create action client.""" try: - self.node: Node = kwargs["node"] + self.node = kwargs["node"] except KeyError as e: self.logger.error( f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" ) - return # Cannot function without node + return # Critical failure try: - try: - action_server_name = self.blackboard_get("action_server_name") - if not isinstance(action_server_name, str) or not action_server_name: - self.logger.warning( - f"[{self.name}] Blackboard 'action_server_name' invalid, using default." - ) - action_server_name = self.DEFAULT_ACTION_SERVER - except KeyError: - self.logger.info( - f"[{self.name}] Input 'action_server_name' not found, using default: {self.DEFAULT_ACTION_SERVER}" - ) - action_server_name = self.DEFAULT_ACTION_SERVER - - self._action_client = ActionClient( - self.node, FollowJointTrajectory, action_server_name - ) - # Check server availability briefly during setup - timeout_sec = 1.0 - if not self._action_client.wait_for_server(timeout_sec=timeout_sec): - self.logger.error( - f"[{self.name}] Action server '{action_server_name}' not available after {timeout_sec}s during setup." + action_server_name_bb = self.blackboard_get("action_server_name") + if not isinstance(action_server_name_bb, str) or not action_server_name_bb: + self.logger.warning( + f"[{self.name}] Blackboard 'action_server_name' invalid or not found, using default: {self.DEFAULT_ACTION_SERVER}" ) - self._action_client = None # Mark as unavailable - return # Setup fails - - # Instantiate Controller Switcher (pass node if its __init__ requires it) - try: - # Assuming ControllerSwitcher might take node as arg - self.controller_switcher = ControllerSwitcher(node=self.node) - except TypeError: # If ControllerSwitcher() doesn't take node - self.controller_switcher = ControllerSwitcher() - + self._action_server_name_str = self.DEFAULT_ACTION_SERVER + else: + self._action_server_name_str = action_server_name_bb + except KeyError: self.logger.info( - f"[{self.name}] Setup complete. Action client created for '{action_server_name}'." + f"[{self.name}] Input 'action_server_name' not found on blackboard, using default: {self.DEFAULT_ACTION_SERVER}" ) + self._action_server_name_str = self.DEFAULT_ACTION_SERVER + + # Define QoS profiles for action client components + service_qos_profile = qos_profile_services_default + feedback_qos_profile = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + history=HistoryPolicy.KEEP_LAST, + depth=10, + ) + status_qos_profile = QoSProfile( + reliability=ReliabilityPolicy.RELIABLE, + durability=DurabilityPolicy.TRANSIENT_LOCAL, + history=HistoryPolicy.KEEP_LAST, + depth=1, + ) - except Exception as e: - self.logger.error(f"[{self.name}] Failed during setup: {e}") - self._action_client = None - self.controller_switcher = None + self._action_client = ActionClient( + self.node, + FollowJointTrajectory, + self._action_server_name_str, # Use stored name + goal_service_qos_profile=service_qos_profile, + result_service_qos_profile=service_qos_profile, + cancel_service_qos_profile=service_qos_profile, + feedback_sub_qos_profile=feedback_qos_profile, + status_sub_qos_profile=status_qos_profile, + ) + self._action_client_initialized = True + self.logger.info( + f"[{self.name}] Action client created for '{self._action_server_name_str}' (readiness check deferred to update)." + ) + self.logger.info(f"[{self.name}] Setup method complete.") @override def initialise(self) -> None: @@ -193,121 +191,104 @@ def initialise(self) -> None: self._send_goal_future = None self._get_result_future = None self._goal_handle = None - self._current_status = ActionExecutionStatus.IDLE - self._result_code = None # Reset result code + self._result_code = None + + if self._action_client_initialized: + self._current_status = ActionExecutionStatus.CHECKING_DEPENDENCIES + else: + self.logger.error( + f"[{self.name}] Action client was not initialized during setup. Failing." + ) + self._current_status = ActionExecutionStatus.FAILED + # Clear blackboard outputs self.blackboard_set("action_goal_accepted", False) - self.blackboard_set( - "action_result_code", self._result_code - ) # Set to None or initial value + self.blackboard_set("action_result_code", self._result_code) + self.blackboard_set("action_status", self._current_status.value) + + def _check_dependencies(self) -> Status: + """Helper to check and wait for the action server.""" + if not self._action_client_initialized: + self.logger.error( + f"[{self.name}] Action client instance not created. Critical setup failure." + ) + return self._handle_failure( + "Action client not initialized", ActionExecutionStatus.FAILED + ) + + if not self._action_client.wait_for_server( + timeout_sec=0.01 + ): # Non-blocking check + self.logger.info( + f"[{self.name}] Waiting for action server '{self._action_server_name_str}'..." # Use stored name + ) + return Status.RUNNING + + self.logger.info( + f"[{self.name}] Action server '{self._action_server_name_str}' is ready." + ) # Use stored name + self._current_status = ActionExecutionStatus.IDLE self.blackboard_set("action_status", self._current_status.value) + return Status.RUNNING @override def update(self) -> Status: """Manage the action client state machine.""" - # Update blackboard status at start of tick self.blackboard_set("action_status", self._current_status.value) - if self._action_client is None or self.controller_switcher is None: - self.logger.error( - f"[{self.name}] Action client or controller switcher not available. Setup failed?" - ) - return Status.FAILURE # Setup must have failed + if self._current_status == ActionExecutionStatus.FAILED: + return Status.FAILURE + + if self._current_status == ActionExecutionStatus.CHECKING_DEPENDENCIES: + return self._check_dependencies() - # --- State 1: IDLE -> Try sending goal --- if self._current_status == ActionExecutionStatus.IDLE: try: trajectory: JointTrajectory = self.blackboard_get("trajectory") - controllers_to_activate: List[str] = self.blackboard_get( - "controllers_to_activate" - ) - controllers_to_deactivate: List[str] = self.blackboard_get( - "controllers_to_deactivate" - ) - - # Validate inputs if not isinstance(trajectory, JointTrajectory): self.logger.error( f"[{self.name}] Input 'trajectory' is not a JointTrajectory message." ) - return Status.FAILURE + return self._handle_failure("Invalid trajectory input type") if not trajectory.points: self.logger.warning( f"[{self.name}] Input 'trajectory' has no points. Succeeding trivially." ) - self._current_status = ActionExecutionStatus.SUCCEEDED - self.blackboard_set("action_status", self._current_status.value) - self.blackboard_set( - "action_result_code", FollowJointTrajectory.Result.SUCCESSFUL - ) - return Status.SUCCESS - if not isinstance(controllers_to_activate, list) or not isinstance( - controllers_to_deactivate, list - ): - self.logger.error(f"[{self.name}] Controller inputs must be lists.") - return Status.FAILURE - - # 1a. Switch Controllers - self._current_status = ActionExecutionStatus.SWITCHING_CONTROLLERS - self.blackboard_set("action_status", self._current_status.value) - self.logger.info( - f"[{self.name}] Requesting controller switch: start={controllers_to_activate}, stop={controllers_to_deactivate}" - ) - try: - # Assuming switch_controllers blocks or returns success/failure quickly - self.controller_switcher.switch_controllers( - activate_controllers=controllers_to_activate, - deactivate_controllers=controllers_to_deactivate, + return self._handle_success( + result_code=FollowJointTrajectory.Result.SUCCESSFUL ) - except Exception as sw_e: - self.logger.error( - f"[{self.name}] Error during controller switch: {sw_e}" - ) - self._current_status = ActionExecutionStatus.FAILED - self.blackboard_set("action_status", self._current_status.value) - return Status.FAILURE - # 1b. Construct Goal Message goal_msg = FollowJointTrajectory.Goal() - # Important: Make sure the trajectory has correct joint names for the action server goal_msg.trajectory = trajectory - # Optional: Add goal time tolerance, path tolerance if needed - # goal_msg.goal_time_tolerance = Duration(sec=1, nanosec=0).to_msg() - - # 1c. Send Goal Asynchronously self.logger.info( - f"[{self.name}] Sending trajectory goal to action server..." + f"[{self.name}] Sending trajectory goal to action server '{self._action_server_name_str}'..." # Use stored name ) self._send_goal_future = self._action_client.send_goal_async(goal_msg) self._current_status = ActionExecutionStatus.SENDING_GOAL self.blackboard_set("action_status", self._current_status.value) return Status.RUNNING - except KeyError as e: - self.logger.error(f"[{self.name}] Blackboard key error: {e}") - return Status.FAILURE + return self._handle_failure(f"Blackboard key error: {e}") except Exception as e: - self.logger.error(f"[{self.name}] Error preparing to send goal: {e}") - return Status.FAILURE + return self._handle_failure(f"Error preparing to send goal: {e}") - # --- State 2: Waiting for Goal Acceptance --- if self._current_status == ActionExecutionStatus.SENDING_GOAL: if self._send_goal_future is None: - return self._handle_invalid_state( - "SENDING_GOAL future None" - ) # Should not happen - + return self._handle_invalid_state("SENDING_GOAL future is None") if self._send_goal_future.done(): try: goal_handle: ClientGoalHandle = self._send_goal_future.result() except Exception as e: + self._send_goal_future = None self.logger.error( - f"[{self.name}] Exception getting goal handle from future: {e}" + f"[{self.name}] Exception waiting for goal handle: {e}. Server might have shut down." ) - return self._handle_failure("Exception getting goal handle") - - self._send_goal_future = None # Clear future - + self._current_status = ActionExecutionStatus.CHECKING_DEPENDENCIES + self.logger.warning( + f"[{self.name}] Re-checking dependencies due to goal send error." + ) + return Status.RUNNING + self._send_goal_future = None if not goal_handle.accepted: self.logger.warning( f"[{self.name}] Goal rejected by action server." @@ -317,37 +298,22 @@ def update(self) -> Status: "Goal rejected", ActionExecutionStatus.GOAL_REJECTED ) else: - # Convert UUID numpy array to hex string for logging try: - # Access the uuid field (numpy array of uint8) - uuid_array = goal_handle.goal_id.uuid - # Convert the numpy array to standard Python bytes - uuid_bytes = bytes(uuid_array) - # Convert bytes to a hex string - uuid_hex = uuid_bytes.hex() - except Exception as log_e: - # Fallback in case something goes wrong with conversion - self.logger.warning( - f"[{self.name}] Could not format goal ID UUID for logging: {log_e}" - ) + uuid_hex = bytes(goal_handle.goal_id.uuid).hex() + except Exception: uuid_hex = "[Error Formatting UUID]" - self.logger.info( f"[{self.name}] Goal accepted by action server (ID: {uuid_hex})." ) self._goal_handle = goal_handle self.blackboard_set("action_goal_accepted", True) self._get_result_future = self._goal_handle.get_result_async() - self._current_status = ( - ActionExecutionStatus.EXECUTING - ) # Or WAITING_FOR_RESULT if preferred + self._current_status = ActionExecutionStatus.EXECUTING self.blackboard_set("action_status", self._current_status.value) return Status.RUNNING else: - # Future not done yet return Status.RUNNING - # --- State 3: Waiting for Result / Monitoring Execution --- if ( self._current_status == ActionExecutionStatus.EXECUTING or self._current_status == ActionExecutionStatus.WAITING_FOR_RESULT @@ -356,29 +322,22 @@ def update(self) -> Status: return self._handle_invalid_state( "EXECUTING/WAITING future or handle None" ) - - # Check goal handle status status = self._goal_handle.status if status == GoalStatus.STATUS_EXECUTING: - self._current_status = ( - ActionExecutionStatus.EXECUTING - ) # Ensure state is correct - # Optional: Process feedback if needed - # self.logger.debug(f"[{self.name}] Goal executing...") + self._current_status = ActionExecutionStatus.EXECUTING elif status == GoalStatus.STATUS_ABORTED: - self.logger.warning(f"[{self.name}] Goal aborted by server.") - # Result future should complete soon, wait for it - self._current_status = ( - ActionExecutionStatus.WAITING_FOR_RESULT - ) # Move to check result + self.logger.warning( + f"[{self.name}] Goal aborted by server (status update)." + ) + self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT elif status == GoalStatus.STATUS_CANCELED: - self.logger.warning(f"[{self.name}] Goal canceled.") + self.logger.warning(f"[{self.name}] Goal canceled (status update).") self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT elif status == GoalStatus.STATUS_SUCCEEDED: - self.logger.info(f"[{self.name}] Goal status reported SUCCEEDED.") + self.logger.info( + f"[{self.name}] Goal status reported SUCCEEDED (status update)." + ) self._current_status = ActionExecutionStatus.WAITING_FOR_RESULT - - # Now check if the result future is done if self._get_result_future.done(): try: result_wrapper = self._get_result_future.result() @@ -389,35 +348,34 @@ def update(self) -> Status: f"[{self.name}] Action result received: Code={self._result_code}, Msg='{result_string}'" ) self.blackboard_set("action_result_code", self._result_code) - - # Final state determination if self._result_code == FollowJointTrajectory.Result.SUCCESSFUL: - return self._handle_success() - elif ( - status == GoalStatus.STATUS_CANCELED - ): # Check status again if needed - return self._handle_failure( - "Goal Canceled", ActionExecutionStatus.GOAL_CANCELLED + return self._handle_success(result_code=self._result_code) + else: + final_status_map = { + GoalStatus.STATUS_ABORTED: ActionExecutionStatus.ABORTED, + GoalStatus.STATUS_CANCELED: ActionExecutionStatus.GOAL_CANCELLED, + } + final_exec_status = final_status_map.get( + status, ActionExecutionStatus.FAILED ) - else: # Any other error code implies failure return self._handle_failure( - f"Action Failed: Code={self._result_code}, Msg='{result_string}'", - ActionExecutionStatus.ABORTED - if status == GoalStatus.STATUS_ABORTED - else ActionExecutionStatus.FAILED, + f"Action Failed by server: Code={self._result_code}, Msg='{result_string}'", + final_exec_status, + result_code=self._result_code, ) - except Exception as e: self.logger.error( f"[{self.name}] Exception getting result from future: {e}" ) - return self._handle_failure("Exception getting result") + self._current_status = ActionExecutionStatus.CHECKING_DEPENDENCIES + self.logger.warning( + f"[{self.name}] Re-checking dependencies due to result error." + ) + return Status.RUNNING else: - # Result future not done yet, goal still active (or transitioning) self.blackboard_set("action_status", self._current_status.value) return Status.RUNNING - # --- Handle Terminal States (Should be reached via returns above) --- if self._current_status == ActionExecutionStatus.SUCCEEDED: return Status.SUCCESS if self._current_status in [ @@ -427,16 +385,11 @@ def update(self) -> Status: ActionExecutionStatus.ABORTED, ]: return Status.FAILURE - - # Fallback if state is somehow invalid return self._handle_invalid_state(f"Unhandled status {self._current_status}") @override def terminate(self, new_status: Status) -> None: - """Cancel active goal if behavior is terminated unexpectedly.""" self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") - # If terminated INTERNALLY (SUCCESS/FAILURE), goal is already finished or failed. - # If terminated EXTERNALLY (INVALID), cancel the goal if it's active. if new_status == Status.INVALID and self._goal_handle is not None: status = self._goal_handle.status if ( @@ -446,24 +399,26 @@ def terminate(self, new_status: Status) -> None: self.logger.warning( f"[{self.name}] Behavior terminated externally while action goal was active (Status: {GoalStatus(status).name}). Requesting cancellation." ) - cancel_future = self._goal_handle.cancel_goal_async() - # Optional: Spin briefly/add callback to ensure cancel sent? Usually not needed. - # else: - # self.logger.debug(f"[{self.name}] Goal handle exists but not in active state ({GoalStatus(status).name}) on termination.") - # else: - # self.logger.debug(f"[{self.name}] No active goal handle or clean termination, no cancellation needed.") - - # Reset internal state variables regardless + self._goal_handle.cancel_goal_async() self._send_goal_future = None self._get_result_future = None self._goal_handle = None - # Don't reset self._current_status here, it might be SUCCEEDED/FAILED from last update - # self._current_status = ActionExecutionStatus.IDLE # Resetting might hide final state + if self._current_status not in [ + ActionExecutionStatus.SUCCEEDED, + ActionExecutionStatus.FAILED, + ActionExecutionStatus.GOAL_REJECTED, + ActionExecutionStatus.GOAL_CANCELLED, + ActionExecutionStatus.ABORTED, + ]: + self._current_status = ActionExecutionStatus.IDLE - # --- Helper methods for state transitions --- - def _handle_success(self) -> Status: + def _handle_success( + self, result_code: int = FollowJointTrajectory.Result.SUCCESSFUL + ) -> Status: + self.logger.info(f"[{self.name}] Action Succeeded. Code: {result_code}") self._current_status = ActionExecutionStatus.SUCCEEDED self.blackboard_set("action_status", self._current_status.value) + self.blackboard_set("action_result_code", result_code) self._goal_handle = None self._get_result_future = None return Status.SUCCESS @@ -472,27 +427,31 @@ def _handle_failure( self, reason: str, final_status: ActionExecutionStatus = ActionExecutionStatus.FAILED, + result_code: Optional[int] = None, ) -> Status: self.logger.error(f"[{self.name}] Failure: {reason}") self._current_status = final_status self.blackboard_set("action_status", self._current_status.value) - # Set result code if available and not already set failure code - if ( + if result_code is not None: + self.blackboard_set("action_result_code", result_code) + elif ( self._result_code is not None and self._result_code != FollowJointTrajectory.Result.SUCCESSFUL ): self.blackboard_set("action_result_code", self._result_code) - elif self._current_status == ActionExecutionStatus.GOAL_REJECTED: - # Use a convention for rejected? - self.blackboard_set("action_result_code", -998) # Example arbitrary code - else: # General failure - self.blackboard_set("action_result_code", -999) # Example arbitrary code - + elif final_status == ActionExecutionStatus.GOAL_REJECTED: + self.blackboard_set("action_result_code", -998) + else: + self.blackboard_set("action_result_code", -999) self._goal_handle = None self._get_result_future = None - self._send_goal_future = None # Ensure this is cleared too + self._send_goal_future = None return Status.FAILURE def _handle_invalid_state(self, message: str) -> Status: - self.logger.error(f"[{self.name}] Reached invalid state: {message}") - return self._handle_failure(f"Invalid internal state: {message}") + self.logger.error( + f"[{self.name}] Reached invalid state: {message}. This is a bug." + ) + return self._handle_failure( + f"Invalid internal state: {message}", ActionExecutionStatus.FAILED + ) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index f558cb53..bf06ffe3 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -518,6 +518,18 @@ def post_acquisition_sequence() -> py_trees.behaviour.Behaviour: }, ), ), + SwitchArticutoolControllers( + name="SwitchArticutoolToJointTrajectory", + ns=name, + inputs={ + "controllers_to_activate": ["joint_trajectory_controller"], + "controllers_to_deactivate": ["velocity_controller"], + }, + outputs={ + "switch_call_succeeded": None, + "switch_response_ok": None, + }, + ), ExecuteArticutoolTrajectory( name="LevelArticutool", ns=name, @@ -1290,6 +1302,22 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: ], ), ), + SwitchArticutoolControllers( + name="SwitchArticutoolToJointTrajectory", + ns=name, + inputs={ + "controllers_to_activate": [ + "joint_trajectory_controller" + ], + "controllers_to_deactivate": [ + "velocity_controller" + ], + }, + outputs={ + "switch_call_succeeded": None, + "switch_response_ok": None, + }, + ), ExecuteArticutoolTrajectory( name="MoveAboveArticutool", ns=name, From 5ccbceb6d5898693a557f75299d0a4d7430562e9 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 3 Jun 2025 09:25:25 -0700 Subject: [PATCH 103/238] Add small delay to allow IMU to settle after motion for reliable calibration --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index bf06ffe3..e0895132 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1421,6 +1421,10 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: "switch_response_ok": None, }, ), + py_trees.timers.Timer( + name="WaitForIMUToSettle", + duration=2.0, + ), TriggerArticutoolCalibration( name="TriggerArticutoolCalibration", ns=name, From a6a739addd862239e77880f17c8d177e146ac5c9 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 4 Jun 2025 17:37:48 -0700 Subject: [PATCH 104/238] Add end_effector_tool args to define from start.py --- ada_feeding/launch/ada_feeding_launch.xml | 2 ++ ada_planning_scene/launch/ada_moveit_launch.xml | 3 +++ start.py | 17 ++++++++++++++--- 3 files changed, 19 insertions(+), 3 deletions(-) diff --git a/ada_feeding/launch/ada_feeding_launch.xml b/ada_feeding/launch/ada_feeding_launch.xml index 57363a26..59ca77be 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_planning_scene/launch/ada_moveit_launch.xml b/ada_planning_scene/launch/ada_moveit_launch.xml index 7de04355..ca011fc9 100644 --- a/ada_planning_scene/launch/ada_moveit_launch.xml +++ b/ada_planning_scene/launch/ada_moveit_launch.xml @@ -10,6 +10,7 @@ + @@ -19,6 +20,7 @@ + @@ -27,6 +29,7 @@ + diff --git a/start.py b/start.py index 4d16d05a..52ec7568 100755 --- a/start.py +++ b/start.py @@ -24,6 +24,12 @@ "launch the web app." ), ) +parser.add_argument( + "--end_effector_tool", + default="fork", + help=("Which end-effector tool to use"), + choices=["fork", "articulable_fork"], +) parser.add_argument( "-t", "--termination_wait_secs", @@ -202,10 +208,12 @@ async def main(args: argparse.Namespace, pwd: str) -> None: ( "ros2 launch ada_feeding ada_feeding_launch.xml use_estop:=false " 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", @@ -276,13 +284,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": [ From 8bdd57d3aac828090e69ef5e0556202e5434ec0e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 4 Jun 2025 17:56:14 -0700 Subject: [PATCH 105/238] Fix choice list for tool --- start.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/start.py b/start.py index 52ec7568..2593aded 100755 --- a/start.py +++ b/start.py @@ -28,7 +28,7 @@ "--end_effector_tool", default="fork", help=("Which end-effector tool to use"), - choices=["fork", "articulable_fork"], + choices=["fork", "spoon"], ) parser.add_argument( "-t", From 5771ab42933460b05a87234dccd9c0e9e68d34da Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 4 Jun 2025 17:56:35 -0700 Subject: [PATCH 106/238] Add liquid scooping action with spoon --- .../config/acquisition_library.yaml | 27 +++++++++++++++++++ 1 file changed, 27 insertions(+) diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index af14ee3f..235b1cef 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -262,6 +262,7 @@ actions: - -0.0486593 - -0.1055426 pre_torque: 4.0 +# Scrape / Scoop with fork - ext_angular: [0.0, 0.0, 0.0] ext_duration: 1.0 ext_force: 50.0 @@ -287,3 +288,29 @@ actions: - 0.6127465 # qz - 0.3531793 # qw pre_torque: 4.0 +# Liquid scoop with spoon +- ext_angular: [0.0, 0.0, 0.0] + ext_duration: 0.5 + ext_force: 50.0 + ext_linear: [0.0, 0.0, 0.04] + ext_torque: 4.0 + grasp_angular: [0.0, 0.0, 0.0] + grasp_duration: 0.0 + grasp_force: 15.0 + grasp_linear: [0.0, 0.0, 0.0] + grasp_torque: 4.0 + pre_force: 20.0 + pre_offset: + - 0.0 + - 0.0 + - 0.0 + pre_pos: + - 0.0 + - 0.0 + - 1.0 + pre_quat: + - 0.353453 + - 0.6122721 + - 0.6127465 + - 0.3531793 + pre_torque: 4.0 From 0530a1290e901b164a7ed6b601a0a15228b767cf Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 4 Jun 2025 18:15:44 -0700 Subject: [PATCH 107/238] Disable orientation control before executing acquisition motions --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index e0895132..34212402 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1302,6 +1302,14 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: ], ), ), + CallSetOrientationControl( + name="DisableArticutoolOrientation", + ns=name, + inputs={ + "control_mode": 0, + }, + outputs={}, + ), SwitchArticutoolControllers( name="SwitchArticutoolToJointTrajectory", ns=name, From 81e590105cf1ef08b3172f4e2a5495a886bc93ef Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 4 Jun 2025 18:17:30 -0700 Subject: [PATCH 108/238] Add space --- start.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/start.py b/start.py index 2593aded..4112dd8d 100755 --- a/start.py +++ b/start.py @@ -207,7 +207,7 @@ 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}" ), ], From 4de0d90bdf43b3d154370030a4094f8be67cf8fa Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 4 Jun 2025 18:32:34 -0700 Subject: [PATCH 109/238] Restore old joint limits --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 34212402..84717be6 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -185,10 +185,8 @@ def create_tree( # The max amount that each joint can move for any computed plan. Intended # to reduce swivels. max_path_len_joint = { - # "j2n6s200_joint_1": np.pi * 5.0 / 6.0, - # "j2n6s200_joint_2": np.pi / 2.0, - "j2n6s200_joint_1": np.pi, - "j2n6s200_joint_2": np.pi / 1.5, + "j2n6s200_joint_1": np.pi * 5.0 / 6.0, + "j2n6s200_joint_2": np.pi / 2.0, } # Get the base lin to publish servo commands in From e4d9edf8bcfc51dd625128eaf54cd742bb5e938f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 4 Jun 2025 18:49:43 -0700 Subject: [PATCH 110/238] Add argument to define action from start.py --- start.py | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/start.py b/start.py index 4112dd8d..f0b89bf3 100755 --- a/start.py +++ b/start.py @@ -30,6 +30,12 @@ help=("Which end-effector tool to use"), choices=["fork", "spoon"], ) +parser.add_argument( + "--action", + default=0, + help=("Which action (index) to use"), + type=int, +) parser.add_argument( "-t", "--termination_wait_secs", @@ -208,7 +214,8 @@ async def main(args: argparse.Namespace, pwd: str) -> None: ( "ros2 launch ada_feeding ada_feeding_launch.xml use_estop:=false " f"policy:={args.policy} " - f"end_effector_tool:={args.end_effector_tool}" + f"end_effector_tool:={args.end_effector_tool} " + f"action:={args.action} " ), ], "moveit": [ @@ -252,7 +259,8 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "run_web_bridge:=false run_food_detection:=false run_face_detection:=false " "run_food_on_fork_detection:=false run_table_detection:=false " "run_real_sense:=false " - f"policy:={args.policy}" + f"policy:={args.policy} " + f"action:={args.action} " ), ], "browser": [ @@ -293,7 +301,8 @@ async def main(args: argparse.Namespace, pwd: str) -> None: ( "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"end_effector_tool:={args.end_effector_tool}" + f"end_effector_tool:={args.end_effector_tool} " + f"action:={args.action} " ), ], "browser": [ From 543fb42e4f7d14c9f28ea078ccf53845825c1645 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 5 Jun 2025 10:22:55 -0700 Subject: [PATCH 111/238] Reduce extract duration for spoon scooping. This helps a bit to retain more liquid --- ada_feeding_action_select/config/acquisition_library.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index 235b1cef..cb638428 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -290,7 +290,7 @@ actions: pre_torque: 4.0 # Liquid scoop with spoon - ext_angular: [0.0, 0.0, 0.0] - ext_duration: 0.5 + ext_duration: 0.1 ext_force: 50.0 ext_linear: [0.0, 0.0, 0.04] ext_torque: 4.0 From d01531107f42156c61e81e78fb34a65c6740797e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 9 Jun 2025 17:18:46 -0700 Subject: [PATCH 112/238] Add screen session for articutool mock --- start.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/start.py b/start.py index f0b89bf3..da051652 100755 --- a/start.py +++ b/start.py @@ -189,6 +189,9 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "run_food_on_fork_detection:=false run_table_detection:=false " ), ], + "articutool": [ + f"ros2 launch articutool_system articutool.launch.py sim:=mock end_effector_tool:={args.end_effector_tool}", + ], "nano_bridge_sender": [ "ros2 launch nano_bridge sender.launch.xml", ], From d2d8fa70001b3d0d1b6b1e3efab1d366fcd98dfd Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 9 Jun 2025 18:40:54 -0700 Subject: [PATCH 113/238] Add articutool system launch to start.py for sim real --- start.py | 36 ++++++++++++++++++++++++++++++++++++ 1 file changed, 36 insertions(+) diff --git a/start.py b/start.py index da051652..411f5705 100755 --- a/start.py +++ b/start.py @@ -286,6 +286,42 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "camera": [ "ssh nano@nano -t './start_nano.sh'", ], + "articutool_stack": [ + # This command chains several Docker commands on the remote RPi (babbage) + # The -t flag for SSH allocates a pseudo-terminal, which is often necessary + # for interactive docker commands and proper signal handling. + 'ssh charles@babbage -t "' + # 1. Stop the container if it's already running. `|| true` prevents errors if it's not running. + "docker stop articutool_container || true; " + # 2. Remove the stopped container to ensure a fresh start. + "docker rm articutool_container || true; " + # 3. Run a new container. + "docker run " + # --rm: Automatically remove the container when it exits/stops. + "--rm " + # -it: Interactive TTY, allows you to attach and see logs. + "-it " + # --name: Assign a consistent name for easy management. + "--name articutool_container " + # --network=host: Shares the host's networking stack. Easiest for ROS 2 discovery. + "--network=host " + # --privileged + volumes: Provide necessary permissions and access to host devices. + "--privileged " + "--device=/dev/imu:/dev/imu " + "--device=/dev/u2d2:/dev/u2d2 " + "--device=/dev/resense_ft:/dev/resense_ft " + "--volume /run/udev:/run/udev:ro " + "--volume /etc/udev:/etc/udev:ro " + "--volume /dev:/dev " + # Pass environment variables needed for ROS 2 communication. + "-e RMW_IMPLEMENTATION=rmw_cyclonedds_cpp " + f"-e ROS_DOMAIN_ID={args.real_domain_id} " # Pass the domain ID to the container! + # The image to run. + "ros2_articutool:latest" + # The CMD from your Dockerfile will be executed, which is: + # ros2 launch articutool_system articutool.launch.py sim:=real + '"' + ], "rosbridge": [ "ros2 launch rosbridge_server rosbridge_websocket_launch.xml", ], From 00371bd078564e5914d6870044792c1e0a75cbcb Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 13 Jun 2025 10:47:23 -0700 Subject: [PATCH 114/238] Add action to scoop granular foods --- .../config/acquisition_library.yaml | 28 ++++++++++++++++++- 1 file changed, 27 insertions(+), 1 deletion(-) diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index cb638428..dc88972b 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -288,7 +288,7 @@ actions: - 0.6127465 # qz - 0.3531793 # qw pre_torque: 4.0 -# Liquid scoop with spoon +# Liquid dip with spoon - ext_angular: [0.0, 0.0, 0.0] ext_duration: 0.1 ext_force: 50.0 @@ -314,3 +314,29 @@ actions: - 0.6127465 - 0.3531793 pre_torque: 4.0 +# Granular scoop with spoon +- ext_angular: [0.0, 0.0, 0.0] + ext_duration: 0.1 + ext_force: 50.0 + ext_linear: [0.0, 0.0, 0.04] + ext_torque: 4.0 + grasp_angular: [0.0, 0.0, 0.0] + grasp_duration: 0.6 + grasp_force: 15.0 + grasp_linear: [0.1, 0.0, 0.0] + grasp_torque: 4.0 + pre_force: 20.0 + pre_offset: + - 0.0 + - 0.0 + - 0.0 + pre_pos: + - 0.0 + - 0.0 + - 1.0 + pre_quat: + - 0.2759853 + - 0.6508167 + - 0.6517596 + - 0.2747348 + pre_torque: 4.0 From 057b6da353159dc8a8a2bd4347b52a91f73c6194 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 13 Jun 2025 17:43:28 -0700 Subject: [PATCH 115/238] Add behavior to execute a named Articutool primitive --- .../behaviors/articutool/__init__.py | 1 + .../articutool/execute_named_primitive.py | 270 ++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 12 + 3 files changed, 283 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/articutool/execute_named_primitive.py diff --git a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py index 5c2762ca..bf126214 100644 --- a/ada_feeding/ada_feeding/behaviors/articutool/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/articutool/__init__.py @@ -12,3 +12,4 @@ ) from .compute_articutool_leveling_joints import ComputeArticutoolLevelingJoints from .trigger_articutool_calibration import TriggerArticutoolCalibration +from .execute_named_primitive import ExecuteNamedPrimitive diff --git a/ada_feeding/ada_feeding/behaviors/articutool/execute_named_primitive.py b/ada_feeding/ada_feeding/behaviors/articutool/execute_named_primitive.py new file mode 100644 index 00000000..182312d4 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/articutool/execute_named_primitive.py @@ -0,0 +1,270 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +Defines the ExecuteNamedPrimitive behavior, which executes a specified +Articutool primitive action by reading its name and parameters from the +blackboard. +""" + +from typing import Union, Optional, Dict, Any, List +from enum import Enum +import traceback + +import rclpy +from rclpy.action import ActionClient +from rclpy.action.client import ClientGoalHandle +from action_msgs.msg import GoalStatus +from rclpy.node import Node +from rclpy.executors import Future +from rclpy.duration import Duration as RCLPYDuration + +from articutool_interfaces.action import ExecuteArticutoolPrimitive + +from overrides import override +import py_trees +import py_trees.blackboard +from py_trees.common import Status + +# Assuming these are your actual base classes and helpers +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class ActionExecutionStatus(Enum): + """Enum for tracking the state of the action client behavior.""" + + IDLE = "IDLE" + CHECKING_SERVER = "CHECKING_SERVER" + SENDING_GOAL = "SENDING_GOAL" + EXECUTING = "EXECUTING" + GOAL_REJECTED = "GOAL_REJECTED" + GOAL_CANCELLED = "GOAL_CANCELLED" + SUCCEEDED = "SUCCEEDED" + ABORTED = "ABORTED" + FAILED = "FAILED" + + +class ExecuteNamedPrimitive(BlackboardBehavior): + """ + Executes a named Articutool primitive by calling the ExecuteArticutoolPrimitive + action server. The primitive name and its parameters are read from the blackboard. + If the primitive name is "NONE" or empty, it succeeds immediately. + """ + + DEFAULT_ACTION_SERVER_NAME = "/orientation_control/execute_primitive" + DEFAULT_WAIT_TIMEOUT_SEC = 2.0 + + def __init__(self, name: str, ns: Optional[str] = None, **kwargs): + super().__init__(name=name, ns=ns if ns else name, **kwargs) + self.node: Optional[Node] = None + self._action_client: Optional[ActionClient] = None + self._send_goal_future: Optional[Future] = None + self._get_result_future: Optional[Future] = None + self._goal_handle: Optional[ClientGoalHandle] = None + self._current_status: ActionExecutionStatus = ActionExecutionStatus.IDLE + self._result: Optional[ExecuteArticutoolPrimitive.Result] = None + + self._action_server_name_str: str = self.DEFAULT_ACTION_SERVER_NAME + self._wait_timeout_float: float = self.DEFAULT_WAIT_TIMEOUT_SEC + + def blackboard_inputs( + self, + primitive_name: Union[BlackboardKey, str], + primitive_params: Union[BlackboardKey, List[float]] = [], + action_server_name: Union[BlackboardKey, str] = DEFAULT_ACTION_SERVER_NAME, + wait_for_server_timeout_sec: Union[ + BlackboardKey, float + ] = DEFAULT_WAIT_TIMEOUT_SEC, + ) -> None: + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + primitive_result: Optional[BlackboardKey] = None, + primitive_status: Optional[BlackboardKey] = None, + ) -> None: + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def setup(self, **kwargs: Any) -> None: + try: + self.node = kwargs["node"] + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + return # Critical failure + + try: + self._action_server_name_str = self.blackboard_get("action_server_name") + self._wait_timeout_float = self.blackboard_get( + "wait_for_server_timeout_sec" + ) + + self._action_client = ActionClient( + self.node, ExecuteArticutoolPrimitive, self._action_server_name_str + ) + self.logger.info( + f"[{self.name}] Action client created for '{self._action_server_name_str}'." + ) + except Exception as e: + self.logger.error( + f"[{self.name}] Failed to create action client during setup: {e} {traceback.format_exc()}" + ) + self._action_client = None + + @override + def initialise(self) -> None: + self.logger.debug(f"[{self.name}] Initialising.") + self._send_goal_future = None + self._get_result_future = None + self._goal_handle = None + self._current_status = ActionExecutionStatus.IDLE + self._result = None + self.blackboard_set("primitive_result", None) + self.blackboard_set("primitive_status", self._current_status.value) + + def _handle_failure( + self, + reason: str, + final_status: ActionExecutionStatus = ActionExecutionStatus.FAILED, + ) -> Status: + self.logger.error(f"[{self.name}] Failure: {reason}") + self._current_status = final_status + self.blackboard_set("primitive_status", self._current_status.value) + return Status.FAILURE + + def _handle_success(self, message: str) -> Status: + self.logger.info(f"[{self.name}] Success: {message}") + self._current_status = ActionExecutionStatus.SUCCEEDED + self.blackboard_set("primitive_status", self._current_status.value) + return Status.SUCCESS + + @override + def update(self) -> Status: + self.blackboard_set("primitive_status", self._current_status.value) + + if self.node is None or self._action_client is None: + return self._handle_failure( + "Behavior not initialized properly (node or action client is None)." + ) + + # --- State: IDLE -> Check inputs and transition --- + if self._current_status == ActionExecutionStatus.IDLE: + try: + primitive_name = self.blackboard_get("primitive_name") + # Handle the "do nothing" case + if not primitive_name or primitive_name.upper() == "NONE": + return self._handle_success("No primitive specified.") + + # If a primitive is specified, transition to check for server + self._current_status = ActionExecutionStatus.CHECKING_SERVER + self.blackboard_set("primitive_status", self._current_status.value) + return Status.RUNNING # Return RUNNING to re-tick in the new state + + except KeyError as e: + return self._handle_failure(f"Blackboard key error: {e}") + + # --- State: CHECKING_SERVER -> Wait for server then send goal --- + if self._current_status == ActionExecutionStatus.CHECKING_SERVER: + if not self._action_client.wait_for_server( + timeout_sec=self._wait_timeout_float + ): + return self._handle_failure( + f"Action server '{self._action_server_name_str}' not available after {self._wait_timeout_float}s timeout." + ) + + try: + # Re-read inputs in case they changed while waiting + primitive_name = self.blackboard_get("primitive_name") + primitive_params = self.blackboard_get("primitive_params") or [] + + goal_msg = ExecuteArticutoolPrimitive.Goal() + goal_msg.primitive_name = primitive_name + goal_msg.parameters = [float(p) for p in primitive_params] + + self.logger.info( + f"[{self.name}] Sending goal '{primitive_name}' with params {goal_msg.parameters}" + ) + self._send_goal_future = self._action_client.send_goal_async(goal_msg) + self._current_status = ActionExecutionStatus.SENDING_GOAL + return Status.RUNNING + except Exception as e: + return self._handle_failure( + f"Error sending goal: {e} {traceback.format_exc()}" + ) + + # --- State: SENDING_GOAL -> Waiting for goal acceptance --- + if self._current_status == ActionExecutionStatus.SENDING_GOAL: + if not self._send_goal_future.done(): + return Status.RUNNING + try: + self._goal_handle = self._send_goal_future.result() + if not self._goal_handle.accepted: + return self._handle_failure( + "Goal rejected by action server.", + ActionExecutionStatus.GOAL_REJECTED, + ) + self._get_result_future = self._goal_handle.get_result_async() + self._current_status = ActionExecutionStatus.EXECUTING + return Status.RUNNING + except Exception as e: + return self._handle_failure(f"Exception getting goal handle: {e}") + + # --- State: EXECUTING -> Waiting for the result --- + if self._current_status == ActionExecutionStatus.EXECUTING: + if not self._get_result_future.done(): + return Status.RUNNING + try: + result_wrapper = self._get_result_future.result() + self._result = result_wrapper.result + self.blackboard_set("primitive_result", self._result) + final_status = result_wrapper.status + if final_status == GoalStatus.STATUS_SUCCEEDED and self._result.success: + return self._handle_success( + f"Primitive completed successfully: {self._result.message}" + ) + else: + final_enum = ( + ActionExecutionStatus.ABORTED + if final_status == GoalStatus.STATUS_ABORTED + else ( + ActionExecutionStatus.GOAL_CANCELLED + if final_status == GoalStatus.STATUS_CANCELED + else ActionExecutionStatus.FAILED + ) + ) + return self._handle_failure( + f"Primitive did not succeed. Final status: {final_status}, Result msg: '{self._result.message}'", + final_enum, + ) + except Exception as e: + return self._handle_failure( + f"Exception while getting action result: {e}" + ) + + # Fallback + return self._handle_failure( + f"Reached unhandled state: {self._current_status.value}" + ) + + @override + def terminate(self, new_status: Status) -> None: + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") + if ( + new_status == Status.INVALID + and self._goal_handle + and self._goal_handle.is_active + ): + self.logger.warning( + f"[{self.name}] Terminated externally. Attempting to cancel active goal." + ) + self._goal_handle.cancel_goal_async() + self.initialise() diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 84717be6..890720cf 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -67,6 +67,7 @@ SwitchArticutoolControllers, ComputeArticutoolLevelingJoints, TriggerArticutoolCalibration, + ExecuteNamedPrimitive, ) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import ( @@ -556,6 +557,17 @@ def post_acquisition_sequence() -> py_trees.behaviour.Behaviour: "switch_response_ok": None, }, ), + # ExecuteNamedPrimitive( + # name="RunPostAcquisitionPrimitive", + # ns=name, + # inputs={ + # "primitive_name": "VIBRATE_ROLL", + # "primitive_params": [3.0, 0.15, 1.0], + # }, + # outputs={ + # "primitive_status": None, + # }, + # ), CallSetOrientationControl( name="SetArticutoolOrientation", ns=name, From a3dec087e18da572878440874dce8eadd6bc8069 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 13 Jun 2025 21:10:47 -0700 Subject: [PATCH 116/238] feat(Acquisition): Integrate configurable primitives into action schema Implements the ability to define and execute Articutool primitives at specific stages of the food acquisition process, driven by the `AcquisitionSchema`. This makes acquisition strategies more flexible and data-driven, allowing different primitives (e.g., twirling, vibrating) to be specified for different food types in a YAML configuration file. This change includes: - **`ada_feeding_msgs`**: Updated `AcquisitionSchema.msg` to include new fields for primitive actions (`post_move_into_primitive_name`, `post_acquisition_primitive_params`, etc.). - **`ada_feeding_action_select`**: - Modified `helpers.py`: The `get_action_library` function now parses the new primitive fields from `acquisition_library.yaml` and correctly populates the `AcquisitionSchema` ROS message objects. - Updated `acquisition_library.yaml` with examples of the new primitive fields. - **`ada_feeding`**: - Modified `ComputeActionConstraints.py` to read the new primitive data from the selected `AcquisitionSchema` and write it to the blackboard. - Updated `AcquireFoodTree.py` to instantiate and use the `ExecuteNamedPrimitive` behavior at the `post-acquisition` and `post-move-into` stages, reading the primitive name and parameters from the blackboard. This resolves an issue where primitives were not being executed because the action schema data was not being correctly parsed and propagated through the system. --- .../acquisition/compute_action_constraints.py | 45 ++++++++++++-- .../ada_feeding/trees/acquire_food_tree.py | 61 +++++++++++++++---- .../ada_feeding_action_select/helpers.py | 14 +++++ .../config/acquisition_library.yaml | 4 ++ ada_feeding_msgs/msg/AcquisitionSchema.msg | 14 +++++ 5 files changed, 123 insertions(+), 15 deletions(-) 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 25164820..fa5c0ce2 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py @@ -5,12 +5,12 @@ """ This module defines behaviors that take an AcquisitionSchema.msg object (or return from AcquisitionSelect.srv) and computes the outputs -needed to send MoveIt2Plan calls. +needed to send MoveIt2Plan calls and execute primitive actions. """ # Standard imports from copy import deepcopy -from typing import Union, Optional +from typing import Union, Optional, List # Third-party imports from geometry_msgs.msg import ( @@ -45,7 +45,7 @@ class ComputeActionConstraints(BlackboardBehavior): """ Checks AcquisitionSelect response, implements stochastic policy choice, and then decomposes into individual - BlackboardKey objects for MoveIt2 Behaviors. + BlackboardKey objects for MoveIt2 Behaviors and Primitive Actions. Also sets static TF from food -> approach frame """ @@ -93,6 +93,10 @@ def blackboard_outputs( ext_thresh: Optional[BlackboardKey], # SetParameters.Request action: Optional[BlackboardKey], # AcquisitionSchema.msg action_index: Optional[BlackboardKey], # int + post_move_into_primitive_name: Optional[BlackboardKey], + post_move_into_primitive_params: Optional[BlackboardKey], + post_acquisition_primitive_name: Optional[BlackboardKey], + post_acquisition_primitive_params: Optional[BlackboardKey], ) -> None: """ Blackboard Outputs @@ -106,6 +110,10 @@ def blackboard_outputs( 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 + post_move_into_primitive_name: The name of the primitive to run after MoveInto. + post_move_into_primitive_params: The parameters for that primitive. + post_acquisition_primitive_name: The name of the primitive to run after acquisition. + post_acquisition_primitive_params: The parameters for that primitive. """ # pylint: disable=unused-argument, duplicate-code # Arguments are handled generically in base class. @@ -206,7 +214,36 @@ def update(self) -> py_trees.common.Status: create_ft_thresh_request(action.ext_force, action.ext_torque), ) - ### Final write to Blackboard + # Post-Move-Into Primitive + # post_move_into_name = ( + # action.post_move_into_primitive_name or "NONE" + # ) # Default to NONE + post_move_into_name = action.post_move_into_primitive_name + post_move_into_params = list(action.post_move_into_primitive_params) + self.blackboard_set("post_move_into_primitive_name", post_move_into_name) + self.blackboard_set( + "post_move_into_primitive_params", post_move_into_params + ) + self.logger.info( + f"[{self.name}] Set Post-MoveInto Primitive to: '{post_move_into_name}' with params: {post_move_into_params}" + ) + + # Post-Acquisition Primitive + # post_acquisition_name = ( + # action.post_acquisition_primitive_name or "NONE" + # ) # Default to NONE + post_acquisition_name = action.post_acquisition_primitive_name + post_acquisition_params = list(action.post_acquisition_primitive_params) + self.blackboard_set( + "post_acquisition_primitive_name", post_acquisition_name + ) + self.blackboard_set( + "post_acquisition_primitive_params", post_acquisition_params + ) + self.logger.info( + f"[{self.name}] Set Post-Acquisition Primitive to: '{post_acquisition_name}' with params: {post_acquisition_params}" + ) + self.blackboard_set("action", action) self.blackboard_set("action_index", index) return py_trees.common.Status.SUCCESS diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 890720cf..e2d9c4c6 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -557,17 +557,21 @@ def post_acquisition_sequence() -> py_trees.behaviour.Behaviour: "switch_response_ok": None, }, ), - # ExecuteNamedPrimitive( - # name="RunPostAcquisitionPrimitive", - # ns=name, - # inputs={ - # "primitive_name": "VIBRATE_ROLL", - # "primitive_params": [3.0, 0.15, 1.0], - # }, - # outputs={ - # "primitive_status": None, - # }, - # ), + ExecuteNamedPrimitive( + name="RunPostAcquisitionPrimitive", + ns=name, + inputs={ + "primitive_name": BlackboardKey( + "post_acquisition_action_name" + ), + "primitive_params": BlackboardKey( + "post_acquisition_action_params" + ), + }, + outputs={ + "primitive_status": None, + }, + ), CallSetOrientationControl( name="SetArticutoolOrientation", ns=name, @@ -656,6 +660,18 @@ def pre_acquisition_sequence( "ext_thresh": BlackboardKey("ext_thresh"), "action": BlackboardKey("action"), "action_index": BlackboardKey("action_index"), + "post_move_into_primitive_name": BlackboardKey( + "post_move_into_action_name" + ), + "post_move_into_primitive_params": BlackboardKey( + "post_move_into_action_params" + ), + "post_acquisition_primitive_name": BlackboardKey( + "post_acquisition_action_name" + ), + "post_acquisition_primitive_params": BlackboardKey( + "post_acquisition_action_params" + ), }, ), ), @@ -1474,6 +1490,29 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: outputs={}, ), ), + CallSetOrientationControl( + name="SetArticutoolOrientation", + ns=name, + inputs={ + "control_mode": 0, + }, + outputs={}, + ), + ExecuteNamedPrimitive( + name="RunPostMoveIntoPrimitive", + ns=name, + inputs={ + "primitive_name": BlackboardKey( + "post_move_into_action_name" + ), + "primitive_params": BlackboardKey( + "post_move_into_action_params" + ), + }, + outputs={ + "primitive_status": None, + }, + ), ### Scoped Behavior for Moveit2_Servo scoped_behavior( name="MoveIt2Servo", diff --git a/ada_feeding_action_select/ada_feeding_action_select/helpers.py b/ada_feeding_action_select/ada_feeding_action_select/helpers.py index 535dab42..5e5f30a0 100644 --- a/ada_feeding_action_select/ada_feeding_action_select/helpers.py +++ b/ada_feeding_action_select/ada_feeding_action_select/helpers.py @@ -117,6 +117,20 @@ def get_action_library( schema.ext_force = element["ext_force"] schema.ext_torque = element["ext_torque"] + schema.post_move_into_primitive_name = str( + element.get("post_move_into_primitive_name", "NONE") + ) + schema.post_move_into_primitive_params = [ + float(p) for p in element.get("post_move_into_primitive_params", []) + ] + + schema.post_acquisition_primitive_name = str( + element.get("post_acquisition_primitive_name", "NONE") + ) + schema.post_acquisition_primitive_params = [ + float(p) for p in element.get("post_acquisition_primitive_params", []) + ] + library.append(schema) return library diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index dc88972b..69c755e4 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -340,3 +340,7 @@ actions: - 0.6517596 - 0.2747348 pre_torque: 4.0 + post_move_into_primitive_name: "NONE" + post_move_into_primitive_params: [] + post_acquisition_primitive_name: "VIBRATE_ROLL" + post_acquisition_primitive_params: [10.0, 0.1, 1.0] # [frequency_hz, amplitude_rad, duration_sec] diff --git a/ada_feeding_msgs/msg/AcquisitionSchema.msg b/ada_feeding_msgs/msg/AcquisitionSchema.msg index 4a796d8c..515d3df9 100644 --- a/ada_feeding_msgs/msg/AcquisitionSchema.msg +++ b/ada_feeding_msgs/msg/AcquisitionSchema.msg @@ -63,3 +63,17 @@ builtin_interfaces/Duration ext_duration # Extraction Force/Torque Thresholds float64 ext_force float64 ext_torque + +# The name of the primitive to run *after* the MoveInto Cartesian push. +# e.g., "TWIRL_CW" or "NONE". +string post_move_into_primitive_name + +# Parameters for the post_move_into_primitive. +float64[] post_move_into_primitive_params + +# The name of the primitive to run *after* the grasp/lift is complete. +# e.g., "VIBRATE_ROLL" to settle food. +string post_acquisition_primitive_name + +# Parameters for the post_acquisition_primitive. +float64[] post_acquisition_primitive_params From 17e702db417ecf4246b71d91c602ba79468f4e0e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 14 Jun 2025 15:36:15 -0700 Subject: [PATCH 117/238] Incorporate food frame rotation to align with robot base inside the ComputeFoodFrame behavior by setting a flag, instead of defining separate behaviors to modify the poses and twists defined relative to the food frame after the fact --- .../acquisition/compute_food_frame.py | 95 +++++++++++++++++-- .../ada_feeding/trees/acquire_food_tree.py | 26 +---- 2 files changed, 89 insertions(+), 32 deletions(-) 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 0a65ec15..ce8c1183 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_food_frame.py @@ -4,14 +4,23 @@ """ This module defines the ComputeFoodFrame behavior, which computes the -food frame from the Mask provided from a perception algorithm. +food frame from the Mask provided from a perception algorithm, with an +option to align it to the robot's base. """ # Standard imports +import math from typing import Optional, Tuple, Union # Third-party imports import cv2 as cv -from geometry_msgs.msg import PointStamped, TransformStamped, Vector3Stamped +from geometry_msgs.msg import ( + PointStamped, + TransformStamped, + Vector3Stamped, + Pose, + Quaternion, + Vector3, +) import numpy as np import numpy.typing as npt from overrides import override @@ -20,6 +29,8 @@ import rclpy from sensor_msgs.msg import CameraInfo import tf2_ros +from scipy.spatial.transform import Rotation as R +import ros2_numpy # Local imports from ada_feeding_msgs.msg import Mask @@ -36,8 +47,10 @@ class ComputeFoodFrame(BlackboardBehavior): """ - Computes the food reference frame. - See definition in AcquisitionSchema.msg + Computes the food reference frame from a perception mask. + Optionally rotates this frame around its Z-axis to align its + local X-axis with the vector from the robot base to the food origin. + See definition in AcquisitionSchema.msg. """ # pylint: disable=arguments-differ @@ -48,6 +61,7 @@ class ComputeFoodFrame(BlackboardBehavior): # pylint: disable=too-many-arguments # These are effectively config definitions # They require a lot of arguments. + EPSILON = 1e-6 def blackboard_inputs( self, @@ -56,6 +70,8 @@ def blackboard_inputs( timestamp: Union[BlackboardKey, rclpy.time.Time] = rclpy.time.Time(), food_frame_id: Union[BlackboardKey, str] = "food", world_frame: Union[BlackboardKey, str] = "world", + robot_base_frame_id: Union[BlackboardKey, str] = "j2n6s200_link_base", + align_to_robot_base: Union[BlackboardKey, bool] = True, flip_food_frame: Union[BlackboardKey, bool] = False, ) -> None: """ @@ -70,6 +86,9 @@ def blackboard_inputs( food_frame_id (string): If len>0, TF frame to publish static transform (relative to world_frame) world_frame (string): ID of the TF frame to represent the food frame in + robot_base_frame_id (string): ID of the robot's base frame for alignment. + align_to_robot_base (bool): If true, rotates the computed food_frame + around its Z-axis to align with the robot. flip_food_frame (bool): whether to rotate the food frame 180 about Z """ # pylint: disable=unused-argument, duplicate-code @@ -217,6 +236,10 @@ def get_mask_center( return (mask.roi.x_offset + c_x, mask.roi.y_offset + c_y, median_depth) + def _normalize_angle(self, angle: float) -> float: + """Normalize an angle to [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + @override def update(self) -> py_trees.common.Status: # Docstring copied from @override @@ -327,15 +350,71 @@ def update(self) -> py_trees.common.Status: # Project to world x-y plane x_pos.vector.z = 0.0 + world_food_initial_quat_xyzw = quat_between_vectors( + Vector3(x=1.0), x_pos.vector + ) + + R_world_food_initial = R.from_quat( + [ + world_food_initial_quat_xyzw.x, + world_food_initial_quat_xyzw.y, + world_food_initial_quat_xyzw.z, + world_food_initial_quat_xyzw.w, + ] + ) + + # Conditionally rotate the food frame to align with the robot base + R_correction_z = R.identity() # Start with no correction + if self.blackboard_get("align_to_robot_base"): + P_food_in_rb = ros2_numpy.numpify(center.point) + L_xy_rb_numpy = np.array([P_food_in_rb[0], P_food_in_rb[1]]) + + if np.linalg.norm(L_xy_rb_numpy) < self.EPSILON: + self.logger.info( + f"[{self.name}] Food origin too close to robot base. No alignment rotation applied." + ) + else: + desired_global_approach_yaw_rb = math.atan2( + L_xy_rb_numpy[1], L_xy_rb_numpy[0] + ) + + X_axis_in_food_frame = np.array([1.0, 0.0, 0.0]) + X_axis_in_rb_frame = R_world_food_initial.apply(X_axis_in_food_frame) + X_axis_xy_in_rb_frame = np.array( + [X_axis_in_rb_frame[0], X_axis_in_rb_frame[1]] + ) + + if np.linalg.norm(X_axis_xy_in_rb_frame) < self.EPSILON: + self.logger.warn( + f"[{self.name}] Food frame X-axis has no projection in robot base XY plane. Cannot align." + ) + else: + current_food_x_yaw_rb = math.atan2( + X_axis_xy_in_rb_frame[1], X_axis_xy_in_rb_frame[0] + ) + delta_psi_food_z = self._normalize_angle( + desired_global_approach_yaw_rb - current_food_x_yaw_rb + ) + + self.logger.info( + f"[{self.name}] Applying alignment rotation of {math.degrees(delta_psi_food_z):.1f} deg to food frame." + ) + R_correction_z = R.from_euler("z", delta_psi_food_z) + + # --- Final Transform Calculation --- + R_world_food_final = R_world_food_initial * R_correction_z + # Convert to TransformStamped world_to_food_transform.transform.translation.x = center.point.x world_to_food_transform.transform.translation.y = center.point.y world_to_food_transform.transform.translation.z = center.point.z - x_unit = Vector3Stamped() - x_unit.vector.x = 1.0 - world_to_food_transform.transform.rotation = quat_between_vectors( - x_unit.vector, x_pos.vector + final_quat_xyzw = R_world_food_final.as_quat() + world_to_food_transform.transform.rotation = Quaternion( + x=final_quat_xyzw[0], + y=final_quat_xyzw[1], + z=final_quat_xyzw[2], + w=final_quat_xyzw[3], ) # # If you need to send a fixed food frame to the robot arm, e.g., to diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index e2d9c4c6..96f19cc4 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -675,28 +675,6 @@ def pre_acquisition_sequence( }, ), ), - RotateLocalApproachPoses( - name="RotateLocalFoodPosesForApproach", - ns=name, - inputs={ - "move_above_pose_in_food_frame_orig": BlackboardKey( - "move_above_pose_food_frame" - ), - "move_into_pose_in_food_frame_orig": BlackboardKey( - "move_into_pose_food_frame" - ), - "food_frame_id": "food", - "robot_base_frame_id": "j2n6s200_link_base", - }, - outputs={ - "move_above_pose_in_food_frame_rotated": BlackboardKey( - "move_above_pose_final_local" - ), - "move_into_pose_in_food_frame_rotated": BlackboardKey( - "move_into_pose_final_local" - ), - }, - ), # Re-Tare FT Sensor and default to 4N threshold pre_moveto_config(name="PreAcquireFTTare"), # --- Prepare MoveAbove Pose for IK --- @@ -704,7 +682,7 @@ def pre_acquisition_sequence( name="StampMoveAbovePoseFood", ns=name, inputs={ - "input_pose": BlackboardKey("move_above_pose_final_local"), + "input_pose": BlackboardKey("move_above_pose_food_frame"), "frame_id": "food", }, outputs={ @@ -732,7 +710,7 @@ def pre_acquisition_sequence( name="StampMoveIntoPoseFood", ns=name, inputs={ - "input_pose": BlackboardKey("move_into_pose_final_local"), + "input_pose": BlackboardKey("move_into_pose_food_frame"), "frame_id": "food", }, outputs={ From 337fcc83a507e4491eacd1f8593e8478c9876097 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 14 Jun 2025 18:41:33 -0700 Subject: [PATCH 118/238] Add behavior to conditionally modify the food frame to align with the robot base. This is necessary because we want to be able to set a flag align_to_robot_base depending on the action being executed, and the food frame is defined before we can extract this flag, so it must be done as a post-processing step --- .../behaviors/acquisition/__init__.py | 1 + .../acquisition/compute_action_constraints.py | 6 + .../conditionally_rotate_food_frame.py | 194 ++++++++++++++++++ .../ada_feeding/trees/acquire_food_tree.py | 20 +- .../ada_feeding_action_select/helpers.py | 2 +- .../config/acquisition_library.yaml | 1 + ada_feeding_msgs/msg/AcquisitionSchema.msg | 4 + 7 files changed, 226 insertions(+), 2 deletions(-) create mode 100644 ada_feeding/ada_feeding/behaviors/acquisition/conditionally_rotate_food_frame.py diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py b/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py index 1b51bdee..151e6b78 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/__init__.py @@ -13,3 +13,4 @@ from .rotate_local_approach_poses import ( RotateLocalApproachPoses, ) +from .conditionally_rotate_food_frame import ConditionallyRotateFoodFrame 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 fa5c0ce2..fd7eee02 100644 --- a/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py +++ b/ada_feeding/ada_feeding/behaviors/acquisition/compute_action_constraints.py @@ -97,6 +97,7 @@ def blackboard_outputs( post_move_into_primitive_params: Optional[BlackboardKey], post_acquisition_primitive_name: Optional[BlackboardKey], post_acquisition_primitive_params: Optional[BlackboardKey], + should_align_to_base: Optional[BlackboardKey], ) -> None: """ Blackboard Outputs @@ -244,6 +245,11 @@ def update(self) -> py_trees.common.Status: f"[{self.name}] Set Post-Acquisition Primitive to: '{post_acquisition_name}' with params: {post_acquisition_params}" ) + self.blackboard_set("should_align_to_base", action.align_to_robot_base) + self.logger.info( + f"[{self.name}] Set 'should_align_to_base' flag to: {action.align_to_robot_base}" + ) + self.blackboard_set("action", action) self.blackboard_set("action_index", index) return py_trees.common.Status.SUCCESS diff --git a/ada_feeding/ada_feeding/behaviors/acquisition/conditionally_rotate_food_frame.py b/ada_feeding/ada_feeding/behaviors/acquisition/conditionally_rotate_food_frame.py new file mode 100644 index 00000000..8f324cd5 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/acquisition/conditionally_rotate_food_frame.py @@ -0,0 +1,194 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This module defines the ConditionallyRotateFoodFrame behavior. +It takes an existing food_frame transform and, based on a boolean flag, +rotates it around its Z-axis to align with the robot's approach vector. +It then re-publishes the updated static TF transform. +""" + +import math +from typing import Union, Optional +from copy import deepcopy + +from geometry_msgs.msg import TransformStamped +import numpy as np +from overrides import override +import py_trees +from py_trees.common import Status +import rclpy +from rclpy.node import Node +from scipy.spatial.transform import Rotation as R +import ros2_numpy +import tf2_ros + +from ada_feeding.behaviors import BlackboardBehavior +from ada_feeding.helpers import BlackboardKey, get_tf_object, set_static_tf + + +class ConditionallyRotateFoodFrame(BlackboardBehavior): + """ + Conditionally rotates the food_frame around its Z-axis to align its + local X-axis with the vector from the robot base to the food origin. + This behavior reads the decision from the loaded action schema. + """ + + EPSILON = 1e-6 + + def blackboard_inputs( + self, + # The initial, un-aligned food_frame transform + initial_food_frame: Union[BlackboardKey, TransformStamped], + # The boolean flag read from the action schema + should_align_to_base: Union[BlackboardKey, bool], + # The frames needed for the calculation + food_frame_id: Union[BlackboardKey, str] = "food", + robot_base_frame_id: Union[BlackboardKey, str] = "j2n6s200_link_base", + ) -> None: + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + # This behavior UPDATES the food_frame on the blackboard, so it needs + # to declare it as an output to get write access. + # It reads the key from its inputs and writes back to the same key. + food_frame_updated: BlackboardKey, + ) -> None: + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def __init__(self, name: str, **kwargs): + super().__init__(name=name, **kwargs) + self.node: Optional[Node] = None + self.tf_buffer: Optional[tf2_ros.Buffer] = None + self.tf_listener: Optional[tf2_ros.TransformListener] = None + # _initialized_properly is handled by the base class setup + + @override + def setup(self, **kwargs): + # This setup pattern is from your original code + try: + self.node = kwargs["node"] + self.tf_buffer, _, self.tf_lock = get_tf_object(self.blackboard, self.node) + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + except Exception as e: + self.logger.error(f"[{self.name}] Error during setup: {e}") + + def _normalize_angle(self, angle: float) -> float: + return (angle + math.pi) % (2 * math.pi) - math.pi + + @override + def update(self) -> Status: + if not hasattr(self, "node") or self.node is None or self.tf_buffer is None: + self.feedback_message = "Behavior not properly initialized." + return Status.FAILURE + + try: + should_align = self.blackboard_get("should_align_to_base") + + if not should_align: + self.feedback_message = ( + "Alignment not required by action schema. Succeeding." + ) + self.logger.info(f"[{self.name}] {self.feedback_message}") + # We still need to write the original frame to the output key + # so the rest of the tree has a consistent key to read from. + original_transform: TransformStamped = self.blackboard_get( + "initial_food_frame" + ) + self.blackboard_set("food_frame_updated", original_transform) + return Status.SUCCESS + + initial_transform: TransformStamped = self.blackboard_get( + "initial_food_frame" + ) + food_fid: str = self.blackboard_get("food_frame_id") + robot_base_fid: str = self.blackboard_get("robot_base_frame_id") + + if initial_transform is None: + self.feedback_message = ( + "Initial food_frame transform not found on blackboard." + ) + return Status.FAILURE + + # This logic assumes world_frame and robot_base_frame are the same. + P_food_in_rb = ros2_numpy.numpify(initial_transform.transform.translation) + R_rb_food_initial = R.from_quat( + ros2_numpy.numpify(initial_transform.transform.rotation) + ) + + # --- Rotation logic from RotateLocalApproachPoses --- + L_xy_rb_numpy = np.array([P_food_in_rb[0], P_food_in_rb[1]]) + if np.linalg.norm(L_xy_rb_numpy) < self.EPSILON: + self.logger.info( + f"[{self.name}] Food origin too close to robot base. No alignment rotation applied." + ) + # Pass through the original transform + self.blackboard_set("food_frame_updated", initial_transform) + return Status.SUCCESS + + desired_global_approach_yaw_rb = math.atan2( + L_xy_rb_numpy[1], L_xy_rb_numpy[0] + ) + + X_axis_in_food_frame = np.array([1.0, 0.0, 0.0]) + X_axis_in_rb_frame = R_rb_food_initial.apply(X_axis_in_food_frame) + X_axis_xy_in_rb_frame = np.array( + [X_axis_in_rb_frame[0], X_axis_in_rb_frame[1]] + ) + + if np.linalg.norm(X_axis_xy_in_rb_frame) < self.EPSILON: + self.logger.warn( + f"[{self.name}] Food frame's X-axis has no projection. Cannot align." + ) + self.blackboard_set( + "food_frame_updated", initial_transform + ) # Pass through + return Status.SUCCESS + + current_food_x_yaw_rb = math.atan2( + X_axis_xy_in_rb_frame[1], X_axis_xy_in_rb_frame[0] + ) + delta_psi_food_z = self._normalize_angle( + desired_global_approach_yaw_rb - current_food_x_yaw_rb + ) + + self.logger.info( + f"[{self.name}] Applying alignment rotation of {math.degrees(delta_psi_food_z):.1f} deg to food frame." + ) + R_correction_z = R.from_euler("z", delta_psi_food_z) + + # --- Apply the rotation and prepare the final transform --- + R_world_food_final = R_rb_food_initial * R_correction_z + + final_transform = deepcopy(initial_transform) + final_quat_xyzw = R_world_food_final.as_quat() + final_transform.transform.rotation.x = final_quat_xyzw[0] + final_transform.transform.rotation.y = final_quat_xyzw[1] + final_transform.transform.rotation.z = final_quat_xyzw[2] + final_transform.transform.rotation.w = final_quat_xyzw[3] + + # Re-publish the static TF with the new, rotated orientation + set_static_tf(final_transform, self.blackboard, self.node) + + # Write the final transform to the output blackboard key + self.blackboard_set("food_frame_updated", final_transform) + + self.feedback_message = ( + f"Food frame realigned by {math.degrees(delta_psi_food_z):.1f} deg." + ) + return Status.SUCCESS + + except Exception as e: + self.feedback_message = f"Unexpected error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 96f19cc4..9a44b429 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -36,6 +36,7 @@ ComputeActionConstraints, ComputeActionTwist, RotateLocalApproachPoses, + ConditionallyRotateFoodFrame, ) from ada_feeding.behaviors.moveit2 import ( MoveIt2JointConstraint, @@ -608,12 +609,13 @@ def pre_acquisition_sequence( # Default food_frame_id = "food" # Default world_frame = "world" "flip_food_frame": flip_food_frame, + "align_to_robot_base": False, }, outputs={ "action_select_request": BlackboardKey( "action_request" ), - "food_frame": None, + "food_frame": BlackboardKey("initial_food_frame"), }, ), ), @@ -672,9 +674,25 @@ def pre_acquisition_sequence( "post_acquisition_primitive_params": BlackboardKey( "post_acquisition_action_params" ), + "should_align_to_base": BlackboardKey( + "should_align_to_base" + ), }, ), ), + ConditionallyRotateFoodFrame( + name="ConditionallyRotateFoodFrame", + ns=name, + inputs={ + "initial_food_frame": BlackboardKey("initial_food_frame"), + "should_align_to_base": BlackboardKey( + "should_align_to_base" + ), + }, + outputs={ + "food_frame_updated": None, + }, + ), # Re-Tare FT Sensor and default to 4N threshold pre_moveto_config(name="PreAcquireFTTare"), # --- Prepare MoveAbove Pose for IK --- diff --git a/ada_feeding_action_select/ada_feeding_action_select/helpers.py b/ada_feeding_action_select/ada_feeding_action_select/helpers.py index 5e5f30a0..59980b7f 100644 --- a/ada_feeding_action_select/ada_feeding_action_select/helpers.py +++ b/ada_feeding_action_select/ada_feeding_action_select/helpers.py @@ -130,7 +130,7 @@ def get_action_library( schema.post_acquisition_primitive_params = [ float(p) for p in element.get("post_acquisition_primitive_params", []) ] - + schema.align_to_robot_base = bool(element.get("align_to_robot_base", False)) library.append(schema) return library diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index 69c755e4..9723479b 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -344,3 +344,4 @@ actions: post_move_into_primitive_params: [] post_acquisition_primitive_name: "VIBRATE_ROLL" post_acquisition_primitive_params: [10.0, 0.1, 1.0] # [frequency_hz, amplitude_rad, duration_sec] + align_to_robot_base: True diff --git a/ada_feeding_msgs/msg/AcquisitionSchema.msg b/ada_feeding_msgs/msg/AcquisitionSchema.msg index 515d3df9..c67559b2 100644 --- a/ada_feeding_msgs/msg/AcquisitionSchema.msg +++ b/ada_feeding_msgs/msg/AcquisitionSchema.msg @@ -77,3 +77,7 @@ string post_acquisition_primitive_name # Parameters for the post_acquisition_primitive. float64[] post_acquisition_primitive_params + +# Determines if the food_frame should be rotated to align its X-axis +# with the robot's approach vector. +bool align_to_robot_base From 4053ce1988f9758c1f23139b9faa36f068b74bbc Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 29 Jun 2025 10:23:31 -0700 Subject: [PATCH 119/238] Add orientation constraint for MoveAbove trajectory --- .../ada_feeding/trees/acquire_food_tree.py | 21 +++++++++++++++++++ 1 file changed, 21 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 9a44b429..39eb0290 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -945,6 +945,24 @@ def move_above_sequence() -> py_trees.behaviour.Behaviour: ) }, ), + MoveIt2OrientationConstraint( + name="SetJacoArmPathConstraintForMoveAbove", + ns=name, + inputs={ + "constraints": None, + "quat_xyzw": (0.707, 0.0, 0.0, 0.707), + "tolerance": ( + np.pi, + 2.0 * np.pi, + 2.0 * np.pi, + ), + }, + outputs={ + "constraints": BlackboardKey( + "move_above_jaco_arm_path_constraints" + ), + }, + ), py_trees.decorators.Timeout( name="MoveAboveJacoArmPlanTimeout", duration=10.0 * self.allowed_planning_time_for_move_above, @@ -955,6 +973,9 @@ def move_above_sequence() -> py_trees.behaviour.Behaviour: "goal_constraints": BlackboardKey( "move_above_jaco_arm_constraints" ), + "path_constraints": BlackboardKey( + "move_above_jaco_arm_path_constraints" + ), "max_velocity_scale": self.max_velocity_scaling_move_above, "max_acceleration_scale": self.max_acceleration_scaling_move_above, "allowed_planning_time": self.allowed_planning_time_for_move_above, From e1e70a9a2219544f36f633ebbe1fdda6929f9614 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 29 Jun 2025 12:11:48 -0700 Subject: [PATCH 120/238] Update CheckArticutoolPathOrientationFeasibility behavior to ensure local continuity of the IK solutions so that the Articutool doesn't try to gravitate towards the second solution during dynamic motions --- ...articutool_path_orientation_feasibility.py | 48 ++++++++++++++----- 1 file changed, 35 insertions(+), 13 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py index e97a309d..bde169c8 100644 --- a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py +++ b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_orientation_feasibility.py @@ -393,6 +393,7 @@ def update(self) -> Status: self.blackboard_set("articutool_is_orientation_feasible", True) return Status.SUCCESS + last_valid_solution: Optional[np.ndarray] = None for traj_idx in indices_to_check: jaco_ee_pose_msg: Pose = jaco_ee_world_poses[traj_idx] R_World_JacoEE = Rotation.from_quat( @@ -420,29 +421,50 @@ def update(self) -> Status: target_y_for_ik_in_atool_base ) - found_valid_solution_for_waypoint = False - for theta_p_sol, theta_r_sol in ik_solutions: + # Filter for solutions within joint limits + valid_solutions = [] + for theta_p, theta_r in ik_solutions: if ( self._pitch_limits_rad[0] - self.EPSILON - <= theta_p_sol + <= theta_p <= self._pitch_limits_rad[1] + self.EPSILON and self._roll_limits_rad[0] - self.EPSILON - <= theta_r_sol - <= self._roll_limits_rad[1] + self.EPSILON + <= theta_r + <= self._pitch_limits_rad[1] + self.EPSILON ): - found_valid_solution_for_waypoint = True - break + valid_solutions.append(np.array([theta_p, theta_r])) - if not found_valid_solution_for_waypoint: - self.feedback_message = ( - f"Articutool IK failed or solution out of limits at traj point {traj_idx}. " - f"Target Y_AtoolBase: {np.round(target_y_for_ik_in_atool_base, 3)}. " - f"Raw IK solutions (tp,tr): {[(round(s[0], 3), round(s[1], 3)) for s in ik_solutions]}" + # If no valid solutions exist, the path is infeasible + if not valid_solutions: + self.logger.warn( + f"[{self.name}] No valid IK solution within joint limits at trajectory point {traj_idx}" ) - self.logger.warn(f"[{self.name}] {self.feedback_message}") self.blackboard_set("articutool_is_orientation_feasible", False) return Status.FAILURE + # Select the best solution based on continuity + if last_valid_solution is None: + # For the first point, find the solution closest to the current tool pose + chosen_solution = valid_solutions[0] + # current_tool_joints = self.blackboard_get( + # "current_articutool_joint_state" + # ) + # distances = [ + # np.linalg.norm(sol - current_tool_joints) + # for sol in valid_solutions + # ] + # chosen_solution = valid_solutions[np.argmin(distances)] + else: + # For subsequent points, find the solution closest to the previous point's solution + distances = [ + np.linalg.norm(sol - last_valid_solution) + for sol in valid_solutions + ] + chosen_solution = valid_solutions[np.argmin(distances)] + + # Update the state for the next iteration + last_valid_solution = chosen_solution + self.logger.debug( f"[{self.name}] Pt {traj_idx}: Articutool IK feasible." ) From d036b5978b01848a2e0ed418a1a3faf7cb60def7 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 29 Jun 2025 12:45:26 -0700 Subject: [PATCH 121/238] Add feasibility check for leveling --- .../ada_feeding/behaviors/state/__init__.py | 1 + ...ck_articutool_path_leveling_feasibility.py | 300 ++++++++++++++++++ 2 files changed, 301 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py index 7a616714..eef4fa06 100644 --- a/ada_feeding/ada_feeding/behaviors/state/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -29,3 +29,4 @@ from .load_pinocchio_model import ( LoadPinocchioModel, ) +from .check_articutool_path_leveling_feasibility import CheckPathLevelingFeasibility diff --git a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py new file mode 100644 index 00000000..7594e426 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py @@ -0,0 +1,300 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This module defines the CheckPathLevelingFeasibility behavior. + +This behavior checks if the Articutool can maintain a level orientation +for its tool tip throughout a given trajectory of the Jaco arm. It ensures +not only that an IK solution exists at each point, but that a continuous +path of solutions can be traced through the joint space of the Articutool, +preventing control instabilities near kinematic singularities. +""" + +# Standard imports +import math +from typing import Union, Optional, List, Tuple + +# Third-party imports +from geometry_msgs.msg import Pose +from moveit_msgs.msg import RobotTrajectory +from trajectory_msgs.msg import JointTrajectory +import numpy as np +from overrides import override +from scipy.spatial.transform import Rotation +import py_trees +import py_trees.blackboard +from py_trees.common import Status +import rclpy.node +import pinocchio as pin + +# Local imports +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class CheckPathLevelingFeasibility(BlackboardBehavior): + """ + Checks Articutool's ability to maintain a level orientation for its tool tip + throughout a Jaco arm trajectory. It verifies that a continuous, within-limits + path of IK solutions exists for the Articutool. + """ + + EPSILON = 1e-6 + WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) + + def blackboard_inputs( + self, + pinocchio_model: Union[BlackboardKey, pin.Model], + pinocchio_data: Union[BlackboardKey, pin.Data], + jaco_joint_names_pin: Union[BlackboardKey, List[str]], + jaco_ee_frame_id_pin: Union[BlackboardKey, int], + jaco_trajectory: Union[BlackboardKey, RobotTrajectory, JointTrajectory], + articutool_pitch_limits_rad: Union[BlackboardKey, Tuple[float, float]], + articutool_roll_limits_rad: Union[BlackboardKey, Tuple[float, float]], + num_trajectory_points_to_check: Union[BlackboardKey, int] = 20, + ) -> None: + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + is_leveling_path_feasible: Optional[BlackboardKey], + ) -> None: + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def __init__(self, name: str, **kwargs): + super().__init__(name=name, **kwargs) + self.node: Optional[rclpy.node.Node] = None + self._pin_model: Optional[pin.Model] = None + self._pin_data: Optional[pin.Data] = None + self._jaco_joint_names_pin: Optional[List[str]] = None + self._jaco_ee_frame_id_pin: Optional[int] = None + self._pitch_limits_rad: Optional[Tuple[float, float]] = None + self._roll_limits_rad: Optional[Tuple[float, float]] = None + self._pinocchio_ready = False + + @override + def setup(self, **kwargs): + """Gets the ROS2 node.""" + try: + self.node = kwargs["node"] + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + + @override + def initialise(self) -> None: + """Reads Pinocchio model and other static info from blackboard.""" + self.logger.debug(f"[{self.name}] Initializing.") + self.blackboard_set("is_leveling_path_feasible", None) + self._pinocchio_ready = self._get_pinocchio_essentials_from_blackboard() + + def _get_pinocchio_essentials_from_blackboard(self) -> bool: + """Reads Pinocchio model, data, and relevant IDs/names from blackboard.""" + if self._pinocchio_ready: + return True + try: + self._pin_model = self.blackboard_get("pinocchio_model") + self._pin_data = self.blackboard_get("pinocchio_data") + self._jaco_joint_names_pin = self.blackboard_get("jaco_joint_names_pin") + self._jaco_ee_frame_id_pin = self.blackboard_get("jaco_ee_frame_id_pin") + self._pitch_limits_rad = self.blackboard_get("articutool_pitch_limits_rad") + self._roll_limits_rad = self.blackboard_get("articutool_roll_limits_rad") + return True + except KeyError as e: + self.logger.error( + f"[{self.name}] Failed to get Pinocchio info from blackboard: {e}." + ) + return False + + def _get_jaco_ee_poses_from_trajectory( + self, trajectory_input: Union[RobotTrajectory, JointTrajectory] + ) -> Optional[List[Pose]]: + """Extracts or computes Jaco end-effector poses from a trajectory message.""" + if not self._pinocchio_ready: + return None + + jaco_ee_world_poses: List[Pose] = [] + # Simplified logic: Assumes trajectory is a JointTrajectory for the Jaco arm. + # A more robust version would handle RobotTrajectory and other cases like in the original file. + if not isinstance(trajectory_input, JointTrajectory): + self.logger.error( + f"[{self.name}] This simplified checker only supports JointTrajectory input." + ) + return None + + jt_points = trajectory_input.points + jt_joint_names = trajectory_input.joint_names + q_robot = pin.neutral(self._pin_model) + + for point_data in jt_points: + temp_jaco_map = { + name: point_data.positions[i] for i, name in enumerate(jt_joint_names) + } + + for j_name_pin in self._jaco_joint_names_pin: + if j_name_pin in temp_jaco_map: + joint_id = self._pin_model.getJointId(j_name_pin) + joint_obj = self._pin_model.joints[joint_id] + theta = temp_jaco_map[j_name_pin] + if joint_obj.nq == 2: # Revolute + q_robot[joint_obj.idx_q] = math.cos(theta) + q_robot[joint_obj.idx_q + 1] = math.sin(theta) + + pin.forwardKinematics(self._pin_model, self._pin_data, q_robot) + pin.updateFramePlacements(self._pin_model, self._pin_data) + jaco_ee_transform_pin: pin.SE3 = self._pin_data.oMf[ + self._jaco_ee_frame_id_pin + ] + + pose = Pose() + pose.position.x, pose.position.y, pose.position.z = ( + jaco_ee_transform_pin.translation + ) + quat_xyzw = Rotation.from_matrix(jaco_ee_transform_pin.rotation).as_quat() + ( + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ) = quat_xyzw + jaco_ee_world_poses.append(pose) + + return jaco_ee_world_poses + + def _normalize_angle(self, angle: float) -> float: + """Normalize angle to be within [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + + def _solve_articutool_ik( + self, target_y_in_atool_base: np.ndarray + ) -> List[np.ndarray]: + """ + Solves the analytical IK for the Articutool to achieve a level orientation. + `target_y_in_atool_base` is the desired "up" vector of the tool tip, expressed in the Articutool's base frame. + """ + vx, vy, vz = target_y_in_atool_base + solutions = [] + + # Solve for theta_r + asin_arg = -vx + if not (-1.0 - self.EPSILON <= asin_arg <= 1.0 + self.EPSILON): + return [] + + theta_r_sol1 = math.asin(np.clip(asin_arg, -1.0, 1.0)) + theta_r_sol2 = self._normalize_angle(math.pi - theta_r_sol1) + + candidate_thetas_r = { + self._normalize_angle(theta_r_sol1), + self._normalize_angle(theta_r_sol2), + } + + # Solve for theta_p for each valid theta_r + for theta_r in candidate_thetas_r: + cos_theta_r = math.cos(theta_r) + if abs(cos_theta_r) > self.EPSILON: + theta_p = math.atan2(vz, vy) + solutions.append(np.array([self._normalize_angle(theta_p), theta_r])) + + return solutions + + @override + def update(self) -> Status: + if not self.node or not self._pinocchio_ready: + self.feedback_message = "Node or Pinocchio model not ready." + return Status.FAILURE + + try: + trajectory_input = self.blackboard_get("jaco_trajectory") + num_points_to_check = self.blackboard_get("num_trajectory_points_to_check") + + jaco_ee_poses = self._get_jaco_ee_poses_from_trajectory(trajectory_input) + + if jaco_ee_poses is None: + self.feedback_message = ( + "Could not extract Jaco EE poses from trajectory." + ) + return Status.FAILURE + + if not jaco_ee_poses: + self.logger.warn( + f"[{self.name}] Trajectory has no waypoints. Assuming feasible." + ) + self.blackboard_set("is_leveling_path_feasible", True) + return Status.SUCCESS + + indices_to_check = np.linspace( + 0, len(jaco_ee_poses) - 1, num_points_to_check, dtype=int + ) + + last_valid_solution = None + + for traj_idx in indices_to_check: + ee_pose = jaco_ee_poses[traj_idx] + R_World_JacoEE = Rotation.from_quat( + [ + ee_pose.orientation.x, + ee_pose.orientation.y, + ee_pose.orientation.z, + ee_pose.orientation.w, + ] + ) + + # Transform world "up" vector into the Jaco EE's local frame + target_y_in_atool_base = R_World_JacoEE.inv().apply( + self.WORLD_UP_VECTOR + ) + + # Get all IK solutions for this orientation + ik_solutions = self._solve_articutool_ik(target_y_in_atool_base) + + # Filter for solutions that are within joint limits + valid_solutions = [ + sol + for sol in ik_solutions + if ( + self._pitch_limits_rad[0] <= sol[0] <= self._pitch_limits_rad[1] + and self._roll_limits_rad[0] + <= sol[1] + <= self._roll_limits_rad[1] + ) + ] + + if not valid_solutions: + self.feedback_message = f"Infeasible: No valid IK solution at trajectory point {traj_idx}." + self.logger.warn(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("is_leveling_path_feasible", False) + return Status.FAILURE + + # Select the best solution based on continuity + if last_valid_solution is None: + chosen_solution = valid_solutions[0] + else: + # Subsequent points: choose solution closest to the previous chosen solution + distances = [ + np.linalg.norm(sol - last_valid_solution) + for sol in valid_solutions + ] + chosen_solution = valid_solutions[np.argmin(distances)] + + last_valid_solution = chosen_solution + + self.feedback_message = "Path is feasible for continuous leveling." + self.logger.info(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("is_leveling_path_feasible", True) + return Status.SUCCESS + + except Exception as e: + self.feedback_message = f"Unexpected error during feasibility check: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + return Status.FAILURE + + @override + def terminate(self, new_status: Status) -> None: + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") From acde1f481c2a1fd35cb520b2efe385a00abd4514 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 29 Jun 2025 12:59:17 -0700 Subject: [PATCH 122/238] Add leveling feasibility check for resting trajectory --- .../ada_feeding/behaviors/state/__init__.py | 4 ++- ...ck_articutool_path_leveling_feasibility.py | 2 +- .../ada_feeding/trees/acquire_food_tree.py | 31 +++++++++++++++++++ 3 files changed, 35 insertions(+), 2 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py index eef4fa06..03e39e7b 100644 --- a/ada_feeding/ada_feeding/behaviors/state/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -29,4 +29,6 @@ from .load_pinocchio_model import ( LoadPinocchioModel, ) -from .check_articutool_path_leveling_feasibility import CheckPathLevelingFeasibility +from .check_articutool_path_leveling_feasibility import ( + CheckArticutoolPathLevelingFeasibility, +) diff --git a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py index 7594e426..4e4dee2f 100644 --- a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py +++ b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py @@ -34,7 +34,7 @@ from ada_feeding.behaviors import BlackboardBehavior -class CheckPathLevelingFeasibility(BlackboardBehavior): +class CheckArticutoolPathLevelingFeasibility(BlackboardBehavior): """ Checks Articutool's ability to maintain a level orientation for its tool tip throughout a Jaco arm trajectory. It verifies that a continuous, within-limits diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 39eb0290..21b9f235 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -58,6 +58,7 @@ ExtractPoseComponents, CheckJacoDirectionalManipulability, CheckArticutoolPathOrientationFeasibility, + CheckArticutoolPathLevelingFeasibility, LoadPinocchioModel, ) from ada_feeding.behaviors.ros.msgs import StampPoseFromPose @@ -249,6 +250,36 @@ def create_tree( }, ), ), + CheckArticutoolPathLevelingFeasibility( + name="CheckArticutoolLevelingFeasibilityForResting", + ns=name, + inputs={ + "pinocchio_model": BlackboardKey("pinocchio_model"), + "pinocchio_data": BlackboardKey("pinocchio_data"), + "jaco_joint_names_pin": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ], + "jaco_ee_frame_id_pin": BlackboardKey( + "jaco_ee_frame_id_pin" + ), + "jaco_trajectory": BlackboardKey( + "move_into_jaco_arm_trajectory" + ), + "articutool_pitch_limits_rad": (-np.pi / 2, np.pi / 2), + "articutool_roll_limits_rad": (-np.pi, np.pi), + "num_trajectory_points_to_check": 20, + }, + outputs={ + "is_leveling_path_feasible": BlackboardKey( + "articutool_can_maintain_leveling" + ) + }, + ), MoveIt2Execute( name="Resting", ns=name, From 8d7f41b89b409de688344bebeef6a7abde4e34d1 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 29 Jun 2025 18:01:21 -0700 Subject: [PATCH 123/238] Add leveling feasibility check to MoveToConfigurationWithWheelchairWallTree, which effectively encompasses the motion to the staging configuration --- ...configuration_with_wheelchair_wall_tree.py | 64 ++++++++++++++++++- 1 file changed, 63 insertions(+), 1 deletion(-) 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 718ad98c..36283e7a 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 @@ -27,7 +27,11 @@ MoveIt2JointConstraint, MoveIt2OrientationConstraint, ) -from ada_feeding.behaviors.state import GetJointStates +from ada_feeding.behaviors.state import ( + GetJointStates, + CheckArticutoolPathLevelingFeasibility, + LoadPinocchioModel, +) from ada_feeding.helpers import BlackboardKey from ada_feeding.idioms import pre_moveto_config, scoped_behavior from ada_feeding.idioms.bite_transfer import ( @@ -38,6 +42,8 @@ MoveToTree, ) +import numpy as np + class MoveToConfigurationWithWheelchairWallTree(MoveToTree): """ @@ -152,6 +158,34 @@ def create_tree( name=name, memory=True, children=[ + LoadPinocchioModel( + name="LoadPinocchioModel", + ns=name, + inputs={ + "urdf_file_path": "package://ada_moveit/config/ada.urdf.xacro", + "jaco_joint_names": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ], + "articutool_joint_names": ["atool_joint1", "atool_joint2"], + "jaco_end_effector_link_name": "j2n6s200_end_effector", + "tool_tip_link_name": "tool_tip", + }, + outputs={ + "pinocchio_model": BlackboardKey("pinocchio_model"), + "pinocchio_data": BlackboardKey("pinocchio_data"), + "jaco_vel_indices_pin": BlackboardKey("jaco_vel_indices_pin"), + "articutool_vel_indices_pin": BlackboardKey( + "articutool_vel_indices_pin" + ), + "jaco_ee_frame_id_pin": BlackboardKey("jaco_ee_frame_id_pin"), + "tool_tip_frame_id_pin": BlackboardKey("tool_tip_frame_id_pin"), + }, + ), # Retare the F/T sensor and set the F/T Thresholds pre_moveto_config( name=name + "PreMoveToConfig", @@ -197,6 +231,34 @@ def create_tree( outputs={"trajectory": BlackboardKey("trajectory")}, ), ), + CheckArticutoolPathLevelingFeasibility( + name="CheckArticutoolLevelingFeasibilityForConfiguration", + ns=name, + inputs={ + "pinocchio_model": BlackboardKey("pinocchio_model"), + "pinocchio_data": BlackboardKey("pinocchio_data"), + "jaco_joint_names_pin": [ + "j2n6s200_joint_1", + "j2n6s200_joint_2", + "j2n6s200_joint_3", + "j2n6s200_joint_4", + "j2n6s200_joint_5", + "j2n6s200_joint_6", + ], + "jaco_ee_frame_id_pin": BlackboardKey( + "jaco_ee_frame_id_pin" + ), + "jaco_trajectory": BlackboardKey("trajectory"), + "articutool_pitch_limits_rad": (-np.pi / 2, np.pi / 2), + "articutool_roll_limits_rad": (-np.pi, np.pi), + "num_trajectory_points_to_check": 20, + }, + outputs={ + "is_leveling_path_feasible": BlackboardKey( + "articutool_can_maintain_leveling" + ) + }, + ), # Execute MoveIt2Execute( name="MoveToStagingConfigurationExecute", From 8531e9e9f20afa77ccf77a580995a55e82d5461f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 29 Jun 2025 19:07:54 -0700 Subject: [PATCH 124/238] Update leveling feasibility check --- ...ck_articutool_path_leveling_feasibility.py | 317 ++++++++++-------- .../ada_feeding/trees/acquire_food_tree.py | 2 +- ...configuration_with_wheelchair_wall_tree.py | 2 +- 3 files changed, 187 insertions(+), 134 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py index 4e4dee2f..7db94ccc 100644 --- a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py +++ b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_leveling_feasibility.py @@ -1,15 +1,15 @@ # -*- coding: utf-8 -*- -# Copyright (c) 2025, Personal Robotics Laboratory +# Copyright (c) 2024-2025, Personal Robotics Laboratory # License: BSD 3-Clause. See LICENSE.md file in root directory. """ -This module defines the CheckPathLevelingFeasibility behavior. +This module defines the CheckArticutoolPathLevelingFeasibility behavior. -This behavior checks if the Articutool can maintain a level orientation -for its tool tip throughout a given trajectory of the Jaco arm. It ensures -not only that an IK solution exists at each point, but that a continuous -path of solutions can be traced through the joint space of the Articutool, -preventing control instabilities near kinematic singularities. +This behavior checks if the Articutool can maintain a "level" orientation +(tool_tip Y-axis aligned against gravity) throughout a given trajectory +of the Jaco arm. It iterates through the trajectory, and for each waypoint, +it calculates if a valid, within-limits IK solution exists for the +Articutool to achieve leveling. """ # Standard imports @@ -24,25 +24,36 @@ from overrides import override from scipy.spatial.transform import Rotation import py_trees -import py_trees.blackboard from py_trees.common import Status import rclpy.node import pinocchio as pin # Local imports -from ada_feeding.helpers import BlackboardKey +# Assuming these are in your project structure from ada_feeding.behaviors import BlackboardBehavior +from ada_feeding.helpers import BlackboardKey class CheckArticutoolPathLevelingFeasibility(BlackboardBehavior): """ - Checks Articutool's ability to maintain a level orientation for its tool tip - throughout a Jaco arm trajectory. It verifies that a continuous, within-limits - path of IK solutions exists for the Articutool. + Checks if the Articutool can remain level throughout a Jaco trajectory. + + This behavior iterates through a specified number of points along a given + Jaco arm trajectory. For each point, it calculates the corresponding + Jaco end-effector pose and then solves the inverse kinematics for the + Articutool to determine if there's a joint configuration that would keep + the tool_tip's Y-axis pointing upwards (level), within the Articutool's + joint limits. + + If a valid leveling solution exists for all checked points, the behavior + returns SUCCESS. If any point is found to be infeasible, it immediately + returns FAILURE. This is crucial for validating transport motions where + spillage must be prevented. """ EPSILON = 1e-6 - WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) + # The desired "up" vector for the tool tip's Y-axis in the world frame. + WORLD_Z_UP_VECTOR = np.array([0.0, 0.0, 1.0]) def blackboard_inputs( self, @@ -53,21 +64,24 @@ def blackboard_inputs( jaco_trajectory: Union[BlackboardKey, RobotTrajectory, JointTrajectory], articutool_pitch_limits_rad: Union[BlackboardKey, Tuple[float, float]], articutool_roll_limits_rad: Union[BlackboardKey, Tuple[float, float]], - num_trajectory_points_to_check: Union[BlackboardKey, int] = 20, + num_trajectory_points_to_check: Union[BlackboardKey, int] = 10, ) -> None: + """Define blackboard inputs.""" super().blackboard_inputs( **{key: value for key, value in locals().items() if key != "self"} ) def blackboard_outputs( self, - is_leveling_path_feasible: Optional[BlackboardKey], + articutool_is_leveling_feasible: Optional[BlackboardKey], ) -> None: + """Define blackboard outputs.""" super().blackboard_outputs( **{key: value for key, value in locals().items() if key != "self"} ) def __init__(self, name: str, **kwargs): + """Initialize the behavior.""" super().__init__(name=name, **kwargs) self.node: Optional[rclpy.node.Node] = None self._pin_model: Optional[pin.Model] = None @@ -80,7 +94,7 @@ def __init__(self, name: str, **kwargs): @override def setup(self, **kwargs): - """Gets the ROS2 node.""" + """Get the ROS2 node from kwargs.""" try: self.node = kwargs["node"] except KeyError as e: @@ -88,15 +102,8 @@ def setup(self, **kwargs): f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" ) - @override - def initialise(self) -> None: - """Reads Pinocchio model and other static info from blackboard.""" - self.logger.debug(f"[{self.name}] Initializing.") - self.blackboard_set("is_leveling_path_feasible", None) - self._pinocchio_ready = self._get_pinocchio_essentials_from_blackboard() - def _get_pinocchio_essentials_from_blackboard(self) -> bool: - """Reads Pinocchio model, data, and relevant IDs/names from blackboard.""" + """Read Pinocchio model, data, and relevant IDs/names from blackboard.""" if self._pinocchio_ready: return True try: @@ -106,6 +113,24 @@ def _get_pinocchio_essentials_from_blackboard(self) -> bool: self._jaco_ee_frame_id_pin = self.blackboard_get("jaco_ee_frame_id_pin") self._pitch_limits_rad = self.blackboard_get("articutool_pitch_limits_rad") self._roll_limits_rad = self.blackboard_get("articutool_roll_limits_rad") + + if not all( + [ + isinstance(self._pin_model, pin.Model), + isinstance(self._pin_data, pin.Data), + isinstance(self._jaco_joint_names_pin, list), + isinstance(self._jaco_ee_frame_id_pin, int), + isinstance(self._pitch_limits_rad, tuple) + and len(self._pitch_limits_rad) == 2, + isinstance(self._roll_limits_rad, tuple) + and len(self._roll_limits_rad) == 2, + ] + ): + self.logger.error( + f"[{self.name}] One or more Pinocchio-related inputs from blackboard are invalid." + ) + return False + self._pinocchio_ready = True return True except KeyError as e: self.logger.error( @@ -113,51 +138,109 @@ def _get_pinocchio_essentials_from_blackboard(self) -> bool: ) return False + def _normalize_angle(self, angle: float) -> float: + """Normalize an angle to the range [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + + def _solve_articutool_ik_for_leveling( + self, target_y_axis_in_atool_base: np.ndarray + ) -> List[Tuple[float, float]]: + """ + Analytical IK solver based on the corrected kinematic model. + Solves for Articutool (pitch, roll) to align its tool_tip Y-axis + with the given target vector expressed in the Articutool's base frame. + """ + vx, vy, vz = target_y_axis_in_atool_base + solutions: List[Tuple[float, float]] = [] + + # From sin(theta_r) = -vx (as per corrected model) + asin_arg_for_tr = -vx + if not (-1.0 - self.EPSILON <= asin_arg_for_tr <= 1.0 + self.EPSILON): + return [] # No real solution for theta_r + + asin_arg_for_tr_clipped = np.clip(asin_arg_for_tr, -1.0, 1.0) + theta_r_sol1 = math.asin(asin_arg_for_tr_clipped) + theta_r_sol2 = self._normalize_angle(math.pi - theta_r_sol1) + + candidate_thetas_r = [theta_r_sol1] + if not math.isclose(theta_r_sol1, theta_r_sol2, abs_tol=self.EPSILON): + candidate_thetas_r.append(theta_r_sol2) + + for theta_r in candidate_thetas_r: + cos_theta_r = math.cos(theta_r) + # Handle singularity when cos(theta_r) is near zero + if math.isclose(cos_theta_r, 0.0, abs_tol=self.EPSILON): + # If singular, vy and vz must also be near zero for a solution to exist. + if math.isclose(vy, 0.0, abs_tol=self.EPSILON) and math.isclose( + vz, 0.0, abs_tol=self.EPSILON + ): + # theta_p is indeterminate, choose a convention (e.g., 0) + solutions.append((0.0, self._normalize_angle(theta_r))) + continue + + # Standard case: theta_p = atan2(vz, vy) as per corrected model + theta_p_sol = math.atan2(vz, vy) + solutions.append( + (self._normalize_angle(theta_p_sol), self._normalize_angle(theta_r)) + ) + return solutions + def _get_jaco_ee_poses_from_trajectory( self, trajectory_input: Union[RobotTrajectory, JointTrajectory] ) -> Optional[List[Pose]]: - """Extracts or computes Jaco end-effector poses from a trajectory message.""" - if not self._pinocchio_ready: - return None - + """Extract or compute Jaco EE poses from a trajectory message via FK.""" jaco_ee_world_poses: List[Pose] = [] - # Simplified logic: Assumes trajectory is a JointTrajectory for the Jaco arm. - # A more robust version would handle RobotTrajectory and other cases like in the original file. - if not isinstance(trajectory_input, JointTrajectory): + + # Determine the source of joint trajectory points + if isinstance(trajectory_input, RobotTrajectory): + # This implementation assumes the trajectory is defined in joint space. + # A multi_dof_joint_trajectory (Cartesian) is not used for FK. + if trajectory_input.multi_dof_joint_trajectory.points: + self.logger.error( + f"[{self.name}] Multi-DOF trajectories are not supported for FK-based checks." + ) + return None + jt = trajectory_input.joint_trajectory + elif isinstance(trajectory_input, JointTrajectory): + jt = trajectory_input + else: self.logger.error( - f"[{self.name}] This simplified checker only supports JointTrajectory input." + f"[{self.name}] Input 'jaco_trajectory' has unexpected type: {type(trajectory_input)}." ) return None - jt_points = trajectory_input.points - jt_joint_names = trajectory_input.joint_names + # Perform Forward Kinematics for each joint trajectory point q_robot = pin.neutral(self._pin_model) - - for point_data in jt_points: - temp_jaco_map = { - name: point_data.positions[i] for i, name in enumerate(jt_joint_names) + for point in jt.points: + # Map positions from trajectory to the correct joints in Pinocchio model + # This assumes jt.joint_names corresponds to the Jaco arm joints + traj_joint_map = { + name: point.positions[i] for i, name in enumerate(jt.joint_names) } - for j_name_pin in self._jaco_joint_names_pin: - if j_name_pin in temp_jaco_map: + if j_name_pin in traj_joint_map: joint_id = self._pin_model.getJointId(j_name_pin) joint_obj = self._pin_model.joints[joint_id] - theta = temp_jaco_map[j_name_pin] - if joint_obj.nq == 2: # Revolute + theta = traj_joint_map[j_name_pin] + + # Assuming revolute joints for Jaco. Update q based on Pinocchio's model. + if ( + joint_obj.nq == 2 and joint_obj.nv == 1 + ): # Revolute joint represented by cos/sin q_robot[joint_obj.idx_q] = math.cos(theta) q_robot[joint_obj.idx_q + 1] = math.sin(theta) + elif ( + joint_obj.nq == 1 and joint_obj.nv == 1 + ): # Revolute or Prismatic + q_robot[joint_obj.idx_q] = theta pin.forwardKinematics(self._pin_model, self._pin_data, q_robot) pin.updateFramePlacements(self._pin_model, self._pin_data) - jaco_ee_transform_pin: pin.SE3 = self._pin_data.oMf[ - self._jaco_ee_frame_id_pin - ] + ee_transform: pin.SE3 = self._pin_data.oMf[self._jaco_ee_frame_id_pin] pose = Pose() - pose.position.x, pose.position.y, pose.position.z = ( - jaco_ee_transform_pin.translation - ) - quat_xyzw = Rotation.from_matrix(jaco_ee_transform_pin.rotation).as_quat() + pose.position.x, pose.position.y, pose.position.z = ee_transform.translation + quat_xyzw = Rotation.from_matrix(ee_transform.rotation).as_quat() ( pose.orientation.x, pose.orientation.y, @@ -168,75 +251,41 @@ def _get_jaco_ee_poses_from_trajectory( return jaco_ee_world_poses - def _normalize_angle(self, angle: float) -> float: - """Normalize angle to be within [-pi, pi].""" - return (angle + math.pi) % (2 * math.pi) - math.pi - - def _solve_articutool_ik( - self, target_y_in_atool_base: np.ndarray - ) -> List[np.ndarray]: - """ - Solves the analytical IK for the Articutool to achieve a level orientation. - `target_y_in_atool_base` is the desired "up" vector of the tool tip, expressed in the Articutool's base frame. - """ - vx, vy, vz = target_y_in_atool_base - solutions = [] - - # Solve for theta_r - asin_arg = -vx - if not (-1.0 - self.EPSILON <= asin_arg <= 1.0 + self.EPSILON): - return [] - - theta_r_sol1 = math.asin(np.clip(asin_arg, -1.0, 1.0)) - theta_r_sol2 = self._normalize_angle(math.pi - theta_r_sol1) - - candidate_thetas_r = { - self._normalize_angle(theta_r_sol1), - self._normalize_angle(theta_r_sol2), - } - - # Solve for theta_p for each valid theta_r - for theta_r in candidate_thetas_r: - cos_theta_r = math.cos(theta_r) - if abs(cos_theta_r) > self.EPSILON: - theta_p = math.atan2(vz, vy) - solutions.append(np.array([self._normalize_angle(theta_p), theta_r])) - - return solutions - @override def update(self) -> Status: - if not self.node or not self._pinocchio_ready: - self.feedback_message = "Node or Pinocchio model not ready." + """Execute the behavior's logic.""" + if not self.node or not self._get_pinocchio_essentials_from_blackboard(): + self.feedback_message = "Behavior not properly initialized." + self.blackboard_set("articutool_is_leveling_feasible", False) return Status.FAILURE try: trajectory_input = self.blackboard_get("jaco_trajectory") - num_points_to_check = self.blackboard_get("num_trajectory_points_to_check") + num_points = self.blackboard_get("num_trajectory_points_to_check") jaco_ee_poses = self._get_jaco_ee_poses_from_trajectory(trajectory_input) if jaco_ee_poses is None: - self.feedback_message = ( - "Could not extract Jaco EE poses from trajectory." - ) + self.feedback_message = "Failed to get Jaco EE poses from trajectory." + self.blackboard_set("articutool_is_leveling_feasible", False) return Status.FAILURE if not jaco_ee_poses: self.logger.warn( - f"[{self.name}] Trajectory has no waypoints. Assuming feasible." + f"[{self.name}] Trajectory is empty. Assuming feasible." ) - self.blackboard_set("is_leveling_path_feasible", True) + self.blackboard_set("articutool_is_leveling_feasible", True) return Status.SUCCESS - indices_to_check = np.linspace( - 0, len(jaco_ee_poses) - 1, num_points_to_check, dtype=int + # Select a subset of points to check for efficiency + indices = ( + np.linspace(0, len(jaco_ee_poses) - 1, num_points, dtype=int) + if num_points < len(jaco_ee_poses) + else range(len(jaco_ee_poses)) ) - last_valid_solution = None - - for traj_idx in indices_to_check: - ee_pose = jaco_ee_poses[traj_idx] + for idx in indices: + ee_pose: Pose = jaco_ee_poses[idx] R_World_JacoEE = Rotation.from_quat( [ ee_pose.orientation.x, @@ -246,55 +295,59 @@ def update(self) -> Status: ] ) - # Transform world "up" vector into the Jaco EE's local frame + # Transform the world "up" vector into the Articutool's base frame target_y_in_atool_base = R_World_JacoEE.inv().apply( - self.WORLD_UP_VECTOR + self.WORLD_Z_UP_VECTOR ) - # Get all IK solutions for this orientation - ik_solutions = self._solve_articutool_ik(target_y_in_atool_base) + ik_solutions = self._solve_articutool_ik_for_leveling( + target_y_in_atool_base + ) - # Filter for solutions that are within joint limits - valid_solutions = [ - sol + # Check if any solution is within joint limits + is_feasible_at_point = any( + self._pitch_limits_rad[0] - self.EPSILON + <= sol[0] + <= self._pitch_limits_rad[1] + self.EPSILON + and self._roll_limits_rad[0] - self.EPSILON + <= sol[1] + <= self._roll_limits_rad[1] + self.EPSILON for sol in ik_solutions - if ( - self._pitch_limits_rad[0] <= sol[0] <= self._pitch_limits_rad[1] - and self._roll_limits_rad[0] - <= sol[1] - <= self._roll_limits_rad[1] - ) - ] + ) - if not valid_solutions: - self.feedback_message = f"Infeasible: No valid IK solution at trajectory point {traj_idx}." + if not is_feasible_at_point: + self.feedback_message = f"Path is infeasible. No leveling solution at trajectory point {idx}." self.logger.warn(f"[{self.name}] {self.feedback_message}") - self.blackboard_set("is_leveling_path_feasible", False) + self.blackboard_set("articutool_is_leveling_feasible", False) return Status.FAILURE - # Select the best solution based on continuity - if last_valid_solution is None: - chosen_solution = valid_solutions[0] - else: - # Subsequent points: choose solution closest to the previous chosen solution - distances = [ - np.linalg.norm(sol - last_valid_solution) - for sol in valid_solutions - ] - chosen_solution = valid_solutions[np.argmin(distances)] - - last_valid_solution = chosen_solution - - self.feedback_message = "Path is feasible for continuous leveling." + # If all checked points are feasible + self.feedback_message = ( + "Articutool can maintain leveling throughout the trajectory." + ) self.logger.info(f"[{self.name}] {self.feedback_message}") - self.blackboard_set("is_leveling_path_feasible", True) + self.blackboard_set("articutool_is_leveling_feasible", True) return Status.SUCCESS + except KeyError as e: + self.feedback_message = f"Blackboard key error: {e}" + self.logger.error(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_is_leveling_feasible", False) + return Status.FAILURE except Exception as e: - self.feedback_message = f"Unexpected error during feasibility check: {e}" + self.feedback_message = f"Unexpected error: {e}" self.logger.error(f"[{self.name}] {self.feedback_message}") + self.blackboard_set("articutool_is_leveling_feasible", False) return Status.FAILURE @override def terminate(self, new_status: Status) -> None: + """Log termination status.""" self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") + + @override + def initialise(self) -> None: + """Reset blackboard output on initialization.""" + self.logger.debug(f"[{self.name}] Initializing.") + self.blackboard_set("articutool_is_leveling_feasible", None) + # Do not reset pinocchio_ready here to avoid re-reading on every tick diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 21b9f235..5c0043de 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -275,7 +275,7 @@ def create_tree( "num_trajectory_points_to_check": 20, }, outputs={ - "is_leveling_path_feasible": BlackboardKey( + "articutool_is_leveling_feasible": BlackboardKey( "articutool_can_maintain_leveling" ) }, 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 36283e7a..6440c3e3 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 @@ -254,7 +254,7 @@ def create_tree( "num_trajectory_points_to_check": 20, }, outputs={ - "is_leveling_path_feasible": BlackboardKey( + "articutool_is_leveling_feasible": BlackboardKey( "articutool_can_maintain_leveling" ) }, From 08667c3bf124ddee373ea77af570bbf83fa3dc07 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 30 Jun 2025 19:05:15 -0700 Subject: [PATCH 125/238] Rename articutool screen session --- start.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/start.py b/start.py index 411f5705..3f568c59 100755 --- a/start.py +++ b/start.py @@ -286,7 +286,7 @@ async def main(args: argparse.Namespace, pwd: str) -> None: "camera": [ "ssh nano@nano -t './start_nano.sh'", ], - "articutool_stack": [ + "articutool": [ # This command chains several Docker commands on the remote RPi (babbage) # The -t flag for SSH allocates a pseudo-terminal, which is often necessary # for interactive docker commands and proper signal handling. From a8d5dc89ce68553217504f5bdfe275865e80d93e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 7 Jul 2025 20:28:59 -0700 Subject: [PATCH 126/238] Adjust granular scooping action --- .../config/acquisition_library.yaml | 22 +++++++++---------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index 9723479b..7d5127a7 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -317,21 +317,21 @@ actions: # Granular scoop with spoon - ext_angular: [0.0, 0.0, 0.0] ext_duration: 0.1 - ext_force: 50.0 - ext_linear: [0.0, 0.0, 0.04] + ext_force: 5.0 + ext_linear: [0.0, 0.0, 0.0] ext_torque: 4.0 grasp_angular: [0.0, 0.0, 0.0] - grasp_duration: 0.6 - grasp_force: 15.0 - grasp_linear: [0.1, 0.0, 0.0] + grasp_duration: 1.0 + grasp_force: 5.0 + grasp_linear: [-0.05, 0.0, 0.0] grasp_torque: 4.0 - pre_force: 20.0 + pre_force: 5.0 pre_offset: + - -0.035 - 0.0 - - 0.0 - - 0.0 + - -0.02 pre_pos: - - 0.0 + - -0.035 - 0.0 - 1.0 pre_quat: @@ -342,6 +342,6 @@ actions: pre_torque: 4.0 post_move_into_primitive_name: "NONE" post_move_into_primitive_params: [] - post_acquisition_primitive_name: "VIBRATE_ROLL" - post_acquisition_primitive_params: [10.0, 0.1, 1.0] # [frequency_hz, amplitude_rad, duration_sec] + post_acquisition_primitive_name: "SETTLE_TOSS" + post_acquisition_primitive_params: [15.0, 1.0, 4.0] # [tilt_angle_deg, tilt_speed_rps, flick_speed_rps] align_to_robot_base: True From dd76d2d08b7b57e0db05cc5052aadb1c2f5b6695 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 8 Jul 2025 17:49:06 -0700 Subject: [PATCH 127/238] Fix IK joint constraints --- .../ada_feeding/behaviors/moveit2/moveit2_compute_ik.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py index 111be0b8..dfe21077 100644 --- a/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py +++ b/ada_feeding/ada_feeding/behaviors/moveit2/moveit2_compute_ik.py @@ -196,12 +196,12 @@ def update(self) -> py_trees.common.Status: if constraint_type == MoveIt2ConstraintType.JOINT: # Assumes wrapper has create_joint_constraint(**kwargs) -> JointConstraint constraint_obj = ( - self.moveit2_obj.create_joint_constraint( + self.moveit2_obj.create_joint_constraints( **constraint_kwargs ) ) if constraint_obj: - ik_constraints_msg.joint_constraints.append( + ik_constraints_msg.joint_constraints.extend( constraint_obj ) elif constraint_type == MoveIt2ConstraintType.POSITION: From 96d6bb7454dcf44cf0d826da236ba166f2d21c58 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 8 Jul 2025 18:29:41 -0700 Subject: [PATCH 128/238] Add behaviors for ExtractPoseFromTransformStamped and OffsetPositionFromPose --- .../ada_feeding/behaviors/state/__init__.py | 6 + .../extract_pose_from_transform_stamped.py | 96 ++++++++++++++++ .../state/offset_position_from_pose.py | 105 ++++++++++++++++++ 3 files changed, 207 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/state/extract_pose_from_transform_stamped.py create mode 100644 ada_feeding/ada_feeding/behaviors/state/offset_position_from_pose.py diff --git a/ada_feeding/ada_feeding/behaviors/state/__init__.py b/ada_feeding/ada_feeding/behaviors/state/__init__.py index 03e39e7b..b5084a04 100644 --- a/ada_feeding/ada_feeding/behaviors/state/__init__.py +++ b/ada_feeding/ada_feeding/behaviors/state/__init__.py @@ -32,3 +32,9 @@ from .check_articutool_path_leveling_feasibility import ( CheckArticutoolPathLevelingFeasibility, ) +from .offset_position_from_pose import ( + OffsetPositionFromPose, +) +from .extract_pose_from_transform_stamped import ( + ExtractPoseFromTransformStamped, +) diff --git a/ada_feeding/ada_feeding/behaviors/state/extract_pose_from_transform_stamped.py b/ada_feeding/ada_feeding/behaviors/state/extract_pose_from_transform_stamped.py new file mode 100644 index 00000000..1f13f791 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/extract_pose_from_transform_stamped.py @@ -0,0 +1,96 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +Defines the ExtractPoseFromTransformStamped behavior. +""" + +# Standard imports +from typing import Union, Optional + +# Third-party imports +from geometry_msgs.msg import Pose, TransformStamped +from overrides import override +import py_trees +from py_trees.common import Status + +# Local imports +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class ExtractPoseFromTransformStamped(BlackboardBehavior): + """ + Extracts the geometry_msgs/Pose from a geometry_msgs/TransformStamped message. + + This is a utility behavior to bridge components that output TransformStamped + (like a TF lookup) with components that expect a Pose as input. + """ + + def blackboard_inputs( + self, + input_transform_stamped: Union[BlackboardKey, TransformStamped], + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + input_transform_stamped: The source TransformStamped message. + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + output_pose: Optional[BlackboardKey], # -> Optional[Pose] + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + output_pose: The extracted Pose message. + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def update(self) -> Status: + """ + Performs the extraction and writes the result to the blackboard. + """ + try: + transform_stamped_in = self.blackboard_get("input_transform_stamped") + + if not isinstance(transform_stamped_in, TransformStamped): + self.logger.error( + f"[{self.name}] Input 'input_transform_stamped' is not a " + f"TransformStamped, but {type(transform_stamped_in)}." + ) + return Status.FAILURE + + # The transform contains a translation (Vector3) and rotation (Quaternion) + # which together form a Pose. + extracted_pose = Pose() + extracted_pose.position.x = transform_stamped_in.transform.translation.x + extracted_pose.position.y = transform_stamped_in.transform.translation.y + extracted_pose.position.z = transform_stamped_in.transform.translation.z + extracted_pose.orientation = transform_stamped_in.transform.rotation + + self.blackboard_set("output_pose", extracted_pose) + + self.logger.info( + f"[{self.name}] Successfully extracted Pose from TransformStamped." + ) + return Status.SUCCESS + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error: {e}") + return Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Unexpected error: {e}") + return Status.FAILURE diff --git a/ada_feeding/ada_feeding/behaviors/state/offset_position_from_pose.py b/ada_feeding/ada_feeding/behaviors/state/offset_position_from_pose.py new file mode 100644 index 00000000..0eb89e70 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/offset_position_from_pose.py @@ -0,0 +1,105 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +Defines the OffsetPositionFromPose behavior, which calculates a new 3D point +by applying a vector offset to the position component of an input pose. +""" + +# Standard imports +from typing import Union, Optional + +# Third-party imports +from geometry_msgs.msg import Pose, PoseStamped, Point, Vector3 +import numpy as np +from overrides import override +import py_trees +from py_trees.common import Status +import ros2_numpy + +# Local imports +from ada_feeding.helpers import BlackboardKey +from ada_feeding.behaviors import BlackboardBehavior + + +class OffsetPositionFromPose(BlackboardBehavior): + """ + Calculates a new position by applying a Vector3 offset to the position + of an input Pose or PoseStamped. This is useful for creating a target + point that is relative to another object's frame. + """ + + def blackboard_inputs( + self, + input_pose: Union[BlackboardKey, Pose, PoseStamped], + offset: Union[BlackboardKey, Vector3], + ) -> None: + """ + Blackboard Inputs + + Parameters + ---------- + input_pose: The source pose from which to take the initial position. + offset: The Vector3 offset to add to the pose's position. + """ + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + output_position: Optional[BlackboardKey], # -> Optional[Point] + ) -> None: + """ + Blackboard Outputs + + Parameters + ---------- + output_position: The resulting position (as a geometry_msgs/Point) + after applying the offset. + """ + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + @override + def update(self) -> Status: + """ + Performs the position extraction, offset addition, and writes + the result to the blackboard. + """ + try: + pose_in = self.blackboard_get("input_pose") + offset_in = self.blackboard_get("offset") + + # Extract the position part of the input pose + if isinstance(pose_in, PoseStamped): + position_np = ros2_numpy.numpify(pose_in.pose.position) + elif isinstance(pose_in, Pose): + position_np = ros2_numpy.numpify(pose_in.position) + else: + self.logger.error( + f"[{self.name}] Input 'input_pose' is not a Pose or PoseStamped, but {type(pose_in)}." + ) + return Status.FAILURE + + # Convert offset to numpy array + offset_np = ros2_numpy.numpify(offset_in) + + # Calculate the new position + new_position_np = position_np + offset_np + + # Convert back to a ROS message and write to blackboard + new_position_msg = ros2_numpy.msgify(Point, new_position_np) + self.blackboard_set("output_position", new_position_msg) + + self.logger.info(f"[{self.name}] Successfully calculated offset position.") + return Status.SUCCESS + + except KeyError as e: + self.logger.error(f"[{self.name}] Blackboard key error: {e}") + return Status.FAILURE + except Exception as e: + self.logger.error(f"[{self.name}] Unexpected error: {e}") + return Status.FAILURE From 6e5c0f630664c5c7ec467a9f01fe6ee41e1345b5 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 8 Jul 2025 18:42:13 -0700 Subject: [PATCH 129/238] Reduce Jaco joint 2 max path length when planning for MoveAbove pose. The purpose of this is to discourage trajectories that tend to jam the arm's elbow into the table --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 5c0043de..7109ca1d 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -189,7 +189,7 @@ def create_tree( # to reduce swivels. max_path_len_joint = { "j2n6s200_joint_1": np.pi * 5.0 / 6.0, - "j2n6s200_joint_2": np.pi / 2.0, + "j2n6s200_joint_2": np.pi / 4.0, } # Get the base lin to publish servo commands in From f64dd357178e10bad7f6ea53d343a9191a2cc5f6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 8 Jul 2025 18:50:48 -0700 Subject: [PATCH 130/238] Add Articutool joint constraints for MoveAbove motion to encourage IK to choose a configuration where the Articutool joints are near their zero configuration. This also helps avoid trajectories that tend to jam the arm in weird ways --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 7109ca1d..496111c9 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -845,6 +845,18 @@ def move_above_sequence() -> py_trees.behaviour.Behaviour: ) }, ), + MoveIt2JointConstraint( + name="CreateMoveAboveArticutoolJointConstraint", + ns=name, + inputs={ + "joint_names": ["atool_joint1", "atool_joint2"], + "joint_positions": [0.0, 0.0], + "tolerance": 0.785, + }, + outputs={ + "constraints": BlackboardKey("move_above_ik_constraints") + }, + ), MoveIt2ComputeIK( name="ComputeIKForMoveAbove", ns=name, @@ -856,6 +868,7 @@ def move_above_sequence() -> py_trees.behaviour.Behaviour: "start_joint_state": BlackboardKey( "current_full_joint_state_for_ik" ), + "constraints": BlackboardKey("move_above_ik_constraints"), }, outputs={ "ik_solution_joint_state": BlackboardKey( From 9eac74960e781b4f2efd045b15cba8d90dadaafb Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 8 Jul 2025 19:19:49 -0700 Subject: [PATCH 131/238] Revert to using the VIBRATE_ROLL primitive for the spoon granular scooping action. This is because the SETTLE_TOSS primitive uses the pitch joint to flick the food into a stable position, which, while effective, tends to exceed the Articutool's joint limits in practice --- ada_feeding_action_select/config/acquisition_library.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index 7d5127a7..e2fd4573 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -342,6 +342,6 @@ actions: pre_torque: 4.0 post_move_into_primitive_name: "NONE" post_move_into_primitive_params: [] - post_acquisition_primitive_name: "SETTLE_TOSS" - post_acquisition_primitive_params: [15.0, 1.0, 4.0] # [tilt_angle_deg, tilt_speed_rps, flick_speed_rps] + post_acquisition_primitive_name: "VIBRATE_ROLL" + post_acquisition_primitive_params: [5.0, 0.1, 1.0] # [frequency_hz, amplitude_rad, duration_sec] align_to_robot_base: True From 533a59f6dd9764f59dece96fa5264a294dd3a786 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 8 Jul 2025 19:21:37 -0700 Subject: [PATCH 132/238] Significantly increase the number of retries for the MoveAbove and MoveInto planning sequences. This is a brute force approach that seems to work quite well. The premise is that each retry has the potential to generate a trajectory which must then also pass a post-feasibility check. Increasing the number of retries directly increases the likelihood that at least one such trajectory passes the feasibility check, albeit in a non-graceful manner --- ada_feeding/ada_feeding/trees/acquire_food_tree.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/ada_feeding/trees/acquire_food_tree.py b/ada_feeding/ada_feeding/trees/acquire_food_tree.py index 496111c9..87e9ac2a 100644 --- a/ada_feeding/ada_feeding/trees/acquire_food_tree.py +++ b/ada_feeding/ada_feeding/trees/acquire_food_tree.py @@ -1379,7 +1379,7 @@ def move_into_sequence() -> py_trees.behaviour.Behaviour: ), py_trees.decorators.Retry( name="PlanAcquisitionSequenceRetry", - num_failures=3, + num_failures=10, child=py_trees.composites.Sequence( name="PlanAcquisitionSequence", memory=True, From 6ebb0505b42fc82f7acd1c4739f1418b459904c4 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 14 Jul 2025 19:28:05 -0700 Subject: [PATCH 133/238] Adjust force/torque thresholds for granular scoop so that it can work with more shallow bowls where the tool needs to scrape along the bottom of the bowl rather than sift through granules. --- .../config/acquisition_library.yaml | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index e2fd4573..56da6158 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -317,15 +317,15 @@ actions: # Granular scoop with spoon - ext_angular: [0.0, 0.0, 0.0] ext_duration: 0.1 - ext_force: 5.0 + ext_force: 50.0 ext_linear: [0.0, 0.0, 0.0] - ext_torque: 4.0 + ext_torque: 8.0 grasp_angular: [0.0, 0.0, 0.0] grasp_duration: 1.0 - grasp_force: 5.0 + grasp_force: 70.0 grasp_linear: [-0.05, 0.0, 0.0] - grasp_torque: 4.0 - pre_force: 5.0 + grasp_torque: 15.0 + pre_force: 20.0 pre_offset: - -0.035 - 0.0 @@ -339,7 +339,7 @@ actions: - 0.6508167 - 0.6517596 - 0.2747348 - pre_torque: 4.0 + pre_torque: 8.0 post_move_into_primitive_name: "NONE" post_move_into_primitive_params: [] post_acquisition_primitive_name: "VIBRATE_ROLL" From e8c28b72400c8cb53b906181930599ef5f682305 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 15 Jul 2025 18:39:50 -0700 Subject: [PATCH 134/238] Add behavior to perform dynamic feasibility check on trajectories generated for the Jaco arm that indicate whether the Articutool is dynamically capable of maintaining its leveling orientation --- ...eck_articutool_path_dynamic_feasibility.py | 318 ++++++++++++++++++ 1 file changed, 318 insertions(+) create mode 100644 ada_feeding/ada_feeding/behaviors/state/check_articutool_path_dynamic_feasibility.py diff --git a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_dynamic_feasibility.py b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_dynamic_feasibility.py new file mode 100644 index 00000000..f4da4ab0 --- /dev/null +++ b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_dynamic_feasibility.py @@ -0,0 +1,318 @@ +# -*- coding: utf-8 -*- +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This module defines the CheckArticutoolPathDynamicFeasibility behavior. + +This behavior extends the kinematic feasibility check by also verifying if the +Articutool has the dynamic capability (i.e., sufficient motor velocity) to +maintain a level orientation throughout a given trajectory of the Jaco arm. +It combines the kinematic check with a dynamic analysis at each waypoint. +""" + +# Standard imports +import math +from typing import Union, Optional, List, Tuple + +# Third-party imports +from geometry_msgs.msg import Pose +from moveit_msgs.msg import RobotTrajectory +from trajectory_msgs.msg import JointTrajectory +import numpy as np +from overrides import override +from scipy.spatial.transform import Rotation +import py_trees +from py_trees.common import Status +import rclpy.node +import pinocchio as pin + +# Local imports +from ada_feeding.behaviors import BlackboardBehavior +from ada_feeding.helpers import BlackboardKey + + +class CheckArticutoolPathDynamicFeasibility(BlackboardBehavior): + """ + Checks if a Jaco trajectory is dynamically feasible for the Articutool. + + For each point along a trajectory, this behavior performs a two-part check: + 1. Kinematic Check: Can the Articutool *be* level at this Jaco pose? + (Same as CheckArticutoolPathLevelingFeasibility). + 2. Dynamic Check: Is the Articutool *fast enough* to counteract the + tilting motion induced by the Jaco arm's movement? + + If both checks pass for all waypoints, it returns SUCCESS. Otherwise, FAILURE. + """ + + EPSILON = 1e-6 + WORLD_Z_UP_VECTOR = np.array([0.0, 0.0, 1.0]) + + def blackboard_inputs( + self, + pinocchio_model: Union[BlackboardKey, pin.Model], + pinocchio_data: Union[BlackboardKey, pin.Data], + jaco_joint_names_pin: Union[BlackboardKey, List[str]], + jaco_vel_indices_pin: Union[BlackboardKey, List[int]], + jaco_ee_frame_id_pin: Union[BlackboardKey, int], + jaco_trajectory: Union[BlackboardKey, RobotTrajectory, JointTrajectory], + articutool_pitch_limits_rad: Union[BlackboardKey, Tuple[float, float]], + articutool_roll_limits_rad: Union[BlackboardKey, Tuple[float, float]], + articutool_max_joint_velocity: Union[BlackboardKey, float], + num_trajectory_points_to_check: Union[BlackboardKey, int] = 10, + ) -> None: + """Define blackboard inputs.""" + super().blackboard_inputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def blackboard_outputs( + self, + articutool_is_dynamic_feasible: Optional[BlackboardKey], + ) -> None: + """Define blackboard outputs.""" + super().blackboard_outputs( + **{key: value for key, value in locals().items() if key != "self"} + ) + + def __init__(self, name: str, **kwargs): + """Initialize the behavior.""" + super().__init__(name=name, **kwargs) + self.node: Optional[rclpy.node.Node] = None + self._pin_model: Optional[pin.Model] = None + self._pin_data: Optional[pin.Data] = None + self._jaco_joint_names_pin: Optional[List[str]] = None + self._jaco_vel_indices_pin: Optional[List[int]] = None + self._jaco_ee_frame_id_pin: Optional[int] = None + self._pitch_limits_rad: Optional[Tuple[float, float]] = None + self._roll_limits_rad: Optional[Tuple[float, float]] = None + self._max_atool_vel: Optional[float] = None + self._pinocchio_ready = False + + @override + def setup(self, **kwargs): + """Get the ROS2 node from kwargs.""" + try: + self.node = kwargs["node"] + except KeyError as e: + self.logger.error( + f"[{self.name}] Behaviour expects 'node' in setup kwargs. {e}" + ) + + def _get_pinocchio_essentials_from_blackboard(self) -> bool: + """Read Pinocchio model, data, and relevant IDs/names from blackboard.""" + if self._pinocchio_ready: + return True + try: + self._pin_model = self.blackboard_get("pinocchio_model") + self._pin_data = self.blackboard_get("pinocchio_data") + self._jaco_joint_names_pin = self.blackboard_get("jaco_joint_names_pin") + self._jaco_vel_indices_pin = self.blackboard_get("jaco_vel_indices_pin") + self._jaco_ee_frame_id_pin = self.blackboard_get("jaco_ee_frame_id_pin") + self._pitch_limits_rad = self.blackboard_get("articutool_pitch_limits_rad") + self._roll_limits_rad = self.blackboard_get("articutool_roll_limits_rad") + self._max_atool_vel = self.blackboard_get("articutool_max_joint_velocity") + self._pinocchio_ready = True + return True + except KeyError as e: + self.logger.error( + f"[{self.name}] Failed to get required info from blackboard: {e}." + ) + return False + + def _normalize_angle(self, angle: float) -> float: + """Normalize an angle to the range [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + + def _solve_articutool_ik_for_leveling( + self, target_y_axis_in_atool_base: np.ndarray + ) -> List[Tuple[float, float]]: + """ + Analytical IK solver based on the reconciled kinematic model. + Solves for Articutool (pitch, roll) to align its tool_tip Y-axis + with the given target vector expressed in the Articutool's base frame. + """ + vx, vy, vz = target_y_axis_in_atool_base + solutions: List[Tuple[float, float]] = [] + + asin_arg_for_tr = -vx + if not (-1.0 - self.EPSILON <= asin_arg_for_tr <= 1.0 + self.EPSILON): + return [] + + asin_arg_for_tr_clipped = np.clip(asin_arg_for_tr, -1.0, 1.0) + theta_r_sol1 = math.asin(asin_arg_for_tr_clipped) + theta_r_sol2 = self._normalize_angle(math.pi - theta_r_sol1) + + candidate_thetas_r = [theta_r_sol1] + if not math.isclose(theta_r_sol1, theta_r_sol2, abs_tol=self.EPSILON): + candidate_thetas_r.append(theta_r_sol2) + + for theta_r in candidate_thetas_r: + cos_theta_r = math.cos(theta_r) + if math.isclose(cos_theta_r, 0.0, abs_tol=self.EPSILON): + if math.isclose(vy, 0.0, abs_tol=self.EPSILON) and math.isclose( + vz, 0.0, abs_tol=self.EPSILON + ): + solutions.append((0.0, self._normalize_angle(theta_r))) + continue + + theta_p_sol = math.atan2(vz, vy) + solutions.append( + (self._normalize_angle(theta_p_sol), self._normalize_angle(theta_r)) + ) + return solutions + + def _compute_articutool_jacobian(self, theta_p: float, theta_r: float) -> np.ndarray: + """ + Computes the 3x2 analytical Jacobian for the Articutool leveling task. + This must be derived from the same FK model that the IK inverts. + """ + cp, sp = math.cos(theta_p), math.sin(theta_p) + cr, sr = math.cos(theta_r), math.sin(theta_r) + + # Partial derivatives of y_F0 = [-sr, cp*cr, sp*cr]^T + # d/d(theta_p) + j11 = 0 + j21 = -sp * cr + j31 = cp * cr + + # d/d(theta_r) + j12 = -cr + j22 = -cp * sr + j32 = -sp * sr + + return np.array([[j11, j12], [j21, j22], [j31, j32]]) + + def _get_jaco_trajectory_points( + self, trajectory_input: Union[RobotTrajectory, JointTrajectory] + ) -> Optional[List[Tuple[np.ndarray, np.ndarray]]]: + """Extracts joint positions and velocities from a trajectory message.""" + if isinstance(trajectory_input, RobotTrajectory): + jt = trajectory_input.joint_trajectory + elif isinstance(trajectory_input, JointTrajectory): + jt = trajectory_input + else: + return None + + points = [] + for point in jt.points: + positions = np.array(point.positions) + velocities = np.array(point.velocities) + points.append((positions, velocities)) + return points + + @override + def update(self) -> Status: + """Execute the behavior's logic.""" + if not self.node or not self._get_pinocchio_essentials_from_blackboard(): + self.feedback_message = "Behavior not properly initialized." + self.blackboard_set("articutool_is_dynamic_feasible", False) + return Status.FAILURE + + try: + trajectory_input = self.blackboard_get("jaco_trajectory") + num_points = self.blackboard_get("num_trajectory_points_to_check") + + jaco_points = self._get_jaco_trajectory_points(trajectory_input) + + if not jaco_points: + self.logger.warn(f"[{self.name}] Trajectory is empty. Assuming feasible.") + self.blackboard_set("articutool_is_dynamic_feasible", True) + return Status.SUCCESS + + indices = np.linspace(0, len(jaco_points) - 1, num_points, dtype=int) + + last_valid_atool_q = None + + for idx in indices: + q_jaco, v_jaco = jaco_points[idx] + + # --- KINEMATIC FEASIBILITY CHECK (Prerequisite) --- + + # 1. Get Articutool base orientation from Jaco FK + pin.forwardKinematics(self._pin_model, self._pin_data, q_jaco) + pin.updateFramePlacements(self._pin_model, self._pin_data) + T_world_atool_base = self._pin_data.oMf[self._jaco_ee_frame_id_pin] + R_world_atool_base = Rotation.from_matrix(T_world_atool_base.rotation) + + # 2. Transform world "up" vector to Articutool's base frame + target_y_in_atool_base = R_world_atool_base.inv().apply(self.WORLD_Z_UP_VECTOR) + + # 3. Solve Articutool IK for leveling + ik_solutions = self._solve_articutool_ik_for_leveling(target_y_in_atool_base) + + valid_solutions = [ + sol for sol in ik_solutions + if self._pitch_limits_rad[0] - self.EPSILON <= sol[0] <= self._pitch_limits_rad[1] + self.EPSILON + and self._roll_limits_rad[0] - self.EPSILON <= sol[1] <= self._roll_limits_rad[1] + self.EPSILON + ] + + if not valid_solutions: + self.feedback_message = f"Path is kinematically infeasible at point {idx}." + self.blackboard_set("articutool_is_dynamic_feasible", False) + return Status.FAILURE + + # Choose best solution (e.g., closest to previous) + if last_valid_atool_q is None: + q_atool = valid_solutions[0] + else: + distances = [np.linalg.norm(np.array(sol) - last_valid_atool_q) for sol in valid_solutions] + q_atool = valid_solutions[np.argmin(distances)] + last_valid_atool_q = np.array(q_atool) + + # --- DYNAMIC FEASIBILITY CHECK --- + + # 1. Calculate disturbance velocity from Jaco arm + J_jaco_full = pin.computeFrameJacobian(self._pin_model, self._pin_data, q_jaco, self._jaco_ee_frame_id_pin, pin.ReferenceFrame.WORLD) + v_jaco_full = J_jaco_full @ v_jaco + omega_disturbance_world = v_jaco_full[3:6] # Angular velocity part + + # 2. Transform disturbance to Articutool's local frame + omega_correction_local = -R_world_atool_base.inv().apply(omega_disturbance_world) + + # 3. Compute Articutool's Jacobian at the required configuration + J_atool = self._compute_articutool_jacobian(q_atool[0], q_atool[1]) + + # 4. Solve for required Articutool joint velocities + try: + J_atool_pinv = np.linalg.pinv(J_atool, rcond=1e-4) + q_dot_atool_required = J_atool_pinv @ omega_correction_local + except np.linalg.LinAlgError: + self.feedback_message = f"Articutool Jacobian is singular at point {idx}." + self.blackboard_set("articutool_is_dynamic_feasible", False) + return Status.FAILURE + + # 5. The final check + if np.linalg.norm(q_dot_atool_required) > self._max_atool_vel: + self.feedback_message = ( + f"Path is dynamically infeasible at point {idx}. " + f"Required vel: {np.linalg.norm(q_dot_atool_required):.2f} > " + f"Max vel: {self._max_atool_vel:.2f}" + ) + self.blackboard_set("articutool_is_dynamic_feasible", False) + return Status.FAILURE + + # If all checks pass + self.feedback_message = "Trajectory is dynamically feasible." + self.blackboard_set("articutool_is_dynamic_feasible", True) + return Status.SUCCESS + + except KeyError as e: + self.feedback_message = f"Blackboard key error: {e}" + self.blackboard_set("articutool_is_dynamic_feasible", False) + return Status.FAILURE + except Exception as e: + self.feedback_message = f"Unexpected error: {e}" + self.blackboard_set("articutool_is_dynamic_feasible", False) + return Status.FAILURE + + @override + def terminate(self, new_status: Status) -> None: + """Log termination status.""" + self.logger.debug(f"[{self.name}] Terminating with status {new_status}.") + + @override + def initialise(self) -> None: + """Reset blackboard output on initialization.""" + self.logger.debug(f"[{self.name}] Initializing.") + self.blackboard_set("articutool_is_dynamic_feasible", None) From 183ea4e1bb5ebbbaab2a659f50a7c3fbc6b37ce5 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 19 Jul 2025 11:29:06 -0700 Subject: [PATCH 135/238] Update visualization script --- ada_feeding/scripts/visualize_trajectory.py | 596 ++++++++++---------- 1 file changed, 286 insertions(+), 310 deletions(-) diff --git a/ada_feeding/scripts/visualize_trajectory.py b/ada_feeding/scripts/visualize_trajectory.py index d4570ace..9f44999d 100755 --- a/ada_feeding/scripts/visualize_trajectory.py +++ b/ada_feeding/scripts/visualize_trajectory.py @@ -7,6 +7,18 @@ using Pinocchio and MeshCat. It can load: 1. Enhanced trajectory JSON files (containing Jaco and Articutool waypoints). 2. Single joint state JSON files (representing a sensor_msgs/msg/JointState). + +New features for figure generation: +--snapshot-frames: Enters a mode to navigate only specific keyframes. +--rigid: Sets the initial state of the Articutool's rigidity. +'t' command: Toggles Articutool rigidity on/off interactively. +'s' command: Saves a labeled screenshot of the current view. + +New features for video recording: +'r' command: Starts and stops recording the current view. The user can move + the camera in MeshCat while recording. +'a' command: Now prompts to record the animation to a file. +--fps: Sets the frames per second for the output video. """ # Standard imports @@ -21,10 +33,17 @@ import argparse import time import sys +import math from typing import Optional, Dict, Any, List +# --- MODIFICATION: Added Pillow for image saving and imageio for video --- +# Note: You may need to install imageio and a backend: +# pip install imageio imageio-ffmpeg +from PIL import Image +import imageio + + # Define Articutool joint names as they appear in the Pinocchio model -# (Verify these names from the script's "Pinocchio Model Joint Details" printout) ARTICUTOOL_PITCH_JOINT_NAME = "atool_joint1" ARTICUTOOL_ROLL_JOINT_NAME = "atool_joint2" @@ -82,23 +101,17 @@ def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=prin ] common_workspace_paths = [ - os.path.expanduser("~/ros2_ws/src"), # Common Foxy/Galactic/Humble - os.path.expanduser("~/dev_ws/src"), # Common Iron+ + os.path.expanduser("~/ros2_ws/src"), + os.path.expanduser("~/dev_ws/src"), os.path.expanduser("~/workspace/src"), ] for wsp in common_workspace_paths: if os.path.isdir(wsp) and wsp not in package_dirs: package_dirs.append(wsp) - # Attempt to find the 'src' directory of the current ROS 2 workspace - # This is a heuristic based on typical workspace layouts. current_dir = os.getcwd() - while current_dir != os.path.dirname(current_dir): # Stop at root - if ( - os.path.basename(current_dir) == "install" - or os.path.basename(current_dir) == "build" - or os.path.basename(current_dir) == "log" - ): + while current_dir != os.path.dirname(current_dir): + if os.path.basename(current_dir) in ["install", "build", "log"]: ws_root = os.path.dirname(current_dir) src_path = os.path.join(ws_root, "src") if os.path.isdir(src_path) and src_path not in package_dirs: @@ -107,7 +120,7 @@ def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=prin current_dir = os.path.dirname(current_dir) if package_dirs: - unique_package_dirs = list(set(package_dirs)) # Remove duplicates + unique_package_dirs = list(set(package_dirs)) logger_func(f"Using package_dirs for Pinocchio: {unique_package_dirs}") model = pin.buildModelFromUrdf( temp_urdf_path, package_dirs=unique_package_dirs @@ -126,7 +139,7 @@ def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=prin ) else: logger_func( - "Warning: ROS_PACKAGE_PATH not found or empty, and common workspace paths not found. Mesh loading might fail if using package:// paths." + "Warning: ROS_PACKAGE_PATH not found or empty. Mesh loading might fail." ) model = pin.buildModelFromUrdf(temp_urdf_path) collision_model = pin.buildGeomFromUrdf( @@ -149,59 +162,20 @@ def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=prin os.remove(temp_urdf_path) -def load_enhanced_trajectory_from_json( - filepath: str, logger_func=print -) -> Optional[Dict[str, Any]]: - """Loads the enhanced trajectory data from a JSON file.""" +def load_json_file(filepath: str, logger_func=print) -> Optional[Dict[str, Any]]: + """Loads data from a generic JSON file.""" try: with open(filepath, "r") as f: - traj_data = json.load(f) - logger_func(f"Enhanced trajectory loaded from {filepath}") - if "jaco_joint_names" not in traj_data or "waypoints" not in traj_data: - logger_func( - "Error: Loaded JSON is missing 'jaco_joint_names' or 'waypoints'." - ) - return None - return traj_data + data = json.load(f) + logger_func(f"Successfully loaded JSON from {filepath}") + return data except FileNotFoundError: - logger_func(f"Error: Trajectory file not found at {filepath}") + logger_func(f"Error: File not found at {filepath}") return None except json.JSONDecodeError: logger_func(f"Error: Could not decode JSON from {filepath}") return None - except Exception as e: - logger_func(f"An unexpected error occurred loading trajectory: {e}") - return None - - -def load_joint_state_from_json( - filepath: str, logger_func=print -) -> Optional[Dict[str, Any]]: - """Loads joint state data (expected sensor_msgs/msg/JointState format) from a JSON file.""" - try: - with open(filepath, "r") as f: - joint_state_data = json.load(f) - logger_func(f"Joint state loaded from {filepath}") - if "name" not in joint_state_data or "position" not in joint_state_data: - logger_func( - "Error: Loaded JSON for joint state is missing 'name' or 'position' fields." - ) - return None - if len(joint_state_data["name"]) != len(joint_state_data["position"]): - logger_func( - "Error: Mismatch between number of names and positions in joint state JSON." - ) - return None - return joint_state_data - except FileNotFoundError: - logger_func(f"Error: Joint state file not found at {filepath}") - return None - except json.JSONDecodeError: - logger_func(f"Error: Could not decode JSON from {filepath}") - return None - except Exception as e: - logger_func(f"An unexpected error occurred loading joint state: {e}") - return None + return None def get_pinocchio_joint_info( @@ -211,7 +185,6 @@ def get_pinocchio_joint_info( if model.existJointName(joint_name): joint_id = model.getJointId(joint_name) joint_obj = model.joints[joint_id] - # Consider only joints that contribute to the configuration vector q if joint_obj.idx_q >= 0 and joint_obj.nq > 0: return { "name": joint_name, @@ -223,44 +196,47 @@ def get_pinocchio_joint_info( return None -def set_q_from_joint_state_data( +def set_q_from_waypoint( q_vector: np.ndarray, model: pin.Model, - joint_names: List[str], - joint_positions: List[float], - logger_func=print, + waypoint: Dict[str, Any], + jaco_mappings, + atool_pitch_info, + atool_roll_info, + is_rigid: bool, ): - """Updates Pinocchio q_vector based on names and positions from a JointState-like structure.""" - if len(joint_names) != len(joint_positions): - logger_func( - "Error in set_q_from_joint_state_data: name and position lists have different lengths." - ) - return - - name_to_pos = {name: joint_positions[i] for i, name in enumerate(joint_names)} - - for i in range( - 1, model.njoints - ): # Iterate through Pinocchio model joints (skip universe) - joint_name_in_model = model.names[i] - joint_info = get_pinocchio_joint_info(model, joint_name_in_model) - - if joint_info and joint_name_in_model in name_to_pos: - theta_input = name_to_pos[joint_name_in_model] + """Updates Pinocchio q_vector based on a waypoint from the trajectory data.""" + jaco_positions = waypoint.get("jaco_positions_rad", []) + for k, mapping_info in enumerate(jaco_mappings): + if mapping_info and k < len(jaco_positions): + theta_traj = jaco_positions[k] q_idx_start, nq, nv = ( - joint_info["q_idx_start"], - joint_info["nq"], - joint_info["nv"], + mapping_info["q_idx_start"], + mapping_info["nq"], + mapping_info["nv"], ) + if nq == 1: + q_vector[q_idx_start] = theta_traj + elif nq == 2 and nv == 1: + q_vector[q_idx_start], q_vector[q_idx_start + 1] = ( + np.cos(theta_traj), + np.sin(theta_traj), + ) + + is_feasible = waypoint.get("articutool_waypoint_feasible", False) + if is_rigid or not is_feasible: + pitch_sol, roll_sol = 0.0, 0.0 + else: + pitch_sol = waypoint.get("articutool_pitch_solution_rad", 0.0) + roll_sol = waypoint.get("articutool_roll_solution_rad", 0.0) - if nq == 1: # Typically for prismatic or revolute if not using cos/sin - q_vector[q_idx_start] = theta_input - elif nq == 2 and nv == 1: # Revolute joint with cos/sin representation - q_vector[q_idx_start] = np.cos(theta_input) - q_vector[q_idx_start + 1] = np.sin(theta_input) - # Add other joint types if necessary (e.g., free flyer) - # else: logger_func(f"Joint {joint_name_in_model} has nq={nq}, nv={nv} - unhandled for direct q setting from JointState.") - # else: logger_func(f"Joint {joint_name_in_model} from model not in provided joint state or not actuated.") + for sol, info in [(pitch_sol, atool_pitch_info), (roll_sol, atool_roll_info)]: + if sol is not None and info: + q_idx, nq, nv = info["q_idx_start"], info["nq"], info["nv"] + if nq == 1: + q_vector[q_idx] = sol + elif nq == 2 and nv == 1: + q_vector[q_idx], q_vector[q_idx + 1] = np.cos(sol), np.sin(sol) def main(args): @@ -268,280 +244,280 @@ def main(args): urdf_string = xacro_to_urdf_string(args.xacro_file) if not urdf_string: - print("Exiting due to Xacro processing failure.") return - load_result = load_pinocchio_model_from_urdf_string(urdf_string) - if not load_result or not load_result[0]: - print("Exiting due to Pinocchio model loading failure.") + model, collision_model, visual_model, data = load_pinocchio_model_from_urdf_string( + urdf_string + ) + if not model: return - model, collision_model, visual_model, data = load_result - - if model: - print("\n--- Pinocchio Model Joint Details ---") - for i in range(1, model.njoints): - joint_name_in_model = model.names[i] - joint_obj = model.joints[i] - print( - f"IdxInModel {i}: Name='{joint_name_in_model}', PinocchioType='{joint_obj.shortname()}', " - f"idx_q={joint_obj.idx_q if joint_obj.idx_q >= 0 else 'N/A'}, nq={joint_obj.nq}, " - f"idx_v={joint_obj.idx_v if joint_obj.idx_v >= 0 else 'N/A'}, nv={joint_obj.nv}" - ) - print("-----------------------------------\n") - print("Initializing MeshCat viewer... Waiting for connection.") + print("Initializing MeshCat viewer...") try: visualizer = meshcat.Visualizer().open() print(f"MeshCat viewer URL: {visualizer.url()}") pin_viz = pin.visualize.MeshcatVisualizer(model, collision_model, visual_model) pin_viz.initViewer(viewer=visualizer) - pin_viz.loadViewerModel( - rootNodeName=model.name if model.name else "pinocchio_robot" - ) - print("MeshCat viewer initialized and model loaded.") + pin_viz.loadViewerModel(rootNodeName=model.name or "robot") + print("MeshCat viewer initialized.") except Exception as e: - print(f"Error initializing MeshCat or Pinocchio visualizer: {e}") + print(f"Error initializing MeshCat: {e}") return - q = pin.neutral(model) - print( - f"Neutral configuration q (size {model.nq}): {q.T if model.nq > 0 else 'N/A'}" - ) - - if args.ik_solution_json: - print(f"\n--- Visualizing Single IK Solution from: {args.ik_solution_json} ---") - joint_state_data = load_joint_state_from_json(args.ik_solution_json) - if not joint_state_data: - print("Failed to load IK solution JSON. Exiting.") - return - - # Set q from the loaded joint state - set_q_from_joint_state_data( - q, model, joint_state_data["name"], joint_state_data["position"] - ) - - print(f"Displaying configuration from IK solution: {q.T}") - pin_viz.display(q) - try: - input("Robot displayed. Press Enter to quit.") - except KeyboardInterrupt: - print("\nQuitting by user interrupt.") - return # Exit after displaying single state - - # --- Original Trajectory Visualization Logic --- if not args.trajectory_file: - print( - "No trajectory file or IK solution JSON provided. Displaying neutral pose." - ) - pin_viz.display(q) - try: - input("Neutral pose displayed. Press Enter to quit.") - except KeyboardInterrupt: - print("\nQuitting by user interrupt.") + print("No trajectory file provided. Displaying neutral pose.") + pin_viz.display(pin.neutral(model)) + input("Press Enter to quit.") return - trajectory_data = load_enhanced_trajectory_from_json(args.trajectory_file) - if not trajectory_data: + trajectory_data = load_json_file(args.trajectory_file) + if ( + not trajectory_data + or "jaco_joint_names" not in trajectory_data + or "waypoints" not in trajectory_data + ): print("Failed to load valid trajectory data. Exiting.") return - jaco_joint_names_from_traj = trajectory_data["jaco_joint_names"] - waypoints_data = trajectory_data["waypoints"] - if not waypoints_data: + waypoints = trajectory_data["waypoints"] + if not waypoints: print("Trajectory contains no waypoints. Exiting.") return - jaco_joint_mappings: List[Optional[Dict[str, Any]]] = [ - get_pinocchio_joint_info(model, name) for name in jaco_joint_names_from_traj - ] - articutool_pitch_joint_info = get_pinocchio_joint_info( - model, ARTICUTOOL_PITCH_JOINT_NAME - ) - articutool_roll_joint_info = get_pinocchio_joint_info( - model, ARTICUTOOL_ROLL_JOINT_NAME - ) + if args.output_dir: + os.makedirs(args.output_dir, exist_ok=True) + print(f"Screenshots and videos will be saved to: {args.output_dir}") - if not articutool_pitch_joint_info or not articutool_roll_joint_info: - print("Error: Articutool pitch or roll joint not found in model. Exiting.") - return + jaco_joint_names = trajectory_data["jaco_joint_names"] + jaco_mappings = [get_pinocchio_joint_info(model, name) for name in jaco_joint_names] + articutool_pitch_info = get_pinocchio_joint_info(model, ARTICUTOOL_PITCH_JOINT_NAME) + articutool_roll_info = get_pinocchio_joint_info(model, ARTICUTOOL_ROLL_JOINT_NAME) - print("\nArticutool Joint Mappings:") - print( - f" Pitch ('{ARTICUTOOL_PITCH_JOINT_NAME}'): Maps to q[{articutool_pitch_joint_info['q_idx_start']}], nq={articutool_pitch_joint_info['nq']}" - ) - print( - f" Roll ('{ARTICUTOOL_ROLL_JOINT_NAME}'): Maps to q[{articutool_roll_joint_info['q_idx_start']}], nq={articutool_roll_joint_info['nq']}" - ) + if not all(jaco_mappings) or not articutool_pitch_info or not articutool_roll_info: + print("Error: Could not map all required joints to Pinocchio model.") + return - def set_q_from_enhanced_waypoint(q_vector: np.ndarray, waypoint: Dict[str, Any]): - jaco_positions = waypoint.get("jaco_positions_rad", []) - for k, mapping_info in enumerate(jaco_joint_mappings): - if mapping_info and k < len(jaco_positions): - theta_traj = jaco_positions[k] - q_idx_start, nq, nv = ( - mapping_info["q_idx_start"], - mapping_info["nq"], - mapping_info["nv"], - ) - if nq == 1: - q_vector[q_idx_start] = theta_traj - elif nq == 2 and nv == 1: - q_vector[q_idx_start], q_vector[q_idx_start + 1] = ( - np.cos(theta_traj), - np.sin(theta_traj), - ) + q = pin.neutral(model) - if waypoint.get("articutool_waypoint_feasible", False): - pitch_sol, roll_sol = ( - waypoint.get("articutool_pitch_solution_rad"), - waypoint.get("articutool_roll_solution_rad"), - ) - for sol, info in [ - (pitch_sol, articutool_pitch_joint_info), - (roll_sol, articutool_roll_joint_info), - ]: - if sol is not None and info: - q_idx, nq, nv = info["q_idx_start"], info["nq"], info["nv"] - if nq == 1: - q_vector[q_idx] = sol - elif nq == 2 and nv == 1: - q_vector[q_idx], q_vector[q_idx + 1] = np.cos(sol), np.sin(sol) - else: # Articutool not feasible, set to neutral (0.0) - for info in [articutool_pitch_joint_info, articutool_roll_joint_info]: - if info: - q_idx, nq, nv = info["q_idx_start"], info["nq"], info["nv"] - if nq == 1: - q_vector[q_idx] = 0.0 - elif nq == 2 and nv == 1: - q_vector[q_idx], q_vector[q_idx + 1] = np.cos(0.0), np.sin(0.0) - - current_q_display = q.copy() - if waypoints_data: - set_q_from_enhanced_waypoint(current_q_display, waypoints_data[0]) - pin_viz.display(current_q_display) - q[:] = current_q_display[:] - - current_point_idx = 0 - num_trajectory_points = len(waypoints_data) - print("\n--- Trajectory Control ---") - print("Open the MeshCat URL in your browser.") - running = True - while running: - wp_info = waypoints_data[current_point_idx] - at_feasible = wp_info.get("articutool_waypoint_feasible", False) - at_pitch = wp_info.get("articutool_pitch_solution_rad", "N/A") - at_roll = wp_info.get("articutool_roll_solution_rad", "N/A") - if isinstance(at_pitch, float): - at_pitch = f"{at_pitch:.3f}" - if isinstance(at_roll, float): - at_roll = f"{at_roll:.3f}" + is_rigid_mode = args.rigid + navigable_indices = [] + if args.snapshot_frames: + navigable_indices = [ + int(f.strip()) - 1 for f in args.snapshot_frames.split(",") + ] + print(f"\n--- Snapshot Navigation Mode ---") print( - f"\nPoint: {current_point_idx + 1}/{num_trajectory_points} | Articutool Feasible: {at_feasible} (P: {at_pitch}, R: {at_roll})" + f"Navigating {len(navigable_indices)} keyframes: {[f + 1 for f in navigable_indices]}" ) - print("Commands: [n]ext, [p]rev, [f]irst, [l]ast, [g ], [a]nimate, [q]uit") - try: + else: + navigable_indices = list(range(len(waypoints))) + print(f"\n--- Full Trajectory Navigation Mode ---") + + current_nav_idx = 0 + + is_recording = False + video_writer = None + current_recording_filepath = None + + try: + while True: + if not navigable_indices: + print("No frames to navigate.") + break + + current_point_idx = navigable_indices[current_nav_idx] + waypoint_info = waypoints[current_point_idx] + + set_q_from_waypoint( + q, + model, + waypoint_info, + jaco_mappings, + articutool_pitch_info, + articutool_roll_info, + is_rigid_mode, + ) + pin_viz.display(q) + + is_atool_active_in_file = waypoint_info.get( + "articutool_waypoint_feasible", False + ) + rigidity_status = ( + "ON (Rigid)" + if is_rigid_mode + else f"OFF (Active: {is_atool_active_in_file})" + ) + + print( + f"\nDisplaying Frame: {current_point_idx + 1}/{len(waypoints)} (Nav index: {current_nav_idx + 1}/{len(navigable_indices)})" + ) + print(f"Articutool Rigidity: {rigidity_status}") + print( + "Commands: [n]ext, [p]rev, [f]irst, [l]ast, [t]oggle rigidity, [a]nimate, [s]creenshot, [r]ecord, [q]uit" + ) + user_input = input("Enter command: ").strip().lower() - new_q_to_display = q.copy() + if user_input == "q": - running = False - print("Quitting.") break - elif user_input == "n": - current_point_idx = min( - current_point_idx + 1, num_trajectory_points - 1 + elif user_input == "s": + print("Taking screenshot...") + img = visualizer.get_image() + rigidity_str = "rigid" if is_rigid_mode else "active" + filename = f"frame_{current_point_idx + 1}_{rigidity_str}.png" + filepath = os.path.join(args.output_dir, filename) + img.save(filepath) + print(f"Screenshot saved to {filepath}") + continue + elif user_input == "r": + if not is_recording: + is_recording = True + rigidity_str = "rigid" if is_rigid_mode else "active" + filename = ( + f"recording_frame_{current_point_idx + 1}_{rigidity_str}.mp4" + ) + current_recording_filepath = os.path.join(args.output_dir, filename) + print( + f"Starting recording... Output file: {current_recording_filepath}" + ) + # --- MODIFICATION: Explicitly set the format to 'FFMPEG' to avoid using the wrong writer --- + video_writer = imageio.get_writer( + current_recording_filepath, fps=args.fps, format="FFMPEG" + ) + + print( + "Recording... Move the camera in MeshCat. Press Ctrl+C to stop." + ) + try: + while True: + img = visualizer.get_image() + video_writer.append_data(np.array(img)) + time.sleep(1.0 / args.fps) + except KeyboardInterrupt: + print("\nStopping manual recording.") + else: + pass + + if video_writer: + video_writer.close() + print(f"Video saved to {current_recording_filepath}") + video_writer = None + current_recording_filepath = None + is_recording = False + continue + + elif user_input == "t": + is_rigid_mode = not is_rigid_mode + print( + f"Articutool rigidity toggled to: {'ON' if is_rigid_mode else 'OFF'}" ) + continue + elif user_input == "n": + current_nav_idx = min(current_nav_idx + 1, len(navigable_indices) - 1) elif user_input == "p": - current_point_idx = max(current_point_idx - 1, 0) + current_nav_idx = max(current_nav_idx - 1, 0) elif user_input == "f": - current_point_idx = 0 + current_nav_idx = 0 elif user_input == "l": - current_point_idx = num_trajectory_points - 1 - elif user_input.startswith("g "): - try: - target_idx = int(user_input.split(" ")[1]) - 1 - if 0 <= target_idx < num_trajectory_points: - current_point_idx = target_idx - else: - print(f"Point number out of range (1-{num_trajectory_points}).") - continue - except: - print("Invalid format. Use 'g '.") - continue + current_nav_idx = len(navigable_indices) - 1 elif user_input == "a": - print("Animating trajectory... Press Ctrl+C to stop animation.") - start_anim_idx = current_point_idx + record_anim = ( + input("Record this animation? (y/n): ").lower().strip() == "y" + ) + anim_writer = None + filepath = "" + if record_anim: + rigidity_str = "rigid" if is_rigid_mode else "active" + filename = f"animation_{rigidity_str}.mp4" + filepath = os.path.join(args.output_dir, filename) + # --- MODIFICATION: Explicitly set the format to 'FFMPEG' to avoid using the wrong writer --- + anim_writer = imageio.get_writer( + filepath, fps=args.fps, format="FFMPEG" + ) + print(f"Recording animation to {filepath}...") + + print("Animating... Press Ctrl+C to stop.") anim_q = q.copy() + start_anim_nav_idx = current_nav_idx try: - for i in range(start_anim_idx, num_trajectory_points): - current_point_idx = i - set_q_from_enhanced_waypoint( - anim_q, waypoints_data[current_point_idx] + for i in range(start_anim_nav_idx, len(navigable_indices)): + anim_point_idx = navigable_indices[i] + set_q_from_waypoint( + anim_q, + model, + waypoints[anim_point_idx], + jaco_mappings, + articutool_pitch_info, + articutool_roll_info, + is_rigid_mode, ) pin_viz.display(anim_q) + + if anim_writer: + img = visualizer.get_image() + anim_writer.append_data(np.array(img)) + print( - f" Displaying point {current_point_idx + 1}/{num_trajectory_points}", + f" Displaying frame {anim_point_idx + 1}/{len(waypoints)}", end="\r", - flush=True, ) - time.sleep(0.05) - print( - "\nAnimation finished. " - ) + time.sleep(1.0 / args.fps) + current_nav_idx = len(navigable_indices) - 1 + print("\nAnimation finished.") except KeyboardInterrupt: - print( - "\nAnimation stopped. " - ) - new_q_to_display[:] = anim_q[:] - q[:] = new_q_to_display[:] - pin_viz.display(q) - continue - elif user_input: - print("Invalid command.") - continue + print("\nAnimation stopped.") + finally: + if anim_writer: + anim_writer.close() + print(f"Animation video saved to {filepath}") + else: + if user_input: + print("Invalid command.") continue - set_q_from_enhanced_waypoint( - new_q_to_display, waypoints_data[current_point_idx] - ) - pin_viz.display(new_q_to_display) - q[:] = new_q_to_display[:] - except EOFError: - print("\nEOF received, quitting.") - running = False - except KeyboardInterrupt: - print("\nInterrupted, quitting.") - running = False - except Exception as e: - print(f"An error occurred in the loop: {e}") - print("Visualizer finished.") + + except (EOFError, KeyboardInterrupt): + print("\nExiting...") + finally: + if video_writer: + video_writer.close() + if current_recording_filepath: + print(f"Final video saved to {current_recording_filepath}") + print("Visualizer finished.") if __name__ == "__main__": parser = argparse.ArgumentParser( - description="Visualize robot trajectories or single IK solutions using Pinocchio and MeshCat." + description="Visualize robot trajectories using Pinocchio and MeshCat." ) parser.add_argument("xacro_file", type=str, help="Path to the robot XACRO file.") - - group = parser.add_mutually_exclusive_group( - required=False - ) # Make providing one of these optional - group.add_argument( + parser.add_argument( "--trajectory_file", type=str, - help="Path to the .json ENHANCED trajectory file.", + help="Path to the .json trajectory file.", + ) + parser.add_argument( + "--rigid", + action="store_true", + help="Sets the initial state to be rigid. Can be toggled interactively.", + ) + parser.add_argument( + "--snapshot-frames", + type=str, + help="Comma-separated list of 1-based frame numbers to navigate (e.g., '1,25,50').", ) - group.add_argument( - "--ik_solution_json", + parser.add_argument( + "--output-dir", type=str, - help="Path to a .json file representing a single sensor_msgs/msg/JointState for IK visualization.", + default="screenshots", + help="Directory to save screenshots and videos. Default: 'screenshots'", + ) + parser.add_argument( + "--fps", + type=int, + default=30, + help="Frames per second for video recording. Default: 30", ) args = parser.parse_args() - - if not args.trajectory_file and not args.ik_solution_json: - print( - "Neither --trajectory_file nor --ik_solution_json provided. Will display neutral pose." - ) - # main will handle displaying neutral if both are None after model load. - main(args) From 63c2a71a844c1a7ec9ff2c33367daccfd9529b20 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Jul 2025 09:50:41 -0700 Subject: [PATCH 136/238] Add action for granular + liquid scoop with spoon --- .../config/acquisition_library.yaml | 31 +++++++++++++++++++ 1 file changed, 31 insertions(+) diff --git a/ada_feeding_action_select/config/acquisition_library.yaml b/ada_feeding_action_select/config/acquisition_library.yaml index 56da6158..5bd9b577 100644 --- a/ada_feeding_action_select/config/acquisition_library.yaml +++ b/ada_feeding_action_select/config/acquisition_library.yaml @@ -345,3 +345,34 @@ actions: post_acquisition_primitive_name: "VIBRATE_ROLL" post_acquisition_primitive_params: [5.0, 0.1, 1.0] # [frequency_hz, amplitude_rad, duration_sec] align_to_robot_base: True +# Granular + Liquid scoop with spoon +- ext_angular: [0.0, 0.0, 0.0] + ext_duration: 0.1 + ext_force: 50.0 + ext_linear: [0.0, 0.0, 0.0] + ext_torque: 8.0 + grasp_angular: [0.0, 0.0, 0.0] + grasp_duration: 1.0 + grasp_force: 70.0 + grasp_linear: [-0.05, 0.0, 0.0] + grasp_torque: 15.0 + pre_force: 20.0 + pre_offset: + - -0.035 + - 0.0 + - -0.02 + pre_pos: + - -0.035 + - 0.0 + - 1.0 + pre_quat: + - -0.1424887 + - -0.6926236 + - -0.6933992 + - -0.1398724 + pre_torque: 8.0 + post_move_into_primitive_name: "NONE" + post_move_into_primitive_params: [] + post_acquisition_primitive_name: "NONE" + post_acquisition_primitive_params: [] + align_to_robot_base: True From e5f7d1556345ed497bcd074df342a84d22823597 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Jul 2025 09:51:28 -0700 Subject: [PATCH 137/238] Run formatting --- ...eck_articutool_path_dynamic_feasibility.py | 68 +++++++++++++------ 1 file changed, 48 insertions(+), 20 deletions(-) diff --git a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_dynamic_feasibility.py b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_dynamic_feasibility.py index f4da4ab0..d3809d03 100644 --- a/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_dynamic_feasibility.py +++ b/ada_feeding/ada_feeding/behaviors/state/check_articutool_path_dynamic_feasibility.py @@ -162,7 +162,9 @@ def _solve_articutool_ik_for_leveling( ) return solutions - def _compute_articutool_jacobian(self, theta_p: float, theta_r: float) -> np.ndarray: + def _compute_articutool_jacobian( + self, theta_p: float, theta_r: float + ) -> np.ndarray: """ Computes the 3x2 analytical Jacobian for the Articutool leveling task. This must be derived from the same FK model that the IK inverts. @@ -175,7 +177,7 @@ def _compute_articutool_jacobian(self, theta_p: float, theta_r: float) -> np.nda j11 = 0 j21 = -sp * cr j31 = cp * cr - + # d/d(theta_r) j12 = -cr j22 = -cp * sr @@ -212,43 +214,56 @@ def update(self) -> Status: try: trajectory_input = self.blackboard_get("jaco_trajectory") num_points = self.blackboard_get("num_trajectory_points_to_check") - + jaco_points = self._get_jaco_trajectory_points(trajectory_input) if not jaco_points: - self.logger.warn(f"[{self.name}] Trajectory is empty. Assuming feasible.") + self.logger.warn( + f"[{self.name}] Trajectory is empty. Assuming feasible." + ) self.blackboard_set("articutool_is_dynamic_feasible", True) return Status.SUCCESS indices = np.linspace(0, len(jaco_points) - 1, num_points, dtype=int) - + last_valid_atool_q = None for idx in indices: q_jaco, v_jaco = jaco_points[idx] - + # --- KINEMATIC FEASIBILITY CHECK (Prerequisite) --- - + # 1. Get Articutool base orientation from Jaco FK pin.forwardKinematics(self._pin_model, self._pin_data, q_jaco) pin.updateFramePlacements(self._pin_model, self._pin_data) T_world_atool_base = self._pin_data.oMf[self._jaco_ee_frame_id_pin] R_world_atool_base = Rotation.from_matrix(T_world_atool_base.rotation) - + # 2. Transform world "up" vector to Articutool's base frame - target_y_in_atool_base = R_world_atool_base.inv().apply(self.WORLD_Z_UP_VECTOR) - + target_y_in_atool_base = R_world_atool_base.inv().apply( + self.WORLD_Z_UP_VECTOR + ) + # 3. Solve Articutool IK for leveling - ik_solutions = self._solve_articutool_ik_for_leveling(target_y_in_atool_base) + ik_solutions = self._solve_articutool_ik_for_leveling( + target_y_in_atool_base + ) valid_solutions = [ - sol for sol in ik_solutions - if self._pitch_limits_rad[0] - self.EPSILON <= sol[0] <= self._pitch_limits_rad[1] + self.EPSILON - and self._roll_limits_rad[0] - self.EPSILON <= sol[1] <= self._roll_limits_rad[1] + self.EPSILON + sol + for sol in ik_solutions + if self._pitch_limits_rad[0] - self.EPSILON + <= sol[0] + <= self._pitch_limits_rad[1] + self.EPSILON + and self._roll_limits_rad[0] - self.EPSILON + <= sol[1] + <= self._roll_limits_rad[1] + self.EPSILON ] if not valid_solutions: - self.feedback_message = f"Path is kinematically infeasible at point {idx}." + self.feedback_message = ( + f"Path is kinematically infeasible at point {idx}." + ) self.blackboard_set("articutool_is_dynamic_feasible", False) return Status.FAILURE @@ -256,19 +271,30 @@ def update(self) -> Status: if last_valid_atool_q is None: q_atool = valid_solutions[0] else: - distances = [np.linalg.norm(np.array(sol) - last_valid_atool_q) for sol in valid_solutions] + distances = [ + np.linalg.norm(np.array(sol) - last_valid_atool_q) + for sol in valid_solutions + ] q_atool = valid_solutions[np.argmin(distances)] last_valid_atool_q = np.array(q_atool) # --- DYNAMIC FEASIBILITY CHECK --- # 1. Calculate disturbance velocity from Jaco arm - J_jaco_full = pin.computeFrameJacobian(self._pin_model, self._pin_data, q_jaco, self._jaco_ee_frame_id_pin, pin.ReferenceFrame.WORLD) + J_jaco_full = pin.computeFrameJacobian( + self._pin_model, + self._pin_data, + q_jaco, + self._jaco_ee_frame_id_pin, + pin.ReferenceFrame.WORLD, + ) v_jaco_full = J_jaco_full @ v_jaco - omega_disturbance_world = v_jaco_full[3:6] # Angular velocity part + omega_disturbance_world = v_jaco_full[3:6] # Angular velocity part # 2. Transform disturbance to Articutool's local frame - omega_correction_local = -R_world_atool_base.inv().apply(omega_disturbance_world) + omega_correction_local = -R_world_atool_base.inv().apply( + omega_disturbance_world + ) # 3. Compute Articutool's Jacobian at the required configuration J_atool = self._compute_articutool_jacobian(q_atool[0], q_atool[1]) @@ -278,7 +304,9 @@ def update(self) -> Status: J_atool_pinv = np.linalg.pinv(J_atool, rcond=1e-4) q_dot_atool_required = J_atool_pinv @ omega_correction_local except np.linalg.LinAlgError: - self.feedback_message = f"Articutool Jacobian is singular at point {idx}." + self.feedback_message = ( + f"Articutool Jacobian is singular at point {idx}." + ) self.blackboard_set("articutool_is_dynamic_feasible", False) return Status.FAILURE From 9253e4ebc7b34378bd9ed12246fd4d14db02752d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Jul 2025 15:46:03 -0700 Subject: [PATCH 138/238] Add a benchmarking script that tests the feasibility checks utilized to verify whether a trajectory allows the Articutool to maintain a level orientation of its tool tip --- .../articutool_feasibility_benchmark.py | 572 ++++++++++++++++++ 1 file changed, 572 insertions(+) create mode 100644 ada_feeding/scripts/articutool_feasibility_benchmark.py diff --git a/ada_feeding/scripts/articutool_feasibility_benchmark.py b/ada_feeding/scripts/articutool_feasibility_benchmark.py new file mode 100644 index 00000000..4fc9a7b6 --- /dev/null +++ b/ada_feeding/scripts/articutool_feasibility_benchmark.py @@ -0,0 +1,572 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script runs a simulation-based benchmark to evaluate the "plan-then-verify" +control methodology for the Articutool, as described in the accompanying paper. + +It operates in a simulation-only mode: +1. Generates a specified number of random start and goal joint configurations for + the Jaco arm. +2. For each pair, it uses MoveIt2 to plan a trajectory. +3. If a plan is found, it performs a post-hoc feasibility analysis on the + trajectory. For each waypoint, it calculates the required Articutool pitch and + roll angles to keep the end-effector level with respect to gravity. +4. It checks if these required angles are within the Articutool's joint limits. +5. It saves an "enhanced" trajectory file (.json) containing the per-waypoint + Articutool solutions, which can then be used by `visualize_trajectory.py`. +""" + +# Standard imports +from collections import namedtuple +from datetime import datetime +import os +import time +from threading import Thread +from typing import Optional, List, Dict, Tuple, Any +import json +import math +import subprocess +import sys + +# Third-party imports +import numpy as np +from pymoveit2 import MoveIt2 +from pymoveit2.robots import kinova +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory +import moveit_msgs.msg +from scipy.spatial.transform import Rotation as R +import pinocchio as pin + +# --- Constants and Named Tuples --- +LOGGER = rclpy.logging.get_logger("articutool_benchmark_sim") +PLANNING_GROUP = "jaco_arm" +JOINT_NAMES = kinova.joint_names() +BASE_LINK = kinova.base_link_name() +END_EFFECTOR_LINK = kinova.end_effector_name() +EPSILON = 1e-6 + +# Define Articutool joint limits in radians +ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) # (-90, 90) degrees +ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) # (-180, 180) degrees + +# Define the world's "up" vector (positive Z axis) +WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) + +# --- Data Structures for Clarity --- +PlanningTask = namedtuple("PlanningTask", ["start_config", "goal_config"]) +ArticutoolWaypointSolution = namedtuple( + "ArticutoolWaypointSolution", + ["waypoint_feasible", "pitch_solution_rad", "roll_solution_rad"], +) +ArticutoolMetrics = namedtuple( + "ArticutoolMetrics", + [ + "path_feasible", + "num_infeasible_points", + "pitch_range_used_percent", + "roll_range_used_percent", + "avg_pitch_abs_rad", + "avg_roll_abs_rad", + ], +) + + +class ArticutoolBenchmarkSimulator: + """Manages the simulation-based benchmarking process.""" + + def __init__( + self, + node: Node, + moveit2: MoveIt2, + xacro_file_path: str, + num_tasks: int = 100, + planning_timeout: float = 5.0, + planner_id: str = "RRTConnectkConfigDefault", + trajectory_save_dir: Optional[str] = None, + ): + self.node = node + self.moveit2 = moveit2 + self.xacro_file_path = xacro_file_path + self.num_tasks = num_tasks + self.planning_timeout = planning_timeout + self.planner_id = planner_id + self.trajectory_save_dir = trajectory_save_dir + self.results: List[Dict[str, Any]] = [] + + # Pinocchio model for forward kinematics + self.pinocchio_model: Optional[pin.Model] = None + self.pinocchio_data: Optional[pin.Data] = None + self.jaco_ee_frame_id_pin: Optional[int] = None + self.joint_name_to_pinocchio_id: Dict[str, int] = {} + self._initialize_pinocchio() + + self.joint_limits = self._get_joint_limits() + + LOGGER.info("Benchmark simulator initialized.") + LOGGER.info(f"Testing with planner: {self.planner_id}") + LOGGER.info(f"Number of random tasks to generate: {self.num_tasks}") + if self.trajectory_save_dir: + os.makedirs(self.trajectory_save_dir, exist_ok=True) + LOGGER.info( + f"Enhanced trajectories will be saved to: {self.trajectory_save_dir}" + ) + + def _initialize_pinocchio(self): + """ + Loads the robot model from a XACRO file into Pinocchio. + This is a more robust method that explicitly handles XACRO to URDF conversion + and ensures Pinocchio can find all mesh files. + """ + LOGGER.info(f"Processing Xacro file: {self.xacro_file_path}") + try: + # Convert XACRO to a URDF string + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", self.xacro_file_path], + check=True, + capture_output=True, + text=True, + ) + urdf_xml_string = process.stdout + except (FileNotFoundError, subprocess.CalledProcessError) as e: + LOGGER.error(f"Failed to process XACRO file: {e}") + LOGGER.error( + "Make sure ROS 2 environment is sourced and xacro is installed." + ) + self.pinocchio_model = None + return + + # Load the model from the URDF string + try: + self.pinocchio_model = pin.buildModelFromXML(urdf_xml_string) + self.pinocchio_data = self.pinocchio_model.createData() + self.jaco_ee_frame_id_pin = self.pinocchio_model.getFrameId( + END_EFFECTOR_LINK + ) + # Map joint names to their Pinocchio model IDs for quick access + for name in JOINT_NAMES: + if self.pinocchio_model.existJointName(name): + self.joint_name_to_pinocchio_id[name] = ( + self.pinocchio_model.getJointId(name) + ) + LOGGER.info("Pinocchio model loaded successfully.") + except Exception as e: + LOGGER.error(f"Failed to build Pinocchio model from URDF string: {e}") + self.pinocchio_model = None + + def _get_joint_limits(self) -> List[Tuple[float, float]]: + """Retrieves joint limits from the Pinocchio model in the correct order.""" + if self.pinocchio_model is None: + LOGGER.warn("Pinocchio model not available, using default joint limits.") + return [(-math.pi, math.pi)] * len(JOINT_NAMES) + + limits = [] + for joint_name in JOINT_NAMES: + if self.pinocchio_model.existJointName(joint_name): + joint_id = self.pinocchio_model.getJointId(joint_name) + joint = self.pinocchio_model.joints[joint_id] + # Get the index in the configuration vector 'q' for this joint + limit_idx = joint.idx_q + lower = self.pinocchio_model.lowerPositionLimit[limit_idx] + upper = self.pinocchio_model.upperPositionLimit[limit_idx] + limits.append((lower, upper)) + else: + # Fallback for safety, though this shouldn't happen with a correct URDF + limits.append((-math.pi, math.pi)) + return limits + + def generate_random_config(self) -> List[float]: + """Generates a random joint configuration within the robot's limits.""" + return [ + ( + np.random.uniform(low, high) + if np.isfinite(low) and np.isfinite(high) + else np.random.uniform(-np.pi, np.pi) + ) + for low, high in self.joint_limits + ] + + def _normalize_angle(self, angle: float) -> float: + """Normalizes an angle to the range [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + + def _solve_articutool_ik_for_leveling( + self, target_y_axis_in_atool_base: np.ndarray + ) -> List[Tuple[float, float]]: + """ + Analytical Inverse Kinematics (IK) solver for the 2-DOF Articutool. + """ + vx, vy, vz = target_y_axis_in_atool_base + solutions: List[Tuple[float, float]] = [] + + # From the kinematic model: sin(theta_r) = -vx + asin_arg_for_tr = -vx + if not (-1.0 - EPSILON <= asin_arg_for_tr <= 1.0 + EPSILON): + return [] + + asin_arg_for_tr_clipped = np.clip(asin_arg_for_tr, -1.0, 1.0) + theta_r_sol1 = math.asin(asin_arg_for_tr_clipped) + theta_r_sol2 = self._normalize_angle(math.pi - theta_r_sol1) + + candidate_thetas_r = list(set([theta_r_sol1, theta_r_sol2])) + + for theta_r in candidate_thetas_r: + cos_theta_r = math.cos(theta_r) + if math.isclose(cos_theta_r, 0.0, abs_tol=EPSILON): + if math.isclose(vy, 0.0, abs_tol=EPSILON) and math.isclose( + vz, 0.0, abs_tol=EPSILON + ): + solutions.append((0.0, self._normalize_angle(theta_r))) + continue + + theta_p_sol = math.atan2(vz, vy) + solutions.append( + (self._normalize_angle(theta_p_sol), self._normalize_angle(theta_r)) + ) + return solutions + + def _analyze_trajectory_for_articutool_leveling( + self, trajectory: JointTrajectory + ) -> Tuple[ArticutoolMetrics, List[ArticutoolWaypointSolution]]: + """ + Performs the core "plan-then-verify" analysis on a trajectory. + """ + if self.pinocchio_model is None: + LOGGER.error( + "Cannot perform feasibility check: Pinocchio model not loaded." + ) + return ( + ArticutoolMetrics( + False, len(trajectory.points), np.nan, np.nan, np.nan, np.nan + ), + [], + ) + + required_pitches, required_rolls = [], [] + num_infeasible_wps = 0 + per_waypoint_solutions: List[ArticutoolWaypointSolution] = [] + q = pin.neutral(self.pinocchio_model) + + for point in trajectory.points: + # ----- KINEMATIC FIX START ----- + # Correctly populate the Pinocchio configuration vector 'q'. + # Some joints (especially revolute) are represented by Pinocchio + # using 2 values (cos(theta), sin(theta)) in the 'q' vector for a + # 1-DOF joint. A simple assignment q[idx] = theta is incorrect. + # We must check the model's representation for each joint. + for i, name in enumerate(JOINT_NAMES): + if name in self.joint_name_to_pinocchio_id: + joint_id = self.joint_name_to_pinocchio_id[name] + joint_obj = self.pinocchio_model.joints[joint_id] + theta = point.positions[i] + + # nq is the size of the joint's representation in 'q' + # nv is the number of degrees of freedom (velocity size) + if joint_obj.nq == 2 and joint_obj.nv == 1: + # This is a revolute joint represented by (cos, sin) + q[joint_obj.idx_q] = math.cos(theta) + q[joint_obj.idx_q + 1] = math.sin(theta) + elif joint_obj.nq == 1 and joint_obj.nv == 1: + # This is a simple 1-to-1 mapping (e.g., prismatic or simple revolute) + q[joint_obj.idx_q] = theta + else: + # Fallback for other joint types, though not expected for Jaco + q[joint_obj.idx_q] = theta + # ----- KINEMATIC FIX END ----- + + pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) + pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) + + ee_transform: pin.SE3 = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + R_world_ee = R.from_matrix(ee_transform.rotation) + + target_up_in_ee_frame = R_world_ee.inv().apply(WORLD_UP_VECTOR) + + # Call the IK solver with the correctly calculated target vector. + ik_solutions = self._solve_articutool_ik_for_leveling(target_up_in_ee_frame) + + valid_solutions = [ + s + for s in ik_solutions + if ( + ARTICUTOOL_PITCH_LIMITS_RAD[0] - EPSILON + <= s[0] + <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + EPSILON + and ARTICUTOOL_ROLL_LIMITS_RAD[0] - EPSILON + <= s[1] + <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + EPSILON + ) + ] + + if not valid_solutions: + num_infeasible_wps += 1 + per_waypoint_solutions.append( + ArticutoolWaypointSolution(False, None, None) + ) + else: + best_sol = min(valid_solutions, key=lambda s: s[0] ** 2 + s[1] ** 2) + required_pitches.append(best_sol[0]) + required_rolls.append(best_sol[1]) + per_waypoint_solutions.append( + ArticutoolWaypointSolution(True, best_sol[0], best_sol[1]) + ) + + path_feasible = num_infeasible_wps == 0 + p_range = ( + (np.max(required_pitches) - np.min(required_pitches)) + if required_pitches + else 0 + ) + r_range = ( + (np.max(required_rolls) - np.min(required_rolls)) if required_rolls else 0 + ) + total_p_range = ARTICUTOOL_PITCH_LIMITS_RAD[1] - ARTICUTOOL_PITCH_LIMITS_RAD[0] + total_r_range = ARTICUTOOL_ROLL_LIMITS_RAD[1] - ARTICUTOOL_ROLL_LIMITS_RAD[0] + + return ( + ArticutoolMetrics( + path_feasible=path_feasible, + num_infeasible_points=num_infeasible_wps, + pitch_range_used_percent=( + (p_range / total_p_range * 100) if total_p_range > EPSILON else 0 + ), + roll_range_used_percent=( + (r_range / total_r_range * 100) if total_r_range > EPSILON else 0 + ), + avg_pitch_abs_rad=( + np.mean(np.abs(required_pitches)) if required_pitches else 0.0 + ), + avg_roll_abs_rad=( + np.mean(np.abs(required_rolls)) if required_rolls else 0.0 + ), + ), + per_waypoint_solutions, + ) + + def _save_enhanced_trajectory_to_file( + self, + original_trajectory: JointTrajectory, + articutool_solutions: List[ArticutoolWaypointSolution], + task_id: int, + ) -> Optional[str]: + """Saves the trajectory data along with Articutool solutions to a JSON file.""" + if not self.trajectory_save_dir: + return None + try: + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S-%f") + filename = f"task_{task_id}_{self.planner_id}_{timestamp}.json" + filepath = os.path.join(self.trajectory_save_dir, filename) + + waypoints_data = [] + for i, jaco_point in enumerate(original_trajectory.points): + at_solution = ( + articutool_solutions[i] + if i < len(articutool_solutions) + else ArticutoolWaypointSolution(False, None, None) + ) + waypoints_data.append( + { + "time_from_start_sec": jaco_point.time_from_start.sec + + jaco_point.time_from_start.nanosec * 1e-9, + "jaco_positions_rad": list(jaco_point.positions), + "articutool_waypoint_feasible": at_solution.waypoint_feasible, + "articutool_pitch_solution_rad": at_solution.pitch_solution_rad, + "articutool_roll_solution_rad": at_solution.roll_solution_rad, + } + ) + + enhanced_data = { + "jaco_joint_names": list(original_trajectory.joint_names), + "waypoints": waypoints_data, + } + + with open(filepath, "w") as f: + json.dump(enhanced_data, f, indent=2) + return filename + except Exception as e: + LOGGER.error(f"Failed to save enhanced trajectory for task {task_id}: {e}") + return None + + def run(self): + """Main benchmark execution loop.""" + if not self.pinocchio_model: + LOGGER.error("Aborting benchmark: Pinocchio model failed to initialize.") + return + + LOGGER.info("Generating random planning tasks...") + tasks = [ + PlanningTask(self.generate_random_config(), self.generate_random_config()) + for _ in range(self.num_tasks) + ] + LOGGER.info(f"Successfully generated {len(tasks)} tasks.") + + for i, task in enumerate(tasks): + total_task_start_time = time.perf_counter() + LOGGER.info(f"--- Running Task {i + 1}/{len(tasks)} ---") + + start_state_msg = moveit_msgs.msg.RobotState() + start_state_msg.joint_state.name = JOINT_NAMES + start_state_msg.joint_state.position = list(task.start_config) + self.moveit2._MoveIt2__move_action_goal.request.start_state = ( + start_state_msg + ) + + self.moveit2.set_joint_goal(list(task.goal_config)) + self.moveit2.planner_id = self.planner_id + self.moveit2.allowed_planning_time = self.planning_timeout + + planning_start_time = time.perf_counter() + plan_future = self.moveit2.plan_async() + + while rclpy.ok() and not plan_future.done(): + time.sleep(0.01) + + trajectory = self.moveit2.get_trajectory(plan_future) + planning_time = time.perf_counter() - planning_start_time + + plan_success = trajectory is not None and bool(trajectory.points) + articutool_metrics = None + feasibility_check_time = 0.0 + trajectory_filename = None + + if plan_success: + LOGGER.info(f" Jaco plan SUCCEEDED in {planning_time:.4f}s.") + feasibility_start_time = time.perf_counter() + articutool_metrics, solutions = ( + self._analyze_trajectory_for_articutool_leveling(trajectory) + ) + feasibility_check_time = time.perf_counter() - feasibility_start_time + LOGGER.info( + f" Articutool Leveling Feasibility: {articutool_metrics.path_feasible} " + f"({articutool_metrics.num_infeasible_points} infeasible points) " + f"(checked in {feasibility_check_time:.4f}s)" + ) + trajectory_filename = self._save_enhanced_trajectory_to_file( + trajectory, solutions, i + ) + else: + LOGGER.warn(f" Jaco plan FAILED in {planning_time:.4f}s.") + + total_task_time = time.perf_counter() - total_task_start_time + self.results.append( + { + "task_id": i, + "jaco_plan_success": plan_success, + "planning_time_s": planning_time, + "feasibility_check_time_s": feasibility_check_time, + "total_task_time_s": total_task_time, + "trajectory_filename": trajectory_filename, + **(articutool_metrics._asdict() if articutool_metrics else {}), + } + ) + + def save_results(self, output_dir: str): + """Saves the benchmark results to a JSON file and prints a summary.""" + os.makedirs(output_dir, exist_ok=True) + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") + filename = os.path.join(output_dir, f"benchmark_results_{timestamp}.json") + + with open(filename, "w") as f: + json.dump(self.results, f, indent=2) + LOGGER.info(f"Benchmark results saved to {filename}") + + total_tasks = len(self.results) + jaco_successes = sum(1 for r in self.results if r["jaco_plan_success"]) + articutool_successes = sum( + 1 for r in self.results if r.get("path_feasible", False) + ) + + jaco_success_rate = ( + (jaco_successes / total_tasks * 100) if total_tasks > 0 else 0 + ) + rejection_rate = ( + ((jaco_successes - articutool_successes) / jaco_successes * 100) + if jaco_successes > 0 + else 0 + ) + + LOGGER.info("\n--- BENCHMARK SUMMARY ---") + LOGGER.info(f"Total Tasks Attempted: {total_tasks}") + LOGGER.info( + f"Jaco Planner Success Rate: {jaco_success_rate:.2f}% ({jaco_successes}/{total_tasks})" + ) + LOGGER.info( + f"Articutool Feasibility Rejection Rate (of successful Jaco plans): {rejection_rate:.2f}%" + ) + + feasible_results = [r for r in self.results if r.get("path_feasible")] + if feasible_results: + avg_plan_time = np.mean([r["planning_time_s"] for r in feasible_results]) + avg_check_time = np.mean( + [r["feasibility_check_time_s"] for r in feasible_results] + ) + LOGGER.info(f"Avg. Planning Time (on feasible paths): {avg_plan_time:.4f}s") + LOGGER.info( + f"Avg. Feasibility Check Time (on feasible paths): {avg_check_time:.4f}s" + ) + LOGGER.info("-------------------------\n") + + +def main(): + if len(sys.argv) < 2: + print( + "Usage: python3 articutool_feasibility_benchmark.py /path/to/your/robot.urdf.xacro" + ) + sys.exit(1) + xacro_file_arg = sys.argv[1] + if not os.path.exists(xacro_file_arg): + print(f"Error: XACRO file not found at '{xacro_file_arg}'") + sys.exit(1) + + rclpy.init() + node = Node("articutool_benchmark_node") + + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + executor_thread = Thread(target=executor.spin, daemon=True) + executor_thread.start() + + moveit2 = MoveIt2( + node=node, + joint_names=JOINT_NAMES, + base_link_name=kinova.base_link_name(), + end_effector_name=END_EFFECTOR_LINK, + group_name=PLANNING_GROUP, + callback_group=ReentrantCallbackGroup(), + ) + + NUM_TASKS = 200 + PLANNING_TIMEOUT = 5.0 + PLANNER_ID = "RRTConnectkConfigDefault" + OUTPUT_DIR = os.path.join(os.getcwd(), "articutool_benchmark_output") + + benchmark = ArticutoolBenchmarkSimulator( + node=node, + moveit2=moveit2, + xacro_file_path=xacro_file_arg, + num_tasks=NUM_TASKS, + planning_timeout=PLANNING_TIMEOUT, + planner_id=PLANNER_ID, + trajectory_save_dir=OUTPUT_DIR, + ) + + try: + if benchmark.pinocchio_model is not None: + benchmark.run() + benchmark.save_results(OUTPUT_DIR) + else: + LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") + except Exception as e: + LOGGER.error(f"An error occurred during the benchmark: {e}") + finally: + LOGGER.info("Shutting down.") + rclpy.shutdown() + executor_thread.join() + + +if __name__ == "__main__": + main() From ae5a12f32b8ebcb1e97d2fcdbf1ff6dd9467d6cd Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Jul 2025 18:04:56 -0700 Subject: [PATCH 139/238] Add benchmark to evaluate feasibility checks when the Jaco arm is commanded to specific positions (xyz) instead of configurations, and optionally commanded to achieve a specific yaw orientation of its end-effector --- .../constrained_task_space_benchmark.py | 500 ++++++++++++++++++ 1 file changed, 500 insertions(+) create mode 100644 ada_feeding/scripts/constrained_task_space_benchmark.py diff --git a/ada_feeding/scripts/constrained_task_space_benchmark.py b/ada_feeding/scripts/constrained_task_space_benchmark.py new file mode 100644 index 00000000..ae79ec28 --- /dev/null +++ b/ada_feeding/scripts/constrained_task_space_benchmark.py @@ -0,0 +1,500 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script runs a benchmark to evaluate a "constraint-driven planning" +methodology for the Articutool system. + +It can be run in two modes: +1. Position-Only Goals (default): Plans to a random (x, y, z) position, + leaving the final orientation unconstrained. +2. Position + Yaw Goals (`--constrain-goal-yaw`): Plans to a random + (x, y, z) position AND a random final yaw orientation. + +In both modes, a "smart" orientation constraint (lenient pitch, free yaw, +strict roll) is applied to the ENTIRE PATH to ensure the Articutool can +always maintain a level configuration. +""" + +# Standard imports +from collections import namedtuple +from datetime import datetime +import os +import time +from threading import Thread +from typing import Optional, List, Dict, Tuple, Any +import json +import math +import subprocess +import sys + +# Third-party imports +import numpy as np +from pymoveit2 import MoveIt2 +from pymoveit2.robots import kinova +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory +import moveit_msgs.msg +from geometry_msgs.msg import Quaternion, Point +from scipy.spatial.transform import Rotation as R +import pinocchio as pin + +# --- Constants and Named Tuples --- +LOGGER = rclpy.logging.get_logger("constrained_task_space_benchmark") +PLANNING_GROUP = "jaco_arm" +JOINT_NAMES = kinova.joint_names() +BASE_LINK = kinova.base_link_name() +END_EFFECTOR_LINK = kinova.end_effector_name() +EPSILON = 1e-6 + +# Define Articutool joint limits in radians +ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) +ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) +WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) + +# --- Parameters for the "Smart" PATH Orientation Constraint --- +PATH_CONSTRAINT_QUAT_XYZW = (0.707, 0.0, 0.0, 0.707) +PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = ( + 1.5, + 3.14, + 0.8, +) # (Pitch, Yaw, Roll) - Roll relaxed to 0.8 + +# --- Parameters for the optional GOAL Yaw Orientation Constraint --- +GOAL_YAW_CONSTRAINT_TOLERANCE_XYZ_RAD = ( + math.pi, + math.pi, + 0.1, +) # Loose Pitch/Roll, Tight Yaw + +# --- Define a reasonable workspace for sampling random positions --- +WORKSPACE_OUTER_RADIUS = 0.7 # Max reach in meters +WORKSPACE_INNER_RADIUS = 0.3 # Min distance to avoid singularity at base + +# --- Data Structures for Clarity --- +ArticutoolWaypointSolution = namedtuple( + "ArticutoolWaypointSolution", + ["waypoint_feasible", "pitch_solution_rad", "roll_solution_rad"], +) +ArticutoolMetrics = namedtuple( + "ArticutoolMetrics", + [ + "path_feasible", + "num_infeasible_points", + ], +) + + +class ConstrainedTaskSpaceBenchmark: + """Manages the task-space benchmark planning process.""" + + def __init__( + self, + node: Node, + moveit2: MoveIt2, + xacro_file_path: str, + num_tasks: int = 100, + planning_timeout: float = 5.0, + planner_id: str = "RRTConnectkConfigDefault", + trajectory_save_dir: Optional[str] = None, + constrain_goal_yaw: bool = False, + ): + self.node = node + self.moveit2 = moveit2 + self.xacro_file_path = xacro_file_path + self.num_tasks = num_tasks + self.planning_timeout = planning_timeout + self.planner_id = planner_id + self.trajectory_save_dir = trajectory_save_dir + self.constrain_goal_yaw = constrain_goal_yaw + self.results: List[Dict[str, Any]] = [] + + self.pinocchio_model: Optional[pin.Model] = None + self.pinocchio_data: Optional[pin.Data] = None + self.jaco_ee_frame_id_pin: Optional[int] = None + self.joint_name_to_pinocchio_id: Dict[str, int] = {} + self._initialize_pinocchio() + + LOGGER.info("Benchmark simulator initialized.") + if self.constrain_goal_yaw: + LOGGER.info("Planning Mode: Position + Yaw Goals.") + else: + LOGGER.info("Planning Mode: Position-Only Goals.") + LOGGER.info(f"Applying 'smart' orientation constraint to all paths.") + + if self.trajectory_save_dir: + os.makedirs(self.trajectory_save_dir, exist_ok=True) + LOGGER.info(f"Trajectories will be saved to: {self.trajectory_save_dir}") + + def _initialize_pinocchio(self): + """Loads the robot model from a XACRO file into Pinocchio.""" + try: + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", self.xacro_file_path], + check=True, + capture_output=True, + text=True, + ) + urdf_xml_string = process.stdout + self.pinocchio_model = pin.buildModelFromXML(urdf_xml_string) + self.pinocchio_data = self.pinocchio_model.createData() + self.jaco_ee_frame_id_pin = self.pinocchio_model.getFrameId( + END_EFFECTOR_LINK + ) + for name in JOINT_NAMES: + if self.pinocchio_model.existJointName(name): + self.joint_name_to_pinocchio_id[name] = ( + self.pinocchio_model.getJointId(name) + ) + LOGGER.info("Pinocchio model loaded successfully.") + except (FileNotFoundError, subprocess.CalledProcessError, Exception) as e: + LOGGER.error(f"Failed to initialize Pinocchio model: {e}") + self.pinocchio_model = None + + def generate_random_position_in_workspace(self) -> np.ndarray: + """Generates a random (x, y, z) position from a half-spherical shell.""" + r_outer, r_inner = WORKSPACE_OUTER_RADIUS, WORKSPACE_INNER_RADIUS + r = (np.random.uniform(r_inner**3, r_outer**3)) ** (1 / 3) + theta = np.random.uniform(0, 2 * math.pi) + cos_phi = np.random.uniform(0, 1) + phi = math.acos(cos_phi) + x = r * math.sin(phi) * math.cos(theta) + y = r * math.sin(phi) * math.sin(theta) + z = r * cos_phi + return np.array([x, y, z]) + + def generate_random_yaw_quaternion(self) -> Tuple[float, float, float, float]: + """Generates a quaternion representing a random yaw.""" + random_yaw_angle = np.random.uniform(-math.pi, math.pi) + # Scipy handles conversion from Euler angles (Z-axis for yaw) to quaternion + quat_xyzw = R.from_euler("z", random_yaw_angle).as_quat() + return tuple(quat_xyzw) + + def _solve_articutool_ik_for_leveling( + self, target_y_axis_in_atool_base: np.ndarray + ) -> List[Tuple[float, float]]: + """Analytical IK solver for the Articutool.""" + vx, vy, vz = target_y_axis_in_atool_base + solutions: List[Tuple[float, float]] = [] + asin_arg_for_tr = -vx + if not (-1.0 - EPSILON <= asin_arg_for_tr <= 1.0 + EPSILON): + return [] + asin_arg_for_tr_clipped = np.clip(asin_arg_for_tr, -1.0, 1.0) + theta_r_sol1 = math.asin(asin_arg_for_tr_clipped) + theta_r_sol2 = (math.pi - theta_r_sol1 + math.pi) % (2 * math.pi) - math.pi + candidate_thetas_r = list(set([theta_r_sol1, theta_r_sol2])) + for theta_r in candidate_thetas_r: + cos_theta_r = math.cos(theta_r) + if math.isclose(cos_theta_r, 0.0, abs_tol=EPSILON): + if math.isclose(vy, 0.0, abs_tol=EPSILON) and math.isclose( + vz, 0.0, abs_tol=EPSILON + ): + solutions.append( + (0.0, (theta_r + math.pi) % (2 * math.pi) - math.pi) + ) + continue + theta_p_sol = math.atan2(vz, vy) + solutions.append( + ( + (theta_p_sol + math.pi) % (2 * math.pi) - math.pi, + (theta_r + math.pi) % (2 * math.pi) - math.pi, + ) + ) + return solutions + + def _verify_trajectory_feasibility( + self, trajectory: JointTrajectory + ) -> Tuple[ArticutoolMetrics, List[ArticutoolWaypointSolution]]: + """Verifies if a given trajectory is feasible for the Articutool.""" + if self.pinocchio_model is None: + return (ArticutoolMetrics(False, len(trajectory.points)), []) + + num_infeasible_wps = 0 + per_waypoint_solutions: List[ArticutoolWaypointSolution] = [] + q = pin.neutral(self.pinocchio_model) + + for point in trajectory.points: + for i, name in enumerate(JOINT_NAMES): + if name in self.joint_name_to_pinocchio_id: + joint_id = self.joint_name_to_pinocchio_id[name] + joint_obj = self.pinocchio_model.joints[joint_id] + theta = point.positions[i] + if joint_obj.nq == 2 and joint_obj.nv == 1: + q[joint_obj.idx_q] = math.cos(theta) + q[joint_obj.idx_q + 1] = math.sin(theta) + else: + q[joint_obj.idx_q] = theta + + pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) + pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) + ee_transform: pin.SE3 = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + R_world_ee = R.from_matrix(ee_transform.rotation) + target_up_in_ee_frame = R_world_ee.inv().apply(WORLD_UP_VECTOR) + ik_solutions = self._solve_articutool_ik_for_leveling(target_up_in_ee_frame) + + valid_solutions = [ + s + for s in ik_solutions + if ( + ARTICUTOOL_PITCH_LIMITS_RAD[0] - EPSILON + <= s[0] + <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + EPSILON + and ARTICUTOOL_ROLL_LIMITS_RAD[0] - EPSILON + <= s[1] + <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + EPSILON + ) + ] + + if not valid_solutions: + num_infeasible_wps += 1 + per_waypoint_solutions.append( + ArticutoolWaypointSolution(False, None, None) + ) + else: + best_sol = min(valid_solutions, key=lambda s: s[0] ** 2 + s[1] ** 2) + per_waypoint_solutions.append( + ArticutoolWaypointSolution(True, best_sol[0], best_sol[1]) + ) + + return ( + ArticutoolMetrics( + path_feasible=(num_infeasible_wps == 0), + num_infeasible_points=num_infeasible_wps, + ), + per_waypoint_solutions, + ) + + def _save_trajectory_to_file( + self, + original_trajectory: JointTrajectory, + articutool_solutions: List[ArticutoolWaypointSolution], + task_id: int, + ) -> Optional[str]: + """Saves the trajectory data to a JSON file.""" + if not self.trajectory_save_dir: + return None + try: + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S-%f") + mode = "pos_yaw" if self.constrain_goal_yaw else "pos_only" + filename = f"task_{mode}_{task_id}_{timestamp}.json" + filepath = os.path.join(self.trajectory_save_dir, filename) + waypoints_data = [ + { + "time_from_start_sec": p.time_from_start.sec + + p.time_from_start.nanosec * 1e-9, + "jaco_positions_rad": list(p.positions), + "articutool_waypoint_feasible": s.waypoint_feasible, + "articutool_pitch_solution_rad": s.pitch_solution_rad, + "articutool_roll_solution_rad": s.roll_solution_rad, + } + for p, s in zip(original_trajectory.points, articutool_solutions) + ] + enhanced_data = { + "jaco_joint_names": list(original_trajectory.joint_names), + "waypoints": waypoints_data, + } + with open(filepath, "w") as f: + json.dump(enhanced_data, f, indent=2) + return filename + except Exception as e: + LOGGER.error(f"Failed to save trajectory for task {task_id}: {e}") + return None + + def run(self): + """Main benchmark execution loop.""" + if not self.pinocchio_model: + LOGGER.error("Aborting benchmark: Pinocchio model failed to initialize.") + return + + for i in range(self.num_tasks): + LOGGER.info(f"--- Running Task {i + 1}/{self.num_tasks} ---") + + # 1. Generate a random target position and optional yaw + target_position = self.generate_random_position_in_workspace() + target_quat_xyzw = None + if self.constrain_goal_yaw: + target_quat_xyzw = self.generate_random_yaw_quaternion() + LOGGER.info( + f" Target Position (x,y,z): {np.round(target_position, 3).tolist()}" + ) + LOGGER.info( + f" Target Goal Yaw Quat (x,y,z,w): {np.round(target_quat_xyzw, 3).tolist()}" + ) + else: + LOGGER.info( + f" Target Position (x,y,z): {np.round(target_position, 3).tolist()}" + ) + + # 2. Set up the planning request + self.moveit2.clear_goal_constraints() + self.moveit2.clear_path_constraints() + + # Goal Constraint(s) + self.moveit2.set_position_goal( + position=target_position.tolist(), + frame_id=BASE_LINK, + target_link=END_EFFECTOR_LINK, + tolerance=0.01, + ) + if self.constrain_goal_yaw and target_quat_xyzw is not None: + self.moveit2.set_orientation_goal( + quat_xyzw=Quaternion( + x=target_quat_xyzw[0], + y=target_quat_xyzw[1], + z=target_quat_xyzw[2], + w=target_quat_xyzw[3], + ), + target_link=END_EFFECTOR_LINK, + tolerance=GOAL_YAW_CONSTRAINT_TOLERANCE_XYZ_RAD, + parameterization=1, + ) + + # Path is always governed by our "smart" OrientationConstraint + self.moveit2.set_path_orientation_constraint( + quat_xyzw=Quaternion( + x=PATH_CONSTRAINT_QUAT_XYZW[0], + y=PATH_CONSTRAINT_QUAT_XYZW[1], + z=PATH_CONSTRAINT_QUAT_XYZW[2], + w=PATH_CONSTRAINT_QUAT_XYZW[3], + ), + frame_id=BASE_LINK, + target_link=END_EFFECTOR_LINK, + tolerance=PATH_CONSTRAINT_TOLERANCE_XYZ_RAD, + weight=1.0, + parameterization=1, + ) + + self.moveit2.planner_id = self.planner_id + self.moveit2.allowed_planning_time = self.planning_timeout + + # 3. Plan the motion + planning_start_time = time.perf_counter() + plan_future = self.moveit2.plan_async() + while rclpy.ok() and not plan_future.done(): + time.sleep(0.01) + trajectory = self.moveit2.get_trajectory(plan_future) + planning_time = time.perf_counter() - planning_start_time + + plan_success = trajectory is not None and bool(trajectory.points) + articutool_metrics = None + if plan_success: + LOGGER.info(f" Jaco plan SUCCEEDED in {planning_time:.4f}s.") + articutool_metrics, solutions = self._verify_trajectory_feasibility( + trajectory + ) + LOGGER.info( + f" Verification Result: Path Feasible = {articutool_metrics.path_feasible} ({articutool_metrics.num_infeasible_points} infeasible points)" + ) + self._save_trajectory_to_file(trajectory, solutions, i) + else: + LOGGER.warn(f" Jaco plan FAILED in {planning_time:.4f}s.") + + self.results.append( + { + "task_id": i, + "target_position": target_position.tolist(), + "target_yaw_quat": target_quat_xyzw, + "plan_success": plan_success, + "planning_time_s": planning_time, + **(articutool_metrics._asdict() if articutool_metrics else {}), + } + ) + + def save_results(self, output_dir: str): + """Saves the benchmark results to a JSON file and prints a summary.""" + os.makedirs(output_dir, exist_ok=True) + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") + mode = "pos_yaw" if self.constrain_goal_yaw else "pos_only" + filename = os.path.join( + output_dir, f"benchmark_results_{mode}_{timestamp}.json" + ) + with open(filename, "w") as f: + json.dump(self.results, f, indent=2) + LOGGER.info(f"Benchmark results saved to {filename}") + + total_tasks = len(self.results) + jaco_successes = sum(1 for r in self.results if r["plan_success"]) + verified_successes = sum( + 1 for r in self.results if r.get("path_feasible", False) + ) + jaco_success_rate = ( + (jaco_successes / total_tasks * 100) if total_tasks > 0 else 0 + ) + + LOGGER.info(f"\n--- TASK-SPACE BENCHMARK SUMMARY ({mode.upper()}) ---") + LOGGER.info(f"Total Tasks Attempted: {total_tasks}") + LOGGER.info( + f"Jaco Planner Success Rate: {jaco_success_rate:.2f}% ({jaco_successes}/{total_tasks})" + ) + if jaco_successes > 0: + verification_rate = verified_successes / jaco_successes * 100 + LOGGER.info( + f"Verification Success Rate (of successful plans): {verification_rate:.2f}%" + ) + LOGGER.info("-------------------------------------\n") + + +def main(): + if len(sys.argv) < 2: + print( + "Usage: python3 constrained_task_space_benchmark.py /path/to/robot.urdf.xacro [--constrain-goal-yaw]" + ) + sys.exit(1) + xacro_file_arg = sys.argv[1] + if not os.path.exists(xacro_file_arg): + print(f"Error: XACRO file not found at '{xacro_file_arg}'") + sys.exit(1) + + constrain_goal_yaw_arg = "--constrain-goal-yaw" in sys.argv + + rclpy.init() + node = Node("constrained_task_space_benchmark_node") + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + executor_thread = Thread(target=executor.spin, daemon=True) + executor_thread.start() + + moveit2 = MoveIt2( + node=node, + joint_names=JOINT_NAMES, + base_link_name=BASE_LINK, + end_effector_name=END_EFFECTOR_LINK, + group_name=PLANNING_GROUP, + callback_group=ReentrantCallbackGroup(), + ) + + NUM_TASKS = 100 + PLANNING_TIMEOUT = 10.0 # Increased slightly for more complex constrained planning + PLANNER_ID = "RRTConnectkConfigDefault" + OUTPUT_DIR = os.path.join(os.getcwd(), "constrained_task_space_benchmark_output") + + benchmark = ConstrainedTaskSpaceBenchmark( + node=node, + moveit2=moveit2, + xacro_file_path=xacro_file_arg, + num_tasks=NUM_TASKS, + planning_timeout=PLANNING_TIMEOUT, + planner_id=PLANNER_ID, + trajectory_save_dir=OUTPUT_DIR, + constrain_goal_yaw=constrain_goal_yaw_arg, + ) + + try: + if benchmark.pinocchio_model is not None: + benchmark.run() + benchmark.save_results(OUTPUT_DIR) + else: + LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") + except Exception as e: + LOGGER.error(f"An error occurred during the benchmark: {e}", exc_info=True) + finally: + LOGGER.info("Shutting down.") + rclpy.shutdown() + executor_thread.join() + + +if __name__ == "__main__": + main() From d68524c5906398695030735b93f964addbc6f730 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Jul 2025 22:17:06 -0700 Subject: [PATCH 140/238] Update constrained task space benchmark to enhance logging, including required velocities for Articutool to maintain orientation --- .../constrained_task_space_benchmark.py | 470 +++++++++++------- 1 file changed, 287 insertions(+), 183 deletions(-) diff --git a/ada_feeding/scripts/constrained_task_space_benchmark.py b/ada_feeding/scripts/constrained_task_space_benchmark.py index ae79ec28..cb1e4a7b 100644 --- a/ada_feeding/scripts/constrained_task_space_benchmark.py +++ b/ada_feeding/scripts/constrained_task_space_benchmark.py @@ -4,11 +4,14 @@ """ This script runs a benchmark to evaluate a "constraint-driven planning" -methodology for the Articutool system. +methodology for the Articutool system. It features comprehensive, single-file +logging designed for easy data analysis and plotting for publications. + +This version is a PURE SIMULATION. It does not move the robot. Each planning +task starts from the end-state of the previously computed plan. It can be run in two modes: -1. Position-Only Goals (default): Plans to a random (x, y, z) position, - leaving the final orientation unconstrained. +1. Position-Only Goals (default): Plans to a random (x, y, z) position. 2. Position + Yaw Goals (`--constrain-goal-yaw`): Plans to a random (x, y, z) position AND a random final yaw orientation. @@ -57,33 +60,25 @@ # --- Parameters for the "Smart" PATH Orientation Constraint --- PATH_CONSTRAINT_QUAT_XYZW = (0.707, 0.0, 0.0, 0.707) -PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = ( - 1.5, - 3.14, - 0.8, -) # (Pitch, Yaw, Roll) - Roll relaxed to 0.8 +PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = (1.5, 3.14, 0.8) # --- Parameters for the optional GOAL Yaw Orientation Constraint --- -GOAL_YAW_CONSTRAINT_TOLERANCE_XYZ_RAD = ( - math.pi, - math.pi, - 0.1, -) # Loose Pitch/Roll, Tight Yaw +GOAL_YAW_CONSTRAINT_TOLERANCE_XYZ_RAD = (math.pi, math.pi, 0.1) # --- Define a reasonable workspace for sampling random positions --- -WORKSPACE_OUTER_RADIUS = 0.7 # Max reach in meters -WORKSPACE_INNER_RADIUS = 0.3 # Min distance to avoid singularity at base +WORKSPACE_OUTER_RADIUS = 0.7 +WORKSPACE_INNER_RADIUS = 0.3 # --- Data Structures for Clarity --- -ArticutoolWaypointSolution = namedtuple( - "ArticutoolWaypointSolution", - ["waypoint_feasible", "pitch_solution_rad", "roll_solution_rad"], -) -ArticutoolMetrics = namedtuple( - "ArticutoolMetrics", +TrajectoryMetrics = namedtuple( + "TrajectoryMetrics", [ - "path_feasible", - "num_infeasible_points", + "duration_s", + "joint_space_path_length_rad", + "final_joint_positions", + "articutool_pitch_stats_rad", + "articutool_roll_stats_rad", + "waypoints_data", ], ) @@ -99,7 +94,7 @@ def __init__( num_tasks: int = 100, planning_timeout: float = 5.0, planner_id: str = "RRTConnectkConfigDefault", - trajectory_save_dir: Optional[str] = None, + output_dir: Optional[str] = None, constrain_goal_yaw: bool = False, ): self.node = node @@ -108,7 +103,7 @@ def __init__( self.num_tasks = num_tasks self.planning_timeout = planning_timeout self.planner_id = planner_id - self.trajectory_save_dir = trajectory_save_dir + self.output_dir = output_dir self.constrain_goal_yaw = constrain_goal_yaw self.results: List[Dict[str, Any]] = [] @@ -116,7 +111,10 @@ def __init__( self.pinocchio_data: Optional[pin.Data] = None self.jaco_ee_frame_id_pin: Optional[int] = None self.joint_name_to_pinocchio_id: Dict[str, int] = {} + self.jaco_vel_indices: List[int] = [] self._initialize_pinocchio() + self.joint_limits = self._get_joint_limits() + self.debug_printed = False # Flag to print debug logs only once LOGGER.info("Benchmark simulator initialized.") if self.constrain_goal_yaw: @@ -125,9 +123,9 @@ def __init__( LOGGER.info("Planning Mode: Position-Only Goals.") LOGGER.info(f"Applying 'smart' orientation constraint to all paths.") - if self.trajectory_save_dir: - os.makedirs(self.trajectory_save_dir, exist_ok=True) - LOGGER.info(f"Trajectories will be saved to: {self.trajectory_save_dir}") + if self.output_dir: + os.makedirs(self.output_dir, exist_ok=True) + LOGGER.info(f"Comprehensive results will be saved to: {self.output_dir}") def _initialize_pinocchio(self): """Loads the robot model from a XACRO file into Pinocchio.""" @@ -146,18 +144,40 @@ def _initialize_pinocchio(self): ) for name in JOINT_NAMES: if self.pinocchio_model.existJointName(name): - self.joint_name_to_pinocchio_id[name] = ( - self.pinocchio_model.getJointId(name) + joint_id = self.pinocchio_model.getJointId(name) + self.joint_name_to_pinocchio_id[name] = joint_id + # Store the velocity index for this joint + self.jaco_vel_indices.append( + self.pinocchio_model.joints[joint_id].idx_v ) + LOGGER.info("Pinocchio model loaded successfully.") except (FileNotFoundError, subprocess.CalledProcessError, Exception) as e: LOGGER.error(f"Failed to initialize Pinocchio model: {e}") self.pinocchio_model = None + def _get_joint_limits(self) -> List[Tuple[float, float]]: + """Retrieves joint limits from the Pinocchio model.""" + if self.pinocchio_model is None: + return [(-math.pi, math.pi)] * len(JOINT_NAMES) + limits = [] + for name in JOINT_NAMES: + if self.pinocchio_model.existJointName(name): + joint_id = self.pinocchio_model.getJointId(name) + idx = self.pinocchio_model.joints[joint_id].idx_q + limits.append( + ( + self.pinocchio_model.lowerPositionLimit[idx], + self.pinocchio_model.upperPositionLimit[idx], + ) + ) + return limits + def generate_random_position_in_workspace(self) -> np.ndarray: """Generates a random (x, y, z) position from a half-spherical shell.""" - r_outer, r_inner = WORKSPACE_OUTER_RADIUS, WORKSPACE_INNER_RADIUS - r = (np.random.uniform(r_inner**3, r_outer**3)) ** (1 / 3) + r = ( + np.random.uniform(WORKSPACE_INNER_RADIUS**3, WORKSPACE_OUTER_RADIUS**3) + ) ** (1 / 3) theta = np.random.uniform(0, 2 * math.pi) cos_phi = np.random.uniform(0, 1) phi = math.acos(cos_phi) @@ -169,10 +189,22 @@ def generate_random_position_in_workspace(self) -> np.ndarray: def generate_random_yaw_quaternion(self) -> Tuple[float, float, float, float]: """Generates a quaternion representing a random yaw.""" random_yaw_angle = np.random.uniform(-math.pi, math.pi) - # Scipy handles conversion from Euler angles (Z-axis for yaw) to quaternion quat_xyzw = R.from_euler("z", random_yaw_angle).as_quat() return tuple(quat_xyzw) + def _get_articutool_jacobian(self, pitch: float, roll: float) -> np.ndarray: + """ + Computes the analytical Jacobian for the Articutool's 'up' vector (y-axis). + This relates Articutool joint velocities to the angular velocity of its y-axis. + J = [∂y/∂θp, ∂y/∂θr] + """ + cp, sp = math.cos(pitch), math.sin(pitch) + cr, sr = math.cos(roll), math.sin(roll) + # Partial derivatives of the y-axis vector [-sr, cp*cr, sp*cr] + dydp = np.array([0, -sp * cr, cp * cr]) + dydr = np.array([-cr, -cp * sr, -sp * sr]) + return np.vstack([dydp, dydr]).T + def _solve_articutool_ik_for_leveling( self, target_y_axis_in_atool_base: np.ndarray ) -> List[Tuple[float, float]]: @@ -205,103 +237,175 @@ def _solve_articutool_ik_for_leveling( ) return solutions - def _verify_trajectory_feasibility( + def _calculate_trajectory_metrics( self, trajectory: JointTrajectory - ) -> Tuple[ArticutoolMetrics, List[ArticutoolWaypointSolution]]: - """Verifies if a given trajectory is feasible for the Articutool.""" - if self.pinocchio_model is None: - return (ArticutoolMetrics(False, len(trajectory.points)), []) + ) -> TrajectoryMetrics: + """Calculates detailed metrics for a given trajectory.""" + if self.pinocchio_model is None or not trajectory.points: + return TrajectoryMetrics(0.0, 0.0, [], {}, {}, []) - num_infeasible_wps = 0 - per_waypoint_solutions: List[ArticutoolWaypointSolution] = [] + joint_space_path_length = 0.0 + waypoints_data = [] q = pin.neutral(self.pinocchio_model) + prev_positions = np.array(trajectory.points[0].positions) + all_pitches, all_rolls = [], [] - for point in trajectory.points: - for i, name in enumerate(JOINT_NAMES): + for i, point in enumerate(trajectory.points): + # Update Pinocchio model for current waypoint + for j, name in enumerate(JOINT_NAMES): if name in self.joint_name_to_pinocchio_id: joint_id = self.joint_name_to_pinocchio_id[name] joint_obj = self.pinocchio_model.joints[joint_id] - theta = point.positions[i] - if joint_obj.nq == 2 and joint_obj.nv == 1: - q[joint_obj.idx_q] = math.cos(theta) - q[joint_obj.idx_q + 1] = math.sin(theta) + theta = point.positions[j] + if joint_obj.nq == 2: + q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ + math.cos(theta), + math.sin(theta), + ] else: q[joint_obj.idx_q] = theta - pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) + pin.computeAllTerms( + self.pinocchio_model, + self.pinocchio_data, + q, + np.zeros(self.pinocchio_model.nv), + ) pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) - ee_transform: pin.SE3 = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + + # Get EE Pose and Articutool IK solution + ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] R_world_ee = R.from_matrix(ee_transform.rotation) - target_up_in_ee_frame = R_world_ee.inv().apply(WORLD_UP_VECTOR) - ik_solutions = self._solve_articutool_ik_for_leveling(target_up_in_ee_frame) - - valid_solutions = [ - s - for s in ik_solutions - if ( - ARTICUTOOL_PITCH_LIMITS_RAD[0] - EPSILON + target_up = R_world_ee.inv().apply(WORLD_UP_VECTOR) + solutions = self._solve_articutool_ik_for_leveling(target_up) + + at_solution, at_velocities = None, None + if solutions: + valid_sols = [ + s + for s in solutions + if ARTICUTOOL_PITCH_LIMITS_RAD[0] <= s[0] - <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + EPSILON - and ARTICUTOOL_ROLL_LIMITS_RAD[0] - EPSILON + <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] <= s[1] - <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + EPSILON - ) - ] - - if not valid_solutions: - num_infeasible_wps += 1 - per_waypoint_solutions.append( - ArticutoolWaypointSolution(False, None, None) - ) - else: - best_sol = min(valid_solutions, key=lambda s: s[0] ** 2 + s[1] ** 2) - per_waypoint_solutions.append( - ArticutoolWaypointSolution(True, best_sol[0], best_sol[1]) - ) - - return ( - ArticutoolMetrics( - path_feasible=(num_infeasible_wps == 0), - num_infeasible_points=num_infeasible_wps, - ), - per_waypoint_solutions, - ) - - def _save_trajectory_to_file( - self, - original_trajectory: JointTrajectory, - articutool_solutions: List[ArticutoolWaypointSolution], - task_id: int, - ) -> Optional[str]: - """Saves the trajectory data to a JSON file.""" - if not self.trajectory_save_dir: - return None - try: - timestamp = datetime.now().strftime("%Y%m%d-%H%M%S-%f") - mode = "pos_yaw" if self.constrain_goal_yaw else "pos_only" - filename = f"task_{mode}_{task_id}_{timestamp}.json" - filepath = os.path.join(self.trajectory_save_dir, filename) - waypoints_data = [ + <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + ] + if valid_sols: + pitch, roll = min(valid_sols, key=lambda s: s[0] ** 2 + s[1] ** 2) + at_solution = {"pitch": pitch, "roll": roll} + all_pitches.append(pitch) + all_rolls.append(roll) + + # Calculate required Articutool velocities + if i > 0: + dt = ( + point.time_from_start.sec + + point.time_from_start.nanosec * 1e-9 + ) - ( + trajectory.points[i - 1].time_from_start.sec + + trajectory.points[i - 1].time_from_start.nanosec * 1e-9 + ) + if dt > EPSILON: + q_dot_jaco = ( + np.array(point.positions) + - np.array(trajectory.points[i - 1].positions) + ) / dt + + J_jaco_full = pin.getFrameJacobian( + self.pinocchio_model, + self.pinocchio_data, + self.jaco_ee_frame_id_pin, + pin.ReferenceFrame.LOCAL, + ) + J_jaco_arm = J_jaco_full[:, self.jaco_vel_indices] + + v_ee = J_jaco_arm @ q_dot_jaco + omega_disturbance_local = v_ee[ + 3: + ] # Angular velocity portion + + J_atool = self._get_articutool_jacobian(pitch, roll) + J_atool_inv = np.linalg.pinv(J_atool) + q_dot_atool = -J_atool_inv @ omega_disturbance_local + at_velocities = { + "pitch_vel": q_dot_atool[0], + "roll_vel": q_dot_atool[1], + } + + # --- START DEBUG LOGS --- + if not self.debug_printed and i < 5: + LOGGER.info(f"--- DEBUG WP {i} ---") + LOGGER.info(f" dt: {dt:.4f}s") + LOGGER.info(f" q_dot_jaco: {np.round(q_dot_jaco, 3)}") + LOGGER.info(f" J_jaco_full shape: {J_jaco_full.shape}") + LOGGER.info(f" J_jaco_arm shape: {J_jaco_arm.shape}") + LOGGER.info(f" v_ee (local): {np.round(v_ee, 3)}") + LOGGER.info( + f" omega_disturbance_local: {np.round(omega_disturbance_local, 3)}" + ) + LOGGER.info(f" J_atool:\n{np.round(J_atool, 3)}") + LOGGER.info( + f" q_dot_atool (req'd): {np.round(q_dot_atool, 3)}" + ) + # --- END DEBUG LOGS --- + + waypoints_data.append( { - "time_from_start_sec": p.time_from_start.sec - + p.time_from_start.nanosec * 1e-9, - "jaco_positions_rad": list(p.positions), - "articutool_waypoint_feasible": s.waypoint_feasible, - "articutool_pitch_solution_rad": s.pitch_solution_rad, - "articutool_roll_solution_rad": s.roll_solution_rad, + "time_from_start_sec": point.time_from_start.sec + + point.time_from_start.nanosec * 1e-9, + "jaco_positions_rad": list(point.positions), + "ee_pose_world": { + "position": ee_transform.translation.tolist(), + "quat_xyzw": R.from_matrix(ee_transform.rotation) + .as_quat() + .tolist(), + }, + "articutool_solution_rad": at_solution, + "articutool_velocities_rad_per_sec": at_velocities, } - for p, s in zip(original_trajectory.points, articutool_solutions) - ] - enhanced_data = { - "jaco_joint_names": list(original_trajectory.joint_names), - "waypoints": waypoints_data, + ) + joint_space_path_length += np.linalg.norm( + np.array(point.positions) - prev_positions + ) + prev_positions = np.array(point.positions) + + self.debug_printed = ( + True # Ensure debug logs only print for the first trajectory + ) + duration = ( + trajectory.points[-1].time_from_start.sec + + trajectory.points[-1].time_from_start.nanosec * 1e-9 + ) + pitch_stats = ( + { + "min": min(all_pitches), + "max": max(all_pitches), + "mean": np.mean(all_pitches), + "std_dev": np.std(all_pitches), } - with open(filepath, "w") as f: - json.dump(enhanced_data, f, indent=2) - return filename - except Exception as e: - LOGGER.error(f"Failed to save trajectory for task {task_id}: {e}") - return None + if all_pitches + else {} + ) + roll_stats = ( + { + "min": min(all_rolls), + "max": max(all_rolls), + "mean": np.mean(all_rolls), + "std_dev": np.std(all_rolls), + } + if all_rolls + else {} + ) + + return TrajectoryMetrics( + duration, + joint_space_path_length, + list(trajectory.points[-1].positions), + pitch_stats, + roll_stats, + waypoints_data, + ) def run(self): """Main benchmark execution loop.""" @@ -312,34 +416,27 @@ def run(self): for i in range(self.num_tasks): LOGGER.info(f"--- Running Task {i + 1}/{self.num_tasks} ---") - # 1. Generate a random target position and optional yaw + # Get the starting joint state for this trial from the robot's current position + start_joint_positions = self.moveit2.joint_state.position + + # Generate task goals target_position = self.generate_random_position_in_workspace() - target_quat_xyzw = None - if self.constrain_goal_yaw: - target_quat_xyzw = self.generate_random_yaw_quaternion() - LOGGER.info( - f" Target Position (x,y,z): {np.round(target_position, 3).tolist()}" - ) - LOGGER.info( - f" Target Goal Yaw Quat (x,y,z,w): {np.round(target_quat_xyzw, 3).tolist()}" - ) - else: - LOGGER.info( - f" Target Position (x,y,z): {np.round(target_position, 3).tolist()}" - ) + target_quat_xyzw = ( + self.generate_random_yaw_quaternion() + if self.constrain_goal_yaw + else None + ) - # 2. Set up the planning request + # Set up and execute planning request self.moveit2.clear_goal_constraints() self.moveit2.clear_path_constraints() - - # Goal Constraint(s) self.moveit2.set_position_goal( position=target_position.tolist(), frame_id=BASE_LINK, target_link=END_EFFECTOR_LINK, tolerance=0.01, ) - if self.constrain_goal_yaw and target_quat_xyzw is not None: + if self.constrain_goal_yaw: self.moveit2.set_orientation_goal( quat_xyzw=Quaternion( x=target_quat_xyzw[0], @@ -351,8 +448,6 @@ def run(self): tolerance=GOAL_YAW_CONSTRAINT_TOLERANCE_XYZ_RAD, parameterization=1, ) - - # Path is always governed by our "smart" OrientationConstraint self.moveit2.set_path_orientation_constraint( quat_xyzw=Quaternion( x=PATH_CONSTRAINT_QUAT_XYZW[0], @@ -366,11 +461,9 @@ def run(self): weight=1.0, parameterization=1, ) - self.moveit2.planner_id = self.planner_id self.moveit2.allowed_planning_time = self.planning_timeout - # 3. Plan the motion planning_start_time = time.perf_counter() plan_future = self.moveit2.plan_async() while rclpy.ok() and not plan_future.done(): @@ -379,60 +472,64 @@ def run(self): planning_time = time.perf_counter() - planning_start_time plan_success = trajectory is not None and bool(trajectory.points) - articutool_metrics = None + trial_data = { + "task_id": i, + "planning_mode": "pos_yaw" if self.constrain_goal_yaw else "pos_only", + "start_joint_positions": list(start_joint_positions), + "target_position": target_position.tolist(), + "target_yaw_quat": target_quat_xyzw, + "plan_success": plan_success, + "planning_time_s": planning_time, + "verification_success": None, + "trajectory_metrics": None, + } + if plan_success: LOGGER.info(f" Jaco plan SUCCEEDED in {planning_time:.4f}s.") - articutool_metrics, solutions = self._verify_trajectory_feasibility( - trajectory + metrics = self._calculate_trajectory_metrics(trajectory) + is_feasible = all( + wp["articutool_solution_rad"] is not None + for wp in metrics.waypoints_data ) - LOGGER.info( - f" Verification Result: Path Feasible = {articutool_metrics.path_feasible} ({articutool_metrics.num_infeasible_points} infeasible points)" - ) - self._save_trajectory_to_file(trajectory, solutions, i) + trial_data["verification_success"] = is_feasible + trial_data["trajectory_metrics"] = metrics._asdict() + LOGGER.info(f" Verification Result: Path Feasible = {is_feasible}") else: LOGGER.warn(f" Jaco plan FAILED in {planning_time:.4f}s.") - self.results.append( - { - "task_id": i, - "target_position": target_position.tolist(), - "target_yaw_quat": target_quat_xyzw, - "plan_success": plan_success, - "planning_time_s": planning_time, - **(articutool_metrics._asdict() if articutool_metrics else {}), - } - ) + self.results.append(trial_data) - def save_results(self, output_dir: str): - """Saves the benchmark results to a JSON file and prints a summary.""" - os.makedirs(output_dir, exist_ok=True) + def save_results(self): + """Saves the comprehensive benchmark results to a single JSON file.""" + if not self.output_dir: + return timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") mode = "pos_yaw" if self.constrain_goal_yaw else "pos_only" filename = os.path.join( - output_dir, f"benchmark_results_{mode}_{timestamp}.json" + self.output_dir, f"benchmark_results_{mode}_{timestamp}.json" ) - with open(filename, "w") as f: - json.dump(self.results, f, indent=2) - LOGGER.info(f"Benchmark results saved to {filename}") - - total_tasks = len(self.results) - jaco_successes = sum(1 for r in self.results if r["plan_success"]) - verified_successes = sum( - 1 for r in self.results if r.get("path_feasible", False) + try: + with open(filename, "w") as f: + json.dump(self.results, f, indent=2) + LOGGER.info(f"Comprehensive benchmark results saved to {filename}") + except Exception as e: + LOGGER.error(f"Failed to save results: {e}") + total_tasks, jaco_successes, verified_successes = ( + len(self.results), + sum(1 for r in self.results if r["plan_success"]), + sum(1 for r in self.results if r.get("verification_success", False)), ) jaco_success_rate = ( (jaco_successes / total_tasks * 100) if total_tasks > 0 else 0 ) - LOGGER.info(f"\n--- TASK-SPACE BENCHMARK SUMMARY ({mode.upper()}) ---") LOGGER.info(f"Total Tasks Attempted: {total_tasks}") LOGGER.info( f"Jaco Planner Success Rate: {jaco_success_rate:.2f}% ({jaco_successes}/{total_tasks})" ) if jaco_successes > 0: - verification_rate = verified_successes / jaco_successes * 100 LOGGER.info( - f"Verification Success Rate (of successful plans): {verification_rate:.2f}%" + f"Verification Success Rate (of successful plans): {(verified_successes / jaco_successes * 100):.2f}%" ) LOGGER.info("-------------------------------------\n") @@ -443,13 +540,14 @@ def main(): "Usage: python3 constrained_task_space_benchmark.py /path/to/robot.urdf.xacro [--constrain-goal-yaw]" ) sys.exit(1) - xacro_file_arg = sys.argv[1] + xacro_file_arg, constrain_goal_yaw_arg = ( + sys.argv[1], + "--constrain-goal-yaw" in sys.argv, + ) if not os.path.exists(xacro_file_arg): print(f"Error: XACRO file not found at '{xacro_file_arg}'") sys.exit(1) - constrain_goal_yaw_arg = "--constrain-goal-yaw" in sys.argv - rclpy.init() node = Node("constrained_task_space_benchmark_node") executor = rclpy.executors.MultiThreadedExecutor() @@ -466,26 +564,32 @@ def main(): callback_group=ReentrantCallbackGroup(), ) - NUM_TASKS = 100 - PLANNING_TIMEOUT = 10.0 # Increased slightly for more complex constrained planning - PLANNER_ID = "RRTConnectkConfigDefault" - OUTPUT_DIR = os.path.join(os.getcwd(), "constrained_task_space_benchmark_output") + NUM_TASKS, PLANNING_TIMEOUT, PLANNER_ID, OUTPUT_DIR = ( + 100, + 10.0, + "RRTConnectkConfigDefault", + os.path.join(os.getcwd(), "constrained_task_space_benchmark_output"), + ) benchmark = ConstrainedTaskSpaceBenchmark( - node=node, - moveit2=moveit2, - xacro_file_path=xacro_file_arg, - num_tasks=NUM_TASKS, - planning_timeout=PLANNING_TIMEOUT, - planner_id=PLANNER_ID, - trajectory_save_dir=OUTPUT_DIR, - constrain_goal_yaw=constrain_goal_yaw_arg, + node, + moveit2, + xacro_file_arg, + NUM_TASKS, + PLANNING_TIMEOUT, + PLANNER_ID, + OUTPUT_DIR, + constrain_goal_yaw_arg, ) try: if benchmark.pinocchio_model is not None: + # This benchmark is now a pure simulation. It starts from the robot's + # current state and each subsequent plan starts from the end of the + # previous one, without actually moving the robot. + LOGGER.info("Ready to begin benchmark from current robot configuration.") benchmark.run() - benchmark.save_results(OUTPUT_DIR) + benchmark.save_results() else: LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") except Exception as e: From bbea5c16b893c7b48867ce137d9a0439bc157c9c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 20 Jul 2025 22:17:47 -0700 Subject: [PATCH 141/238] Add script to analyze and plot benchmark data --- .../benchmark_analysis_and_plotting.py | 278 ++++++++++++++++++ 1 file changed, 278 insertions(+) create mode 100644 ada_feeding/scripts/benchmark_analysis_and_plotting.py diff --git a/ada_feeding/scripts/benchmark_analysis_and_plotting.py b/ada_feeding/scripts/benchmark_analysis_and_plotting.py new file mode 100644 index 00000000..c9b415d7 --- /dev/null +++ b/ada_feeding/scripts/benchmark_analysis_and_plotting.py @@ -0,0 +1,278 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script loads the comprehensive JSON results file generated by +`constrained_task_space_benchmark.py`, performs a thorough analysis, +and generates a series of high-quality, interactive plots using Plotly. + +The generated plots are saved as HTML files, suitable for inclusion in +publications and presentations. + +Requires: pandas, plotly +Install with: pip install pandas plotly +""" + +import argparse +import json +import os +from typing import List, Dict, Any + +import numpy as np +import pandas as pd +import plotly.express as px +import plotly.graph_objects as go + + +def load_data(json_path: str) -> pd.DataFrame: + """Loads the benchmark results from a JSON file into a pandas DataFrame.""" + print(f"Loading data from {json_path}...") + try: + with open(json_path, "r") as f: + data = json.load(f) + # Normalize the nested dictionary structure into a flat DataFrame + df = pd.json_normalize(data, sep="_") + print(f"Successfully loaded {len(df)} trials.") + return df + except (FileNotFoundError, json.JSONDecodeError, KeyError) as e: + print(f"Error loading or parsing JSON file: {e}") + return pd.DataFrame() + + +def plot_success_rates(df: pd.DataFrame, output_dir: str): + """Plots planner success rate and verification success rate by planning mode.""" + if df.empty: + return + + print("Generating success rate plot...") + # Group by planning mode and calculate mean success rates + summary = ( + df.groupby("planning_mode") + .agg( + planner_success_rate=("plan_success", "mean"), + verification_success_rate=("verification_success", "mean"), + ) + .reset_index() + ) + summary["planner_success_rate"] *= 100 + summary["verification_success_rate"] *= 100 + + # --- FIX: Convert data from wide format to long format for robust plotting --- + # This resolves the "columns of different type" error by restructuring the data. + summary_long = summary.melt( + id_vars="planning_mode", + value_vars=["planner_success_rate", "verification_success_rate"], + var_name="Metric", + value_name="Success Rate (%)", + ) + + fig = px.bar( + summary_long, + x="planning_mode", + y="Success Rate (%)", + color="Metric", # Use color to differentiate the metrics + title="Planner and Verification Success Rates by Planning Mode", + labels={ + "Success Rate (%)": "Success Rate (%)", + "planning_mode": "Planning Mode", + }, + barmode="group", + text_auto=".2f", + ) + # --- END FIX --- + + fig.update_layout(yaxis_range=[0, 105]) + + filename = os.path.join(output_dir, "success_rates.html") + fig.write_html(filename) + print(f" - Saved to {filename}") + + +def plot_planning_times(df: pd.DataFrame, output_dir: str): + """Plots the distribution of planning times for successful plans.""" + if df.empty: + return + + print("Generating planning time distribution plot...") + successful_plans = df[df["plan_success"] == True].copy() + + fig = px.box( + successful_plans, + x="planning_mode", + y="planning_time_s", + color="planning_mode", + title="Distribution of Planning Times for Successful Plans", + labels={ + "planning_time_s": "Planning Time (s)", + "planning_mode": "Planning Mode", + }, + points="all", # Show all data points + ) + + filename = os.path.join(output_dir, "planning_times.html") + fig.write_html(filename) + print(f" - Saved to {filename}") + + +def plot_3d_trajectories(df: pd.DataFrame, output_dir: str, num_examples: int = 5): + """Plots start/end points and example trajectories in 3D space.""" + if df.empty or "trajectory_metrics_waypoints_data" not in df.columns: + return + + print("Generating 3D trajectory plot...") + successful_plans = df[df["plan_success"] == True].copy() + if successful_plans.empty: + print(" - No successful plans to plot.") + return + + # Extract start and end points + starts = np.array(successful_plans["start_joint_positions"].tolist()) + + # We need to compute FK for start and end points + # For simplicity in this offline script, we'll plot the target position + # and the first waypoint's EE position. + + end_points = [] + start_points = [] + for _, row in successful_plans.iterrows(): + waypoints = row["trajectory_metrics_waypoints_data"] + if waypoints and isinstance(waypoints, list) and len(waypoints) > 0: + start_points.append(waypoints[0]["ee_pose_world"]["position"]) + end_points.append(waypoints[-1]["ee_pose_world"]["position"]) + + fig = go.Figure() + + # Add scatter plot for start and end points + fig.add_trace( + go.Scatter3d( + x=[p[0] for p in start_points], + y=[p[1] for p in start_points], + z=[p[2] for p in start_points], + mode="markers", + marker=dict(color="blue", size=4, opacity=0.6), + name="Start EE Position", + ) + ) + fig.add_trace( + go.Scatter3d( + x=[p[0] for p in end_points], + y=[p[1] for p in end_points], + z=[p[2] for p in end_points], + mode="markers", + marker=dict(color="red", size=4, opacity=0.6), + name="End EE Position", + ) + ) + + # Add line plots for a few example trajectories + example_indices = np.random.choice( + successful_plans.index, min(num_examples, len(successful_plans)), replace=False + ) + for i, index in enumerate(example_indices): + waypoints = successful_plans.loc[index, "trajectory_metrics_waypoints_data"] + path_x = [wp["ee_pose_world"]["position"][0] for wp in waypoints] + path_y = [wp["ee_pose_world"]["position"][1] for wp in waypoints] + path_z = [wp["ee_pose_world"]["position"][2] for wp in waypoints] + fig.add_trace( + go.Scatter3d( + x=path_x, + y=path_y, + z=path_z, + mode="lines", + line=dict(width=5), + name=f"Example Traj {i + 1}", + ) + ) + + fig.update_layout( + title="3D Visualization of End-Effector Trajectories", + scene=dict( + xaxis_title="X (m)", + yaxis_title="Y (m)", + zaxis_title="Z (m)", + aspectmode="data", # Ensure aspect ratio is realistic + ), + margin=dict(l=0, r=0, b=0, t=40), + ) + + filename = os.path.join(output_dir, "3d_trajectories.html") + fig.write_html(filename) + print(f" - Saved to {filename}") + + +def plot_articutool_velocities(df: pd.DataFrame, output_dir: str): + """Plots the required Articutool joint velocities for all successful waypoints.""" + if df.empty or "trajectory_metrics_waypoints_data" not in df.columns: + return + + print("Generating Articutool velocity plot...") + successful_plans = df[df["plan_success"] == True].copy() + + all_pitch_vels = [] + all_roll_vels = [] + + for _, row in successful_plans.iterrows(): + waypoints = row["trajectory_metrics_waypoints_data"] + if waypoints and isinstance(waypoints, list): + for wp in waypoints: + vels = wp.get("articutool_velocities_rad_per_sec") + if vels and isinstance(vels, dict): + all_pitch_vels.append(vels.get("pitch_vel")) + all_roll_vels.append(vels.get("roll_vel")) + + if not all_pitch_vels: + print(" - No velocity data found to plot.") + return + + fig = px.scatter( + x=all_pitch_vels, + y=all_roll_vels, + title="Required Articutool Joint Velocities for Leveling", + labels={"x": "Pitch Velocity (rad/s)", "y": "Roll Velocity (rad/s)"}, + opacity=0.5, + ) + fig.update_traces(marker=dict(size=5)) + + filename = os.path.join(output_dir, "articutool_velocities.html") + fig.write_html(filename) + print(f" - Saved to {filename}") + + +def main(): + """Main function to load data and generate all plots.""" + parser = argparse.ArgumentParser( + description="Analyze benchmark results and generate plots." + ) + parser.add_argument( + "json_file", + type=str, + help="Path to the comprehensive benchmark results JSON file.", + ) + parser.add_argument( + "--output_dir", + type=str, + default="benchmark_plots", + help="Directory to save the generated plot HTML files.", + ) + args = parser.parse_args() + + # Create output directory if it doesn't exist + os.makedirs(args.output_dir, exist_ok=True) + + # Load data + df = load_data(args.json_file) + if df.empty: + return + + # Generate plots + plot_success_rates(df, args.output_dir) + plot_planning_times(df, args.output_dir) + plot_3d_trajectories(df, args.output_dir) + plot_articutool_velocities(df, args.output_dir) + + print("\nAnalysis complete. Plots saved in:", args.output_dir) + + +if __name__ == "__main__": + main() From c7402533c206bbbc4d07c5cdbf5ea09c6f90ac81 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 21 Jul 2025 23:02:03 -0700 Subject: [PATCH 142/238] Add WIP implementation for a more holistic benchmark that seeks to compare various planning strategies --- ada_feeding/scripts/holistic_benchmark.py | 617 ++++++++++++++++++ .../scripts/holistic_benchmark_analysis.py | 307 +++++++++ 2 files changed, 924 insertions(+) create mode 100644 ada_feeding/scripts/holistic_benchmark.py create mode 100644 ada_feeding/scripts/holistic_benchmark_analysis.py diff --git a/ada_feeding/scripts/holistic_benchmark.py b/ada_feeding/scripts/holistic_benchmark.py new file mode 100644 index 00000000..4c3a7cd2 --- /dev/null +++ b/ada_feeding/scripts/holistic_benchmark.py @@ -0,0 +1,617 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script provides a holistic benchmark to compare multiple planning +methodologies for the Jaco + Articutool system. Its goal is to generate a +comprehensive dataset to empirically evaluate the trade-offs between planner +success, motion flexibility, and guaranteed kinematic/dynamic feasibility. + +This version uses a collision-checking service to find a valid, random start +state for each trial, ensuring a robust and diverse evaluation. + +The script can be run in one of four modes via the `--mode` flag: +1. `joint_unconstrained`: The baseline. Plans between random joint-space goals + with no path constraints. +2. `task_pos_unconstrained`: Plans to a random task-space position goal with + no path constraints. +3. `task_pos_constrained`: Plans to a random task-space position goal WITH the + "smart" orientation path constraint. +4. `task_pos_yaw_constrained`: Plans to a random task-space position AND yaw + goal, WITH the "smart" orientation path constraint. +""" + +# Standard imports +from collections import namedtuple +from datetime import datetime +import os +import time +from threading import Thread +from typing import Optional, List, Dict, Tuple, Any +import json +import math +import subprocess +import sys +import argparse + +# Third-party imports +import numpy as np +from pymoveit2 import MoveIt2 +from pymoveit2.robots import kinova +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory +from geometry_msgs.msg import Quaternion +from moveit_msgs.srv import GetStateValidity +from moveit_msgs.msg import RobotState +from sensor_msgs.msg import JointState +from scipy.spatial.transform import Rotation as R +import pinocchio as pin + +# --- Constants --- +LOGGER = rclpy.logging.get_logger("holistic_benchmark") +PLANNING_GROUP = "jaco_arm" +JOINT_NAMES = kinova.joint_names() +BASE_LINK = kinova.base_link_name() +END_EFFECTOR_LINK = kinova.end_effector_name() +EPSILON = 1e-6 +ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) +ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) +WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) +PATH_CONSTRAINT_QUAT_XYZW = (0.707, 0.0, 0.0, 0.707) +PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = (1.5, 3.14, 0.8) +GOAL_YAW_CONSTRAINT_TOLERANCE_XYZ_RAD = (math.pi, math.pi, 0.1) +WORKSPACE_OUTER_RADIUS = 0.7 +WORKSPACE_INNER_RADIUS = 0.3 + +# --- Data Structures --- +TrajectoryMetrics = namedtuple( + "TrajectoryMetrics", + [ + "duration_s", + "joint_space_path_length_rad", + "final_joint_positions", + "waypoints_data", + ], +) + + +class HolisticBenchmark: + """Manages the holistic benchmark planning process.""" + + def __init__( + self, + node: Node, + moveit2: MoveIt2, + xacro_file_path: str, + mode: str, + num_tasks: int = 100, + planning_timeout: float = 10.0, + planner_id: str = "RRTConnectkConfigDefault", + output_dir: Optional[str] = None, + ): + self.node = node + self.moveit2 = moveit2 + self.xacro_file_path = xacro_file_path + self.mode = mode + self.num_tasks = num_tasks + self.planning_timeout = planning_timeout + self.planner_id = planner_id + self.output_dir = output_dir + self.results: List[Dict[str, Any]] = [] + + self.pinocchio_model: Optional[pin.Model] = None + self.pinocchio_data: Optional[pin.Data] = None + self.jaco_ee_frame_id_pin: Optional[int] = None + self.jaco_vel_indices: List[int] = [] + self._initialize_pinocchio() + self.joint_limits = self._get_joint_limits() + + self.service_callback_group = ReentrantCallbackGroup() + self.state_validity_client = self.node.create_client( + GetStateValidity, + "/check_state_validity", + callback_group=self.service_callback_group, + ) + if not self.state_validity_client.wait_for_service(timeout_sec=5.0): + LOGGER.error( + "Could not connect to /check_state_validity service. Cannot guarantee valid start states." + ) + self.state_validity_client = None + + LOGGER.info("Holistic Benchmark Initialized.") + LOGGER.info(f" - Planning Mode: {self.mode}") + if self.output_dir: + os.makedirs(self.output_dir, exist_ok=True) + LOGGER.info(f" - Results will be saved to: {self.output_dir}") + + def _initialize_pinocchio(self): + """Loads the robot model from a XACRO file into Pinocchio.""" + try: + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", self.xacro_file_path], + check=True, + capture_output=True, + text=True, + ) + urdf_xml_string = process.stdout + self.pinocchio_model = pin.buildModelFromXML(urdf_xml_string) + self.pinocchio_data = self.pinocchio_model.createData() + self.jaco_ee_frame_id_pin = self.pinocchio_model.getFrameId( + END_EFFECTOR_LINK + ) + for name in JOINT_NAMES: + if self.pinocchio_model.existJointName(name): + joint_id = self.pinocchio_model.getJointId(name) + self.jaco_vel_indices.append( + self.pinocchio_model.joints[joint_id].idx_v + ) + LOGGER.info("Pinocchio model loaded successfully.") + except Exception as e: + LOGGER.error(f"Failed to initialize Pinocchio model: {e}") + self.pinocchio_model = None + + def _get_joint_limits(self) -> List[Tuple[float, float]]: + """Retrieves joint limits from the Pinocchio model.""" + if self.pinocchio_model is None: + return [(-math.pi, math.pi)] * len(JOINT_NAMES) + limits = [] + for name in JOINT_NAMES: + if self.pinocchio_model.existJointName(name): + joint_id = self.pinocchio_model.getJointId(name) + idx = self.pinocchio_model.joints[joint_id].idx_q + limits.append( + ( + self.pinocchio_model.lowerPositionLimit[idx], + self.pinocchio_model.upperPositionLimit[idx], + ) + ) + return limits + + def generate_random_joint_config(self) -> List[float]: + """Generates a random joint configuration within limits.""" + return [np.random.uniform(low, high) for low, high in self.joint_limits] + + def is_state_valid(self, joint_positions: List[float]) -> bool: + """Checks if a joint configuration is collision-free using MoveIt's service.""" + if ( + not self.state_validity_client + or not self.state_validity_client.service_is_ready() + ): + LOGGER.warn("State validity client not available. Assuming state is valid.") + return True + + # --- FIX: Build a complete RobotState to avoid ambiguity --- + # Get the current full state of the robot + full_joint_state = self.moveit2.joint_state + joint_state_map = { + name: pos + for name, pos in zip(full_joint_state.name, full_joint_state.position) + } + + # Overwrite the Jaco arm joints with the configuration we want to test + for i, name in enumerate(JOINT_NAMES): + joint_state_map[name] = joint_positions[i] + + # Build the RobotState message with all joints + robot_state = RobotState() + robot_state.joint_state.name = list(joint_state_map.keys()) + robot_state.joint_state.position = list(joint_state_map.values()) + + req = GetStateValidity.Request() + req.group_name = PLANNING_GROUP + req.robot_state = robot_state + + future = self.state_validity_client.call_async(req) + + timeout_sec = 2.0 + start_time = self.node.get_clock().now() + while ( + rclpy.ok() + and (self.node.get_clock().now() - start_time).nanoseconds / 1e9 + < timeout_sec + ): + if future.done(): + try: + response = future.result() + return response.valid if response is not None else False + except Exception as e: + LOGGER.error( + f"Exception getting result from /check_state_validity: {e}" + ) + return False + time.sleep(0.01) + + LOGGER.error(f"Service call to /check_state_validity timed out.") + return False + + def generate_valid_random_joint_config( + self, max_attempts=100 + ) -> Optional[List[float]]: + """Generates a random, collision-free joint configuration.""" + for attempt in range(max_attempts): + config = self.generate_random_joint_config() + if self.is_state_valid(config): + LOGGER.info( + f" Found valid start configuration on attempt {attempt + 1}." + ) + return config + LOGGER.error( + f"Failed to find a valid random joint configuration after {max_attempts} attempts." + ) + return None + + def generate_random_position_in_workspace(self) -> np.ndarray: + """Generates a random (x, y, z) position from a half-spherical shell.""" + r = ( + np.random.uniform(WORKSPACE_INNER_RADIUS**3, WORKSPACE_OUTER_RADIUS**3) + ) ** (1 / 3) + theta = np.random.uniform(0, 2 * math.pi) + cos_phi = np.random.uniform(0, 1) + phi = math.acos(cos_phi) + return np.array( + [ + r * math.sin(phi) * math.cos(theta), + r * math.sin(phi) * math.sin(theta), + r * cos_phi, + ] + ) + + def generate_random_yaw_quaternion(self) -> Tuple[float, float, float, float]: + """Generates a quaternion representing a random yaw.""" + return tuple(R.from_euler("z", np.random.uniform(-math.pi, math.pi)).as_quat()) + + def _get_articutool_jacobian(self, pitch: float, roll: float) -> np.ndarray: + """Computes the analytical Jacobian for the Articutool.""" + cp, sp = math.cos(pitch), math.sin(pitch) + cr, sr = math.cos(roll), math.sin(roll) + dydp = np.array([0, -sp * cr, cp * cr]) + dydr = np.array([-cr, -cp * sr, -sp * sr]) + return np.vstack([dydp, dydr]).T + + def _solve_articutool_ik( + self, target_vector: np.ndarray + ) -> List[Tuple[float, float]]: + """Analytical IK solver for the Articutool.""" + vx, vy, vz = target_vector + solutions = [] + asin_arg = -vx + if not (-1.0 - EPSILON <= asin_arg <= 1.0 + EPSILON): + return [] + theta_r1 = math.asin(np.clip(asin_arg, -1.0, 1.0)) + theta_r2 = (math.pi - theta_r1 + math.pi) % (2 * math.pi) - math.pi + for theta_r in list(set([theta_r1, theta_r2])): + cos_tr = math.cos(theta_r) + if math.isclose(cos_tr, 0.0, abs_tol=EPSILON): + if math.isclose(vy, 0.0, abs_tol=EPSILON) and math.isclose( + vz, 0.0, abs_tol=EPSILON + ): + solutions.append( + (0.0, (theta_r + math.pi) % (2 * math.pi) - math.pi) + ) + continue + theta_p = math.atan2(vz, vy) + solutions.append( + ( + (theta_p + math.pi) % (2 * math.pi) - math.pi, + (theta_r + math.pi) % (2 * math.pi) - math.pi, + ) + ) + return solutions + + def _calculate_trajectory_metrics( + self, trajectory: JointTrajectory + ) -> TrajectoryMetrics: + """Calculates detailed metrics for a given trajectory.""" + if self.pinocchio_model is None or not trajectory.points: + return TrajectoryMetrics(0.0, 0.0, [], []) + + q = pin.neutral(self.pinocchio_model) + prev_pos = np.array(trajectory.points[0].positions) + waypoints_data, path_len = [], 0.0 + + for i, point in enumerate(trajectory.points): + for j, name in enumerate(JOINT_NAMES): + joint_id = self.pinocchio_model.getJointId(name) + joint_obj = self.pinocchio_model.joints[joint_id] + if joint_obj.nq == 2: + q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ + math.cos(point.positions[j]), + math.sin(point.positions[j]), + ] + else: + q[joint_obj.idx_q] = point.positions[j] + + pin.computeAllTerms( + self.pinocchio_model, + self.pinocchio_data, + q, + np.zeros(self.pinocchio_model.nv), + ) + ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + target_up = ( + R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) + ) + solutions = self._solve_articutool_ik(target_up) + + at_sol, at_vel = None, None + if solutions: + valid_sols = [ + s + for s in solutions + if ARTICUTOOL_PITCH_LIMITS_RAD[0] + <= s[0] + <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] + <= s[1] + <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + ] + if valid_sols: + pitch, roll = min(valid_sols, key=lambda s: s[0] ** 2 + s[1] ** 2) + at_sol = {"pitch": pitch, "roll": roll} + if i > 0: + dt = ( + point.time_from_start.sec + + 1e-9 * point.time_from_start.nanosec + ) - ( + trajectory.points[i - 1].time_from_start.sec + + 1e-9 * trajectory.points[i - 1].time_from_start.nanosec + ) + if dt > EPSILON: + q_dot = ( + np.array(point.positions) + - np.array(trajectory.points[i - 1].positions) + ) / dt + J_full = pin.getFrameJacobian( + self.pinocchio_model, + self.pinocchio_data, + self.jaco_ee_frame_id_pin, + pin.ReferenceFrame.LOCAL, + ) + J_arm = J_full[:, self.jaco_vel_indices] + omega = (J_arm @ q_dot)[3:] + J_atool_inv = np.linalg.pinv( + self._get_articutool_jacobian(pitch, roll) + ) + q_dot_atool = -J_atool_inv @ omega + at_vel = { + "pitch_vel": q_dot_atool[0], + "roll_vel": q_dot_atool[1], + } + + waypoints_data.append( + { + "jaco_positions_rad": list(point.positions), + "ee_pose_world": { + "position": ee_transform.translation.tolist(), + "quat_xyzw": R.from_matrix(ee_transform.rotation) + .as_quat() + .tolist(), + }, + "articutool_solution_rad": at_sol, + "articutool_velocities_rad_per_sec": at_vel, + } + ) + path_len += np.linalg.norm(np.array(point.positions) - prev_pos) + prev_pos = np.array(point.positions) + + duration = ( + trajectory.points[-1].time_from_start.sec + + 1e-9 * trajectory.points[-1].time_from_start.nanosec + ) + return TrajectoryMetrics( + duration, path_len, list(trajectory.points[-1].positions), waypoints_data + ) + + def run(self): + """Main benchmark execution loop.""" + if not self.pinocchio_model: + return + + for i in range(self.num_tasks): + LOGGER.info(f"--- Running Task {i + 1}/{self.num_tasks} ---") + + LOGGER.info(" Searching for a valid random start configuration...") + start_pos = self.generate_valid_random_joint_config() + if start_pos is None: + LOGGER.warn(" Skipping task, could not find a valid start state.") + continue + + self.moveit2.clear_goal_constraints() + self.moveit2.clear_path_constraints() + + target_pos, target_quat, goal_joint_pos = None, None, None + if self.mode == "joint_unconstrained": + goal_joint_pos = self.generate_valid_random_joint_config() + if goal_joint_pos is None: + LOGGER.warn(" Skipping task, could not find a valid goal state.") + continue + self.moveit2.set_joint_goal(joint_positions=goal_joint_pos) + else: + target_pos = self.generate_random_position_in_workspace() + self.moveit2.set_position_goal( + position=target_pos.tolist(), + frame_id=BASE_LINK, + target_link=END_EFFECTOR_LINK, + tolerance=0.01, + ) + if self.mode == "task_pos_yaw_constrained": + target_quat = self.generate_random_yaw_quaternion() + self.moveit2.set_orientation_goal( + quat_xyzw=Quaternion( + x=target_quat[0], + y=target_quat[1], + z=target_quat[2], + w=target_quat[3], + ), + target_link=END_EFFECTOR_LINK, + tolerance=GOAL_YAW_CONSTRAINT_TOLERANCE_XYZ_RAD, + parameterization=1, + ) + if "constrained" in self.mode: + self.moveit2.set_path_orientation_constraint( + quat_xyzw=Quaternion( + x=PATH_CONSTRAINT_QUAT_XYZW[0], + y=PATH_CONSTRAINT_QUAT_XYZW[1], + z=PATH_CONSTRAINT_QUAT_XYZW[2], + w=PATH_CONSTRAINT_QUAT_XYZW[3], + ), + frame_id=BASE_LINK, + target_link=END_EFFECTOR_LINK, + tolerance=PATH_CONSTRAINT_TOLERANCE_XYZ_RAD, + weight=1.0, + parameterization=1, + ) + + planning_start = time.perf_counter() + future = self.moveit2.plan_async(start_joint_state=start_pos) + while rclpy.ok() and not future.done(): + time.sleep(0.01) + traj = self.moveit2.get_trajectory(future) + planning_time = time.perf_counter() - planning_start + + success = traj is not None and bool(traj.points) + trial_data = { + "task_id": i, + "planning_mode": self.mode, + "start_joint_positions": list(start_pos), + "plan_success": success, + "planning_time_s": planning_time, + } + if goal_joint_pos: + trial_data["target_joint_positions"] = goal_joint_pos + if target_pos is not None: + trial_data["target_position"] = target_pos.tolist() + if target_quat: + trial_data["target_yaw_quat"] = target_quat + + if success: + LOGGER.info(f" Plan SUCCEEDED in {planning_time:.4f}s.") + metrics = self._calculate_trajectory_metrics(traj) + is_feasible = all( + wp["articutool_solution_rad"] is not None + for wp in metrics.waypoints_data + ) + trial_data["verification_success"] = is_feasible + trial_data["trajectory_metrics"] = metrics._asdict() + LOGGER.info(f" Verification Result: Feasible = {is_feasible}") + else: + LOGGER.warn(f" Plan FAILED in {planning_time:.4f}s.") + + self.results.append(trial_data) + + def save_results(self): + """Saves the comprehensive benchmark results to a single JSON file.""" + if not self.output_dir: + return + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") + filename = os.path.join( + self.output_dir, f"holistic_benchmark_results_{self.mode}_{timestamp}.json" + ) + try: + with open(filename, "w") as f: + json.dump(self.results, f, indent=2) + LOGGER.info(f"Comprehensive results saved to {filename}") + except Exception as e: + LOGGER.error(f"Failed to save results: {e}") + + total = len(self.results) + successes = sum(1 for r in self.results if r["plan_success"]) + verified = sum(1 for r in self.results if r.get("verification_success", False)) + success_rate = (successes / total * 100) if total > 0 else 0 + LOGGER.info(f"\n--- HOLISTIC BENCHMARK SUMMARY ({self.mode.upper()}) ---") + LOGGER.info( + f"Total Tasks: {total}, Planner Success Rate: {success_rate:.2f}% ({successes}/{total})" + ) + if successes > 0: + LOGGER.info( + f"Verification Success Rate (of successful plans): {(verified / successes * 100):.2f}%" + ) + LOGGER.info("---------------------------------------------------\n") + + +def main(): + parser = argparse.ArgumentParser( + description="Holistic benchmark for Articutool planning methodologies." + ) + parser.add_argument( + "xacro_file", type=str, help="Path to the robot URDF/XACRO file." + ) + parser.add_argument( + "--mode", + type=str, + required=True, + choices=[ + "joint_unconstrained", + "task_pos_unconstrained", + "task_pos_constrained", + "task_pos_yaw_constrained", + ], + help="The planning methodology to benchmark.", + ) + parser.add_argument( + "--num_tasks", type=int, default=100, help="Number of random tasks to generate." + ) + parser.add_argument( + "--timeout", type=float, default=10.0, help="Planning timeout in seconds." + ) + parser.add_argument( + "--output_dir", + type=str, + default="holistic_benchmark_output", + help="Directory to save results.", + ) + args = parser.parse_args() + + if not os.path.exists(args.xacro_file): + print(f"Error: XACRO file not found at '{args.xacro_file}'") + sys.exit(1) + + rclpy.init() + node = Node("holistic_benchmark_node") + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + executor_thread = Thread(target=executor.spin, daemon=True) + executor_thread.start() + + moveit2 = MoveIt2( + node=node, + joint_names=JOINT_NAMES, + base_link_name=BASE_LINK, + end_effector_name=END_EFFECTOR_LINK, + group_name=PLANNING_GROUP, + callback_group=ReentrantCallbackGroup(), + ) + + benchmark = HolisticBenchmark( + node, + moveit2, + args.xacro_file, + args.mode, + args.num_tasks, + args.timeout, + "RRTConnectkConfigDefault", + args.output_dir, + ) + + try: + if benchmark.pinocchio_model is not None: + LOGGER.info( + "Ready to begin benchmark. Each trial will start from a new random, collision-free state." + ) + benchmark.run() + benchmark.save_results() + else: + LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") + except Exception as e: + LOGGER.error(f"An error occurred during the benchmark: {e}", exc_info=True) + finally: + LOGGER.info("Shutting down.") + rclpy.shutdown() + executor_thread.join() + + +if __name__ == "__main__": + main() diff --git a/ada_feeding/scripts/holistic_benchmark_analysis.py b/ada_feeding/scripts/holistic_benchmark_analysis.py new file mode 100644 index 00000000..ac158497 --- /dev/null +++ b/ada_feeding/scripts/holistic_benchmark_analysis.py @@ -0,0 +1,307 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script loads and analyzes one or more comprehensive JSON results files +generated by `constrained_task_space_benchmark.py`. It generates a series of +side-by-side comparative plots to evaluate different planning methodologies. + +Requires: pandas, plotly +Install with: pip install pandas plotly +""" + +import argparse +import json +import os +import pandas as pd +import plotly.express as px +import plotly.graph_objects as go + + +def load_data(json_files: list[str]) -> pd.DataFrame: + """Loads and concatenates results from multiple JSON files.""" + all_data = [] + for file_path in json_files: + print(f"Loading data from {file_path}...") + try: + with open(file_path, "r") as f: + data = json.load(f) + all_data.extend(data) + except (FileNotFoundError, json.JSONDecodeError) as e: + print( + f" - Warning: Could not load or parse {file_path}. Skipping. Error: {e}" + ) + continue + + if not all_data: + print("Error: No valid data loaded.") + return pd.DataFrame() + + df = pd.json_normalize(all_data, sep="_") + print( + f"Successfully loaded a total of {len(df)} trials from {len(json_files)} file(s)." + ) + return df + + +def plot_success_rates(df: pd.DataFrame, output_dir: str): + """Plots planner and verification success rates grouped by planning mode.""" + print("Generating success rate plot...") + summary = ( + df.groupby("planning_mode") + .agg( + planner_success=("plan_success", "mean"), + verification_success=("verification_success", "mean"), + ) + .reset_index() + ) + summary["planner_success"] *= 100 + summary["verification_success"] *= 100 + + summary_long = summary.melt( + id_vars="planning_mode", + value_vars=["planner_success", "verification_success"], + var_name="Metric", + value_name="Success Rate (%)", + ) + + fig = px.bar( + summary_long, + x="planning_mode", + y="Success Rate (%)", + color="Metric", + title="Planner vs. Verification Success Rates by Methodology", + labels={"planning_mode": "Planning Methodology"}, + barmode="group", + text_auto=".1f", + category_orders={ + "planning_mode": [ + "joint_unconstrained", + "task_pos_unconstrained", + "task_pos_constrained", + "task_pos_yaw_constrained", + ] + }, + ) + fig.update_layout(yaxis_range=[0, 105], legend_title_text="Success Metric") + + filename = os.path.join(output_dir, "comparative_success_rates.html") + fig.write_html(filename) + print(f" - Saved to {filename}") + + +def plot_comparative_metric( + df: pd.DataFrame, + metric_col: str, + title: str, + yaxis_title: str, + filename: str, + output_dir: str, +): + """Generic function to create a comparative box plot for a given metric.""" + print(f"Generating plot for: {title}...") + successful_plans = df[df["plan_success"] == True].copy() + if successful_plans.empty or metric_col not in successful_plans.columns: + print(f" - No data for metric '{metric_col}'. Skipping plot.") + return + + fig = px.box( + successful_plans, + x="planning_mode", + y=metric_col, + color="planning_mode", + title=title, + labels={"planning_mode": "Planning Methodology", metric_col: yaxis_title}, + category_orders={ + "planning_mode": [ + "joint_unconstrained", + "task_pos_unconstrained", + "task_pos_constrained", + "task_pos_yaw_constrained", + ] + }, + ) + + filepath = os.path.join(output_dir, filename) + fig.write_html(filepath) + print(f" - Saved to {filepath}") + + +def plot_articutool_velocities_comparison(df: pd.DataFrame, output_dir: str): + """Plots a 2D histogram of required Articutool velocities, faceted by mode.""" + print("Generating Articutool velocity comparison plot...") + successful_plans = df[df["plan_success"] == True].copy() + + velocity_data = [] + for index, row in successful_plans.iterrows(): + waypoints = row.get("trajectory_metrics_waypoints_data") + if waypoints and isinstance(waypoints, list): + for wp in waypoints: + vels = wp.get("articutool_velocities_rad_per_sec") + if vels and isinstance(vels, dict): + velocity_data.append( + { + "planning_mode": row["planning_mode"], + "pitch_vel": vels.get("pitch_vel"), + "roll_vel": vels.get("roll_vel"), + } + ) + + if not velocity_data: + print(" - No velocity data found to plot.") + return + + vel_df = pd.DataFrame(velocity_data).dropna() + + fig = px.density_heatmap( + vel_df, + x="pitch_vel", + y="roll_vel", + facet_col="planning_mode", + facet_col_wrap=2, + title="Distribution of Required Articutool Velocities by Methodology", + labels={ + "pitch_vel": "Pitch Velocity (rad/s)", + "roll_vel": "Roll Velocity (rad/s)", + }, + category_orders={ + "planning_mode": [ + "joint_unconstrained", + "task_pos_unconstrained", + "task_pos_constrained", + "task_pos_yaw_constrained", + ] + }, + ) + + filename = os.path.join(output_dir, "comparative_articutool_velocities.html") + fig.write_html(filename) + print(f" - Saved to {filename}") + + +def plot_3d_trajectories(df: pd.DataFrame, output_dir: str, num_examples: int = 5): + """Plots start/end points and example trajectories in 3D space.""" + if df.empty or "trajectory_metrics_waypoints_data" not in df.columns: + return + + print("Generating 3D trajectory plot...") + successful_plans = df[df["plan_success"] == True].copy() + if successful_plans.empty: + print(" - No successful plans to plot.") + return + + end_points, start_points = [], [] + for _, row in successful_plans.iterrows(): + waypoints = row["trajectory_metrics_waypoints_data"] + if waypoints and isinstance(waypoints, list) and len(waypoints) > 0: + start_points.append(waypoints[0]["ee_pose_world"]["position"]) + end_points.append(waypoints[-1]["ee_pose_world"]["position"]) + + fig = go.Figure() + + fig.add_trace( + go.Scatter3d( + x=[p[0] for p in start_points], + y=[p[1] for p in start_points], + z=[p[2] for p in start_points], + mode="markers", + marker=dict(color="blue", size=4, opacity=0.6), + name="Start EE Position", + ) + ) + fig.add_trace( + go.Scatter3d( + x=[p[0] for p in end_points], + y=[p[1] for p in end_points], + z=[p[2] for p in end_points], + mode="markers", + marker=dict(color="red", size=4, opacity=0.6), + name="End EE Position", + ) + ) + + example_indices = np.random.choice( + successful_plans.index, min(num_examples, len(successful_plans)), replace=False + ) + for i, index in enumerate(example_indices): + waypoints = successful_plans.loc[index, "trajectory_metrics_waypoints_data"] + path_x = [wp["ee_pose_world"]["position"][0] for wp in waypoints] + path_y = [wp["ee_pose_world"]["position"][1] for wp in waypoints] + path_z = [wp["ee_pose_world"]["position"][2] for wp in waypoints] + fig.add_trace( + go.Scatter3d( + x=path_x, + y=path_y, + z=path_z, + mode="lines", + line=dict(width=5), + name=f"Example Traj {i + 1}", + ) + ) + + fig.update_layout( + title="3D Visualization of End-Effector Trajectories", + scene=dict( + xaxis_title="X (m)", + yaxis_title="Y (m)", + zaxis_title="Z (m)", + aspectmode="data", + ), + margin=dict(l=0, r=0, b=0, t=40), + ) + + filename = os.path.join(output_dir, "3d_trajectories.html") + fig.write_html(filename) + print(f" - Saved to {filename}") + + +def main(): + """Main function to load data and generate all plots.""" + parser = argparse.ArgumentParser( + description="Holistic analysis of benchmark results." + ) + parser.add_argument( + "json_files", + nargs="+", + help="Path(s) to the comprehensive benchmark results JSON file(s).", + ) + parser.add_argument( + "--output_dir", + type=str, + default="holistic_analysis_plots", + help="Directory to save the generated plots.", + ) + args = parser.parse_args() + + os.makedirs(args.output_dir, exist_ok=True) + df = load_data(args.json_files) + if df.empty: + return + + # Generate all plots + plot_success_rates(df, args.output_dir) + plot_comparative_metric( + df, + "planning_time_s", + "Planning Time Comparison", + "Planning Time (s)", + "comparative_planning_times.html", + args.output_dir, + ) + plot_comparative_metric( + df, + "trajectory_metrics_joint_space_path_length_rad", + "Joint-Space Path Length Comparison", + "Path Length (rad)", + "comparative_path_lengths.html", + args.output_dir, + ) + plot_articutool_velocities_comparison(df, args.output_dir) + plot_3d_trajectories(df, args.output_dir) + + print("\nAnalysis complete. Plots saved in:", args.output_dir) + + +if __name__ == "__main__": + main() From 7a7059d78ef9f0e4ef7129ca5ebd16cb9ac54dbc Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 22 Jul 2025 17:03:46 -0700 Subject: [PATCH 143/238] Update holistic benchmark, and add holistic benchmark analysis script --- ada_feeding/scripts/holistic_benchmark.py | 221 +++++++----------- .../scripts/holistic_benchmark_analysis.py | 1 + 2 files changed, 87 insertions(+), 135 deletions(-) diff --git a/ada_feeding/scripts/holistic_benchmark.py b/ada_feeding/scripts/holistic_benchmark.py index 4c3a7cd2..09be6b5e 100644 --- a/ada_feeding/scripts/holistic_benchmark.py +++ b/ada_feeding/scripts/holistic_benchmark.py @@ -8,8 +8,9 @@ comprehensive dataset to empirically evaluate the trade-offs between planner success, motion flexibility, and guaranteed kinematic/dynamic feasibility. -This version uses a collision-checking service to find a valid, random start -state for each trial, ensuring a robust and diverse evaluation. +This version uses a resilient "generate-and-test" approach, continuing to +sample random start/goal states until the desired number of successful plans +have been generated. The script can be run in one of four modes via the `--mode` flag: 1. `joint_unconstrained`: The baseline. Plans between random joint-space goals @@ -44,9 +45,7 @@ from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory from geometry_msgs.msg import Quaternion -from moveit_msgs.srv import GetStateValidity from moveit_msgs.msg import RobotState -from sensor_msgs.msg import JointState from scipy.spatial.transform import Rotation as R import pinocchio as pin @@ -97,7 +96,8 @@ def __init__( self.xacro_file_path = xacro_file_path self.mode = mode self.num_tasks = num_tasks - self.planning_timeout = planning_timeout + # Set the planning timeout on the MoveIt2 object itself + self.moveit2.planning_time = planning_timeout self.planner_id = planner_id self.output_dir = output_dir self.results: List[Dict[str, Any]] = [] @@ -109,18 +109,6 @@ def __init__( self._initialize_pinocchio() self.joint_limits = self._get_joint_limits() - self.service_callback_group = ReentrantCallbackGroup() - self.state_validity_client = self.node.create_client( - GetStateValidity, - "/check_state_validity", - callback_group=self.service_callback_group, - ) - if not self.state_validity_client.wait_for_service(timeout_sec=5.0): - LOGGER.error( - "Could not connect to /check_state_validity service. Cannot guarantee valid start states." - ) - self.state_validity_client = None - LOGGER.info("Holistic Benchmark Initialized.") LOGGER.info(f" - Planning Mode: {self.mode}") if self.output_dir: @@ -174,75 +162,6 @@ def generate_random_joint_config(self) -> List[float]: """Generates a random joint configuration within limits.""" return [np.random.uniform(low, high) for low, high in self.joint_limits] - def is_state_valid(self, joint_positions: List[float]) -> bool: - """Checks if a joint configuration is collision-free using MoveIt's service.""" - if ( - not self.state_validity_client - or not self.state_validity_client.service_is_ready() - ): - LOGGER.warn("State validity client not available. Assuming state is valid.") - return True - - # --- FIX: Build a complete RobotState to avoid ambiguity --- - # Get the current full state of the robot - full_joint_state = self.moveit2.joint_state - joint_state_map = { - name: pos - for name, pos in zip(full_joint_state.name, full_joint_state.position) - } - - # Overwrite the Jaco arm joints with the configuration we want to test - for i, name in enumerate(JOINT_NAMES): - joint_state_map[name] = joint_positions[i] - - # Build the RobotState message with all joints - robot_state = RobotState() - robot_state.joint_state.name = list(joint_state_map.keys()) - robot_state.joint_state.position = list(joint_state_map.values()) - - req = GetStateValidity.Request() - req.group_name = PLANNING_GROUP - req.robot_state = robot_state - - future = self.state_validity_client.call_async(req) - - timeout_sec = 2.0 - start_time = self.node.get_clock().now() - while ( - rclpy.ok() - and (self.node.get_clock().now() - start_time).nanoseconds / 1e9 - < timeout_sec - ): - if future.done(): - try: - response = future.result() - return response.valid if response is not None else False - except Exception as e: - LOGGER.error( - f"Exception getting result from /check_state_validity: {e}" - ) - return False - time.sleep(0.01) - - LOGGER.error(f"Service call to /check_state_validity timed out.") - return False - - def generate_valid_random_joint_config( - self, max_attempts=100 - ) -> Optional[List[float]]: - """Generates a random, collision-free joint configuration.""" - for attempt in range(max_attempts): - config = self.generate_random_joint_config() - if self.is_state_valid(config): - LOGGER.info( - f" Found valid start configuration on attempt {attempt + 1}." - ) - return config - LOGGER.error( - f"Failed to find a valid random joint configuration after {max_attempts} attempts." - ) - return None - def generate_random_position_in_workspace(self) -> np.ndarray: """Generates a random (x, y, z) position from a half-spherical shell.""" r = ( @@ -408,28 +327,37 @@ def _calculate_trajectory_metrics( def run(self): """Main benchmark execution loop.""" if not self.pinocchio_model: + LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") return - for i in range(self.num_tasks): - LOGGER.info(f"--- Running Task {i + 1}/{self.num_tasks} ---") + successful_tasks = 0 + total_attempts = 0 + # Set a generous limit on attempts to prevent an infinite loop + max_attempts = self.num_tasks * 25 - LOGGER.info(" Searching for a valid random start configuration...") - start_pos = self.generate_valid_random_joint_config() - if start_pos is None: - LOGGER.warn(" Skipping task, could not find a valid start state.") - continue + LOGGER.info( + f"Attempting to collect {self.num_tasks} successful planning tasks..." + ) + + while successful_tasks < self.num_tasks and total_attempts < max_attempts: + total_attempts += 1 + LOGGER.info( + f"--- Task {successful_tasks + 1}/{self.num_tasks} (Attempt {total_attempts}) ---" + ) + + # Generate a random start configuration without pre-validation + start_pos = self.generate_random_joint_config() self.moveit2.clear_goal_constraints() self.moveit2.clear_path_constraints() target_pos, target_quat, goal_joint_pos = None, None, None if self.mode == "joint_unconstrained": - goal_joint_pos = self.generate_valid_random_joint_config() - if goal_joint_pos is None: - LOGGER.warn(" Skipping task, could not find a valid goal state.") - continue + # Generate a random goal configuration without pre-validation + goal_joint_pos = self.generate_random_joint_config() self.moveit2.set_joint_goal(joint_positions=goal_joint_pos) else: + # Task-space goals target_pos = self.generate_random_position_in_workspace() self.moveit2.set_position_goal( position=target_pos.tolist(), @@ -466,41 +394,59 @@ def run(self): ) planning_start = time.perf_counter() + # Use the randomly generated start state directly. The planner will + # fail if the start state is invalid (e.g., in collision). future = self.moveit2.plan_async(start_joint_state=start_pos) - while rclpy.ok() and not future.done(): - time.sleep(0.01) + + # Wait for the future to complete, respecting the planning timeout + rclpy.spin_until_future_complete( + self.node, future, timeout_sec=self.moveit2.planning_time + 1.0 + ) + traj = self.moveit2.get_trajectory(future) planning_time = time.perf_counter() - planning_start success = traj is not None and bool(traj.points) - trial_data = { - "task_id": i, - "planning_mode": self.mode, - "start_joint_positions": list(start_pos), - "plan_success": success, - "planning_time_s": planning_time, - } - if goal_joint_pos: - trial_data["target_joint_positions"] = goal_joint_pos - if target_pos is not None: - trial_data["target_position"] = target_pos.tolist() - if target_quat: - trial_data["target_yaw_quat"] = target_quat if success: - LOGGER.info(f" Plan SUCCEEDED in {planning_time:.4f}s.") + successful_tasks += 1 + LOGGER.info( + f" Plan SUCCEEDED in {planning_time:.4f}s. ({successful_tasks}/{self.num_tasks} collected)" + ) + metrics = self._calculate_trajectory_metrics(traj) is_feasible = all( wp["articutool_solution_rad"] is not None for wp in metrics.waypoints_data ) - trial_data["verification_success"] = is_feasible - trial_data["trajectory_metrics"] = metrics._asdict() - LOGGER.info(f" Verification Result: Feasible = {is_feasible}") + + trial_data = { + "task_id": successful_tasks, + "attempt_id": total_attempts, + "planning_mode": self.mode, + "start_joint_positions": list(start_pos), + "plan_success": True, + "planning_time_s": planning_time, + "verification_success": is_feasible, + "trajectory_metrics": metrics._asdict(), + } + if goal_joint_pos: + trial_data["target_joint_positions"] = list(goal_joint_pos) + if target_pos is not None: + trial_data["target_position"] = target_pos.tolist() + if target_quat: + trial_data["target_yaw_quat"] = list(target_quat) + + self.results.append(trial_data) else: - LOGGER.warn(f" Plan FAILED in {planning_time:.4f}s.") + LOGGER.warn( + f" Plan FAILED in {planning_time:.4f}s. Continuing to next attempt." + ) - self.results.append(trial_data) + if successful_tasks < self.num_tasks: + LOGGER.error( + f"Benchmark finished due to max attempts ({max_attempts}). Only collected {successful_tasks}/{self.num_tasks} tasks." + ) def save_results(self): """Saves the comprehensive benchmark results to a single JSON file.""" @@ -517,17 +463,23 @@ def save_results(self): except Exception as e: LOGGER.error(f"Failed to save results: {e}") - total = len(self.results) - successes = sum(1 for r in self.results if r["plan_success"]) - verified = sum(1 for r in self.results if r.get("verification_success", False)) - success_rate = (successes / total * 100) if total > 0 else 0 + total_successful = len(self.results) + total_attempts = self.results[-1]["attempt_id"] if self.results else 0 + success_rate = ( + (total_successful / total_attempts * 100) if total_attempts > 0 else 0 + ) + LOGGER.info(f"\n--- HOLISTIC BENCHMARK SUMMARY ({self.mode.upper()}) ---") LOGGER.info( - f"Total Tasks: {total}, Planner Success Rate: {success_rate:.2f}% ({successes}/{total})" + f"Collected {total_successful} successful plans out of {total_attempts} attempts. " + f"Overall Success Rate: {success_rate:.2f}%" ) - if successes > 0: + if total_successful > 0: + verified = sum( + 1 for r in self.results if r.get("verification_success", False) + ) LOGGER.info( - f"Verification Success Rate (of successful plans): {(verified / successes * 100):.2f}%" + f"Verification Success Rate (of successful plans): {(verified / total_successful * 100):.2f}%" ) LOGGER.info("---------------------------------------------------\n") @@ -552,7 +504,10 @@ def main(): help="The planning methodology to benchmark.", ) parser.add_argument( - "--num_tasks", type=int, default=100, help="Number of random tasks to generate." + "--num_tasks", + type=int, + default=100, + help="Number of successful tasks to generate.", ) parser.add_argument( "--timeout", type=float, default=10.0, help="Planning timeout in seconds." @@ -597,16 +552,12 @@ def main(): ) try: - if benchmark.pinocchio_model is not None: - LOGGER.info( - "Ready to begin benchmark. Each trial will start from a new random, collision-free state." - ) - benchmark.run() - benchmark.save_results() - else: - LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") + benchmark.run() + benchmark.save_results() except Exception as e: - LOGGER.error(f"An error occurred during the benchmark: {e}", exc_info=True) + LOGGER.error( + f"An unhandled error occurred during the benchmark: {e}", exc_info=True + ) finally: LOGGER.info("Shutting down.") rclpy.shutdown() diff --git a/ada_feeding/scripts/holistic_benchmark_analysis.py b/ada_feeding/scripts/holistic_benchmark_analysis.py index ac158497..ad91b6d4 100644 --- a/ada_feeding/scripts/holistic_benchmark_analysis.py +++ b/ada_feeding/scripts/holistic_benchmark_analysis.py @@ -11,6 +11,7 @@ Install with: pip install pandas plotly """ +import numpy as np import argparse import json import os From b967cb6c1b528ed6eb3fa0f89655c7092317d1ac Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 22 Jul 2025 19:49:11 -0700 Subject: [PATCH 144/238] Add granular metrics for planning failure reasons --- ada_feeding/scripts/holistic_benchmark.py | 202 +++++++++++++++------- 1 file changed, 136 insertions(+), 66 deletions(-) diff --git a/ada_feeding/scripts/holistic_benchmark.py b/ada_feeding/scripts/holistic_benchmark.py index 09be6b5e..8e3c5a46 100644 --- a/ada_feeding/scripts/holistic_benchmark.py +++ b/ada_feeding/scripts/holistic_benchmark.py @@ -8,9 +8,8 @@ comprehensive dataset to empirically evaluate the trade-offs between planner success, motion flexibility, and guaranteed kinematic/dynamic feasibility. -This version uses a resilient "generate-and-test" approach, continuing to -sample random start/goal states until the desired number of successful plans -have been generated. +This version uses a resilient "generate-and-test" approach and implements +granular failure logging to distinguish between different failure modes. The script can be run in one of four modes via the `--mode` flag: 1. `joint_unconstrained`: The baseline. Plans between random joint-space goals @@ -24,7 +23,7 @@ """ # Standard imports -from collections import namedtuple +from collections import namedtuple, Counter from datetime import datetime import os import time @@ -35,6 +34,7 @@ import subprocess import sys import argparse +from enum import Enum # Third-party imports import numpy as np @@ -45,7 +45,6 @@ from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory from geometry_msgs.msg import Quaternion -from moveit_msgs.msg import RobotState from scipy.spatial.transform import Rotation as R import pinocchio as pin @@ -77,6 +76,15 @@ ) +class TrialStatus(Enum): + """Enumerates the possible outcomes of a benchmark trial.""" + + SUCCESS = "Success" + PLANNER_FAILURE = "Planner Failure" + START_STATE_INFEASIBLE = "Start State Infeasible" + PATH_VERIFICATION_FAILURE = "Path Verification Failure" + + class HolisticBenchmark: """Manages the holistic benchmark planning process.""" @@ -96,7 +104,6 @@ def __init__( self.xacro_file_path = xacro_file_path self.mode = mode self.num_tasks = num_tasks - # Set the planning timeout on the MoveIt2 object itself self.moveit2.planning_time = planning_timeout self.planner_id = planner_id self.output_dir = output_dir @@ -162,6 +169,45 @@ def generate_random_joint_config(self) -> List[float]: """Generates a random joint configuration within limits.""" return [np.random.uniform(low, high) for low, high in self.joint_limits] + def _check_articutool_feasibility_at_config( + self, jaco_joint_config: List[float] + ) -> bool: + """Checks if the Articutool can maintain leveling at a single Jaco configuration.""" + if self.pinocchio_model is None: + return False + + q = pin.neutral(self.pinocchio_model) + for j, name in enumerate(JOINT_NAMES): + joint_id = self.pinocchio_model.getJointId(name) + joint_obj = self.pinocchio_model.joints[joint_id] + if joint_obj.nq == 2: # Prismatic joint + q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ + math.cos(jaco_joint_config[j]), + math.sin(jaco_joint_config[j]), + ] + else: # Revolute joint + q[joint_obj.idx_q] = jaco_joint_config[j] + + pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) + pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) + + ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + target_up = R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) + solutions = self._solve_articutool_ik(target_up) + + if not solutions: + return False + + return any( + ARTICUTOOL_PITCH_LIMITS_RAD[0] <= s[0] <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] <= s[1] <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + for s in solutions + ) + + def is_start_state_feasible(self, start_joint_config: List[float]) -> bool: + """Checks if a given start state is kinematically feasible for the Articutool.""" + return self._check_articutool_feasibility_at_config(start_joint_config) + def generate_random_position_in_workspace(self) -> np.ndarray: """Generates a random (x, y, z) position from a half-spherical shell.""" r = ( @@ -222,16 +268,27 @@ def _solve_articutool_ik( def _calculate_trajectory_metrics( self, trajectory: JointTrajectory - ) -> TrajectoryMetrics: - """Calculates detailed metrics for a given trajectory.""" + ) -> Tuple[bool, Optional[TrajectoryMetrics]]: + """ + Calculates detailed metrics for a given trajectory and verifies its feasibility. + Returns a tuple: (is_feasible, metrics_object). + """ if self.pinocchio_model is None or not trajectory.points: - return TrajectoryMetrics(0.0, 0.0, [], []) + return False, None q = pin.neutral(self.pinocchio_model) prev_pos = np.array(trajectory.points[0].positions) waypoints_data, path_len = [], 0.0 + is_path_feasible = True for i, point in enumerate(trajectory.points): + is_waypoint_feasible = self._check_articutool_feasibility_at_config( + point.positions + ) + if not is_waypoint_feasible: + is_path_feasible = False + + # Continue calculating metrics even if path is infeasible to log data for j, name in enumerate(JOINT_NAMES): joint_id = self.pinocchio_model.getJointId(name) joint_obj = self.pinocchio_model.joints[joint_id] @@ -250,6 +307,7 @@ def _calculate_trajectory_metrics( np.zeros(self.pinocchio_model.nv), ) ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + # This calculation is now duplicated, but kept for velocity analysis target_up = ( R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) ) @@ -320,20 +378,20 @@ def _calculate_trajectory_metrics( trajectory.points[-1].time_from_start.sec + 1e-9 * trajectory.points[-1].time_from_start.nanosec ) - return TrajectoryMetrics( + metrics = TrajectoryMetrics( duration, path_len, list(trajectory.points[-1].positions), waypoints_data ) + return is_path_feasible, metrics def run(self): - """Main benchmark execution loop.""" + """Main benchmark execution loop with granular failure logging.""" if not self.pinocchio_model: LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") return successful_tasks = 0 total_attempts = 0 - # Set a generous limit on attempts to prevent an infinite loop - max_attempts = self.num_tasks * 25 + max_attempts = self.num_tasks * 50 # Increased max attempts LOGGER.info( f"Attempting to collect {self.num_tasks} successful planning tasks..." @@ -345,19 +403,32 @@ def run(self): f"--- Task {successful_tasks + 1}/{self.num_tasks} (Attempt {total_attempts}) ---" ) - # Generate a random start configuration without pre-validation start_pos = self.generate_random_joint_config() + # 1. Check if the start state is feasible for the Articutool + if not self.is_start_state_feasible(start_pos): + LOGGER.warn( + " Attempt FAILED: Start state is kinematically infeasible for Articutool." + ) + self.results.append( + { + "task_id": successful_tasks + 1, + "attempt_id": total_attempts, + "planning_mode": self.mode, + "status": TrialStatus.START_STATE_INFEASIBLE.value, + "start_joint_positions": list(start_pos), + } + ) + continue + self.moveit2.clear_goal_constraints() self.moveit2.clear_path_constraints() target_pos, target_quat, goal_joint_pos = None, None, None if self.mode == "joint_unconstrained": - # Generate a random goal configuration without pre-validation goal_joint_pos = self.generate_random_joint_config() self.moveit2.set_joint_goal(joint_positions=goal_joint_pos) else: - # Task-space goals target_pos = self.generate_random_position_in_workspace() self.moveit2.set_position_goal( position=target_pos.tolist(), @@ -394,54 +465,55 @@ def run(self): ) planning_start = time.perf_counter() - # Use the randomly generated start state directly. The planner will - # fail if the start state is invalid (e.g., in collision). future = self.moveit2.plan_async(start_joint_state=start_pos) - - # Wait for the future to complete, respecting the planning timeout rclpy.spin_until_future_complete( self.node, future, timeout_sec=self.moveit2.planning_time + 1.0 ) - traj = self.moveit2.get_trajectory(future) planning_time = time.perf_counter() - planning_start - success = traj is not None and bool(traj.points) - - if success: - successful_tasks += 1 - LOGGER.info( - f" Plan SUCCEEDED in {planning_time:.4f}s. ({successful_tasks}/{self.num_tasks} collected)" - ) - - metrics = self._calculate_trajectory_metrics(traj) - is_feasible = all( - wp["articutool_solution_rad"] is not None - for wp in metrics.waypoints_data + trial_data = { + "task_id": successful_tasks + 1, + "attempt_id": total_attempts, + "planning_mode": self.mode, + "start_joint_positions": list(start_pos), + "planning_time_s": planning_time, + } + if goal_joint_pos: + trial_data["target_joint_positions"] = list(goal_joint_pos) + if target_pos is not None: + trial_data["target_position"] = target_pos.tolist() + if target_quat: + trial_data["target_yaw_quat"] = list(target_quat) + + # 2. Check if the planner found a solution + if not traj or not traj.points: + LOGGER.warn( + f" Attempt FAILED: Planner did not find a solution in {planning_time:.4f}s." ) + trial_data["status"] = TrialStatus.PLANNER_FAILURE.value + self.results.append(trial_data) + continue - trial_data = { - "task_id": successful_tasks, - "attempt_id": total_attempts, - "planning_mode": self.mode, - "start_joint_positions": list(start_pos), - "plan_success": True, - "planning_time_s": planning_time, - "verification_success": is_feasible, - "trajectory_metrics": metrics._asdict(), - } - if goal_joint_pos: - trial_data["target_joint_positions"] = list(goal_joint_pos) - if target_pos is not None: - trial_data["target_position"] = target_pos.tolist() - if target_quat: - trial_data["target_yaw_quat"] = list(target_quat) + # 3. Verify the path and calculate metrics + is_feasible, metrics = self._calculate_trajectory_metrics(traj) + trial_data["trajectory_metrics"] = metrics._asdict() if metrics else None - self.results.append(trial_data) - else: + if not is_feasible: LOGGER.warn( - f" Plan FAILED in {planning_time:.4f}s. Continuing to next attempt." + f" Attempt FAILED: Path verification failed for a {planning_time:.4f}s plan." ) + trial_data["status"] = TrialStatus.PATH_VERIFICATION_FAILURE.value + self.results.append(trial_data) + continue + + # 4. If all checks pass, it's a success + successful_tasks += 1 + LOGGER.info( + f" Plan SUCCEEDED and VERIFIED in {planning_time:.4f}s. ({successful_tasks}/{self.num_tasks} collected)" + ) + trial_data["status"] = TrialStatus.SUCCESS.value + self.results.append(trial_data) if successful_tasks < self.num_tasks: LOGGER.error( @@ -463,24 +535,22 @@ def save_results(self): except Exception as e: LOGGER.error(f"Failed to save results: {e}") - total_successful = len(self.results) - total_attempts = self.results[-1]["attempt_id"] if self.results else 0 - success_rate = ( - (total_successful / total_attempts * 100) if total_attempts > 0 else 0 - ) + # New summary with granular failure counts + status_counts = Counter(r["status"] for r in self.results) + total_attempts = len(self.results) + total_successful = status_counts[TrialStatus.SUCCESS.value] LOGGER.info(f"\n--- HOLISTIC BENCHMARK SUMMARY ({self.mode.upper()}) ---") + LOGGER.info(f"Total Attempts: {total_attempts}") LOGGER.info( - f"Collected {total_successful} successful plans out of {total_attempts} attempts. " - f"Overall Success Rate: {success_rate:.2f}%" + f" - Success: {total_successful} ({total_successful / total_attempts * 100:.2f}%)" ) - if total_successful > 0: - verified = sum( - 1 for r in self.results if r.get("verification_success", False) - ) - LOGGER.info( - f"Verification Success Rate (of successful plans): {(verified / total_successful * 100):.2f}%" - ) + for status in TrialStatus: + if status != TrialStatus.SUCCESS: + count = status_counts[status.value] + LOGGER.info( + f" - {status.name}: {count} ({count / total_attempts * 100:.2f}%)" + ) LOGGER.info("---------------------------------------------------\n") From 2901f142ef7f9bb918b77cf3f9b2622a897bd749 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 22 Jul 2025 19:50:08 -0700 Subject: [PATCH 145/238] Add scripts to compute the feasibility manifold, visualize it, and analyze which joints contribute most to whether a configuration is within the feasibility manifold --- ada_feeding/scripts/joint_analyzer.py | 139 +++++++++++ ada_feeding/scripts/manifold_explorer.py | 278 +++++++++++++++++++++ ada_feeding/scripts/manifold_visualizer.py | 130 ++++++++++ 3 files changed, 547 insertions(+) create mode 100644 ada_feeding/scripts/joint_analyzer.py create mode 100644 ada_feeding/scripts/manifold_explorer.py create mode 100644 ada_feeding/scripts/manifold_visualizer.py diff --git a/ada_feeding/scripts/joint_analyzer.py b/ada_feeding/scripts/joint_analyzer.py new file mode 100644 index 00000000..c5776db4 --- /dev/null +++ b/ada_feeding/scripts/joint_analyzer.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script performs a quantitative analysis of the manifold data generated by +manifold_explorer.py. + +For each joint, it creates an overlapping histogram to visualize the distribution +of joint values for both feasible and infeasible configurations. This helps +identify which joints, and which specific ranges of motion for those joints, +are critical for the Articutool's leveling task. +""" + +import argparse +import sys +import os +import pandas as pd +import plotly.graph_objects as go +from plotly.subplots import make_subplots +import math + + +def analyze_joint_distributions(csv_file_path: str, output_dir: str): + """ + Loads manifold data and generates overlapping histograms for each joint. + """ + print(f"Loading data from {csv_file_path}...") + try: + df = pd.read_csv(csv_file_path) + except FileNotFoundError: + print(f"Error: File not found at '{csv_file_path}'", file=sys.stderr) + sys.exit(1) + except Exception as e: + print(f"Error reading CSV file: {e}", file=sys.stderr) + sys.exit(1) + + # --- Data Preparation --- + # Get the names of the joint columns (assumes they start with 'j') + joint_columns = [col for col in df.columns if col.startswith("j")] + if not joint_columns: + print( + "Error: No joint columns (e.g., 'j1', 'j2') found in the CSV file.", + file=sys.stderr, + ) + sys.exit(1) + + # Separate the DataFrame into feasible and infeasible sets + feasible_df = df[df["is_feasible"] == 1] + infeasible_df = df[df["is_feasible"] == 0] + + print( + f"Found {len(feasible_df)} feasible and {len(infeasible_df)} infeasible samples." + ) + + # --- Plotting --- + # Create a subplot grid for all joints + num_joints = len(joint_columns) + rows = math.ceil(num_joints / 2) + fig = make_subplots( + rows=rows, + cols=2, + subplot_titles=[f"Joint {i + 1} Distribution" for i in range(num_joints)], + ) + + print("Generating joint distribution plots...") + for i, joint in enumerate(joint_columns): + row = (i // 2) + 1 + col = (i % 2) + 1 + + # Add histogram for feasible configurations + fig.add_trace( + go.Histogram( + x=feasible_df[joint], + name="Feasible", + marker_color="#2ca02c", # Green + opacity=0.7, + legendgroup="feasible", + showlegend=(i == 0), # Show legend only for the first plot + ), + row=row, + col=col, + ) + + # Add histogram for infeasible configurations + fig.add_trace( + go.Histogram( + x=infeasible_df[joint], + name="Infeasible", + marker_color="#d62728", # Red + opacity=0.6, + legendgroup="infeasible", + showlegend=(i == 0), # Show legend only for the first plot + ), + row=row, + col=col, + ) + + fig.update_xaxes(title_text="Joint Value (radians)", row=row, col=col) + fig.update_yaxes(title_text="Count", row=row, col=col) + + # Update the overall layout + fig.update_layout( + height=400 * rows, + width=900, + title_text="Joint Value Distributions for Feasible vs. Infeasible Configurations", + barmode="overlay", # Overlay histograms + legend_title_text="Configuration Status", + ) + + # Save the plot to an HTML file + output_filename = os.path.join( + output_dir, + f"joint_distributions_{os.path.basename(csv_file_path).replace('.csv', '.html')}", + ) + fig.write_html(output_filename) + print(f"\nSuccessfully saved analysis to:\n{output_filename}") + + +def main(): + parser = argparse.ArgumentParser( + description="Analyze and visualize joint distributions from manifold data." + ) + parser.add_argument( + "csv_file", type=str, help="Path to the manifold data CSV file." + ) + parser.add_argument( + "--output_dir", + type=str, + default="manifold_data", + help="Directory to save the output HTML file.", + ) + args = parser.parse_args() + + analyze_joint_distributions(args.csv_file, args.output_dir) + + +if __name__ == "__main__": + main() diff --git a/ada_feeding/scripts/manifold_explorer.py b/ada_feeding/scripts/manifold_explorer.py new file mode 100644 index 00000000..da64cb55 --- /dev/null +++ b/ada_feeding/scripts/manifold_explorer.py @@ -0,0 +1,278 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script explores the "Leveling Feasibility Manifold" for the Jaco arm +equipped with the Articutool. It densely samples the robot's configuration +space and, for each sample, determines if the Articutool can achieve a +level orientation. + +The output is a CSV file containing the sampled joint configurations and their +feasibility, which can then be used for visualization and analysis. +""" + +# Standard imports +from datetime import datetime +import os +import math +import subprocess +import sys +import argparse +import csv +from typing import Optional, List, Tuple + +# Third-party imports +import numpy as np +from scipy.spatial.transform import Rotation as R +import pinocchio as pin + +# --- Constants --- +# These should be adjusted to match your robot's configuration +JOINT_NAMES = [f"j2n6s200_joint_{i + 1}" for i in range(6)] +END_EFFECTOR_LINK = "j2n6s200_end_effector" +WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) +ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) +ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) +EPSILON = 1e-6 + + +class ManifoldExplorer: + """ + Samples and analyzes the configuration space of the Jaco arm to find the + Articutool's leveling feasibility manifold. + """ + + def __init__(self, xacro_file_path: str): + self.xacro_file_path = xacro_file_path + self.pinocchio_model: Optional[pin.Model] = None + self.pinocchio_data: Optional[pin.Data] = None + self.jaco_ee_frame_id_pin: Optional[int] = None + self.joint_limits: List[Tuple[float, float]] = [] + + self._initialize_pinocchio() + if self.pinocchio_model: + self.joint_limits = self._get_joint_limits() + + def _initialize_pinocchio(self): + """Loads the robot model from a XACRO file into Pinocchio.""" + print("Initializing Pinocchio model from XACRO...") + try: + # Use ros2 run to convert xacro to urdf in memory + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", self.xacro_file_path], + check=True, + capture_output=True, + text=True, + ) + urdf_xml_string = process.stdout + self.pinocchio_model = pin.buildModelFromXML(urdf_xml_string) + self.pinocchio_data = self.pinocchio_model.createData() + self.jaco_ee_frame_id_pin = self.pinocchio_model.getFrameId( + END_EFFECTOR_LINK + ) + print("Pinocchio model loaded successfully.") + except FileNotFoundError: + print( + f"Error: 'ros2' command not found. Is ROS 2 sourced?", file=sys.stderr + ) + sys.exit(1) + except subprocess.CalledProcessError as e: + print(f"Error running xacro: {e}", file=sys.stderr) + print(f"Stderr: {e.stderr}", file=sys.stderr) + sys.exit(1) + except Exception as e: + print(f"Failed to initialize Pinocchio model: {e}", file=sys.stderr) + self.pinocchio_model = None + + def _get_joint_limits(self) -> List[Tuple[float, float]]: + """Retrieves joint limits from the Pinocchio model.""" + if self.pinocchio_model is None: + return [(-math.pi, math.pi)] * len(JOINT_NAMES) + + limits = [] + for name in JOINT_NAMES: + if self.pinocchio_model.existJointName(name): + joint_id = self.pinocchio_model.getJointId(name) + # In Pinocchio, joint indices for position/velocity can differ + idx_q = self.pinocchio_model.joints[joint_id].idx_q + limits.append( + ( + self.pinocchio_model.lowerPositionLimit[idx_q], + self.pinocchio_model.upperPositionLimit[idx_q], + ) + ) + else: + print( + f"Warning: Joint '{name}' not found in Pinocchio model.", + file=sys.stderr, + ) + limits.append((-math.pi, math.pi)) # Default limit + return limits + + def generate_random_joint_config(self) -> List[float]: + """Generates a random joint configuration within limits.""" + return [np.random.uniform(low, high) for low, high in self.joint_limits] + + def _solve_articutool_ik( + self, target_vector: np.ndarray + ) -> List[Tuple[float, float]]: + """Analytical IK solver for the Articutool.""" + vx, vy, vz = target_vector + solutions = [] + + # Check if arcsin argument is valid + asin_arg = -vx + if not (-1.0 - EPSILON <= asin_arg <= 1.0 + EPSILON): + return [] + + # Clamp the argument to be safe + theta_r1 = math.asin(np.clip(asin_arg, -1.0, 1.0)) + # Find the second solution for roll + theta_r2 = (math.pi - theta_r1 + math.pi) % (2 * math.pi) - math.pi + + for theta_r in list(set([theta_r1, theta_r2])): + cos_tr = math.cos(theta_r) + # Handle the case where cos(theta_r) is near zero + if math.isclose(cos_tr, 0.0, abs_tol=EPSILON): + # If cos(theta_r) is 0, then vy and vz must also be 0 for a solution + if math.isclose(vy, 0.0, abs_tol=EPSILON) and math.isclose( + vz, 0.0, abs_tol=EPSILON + ): + # Pitch can be anything, but 0 is a reasonable choice + solutions.append( + (0.0, (theta_r + math.pi) % (2 * math.pi) - math.pi) + ) + continue + + # General solution for pitch + theta_p = math.atan2(vz, vy) + solutions.append( + ( + (theta_p + math.pi) % (2 * math.pi) - math.pi, + (theta_r + math.pi) % (2 * math.pi) - math.pi, + ) + ) + return solutions + + def check_feasibility(self, jaco_joint_config: List[float]) -> bool: + """ + Checks if the Articutool can maintain leveling at a single Jaco configuration. + Returns True if a valid Articutool joint configuration exists, False otherwise. + """ + if ( + self.pinocchio_model is None + or self.pinocchio_data is None + or self.jaco_ee_frame_id_pin is None + ): + return False + + q = pin.neutral(self.pinocchio_model) + for j, name in enumerate(JOINT_NAMES): + if self.pinocchio_model.existJointName(name): + joint_id = self.pinocchio_model.getJointId(name) + joint_obj = self.pinocchio_model.joints[joint_id] + # Handle different joint types (revolute vs. prismatic, etc.) + if joint_obj.nq == 2 and not joint_obj.shortname().startswith( + "JointModelRX" + ): # e.g., for SO(2) joints + q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ + math.cos(jaco_joint_config[j]), + math.sin(jaco_joint_config[j]), + ] + else: # Standard revolute joint + q[joint_obj.idx_q] = jaco_joint_config[j] + + # Run forward kinematics to get frame placements + pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) + pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) + + # Get the end-effector's transformation matrix + ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + + # Determine the "up" vector in the end-effector's frame + target_up_in_ee_frame = ( + R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) + ) + + # Solve the Articutool's IK for this target vector + solutions = self._solve_articutool_ik(target_up_in_ee_frame) + + if not solutions: + return False + + # Check if any solution is within the Articutool's joint limits + return any( + ARTICUTOOL_PITCH_LIMITS_RAD[0] <= pitch <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] <= roll <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + for pitch, roll in solutions + ) + + def run_exploration(self, num_samples: int, output_file: str): + """ + Main exploration loop. Samples the C-space and saves results to a file. + """ + if not self.pinocchio_model: + print( + "Cannot run exploration: Pinocchio model not loaded.", file=sys.stderr + ) + return + + print(f"Starting exploration of {num_samples} samples...") + results = [] + header = [f"j{i + 1}" for i in range(len(JOINT_NAMES))] + ["is_feasible"] + + for i in range(num_samples): + config = self.generate_random_joint_config() + is_feasible = self.check_feasibility(config) + results.append(config + [1 if is_feasible else 0]) + + if (i + 1) % 1000 == 0: + print(f" ...processed {i + 1}/{num_samples} samples.") + + print(f"Exploration complete. Saving results to {output_file}...") + try: + with open(output_file, "w", newline="") as f: + writer = csv.writer(f) + writer.writerow(header) + writer.writerows(results) + print("Successfully saved results.") + except IOError as e: + print(f"Error saving file: {e}", file=sys.stderr) + + +def main(): + parser = argparse.ArgumentParser( + description="Explore the Articutool's leveling feasibility manifold." + ) + parser.add_argument( + "xacro_file", type=str, help="Path to the robot URDF/XACRO file." + ) + parser.add_argument( + "--num_samples", + type=int, + default=50000, + help="Number of random configurations to sample.", + ) + parser.add_argument( + "--output_dir", + type=str, + default="manifold_data", + help="Directory to save the output CSV file.", + ) + args = parser.parse_args() + + if not os.path.exists(args.xacro_file): + print(f"Error: XACRO file not found at '{args.xacro_file}'", file=sys.stderr) + sys.exit(1) + + os.makedirs(args.output_dir, exist_ok=True) + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") + output_file = os.path.join(args.output_dir, f"manifold_samples_{timestamp}.csv") + + explorer = ManifoldExplorer(args.xacro_file) + explorer.run_exploration(args.num_samples, output_file) + + +if __name__ == "__main__": + main() diff --git a/ada_feeding/scripts/manifold_visualizer.py b/ada_feeding/scripts/manifold_visualizer.py new file mode 100644 index 00000000..3f0c21ef --- /dev/null +++ b/ada_feeding/scripts/manifold_visualizer.py @@ -0,0 +1,130 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script visualizes the high-dimensional feasibility manifold data generated +by manifold_explorer.py. It uses t-SNE for dimensionality reduction to project +the 6D joint space data into a 2D plot. + +The resulting interactive plot helps in understanding the structure, size, and +fragmentation of the feasible configuration space. +""" + +import argparse +import sys +import os +import pandas as pd +from sklearn.manifold import TSNE +import plotly.express as px + + +def visualize_manifold(csv_file_path: str, output_dir: str): + """ + Loads manifold data, performs t-SNE, and generates an interactive plot. + """ + print(f"Loading data from {csv_file_path}...") + try: + df = pd.read_csv(csv_file_path) + except FileNotFoundError: + print(f"Error: File not found at '{csv_file_path}'", file=sys.stderr) + sys.exit(1) + except Exception as e: + print(f"Error reading CSV file: {e}", file=sys.stderr) + sys.exit(1) + + # Separate features (joint values) and labels (feasibility) + features = df.drop(columns=["is_feasible"]) + labels = df["is_feasible"] + + # --- Basic Analysis: Feasibility Ratio --- + feasibility_ratio = labels.mean() + print(f"\nFeasibility Ratio: {feasibility_ratio:.2%}") + print(f"Out of {len(df)} samples, {int(labels.sum())} were feasible.") + + # --- Dimensionality Reduction with t-SNE --- + # Note: t-SNE can be slow on large datasets. For very large files (>50k samples), + # you might consider using a subset of the data for faster iteration. + print("\nPerforming t-SNE for dimensionality reduction (this may take a while)...") + tsne = TSNE(n_components=2, verbose=1, perplexity=40, n_iter=300, random_state=42) + features_2d = tsne.fit_transform(features) + print("t-SNE complete.") + + # --- Create a new, combined DataFrame for plotting --- + # This DataFrame will contain the t-SNE components for the plot axes, + # the original joint values for the hover tooltip, and the feasibility label for color. + plot_df = pd.DataFrame(features_2d, columns=["tsne_1", "tsne_2"]) + + # Reset indices of original data to ensure correct concatenation + features.reset_index(drop=True, inplace=True) + labels.reset_index(drop=True, inplace=True) + + plot_df = pd.concat([plot_df, features, labels], axis=1) + plot_df["is_feasible"] = plot_df["is_feasible"].astype( + bool + ) # Convert 0/1 to False/True for a better legend + + # --- Generate the Interactive Plot --- + print("Generating interactive plot...") + fig = px.scatter( + plot_df, # Use the new, combined DataFrame + x="tsne_1", + y="tsne_2", + color="is_feasible", + color_discrete_map={ + True: "rgba(44, 160, 44, 0.7)", # Green, slightly transparent + False: "rgba(214, 39, 40, 0.7)", # Red, slightly transparent + }, + title="2D Visualization of the Articutool Leveling Feasibility Manifold", + labels={"color": "Is Feasible"}, + # The hover_data argument now works correctly because the 'j1', 'j2', etc. + # columns exist in the `plot_df` DataFrame. + hover_data={ + "tsne_1": False, # Hide the t-SNE coordinates from the hover tooltip + "tsne_2": False, + "is_feasible": True, + "j1": ":.3f", + "j2": ":.3f", + "j3": ":.3f", + "j4": ":.3f", + "j5": ":.3f", + "j6": ":.3f", + }, + ) + + fig.update_layout( + xaxis_title="t-SNE Dimension 1", + yaxis_title="t-SNE Dimension 2", + legend_title_text="Feasibility", + plot_bgcolor="rgba(240, 240, 240, 0.95)", + ) + + # Save the plot to an HTML file + output_filename = os.path.join( + output_dir, + f"manifold_visualization_{os.path.basename(csv_file_path).replace('.csv', '.html')}", + ) + fig.write_html(output_filename) + print(f"\nSuccessfully saved visualization to:\n{output_filename}") + + +def main(): + parser = argparse.ArgumentParser( + description="Visualize high-dimensional manifold data using t-SNE." + ) + parser.add_argument( + "csv_file", type=str, help="Path to the manifold data CSV file." + ) + parser.add_argument( + "--output_dir", + type=str, + default="manifold_data", + help="Directory to save the output HTML file.", + ) + args = parser.parse_args() + + visualize_manifold(args.csv_file, args.output_dir) + + +if __name__ == "__main__": + main() From 9f6a1baa5fbf163ec8d41def4a88ac5e691d315f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 22 Jul 2025 20:28:05 -0700 Subject: [PATCH 146/238] Implement constraint aware start states, and update the analysis script to read new status column --- ada_feeding/scripts/holistic_benchmark.py | 206 +++++----- .../scripts/holistic_benchmark_analysis.py | 377 +++++++++--------- 2 files changed, 292 insertions(+), 291 deletions(-) diff --git a/ada_feeding/scripts/holistic_benchmark.py b/ada_feeding/scripts/holistic_benchmark.py index 8e3c5a46..c055eab4 100644 --- a/ada_feeding/scripts/holistic_benchmark.py +++ b/ada_feeding/scripts/holistic_benchmark.py @@ -8,18 +8,10 @@ comprehensive dataset to empirically evaluate the trade-offs between planner success, motion flexibility, and guaranteed kinematic/dynamic feasibility. -This version uses a resilient "generate-and-test" approach and implements -granular failure logging to distinguish between different failure modes. - -The script can be run in one of four modes via the `--mode` flag: -1. `joint_unconstrained`: The baseline. Plans between random joint-space goals - with no path constraints. -2. `task_pos_unconstrained`: Plans to a random task-space position goal with - no path constraints. -3. `task_pos_constrained`: Plans to a random task-space position goal WITH the - "smart" orientation path constraint. -4. `task_pos_yaw_constrained`: Plans to a random task-space position AND yaw - goal, WITH the "smart" orientation path constraint. +This version uses "constraint-aware" start state sampling, ensuring that each +trial begins from a configuration that is kinematically feasible for the +Articutool to maintain a level orientation. It also uses granular failure +logging to distinguish between different failure modes. """ # Standard imports @@ -81,7 +73,7 @@ class TrialStatus(Enum): SUCCESS = "Success" PLANNER_FAILURE = "Planner Failure" - START_STATE_INFEASIBLE = "Start State Infeasible" + START_STATE_SEARCH_FAILED = "Start State Search Failed" PATH_VERIFICATION_FAILURE = "Path Verification Failure" @@ -169,44 +161,92 @@ def generate_random_joint_config(self) -> List[float]: """Generates a random joint configuration within limits.""" return [np.random.uniform(low, high) for low, high in self.joint_limits] - def _check_articutool_feasibility_at_config( - self, jaco_joint_config: List[float] - ) -> bool: - """Checks if the Articutool can maintain leveling at a single Jaco configuration.""" - if self.pinocchio_model is None: + def _solve_articutool_ik( + self, target_vector: np.ndarray + ) -> List[Tuple[float, float]]: + """Analytical IK solver for the Articutool.""" + vx, vy, vz = target_vector + solutions = [] + asin_arg = -vx + if not (-1.0 - EPSILON <= asin_arg <= 1.0 + EPSILON): + return [] + theta_r1 = math.asin(np.clip(asin_arg, -1.0, 1.0)) + theta_r2 = (math.pi - theta_r1 + math.pi) % (2 * math.pi) - math.pi + for theta_r in list(set([theta_r1, theta_r2])): + cos_tr = math.cos(theta_r) + if math.isclose(cos_tr, 0.0, abs_tol=EPSILON): + if math.isclose(vy, 0.0, abs_tol=EPSILON) and math.isclose( + vz, 0.0, abs_tol=EPSILON + ): + solutions.append( + (0.0, (theta_r + math.pi) % (2 * math.pi) - math.pi) + ) + continue + theta_p = math.atan2(vz, vy) + solutions.append( + ( + (theta_p + math.pi) % (2 * math.pi) - math.pi, + (theta_r + math.pi) % (2 * math.pi) - math.pi, + ) + ) + return solutions + + def _is_config_kinematically_feasible(self, jaco_joint_config: List[float]) -> bool: + """ + Checks if the Articutool can maintain leveling at a single Jaco configuration. + This is our "oracle" for testing membership in the true feasibility manifold. + """ + if ( + self.pinocchio_model is None + or self.pinocchio_data is None + or self.jaco_ee_frame_id_pin is None + ): return False q = pin.neutral(self.pinocchio_model) for j, name in enumerate(JOINT_NAMES): - joint_id = self.pinocchio_model.getJointId(name) - joint_obj = self.pinocchio_model.joints[joint_id] - if joint_obj.nq == 2: # Prismatic joint - q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ - math.cos(jaco_joint_config[j]), - math.sin(jaco_joint_config[j]), - ] - else: # Revolute joint - q[joint_obj.idx_q] = jaco_joint_config[j] + if self.pinocchio_model.existJointName(name): + joint_id = self.pinocchio_model.getJointId(name) + joint_obj = self.pinocchio_model.joints[joint_id] + if joint_obj.nq == 2 and not joint_obj.shortname().startswith( + "JointModelRX" + ): + q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ + math.cos(jaco_joint_config[j]), + math.sin(jaco_joint_config[j]), + ] + else: + q[joint_obj.idx_q] = jaco_joint_config[j] pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] - target_up = R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) - solutions = self._solve_articutool_ik(target_up) + target_up_in_ee_frame = ( + R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) + ) + solutions = self._solve_articutool_ik(target_up_in_ee_frame) if not solutions: return False return any( - ARTICUTOOL_PITCH_LIMITS_RAD[0] <= s[0] <= ARTICUTOOL_PITCH_LIMITS_RAD[1] - and ARTICUTOOL_ROLL_LIMITS_RAD[0] <= s[1] <= ARTICUTOOL_ROLL_LIMITS_RAD[1] - for s in solutions + ARTICUTOOL_PITCH_LIMITS_RAD[0] <= pitch <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] <= roll <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + for pitch, roll in solutions ) - def is_start_state_feasible(self, start_joint_config: List[float]) -> bool: - """Checks if a given start state is kinematically feasible for the Articutool.""" - return self._check_articutool_feasibility_at_config(start_joint_config) + def _generate_feasible_start_config( + self, max_attempts=200 + ) -> Optional[List[float]]: + """ + Generates a random start configuration that lies on the true feasibility manifold. + """ + for _ in range(max_attempts): + config = self.generate_random_joint_config() + if self._is_config_kinematically_feasible(config): + return config + return None def generate_random_position_in_workspace(self) -> np.ndarray: """Generates a random (x, y, z) position from a half-spherical shell.""" @@ -236,36 +276,6 @@ def _get_articutool_jacobian(self, pitch: float, roll: float) -> np.ndarray: dydr = np.array([-cr, -cp * sr, -sp * sr]) return np.vstack([dydp, dydr]).T - def _solve_articutool_ik( - self, target_vector: np.ndarray - ) -> List[Tuple[float, float]]: - """Analytical IK solver for the Articutool.""" - vx, vy, vz = target_vector - solutions = [] - asin_arg = -vx - if not (-1.0 - EPSILON <= asin_arg <= 1.0 + EPSILON): - return [] - theta_r1 = math.asin(np.clip(asin_arg, -1.0, 1.0)) - theta_r2 = (math.pi - theta_r1 + math.pi) % (2 * math.pi) - math.pi - for theta_r in list(set([theta_r1, theta_r2])): - cos_tr = math.cos(theta_r) - if math.isclose(cos_tr, 0.0, abs_tol=EPSILON): - if math.isclose(vy, 0.0, abs_tol=EPSILON) and math.isclose( - vz, 0.0, abs_tol=EPSILON - ): - solutions.append( - (0.0, (theta_r + math.pi) % (2 * math.pi) - math.pi) - ) - continue - theta_p = math.atan2(vz, vy) - solutions.append( - ( - (theta_p + math.pi) % (2 * math.pi) - math.pi, - (theta_r + math.pi) % (2 * math.pi) - math.pi, - ) - ) - return solutions - def _calculate_trajectory_metrics( self, trajectory: JointTrajectory ) -> Tuple[bool, Optional[TrajectoryMetrics]]: @@ -282,13 +292,11 @@ def _calculate_trajectory_metrics( is_path_feasible = True for i, point in enumerate(trajectory.points): - is_waypoint_feasible = self._check_articutool_feasibility_at_config( - point.positions - ) - if not is_waypoint_feasible: + if not self._is_config_kinematically_feasible(point.positions): is_path_feasible = False + # We can break early if we only care about the boolean result, + # but we continue here to gather full data for the failed path. - # Continue calculating metrics even if path is infeasible to log data for j, name in enumerate(JOINT_NAMES): joint_id = self.pinocchio_model.getJointId(name) joint_obj = self.pinocchio_model.joints[joint_id] @@ -307,7 +315,6 @@ def _calculate_trajectory_metrics( np.zeros(self.pinocchio_model.nv), ) ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] - # This calculation is now duplicated, but kept for velocity analysis target_up = ( R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) ) @@ -384,42 +391,48 @@ def _calculate_trajectory_metrics( return is_path_feasible, metrics def run(self): - """Main benchmark execution loop with granular failure logging.""" + """Main benchmark execution loop with constraint-aware start state sampling.""" if not self.pinocchio_model: LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") return successful_tasks = 0 total_attempts = 0 - max_attempts = self.num_tasks * 50 # Increased max attempts + max_total_attempts = self.num_tasks * 50 LOGGER.info( f"Attempting to collect {self.num_tasks} successful planning tasks..." ) - while successful_tasks < self.num_tasks and total_attempts < max_attempts: + while successful_tasks < self.num_tasks and total_attempts < max_total_attempts: total_attempts += 1 LOGGER.info( f"--- Task {successful_tasks + 1}/{self.num_tasks} (Attempt {total_attempts}) ---" ) - start_pos = self.generate_random_joint_config() - - # 1. Check if the start state is feasible for the Articutool - if not self.is_start_state_feasible(start_pos): - LOGGER.warn( - " Attempt FAILED: Start state is kinematically infeasible for Articutool." - ) - self.results.append( - { - "task_id": successful_tasks + 1, - "attempt_id": total_attempts, - "planning_mode": self.mode, - "status": TrialStatus.START_STATE_INFEASIBLE.value, - "start_joint_positions": list(start_pos), - } + # For modes requiring leveling, find a start state on the true manifold. + # For pure joint-space planning, any random start is fine. + if self.mode != "joint_unconstrained": + LOGGER.info( + " Searching for a kinematically feasible start configuration..." ) - continue + start_pos = self._generate_feasible_start_config() + if start_pos is None: + LOGGER.warn( + " Attempt FAILED: Could not find a feasible start state within max attempts." + ) + self.results.append( + { + "task_id": successful_tasks + 1, + "attempt_id": total_attempts, + "planning_mode": self.mode, + "status": TrialStatus.START_STATE_SEARCH_FAILED.value, + } + ) + continue + LOGGER.info(" Found feasible start state.") + else: + start_pos = self.generate_random_joint_config() self.moveit2.clear_goal_constraints() self.moveit2.clear_path_constraints() @@ -486,7 +499,6 @@ def run(self): if target_quat: trial_data["target_yaw_quat"] = list(target_quat) - # 2. Check if the planner found a solution if not traj or not traj.points: LOGGER.warn( f" Attempt FAILED: Planner did not find a solution in {planning_time:.4f}s." @@ -495,7 +507,6 @@ def run(self): self.results.append(trial_data) continue - # 3. Verify the path and calculate metrics is_feasible, metrics = self._calculate_trajectory_metrics(traj) trial_data["trajectory_metrics"] = metrics._asdict() if metrics else None @@ -507,7 +518,6 @@ def run(self): self.results.append(trial_data) continue - # 4. If all checks pass, it's a success successful_tasks += 1 LOGGER.info( f" Plan SUCCEEDED and VERIFIED in {planning_time:.4f}s. ({successful_tasks}/{self.num_tasks} collected)" @@ -517,7 +527,7 @@ def run(self): if successful_tasks < self.num_tasks: LOGGER.error( - f"Benchmark finished due to max attempts ({max_attempts}). Only collected {successful_tasks}/{self.num_tasks} tasks." + f"Benchmark finished due to max attempts ({max_total_attempts}). Only collected {successful_tasks}/{self.num_tasks} tasks." ) def save_results(self): @@ -535,7 +545,6 @@ def save_results(self): except Exception as e: LOGGER.error(f"Failed to save results: {e}") - # New summary with granular failure counts status_counts = Counter(r["status"] for r in self.results) total_attempts = len(self.results) total_successful = status_counts[TrialStatus.SUCCESS.value] @@ -548,9 +557,10 @@ def save_results(self): for status in TrialStatus: if status != TrialStatus.SUCCESS: count = status_counts[status.value] - LOGGER.info( - f" - {status.name}: {count} ({count / total_attempts * 100:.2f}%)" - ) + if count > 0: + LOGGER.info( + f" - {status.name}: {count} ({count / total_attempts * 100:.2f}%)" + ) LOGGER.info("---------------------------------------------------\n") diff --git a/ada_feeding/scripts/holistic_benchmark_analysis.py b/ada_feeding/scripts/holistic_benchmark_analysis.py index ad91b6d4..9c574c61 100644 --- a/ada_feeding/scripts/holistic_benchmark_analysis.py +++ b/ada_feeding/scripts/holistic_benchmark_analysis.py @@ -3,79 +3,88 @@ # License: BSD 3-Clause. See LICENSE.md file in root directory. """ -This script loads and analyzes one or more comprehensive JSON results files -generated by `constrained_task_space_benchmark.py`. It generates a series of -side-by-side comparative plots to evaluate different planning methodologies. - -Requires: pandas, plotly -Install with: pip install pandas plotly +This script analyzes the output of the holistic_benchmark.py script. +It loads one or more JSON result files, concatenates them, and generates +a series of comparative plots to evaluate the different planning methodologies. """ -import numpy as np import argparse -import json +import sys import os import pandas as pd import plotly.express as px import plotly.graph_objects as go +from plotly.subplots import make_subplots +import json +from typing import List, Dict, Any -def load_data(json_files: list[str]) -> pd.DataFrame: - """Loads and concatenates results from multiple JSON files.""" +def load_data(file_paths: List[str]) -> pd.DataFrame: + """Loads and concatenates data from multiple JSON files.""" all_data = [] - for file_path in json_files: + for file_path in file_paths: print(f"Loading data from {file_path}...") try: with open(file_path, "r") as f: data = json.load(f) all_data.extend(data) - except (FileNotFoundError, json.JSONDecodeError) as e: + except Exception as e: print( - f" - Warning: Could not load or parse {file_path}. Skipping. Error: {e}" + f"Warning: Could not load or parse {file_path}. Error: {e}", + file=sys.stderr, ) - continue if not all_data: - print("Error: No valid data loaded.") - return pd.DataFrame() + print("Error: No data loaded. Exiting.", file=sys.stderr) + sys.exit(1) - df = pd.json_normalize(all_data, sep="_") print( - f"Successfully loaded a total of {len(df)} trials from {len(json_files)} file(s)." + f"Successfully loaded a total of {len(all_data)} trials from {len(file_paths)} file(s)." ) - return df + return pd.json_normalize(all_data, sep="_") def plot_success_rates(df: pd.DataFrame, output_dir: str): - """Plots planner and verification success rates grouped by planning mode.""" + """ + Generates a stacked bar chart showing the proportion of each trial status + for each planning methodology. + """ print("Generating success rate plot...") - summary = ( - df.groupby("planning_mode") - .agg( - planner_success=("plan_success", "mean"), - verification_success=("verification_success", "mean"), - ) - .reset_index() - ) - summary["planner_success"] *= 100 - summary["verification_success"] *= 100 - - summary_long = summary.melt( - id_vars="planning_mode", - value_vars=["planner_success", "verification_success"], - var_name="Metric", - value_name="Success Rate (%)", - ) + # --- Data Preparation --- + # Calculate the counts of each status for each planning mode + status_counts = df.groupby(["planning_mode", "status"]).size().unstack(fill_value=0) + + # Calculate the total attempts for each mode to normalize + total_attempts = status_counts.sum(axis=1) + + # Calculate percentages + status_percentages = status_counts.div(total_attempts, axis=0) * 100 + status_percentages = status_percentages.reset_index() + + # Define the desired order of statuses for plotting + status_order = [ + "Success", + "Path Verification Failure", + "Planner Failure", + "Start State Search Failed", + ] + + # Ensure all status columns exist, adding any that are missing with a value of 0 + for status in status_order: + if status not in status_percentages.columns: + status_percentages[status] = 0 + + # --- Plotting --- fig = px.bar( - summary_long, + status_percentages, x="planning_mode", - y="Success Rate (%)", - color="Metric", - title="Planner vs. Verification Success Rates by Methodology", - labels={"planning_mode": "Planning Methodology"}, - barmode="group", - text_auto=".1f", + y=status_order, # Plot each status as a segment of the bar + title="Breakdown of Trial Outcomes by Planning Methodology", + labels={ + "planning_mode": "Planning Methodology", + "value": "Percentage of Trials (%)", + }, category_orders={ "planning_mode": [ "joint_unconstrained", @@ -84,36 +93,47 @@ def plot_success_rates(df: pd.DataFrame, output_dir: str): "task_pos_yaw_constrained", ] }, + color_discrete_map={ + "Success": "#2ca02c", + "Path Verification Failure": "#ff7f0e", + "Planner Failure": "#d62728", + "Start State Search Failed": "#9467bd", + }, + ) + + fig.update_layout( + barmode="stack", + yaxis_title="Percentage of Total Trials (%)", + xaxis_title="Planning Methodology", + legend_title_text="Trial Outcome", ) - fig.update_layout(yaxis_range=[0, 105], legend_title_text="Success Metric") - - filename = os.path.join(output_dir, "comparative_success_rates.html") - fig.write_html(filename) - print(f" - Saved to {filename}") - - -def plot_comparative_metric( - df: pd.DataFrame, - metric_col: str, - title: str, - yaxis_title: str, - filename: str, - output_dir: str, -): - """Generic function to create a comparative box plot for a given metric.""" - print(f"Generating plot for: {title}...") - successful_plans = df[df["plan_success"] == True].copy() - if successful_plans.empty or metric_col not in successful_plans.columns: - print(f" - No data for metric '{metric_col}'. Skipping plot.") + + output_path = os.path.join(output_dir, "comparative_outcomes.html") + fig.write_html(output_path) + print(f"Saved success rate plot to {output_path}") + + +def plot_planning_times(df: pd.DataFrame, output_dir: str): + """Generates a box plot of planning times for successful trials.""" + print("Generating planning time plot...") + + # Filter for successful trials only + success_df = df[df["status"] == "Success"].copy() + + if success_df.empty: + print("Warning: No successful trials found. Skipping planning time plot.") return fig = px.box( - successful_plans, + success_df, x="planning_mode", - y=metric_col, + y="planning_time_s", color="planning_mode", - title=title, - labels={"planning_mode": "Planning Methodology", metric_col: yaxis_title}, + title="Planning Time for Successful Trials", + labels={ + "planning_mode": "Planning Methodology", + "planning_time_s": "Planning Time (s)", + }, category_orders={ "planning_mode": [ "joint_unconstrained", @@ -124,47 +144,37 @@ def plot_comparative_metric( }, ) - filepath = os.path.join(output_dir, filename) - fig.write_html(filepath) - print(f" - Saved to {filepath}") + output_path = os.path.join(output_dir, "comparative_planning_times.html") + fig.write_html(output_path) + print(f"Saved planning time plot to {output_path}") -def plot_articutool_velocities_comparison(df: pd.DataFrame, output_dir: str): - """Plots a 2D histogram of required Articutool velocities, faceted by mode.""" - print("Generating Articutool velocity comparison plot...") - successful_plans = df[df["plan_success"] == True].copy() +def plot_path_lengths(df: pd.DataFrame, output_dir: str): + """Generates a box plot of joint-space path lengths for successful trials.""" + print("Generating path length plot...") - velocity_data = [] - for index, row in successful_plans.iterrows(): - waypoints = row.get("trajectory_metrics_waypoints_data") - if waypoints and isinstance(waypoints, list): - for wp in waypoints: - vels = wp.get("articutool_velocities_rad_per_sec") - if vels and isinstance(vels, dict): - velocity_data.append( - { - "planning_mode": row["planning_mode"], - "pitch_vel": vels.get("pitch_vel"), - "roll_vel": vels.get("roll_vel"), - } - ) + success_df = df[df["status"] == "Success"].copy() - if not velocity_data: - print(" - No velocity data found to plot.") + if success_df.empty: + print("Warning: No successful trials found. Skipping path length plot.") return - vel_df = pd.DataFrame(velocity_data).dropna() + # Ensure the path length column exists + if "trajectory_metrics_joint_space_path_length_rad" not in success_df.columns: + print( + "Warning: 'trajectory_metrics_joint_space_path_length_rad' column not found. Skipping path length plot." + ) + return - fig = px.density_heatmap( - vel_df, - x="pitch_vel", - y="roll_vel", - facet_col="planning_mode", - facet_col_wrap=2, - title="Distribution of Required Articutool Velocities by Methodology", + fig = px.box( + success_df, + x="planning_mode", + y="trajectory_metrics_joint_space_path_length_rad", + color="planning_mode", + title="Joint-Space Path Length for Successful Trials", labels={ - "pitch_vel": "Pitch Velocity (rad/s)", - "roll_vel": "Roll Velocity (rad/s)", + "planning_mode": "Planning Methodology", + "trajectory_metrics_joint_space_path_length_rad": "Path Length (rad)", }, category_orders={ "planning_mode": [ @@ -176,132 +186,113 @@ def plot_articutool_velocities_comparison(df: pd.DataFrame, output_dir: str): }, ) - filename = os.path.join(output_dir, "comparative_articutool_velocities.html") - fig.write_html(filename) - print(f" - Saved to {filename}") + output_path = os.path.join(output_dir, "comparative_path_lengths.html") + fig.write_html(output_path) + print(f"Saved path length plot to {output_path}") + + +def plot_articutool_velocities(df: pd.DataFrame, output_dir: str): + """Generates violin plots of required Articutool velocities.""" + print("Generating Articutool velocity plots...") + success_df = df[df["status"] == "Success"].copy() -def plot_3d_trajectories(df: pd.DataFrame, output_dir: str, num_examples: int = 5): - """Plots start/end points and example trajectories in 3D space.""" - if df.empty or "trajectory_metrics_waypoints_data" not in df.columns: + if success_df.empty: + print("Warning: No successful trials found. Skipping Articutool velocity plot.") return - print("Generating 3D trajectory plot...") - successful_plans = df[df["plan_success"] == True].copy() - if successful_plans.empty: - print(" - No successful plans to plot.") + # Explode the waypoints data + waypoints_data = [] + for _, row in success_df.iterrows(): + if isinstance(row.get("trajectory_metrics_waypoints_data"), list): + for waypoint in row["trajectory_metrics_waypoints_data"]: + if waypoint and isinstance( + waypoint.get("articutool_velocities_rad_per_sec"), dict + ): + waypoints_data.append( + { + "planning_mode": row["planning_mode"], + "pitch_vel": waypoint[ + "articutool_velocities_rad_per_sec" + ].get("pitch_vel"), + "roll_vel": waypoint[ + "articutool_velocities_rad_per_sec" + ].get("roll_vel"), + } + ) + + if not waypoints_data: + print("Warning: No valid Articutool velocity data found. Skipping plot.") return - end_points, start_points = [], [] - for _, row in successful_plans.iterrows(): - waypoints = row["trajectory_metrics_waypoints_data"] - if waypoints and isinstance(waypoints, list) and len(waypoints) > 0: - start_points.append(waypoints[0]["ee_pose_world"]["position"]) - end_points.append(waypoints[-1]["ee_pose_world"]["position"]) - - fig = go.Figure() - - fig.add_trace( - go.Scatter3d( - x=[p[0] for p in start_points], - y=[p[1] for p in start_points], - z=[p[2] for p in start_points], - mode="markers", - marker=dict(color="blue", size=4, opacity=0.6), - name="Start EE Position", - ) - ) - fig.add_trace( - go.Scatter3d( - x=[p[0] for p in end_points], - y=[p[1] for p in end_points], - z=[p[2] for p in end_points], - mode="markers", - marker=dict(color="red", size=4, opacity=0.6), - name="End EE Position", - ) - ) + vel_df = pd.DataFrame(waypoints_data).dropna() - example_indices = np.random.choice( - successful_plans.index, min(num_examples, len(successful_plans)), replace=False + fig = make_subplots( + rows=1, cols=2, subplot_titles=("Pitch Velocity", "Roll Velocity") ) - for i, index in enumerate(example_indices): - waypoints = successful_plans.loc[index, "trajectory_metrics_waypoints_data"] - path_x = [wp["ee_pose_world"]["position"][0] for wp in waypoints] - path_y = [wp["ee_pose_world"]["position"][1] for wp in waypoints] - path_z = [wp["ee_pose_world"]["position"][2] for wp in waypoints] - fig.add_trace( - go.Scatter3d( - x=path_x, - y=path_y, - z=path_z, - mode="lines", - line=dict(width=5), - name=f"Example Traj {i + 1}", - ) + + for i, vel_type in enumerate(["pitch_vel", "roll_vel"]): + sub_fig = px.violin( + vel_df, + x="planning_mode", + y=vel_type, + color="planning_mode", + category_orders={ + "planning_mode": [ + "joint_unconstrained", + "task_pos_unconstrained", + "task_pos_constrained", + "task_pos_yaw_constrained", + ] + }, ) + for trace in sub_fig.data: + fig.add_trace(trace, row=1, col=i + 1) fig.update_layout( - title="3D Visualization of End-Effector Trajectories", - scene=dict( - xaxis_title="X (m)", - yaxis_title="Y (m)", - zaxis_title="Z (m)", - aspectmode="data", - ), - margin=dict(l=0, r=0, b=0, t=40), + title_text="Required Articutool Velocities During Successful Trajectories", + showlegend=False, ) + fig.update_yaxes(title_text="Velocity (rad/s)", row=1, col=1) + fig.update_yaxes(title_text="Velocity (rad/s)", row=1, col=2) + fig.update_xaxes(title_text="Planning Methodology", row=1, col=1) + fig.update_xaxes(title_text="Planning Methodology", row=1, col=2) - filename = os.path.join(output_dir, "3d_trajectories.html") - fig.write_html(filename) - print(f" - Saved to {filename}") + output_path = os.path.join(output_dir, "comparative_articutool_velocities.html") + fig.write_html(output_path) + print(f"Saved Articutool velocity plot to {output_path}") def main(): - """Main function to load data and generate all plots.""" parser = argparse.ArgumentParser( - description="Holistic analysis of benchmark results." + description="Analyze results from the holistic benchmark." ) parser.add_argument( "json_files", nargs="+", - help="Path(s) to the comprehensive benchmark results JSON file(s).", + help="One or more JSON result files to analyze (e.g., 'output/*.json').", ) parser.add_argument( "--output_dir", type=str, - default="holistic_analysis_plots", - help="Directory to save the generated plots.", + default="benchmark_analysis_plots", + help="Directory to save the output HTML plots.", ) args = parser.parse_args() + # Create output directory if it doesn't exist os.makedirs(args.output_dir, exist_ok=True) + + # Load and process data df = load_data(args.json_files) - if df.empty: - return # Generate all plots plot_success_rates(df, args.output_dir) - plot_comparative_metric( - df, - "planning_time_s", - "Planning Time Comparison", - "Planning Time (s)", - "comparative_planning_times.html", - args.output_dir, - ) - plot_comparative_metric( - df, - "trajectory_metrics_joint_space_path_length_rad", - "Joint-Space Path Length Comparison", - "Path Length (rad)", - "comparative_path_lengths.html", - args.output_dir, - ) - plot_articutool_velocities_comparison(df, args.output_dir) - plot_3d_trajectories(df, args.output_dir) + plot_planning_times(df, args.output_dir) + plot_path_lengths(df, args.output_dir) + plot_articutool_velocities(df, args.output_dir) - print("\nAnalysis complete. Plots saved in:", args.output_dir) + print("\nAnalysis complete.") if __name__ == "__main__": From 7fe2c2d55b4bc798c42dff7290d51e8ac5329c5c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 23 Jul 2025 18:37:52 -0700 Subject: [PATCH 147/238] Add constraint adherence score (WIP) --- .../scripts/holistic_benchmark_analysis.py | 84 +++++++++++++++++-- 1 file changed, 76 insertions(+), 8 deletions(-) diff --git a/ada_feeding/scripts/holistic_benchmark_analysis.py b/ada_feeding/scripts/holistic_benchmark_analysis.py index 9c574c61..90bb9064 100644 --- a/ada_feeding/scripts/holistic_benchmark_analysis.py +++ b/ada_feeding/scripts/holistic_benchmark_analysis.py @@ -5,7 +5,8 @@ """ This script analyzes the output of the holistic_benchmark.py script. It loads one or more JSON result files, concatenates them, and generates -a series of comparative plots to evaluate the different planning methodologies. +a series of comparative plots to evaluate the different planning methodologies, +including a new "Manifold Adherence" metric. """ import argparse @@ -44,12 +45,12 @@ def load_data(file_paths: List[str]) -> pd.DataFrame: return pd.json_normalize(all_data, sep="_") -def plot_success_rates(df: pd.DataFrame, output_dir: str): +def plot_outcomes(df: pd.DataFrame, output_dir: str): """ Generates a stacked bar chart showing the proportion of each trial status for each planning methodology. """ - print("Generating success rate plot...") + print("Generating trial outcomes plot...") # --- Data Preparation --- # Calculate the counts of each status for each planning mode @@ -110,7 +111,7 @@ def plot_success_rates(df: pd.DataFrame, output_dir: str): output_path = os.path.join(output_dir, "comparative_outcomes.html") fig.write_html(output_path) - print(f"Saved success rate plot to {output_path}") + print(f"Saved outcomes plot to {output_path}") def plot_planning_times(df: pd.DataFrame, output_dir: str): @@ -263,6 +264,75 @@ def plot_articutool_velocities(df: pd.DataFrame, output_dir: str): print(f"Saved Articutool velocity plot to {output_path}") +def plot_manifold_adherence(df: pd.DataFrame, output_dir: str): + """ + Calculates and plots the 'Manifold Adherence' for each successful trajectory. + Adherence is the percentage of waypoints within the preferred joint ranges + for joints 2 and 3, as discovered during manifold exploration. + """ + print("Generating manifold adherence plot...") + + success_df = df[df["status"] == "Success"].copy() + + if success_df.empty: + print("Warning: No successful trials found. Skipping manifold adherence plot.") + return + + adherence_scores = [] + for _, row in success_df.iterrows(): + waypoints = row.get("trajectory_metrics_waypoints_data") + if not isinstance(waypoints, list) or not waypoints: + continue + + adherent_waypoints = 0 + for wp in waypoints: + pos = wp.get("jaco_positions_rad") + if pos and len(pos) >= 3: + # FIX: Correctly check the preferred ranges based on joint distribution analysis. + # Joint 2 (pos[1]) preferred negative values. + # Joint 3 (pos[2]) preferred positive values. + if pos[1] < 0 and pos[2] > 0: + adherent_waypoints += 1 + + adherence_scores.append( + { + "planning_mode": row["planning_mode"], + "adherence_score": (adherent_waypoints / len(waypoints)) * 100, + } + ) + + if not adherence_scores: + print("Warning: Could not calculate any adherence scores. Skipping plot.") + return + + adherence_df = pd.DataFrame(adherence_scores) + + fig = px.box( + adherence_df, + x="planning_mode", + y="adherence_score", + color="planning_mode", + title="Manifold Adherence of Successful Trajectories", + labels={ + "planning_mode": "Planning Methodology", + "adherence_score": "Adherence Score (%)", + }, + category_orders={ + "planning_mode": [ + "joint_unconstrained", + "task_pos_unconstrained", + "task_pos_constrained", + "task_pos_yaw_constrained", + ] + }, + ) + fig.update_yaxes(range=[-5, 105]) + + output_path = os.path.join(output_dir, "comparative_manifold_adherence.html") + fig.write_html(output_path) + print(f"Saved manifold adherence plot to {output_path}") + + def main(): parser = argparse.ArgumentParser( description="Analyze results from the holistic benchmark." @@ -280,17 +350,15 @@ def main(): ) args = parser.parse_args() - # Create output directory if it doesn't exist os.makedirs(args.output_dir, exist_ok=True) - # Load and process data df = load_data(args.json_files) - # Generate all plots - plot_success_rates(df, args.output_dir) + plot_outcomes(df, args.output_dir) plot_planning_times(df, args.output_dir) plot_path_lengths(df, args.output_dir) plot_articutool_velocities(df, args.output_dir) + plot_manifold_adherence(df, args.output_dir) print("\nAnalysis complete.") From a0978ba68d5219b1738d5c6aa399ff5b2ff0d9d2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 25 Jul 2025 15:07:08 -0700 Subject: [PATCH 148/238] Add rigid-wrist baseline mode for holistic benchmark --- ada_feeding/scripts/holistic_benchmark.py | 200 +++++++++++++++------- 1 file changed, 134 insertions(+), 66 deletions(-) diff --git a/ada_feeding/scripts/holistic_benchmark.py b/ada_feeding/scripts/holistic_benchmark.py index c055eab4..580366b5 100644 --- a/ada_feeding/scripts/holistic_benchmark.py +++ b/ada_feeding/scripts/holistic_benchmark.py @@ -8,10 +8,8 @@ comprehensive dataset to empirically evaluate the trade-offs between planner success, motion flexibility, and guaranteed kinematic/dynamic feasibility. -This version uses "constraint-aware" start state sampling, ensuring that each -trial begins from a configuration that is kinematically feasible for the -Articutool to maintain a level orientation. It also uses granular failure -logging to distinguish between different failure modes. +This version includes a "rigid_wrist" baseline mode to quantify the +performance of a standard 6-DOF arm on the same leveling task. """ # Standard imports @@ -50,8 +48,15 @@ ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) -PATH_CONSTRAINT_QUAT_XYZW = (0.707, 0.0, 0.0, 0.707) -PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = (1.5, 3.14, 0.8) +# This quaternion represents the end-effector pointing forward and level +LEVEL_ORIENTATION_QUAT = R.from_euler("y", 90, degrees=True).as_quat() # [x, y, z, w] +# For the rigid wrist, the tolerance for being "level" is tight +RIGID_WRIST_LEVEL_TOLERANCE_RAD = math.radians(5.0) +PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = ( + math.pi, + math.pi, + math.pi, +) # Loose tolerance for Articutool modes GOAL_YAW_CONSTRAINT_TOLERANCE_XYZ_RAD = (math.pi, math.pi, 0.1) WORKSPACE_OUTER_RADIUS = 0.7 WORKSPACE_INNER_RADIUS = 0.3 @@ -64,6 +69,7 @@ "joint_space_path_length_rad", "final_joint_positions", "waypoints_data", + "max_leveling_error_rad", # New metric for rigid wrist mode ], ) @@ -194,7 +200,6 @@ def _solve_articutool_ik( def _is_config_kinematically_feasible(self, jaco_joint_config: List[float]) -> bool: """ Checks if the Articutool can maintain leveling at a single Jaco configuration. - This is our "oracle" for testing membership in the true feasibility manifold. """ if ( self.pinocchio_model is None @@ -290,13 +295,31 @@ def _calculate_trajectory_metrics( prev_pos = np.array(trajectory.points[0].positions) waypoints_data, path_len = [], 0.0 is_path_feasible = True + max_leveling_error = 0.0 for i, point in enumerate(trajectory.points): - if not self._is_config_kinematically_feasible(point.positions): - is_path_feasible = False - # We can break early if we only care about the boolean result, - # but we continue here to gather full data for the failed path. + # --- Verification Step --- + if self.mode == "rigid_wrist": + # For rigid wrist, "feasibility" is how well it stayed level + pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) + pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) + ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + + # Get the orientation of the end-effector's "up" vector in the world + ee_up_vector = R.from_matrix(ee_transform.rotation).apply([0, 1, 0]) + + # Calculate the angle between the EE up vector and the world up vector + angle = np.arccos( + np.clip(np.dot(ee_up_vector, WORLD_UP_VECTOR), -1.0, 1.0) + ) + if angle > max_leveling_error: + max_leveling_error = angle + else: + # For Articutool modes, feasibility is based on the IK solution + if not self._is_config_kinematically_feasible(point.positions): + is_path_feasible = False + # --- Metric Calculation (continues regardless of feasibility) --- for j, name in enumerate(JOINT_NAMES): joint_id = self.pinocchio_model.getJointId(name) joint_obj = self.pinocchio_model.joints[joint_id] @@ -315,55 +338,59 @@ def _calculate_trajectory_metrics( np.zeros(self.pinocchio_model.nv), ) ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] - target_up = ( - R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) - ) - solutions = self._solve_articutool_ik(target_up) at_sol, at_vel = None, None - if solutions: - valid_sols = [ - s - for s in solutions - if ARTICUTOOL_PITCH_LIMITS_RAD[0] - <= s[0] - <= ARTICUTOOL_PITCH_LIMITS_RAD[1] - and ARTICUTOOL_ROLL_LIMITS_RAD[0] - <= s[1] - <= ARTICUTOOL_ROLL_LIMITS_RAD[1] - ] - if valid_sols: - pitch, roll = min(valid_sols, key=lambda s: s[0] ** 2 + s[1] ** 2) - at_sol = {"pitch": pitch, "roll": roll} - if i > 0: - dt = ( - point.time_from_start.sec - + 1e-9 * point.time_from_start.nanosec - ) - ( - trajectory.points[i - 1].time_from_start.sec - + 1e-9 * trajectory.points[i - 1].time_from_start.nanosec + if self.mode != "rigid_wrist": + target_up = ( + R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) + ) + solutions = self._solve_articutool_ik(target_up) + if solutions: + valid_sols = [ + s + for s in solutions + if ARTICUTOOL_PITCH_LIMITS_RAD[0] + <= s[0] + <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] + <= s[1] + <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + ] + if valid_sols: + pitch, roll = min( + valid_sols, key=lambda s: s[0] ** 2 + s[1] ** 2 ) - if dt > EPSILON: - q_dot = ( - np.array(point.positions) - - np.array(trajectory.points[i - 1].positions) - ) / dt - J_full = pin.getFrameJacobian( - self.pinocchio_model, - self.pinocchio_data, - self.jaco_ee_frame_id_pin, - pin.ReferenceFrame.LOCAL, - ) - J_arm = J_full[:, self.jaco_vel_indices] - omega = (J_arm @ q_dot)[3:] - J_atool_inv = np.linalg.pinv( - self._get_articutool_jacobian(pitch, roll) + at_sol = {"pitch": pitch, "roll": roll} + if i > 0: + dt = ( + point.time_from_start.sec + + 1e-9 * point.time_from_start.nanosec + ) - ( + trajectory.points[i - 1].time_from_start.sec + + 1e-9 + * trajectory.points[i - 1].time_from_start.nanosec ) - q_dot_atool = -J_atool_inv @ omega - at_vel = { - "pitch_vel": q_dot_atool[0], - "roll_vel": q_dot_atool[1], - } + if dt > EPSILON: + q_dot = ( + np.array(point.positions) + - np.array(trajectory.points[i - 1].positions) + ) / dt + J_full = pin.getFrameJacobian( + self.pinocchio_model, + self.pinocchio_data, + self.jaco_ee_frame_id_pin, + pin.ReferenceFrame.LOCAL, + ) + J_arm = J_full[:, self.jaco_vel_indices] + omega = (J_arm @ q_dot)[3:] + J_atool_inv = np.linalg.pinv( + self._get_articutool_jacobian(pitch, roll) + ) + q_dot_atool = -J_atool_inv @ omega + at_vel = { + "pitch_vel": q_dot_atool[0], + "roll_vel": q_dot_atool[1], + } waypoints_data.append( { @@ -381,17 +408,24 @@ def _calculate_trajectory_metrics( path_len += np.linalg.norm(np.array(point.positions) - prev_pos) prev_pos = np.array(point.positions) + if self.mode == "rigid_wrist": + is_path_feasible = max_leveling_error <= RIGID_WRIST_LEVEL_TOLERANCE_RAD + duration = ( trajectory.points[-1].time_from_start.sec + 1e-9 * trajectory.points[-1].time_from_start.nanosec ) metrics = TrajectoryMetrics( - duration, path_len, list(trajectory.points[-1].positions), waypoints_data + duration, + path_len, + list(trajectory.points[-1].positions), + waypoints_data, + max_leveling_error, ) return is_path_feasible, metrics def run(self): - """Main benchmark execution loop with constraint-aware start state sampling.""" + """Main benchmark execution loop.""" if not self.pinocchio_model: LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") return @@ -410,9 +444,8 @@ def run(self): f"--- Task {successful_tasks + 1}/{self.num_tasks} (Attempt {total_attempts}) ---" ) - # For modes requiring leveling, find a start state on the true manifold. - # For pure joint-space planning, any random start is fine. - if self.mode != "joint_unconstrained": + start_pos = None + if self.mode not in ["joint_unconstrained", "rigid_wrist"]: LOGGER.info( " Searching for a kinematically feasible start configuration..." ) @@ -432,6 +465,7 @@ def run(self): continue LOGGER.info(" Found feasible start state.") else: + # For joint space and rigid wrist, any random start is fine. start_pos = self.generate_random_joint_config() self.moveit2.clear_goal_constraints() @@ -449,7 +483,38 @@ def run(self): target_link=END_EFFECTOR_LINK, tolerance=0.01, ) - if self.mode == "task_pos_yaw_constrained": + + if self.mode == "rigid_wrist": + self.moveit2.set_orientation_goal( + quat_xyzw=Quaternion( + x=LEVEL_ORIENTATION_QUAT[0], + y=LEVEL_ORIENTATION_QUAT[1], + z=LEVEL_ORIENTATION_QUAT[2], + w=LEVEL_ORIENTATION_QUAT[3], + ), + target_link=END_EFFECTOR_LINK, + tolerance=( + RIGID_WRIST_LEVEL_TOLERANCE_RAD, + RIGID_WRIST_LEVEL_TOLERANCE_RAD, + math.pi, + ), + ) + self.moveit2.set_path_orientation_constraint( + quat_xyzw=Quaternion( + x=LEVEL_ORIENTATION_QUAT[0], + y=LEVEL_ORIENTATION_QUAT[1], + z=LEVEL_ORIENTATION_QUAT[2], + w=LEVEL_ORIENTATION_QUAT[3], + ), + target_link=END_EFFECTOR_LINK, + tolerance=( + RIGID_WRIST_LEVEL_TOLERANCE_RAD, + RIGID_WRIST_LEVEL_TOLERANCE_RAD, + math.pi, + ), + weight=1.0, + ) + elif self.mode == "task_pos_yaw_constrained": target_quat = self.generate_random_yaw_quaternion() self.moveit2.set_orientation_goal( quat_xyzw=Quaternion( @@ -463,12 +528,14 @@ def run(self): parameterization=1, ) if "constrained" in self.mode: + # This is the original, looser constraint for the Articutool modes + path_constraint_quat = R.from_euler("y", 90, degrees=True).as_quat() self.moveit2.set_path_orientation_constraint( quat_xyzw=Quaternion( - x=PATH_CONSTRAINT_QUAT_XYZW[0], - y=PATH_CONSTRAINT_QUAT_XYZW[1], - z=PATH_CONSTRAINT_QUAT_XYZW[2], - w=PATH_CONSTRAINT_QUAT_XYZW[3], + x=path_constraint_quat[0], + y=path_constraint_quat[1], + z=path_constraint_quat[2], + w=path_constraint_quat[3], ), frame_id=BASE_LINK, target_link=END_EFFECTOR_LINK, @@ -580,6 +647,7 @@ def main(): "task_pos_unconstrained", "task_pos_constrained", "task_pos_yaw_constrained", + "rigid_wrist", # New baseline mode ], help="The planning methodology to benchmark.", ) From dbe12dea7745e758701976619536599edcd39fe2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 26 Jul 2025 12:30:08 -0700 Subject: [PATCH 149/238] Revert rigid tool mode, add mode to benchmark all control methods --- ada_feeding/scripts/holistic_benchmark.py | 272 +++++++++------------- 1 file changed, 113 insertions(+), 159 deletions(-) diff --git a/ada_feeding/scripts/holistic_benchmark.py b/ada_feeding/scripts/holistic_benchmark.py index 580366b5..f8a26d0a 100644 --- a/ada_feeding/scripts/holistic_benchmark.py +++ b/ada_feeding/scripts/holistic_benchmark.py @@ -8,8 +8,10 @@ comprehensive dataset to empirically evaluate the trade-offs between planner success, motion flexibility, and guaranteed kinematic/dynamic feasibility. -This version includes a "rigid_wrist" baseline mode to quantify the -performance of a standard 6-DOF arm on the same leveling task. +This version uses "constraint-aware" start state sampling, ensuring that each +trial begins from a configuration that is kinematically feasible for the +Articutool to maintain a level orientation. It also uses granular failure +logging to distinguish between different failure modes. """ # Standard imports @@ -48,15 +50,8 @@ ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) -# This quaternion represents the end-effector pointing forward and level -LEVEL_ORIENTATION_QUAT = R.from_euler("y", 90, degrees=True).as_quat() # [x, y, z, w] -# For the rigid wrist, the tolerance for being "level" is tight -RIGID_WRIST_LEVEL_TOLERANCE_RAD = math.radians(5.0) -PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = ( - math.pi, - math.pi, - math.pi, -) # Loose tolerance for Articutool modes +PATH_CONSTRAINT_QUAT_XYZW = (0.707, 0.0, 0.0, 0.707) +PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = (1.5, 3.14, 0.8) GOAL_YAW_CONSTRAINT_TOLERANCE_XYZ_RAD = (math.pi, math.pi, 0.1) WORKSPACE_OUTER_RADIUS = 0.7 WORKSPACE_INNER_RADIUS = 0.3 @@ -69,7 +64,6 @@ "joint_space_path_length_rad", "final_joint_positions", "waypoints_data", - "max_leveling_error_rad", # New metric for rigid wrist mode ], ) @@ -114,8 +108,7 @@ def __init__( self._initialize_pinocchio() self.joint_limits = self._get_joint_limits() - LOGGER.info("Holistic Benchmark Initialized.") - LOGGER.info(f" - Planning Mode: {self.mode}") + LOGGER.info(f"--- Initializing Benchmark: Mode '{self.mode}' ---") if self.output_dir: os.makedirs(self.output_dir, exist_ok=True) LOGGER.info(f" - Results will be saved to: {self.output_dir}") @@ -200,6 +193,7 @@ def _solve_articutool_ik( def _is_config_kinematically_feasible(self, jaco_joint_config: List[float]) -> bool: """ Checks if the Articutool can maintain leveling at a single Jaco configuration. + This is our "oracle" for testing membership in the true feasibility manifold. """ if ( self.pinocchio_model is None @@ -295,31 +289,13 @@ def _calculate_trajectory_metrics( prev_pos = np.array(trajectory.points[0].positions) waypoints_data, path_len = [], 0.0 is_path_feasible = True - max_leveling_error = 0.0 for i, point in enumerate(trajectory.points): - # --- Verification Step --- - if self.mode == "rigid_wrist": - # For rigid wrist, "feasibility" is how well it stayed level - pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) - pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) - ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] - - # Get the orientation of the end-effector's "up" vector in the world - ee_up_vector = R.from_matrix(ee_transform.rotation).apply([0, 1, 0]) - - # Calculate the angle between the EE up vector and the world up vector - angle = np.arccos( - np.clip(np.dot(ee_up_vector, WORLD_UP_VECTOR), -1.0, 1.0) - ) - if angle > max_leveling_error: - max_leveling_error = angle - else: - # For Articutool modes, feasibility is based on the IK solution - if not self._is_config_kinematically_feasible(point.positions): - is_path_feasible = False + if not self._is_config_kinematically_feasible(point.positions): + is_path_feasible = False + # We can break early if we only care about the boolean result, + # but we continue here to gather full data for the failed path. - # --- Metric Calculation (continues regardless of feasibility) --- for j, name in enumerate(JOINT_NAMES): joint_id = self.pinocchio_model.getJointId(name) joint_obj = self.pinocchio_model.joints[joint_id] @@ -338,59 +314,55 @@ def _calculate_trajectory_metrics( np.zeros(self.pinocchio_model.nv), ) ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + target_up = ( + R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) + ) + solutions = self._solve_articutool_ik(target_up) at_sol, at_vel = None, None - if self.mode != "rigid_wrist": - target_up = ( - R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) - ) - solutions = self._solve_articutool_ik(target_up) - if solutions: - valid_sols = [ - s - for s in solutions - if ARTICUTOOL_PITCH_LIMITS_RAD[0] - <= s[0] - <= ARTICUTOOL_PITCH_LIMITS_RAD[1] - and ARTICUTOOL_ROLL_LIMITS_RAD[0] - <= s[1] - <= ARTICUTOOL_ROLL_LIMITS_RAD[1] - ] - if valid_sols: - pitch, roll = min( - valid_sols, key=lambda s: s[0] ** 2 + s[1] ** 2 + if solutions: + valid_sols = [ + s + for s in solutions + if ARTICUTOOL_PITCH_LIMITS_RAD[0] + <= s[0] + <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] + <= s[1] + <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + ] + if valid_sols: + pitch, roll = min(valid_sols, key=lambda s: s[0] ** 2 + s[1] ** 2) + at_sol = {"pitch": pitch, "roll": roll} + if i > 0: + dt = ( + point.time_from_start.sec + + 1e-9 * point.time_from_start.nanosec + ) - ( + trajectory.points[i - 1].time_from_start.sec + + 1e-9 * trajectory.points[i - 1].time_from_start.nanosec ) - at_sol = {"pitch": pitch, "roll": roll} - if i > 0: - dt = ( - point.time_from_start.sec - + 1e-9 * point.time_from_start.nanosec - ) - ( - trajectory.points[i - 1].time_from_start.sec - + 1e-9 - * trajectory.points[i - 1].time_from_start.nanosec + if dt > EPSILON: + q_dot = ( + np.array(point.positions) + - np.array(trajectory.points[i - 1].positions) + ) / dt + J_full = pin.getFrameJacobian( + self.pinocchio_model, + self.pinocchio_data, + self.jaco_ee_frame_id_pin, + pin.ReferenceFrame.LOCAL, ) - if dt > EPSILON: - q_dot = ( - np.array(point.positions) - - np.array(trajectory.points[i - 1].positions) - ) / dt - J_full = pin.getFrameJacobian( - self.pinocchio_model, - self.pinocchio_data, - self.jaco_ee_frame_id_pin, - pin.ReferenceFrame.LOCAL, - ) - J_arm = J_full[:, self.jaco_vel_indices] - omega = (J_arm @ q_dot)[3:] - J_atool_inv = np.linalg.pinv( - self._get_articutool_jacobian(pitch, roll) - ) - q_dot_atool = -J_atool_inv @ omega - at_vel = { - "pitch_vel": q_dot_atool[0], - "roll_vel": q_dot_atool[1], - } + J_arm = J_full[:, self.jaco_vel_indices] + omega = (J_arm @ q_dot)[3:] + J_atool_inv = np.linalg.pinv( + self._get_articutool_jacobian(pitch, roll) + ) + q_dot_atool = -J_atool_inv @ omega + at_vel = { + "pitch_vel": q_dot_atool[0], + "roll_vel": q_dot_atool[1], + } waypoints_data.append( { @@ -408,24 +380,17 @@ def _calculate_trajectory_metrics( path_len += np.linalg.norm(np.array(point.positions) - prev_pos) prev_pos = np.array(point.positions) - if self.mode == "rigid_wrist": - is_path_feasible = max_leveling_error <= RIGID_WRIST_LEVEL_TOLERANCE_RAD - duration = ( trajectory.points[-1].time_from_start.sec + 1e-9 * trajectory.points[-1].time_from_start.nanosec ) metrics = TrajectoryMetrics( - duration, - path_len, - list(trajectory.points[-1].positions), - waypoints_data, - max_leveling_error, + duration, path_len, list(trajectory.points[-1].positions), waypoints_data ) return is_path_feasible, metrics def run(self): - """Main benchmark execution loop.""" + """Main benchmark execution loop with constraint-aware start state sampling.""" if not self.pinocchio_model: LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") return @@ -444,8 +409,9 @@ def run(self): f"--- Task {successful_tasks + 1}/{self.num_tasks} (Attempt {total_attempts}) ---" ) - start_pos = None - if self.mode not in ["joint_unconstrained", "rigid_wrist"]: + # For modes requiring leveling, find a start state on the true manifold. + # For pure joint-space planning, any random start is fine. + if self.mode != "joint_unconstrained": LOGGER.info( " Searching for a kinematically feasible start configuration..." ) @@ -465,7 +431,6 @@ def run(self): continue LOGGER.info(" Found feasible start state.") else: - # For joint space and rigid wrist, any random start is fine. start_pos = self.generate_random_joint_config() self.moveit2.clear_goal_constraints() @@ -483,38 +448,7 @@ def run(self): target_link=END_EFFECTOR_LINK, tolerance=0.01, ) - - if self.mode == "rigid_wrist": - self.moveit2.set_orientation_goal( - quat_xyzw=Quaternion( - x=LEVEL_ORIENTATION_QUAT[0], - y=LEVEL_ORIENTATION_QUAT[1], - z=LEVEL_ORIENTATION_QUAT[2], - w=LEVEL_ORIENTATION_QUAT[3], - ), - target_link=END_EFFECTOR_LINK, - tolerance=( - RIGID_WRIST_LEVEL_TOLERANCE_RAD, - RIGID_WRIST_LEVEL_TOLERANCE_RAD, - math.pi, - ), - ) - self.moveit2.set_path_orientation_constraint( - quat_xyzw=Quaternion( - x=LEVEL_ORIENTATION_QUAT[0], - y=LEVEL_ORIENTATION_QUAT[1], - z=LEVEL_ORIENTATION_QUAT[2], - w=LEVEL_ORIENTATION_QUAT[3], - ), - target_link=END_EFFECTOR_LINK, - tolerance=( - RIGID_WRIST_LEVEL_TOLERANCE_RAD, - RIGID_WRIST_LEVEL_TOLERANCE_RAD, - math.pi, - ), - weight=1.0, - ) - elif self.mode == "task_pos_yaw_constrained": + if self.mode == "task_pos_yaw_constrained": target_quat = self.generate_random_yaw_quaternion() self.moveit2.set_orientation_goal( quat_xyzw=Quaternion( @@ -528,14 +462,12 @@ def run(self): parameterization=1, ) if "constrained" in self.mode: - # This is the original, looser constraint for the Articutool modes - path_constraint_quat = R.from_euler("y", 90, degrees=True).as_quat() self.moveit2.set_path_orientation_constraint( quat_xyzw=Quaternion( - x=path_constraint_quat[0], - y=path_constraint_quat[1], - z=path_constraint_quat[2], - w=path_constraint_quat[3], + x=PATH_CONSTRAINT_QUAT_XYZW[0], + y=PATH_CONSTRAINT_QUAT_XYZW[1], + z=PATH_CONSTRAINT_QUAT_XYZW[2], + w=PATH_CONSTRAINT_QUAT_XYZW[3], ), frame_id=BASE_LINK, target_link=END_EFFECTOR_LINK, @@ -638,6 +570,7 @@ def main(): parser.add_argument( "xacro_file", type=str, help="Path to the robot URDF/XACRO file." ) + # MODIFICATION: Add 'all' to choices and update help text parser.add_argument( "--mode", type=str, @@ -647,15 +580,15 @@ def main(): "task_pos_unconstrained", "task_pos_constrained", "task_pos_yaw_constrained", - "rigid_wrist", # New baseline mode + "all", ], - help="The planning methodology to benchmark.", + help="The planning methodology to benchmark, or 'all' to run every mode.", ) parser.add_argument( "--num_tasks", type=int, default=100, - help="Number of successful tasks to generate.", + help="Number of successful tasks to generate per mode.", ) parser.add_argument( "--timeout", type=float, default=10.0, help="Planning timeout in seconds." @@ -672,6 +605,18 @@ def main(): print(f"Error: XACRO file not found at '{args.xacro_file}'") sys.exit(1) + # MODIFICATION: Logic to handle running a single mode or all modes + scenarios_to_run = [] + if args.mode == "all": + scenarios_to_run = [ + "joint_unconstrained", + "task_pos_unconstrained", + "task_pos_constrained", + "task_pos_yaw_constrained", + ] + else: + scenarios_to_run.append(args.mode) + rclpy.init() node = Node("holistic_benchmark_node") executor = rclpy.executors.MultiThreadedExecutor() @@ -688,28 +633,37 @@ def main(): callback_group=ReentrantCallbackGroup(), ) - benchmark = HolisticBenchmark( - node, - moveit2, - args.xacro_file, - args.mode, - args.num_tasks, - args.timeout, - "RRTConnectkConfigDefault", - args.output_dir, - ) - - try: - benchmark.run() - benchmark.save_results() - except Exception as e: - LOGGER.error( - f"An unhandled error occurred during the benchmark: {e}", exc_info=True + # MODIFICATION: Loop through the selected scenarios + for mode in scenarios_to_run: + LOGGER.info(f"\n\n===== STARTING BENCHMARK FOR MODE: {mode.upper()} =====\n") + + # Instantiate a new benchmark object for each mode to ensure clean state + benchmark = HolisticBenchmark( + node, + moveit2, + args.xacro_file, + mode, + args.num_tasks, + args.timeout, + "RRTConnectkConfigDefault", + args.output_dir, ) - finally: - LOGGER.info("Shutting down.") - rclpy.shutdown() - executor_thread.join() + + try: + benchmark.run() + benchmark.save_results() + except Exception as e: + LOGGER.error( + f"An unhandled error occurred during the benchmark for mode '{mode}': {e}", + exc_info=True, + ) + + # A brief pause can help separate logs if running multiple modes + time.sleep(2) + + LOGGER.info("All benchmark scenarios complete.") + rclpy.shutdown() + executor_thread.join() if __name__ == "__main__": From 97fbd6bf9ef65e1adbf45103ef62166aa90255a0 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 26 Jul 2025 14:34:48 -0700 Subject: [PATCH 150/238] Update benchmark analysis and trajectory visualization script --- .../scripts/holistic_benchmark_analysis.py | 511 ++++++++------ ada_feeding/scripts/visualize_trajectory.py | 655 ++++++++---------- 2 files changed, 613 insertions(+), 553 deletions(-) diff --git a/ada_feeding/scripts/holistic_benchmark_analysis.py b/ada_feeding/scripts/holistic_benchmark_analysis.py index 90bb9064..b36a69a3 100644 --- a/ada_feeding/scripts/holistic_benchmark_analysis.py +++ b/ada_feeding/scripts/holistic_benchmark_analysis.py @@ -5,8 +5,14 @@ """ This script analyzes the output of the holistic_benchmark.py script. It loads one or more JSON result files, concatenates them, and generates -a series of comparative plots to evaluate the different planning methodologies, -including a new "Manifold Adherence" metric. +a series of comparative plots to evaluate the different planning methodologies. + +Improvements in this version: +- Centralized constants for plot styling and ordering. +- A helper function to streamline extraction of granular waypoint data. +- New plots for Articutool joint angles, motion smoothness (jerk), and + a 3D visualization of where tasks succeed or fail in the workspace. +- A text-based summary of results printed to the console. """ import argparse @@ -18,6 +24,33 @@ from plotly.subplots import make_subplots import json from typing import List, Dict, Any +import numpy as np + +# --- Plotting Constants --- +# Centralize category order and color maps to ensure consistency across all plots. +PLANNING_MODE_ORDER = [ + "joint_unconstrained", + "task_pos_unconstrained", + "task_pos_constrained", + "task_pos_yaw_constrained", +] +STATUS_ORDER = [ + "Success", + "Path Verification Failure", + "Planner Failure", + "Start State Search Failed", +] +STATUS_COLOR_MAP = { + "Success": "#2ca02c", # Green + "Path Verification Failure": "#ff7f0e", # Orange + "Planner Failure": "#d62728", # Red + "Start State Search Failed": "#9467bd", # Purple +} +# Preferred joint ranges for manifold adherence, based on prior analysis. +# Jaco Joint 2 (index 1) prefers negative values. +# Jaco Joint 3 (index 2) prefers positive values. +PREFERRED_JOINT_2_RANGE = (-float("inf"), 0.0) +PREFERRED_JOINT_3_RANGE = (0.0, float("inf")) def load_data(file_paths: List[str]) -> pd.DataFrame: @@ -34,81 +67,89 @@ def load_data(file_paths: List[str]) -> pd.DataFrame: f"Warning: Could not load or parse {file_path}. Error: {e}", file=sys.stderr, ) - if not all_data: print("Error: No data loaded. Exiting.", file=sys.stderr) sys.exit(1) - - print( - f"Successfully loaded a total of {len(all_data)} trials from {len(file_paths)} file(s)." - ) + print(f"Successfully loaded {len(all_data)} trials from {len(file_paths)} file(s).") return pd.json_normalize(all_data, sep="_") -def plot_outcomes(df: pd.DataFrame, output_dir: str): - """ - Generates a stacked bar chart showing the proportion of each trial status - for each planning methodology. - """ - print("Generating trial outcomes plot...") +def _extract_waypoint_data(df: pd.DataFrame) -> pd.DataFrame: + """Helper function to parse and explode waypoint data from the main dataframe.""" + waypoint_rows = [] + for _, row in df.iterrows(): + waypoints = row.get("trajectory_metrics_waypoints_data") + if isinstance(waypoints, list): + for wp in waypoints: + if not isinstance(wp, dict): + continue + + # Extract data from the nested dictionaries with checks + at_sol = wp.get("articutool_solution_rad", {}) or {} + at_vel = wp.get("articutool_velocities_rad_per_sec", {}) or {} + ee_pose = wp.get("ee_pose_world", {}) or {} + + waypoint_rows.append( + { + "planning_mode": row["planning_mode"], + "task_id": row["task_id"], + "jaco_positions": wp.get("jaco_positions_rad"), + "ee_position": ee_pose.get("position"), + "time": wp.get("time_from_start_sec", 0.0), + "pitch_angle": at_sol.get("pitch"), + "roll_angle": at_sol.get("roll"), + "pitch_vel": at_vel.get("pitch_vel"), + "roll_vel": at_vel.get("roll_vel"), + } + ) + return pd.DataFrame(waypoint_rows).dropna(subset=["jaco_positions", "ee_position"]) + + +def print_summary_table(df: pd.DataFrame): + """Prints a text-based summary of trial outcomes to the console.""" + print("\n--- Benchmark Summary ---") + summary = ( + df.groupby("planning_mode")["status"] + .value_counts(normalize=True) + .unstack(fill_value=0) + * 100 + ) + summary["Total_Attempts"] = df.groupby("planning_mode").size() - # --- Data Preparation --- - # Calculate the counts of each status for each planning mode - status_counts = df.groupby(["planning_mode", "status"]).size().unstack(fill_value=0) + # Ensure all status columns exist for consistent printing + for status in STATUS_ORDER: + if status not in summary.columns: + summary[status] = 0.0 - # Calculate the total attempts for each mode to normalize - total_attempts = status_counts.sum(axis=1) + # Reorder columns for clarity + summary = summary[["Total_Attempts"] + STATUS_ORDER] - # Calculate percentages - status_percentages = status_counts.div(total_attempts, axis=0) * 100 - status_percentages = status_percentages.reset_index() + print(summary.to_string(float_format="%.2f%%")) + print("-------------------------\n") - # Define the desired order of statuses for plotting - status_order = [ - "Success", - "Path Verification Failure", - "Planner Failure", - "Start State Search Failed", - ] - # Ensure all status columns exist, adding any that are missing with a value of 0 - for status in status_order: - if status not in status_percentages.columns: - status_percentages[status] = 0 +def plot_outcomes(df: pd.DataFrame, output_dir: str): + """Generates a stacked bar chart of trial outcomes.""" + print("Generating trial outcomes plot...") + status_counts = df.groupby(["planning_mode", "status"]).size().unstack(fill_value=0) + status_percentages = status_counts.div(status_counts.sum(axis=1), axis=0) * 100 + status_percentages = status_percentages.reset_index() - # --- Plotting --- fig = px.bar( status_percentages, x="planning_mode", - y=status_order, # Plot each status as a segment of the bar + y=STATUS_ORDER, title="Breakdown of Trial Outcomes by Planning Methodology", - labels={ - "planning_mode": "Planning Methodology", - "value": "Percentage of Trials (%)", - }, - category_orders={ - "planning_mode": [ - "joint_unconstrained", - "task_pos_unconstrained", - "task_pos_constrained", - "task_pos_yaw_constrained", - ] - }, - color_discrete_map={ - "Success": "#2ca02c", - "Path Verification Failure": "#ff7f0e", - "Planner Failure": "#d62728", - "Start State Search Failed": "#9467bd", - }, + labels={"value": "Percentage of Trials (%)"}, + category_orders={"planning_mode": PLANNING_MODE_ORDER}, + color_discrete_map=STATUS_COLOR_MAP, ) - fig.update_layout( barmode="stack", yaxis_title="Percentage of Total Trials (%)", xaxis_title="Planning Methodology", legend_title_text="Trial Outcome", ) - output_path = os.path.join(output_dir, "comparative_outcomes.html") fig.write_html(output_path) print(f"Saved outcomes plot to {output_path}") @@ -117,10 +158,7 @@ def plot_outcomes(df: pd.DataFrame, output_dir: str): def plot_planning_times(df: pd.DataFrame, output_dir: str): """Generates a box plot of planning times for successful trials.""" print("Generating planning time plot...") - - # Filter for successful trials only - success_df = df[df["status"] == "Success"].copy() - + success_df = df[df["status"] == "Success"] if success_df.empty: print("Warning: No successful trials found. Skipping planning time plot.") return @@ -131,20 +169,10 @@ def plot_planning_times(df: pd.DataFrame, output_dir: str): y="planning_time_s", color="planning_mode", title="Planning Time for Successful Trials", - labels={ - "planning_mode": "Planning Methodology", - "planning_time_s": "Planning Time (s)", - }, - category_orders={ - "planning_mode": [ - "joint_unconstrained", - "task_pos_unconstrained", - "task_pos_constrained", - "task_pos_yaw_constrained", - ] - }, + labels={"planning_time_s": "Planning Time (s)"}, + category_orders={"planning_mode": PLANNING_MODE_ORDER}, ) - + fig.update_layout(xaxis_title="Planning Methodology") output_path = os.path.join(output_dir, "comparative_planning_times.html") fig.write_html(output_path) print(f"Saved planning time plot to {output_path}") @@ -153,195 +181,280 @@ def plot_planning_times(df: pd.DataFrame, output_dir: str): def plot_path_lengths(df: pd.DataFrame, output_dir: str): """Generates a box plot of joint-space path lengths for successful trials.""" print("Generating path length plot...") - - success_df = df[df["status"] == "Success"].copy() - + success_df = df[df["status"] == "Success"] if success_df.empty: print("Warning: No successful trials found. Skipping path length plot.") return - # Ensure the path length column exists - if "trajectory_metrics_joint_space_path_length_rad" not in success_df.columns: - print( - "Warning: 'trajectory_metrics_joint_space_path_length_rad' column not found. Skipping path length plot." - ) - return - fig = px.box( success_df, x="planning_mode", y="trajectory_metrics_joint_space_path_length_rad", color="planning_mode", title="Joint-Space Path Length for Successful Trials", - labels={ - "planning_mode": "Planning Methodology", - "trajectory_metrics_joint_space_path_length_rad": "Path Length (rad)", - }, - category_orders={ - "planning_mode": [ - "joint_unconstrained", - "task_pos_unconstrained", - "task_pos_constrained", - "task_pos_yaw_constrained", - ] - }, + labels={"trajectory_metrics_joint_space_path_length_rad": "Path Length (rad)"}, + category_orders={"planning_mode": PLANNING_MODE_ORDER}, ) - + fig.update_layout(xaxis_title="Planning Methodology") output_path = os.path.join(output_dir, "comparative_path_lengths.html") fig.write_html(output_path) print(f"Saved path length plot to {output_path}") -def plot_articutool_velocities(df: pd.DataFrame, output_dir: str): - """Generates violin plots of required Articutool velocities.""" - print("Generating Articutool velocity plots...") - +def plot_articutool_metrics(df: pd.DataFrame, output_dir: str): + """Generates violin plots for Articutool angles and velocities.""" + print("Generating Articutool performance plots (angles and velocities)...") success_df = df[df["status"] == "Success"].copy() - if success_df.empty: - print("Warning: No successful trials found. Skipping Articutool velocity plot.") + print("Warning: No successful trials found. Skipping Articutool plots.") return - # Explode the waypoints data - waypoints_data = [] - for _, row in success_df.iterrows(): - if isinstance(row.get("trajectory_metrics_waypoints_data"), list): - for waypoint in row["trajectory_metrics_waypoints_data"]: - if waypoint and isinstance( - waypoint.get("articutool_velocities_rad_per_sec"), dict - ): - waypoints_data.append( - { - "planning_mode": row["planning_mode"], - "pitch_vel": waypoint[ - "articutool_velocities_rad_per_sec" - ].get("pitch_vel"), - "roll_vel": waypoint[ - "articutool_velocities_rad_per_sec" - ].get("roll_vel"), - } - ) - - if not waypoints_data: - print("Warning: No valid Articutool velocity data found. Skipping plot.") + wp_df = _extract_waypoint_data(success_df) + if wp_df.empty: + print("Warning: No valid waypoint data found. Skipping Articutool plots.") return - vel_df = pd.DataFrame(waypoints_data).dropna() - + # Create a 2x2 subplot fig = make_subplots( - rows=1, cols=2, subplot_titles=("Pitch Velocity", "Roll Velocity") + rows=2, + cols=2, + subplot_titles=( + "Required Pitch Angle", + "Required Roll Angle", + "Required Pitch Velocity", + "Required Roll Velocity", + ), + vertical_spacing=0.15, ) - for i, vel_type in enumerate(["pitch_vel", "roll_vel"]): + metrics_to_plot = { + (1, 1): ("pitch_angle", "Angle (rad)"), + (1, 2): ("roll_angle", "Angle (rad)"), + (2, 1): ("pitch_vel", "Velocity (rad/s)"), + (2, 2): ("roll_vel", "Velocity (rad/s)"), + } + + for (row, col), (metric, y_title) in metrics_to_plot.items(): sub_fig = px.violin( - vel_df, + wp_df, x="planning_mode", - y=vel_type, + y=metric, color="planning_mode", - category_orders={ - "planning_mode": [ - "joint_unconstrained", - "task_pos_unconstrained", - "task_pos_constrained", - "task_pos_yaw_constrained", - ] - }, + category_orders={"planning_mode": PLANNING_MODE_ORDER}, ) for trace in sub_fig.data: - fig.add_trace(trace, row=1, col=i + 1) + fig.add_trace(trace, row=row, col=col) + fig.update_yaxes(title_text=y_title, row=row, col=col) + fig.update_xaxes( + title_text="Planning Methodology" if row == 2 else "", + showticklabels=True if row == 2 else False, + row=row, + col=col, + ) fig.update_layout( - title_text="Required Articutool Velocities During Successful Trajectories", + height=800, + title_text="Articutool Performance Metrics During Successful Trajectories", showlegend=False, ) - fig.update_yaxes(title_text="Velocity (rad/s)", row=1, col=1) - fig.update_yaxes(title_text="Velocity (rad/s)", row=1, col=2) - fig.update_xaxes(title_text="Planning Methodology", row=1, col=1) - fig.update_xaxes(title_text="Planning Methodology", row=1, col=2) - - output_path = os.path.join(output_dir, "comparative_articutool_velocities.html") + output_path = os.path.join(output_dir, "comparative_articutool_performance.html") fig.write_html(output_path) - print(f"Saved Articutool velocity plot to {output_path}") + print(f"Saved Articutool performance plot to {output_path}") def plot_manifold_adherence(df: pd.DataFrame, output_dir: str): - """ - Calculates and plots the 'Manifold Adherence' for each successful trajectory. - Adherence is the percentage of waypoints within the preferred joint ranges - for joints 2 and 3, as discovered during manifold exploration. - """ + """Calculates and plots the 'Manifold Adherence' for each successful trajectory.""" print("Generating manifold adherence plot...") - success_df = df[df["status"] == "Success"].copy() - if success_df.empty: print("Warning: No successful trials found. Skipping manifold adherence plot.") return - adherence_scores = [] - for _, row in success_df.iterrows(): - waypoints = row.get("trajectory_metrics_waypoints_data") - if not isinstance(waypoints, list) or not waypoints: - continue - - adherent_waypoints = 0 - for wp in waypoints: - pos = wp.get("jaco_positions_rad") - if pos and len(pos) >= 3: - # FIX: Correctly check the preferred ranges based on joint distribution analysis. - # Joint 2 (pos[1]) preferred negative values. - # Joint 3 (pos[2]) preferred positive values. - if pos[1] < 0 and pos[2] > 0: - adherent_waypoints += 1 - - adherence_scores.append( - { - "planning_mode": row["planning_mode"], - "adherence_score": (adherent_waypoints / len(waypoints)) * 100, - } + wp_df = _extract_waypoint_data(success_df) + if wp_df.empty: + print( + "Warning: No valid waypoint data found. Skipping manifold adherence plot." ) - - if not adherence_scores: - print("Warning: Could not calculate any adherence scores. Skipping plot.") return - adherence_df = pd.DataFrame(adherence_scores) + # Calculate adherence for each waypoint + wp_df["is_adherent"] = wp_df.apply( + lambda row: PREFERRED_JOINT_2_RANGE[0] + <= row["jaco_positions"][1] + <= PREFERRED_JOINT_2_RANGE[1] + and PREFERRED_JOINT_3_RANGE[0] + <= row["jaco_positions"][2] + <= PREFERRED_JOINT_3_RANGE[1], + axis=1, + ) + + # Group by trajectory and calculate percentage + adherence_df = ( + wp_df.groupby(["planning_mode", "task_id"])["is_adherent"].mean().reset_index() + ) + adherence_df["adherence_score"] = adherence_df["is_adherent"] * 100 fig = px.box( adherence_df, x="planning_mode", y="adherence_score", color="planning_mode", - title="Manifold Adherence of Successful Trajectories", - labels={ - "planning_mode": "Planning Methodology", - "adherence_score": "Adherence Score (%)", - }, - category_orders={ - "planning_mode": [ - "joint_unconstrained", - "task_pos_unconstrained", - "task_pos_constrained", - "task_pos_yaw_constrained", - ] - }, + title="Manifold Adherence of Successful Trajectories
Percentage of waypoints within preferred ranges for Joints 2 & 3", + labels={"adherence_score": "Adherence Score (%)"}, + category_orders={"planning_mode": PLANNING_MODE_ORDER}, ) fig.update_yaxes(range=[-5, 105]) - + fig.update_layout(xaxis_title="Planning Methodology") output_path = os.path.join(output_dir, "comparative_manifold_adherence.html") fig.write_html(output_path) print(f"Saved manifold adherence plot to {output_path}") +def plot_motion_smoothness(df: pd.DataFrame, output_dir: str): + """Calculates and plots end-effector jerk as a measure of motion smoothness.""" + print("Generating motion smoothness (jerk) plot...") + success_df = df[df["status"] == "Success"].copy() + if success_df.empty: + print("Warning: No successful trials found. Skipping smoothness plot.") + return + + wp_df = _extract_waypoint_data(success_df) + if wp_df.empty: + print("Warning: No valid EE position data found. Skipping smoothness plot.") + return + + wp_df = wp_df.sort_values(by=["planning_mode", "task_id", "time"]) + + # Calculate derivatives (velocity, acceleration, jerk) in a more robust way + # to avoid the multi-column assignment error with groupby. + + # Group by individual trajectory + grouped = wp_df.groupby(["planning_mode", "task_id"]) + + # Calculate delta_t once for efficiency + delta_t = grouped["time"].diff() + + # Position columns + pos_cols = ["vx", "vy", "vz"] + wp_df[pos_cols] = pd.DataFrame(wp_df["ee_position"].tolist(), index=wp_df.index) + + # Velocity + vel_cols = ["vel_x", "vel_y", "vel_z"] + velocity = grouped[pos_cols].diff().div(delta_t, axis=0) + velocity.columns = vel_cols # Rename columns to the target names + wp_df[vel_cols] = velocity + + # Acceleration + acc_cols = ["acc_x", "acc_y", "acc_z"] + acceleration = grouped[vel_cols].diff().div(delta_t, axis=0) + acceleration.columns = acc_cols + wp_df[acc_cols] = acceleration + + # Jerk + jerk_cols = ["jerk_x", "jerk_y", "jerk_z"] + jerk = grouped[acc_cols].diff().div(delta_t, axis=0) + jerk.columns = jerk_cols + wp_df[jerk_cols] = jerk + + wp_df["jerk_magnitude"] = np.linalg.norm(wp_df[jerk_cols].fillna(0), axis=1) + + fig = px.box( + wp_df, + x="planning_mode", + y="jerk_magnitude", + color="planning_mode", + title="Motion Smoothness: End-Effector Jerk", + labels={"jerk_magnitude": "Jerk Magnitude (m/s³)"}, + category_orders={"planning_mode": PLANNING_MODE_ORDER}, + ) + # Use a logarithmic scale for jerk as it can have a very large range, + # and clip the view to a reasonable upper quantile to hide extreme outliers. + fig.update_yaxes(type="log") + fig.update_layout(xaxis_title="Planning Methodology") + output_path = os.path.join(output_dir, "comparative_motion_smoothness.html") + fig.write_html(output_path) + print(f"Saved motion smoothness plot to {output_path}") + + +def plot_workspace_outcomes(df: pd.DataFrame, output_dir: str): + """Generates a 3D scatter plot of where tasks succeed and fail.""" + print("Generating workspace outcomes plot...") + + task_space_df = df[df["target_position"].notna()].copy() + if task_space_df.empty: + print( + "Warning: No trials with target positions found. Skipping workspace plot." + ) + return + + task_space_df[["target_x", "target_y", "target_z"]] = pd.DataFrame( + task_space_df["target_position"].tolist(), index=task_space_df.index + ) + + # --- FIX STARTS HERE --- + # Manually create subplots because facet_col is not supported for 3D scatter plots + # in all versions of Plotly. + + # We only care about task-space modes for this plot + task_modes = [m for m in PLANNING_MODE_ORDER if "task_pos" in m] + + fig = make_subplots( + rows=1, + cols=len(task_modes), + specs=[[{"type": "scene"}] * len(task_modes)], + subplot_titles=task_modes, + ) + + # Keep track of which legend items have been added to avoid duplicates + legend_added = set() + + for i, mode in enumerate(task_modes): + mode_df = task_space_df[task_space_df["planning_mode"] == mode] + + for status in STATUS_ORDER: + status_df = mode_df[mode_df["status"] == status] + if status_df.empty: + continue + + show_legend_for_trace = status not in legend_added + legend_added.add(status) + + fig.add_trace( + go.Scatter3d( + x=status_df["target_x"], + y=status_df["target_y"], + z=status_df["target_z"], + mode="markers", + marker=dict( + color=STATUS_COLOR_MAP.get(status, "grey"), + size=3, + opacity=0.7, + ), + name=status, + legendgroup=status, + showlegend=show_legend_for_trace, + ), + row=1, + col=i + 1, + ) + + fig.update_layout( + height=600, + title_text="Success and Failure Locations in the Workspace", + legend_title_text="Trial Outcome", + ) + # --- FIX ENDS HERE --- + + output_path = os.path.join(output_dir, "comparative_workspace_outcomes.html") + fig.write_html(output_path) + print(f"Saved workspace outcomes plot to {output_path}") + + def main(): parser = argparse.ArgumentParser( description="Analyze results from the holistic benchmark." ) - parser.add_argument( - "json_files", - nargs="+", - help="One or more JSON result files to analyze (e.g., 'output/*.json').", - ) + parser.add_argument("json_files", nargs="+", help="One or more JSON result files.") parser.add_argument( "--output_dir", type=str, @@ -354,11 +467,15 @@ def main(): df = load_data(args.json_files) + # --- Generate Analysis --- + print_summary_table(df) plot_outcomes(df, args.output_dir) plot_planning_times(df, args.output_dir) plot_path_lengths(df, args.output_dir) - plot_articutool_velocities(df, args.output_dir) + plot_articutool_metrics(df, args.output_dir) plot_manifold_adherence(df, args.output_dir) + plot_motion_smoothness(df, args.output_dir) + plot_workspace_outcomes(df, args.output_dir) print("\nAnalysis complete.") diff --git a/ada_feeding/scripts/visualize_trajectory.py b/ada_feeding/scripts/visualize_trajectory.py index 9f44999d..1938561b 100755 --- a/ada_feeding/scripts/visualize_trajectory.py +++ b/ada_feeding/scripts/visualize_trajectory.py @@ -3,22 +3,13 @@ # License: BSD 3-Clause. See LICENSE.md file in root directory. """ -This script is used to visualize robot trajectories or single joint states -using Pinocchio and MeshCat. It can load: -1. Enhanced trajectory JSON files (containing Jaco and Articutool waypoints). -2. Single joint state JSON files (representing a sensor_msgs/msg/JointState). - -New features for figure generation: ---snapshot-frames: Enters a mode to navigate only specific keyframes. ---rigid: Sets the initial state of the Articutool's rigidity. -'t' command: Toggles Articutool rigidity on/off interactively. -'s' command: Saves a labeled screenshot of the current view. - -New features for video recording: -'r' command: Starts and stops recording the current view. The user can move - the camera in MeshCat while recording. -'a' command: Now prompts to record the animation to a file. ---fps: Sets the frames per second for the output video. +This script interactively visualizes trajectories from holistic_benchmark.py +output files using Pinocchio and MeshCat. + +It allows a user to load multiple benchmark result files, choose a specific +planning mode, and then select a successful trajectory from that mode to +inspect frame-by-frame or animate. The script runs in a persistent session, +allowing multiple trajectories to be viewed without relaunching. """ # Standard imports @@ -35,8 +26,9 @@ import sys import math from typing import Optional, Dict, Any, List +import pandas as pd -# --- MODIFICATION: Added Pillow for image saving and imageio for video --- +# Third-party imports for saving images/videos # Note: You may need to install imageio and a backend: # pip install imageio imageio-ffmpeg from PIL import Image @@ -53,8 +45,6 @@ def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str if not os.path.exists(xacro_filename): logger_func(f"Error: Xacro file not found at {xacro_filename}") return None - - logger_func(f"Processing Xacro file: {xacro_filename}") try: process = subprocess.run( ["ros2", "run", "xacro", "xacro", xacro_filename], @@ -62,136 +52,119 @@ def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str capture_output=True, text=True, ) - urdf_xml_string = process.stdout - logger_func("XACRO processing successful.") - return urdf_xml_string - except FileNotFoundError: - logger_func( - "Fatal: Command 'ros2' not found. Make sure ROS 2 environment is sourced." - ) - return None - except subprocess.CalledProcessError as e: - logger_func( - f"Fatal: XACRO processing command failed with exit code {e.returncode}." - ) - logger_func(f"XACRO stdout:\n{e.stdout}") - logger_func(f"XACRO stderr:\n{e.stderr}") - return None + return process.stdout except Exception as e: logger_func(f"An unexpected error occurred during XACRO processing: {e}") return None -def load_pinocchio_model_from_urdf_string(urdf_xml_string: str, logger_func=print): +def find_ros_workspace_root(start_path: str) -> Optional[str]: + """Searches upwards from a starting path for a directory containing 'src'.""" + current_path = os.path.abspath(start_path) + while True: + if os.path.isdir(os.path.join(current_path, "src")): + return current_path + parent_path = os.path.dirname(current_path) + if parent_path == current_path: # Reached the filesystem root + return None + current_path = parent_path + + +def load_pinocchio_model_from_urdf_string( + urdf_xml_string: str, xacro_file_path: str, logger_func=print +): """Loads a Pinocchio model from a URDF XML string via a temporary file.""" temp_urdf_path = "" + package_dirs = [] try: with tempfile.NamedTemporaryFile( mode="w", suffix=".urdf", delete=False ) as temp_file: temp_urdf_path = temp_file.name temp_file.write(urdf_xml_string) - logger_func(f"Generated temporary URDF file: {temp_urdf_path}") - package_dirs = [] + # Robustly find package directories for mesh files ros_package_path = os.environ.get("ROS_PACKAGE_PATH") if ros_package_path: - package_dirs = [ - p for p in ros_package_path.split(os.pathsep) if os.path.isdir(p) - ] - - common_workspace_paths = [ - os.path.expanduser("~/ros2_ws/src"), - os.path.expanduser("~/dev_ws/src"), - os.path.expanduser("~/workspace/src"), - ] - for wsp in common_workspace_paths: - if os.path.isdir(wsp) and wsp not in package_dirs: - package_dirs.append(wsp) - - current_dir = os.getcwd() - while current_dir != os.path.dirname(current_dir): - if os.path.basename(current_dir) in ["install", "build", "log"]: - ws_root = os.path.dirname(current_dir) - src_path = os.path.join(ws_root, "src") - if os.path.isdir(src_path) and src_path not in package_dirs: - package_dirs.append(src_path) - break - current_dir = os.path.dirname(current_dir) - - if package_dirs: - unique_package_dirs = list(set(package_dirs)) - logger_func(f"Using package_dirs for Pinocchio: {unique_package_dirs}") - model = pin.buildModelFromUrdf( - temp_urdf_path, package_dirs=unique_package_dirs - ) - collision_model = pin.buildGeomFromUrdf( - model, - temp_urdf_path, - pin.GeometryType.COLLISION, - package_dirs=unique_package_dirs, - ) - visual_model = pin.buildGeomFromUrdf( - model, - temp_urdf_path, - pin.GeometryType.VISUAL, - package_dirs=unique_package_dirs, - ) - else: - logger_func( - "Warning: ROS_PACKAGE_PATH not found or empty. Mesh loading might fail." - ) - model = pin.buildModelFromUrdf(temp_urdf_path) - collision_model = pin.buildGeomFromUrdf( - model, temp_urdf_path, pin.GeometryType.COLLISION - ) - visual_model = pin.buildGeomFromUrdf( - model, temp_urdf_path, pin.GeometryType.VISUAL + package_dirs.extend( + [p for p in ros_package_path.split(os.pathsep) if os.path.isdir(p)] ) - data = model.createData() - logger_func( - f"Pinocchio model loaded. Name: {model.name}, Nq: {model.nq}, Nv: {model.nv}, NJoints: {model.njoints}" + ws_root = find_ros_workspace_root(os.path.dirname(xacro_file_path)) + if ws_root: + logger_func(f"Found potential workspace root at: {ws_root}") + src_dir = os.path.join(ws_root, "src") + if os.path.isdir(src_dir) and src_dir not in package_dirs: + package_dirs.append(src_dir) + install_dir = os.path.join(ws_root, "install") + if os.path.isdir(install_dir) and install_dir not in package_dirs: + package_dirs.append(install_dir) + + package_dirs = list(dict.fromkeys(package_dirs)) + logger_func(f"Using package search paths for Pinocchio: {package_dirs}") + + # Load the kinematic model from the temporary URDF file path. + model = pin.buildModelFromUrdf(temp_urdf_path) + + # Load the geometry (visual/collision meshes) using the package search paths. + visual_model = pin.buildGeomFromUrdf( + model, temp_urdf_path, pin.GeometryType.VISUAL, package_dirs=package_dirs + ) + collision_model = pin.buildGeomFromUrdf( + model, temp_urdf_path, pin.GeometryType.COLLISION, package_dirs=package_dirs ) + + data = model.createData() + logger_func(f"Pinocchio model loaded. Nq: {model.nq}, Nv: {model.nv}") return model, collision_model, visual_model, data except Exception as e: - logger_func(f"Error loading Pinocchio model: {e}") + logger_func("\n" + "=" * 80) + logger_func("FATAL: Error loading Pinocchio model.") + logger_func( + "This often happens if mesh files (.stl, .dae) referenced in the URDF cannot be found." + ) + logger_func( + "Please ensure your ROS 2 workspace is sourced correctly, so that `package://` paths can be resolved." + ) + logger_func(f"Pinocchio search paths used: {package_dirs}") + logger_func(f"Original Pinocchio error: {e}") + logger_func("=" * 80 + "\n") return None, None, None, None finally: if temp_urdf_path and os.path.exists(temp_urdf_path): os.remove(temp_urdf_path) -def load_json_file(filepath: str, logger_func=print) -> Optional[Dict[str, Any]]: - """Loads data from a generic JSON file.""" - try: - with open(filepath, "r") as f: - data = json.load(f) - logger_func(f"Successfully loaded JSON from {filepath}") - return data - except FileNotFoundError: - logger_func(f"Error: File not found at {filepath}") - return None - except json.JSONDecodeError: - logger_func(f"Error: Could not decode JSON from {filepath}") - return None - return None +def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFrame: + """Loads and concatenates data from multiple benchmark JSON files.""" + all_data = [] + for file_path in file_paths: + try: + with open(file_path, "r") as f: + data = json.load(f) + all_data.extend(data) + except Exception as e: + logger_func(f"Warning: Could not load or parse {file_path}. Error: {e}") + if not all_data: + return pd.DataFrame() + logger_func( + f"Successfully loaded {len(all_data)} total trials from {len(file_paths)} file(s)." + ) + return pd.json_normalize(all_data, sep="_") def get_pinocchio_joint_info( model: pin.Model, joint_name: str ) -> Optional[Dict[str, Any]]: - """Gets q_idx_start, nq, and nv for a named joint in the Pinocchio model.""" + """Gets key info for a named joint in the Pinocchio model.""" if model.existJointName(joint_name): joint_id = model.getJointId(joint_name) joint_obj = model.joints[joint_id] - if joint_obj.idx_q >= 0 and joint_obj.nq > 0: + if joint_obj.idx_q >= 0: return { - "name": joint_name, "q_idx_start": joint_obj.idx_q, "nq": joint_obj.nq, "nv": joint_obj.nv, - "id": joint_id, } return None @@ -205,319 +178,289 @@ def set_q_from_waypoint( atool_roll_info, is_rigid: bool, ): - """Updates Pinocchio q_vector based on a waypoint from the trajectory data.""" + """Updates Pinocchio q_vector based on a waypoint from the benchmark data.""" + # Set Jaco arm joints jaco_positions = waypoint.get("jaco_positions_rad", []) for k, mapping_info in enumerate(jaco_mappings): if mapping_info and k < len(jaco_positions): theta_traj = jaco_positions[k] - q_idx_start, nq, nv = ( - mapping_info["q_idx_start"], - mapping_info["nq"], - mapping_info["nv"], - ) + q_idx_start, nq = mapping_info["q_idx_start"], mapping_info["nq"] if nq == 1: q_vector[q_idx_start] = theta_traj - elif nq == 2 and nv == 1: + elif nq == 2: # Revolute joint with quaternion representation q_vector[q_idx_start], q_vector[q_idx_start + 1] = ( np.cos(theta_traj), np.sin(theta_traj), ) - is_feasible = waypoint.get("articutool_waypoint_feasible", False) + # Set Articutool joints + articutool_solution = waypoint.get("articutool_solution_rad") + is_feasible = isinstance(articutool_solution, dict) + if is_rigid or not is_feasible: pitch_sol, roll_sol = 0.0, 0.0 else: - pitch_sol = waypoint.get("articutool_pitch_solution_rad", 0.0) - roll_sol = waypoint.get("articutool_roll_solution_rad", 0.0) + pitch_sol = articutool_solution.get("pitch", 0.0) + roll_sol = articutool_solution.get("roll", 0.0) for sol, info in [(pitch_sol, atool_pitch_info), (roll_sol, atool_roll_info)]: if sol is not None and info: - q_idx, nq, nv = info["q_idx_start"], info["nq"], info["nv"] + q_idx, nq = info["q_idx_start"], info["nq"] if nq == 1: q_vector[q_idx] = sol - elif nq == 2 and nv == 1: + elif nq == 2: q_vector[q_idx], q_vector[q_idx + 1] = np.cos(sol), np.sin(sol) +def select_trajectory(df: pd.DataFrame) -> Optional[Dict[str, Any]]: + """Interactively prompts the user to select a mode and trajectory.""" + # --- Step 1: Select Planning Mode --- + modes = df["planning_mode"].unique() + print("\n--- Please Select a Planning Mode ---") + for i, mode in enumerate(modes): + print(f" [{i + 1}] {mode}") + print(" [q] Quit") + + try: + choice_str = input(f"Enter choice (1-{len(modes)} or q): ").strip().lower() + if choice_str == "q": + return None + mode_choice = int(choice_str) - 1 + if not 0 <= mode_choice < len(modes): + raise ValueError + selected_mode = modes[mode_choice] + except (ValueError, IndexError): + print("Invalid choice.") + return "retry" # Special value to retry selection + + # --- Step 2: Select Trajectory from Successful Trials --- + mode_df = df[(df["planning_mode"] == selected_mode) & (df["status"] == "Success")] + if mode_df.empty: + print(f"No successful trials found for mode: {selected_mode}") + return "retry" + + trajectories = mode_df.sort_values("task_id").to_dict("records") + print(f"\n--- Select a Successful Trajectory from '{selected_mode}' ---") + for i, trial in enumerate(trajectories): + print( + f" [{i + 1}] Task ID: {trial['task_id']} (Planning Time: {trial['planning_time_s']:.2f}s)" + ) + print(" [m] Back to Mode Selection") + + try: + choice_str = ( + input(f"Enter choice (1-{len(trajectories)} or m): ").strip().lower() + ) + if choice_str == "m": + return "retry" + traj_choice = int(choice_str) - 1 + if not 0 <= traj_choice < len(trajectories): + raise ValueError + return trajectories[traj_choice] + except (ValueError, IndexError): + print("Invalid choice.") + return "retry" + + +def visualization_loop( + pin_viz, model, waypoints, jaco_mappings, atool_pitch_info, atool_roll_info, args +): + """Runs the interactive UI for a single selected trajectory.""" + q = pin.neutral(model) + is_rigid_mode = args.rigid + current_idx = 0 + + while True: + waypoint_info = waypoints[current_idx] + set_q_from_waypoint( + q, + model, + waypoint_info, + jaco_mappings, + atool_pitch_info, + atool_roll_info, + is_rigid_mode, + ) + pin_viz.display(q) + + is_feasible_in_data = isinstance( + waypoint_info.get("articutool_solution_rad"), dict + ) + rigidity_status = ( + "ON (Rigid)" + if is_rigid_mode + else f"OFF (Data says feasible: {is_feasible_in_data})" + ) + + print(f"\nDisplaying Frame: {current_idx + 1}/{len(waypoints)}") + print(f"Articutool Rigidity: {rigidity_status}") + print( + "Commands: [n]ext, [p]rev, [f]irst, [l]ast, [t]oggle, [a]nimate, [s]creenshot, [m]enu, [q]uit" + ) + + user_input = input("Enter command: ").strip().lower() + + if user_input == "q": + return "quit" + if user_input == "m": + return "menu" + + if user_input == "n": + current_idx = min(current_idx + 1, len(waypoints) - 1) + elif user_input == "p": + current_idx = max(current_idx - 1, 0) + elif user_input == "f": + current_idx = 0 + elif user_input == "l": + current_idx = len(waypoints) - 1 + elif user_input == "t": + is_rigid_mode = not is_rigid_mode + print(f"Articutool rigidity toggled to: {'ON' if is_rigid_mode else 'OFF'}") + elif user_input == "s": + print("Taking screenshot...") + img = visualizer.get_image() + rigidity_str = "rigid" if is_rigid_mode else "active" + filepath = os.path.join( + args.output_dir, f"frame_{current_idx + 1}_{rigidity_str}.png" + ) + img.save(filepath) + print(f"Screenshot saved to {filepath}") + elif user_input == "a": + print("Animating... Press Ctrl+C to stop.") + try: + for i in range(current_idx, len(waypoints)): + set_q_from_waypoint( + q, + model, + waypoints[i], + jaco_mappings, + atool_pitch_info, + atool_roll_info, + is_rigid_mode, + ) + pin_viz.display(q) + print(f" Displaying frame {i + 1}/{len(waypoints)}", end="\r") + time.sleep(1.0 / args.fps) + current_idx = len(waypoints) - 1 + print("\nAnimation finished.") + except KeyboardInterrupt: + print("\nAnimation stopped.") + elif user_input: + print("Invalid command.") + + def main(args): - print("--- Pinocchio + MeshCat Visualizer ---") + print("--- Interactive Benchmark Trajectory Visualizer ---") + + # --- Load Data and Models (One-time setup) --- + df = load_benchmark_data(args.benchmark_files) + if df.empty: + print("No data could be loaded. Exiting.") + return urdf_string = xacro_to_urdf_string(args.xacro_file) if not urdf_string: return model, collision_model, visual_model, data = load_pinocchio_model_from_urdf_string( - urdf_string + urdf_string, args.xacro_file ) if not model: return - print("Initializing MeshCat viewer...") + # --- Initialize Visualizer (One-time setup) --- try: + global visualizer visualizer = meshcat.Visualizer().open() - print(f"MeshCat viewer URL: {visualizer.url()}") pin_viz = pin.visualize.MeshcatVisualizer(model, collision_model, visual_model) pin_viz.initViewer(viewer=visualizer) pin_viz.loadViewerModel(rootNodeName=model.name or "robot") - print("MeshCat viewer initialized.") + print(f"MeshCat viewer URL: {visualizer.url()}") except Exception as e: print(f"Error initializing MeshCat: {e}") return - if not args.trajectory_file: - print("No trajectory file provided. Displaying neutral pose.") - pin_viz.display(pin.neutral(model)) - input("Press Enter to quit.") - return - - trajectory_data = load_json_file(args.trajectory_file) - if ( - not trajectory_data - or "jaco_joint_names" not in trajectory_data - or "waypoints" not in trajectory_data - ): - print("Failed to load valid trajectory data. Exiting.") - return - - waypoints = trajectory_data["waypoints"] - if not waypoints: - print("Trajectory contains no waypoints. Exiting.") - return - - if args.output_dir: - os.makedirs(args.output_dir, exist_ok=True) - print(f"Screenshots and videos will be saved to: {args.output_dir}") + # --- Main Application Loop --- + while True: + selected_trial = select_trajectory(df) - jaco_joint_names = trajectory_data["jaco_joint_names"] - jaco_mappings = [get_pinocchio_joint_info(model, name) for name in jaco_joint_names] - articutool_pitch_info = get_pinocchio_joint_info(model, ARTICUTOOL_PITCH_JOINT_NAME) - articutool_roll_info = get_pinocchio_joint_info(model, ARTICUTOOL_ROLL_JOINT_NAME) + if selected_trial is None: # User chose to quit + break + if selected_trial == "retry": # User made an invalid choice or wants to go back + continue - if not all(jaco_mappings) or not articutool_pitch_info or not articutool_roll_info: - print("Error: Could not map all required joints to Pinocchio model.") - return + waypoints = selected_trial.get("trajectory_metrics_waypoints_data", []) + if not waypoints: + print("Selected trial contains no waypoints. Returning to menu.") + continue - q = pin.neutral(model) + if args.output_dir: + os.makedirs(args.output_dir, exist_ok=True) + print(f"Screenshots and videos will be saved to: {args.output_dir}") - is_rigid_mode = args.rigid - navigable_indices = [] - if args.snapshot_frames: - navigable_indices = [ - int(f.strip()) - 1 for f in args.snapshot_frames.split(",") + jaco_joint_names = [f"{args.prefix}_joint_{i + 1}" for i in range(6)] + jaco_mappings = [ + get_pinocchio_joint_info(model, name) for name in jaco_joint_names ] - print(f"\n--- Snapshot Navigation Mode ---") - print( - f"Navigating {len(navigable_indices)} keyframes: {[f + 1 for f in navigable_indices]}" + articutool_pitch_info = get_pinocchio_joint_info( + model, ARTICUTOOL_PITCH_JOINT_NAME + ) + articutool_roll_info = get_pinocchio_joint_info( + model, ARTICUTOOL_ROLL_JOINT_NAME ) - else: - navigable_indices = list(range(len(waypoints))) - print(f"\n--- Full Trajectory Navigation Mode ---") - - current_nav_idx = 0 - - is_recording = False - video_writer = None - current_recording_filepath = None - try: - while True: - if not navigable_indices: - print("No frames to navigate.") - break - - current_point_idx = navigable_indices[current_nav_idx] - waypoint_info = waypoints[current_point_idx] - - set_q_from_waypoint( - q, - model, - waypoint_info, - jaco_mappings, - articutool_pitch_info, - articutool_roll_info, - is_rigid_mode, - ) - pin_viz.display(q) + if ( + not all(jaco_mappings) + or not articutool_pitch_info + or not articutool_roll_info + ): + print("Error: Could not map all required joints to Pinocchio model.") + for name, mapping in zip(jaco_joint_names, jaco_mappings): + if not mapping: + print(f" - FAILED to find joint '{name}' in the Pinocchio model.") + continue + + # Enter the visualization UI for the selected trajectory + action = visualization_loop( + pin_viz, + model, + waypoints, + jaco_mappings, + articutool_pitch_info, + articutool_roll_info, + args, + ) - is_atool_active_in_file = waypoint_info.get( - "articutool_waypoint_feasible", False - ) - rigidity_status = ( - "ON (Rigid)" - if is_rigid_mode - else f"OFF (Active: {is_atool_active_in_file})" - ) + if action == "quit": + break - print( - f"\nDisplaying Frame: {current_point_idx + 1}/{len(waypoints)} (Nav index: {current_nav_idx + 1}/{len(navigable_indices)})" - ) - print(f"Articutool Rigidity: {rigidity_status}") - print( - "Commands: [n]ext, [p]rev, [f]irst, [l]ast, [t]oggle rigidity, [a]nimate, [s]creenshot, [r]ecord, [q]uit" - ) - - user_input = input("Enter command: ").strip().lower() - - if user_input == "q": - break - elif user_input == "s": - print("Taking screenshot...") - img = visualizer.get_image() - rigidity_str = "rigid" if is_rigid_mode else "active" - filename = f"frame_{current_point_idx + 1}_{rigidity_str}.png" - filepath = os.path.join(args.output_dir, filename) - img.save(filepath) - print(f"Screenshot saved to {filepath}") - continue - elif user_input == "r": - if not is_recording: - is_recording = True - rigidity_str = "rigid" if is_rigid_mode else "active" - filename = ( - f"recording_frame_{current_point_idx + 1}_{rigidity_str}.mp4" - ) - current_recording_filepath = os.path.join(args.output_dir, filename) - print( - f"Starting recording... Output file: {current_recording_filepath}" - ) - # --- MODIFICATION: Explicitly set the format to 'FFMPEG' to avoid using the wrong writer --- - video_writer = imageio.get_writer( - current_recording_filepath, fps=args.fps, format="FFMPEG" - ) - - print( - "Recording... Move the camera in MeshCat. Press Ctrl+C to stop." - ) - try: - while True: - img = visualizer.get_image() - video_writer.append_data(np.array(img)) - time.sleep(1.0 / args.fps) - except KeyboardInterrupt: - print("\nStopping manual recording.") - else: - pass - - if video_writer: - video_writer.close() - print(f"Video saved to {current_recording_filepath}") - video_writer = None - current_recording_filepath = None - is_recording = False - continue - - elif user_input == "t": - is_rigid_mode = not is_rigid_mode - print( - f"Articutool rigidity toggled to: {'ON' if is_rigid_mode else 'OFF'}" - ) - continue - elif user_input == "n": - current_nav_idx = min(current_nav_idx + 1, len(navigable_indices) - 1) - elif user_input == "p": - current_nav_idx = max(current_nav_idx - 1, 0) - elif user_input == "f": - current_nav_idx = 0 - elif user_input == "l": - current_nav_idx = len(navigable_indices) - 1 - elif user_input == "a": - record_anim = ( - input("Record this animation? (y/n): ").lower().strip() == "y" - ) - anim_writer = None - filepath = "" - if record_anim: - rigidity_str = "rigid" if is_rigid_mode else "active" - filename = f"animation_{rigidity_str}.mp4" - filepath = os.path.join(args.output_dir, filename) - # --- MODIFICATION: Explicitly set the format to 'FFMPEG' to avoid using the wrong writer --- - anim_writer = imageio.get_writer( - filepath, fps=args.fps, format="FFMPEG" - ) - print(f"Recording animation to {filepath}...") - - print("Animating... Press Ctrl+C to stop.") - anim_q = q.copy() - start_anim_nav_idx = current_nav_idx - try: - for i in range(start_anim_nav_idx, len(navigable_indices)): - anim_point_idx = navigable_indices[i] - set_q_from_waypoint( - anim_q, - model, - waypoints[anim_point_idx], - jaco_mappings, - articutool_pitch_info, - articutool_roll_info, - is_rigid_mode, - ) - pin_viz.display(anim_q) - - if anim_writer: - img = visualizer.get_image() - anim_writer.append_data(np.array(img)) - - print( - f" Displaying frame {anim_point_idx + 1}/{len(waypoints)}", - end="\r", - ) - time.sleep(1.0 / args.fps) - current_nav_idx = len(navigable_indices) - 1 - print("\nAnimation finished.") - except KeyboardInterrupt: - print("\nAnimation stopped.") - finally: - if anim_writer: - anim_writer.close() - print(f"Animation video saved to {filepath}") - - else: - if user_input: - print("Invalid command.") - continue - - except (EOFError, KeyboardInterrupt): - print("\nExiting...") - finally: - if video_writer: - video_writer.close() - if current_recording_filepath: - print(f"Final video saved to {current_recording_filepath}") - print("Visualizer finished.") + print("Visualizer finished.") if __name__ == "__main__": parser = argparse.ArgumentParser( - description="Visualize robot trajectories using Pinocchio and MeshCat." + description="Visualize robot trajectories from benchmark files." ) parser.add_argument("xacro_file", type=str, help="Path to the robot XACRO file.") parser.add_argument( - "--trajectory_file", - type=str, - help="Path to the .json trajectory file.", + "benchmark_files", nargs="+", help="One or more benchmark JSON result files." ) + # --- FIX: Changed default prefix to the correct one --- parser.add_argument( - "--rigid", - action="store_true", - help="Sets the initial state to be rigid. Can be toggled interactively.", - ) - parser.add_argument( - "--snapshot-frames", + "--prefix", type=str, - help="Comma-separated list of 1-based frame numbers to navigate (e.g., '1,25,50').", + default="j2n6s200", + help="The prefix for the Jaco arm's joint names (e.g., j2n6s200).", ) + parser.add_argument("--rigid", action="store_true", help="Start in rigid mode.") parser.add_argument( "--output-dir", type=str, - default="screenshots", - help="Directory to save screenshots and videos. Default: 'screenshots'", + default="visualization_output", + help="Directory to save screenshots and videos.", ) parser.add_argument( - "--fps", - type=int, - default=30, - help="Frames per second for video recording. Default: 30", + "--fps", type=int, default=30, help="Frames per second for animation." ) - args = parser.parse_args() main(args) From ebe9729ba4778e9e1be5a547c0673c2d68661f57 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 28 Jul 2025 18:12:28 -0700 Subject: [PATCH 151/238] Initial commit for end-to-end benchmark --- .../scripts/end_to_end_feeding_benchmark.py | 540 ++++++++++++++++++ 1 file changed, 540 insertions(+) create mode 100644 ada_feeding/scripts/end_to_end_feeding_benchmark.py diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py new file mode 100644 index 00000000..1a1a2945 --- /dev/null +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -0,0 +1,540 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script implements the End-to-End Assistive Feeding Benchmark. + +It evaluates a set of generalizable motion primitives across a complete, +simulated assistive feeding cycle. The benchmark operates within a variety of +procedurally generated, robot-centric planning scenes to test the +generalization and robustness of different control strategies. +""" + +# Standard imports +from collections import namedtuple +from datetime import datetime +import os +import time +from threading import Thread +from typing import Optional, List, Dict, Tuple, Any +import json +import math +import subprocess +import sys +import argparse +from enum import Enum + +# Third-party imports +import numpy as np +from pymoveit2 import MoveIt2 +from pymoveit2.robots import kinova +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory +from geometry_msgs.msg import Pose, Point, Quaternion +from scipy.spatial.transform import Rotation as R +import pinocchio as pin + +# --- Constants --- +LOGGER = rclpy.logging.get_logger("end_to_end_benchmark") +PLANNING_GROUP_JACO = "jaco_arm" +PLANNING_GROUP_ATOOL = "articutool" +PLANNING_GROUP_FULL = "jaco_arm_with_articutool" +JOINT_NAMES_JACO = [f"j2n6s200_joint_{i + 1}" for i in range(6)] +JOINT_NAMES_ATOOL = [f"atool_joint_{i + 1}" for i in range(2)] +JOINT_NAMES_FULL = JOINT_NAMES_JACO + JOINT_NAMES_ATOOL +BASE_LINK_JACO = "j2n6s200_link_base" +BASE_LINK_ATOOL = "atool_link_base" +BASE_LINK_FULL = BASE_LINK_JACO +END_EFFECTOR_LINK_JACO = "j2n6s200_end_effector" +END_EFFECTOR_LINK_ATOOL = "tool_tip" +END_EFFECTOR_LINK_FULL = END_EFFECTOR_LINK_ATOOL +EPSILON = 1e-6 +ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) +ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) +WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) + + +# --- Data Structures --- +class TrialStatus(Enum): + SUCCESS = "Success" + IK_FAILURE = "IK Failure" + PLANNER_FAILURE = "Planner Failure" + VERIFICATION_FAILURE = "Path Verification Failure" + SKIPPED = "Skipped" + + +class EndToEndBenchmark: + """Manages the end-to-end benchmark planning process.""" + + def __init__( + self, + node: Node, + moveit2_jaco: MoveIt2, + moveit2_atool: MoveIt2, + moveit2_full: MoveIt2, + xacro_file_path: str, + num_trials: int = 100, + planning_timeout: float = 5.0, + output_dir: Optional[str] = None, + ): + self.node = node + self.moveit2_jaco = moveit2_jaco + self.moveit2_atool = moveit2_atool + self.moveit2_full = moveit2_full + self.xacro_file_path = xacro_file_path + self.num_trials = num_trials + self.planning_timeout = planning_timeout + self.output_dir = output_dir + self.results: List[Dict[str, Any]] = [] + + # Pinocchio model for feasibility checks + self.pinocchio_model: Optional[pin.Model] = None + self.pinocchio_data: Optional[pin.Data] = None + self.jaco_ee_frame_id_pin: Optional[int] = None + self._initialize_pinocchio() + + if self.output_dir: + os.makedirs(self.output_dir, exist_ok=True) + + def _initialize_pinocchio(self): + """Loads the robot model from a XACRO file into Pinocchio.""" + try: + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", self.xacro_file_path], + check=True, + capture_output=True, + text=True, + ) + urdf_xml_string = process.stdout + self.pinocchio_model = pin.buildModelFromXML(urdf_xml_string) + self.pinocchio_data = self.pinocchio_model.createData() + self.jaco_ee_frame_id_pin = self.pinocchio_model.getFrameId( + END_EFFECTOR_LINK_FULL + ) + LOGGER.info("Pinocchio model loaded successfully.") + except Exception as e: + LOGGER.error(f"Failed to initialize Pinocchio model: {e}", exc_info=True) + self.pinocchio_model = None + + # --- Scene Generation --- + def _generate_scene(self, base_z_offset: float) -> Dict[str, Any]: + """Generates a randomized, robot-centric planning scene.""" + scene = {"base_z_offset": base_z_offset} + + # Sample food and mouth poses + scene["food_pose"] = self._sample_pose_in_spherical_shell() + scene["mouth_pose"] = self._sample_pose_in_spherical_shell() + + # Define derived poses + scene["home_config"] = [-1.47568, 2.92779, 1.00845, -2.0847, 1.43588, 1.32575] + scene["above_plate_pose"] = self._calculate_above_plate_pose(scene["food_pose"]) + scene["move_above_pose"] = self._calculate_move_above_pose(scene["food_pose"]) + scene["move_into_pose"] = self._calculate_move_into_pose( + scene["food_pose"], scene["move_above_pose"] + ) + scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) + scene["resting_pose"] = Pose(position=Point(x=0.4, y=-0.4, z=0.3)) + + return scene + + def _sample_pose_in_spherical_shell(self) -> Pose: + """Samples a random pose within a spherical shell in front of the robot.""" + inner_radius, outer_radius = 0.3, 0.7 + + # Sample position + r = np.random.uniform(inner_radius**3, outer_radius**3) ** (1 / 3) + theta = np.random.uniform(-np.pi / 2, np.pi / 2) # Front half-space + phi = np.random.uniform(0, np.pi) + x = r * np.cos(theta) * np.sin(phi) + y = r * np.sin(theta) * np.sin(phi) + z = r * np.cos(phi) + position = Point(x=x, y=y, z=z) + + # Enforce "look-at-robot" constraint for orientation + direction_vector = -np.array([x, y, z]) + direction_vector /= np.linalg.norm(direction_vector) + + # Create rotation that aligns a frame's X-axis with this vector + # With some variability + rand_axis = np.random.randn(3) + rand_axis /= np.linalg.norm(rand_axis) + rand_angle = np.random.uniform(-np.deg2rad(30), np.deg2rad(30)) + variability_rot = R.from_rotvec(rand_angle * rand_axis) + + # Main rotation to look at origin + up_vector = np.array([0, 0, 1]) + x_axis = direction_vector + y_axis = np.cross(up_vector, x_axis) + y_axis /= np.linalg.norm(y_axis) + z_axis = np.cross(x_axis, y_axis) + + rotation_matrix = np.array([x_axis, y_axis, z_axis]).T + main_rot = R.from_matrix(rotation_matrix) + + final_rot = main_rot * variability_rot + quat = final_rot.as_quat() + + return Pose( + position=position, + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), + ) + + def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: + """Calculate a camera pose that looks down at the food.""" + # For now, a simplified version. A real implementation would be more complex. + p = food_pose.position + pose = Pose() + pose.position = Point(x=p.x, y=p.y, z=p.z + 0.3) + # Orientation looking down with some variability + r = R.from_euler("y", np.deg2rad(-90 + np.random.uniform(-15, 15))) + q = r.as_quat() + pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + return pose + + def _calculate_move_above_pose(self, food_pose: Pose) -> Pose: + """Calculate the pre-acquisition pose based on ADA action schema.""" + # Parameterize with approach vector (polar and azimuthal angles) + polar_angle = np.random.uniform(np.deg2rad(30), np.deg2rad(60)) + azimuthal_angle = np.random.uniform(-np.deg2rad(45), np.deg2rad(45)) + offset_dist = 0.1 + + # Calculate offset in a frame aligned with the food pose + x_offset = offset_dist * np.sin(polar_angle) * np.cos(azimuthal_angle) + y_offset = offset_dist * np.sin(polar_angle) * np.sin(azimuthal_angle) + z_offset = offset_dist * np.cos(polar_angle) + + # Create rotation from angles + rot = R.from_euler("zy", [-azimuthal_angle, -polar_angle]) + + # Apply this transform to the food pose + food_rot = R.from_quat( + [ + food_pose.orientation.x, + food_pose.orientation.y, + food_pose.orientation.z, + food_pose.orientation.w, + ] + ) + final_rot = food_rot * rot + q = final_rot.as_quat() + + p = food_pose.position + final_pos = Point(x=p.x - x_offset, y=p.y - y_offset, z=p.z + z_offset) + + return Pose( + position=final_pos, orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + ) + + def _calculate_move_into_pose(self, food_pose: Pose, move_above_pose: Pose) -> Pose: + """The MoveInto pose has the same orientation as MoveAbove.""" + return Pose( + position=food_pose.position, orientation=move_above_pose.orientation + ) + + def _calculate_staging_pose(self, mouth_pose: Pose) -> Pose: + """Calculate the staging pose relative to the mouth.""" + offset_dist = 0.15 + + p = mouth_pose.position + q = mouth_pose.orientation + mouth_rot = R.from_quat([q.x, q.y, q.z, q.w]) + + # Offset is along the mouth's forward-facing X-axis + offset_vec = mouth_rot.apply([offset_dist, 0, 0]) + + staged_pos = Point( + x=p.x - offset_vec[0], y=p.y - offset_vec[1], z=p.z - offset_vec[2] + ) + + # Here we would add the complex dual-orientation constraint logic + # For now, we use the same orientation as the mouth + return Pose(position=staged_pos, orientation=q) + + def _solve_articutool_ik( + self, target_vector: np.ndarray + ) -> List[Tuple[float, float]]: + """Analytical IK solver for the Articutool.""" + vx, vy, vz = target_vector + solutions = [] + asin_arg = -vx + if not (-1.0 - EPSILON <= asin_arg <= 1.0 + EPSILON): + return [] + theta_r1 = math.asin(np.clip(asin_arg, -1.0, 1.0)) + theta_r2 = (math.pi - theta_r1 + math.pi) % (2 * math.pi) - math.pi + for theta_r in list(set([theta_r1, theta_r2])): + cos_tr = math.cos(theta_r) + if math.isclose(cos_tr, 0.0, abs_tol=EPSILON): + if math.isclose(vy, 0.0, abs_tol=EPSILON) and math.isclose( + vz, 0.0, abs_tol=EPSILON + ): + solutions.append( + (0.0, (theta_r + math.pi) % (2 * math.pi) - math.pi) + ) + continue + theta_p = math.atan2(vz, vy) + solutions.append( + ( + (theta_p + math.pi) % (2 * math.pi) - math.pi, + (theta_r + math.pi) % (2 * math.pi) - math.pi, + ) + ) + return solutions + + # --- Feasibility Checking --- + def _is_config_kinematically_feasible(self, jaco_joint_config: List[float]) -> bool: + """ + Checks if the Articutool can maintain leveling at a single Jaco configuration. + This is our "oracle" for testing membership in the true feasibility manifold. + """ + if ( + self.pinocchio_model is None + or self.pinocchio_data is None + or self.jaco_ee_frame_id_pin is None + ): + return False + + q = pin.neutral(self.pinocchio_model) + for j, name in enumerate(JOINT_NAMES_FULL): + if self.pinocchio_model.existJointName(name): + joint_id = self.pinocchio_model.getJointId(name) + joint_obj = self.pinocchio_model.joints[joint_id] + if joint_obj.nq == 2 and not joint_obj.shortname().startswith( + "JointModelRX" + ): + q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ + math.cos(jaco_joint_config[j]), + math.sin(jaco_joint_config[j]), + ] + else: + q[joint_obj.idx_q] = jaco_joint_config[j] + + pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) + pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) + + ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + target_up_in_ee_frame = ( + R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) + ) + solutions = self._solve_articutool_ik(target_up_in_ee_frame) + + if not solutions: + return False + + return any( + ARTICUTOOL_PITCH_LIMITS_RAD[0] <= pitch <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] <= roll <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + for pitch, roll in solutions + ) + + def _verify_trajectory(self, trajectory: JointTrajectory) -> float: + """Verifies a trajectory and returns the percentage of feasible waypoints.""" + if not trajectory or not trajectory.points: + return 0.0 + + feasible_waypoints = 0 + for point in trajectory.points: + if self._is_config_kinematically_feasible(point.positions): + feasible_waypoints += 1 + + return (feasible_waypoints / len(trajectory.points)) * 100.0 + + # --- Planning Primitive Placeholders --- + def _plan_s1_unconstrained( + self, goal_pose: Pose, start_state: Any + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") + # TODO: Implement MoveIt2 call for unconstrained planning + return TrialStatus.SKIPPED, None + + def _plan_s2_guided( + self, goal_pose: Pose, start_state: Any + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + LOGGER.info(" Planning with S2 (6-DOF Guided)...") + # TODO: Implement MoveIt2 call with smart constraint and verification + return TrialStatus.SKIPPED, None + + def _plan_s3_coordinated( + self, goal_pose: Pose, start_state: Any + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + LOGGER.info(" Planning with S3 (8-DOF Coordinated)...") + # TODO: Implement 8-DOF IK solve and sequential planning + return TrialStatus.SKIPPED, None + + def _plan_s4_cartesian( + self, goal_pose: Pose, start_state: Any + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + LOGGER.info(" Planning with S4 (6-DOF Cartesian)...") + # TODO: Implement MoveIt2 call for Cartesian planning + return TrialStatus.SKIPPED, None + + # --- Main Benchmark Loop --- + def run(self): + """Main benchmark execution loop.""" + if not self.pinocchio_model: + LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") + return + + for i in range(self.num_trials): + LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") + + # 1. Generate a new scene + scene = self._generate_scene(base_z_offset=0.1) + + # 2. Simulate the feeding cycle state machine + food_on_tool = False + current_state = scene["home_config"] + + # Stage 1: Home -> AbovePlate (P1) + LOGGER.info("Stage 1: Home -> AbovePlate") + # This would be a plan to an IK solution for the pose + # For now, we just log it as a placeholder + self.results.append( + { + "trial_id": i, + "stage": "HomeToAbovePlate", + "status": TrialStatus.SKIPPED.value, + } + ) + + # Stage 2: AbovePlate -> MoveAbove (P2, P4) + LOGGER.info("Stage 2: AbovePlate -> MoveAbove") + status, traj = self._plan_s2_guided(scene["move_above_pose"], current_state) + self.results.append( + { + "trial_id": i, + "stage": "AbovePlateToMoveAbove", + "status": status.value, + } + ) + if status != TrialStatus.SUCCESS: + continue # End trial on failure + + # Stage 3: MoveToStaging + LOGGER.info("Stage 3: MoveToStaging (Food on tool!)") + food_on_tool = True + status, traj = self._plan_s2_guided(scene["staging_pose"], current_state) + + # The real verification would happen inside the planning primitive + feasibility = self._verify_trajectory(traj) if traj else 0.0 + + final_status = status + if status == TrialStatus.SUCCESS and feasibility < 99.0: + final_status = TrialStatus.VERIFICATION_FAILURE + + self.results.append( + { + "trial_id": i, + "stage": "AcquiredToStaging", + "status": final_status.value, + "leveling_feasibility": feasibility, + } + ) + if final_status != TrialStatus.SUCCESS: + continue + + self.save_results() + + def save_results(self): + """Saves the comprehensive benchmark results to a single JSON file.""" + if not self.output_dir: + return + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") + filename = os.path.join( + self.output_dir, f"end_to_end_benchmark_{timestamp}.json" + ) + try: + with open(filename, "w") as f: + json.dump(self.results, f, indent=2) + LOGGER.info(f"Benchmark results saved to {filename}") + except Exception as e: + LOGGER.error(f"Failed to save results: {e}") + + +def main(): + parser = argparse.ArgumentParser( + description="Run the end-to-end feeding benchmark." + ) + parser.add_argument( + "--xacro_file", + type=str, + default="/home/regulus/ada_ws/src/ada_ros2/ada_moveit/config/ada.urdf.xacro", + help="Path to the robot URDF/XACRO file.", + ) + parser.add_argument( + "--num_trials", + type=int, + default=100, + help="Number of full feeding trials to run.", + ) + parser.add_argument( + "--timeout", type=float, default=5.0, help="Planning timeout in seconds." + ) + parser.add_argument( + "--output_dir", + type=str, + default="e2e_benchmark_output", + help="Directory to save results.", + ) + args = parser.parse_args() + + rclpy.init() + node = Node("end_to_end_benchmark_node") + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + executor_thread = Thread(target=executor.spin, daemon=True) + executor_thread.start() + + # Initialize MoveIt2 for the Jaco arm, Articutool, and Full + moveit2_jaco = MoveIt2( + node=node, + joint_names=JOINT_NAMES_JACO, + base_link_name=BASE_LINK_JACO, + end_effector_name=END_EFFECTOR_LINK_JACO, + group_name=PLANNING_GROUP_JACO, + callback_group=ReentrantCallbackGroup(), + ) + moveit2_atool = MoveIt2( + node=node, + joint_names=JOINT_NAMES_ATOOL, + base_link_name=BASE_LINK_ATOOL, + end_effector_name=END_EFFECTOR_LINK_ATOOL, + group_name=PLANNING_GROUP_ATOOL, + callback_group=ReentrantCallbackGroup(), + ) + moveit2_full = MoveIt2( + node=node, + joint_names=JOINT_NAMES_FULL, + base_link_name=BASE_LINK_FULL, + end_effector_name=END_EFFECTOR_LINK_FULL, + group_name=PLANNING_GROUP_FULL, + callback_group=ReentrantCallbackGroup(), + ) + + benchmark = EndToEndBenchmark( + node, + moveit2_jaco, + moveit2_atool, + moveit2_full, + args.xacro_file, + args.num_trials, + args.timeout, + args.output_dir, + ) + + try: + benchmark.run() + except KeyboardInterrupt: + LOGGER.info("Benchmark interrupted by user.") + except Exception as e: + LOGGER.error(f"An unhandled error occurred: {e}") + finally: + LOGGER.info("Shutting down.") + rclpy.shutdown() + executor_thread.join() + + +if __name__ == "__main__": + main() From 00005953fea8116f226ec0eb11ad339b037ec560 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 28 Jul 2025 18:43:46 -0700 Subject: [PATCH 152/238] Add implementation for S1 primitive --- .../scripts/end_to_end_feeding_benchmark.py | 19 +++++++++++++++++-- 1 file changed, 17 insertions(+), 2 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 1a1a2945..3739ba75 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -55,6 +55,8 @@ ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) +PATH_CONSTRAINT_QUAT_XYZW = (0.707, 0.0, 0.0, 0.707) +PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = (1.5, 3.14, 0.8) # --- Data Structures --- @@ -346,8 +348,21 @@ def _plan_s1_unconstrained( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") - # TODO: Implement MoveIt2 call for unconstrained planning - return TrialStatus.SKIPPED, None + self.moveit2_jaco.clear_goal_constraints() + self.moveit2_jaco.clear_path_constraints() + + self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) + + future = self.moveit2_jaco.plan_async(start_joint_state=start_state) + rclpy.spin_until_future_complete( + self.node, future, timeout_sec=self.planning_timeout + ) + traj = self.moveit2_jaco.get_trajectory(future) + + if not traj or not traj.points: + return TrialStatus.PLANNER_FAILURE, None + + return TrialStatus.SUCCESS, traj def _plan_s2_guided( self, goal_pose: Pose, start_state: Any From 4b39c5ceb5feed7aabcdfc9a8914027fee4aff3c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 28 Jul 2025 19:36:21 -0700 Subject: [PATCH 153/238] Implement S2 planning primitives --- .../scripts/end_to_end_feeding_benchmark.py | 68 ++++++++++++++----- 1 file changed, 50 insertions(+), 18 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 3739ba75..43274358 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -366,10 +366,40 @@ def _plan_s1_unconstrained( def _plan_s2_guided( self, goal_pose: Pose, start_state: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: LOGGER.info(" Planning with S2 (6-DOF Guided)...") - # TODO: Implement MoveIt2 call with smart constraint and verification - return TrialStatus.SKIPPED, None + self.moveit2_jaco.clear_goal_constraints() + self.moveit2_jaco.clear_path_constraints() + + self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) + self.moveit2_jaco.set_path_orientation_constraint( + quat_xyzw=Quaternion( + x=PATH_CONSTRAINT_QUAT_XYZW[0], + y=PATH_CONSTRAINT_QUAT_XYZW[1], + z=PATH_CONSTRAINT_QUAT_XYZW[2], + w=PATH_CONSTRAINT_QUAT_XYZW[3], + ), + target_link=END_EFFECTOR_LINK_JACO, + tolerance=PATH_CONSTRAINT_TOLERANCE_XYZ_RAD, + weight=1.0, + ) + + future = self.moveit2_jaco.plan_async( + start_joint_state=start_state, + ) + rclpy.spin_until_future_complete( + self.node, future, timeout_sec=self.planning_timeout + ) + traj = self.moveit2_jaco.get_trajectory(future) + + if not traj or not traj.points: + return TrialStatus.PLANNER_FAILURE, None, 0.0 + + feasibility_percent = self._verify_trajectory(traj) + if feasibility_percent < 99.0: + return TrialStatus.VERIFICATION_FAILURE, traj, feasibility_percent + + return TrialStatus.SUCCESS, traj, feasibility_percent def _plan_s3_coordinated( self, goal_pose: Pose, start_state: Any @@ -400,7 +430,7 @@ def run(self): # 2. Simulate the feeding cycle state machine food_on_tool = False - current_state = scene["home_config"] + current_jaco_state = scene["home_config"] # Stage 1: Home -> AbovePlate (P1) LOGGER.info("Stage 1: Home -> AbovePlate") @@ -413,42 +443,44 @@ def run(self): "status": TrialStatus.SKIPPED.value, } ) + # TODO: Replace placeholder for the actual joint state after moving + current_jaco_state = scene["home_config"] # Stage 2: AbovePlate -> MoveAbove (P2, P4) LOGGER.info("Stage 2: AbovePlate -> MoveAbove") - status, traj = self._plan_s2_guided(scene["move_above_pose"], current_state) + status_s1, traj_s1 = self._plan_s1_unconstrained( + scene["move_above_pose"], current_jaco_state + ) self.results.append( { "trial_id": i, "stage": "AbovePlateToMoveAbove", - "status": status.value, + "status": status_s1.value, } ) - if status != TrialStatus.SUCCESS: + if status_s1 != TrialStatus.SUCCESS: continue # End trial on failure + current_jaco_state = traj_s1.points[-1].positions # Stage 3: MoveToStaging LOGGER.info("Stage 3: MoveToStaging (Food on tool!)") food_on_tool = True - status, traj = self._plan_s2_guided(scene["staging_pose"], current_state) - - # The real verification would happen inside the planning primitive - feasibility = self._verify_trajectory(traj) if traj else 0.0 - - final_status = status - if status == TrialStatus.SUCCESS and feasibility < 99.0: - final_status = TrialStatus.VERIFICATION_FAILURE + status_s2, traj_s2, feasibility_percent = self._plan_s2_guided( + scene["staging_pose"], current_jaco_state + ) self.results.append( { "trial_id": i, "stage": "AcquiredToStaging", - "status": final_status.value, - "leveling_feasibility": feasibility, + "primitive": "S2", + "status": status_s2.value, + "leveling_feasibility": feasibility_percent, } ) - if final_status != TrialStatus.SUCCESS: + if status_s2 != TrialStatus.SUCCESS: continue + current_jaco_state = traj_s2.points[-1].positions self.save_results() From ee4d813a16b9077617910049a6853eac940679a0 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 28 Jul 2025 20:03:21 -0700 Subject: [PATCH 154/238] Add logs for generated scene parameters --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 12 ++++++++++++ 1 file changed, 12 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 43274358..fe025a64 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -427,6 +427,18 @@ def run(self): # 1. Generate a new scene scene = self._generate_scene(base_z_offset=0.1) + LOGGER.info( + f""" + Generated Scene Parameters for Trial {i + 1}: + - Robot Base Z Offset: {scene["base_z_offset"]:.3f}m + - Food Pose: + Position: [x={scene["food_pose"].position.x:.3f}, y={scene["food_pose"].position.y:.3f}, z={scene["food_pose"].position.z:.3f}] + Orientation: [x={scene["food_pose"].orientation.x:.3f}, y={scene["food_pose"].orientation.y:.3f}, z={scene["food_pose"].orientation.z:.3f}, w={scene["food_pose"].orientation.w:.3f}] + - Mouth Pose: + Position: [x={scene["mouth_pose"].position.x:.3f}, y={scene["mouth_pose"].position.y:.3f}, z={scene["mouth_pose"].position.z:.3f}] + Orientation: [x={scene["mouth_pose"].orientation.x:.3f}, y={scene["mouth_pose"].orientation.y:.3f}, z={scene["mouth_pose"].orientation.z:.3f}, w={scene["mouth_pose"].orientation.w:.3f}] + """ + ) # 2. Simulate the feeding cycle state machine food_on_tool = False From 8fcb0ffe4c8145c9fc0e1603ec3825c9ee51adfd Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 28 Jul 2025 22:17:24 -0700 Subject: [PATCH 155/238] Solve IK solution to compute AbovePlate configuration --- .../scripts/end_to_end_feeding_benchmark.py | 23 +++++++++++++------ 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index fe025a64..2211db25 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -446,17 +446,25 @@ def run(self): # Stage 1: Home -> AbovePlate (P1) LOGGER.info("Stage 1: Home -> AbovePlate") - # This would be a plan to an IK solution for the pose - # For now, we just log it as a placeholder + above_plate_config = self.moveit2_jaco.compute_ik( + position=scene["move_above_pose"].position, + quat_xyzw=scene["move_above_pose"].orientation, + start_joint_state=current_jaco_state, + ) + ik_status = ( + TrialStatus.SUCCESS if above_plate_config else TrialStatus.IK_FAILURE + ) self.results.append( { "trial_id": i, "stage": "HomeToAbovePlate", - "status": TrialStatus.SKIPPED.value, + "primitive": "IK", + "status": ik_status.value, } ) - # TODO: Replace placeholder for the actual joint state after moving - current_jaco_state = scene["home_config"] + if ik_status != TrialStatus.SUCCESS: + continue + current_jaco_state = list(above_plate_config.position) # Stage 2: AbovePlate -> MoveAbove (P2, P4) LOGGER.info("Stage 2: AbovePlate -> MoveAbove") @@ -467,12 +475,13 @@ def run(self): { "trial_id": i, "stage": "AbovePlateToMoveAbove", + "primitive": "S2", "status": status_s1.value, } ) if status_s1 != TrialStatus.SUCCESS: continue # End trial on failure - current_jaco_state = traj_s1.points[-1].positions + current_jaco_state = list(traj_s1.points[-1].positions) # Stage 3: MoveToStaging LOGGER.info("Stage 3: MoveToStaging (Food on tool!)") @@ -492,7 +501,7 @@ def run(self): ) if status_s2 != TrialStatus.SUCCESS: continue - current_jaco_state = traj_s2.points[-1].positions + current_jaco_state = list(traj_s2.points[-1].positions) self.save_results() From a7786760787b8d66ba423571d955608dd307f799 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 29 Jul 2025 18:21:31 -0700 Subject: [PATCH 156/238] Adjust position sampling for food and mouth poses to be within the a half-sphere above the ground plane --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 2211db25..e11948af 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -148,8 +148,8 @@ def _sample_pose_in_spherical_shell(self) -> Pose: # Sample position r = np.random.uniform(inner_radius**3, outer_radius**3) ** (1 / 3) - theta = np.random.uniform(-np.pi / 2, np.pi / 2) # Front half-space - phi = np.random.uniform(0, np.pi) + theta = np.random.uniform(0, 2 * np.pi) + phi = np.random.uniform(0, np.pi / 2) x = r * np.cos(theta) * np.sin(phi) y = r * np.sin(theta) * np.sin(phi) z = r * np.cos(phi) From a42938771a84c483d740c43996fdbf3a0629456a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 29 Jul 2025 18:26:14 -0700 Subject: [PATCH 157/238] Augment logs for generated scene parameters --- .../scripts/end_to_end_feeding_benchmark.py | 37 +++++++++++++++---- 1 file changed, 30 insertions(+), 7 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index e11948af..a753935c 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -429,14 +429,37 @@ def run(self): scene = self._generate_scene(base_z_offset=0.1) LOGGER.info( f""" - Generated Scene Parameters for Trial {i + 1}: + --- Generated Scene Parameters for Trial {i + 1} --- - Robot Base Z Offset: {scene["base_z_offset"]:.3f}m - - Food Pose: - Position: [x={scene["food_pose"].position.x:.3f}, y={scene["food_pose"].position.y:.3f}, z={scene["food_pose"].position.z:.3f}] - Orientation: [x={scene["food_pose"].orientation.x:.3f}, y={scene["food_pose"].orientation.y:.3f}, z={scene["food_pose"].orientation.z:.3f}, w={scene["food_pose"].orientation.w:.3f}] - - Mouth Pose: - Position: [x={scene["mouth_pose"].position.x:.3f}, y={scene["mouth_pose"].position.y:.3f}, z={scene["mouth_pose"].position.z:.3f}] - Orientation: [x={scene["mouth_pose"].orientation.x:.3f}, y={scene["mouth_pose"].orientation.y:.3f}, z={scene["mouth_pose"].orientation.z:.3f}, w={scene["mouth_pose"].orientation.w:.3f}] + + - Initial State: + - Home Config: [{", ".join(f"{j:.4f}" for j in scene["home_config"])}] + + - Core Sampled Poses: + - Food Pose: + Position: [x={scene["food_pose"].position.x:.3f}, y={scene["food_pose"].position.y:.3f}, z={scene["food_pose"].position.z:.3f}] + Orientation: [x={scene["food_pose"].orientation.x:.3f}, y={scene["food_pose"].orientation.y:.3f}, z={scene["food_pose"].orientation.z:.3f}, w={scene["food_pose"].orientation.w:.3f}] + - Mouth Pose: + Position: [x={scene["mouth_pose"].position.x:.3f}, y={scene["mouth_pose"].position.y:.3f}, z={scene["mouth_pose"].position.z:.3f}] + Orientation: [x={scene["mouth_pose"].orientation.x:.3f}, y={scene["mouth_pose"].orientation.y:.3f}, z={scene["mouth_pose"].orientation.z:.3f}, w={scene["mouth_pose"].orientation.w:.3f}] + + - Derived Poses for Feeding Cycle: + - Above Plate Pose: + Position: [x={scene["above_plate_pose"].position.x:.3f}, y={scene["above_plate_pose"].position.y:.3f}, z={scene["above_plate_pose"].position.z:.3f}] + Orientation: [x={scene["above_plate_pose"].orientation.x:.3f}, y={scene["above_plate_pose"].orientation.y:.3f}, z={scene["above_plate_pose"].orientation.z:.3f}, w={scene["above_plate_pose"].orientation.w:.3f}] + - Move Above Pose: + Position: [x={scene["move_above_pose"].position.x:.3f}, y={scene["move_above_pose"].position.y:.3f}, z={scene["move_above_pose"].position.z:.3f}] + Orientation: [x={scene["move_above_pose"].orientation.x:.3f}, y={scene["move_above_pose"].orientation.y:.3f}, z={scene["move_above_pose"].orientation.z:.3f}, w={scene["move_above_pose"].orientation.w:.3f}] + - Move Into Pose: + Position: [x={scene["move_into_pose"].position.x:.3f}, y={scene["move_into_pose"].position.y:.3f}, z={scene["move_into_pose"].position.z:.3f}] + Orientation: [x={scene["move_into_pose"].orientation.x:.3f}, y={scene["move_into_pose"].orientation.y:.3f}, z={scene["move_into_pose"].orientation.z:.3f}, w={scene["move_into_pose"].orientation.w:.3f}] + - Staging Pose: + Position: [x={scene["staging_pose"].position.x:.3f}, y={scene["staging_pose"].position.y:.3f}, z={scene["staging_pose"].position.z:.3f}] + Orientation: [x={scene["staging_pose"].orientation.x:.3f}, y={scene["staging_pose"].orientation.y:.3f}, z={scene["staging_pose"].orientation.z:.3f}, w={scene["staging_pose"].orientation.w:.3f}] + - Resting Pose: + Position: [x={scene["resting_pose"].position.x:.3f}, y={scene["resting_pose"].position.y:.3f}, z={scene["resting_pose"].position.z:.3f}] + Orientation: [x={scene["resting_pose"].orientation.x:.3f}, y={scene["resting_pose"].orientation.y:.3f}, z={scene["resting_pose"].orientation.z:.3f}, w={scene["resting_pose"].orientation.w:.3f}] + ------------------------------------------------- """ ) From 1cdbb3bc495fe503a1e349ec235c5810eb257e1d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 4 Aug 2025 19:26:40 -0700 Subject: [PATCH 158/238] Add a bounding cylinder that encompasses the achievable workspace of the Articutool so that planning for the 6-DOF arm will not cause the Articutool to collide with the environment during transfer --- .../scripts/end_to_end_feeding_benchmark.py | 126 +++++++++++++++++- 1 file changed, 125 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index a753935c..c9a349ad 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -36,6 +36,10 @@ from geometry_msgs.msg import Pose, Point, Quaternion from scipy.spatial.transform import Rotation as R import pinocchio as pin +from moveit_msgs.msg import PlanningScene, AllowedCollisionEntry, AllowedCollisionMatrix +from moveit_msgs.srv import GetPlanningScene +from moveit_msgs.msg import CollisionObject +from shape_msgs.msg import SolidPrimitive # --- Constants --- LOGGER = rclpy.logging.get_logger("end_to_end_benchmark") @@ -57,6 +61,7 @@ WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) PATH_CONSTRAINT_QUAT_XYZW = (0.707, 0.0, 0.0, 0.707) PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = (1.5, 3.14, 0.8) +ARTICUTOOL_LENGTH_M = 0.14 # --- Data Structures --- @@ -121,6 +126,125 @@ def _initialize_pinocchio(self): LOGGER.error(f"Failed to initialize Pinocchio model: {e}", exc_info=True) self.pinocchio_model = None + def add_articutool_bounding_cylinder(self): + """ + Adds a cylindrical collision object to represent the Articutool + and updates the ACM to prevent spurious collisions. + """ + # Create a publisher to the planning scene topic if it doesn't exist + if not hasattr(self, "planning_scene_publisher"): + self.planning_scene_publisher = self.node.create_publisher( + PlanningScene, "/planning_scene", 10 + ) + + # --- Step 1: Define and add the CollisionObject --- + collision_object = CollisionObject() + collision_object.header.frame_id = ( + END_EFFECTOR_LINK_JACO # Attach to Jaco wrist + ) + collision_object.id = "articutool_bounding_cylinder" + collision_object.operation = CollisionObject.ADD + + # Define the cylinder's dimensions and pose relative to the wrist link + cylinder = SolidPrimitive() + cylinder.type = SolidPrimitive.CYLINDER + cylinder.dimensions = [ + 0.04, # height + ARTICUTOOL_LENGTH_M, # radius + ] + cylinder_pose = Pose() + cylinder_pose.position = Point(x=0.0, y=0.0, z=0.132) + cylinder_pose.orientation = Quaternion(x=0.0, y=0.707, z=0.0, w=0.707) + + collision_object.primitives.append(cylinder) + collision_object.primitive_poses.append(cylinder_pose) + + # Create a PlanningScene message to add the object + planning_scene_msg = PlanningScene() + planning_scene_msg.world.collision_objects.append(collision_object) + planning_scene_msg.is_diff = True + self.planning_scene_publisher.publish(planning_scene_msg) + self.node.get_logger().info("Published bounding cylinder to planning scene.") + + # Short delay to ensure the scene is updated + time.sleep(1.0) + + # --- Step 2: Modify the Allowed Collision Matrix (ACM) --- + # Create a client to get the planning scene + scene_client = self.node.create_client(GetPlanningScene, "/get_planning_scene") + while not scene_client.wait_for_service(timeout_sec=1.0): + self.node.get_logger().info( + 'Service "/get_planning_scene" not available, waiting...' + ) + + # Request the full scene, including the ACM + req = GetPlanningScene.Request() + req.components.components = req.components.ALLOWED_COLLISION_MATRIX + future = scene_client.call_async(req) + + # Add a callback to the future, which will be executed by the executor thread + future.add_done_callback(self._update_acm_callback) + + def _update_acm_callback(self, future): + """ + This callback is executed once the GetPlanningScene service returns a result. + """ + try: + result = future.result() + if result is None: + self.node.get_logger().error("Failed to get planning scene") + return + + acm = result.scene.allowed_collision_matrix + object_name = "articutool_bounding_cylinder" + links_to_allow = [ + "j2n6s200_link_6", + "atool_base", + "atool_electronics_holder_bottom_plate", + "atool_electronics_holder_upper_plate", + "atool_electronics_holder_wire_guard", + "atool_ft_adapter", + "atool_handle", + "atool_handle_cover", + "atool_link1", + "atool_link2", + "atool_motor_link", + "atool_u2d2", + "tool", + "j2n6s200_link_finger_tip_1", + "j2n6s200_link_finger_tip_2", + ] + + if object_name not in acm.entry_names: + for row in acm.entry_values: + row.enabled.append(False) + acm.entry_names.append(object_name) + new_row = AllowedCollisionEntry() + new_row.enabled = [False] * len(acm.entry_names) + acm.entry_values.append(new_row) + + obj_idx = acm.entry_names.index(object_name) + link_indices = [] + for name in links_to_allow: + try: + link_indices.append(acm.entry_names.index(name)) + except ValueError: + self.node.get_logger().warning( + f"Link '{name}' not in ACM, skipping." + ) + + for link_idx in link_indices: + acm.entry_values[obj_idx].enabled[link_idx] = True + acm.entry_values[link_idx].enabled[obj_idx] = True + + scene_update = PlanningScene(is_diff=True) + scene_update.allowed_collision_matrix = acm + self.planning_scene_publisher.publish(scene_update) + self.node.get_logger().info("Successfully updated ACM via callback.") + + except Exception as e: + self.node.get_logger().error(f"Error in ACM callback: {e}") + # --- Scene Generation --- def _generate_scene(self, base_z_offset: float) -> Dict[str, Any]: """Generates a randomized, robot-centric planning scene.""" @@ -421,7 +545,7 @@ def run(self): if not self.pinocchio_model: LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") return - + self.add_articutool_bounding_cylinder() for i in range(self.num_trials): LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") From a68acd879732ac492badcd80739b366f866d292c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 4 Aug 2025 19:38:08 -0700 Subject: [PATCH 159/238] Extract joints from IK solution corresponding to the Jaco arm planning group --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index c9a349ad..25ece3e9 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -611,7 +611,15 @@ def run(self): ) if ik_status != TrialStatus.SUCCESS: continue - current_jaco_state = list(above_plate_config.position) + # --- Robustly extract the 6 Jaco joints by name --- + # Create a dictionary mapping joint names to their positions from the IK solution + solution_joint_map = dict( + zip(above_plate_config.name, above_plate_config.position) + ) + + # Reconstruct the current_jaco_state in the correct order using JOINT_NAMES_JACO + # This ensures we get the right 6 joints in the expected order. + current_jaco_state = [solution_joint_map[name] for name in JOINT_NAMES_JACO] # Stage 2: AbovePlate -> MoveAbove (P2, P4) LOGGER.info("Stage 2: AbovePlate -> MoveAbove") From 77c1c03d6c7dc89aea78820bb06bc824fdeab4ef Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 4 Aug 2025 19:42:27 -0700 Subject: [PATCH 160/238] Add missing collision links --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 25ece3e9..6761d5d9 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -211,6 +211,8 @@ def _update_acm_callback(self, future): "atool_motor_link", "atool_u2d2", "tool", + "j2n6s200_link_finger_1", + "j2n6s200_link_finger_2", "j2n6s200_link_finger_tip_1", "j2n6s200_link_finger_tip_2", ] From ea7125ae4460489305205bdb1862ebeb5594d297 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 4 Aug 2025 19:58:59 -0700 Subject: [PATCH 161/238] Add IK retry logic --- .../scripts/end_to_end_feeding_benchmark.py | 33 ++++++++++++++----- 1 file changed, 25 insertions(+), 8 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 6761d5d9..0ea9dac7 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -595,23 +595,40 @@ def run(self): # Stage 1: Home -> AbovePlate (P1) LOGGER.info("Stage 1: Home -> AbovePlate") - above_plate_config = self.moveit2_jaco.compute_ik( - position=scene["move_above_pose"].position, - quat_xyzw=scene["move_above_pose"].orientation, - start_joint_state=current_jaco_state, - ) - ik_status = ( - TrialStatus.SUCCESS if above_plate_config else TrialStatus.IK_FAILURE - ) + max_ik_attempts = 5 + ik_attempts = 0 + above_plate_config = None + ik_status = TrialStatus.IK_FAILURE + for attempt in range(max_ik_attempts): + ik_attempts = ik_attempts + 1 + LOGGER.info( + f" Attempting IK solve ({ik_attempts}/{max_ik_attempts})..." + ) + start_joint_state = current_jaco_state + above_plate_config = self.moveit2_jaco.compute_ik( + position=scene["move_above_pose"].position, + quat_xyzw=scene["move_above_pose"].orientation, + start_joint_state=start_joint_state, + ) + if above_plate_config: + ik_status = TrialStatus.SUCCESS + LOGGER.info(f" IK solution found on attempt {ik_attempts}.") + break + else: + LOGGER.warning(f" IK attempt {ik_attempts} failed.") self.results.append( { "trial_id": i, "stage": "HomeToAbovePlate", "primitive": "IK", "status": ik_status.value, + "ik_attempts": ik_attempts, } ) if ik_status != TrialStatus.SUCCESS: + LOGGER.error( + f" IK failed after {ik_attempts} attempts. Skipping trial." + ) continue # --- Robustly extract the 6 Jaco joints by name --- # Create a dictionary mapping joint names to their positions from the IK solution From d3be3af10ec05c42663ace5d829302d3c422729e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 4 Aug 2025 20:18:37 -0700 Subject: [PATCH 162/238] Roll back stages past stage 1, need to focus on getting metrics right first --- .../scripts/end_to_end_feeding_benchmark.py | 29 ++----------------- 1 file changed, 3 insertions(+), 26 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 0ea9dac7..627f3a1d 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -639,43 +639,20 @@ def run(self): # Reconstruct the current_jaco_state in the correct order using JOINT_NAMES_JACO # This ensures we get the right 6 joints in the expected order. current_jaco_state = [solution_joint_map[name] for name in JOINT_NAMES_JACO] - - # Stage 2: AbovePlate -> MoveAbove (P2, P4) - LOGGER.info("Stage 2: AbovePlate -> MoveAbove") status_s1, traj_s1 = self._plan_s1_unconstrained( scene["move_above_pose"], current_jaco_state ) self.results.append( { "trial_id": i, - "stage": "AbovePlateToMoveAbove", - "primitive": "S2", + "stage": "HomeToAbovePlate", + "primitive": "S1", "status": status_s1.value, } ) if status_s1 != TrialStatus.SUCCESS: - continue # End trial on failure - current_jaco_state = list(traj_s1.points[-1].positions) - - # Stage 3: MoveToStaging - LOGGER.info("Stage 3: MoveToStaging (Food on tool!)") - food_on_tool = True - status_s2, traj_s2, feasibility_percent = self._plan_s2_guided( - scene["staging_pose"], current_jaco_state - ) - - self.results.append( - { - "trial_id": i, - "stage": "AcquiredToStaging", - "primitive": "S2", - "status": status_s2.value, - "leveling_feasibility": feasibility_percent, - } - ) - if status_s2 != TrialStatus.SUCCESS: continue - current_jaco_state = list(traj_s2.points[-1].positions) + current_jaco_state = list(traj_s1.points[-1].positions) self.save_results() From 64936be4bb5bcff9769878daf0c566f08ba45a5c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 4 Aug 2025 20:26:18 -0700 Subject: [PATCH 163/238] Consolidate logic, serialize/save trajectory, and update the state after planning --- .../scripts/end_to_end_feeding_benchmark.py | 101 +++++++++++------- 1 file changed, 65 insertions(+), 36 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 627f3a1d..aadc4a7d 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -469,6 +469,30 @@ def _verify_trajectory(self, trajectory: JointTrajectory) -> float: return (feasible_waypoints / len(trajectory.points)) * 100.0 + def _serialize_trajectory( + self, trajectory: JointTrajectory + ) -> Optional[Dict[str, Any]]: + """Converts a JointTrajectory message to a JSON-serializable dictionary.""" + if not trajectory or not trajectory.points: + return None + + serialized_points = [] + for point in trajectory.points: + serialized_points.append( + { + "positions": list(point.positions), + "velocities": list(point.velocities), + "accelerations": list(point.accelerations), + "time_from_start_sec": point.time_from_start.sec, + "time_from_start_nanosec": point.time_from_start.nanosec, + } + ) + + return { + "joint_names": list(trajectory.joint_names), + "points": serialized_points, + } + # --- Planning Primitive Placeholders --- def _plan_s1_unconstrained( self, goal_pose: Pose, start_state: Any @@ -593,66 +617,71 @@ def run(self): food_on_tool = False current_jaco_state = scene["home_config"] - # Stage 1: Home -> AbovePlate (P1) + # --- Stage 1: Home -> AbovePlate (P1 with S1) --- LOGGER.info("Stage 1: Home -> AbovePlate") + + # --- IK with Retries --- max_ik_attempts = 5 ik_attempts = 0 above_plate_config = None - ik_status = TrialStatus.IK_FAILURE for attempt in range(max_ik_attempts): - ik_attempts = ik_attempts + 1 + ik_attempts += 1 LOGGER.info( f" Attempting IK solve ({ik_attempts}/{max_ik_attempts})..." ) - start_joint_state = current_jaco_state - above_plate_config = self.moveit2_jaco.compute_ik( - position=scene["move_above_pose"].position, - quat_xyzw=scene["move_above_pose"].orientation, - start_joint_state=start_joint_state, + + # Using moveit2_jaco for IK as per your latest code + config = self.moveit2_jaco.compute_ik( + position=scene["above_plate_pose"].position, + quat_xyzw=scene["above_plate_pose"].orientation, + start_joint_state=current_jaco_state, ) - if above_plate_config: - ik_status = TrialStatus.SUCCESS + + if config: + above_plate_config = config LOGGER.info(f" IK solution found on attempt {ik_attempts}.") break else: LOGGER.warning(f" IK attempt {ik_attempts} failed.") + + # --- Planning --- + status = TrialStatus.IK_FAILURE # Default status + trajectory = None + if above_plate_config: + # IK succeeded, now attempt to plan + solution_joint_map = dict( + zip(above_plate_config.name, above_plate_config.position) + ) + ik_solution_for_planning = [ + solution_joint_map[name] for name in JOINT_NAMES_JACO + ] + + status, trajectory = self._plan_s1_unconstrained( + scene["above_plate_pose"], ik_solution_for_planning + ) + + # --- Result Logging for Stage 1 --- + # Consolidate all metrics for this stage into a single record. self.results.append( { "trial_id": i, "stage": "HomeToAbovePlate", - "primitive": "IK", - "status": ik_status.value, + "primitive": "S1", + "status": status.value, "ik_attempts": ik_attempts, + # Serialize the trajectory for visualization, will be None on failure + "trajectory": self._serialize_trajectory(trajectory), } ) - if ik_status != TrialStatus.SUCCESS: + + if status != TrialStatus.SUCCESS: LOGGER.error( - f" IK failed after {ik_attempts} attempts. Skipping trial." + f" Stage 1 failed with status: {status.value}. Skipping trial." ) continue - # --- Robustly extract the 6 Jaco joints by name --- - # Create a dictionary mapping joint names to their positions from the IK solution - solution_joint_map = dict( - zip(above_plate_config.name, above_plate_config.position) - ) - # Reconstruct the current_jaco_state in the correct order using JOINT_NAMES_JACO - # This ensures we get the right 6 joints in the expected order. - current_jaco_state = [solution_joint_map[name] for name in JOINT_NAMES_JACO] - status_s1, traj_s1 = self._plan_s1_unconstrained( - scene["move_above_pose"], current_jaco_state - ) - self.results.append( - { - "trial_id": i, - "stage": "HomeToAbovePlate", - "primitive": "S1", - "status": status_s1.value, - } - ) - if status_s1 != TrialStatus.SUCCESS: - continue - current_jaco_state = list(traj_s1.points[-1].positions) + # Update state for the next stage + current_jaco_state = list(trajectory.points[-1].positions) self.save_results() From 55e9eaf2b5b195567fe8a09b88b55c611f75de38 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 6 Aug 2025 17:03:29 -0700 Subject: [PATCH 164/238] Add visualization script for end-to-end benchmark --- .../scripts/visualize_e2e_benchmark.py | 812 ++++++++++++++++++ 1 file changed, 812 insertions(+) create mode 100644 ada_feeding/scripts/visualize_e2e_benchmark.py diff --git a/ada_feeding/scripts/visualize_e2e_benchmark.py b/ada_feeding/scripts/visualize_e2e_benchmark.py new file mode 100644 index 00000000..c66ffd76 --- /dev/null +++ b/ada_feeding/scripts/visualize_e2e_benchmark.py @@ -0,0 +1,812 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024-2025, Personal Robotics Laboratory +# License: BSD 3-Clause. See LICENSE.md file in root directory. + +""" +This script implements the End-to-End Assistive Feeding Benchmark. + +It evaluates a set of generalizable motion primitives across a complete, +simulated assistive feeding cycle. The benchmark operates within a variety of +procedurally generated, robot-centric planning scenes to test the +generalization and robustness of different control strategies. +""" + +# Standard imports +from collections import namedtuple +from datetime import datetime +import os +import time +from threading import Thread +from typing import Optional, List, Dict, Tuple, Any +import json +import math +import subprocess +import sys +import argparse +from enum import Enum + +# Third-party imports +import numpy as np +from pymoveit2 import MoveIt2 +from pymoveit2.robots import kinova +import rclpy +from rclpy.callback_groups import ReentrantCallbackGroup +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory +from geometry_msgs.msg import Pose, Point, Quaternion +from scipy.spatial.transform import Rotation as R +import pinocchio as pin +from moveit_msgs.msg import PlanningScene, AllowedCollisionEntry, AllowedCollisionMatrix +from moveit_msgs.srv import GetPlanningScene +from moveit_msgs.msg import CollisionObject +from shape_msgs.msg import SolidPrimitive + +# --- Constants --- +LOGGER = rclpy.logging.get_logger("end_to_end_benchmark") +PLANNING_GROUP_JACO = "jaco_arm" +PLANNING_GROUP_ATOOL = "articutool" +PLANNING_GROUP_FULL = "jaco_arm_with_articutool" +JOINT_NAMES_JACO = [f"j2n6s200_joint_{i + 1}" for i in range(6)] +JOINT_NAMES_ATOOL = [f"atool_joint_{i + 1}" for i in range(2)] +JOINT_NAMES_FULL = JOINT_NAMES_JACO + JOINT_NAMES_ATOOL +BASE_LINK_JACO = "j2n6s200_link_base" +BASE_LINK_ATOOL = "atool_link_base" +BASE_LINK_FULL = BASE_LINK_JACO +END_EFFECTOR_LINK_JACO = "j2n6s200_end_effector" +END_EFFECTOR_LINK_ATOOL = "tool_tip" +END_EFFECTOR_LINK_FULL = END_EFFECTOR_LINK_ATOOL +EPSILON = 1e-6 +ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) +ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) +WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) +PATH_CONSTRAINT_QUAT_XYZW = (0.707, 0.0, 0.0, 0.707) +PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = (1.5, 3.14, 0.8) +ARTICUTOOL_LENGTH_M = 0.14 + + +# --- Data Structures --- +class TrialStatus(Enum): + SUCCESS = "Success" + IK_FAILURE = "IK Failure" + PLANNER_FAILURE = "Planner Failure" + VERIFICATION_FAILURE = "Path Verification Failure" + SKIPPED = "Skipped" + + +class EndToEndBenchmark: + """Manages the end-to-end benchmark planning process.""" + + def __init__( + self, + node: Node, + moveit2_jaco: MoveIt2, + moveit2_atool: MoveIt2, + moveit2_full: MoveIt2, + xacro_file_path: str, + num_trials: int = 100, + planning_timeout: float = 5.0, + output_dir: Optional[str] = None, + ): + self.node = node + self.moveit2_jaco = moveit2_jaco + self.moveit2_atool = moveit2_atool + self.moveit2_full = moveit2_full + self.xacro_file_path = xacro_file_path + self.num_trials = num_trials + self.planning_timeout = planning_timeout + self.output_dir = output_dir + self.results: List[Dict[str, Any]] = [] + + # Pinocchio model for feasibility checks + self.pinocchio_model: Optional[pin.Model] = None + self.pinocchio_data: Optional[pin.Data] = None + self.jaco_ee_frame_id_pin: Optional[int] = None + self._initialize_pinocchio() + + if self.output_dir: + os.makedirs(self.output_dir, exist_ok=True) + + def _initialize_pinocchio(self): + """Loads the robot model from a XACRO file into Pinocchio.""" + try: + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", self.xacro_file_path], + check=True, + capture_output=True, + text=True, + ) + urdf_xml_string = process.stdout + self.pinocchio_model = pin.buildModelFromXML(urdf_xml_string) + self.pinocchio_data = self.pinocchio_model.createData() + self.jaco_ee_frame_id_pin = self.pinocchio_model.getFrameId( + END_EFFECTOR_LINK_FULL + ) + LOGGER.info("Pinocchio model loaded successfully.") + except Exception as e: + LOGGER.error(f"Failed to initialize Pinocchio model: {e}", exc_info=True) + self.pinocchio_model = None + + def add_articutool_bounding_cylinder(self): + """ + Adds a cylindrical collision object to represent the Articutool + and updates the ACM to prevent spurious collisions. + """ + # Create a publisher to the planning scene topic if it doesn't exist + if not hasattr(self, "planning_scene_publisher"): + self.planning_scene_publisher = self.node.create_publisher( + PlanningScene, "/planning_scene", 10 + ) + + # --- Step 1: Define and add the CollisionObject --- + collision_object = CollisionObject() + collision_object.header.frame_id = ( + END_EFFECTOR_LINK_JACO # Attach to Jaco wrist + ) + collision_object.id = "articutool_bounding_cylinder" + collision_object.operation = CollisionObject.ADD + + # Define the cylinder's dimensions and pose relative to the wrist link + cylinder = SolidPrimitive() + cylinder.type = SolidPrimitive.CYLINDER + cylinder.dimensions = [ + 0.04, # height + ARTICUTOOL_LENGTH_M, # radius + ] + cylinder_pose = Pose() + cylinder_pose.position = Point(x=0.0, y=0.0, z=0.132) + cylinder_pose.orientation = Quaternion(x=0.0, y=0.707, z=0.0, w=0.707) + + collision_object.primitives.append(cylinder) + collision_object.primitive_poses.append(cylinder_pose) + + # Create a PlanningScene message to add the object + planning_scene_msg = PlanningScene() + planning_scene_msg.world.collision_objects.append(collision_object) + planning_scene_msg.is_diff = True + self.planning_scene_publisher.publish(planning_scene_msg) + self.node.get_logger().info("Published bounding cylinder to planning scene.") + + # Short delay to ensure the scene is updated + time.sleep(1.0) + + # --- Step 2: Modify the Allowed Collision Matrix (ACM) --- + # Create a client to get the planning scene + scene_client = self.node.create_client(GetPlanningScene, "/get_planning_scene") + while not scene_client.wait_for_service(timeout_sec=1.0): + self.node.get_logger().info( + 'Service "/get_planning_scene" not available, waiting...' + ) + + # Request the full scene, including the ACM + req = GetPlanningScene.Request() + req.components.components = req.components.ALLOWED_COLLISION_MATRIX + future = scene_client.call_async(req) + + # Add a callback to the future, which will be executed by the executor thread + future.add_done_callback(self._update_acm_callback) + + def _update_acm_callback(self, future): + """ + This callback is executed once the GetPlanningScene service returns a result. + """ + try: + result = future.result() + if result is None: + self.node.get_logger().error("Failed to get planning scene") + return + + acm = result.scene.allowed_collision_matrix + object_name = "articutool_bounding_cylinder" + links_to_allow = [ + "j2n6s200_link_6", + "atool_base", + "atool_electronics_holder_bottom_plate", + "atool_electronics_holder_upper_plate", + "atool_electronics_holder_wire_guard", + "atool_ft_adapter", + "atool_handle", + "atool_handle_cover", + "atool_link1", + "atool_link2", + "atool_motor_link", + "atool_u2d2", + "tool", + "j2n6s200_link_finger_1", + "j2n6s200_link_finger_2", + "j2n6s200_link_finger_tip_1", + "j2n6s200_link_finger_tip_2", + ] + + if object_name not in acm.entry_names: + for row in acm.entry_values: + row.enabled.append(False) + acm.entry_names.append(object_name) + new_row = AllowedCollisionEntry() + new_row.enabled = [False] * len(acm.entry_names) + acm.entry_values.append(new_row) + + obj_idx = acm.entry_names.index(object_name) + link_indices = [] + for name in links_to_allow: + try: + link_indices.append(acm.entry_names.index(name)) + except ValueError: + self.node.get_logger().warning( + f"Link '{name}' not in ACM, skipping." + ) + + for link_idx in link_indices: + acm.entry_values[obj_idx].enabled[link_idx] = True + acm.entry_values[link_idx].enabled[obj_idx] = True + + scene_update = PlanningScene(is_diff=True) + scene_update.allowed_collision_matrix = acm + self.planning_scene_publisher.publish(scene_update) + self.node.get_logger().info("Successfully updated ACM via callback.") + + except Exception as e: + self.node.get_logger().error(f"Error in ACM callback: {e}") + + # --- Scene Generation --- + def _generate_scene(self, base_z_offset: float) -> Dict[str, Any]: + """Generates a randomized, robot-centric planning scene.""" + scene = {"base_z_offset": base_z_offset} + + # Sample food and mouth poses + scene["food_pose"] = self._sample_pose_in_spherical_shell() + scene["mouth_pose"] = self._sample_pose_in_spherical_shell() + + # Define derived poses + scene["home_config"] = [-1.47568, 2.92779, 1.00845, -2.0847, 1.43588, 1.32575] + scene["above_plate_pose"] = self._calculate_above_plate_pose(scene["food_pose"]) + scene["move_above_pose"] = self._calculate_move_above_pose(scene["food_pose"]) + scene["move_into_pose"] = self._calculate_move_into_pose( + scene["food_pose"], scene["move_above_pose"] + ) + scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) + scene["resting_pose"] = Pose(position=Point(x=0.4, y=-0.4, z=0.3)) + + return scene + + def _sample_pose_in_spherical_shell(self) -> Pose: + """Samples a random pose within a spherical shell in front of the robot.""" + inner_radius, outer_radius = 0.3, 0.7 + + # Sample position + r = np.random.uniform(inner_radius**3, outer_radius**3) ** (1 / 3) + theta = np.random.uniform(0, 2 * np.pi) + phi = np.random.uniform(0, np.pi / 2) + x = r * np.cos(theta) * np.sin(phi) + y = r * np.sin(theta) * np.sin(phi) + z = r * np.cos(phi) + position = Point(x=x, y=y, z=z) + + # Enforce "look-at-robot" constraint for orientation + direction_vector = -np.array([x, y, z]) + direction_vector /= np.linalg.norm(direction_vector) + + # Create rotation that aligns a frame's X-axis with this vector + # With some variability + rand_axis = np.random.randn(3) + rand_axis /= np.linalg.norm(rand_axis) + rand_angle = np.random.uniform(-np.deg2rad(30), np.deg2rad(30)) + variability_rot = R.from_rotvec(rand_angle * rand_axis) + + # Main rotation to look at origin + up_vector = np.array([0, 0, 1]) + x_axis = direction_vector + y_axis = np.cross(up_vector, x_axis) + y_axis /= np.linalg.norm(y_axis) + z_axis = np.cross(x_axis, y_axis) + + rotation_matrix = np.array([x_axis, y_axis, z_axis]).T + main_rot = R.from_matrix(rotation_matrix) + + final_rot = main_rot * variability_rot + quat = final_rot.as_quat() + + return Pose( + position=position, + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), + ) + + def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: + """Calculate a camera pose that looks down at the food.""" + # For now, a simplified version. A real implementation would be more complex. + p = food_pose.position + pose = Pose() + pose.position = Point(x=p.x, y=p.y, z=p.z + ARTICUTOOL_LENGTH_M) + # Orientation looking down with some variability + r = R.from_euler("y", np.deg2rad(-90 + np.random.uniform(-15, 15))) + q = r.as_quat() + pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + return pose + + def _calculate_move_above_pose(self, food_pose: Pose) -> Pose: + """Calculate the pre-acquisition pose based on ADA action schema.""" + # Parameterize with approach vector (polar and azimuthal angles) + polar_angle = np.random.uniform(np.deg2rad(30), np.deg2rad(60)) + azimuthal_angle = np.random.uniform(-np.deg2rad(45), np.deg2rad(45)) + offset_dist = 0.1 + + # Calculate offset in a frame aligned with the food pose + x_offset = offset_dist * np.sin(polar_angle) * np.cos(azimuthal_angle) + y_offset = offset_dist * np.sin(polar_angle) * np.sin(azimuthal_angle) + z_offset = offset_dist * np.cos(polar_angle) + + # Create rotation from angles + rot = R.from_euler("zy", [-azimuthal_angle, -polar_angle]) + + # Apply this transform to the food pose + food_rot = R.from_quat( + [ + food_pose.orientation.x, + food_pose.orientation.y, + food_pose.orientation.z, + food_pose.orientation.w, + ] + ) + final_rot = food_rot * rot + q = final_rot.as_quat() + + p = food_pose.position + final_pos = Point(x=p.x - x_offset, y=p.y - y_offset, z=p.z + z_offset) + + return Pose( + position=final_pos, orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + ) + + def _calculate_move_into_pose(self, food_pose: Pose, move_above_pose: Pose) -> Pose: + """The MoveInto pose has the same orientation as MoveAbove.""" + return Pose( + position=food_pose.position, orientation=move_above_pose.orientation + ) + + def _calculate_staging_pose(self, mouth_pose: Pose) -> Pose: + """Calculate the staging pose relative to the mouth.""" + offset_dist = 0.15 + + p = mouth_pose.position + q = mouth_pose.orientation + mouth_rot = R.from_quat([q.x, q.y, q.z, q.w]) + + # Offset is along the mouth's forward-facing X-axis + offset_vec = mouth_rot.apply([offset_dist, 0, 0]) + + staged_pos = Point( + x=p.x - offset_vec[0], y=p.y - offset_vec[1], z=p.z - offset_vec[2] + ) + + # Here we would add the complex dual-orientation constraint logic + # For now, we use the same orientation as the mouth + return Pose(position=staged_pos, orientation=q) + + def _solve_articutool_ik( + self, target_vector: np.ndarray + ) -> List[Tuple[float, float]]: + """Analytical IK solver for the Articutool.""" + vx, vy, vz = target_vector + solutions = [] + asin_arg = -vx + if not (-1.0 - EPSILON <= asin_arg <= 1.0 + EPSILON): + return [] + theta_r1 = math.asin(np.clip(asin_arg, -1.0, 1.0)) + theta_r2 = (math.pi - theta_r1 + math.pi) % (2 * math.pi) - math.pi + for theta_r in list(set([theta_r1, theta_r2])): + cos_tr = math.cos(theta_r) + if math.isclose(cos_tr, 0.0, abs_tol=EPSILON): + if math.isclose(vy, 0.0, abs_tol=EPSILON) and math.isclose( + vz, 0.0, abs_tol=EPSILON + ): + solutions.append( + (0.0, (theta_r + math.pi) % (2 * math.pi) - math.pi) + ) + continue + theta_p = math.atan2(vz, vy) + solutions.append( + ( + (theta_p + math.pi) % (2 * math.pi) - math.pi, + (theta_r + math.pi) % (2 * math.pi) - math.pi, + ) + ) + return solutions + + # --- Feasibility Checking --- + def _is_config_kinematically_feasible(self, jaco_joint_config: List[float]) -> bool: + """ + Checks if the Articutool can maintain leveling at a single Jaco configuration. + This is our "oracle" for testing membership in the true feasibility manifold. + """ + if ( + self.pinocchio_model is None + or self.pinocchio_data is None + or self.jaco_ee_frame_id_pin is None + ): + return False + + q = pin.neutral(self.pinocchio_model) + for j, name in enumerate(JOINT_NAMES_FULL): + if self.pinocchio_model.existJointName(name): + joint_id = self.pinocchio_model.getJointId(name) + joint_obj = self.pinocchio_model.joints[joint_id] + if joint_obj.nq == 2 and not joint_obj.shortname().startswith( + "JointModelRX" + ): + q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ + math.cos(jaco_joint_config[j]), + math.sin(jaco_joint_config[j]), + ] + else: + q[joint_obj.idx_q] = jaco_joint_config[j] + + pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) + pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) + + ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] + target_up_in_ee_frame = ( + R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) + ) + solutions = self._solve_articutool_ik(target_up_in_ee_frame) + + if not solutions: + return False + + return any( + ARTICUTOOL_PITCH_LIMITS_RAD[0] <= pitch <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] <= roll <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + for pitch, roll in solutions + ) + + def _verify_trajectory(self, trajectory: JointTrajectory) -> float: + """Verifies a trajectory and returns the percentage of feasible waypoints.""" + if not trajectory or not trajectory.points: + return 0.0 + + feasible_waypoints = 0 + for point in trajectory.points: + if self._is_config_kinematically_feasible(point.positions): + feasible_waypoints += 1 + + return (feasible_waypoints / len(trajectory.points)) * 100.0 + + def _serialize_pose(self, pose: Pose) -> Dict[str, List[float]]: + """Converts a Pose message to a JSON-serializable dictionary.""" + return { + "position": [pose.position.x, pose.position.y, pose.position.z], + "orientation_xyzw": [ + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ], + } + + def _serialize_trajectory( + self, trajectory: JointTrajectory + ) -> Optional[Dict[str, Any]]: + """Converts a JointTrajectory message to a JSON-serializable dictionary.""" + if not trajectory or not trajectory.points: + return None + + serialized_points = [] + for point in trajectory.points: + serialized_points.append( + { + "positions": list(point.positions), + "velocities": list(point.velocities), + "accelerations": list(point.accelerations), + "time_from_start_sec": point.time_from_start.sec, + "time_from_start_nanosec": point.time_from_start.nanosec, + } + ) + + return { + "joint_names": list(trajectory.joint_names), + "points": serialized_points, + } + + # --- Planning Primitive Placeholders --- + def _plan_s1_unconstrained( + self, goal_pose: Pose, start_state: Any + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") + self.moveit2_jaco.clear_goal_constraints() + self.moveit2_jaco.clear_path_constraints() + + self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) + + future = self.moveit2_jaco.plan_async(start_joint_state=start_state) + rclpy.spin_until_future_complete( + self.node, future, timeout_sec=self.planning_timeout + ) + traj = self.moveit2_jaco.get_trajectory(future) + + if not traj or not traj.points: + return TrialStatus.PLANNER_FAILURE, None + + return TrialStatus.SUCCESS, traj + + def _plan_s2_guided( + self, goal_pose: Pose, start_state: Any + ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: + LOGGER.info(" Planning with S2 (6-DOF Guided)...") + self.moveit2_jaco.clear_goal_constraints() + self.moveit2_jaco.clear_path_constraints() + + self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) + self.moveit2_jaco.set_path_orientation_constraint( + quat_xyzw=Quaternion( + x=PATH_CONSTRAINT_QUAT_XYZW[0], + y=PATH_CONSTRAINT_QUAT_XYZW[1], + z=PATH_CONSTRAINT_QUAT_XYZW[2], + w=PATH_CONSTRAINT_QUAT_XYZW[3], + ), + target_link=END_EFFECTOR_LINK_JACO, + tolerance=PATH_CONSTRAINT_TOLERANCE_XYZ_RAD, + weight=1.0, + ) + + future = self.moveit2_jaco.plan_async( + start_joint_state=start_state, + ) + rclpy.spin_until_future_complete( + self.node, future, timeout_sec=self.planning_timeout + ) + traj = self.moveit2_jaco.get_trajectory(future) + + if not traj or not traj.points: + return TrialStatus.PLANNER_FAILURE, None, 0.0 + + feasibility_percent = self._verify_trajectory(traj) + if feasibility_percent < 99.0: + return TrialStatus.VERIFICATION_FAILURE, traj, feasibility_percent + + return TrialStatus.SUCCESS, traj, feasibility_percent + + def _plan_s3_coordinated( + self, goal_pose: Pose, start_state: Any + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + LOGGER.info(" Planning with S3 (8-DOF Coordinated)...") + # TODO: Implement 8-DOF IK solve and sequential planning + return TrialStatus.SKIPPED, None + + def _plan_s4_cartesian( + self, goal_pose: Pose, start_state: Any + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + LOGGER.info(" Planning with S4 (6-DOF Cartesian)...") + # TODO: Implement MoveIt2 call for Cartesian planning + return TrialStatus.SKIPPED, None + + # --- Main Benchmark Loop --- + def run(self): + """Main benchmark execution loop.""" + if not self.pinocchio_model: + LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") + return + self.add_articutool_bounding_cylinder() + for i in range(self.num_trials): + LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") + + # 1. Generate a new scene + scene = self._generate_scene(base_z_offset=0.1) + LOGGER.info( + f""" + --- Generated Scene Parameters for Trial {i + 1} --- + - Robot Base Z Offset: {scene["base_z_offset"]:.3f}m + + - Initial State: + - Home Config: [{", ".join(f"{j:.4f}" for j in scene["home_config"])}] + + - Core Sampled Poses: + - Food Pose: + Position: [x={scene["food_pose"].position.x:.3f}, y={scene["food_pose"].position.y:.3f}, z={scene["food_pose"].position.z:.3f}] + Orientation: [x={scene["food_pose"].orientation.x:.3f}, y={scene["food_pose"].orientation.y:.3f}, z={scene["food_pose"].orientation.z:.3f}, w={scene["food_pose"].orientation.w:.3f}] + - Mouth Pose: + Position: [x={scene["mouth_pose"].position.x:.3f}, y={scene["mouth_pose"].position.y:.3f}, z={scene["mouth_pose"].position.z:.3f}] + Orientation: [x={scene["mouth_pose"].orientation.x:.3f}, y={scene["mouth_pose"].orientation.y:.3f}, z={scene["mouth_pose"].orientation.z:.3f}, w={scene["mouth_pose"].orientation.w:.3f}] + + - Derived Poses for Feeding Cycle: + - Above Plate Pose: + Position: [x={scene["above_plate_pose"].position.x:.3f}, y={scene["above_plate_pose"].position.y:.3f}, z={scene["above_plate_pose"].position.z:.3f}] + Orientation: [x={scene["above_plate_pose"].orientation.x:.3f}, y={scene["above_plate_pose"].orientation.y:.3f}, z={scene["above_plate_pose"].orientation.z:.3f}, w={scene["above_plate_pose"].orientation.w:.3f}] + - Move Above Pose: + Position: [x={scene["move_above_pose"].position.x:.3f}, y={scene["move_above_pose"].position.y:.3f}, z={scene["move_above_pose"].position.z:.3f}] + Orientation: [x={scene["move_above_pose"].orientation.x:.3f}, y={scene["move_above_pose"].orientation.y:.3f}, z={scene["move_above_pose"].orientation.z:.3f}, w={scene["move_above_pose"].orientation.w:.3f}] + - Move Into Pose: + Position: [x={scene["move_into_pose"].position.x:.3f}, y={scene["move_into_pose"].position.y:.3f}, z={scene["move_into_pose"].position.z:.3f}] + Orientation: [x={scene["move_into_pose"].orientation.x:.3f}, y={scene["move_into_pose"].orientation.y:.3f}, z={scene["move_into_pose"].orientation.z:.3f}, w={scene["move_into_pose"].orientation.w:.3f}] + - Staging Pose: + Position: [x={scene["staging_pose"].position.x:.3f}, y={scene["staging_pose"].position.y:.3f}, z={scene["staging_pose"].position.z:.3f}] + Orientation: [x={scene["staging_pose"].orientation.x:.3f}, y={scene["staging_pose"].orientation.y:.3f}, z={scene["staging_pose"].orientation.z:.3f}, w={scene["staging_pose"].orientation.w:.3f}] + - Resting Pose: + Position: [x={scene["resting_pose"].position.x:.3f}, y={scene["resting_pose"].position.y:.3f}, z={scene["resting_pose"].position.z:.3f}] + Orientation: [x={scene["resting_pose"].orientation.x:.3f}, y={scene["resting_pose"].orientation.y:.3f}, z={scene["resting_pose"].orientation.z:.3f}, w={scene["resting_pose"].orientation.w:.3f}] + ------------------------------------------------- + """ + ) + + # 2. Simulate the feeding cycle state machine + food_on_tool = False + current_jaco_state = scene["home_config"] + + # --- Stage 1: Home -> AbovePlate (P1 with S1) --- + LOGGER.info("Stage 1: Home -> AbovePlate") + + # --- IK with Retries --- + max_ik_attempts = 5 + ik_attempts = 0 + above_plate_config = None + for attempt in range(max_ik_attempts): + ik_attempts += 1 + LOGGER.info( + f" Attempting IK solve ({ik_attempts}/{max_ik_attempts})..." + ) + + # Using moveit2_jaco for IK as per your latest code + config = self.moveit2_jaco.compute_ik( + position=scene["above_plate_pose"].position, + quat_xyzw=scene["above_plate_pose"].orientation, + start_joint_state=current_jaco_state, + ) + + if config: + above_plate_config = config + LOGGER.info(f" IK solution found on attempt {ik_attempts}.") + break + else: + LOGGER.warning(f" IK attempt {ik_attempts} failed.") + + # --- Planning --- + status = TrialStatus.IK_FAILURE # Default status + trajectory = None + if above_plate_config: + # IK succeeded, now attempt to plan + solution_joint_map = dict( + zip(above_plate_config.name, above_plate_config.position) + ) + ik_solution_for_planning = [ + solution_joint_map[name] for name in JOINT_NAMES_JACO + ] + + status, trajectory = self._plan_s1_unconstrained( + scene["above_plate_pose"], ik_solution_for_planning + ) + + # Create a dictionary of all generated poses for this trial + scene_poses = { + "food_pose": self._serialize_pose(scene["food_pose"]), + "mouth_pose": self._serialize_pose(scene["mouth_pose"]), + "above_plate_pose": self._serialize_pose(scene["above_plate_pose"]), + "move_above_pose": self._serialize_pose(scene["move_above_pose"]), + "move_into_pose": self._serialize_pose(scene["move_into_pose"]), + "staging_pose": self._serialize_pose(scene["staging_pose"]), + "resting_pose": self._serialize_pose(scene["resting_pose"]), + } + + # --- Result Logging for Stage 1 --- + # Consolidate all metrics for this stage into a single record. + self.results.append( + { + "trial_id": i, + "stage": "HomeToAbovePlate", + "primitive": "S1", + "status": status.value, + "ik_attempts": ik_attempts, + # Serialize the trajectory for visualization, will be None on failure + "trajectory": self._serialize_trajectory(trajectory), + "scene_poses": scene_poses, + } + ) + + if status != TrialStatus.SUCCESS: + LOGGER.error( + f" Stage 1 failed with status: {status.value}. Skipping trial." + ) + continue + + # Update state for the next stage + current_jaco_state = list(trajectory.points[-1].positions) + + self.save_results() + + def save_results(self): + """Saves the comprehensive benchmark results to a single JSON file.""" + if not self.output_dir: + return + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") + filename = os.path.join( + self.output_dir, f"end_to_end_benchmark_{timestamp}.json" + ) + try: + with open(filename, "w") as f: + json.dump(self.results, f, indent=2) + LOGGER.info(f"Benchmark results saved to {filename}") + except Exception as e: + LOGGER.error(f"Failed to save results: {e}") + + +def main(): + parser = argparse.ArgumentParser( + description="Run the end-to-end feeding benchmark." + ) + parser.add_argument( + "--xacro_file", + type=str, + default="/home/regulus/ada_ws/src/ada_ros2/ada_moveit/config/ada.urdf.xacro", + help="Path to the robot URDF/XACRO file.", + ) + parser.add_argument( + "--num_trials", + type=int, + default=100, + help="Number of full feeding trials to run.", + ) + parser.add_argument( + "--timeout", type=float, default=5.0, help="Planning timeout in seconds." + ) + parser.add_argument( + "--output_dir", + type=str, + default="e2e_benchmark_output", + help="Directory to save results.", + ) + args = parser.parse_args() + + rclpy.init() + node = Node("end_to_end_benchmark_node") + executor = rclpy.executors.MultiThreadedExecutor() + executor.add_node(node) + executor_thread = Thread(target=executor.spin, daemon=True) + executor_thread.start() + + # Initialize MoveIt2 for the Jaco arm, Articutool, and Full + moveit2_jaco = MoveIt2( + node=node, + joint_names=JOINT_NAMES_JACO, + base_link_name=BASE_LINK_JACO, + end_effector_name=END_EFFECTOR_LINK_JACO, + group_name=PLANNING_GROUP_JACO, + callback_group=ReentrantCallbackGroup(), + ) + moveit2_atool = MoveIt2( + node=node, + joint_names=JOINT_NAMES_ATOOL, + base_link_name=BASE_LINK_ATOOL, + end_effector_name=END_EFFECTOR_LINK_ATOOL, + group_name=PLANNING_GROUP_ATOOL, + callback_group=ReentrantCallbackGroup(), + ) + moveit2_full = MoveIt2( + node=node, + joint_names=JOINT_NAMES_FULL, + base_link_name=BASE_LINK_FULL, + end_effector_name=END_EFFECTOR_LINK_FULL, + group_name=PLANNING_GROUP_FULL, + callback_group=ReentrantCallbackGroup(), + ) + + benchmark = EndToEndBenchmark( + node, + moveit2_jaco, + moveit2_atool, + moveit2_full, + args.xacro_file, + args.num_trials, + args.timeout, + args.output_dir, + ) + + try: + benchmark.run() + except KeyboardInterrupt: + LOGGER.info("Benchmark interrupted by user.") + except Exception as e: + LOGGER.error(f"An unhandled error occurred: {e}") + finally: + LOGGER.info("Shutting down.") + rclpy.shutdown() + executor_thread.join() + + +if __name__ == "__main__": + main() From 57c4a5ac0aceec1d619749ea0b16da7c79b1db01 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 6 Aug 2025 17:04:03 -0700 Subject: [PATCH 165/238] Serialize and store generated poses in results --- .../scripts/end_to_end_feeding_benchmark.py | 26 ++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index aadc4a7d..c66ffd76 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -315,7 +315,7 @@ def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: # For now, a simplified version. A real implementation would be more complex. p = food_pose.position pose = Pose() - pose.position = Point(x=p.x, y=p.y, z=p.z + 0.3) + pose.position = Point(x=p.x, y=p.y, z=p.z + ARTICUTOOL_LENGTH_M) # Orientation looking down with some variability r = R.from_euler("y", np.deg2rad(-90 + np.random.uniform(-15, 15))) q = r.as_quat() @@ -469,6 +469,18 @@ def _verify_trajectory(self, trajectory: JointTrajectory) -> float: return (feasible_waypoints / len(trajectory.points)) * 100.0 + def _serialize_pose(self, pose: Pose) -> Dict[str, List[float]]: + """Converts a Pose message to a JSON-serializable dictionary.""" + return { + "position": [pose.position.x, pose.position.y, pose.position.z], + "orientation_xyzw": [ + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ], + } + def _serialize_trajectory( self, trajectory: JointTrajectory ) -> Optional[Dict[str, Any]]: @@ -660,6 +672,17 @@ def run(self): scene["above_plate_pose"], ik_solution_for_planning ) + # Create a dictionary of all generated poses for this trial + scene_poses = { + "food_pose": self._serialize_pose(scene["food_pose"]), + "mouth_pose": self._serialize_pose(scene["mouth_pose"]), + "above_plate_pose": self._serialize_pose(scene["above_plate_pose"]), + "move_above_pose": self._serialize_pose(scene["move_above_pose"]), + "move_into_pose": self._serialize_pose(scene["move_into_pose"]), + "staging_pose": self._serialize_pose(scene["staging_pose"]), + "resting_pose": self._serialize_pose(scene["resting_pose"]), + } + # --- Result Logging for Stage 1 --- # Consolidate all metrics for this stage into a single record. self.results.append( @@ -671,6 +694,7 @@ def run(self): "ik_attempts": ik_attempts, # Serialize the trajectory for visualization, will be None on failure "trajectory": self._serialize_trajectory(trajectory), + "scene_poses": scene_poses, } ) From b10a7e3cbacd9ff22549fa6c6771af898b719b5d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 6 Aug 2025 17:40:10 -0700 Subject: [PATCH 166/238] Update end-to-end visualization script --- .../scripts/visualize_e2e_benchmark.py | 1073 +++++------------ 1 file changed, 306 insertions(+), 767 deletions(-) diff --git a/ada_feeding/scripts/visualize_e2e_benchmark.py b/ada_feeding/scripts/visualize_e2e_benchmark.py index c66ffd76..a4364407 100644 --- a/ada_feeding/scripts/visualize_e2e_benchmark.py +++ b/ada_feeding/scripts/visualize_e2e_benchmark.py @@ -3,810 +3,349 @@ # License: BSD 3-Clause. See LICENSE.md file in root directory. """ -This script implements the End-to-End Assistive Feeding Benchmark. +This script interactively visualizes trajectories from end_to_end_benchmark.py +output files using Pinocchio and MeshCat. -It evaluates a set of generalizable motion primitives across a complete, -simulated assistive feeding cycle. The benchmark operates within a variety of -procedurally generated, robot-centric planning scenes to test the -generalization and robustness of different control strategies. +It allows a user to load a benchmark result file, choose a specific +planning stage and trial, and then inspect the resulting trajectory +frame-by-frame or animate it. This version includes robust mesh path finding +and explicit kinematic updates to prevent rendering artifacts. """ # Standard imports -from collections import namedtuple -from datetime import datetime -import os -import time -from threading import Thread -from typing import Optional, List, Dict, Tuple, Any +import argparse import json -import math +import os import subprocess -import sys -import argparse -from enum import Enum +import tempfile +import time +from typing import Any, Dict, List, Optional -# Third-party imports +import meshcat import numpy as np -from pymoveit2 import MoveIt2 -from pymoveit2.robots import kinova -import rclpy -from rclpy.callback_groups import ReentrantCallbackGroup -from rclpy.node import Node -from trajectory_msgs.msg import JointTrajectory -from geometry_msgs.msg import Pose, Point, Quaternion -from scipy.spatial.transform import Rotation as R +import pandas as pd import pinocchio as pin -from moveit_msgs.msg import PlanningScene, AllowedCollisionEntry, AllowedCollisionMatrix -from moveit_msgs.srv import GetPlanningScene -from moveit_msgs.msg import CollisionObject -from shape_msgs.msg import SolidPrimitive - -# --- Constants --- -LOGGER = rclpy.logging.get_logger("end_to_end_benchmark") -PLANNING_GROUP_JACO = "jaco_arm" -PLANNING_GROUP_ATOOL = "articutool" -PLANNING_GROUP_FULL = "jaco_arm_with_articutool" -JOINT_NAMES_JACO = [f"j2n6s200_joint_{i + 1}" for i in range(6)] -JOINT_NAMES_ATOOL = [f"atool_joint_{i + 1}" for i in range(2)] -JOINT_NAMES_FULL = JOINT_NAMES_JACO + JOINT_NAMES_ATOOL -BASE_LINK_JACO = "j2n6s200_link_base" -BASE_LINK_ATOOL = "atool_link_base" -BASE_LINK_FULL = BASE_LINK_JACO -END_EFFECTOR_LINK_JACO = "j2n6s200_end_effector" -END_EFFECTOR_LINK_ATOOL = "tool_tip" -END_EFFECTOR_LINK_FULL = END_EFFECTOR_LINK_ATOOL -EPSILON = 1e-6 -ARTICUTOOL_PITCH_LIMITS_RAD = (-math.pi / 2, math.pi / 2) -ARTICUTOOL_ROLL_LIMITS_RAD = (-math.pi, math.pi) -WORLD_UP_VECTOR = np.array([0.0, 0.0, 1.0]) -PATH_CONSTRAINT_QUAT_XYZW = (0.707, 0.0, 0.0, 0.707) -PATH_CONSTRAINT_TOLERANCE_XYZ_RAD = (1.5, 3.14, 0.8) -ARTICUTOOL_LENGTH_M = 0.14 - - -# --- Data Structures --- -class TrialStatus(Enum): - SUCCESS = "Success" - IK_FAILURE = "IK Failure" - PLANNER_FAILURE = "Planner Failure" - VERIFICATION_FAILURE = "Path Verification Failure" - SKIPPED = "Skipped" - - -class EndToEndBenchmark: - """Manages the end-to-end benchmark planning process.""" - - def __init__( - self, - node: Node, - moveit2_jaco: MoveIt2, - moveit2_atool: MoveIt2, - moveit2_full: MoveIt2, - xacro_file_path: str, - num_trials: int = 100, - planning_timeout: float = 5.0, - output_dir: Optional[str] = None, - ): - self.node = node - self.moveit2_jaco = moveit2_jaco - self.moveit2_atool = moveit2_atool - self.moveit2_full = moveit2_full - self.xacro_file_path = xacro_file_path - self.num_trials = num_trials - self.planning_timeout = planning_timeout - self.output_dir = output_dir - self.results: List[Dict[str, Any]] = [] - - # Pinocchio model for feasibility checks - self.pinocchio_model: Optional[pin.Model] = None - self.pinocchio_data: Optional[pin.Data] = None - self.jaco_ee_frame_id_pin: Optional[int] = None - self._initialize_pinocchio() - - if self.output_dir: - os.makedirs(self.output_dir, exist_ok=True) - - def _initialize_pinocchio(self): - """Loads the robot model from a XACRO file into Pinocchio.""" - try: - process = subprocess.run( - ["ros2", "run", "xacro", "xacro", self.xacro_file_path], - check=True, - capture_output=True, - text=True, - ) - urdf_xml_string = process.stdout - self.pinocchio_model = pin.buildModelFromXML(urdf_xml_string) - self.pinocchio_data = self.pinocchio_model.createData() - self.jaco_ee_frame_id_pin = self.pinocchio_model.getFrameId( - END_EFFECTOR_LINK_FULL - ) - LOGGER.info("Pinocchio model loaded successfully.") - except Exception as e: - LOGGER.error(f"Failed to initialize Pinocchio model: {e}", exc_info=True) - self.pinocchio_model = None - - def add_articutool_bounding_cylinder(self): - """ - Adds a cylindrical collision object to represent the Articutool - and updates the ACM to prevent spurious collisions. - """ - # Create a publisher to the planning scene topic if it doesn't exist - if not hasattr(self, "planning_scene_publisher"): - self.planning_scene_publisher = self.node.create_publisher( - PlanningScene, "/planning_scene", 10 - ) - - # --- Step 1: Define and add the CollisionObject --- - collision_object = CollisionObject() - collision_object.header.frame_id = ( - END_EFFECTOR_LINK_JACO # Attach to Jaco wrist - ) - collision_object.id = "articutool_bounding_cylinder" - collision_object.operation = CollisionObject.ADD - - # Define the cylinder's dimensions and pose relative to the wrist link - cylinder = SolidPrimitive() - cylinder.type = SolidPrimitive.CYLINDER - cylinder.dimensions = [ - 0.04, # height - ARTICUTOOL_LENGTH_M, # radius - ] - cylinder_pose = Pose() - cylinder_pose.position = Point(x=0.0, y=0.0, z=0.132) - cylinder_pose.orientation = Quaternion(x=0.0, y=0.707, z=0.0, w=0.707) - - collision_object.primitives.append(cylinder) - collision_object.primitive_poses.append(cylinder_pose) - - # Create a PlanningScene message to add the object - planning_scene_msg = PlanningScene() - planning_scene_msg.world.collision_objects.append(collision_object) - planning_scene_msg.is_diff = True - self.planning_scene_publisher.publish(planning_scene_msg) - self.node.get_logger().info("Published bounding cylinder to planning scene.") - - # Short delay to ensure the scene is updated - time.sleep(1.0) - - # --- Step 2: Modify the Allowed Collision Matrix (ACM) --- - # Create a client to get the planning scene - scene_client = self.node.create_client(GetPlanningScene, "/get_planning_scene") - while not scene_client.wait_for_service(timeout_sec=1.0): - self.node.get_logger().info( - 'Service "/get_planning_scene" not available, waiting...' - ) - - # Request the full scene, including the ACM - req = GetPlanningScene.Request() - req.components.components = req.components.ALLOWED_COLLISION_MATRIX - future = scene_client.call_async(req) +import pinocchio.visualize - # Add a callback to the future, which will be executed by the executor thread - future.add_done_callback(self._update_acm_callback) - def _update_acm_callback(self, future): - """ - This callback is executed once the GetPlanningScene service returns a result. - """ - try: - result = future.result() - if result is None: - self.node.get_logger().error("Failed to get planning scene") - return - - acm = result.scene.allowed_collision_matrix - object_name = "articutool_bounding_cylinder" - links_to_allow = [ - "j2n6s200_link_6", - "atool_base", - "atool_electronics_holder_bottom_plate", - "atool_electronics_holder_upper_plate", - "atool_electronics_holder_wire_guard", - "atool_ft_adapter", - "atool_handle", - "atool_handle_cover", - "atool_link1", - "atool_link2", - "atool_motor_link", - "atool_u2d2", - "tool", - "j2n6s200_link_finger_1", - "j2n6s200_link_finger_2", - "j2n6s200_link_finger_tip_1", - "j2n6s200_link_finger_tip_2", - ] - - if object_name not in acm.entry_names: - for row in acm.entry_values: - row.enabled.append(False) - acm.entry_names.append(object_name) - new_row = AllowedCollisionEntry() - new_row.enabled = [False] * len(acm.entry_names) - acm.entry_values.append(new_row) - - obj_idx = acm.entry_names.index(object_name) - link_indices = [] - for name in links_to_allow: - try: - link_indices.append(acm.entry_names.index(name)) - except ValueError: - self.node.get_logger().warning( - f"Link '{name}' not in ACM, skipping." - ) - - for link_idx in link_indices: - acm.entry_values[obj_idx].enabled[link_idx] = True - acm.entry_values[link_idx].enabled[obj_idx] = True - - scene_update = PlanningScene(is_diff=True) - scene_update.allowed_collision_matrix = acm - self.planning_scene_publisher.publish(scene_update) - self.node.get_logger().info("Successfully updated ACM via callback.") - - except Exception as e: - self.node.get_logger().error(f"Error in ACM callback: {e}") - - # --- Scene Generation --- - def _generate_scene(self, base_z_offset: float) -> Dict[str, Any]: - """Generates a randomized, robot-centric planning scene.""" - scene = {"base_z_offset": base_z_offset} - - # Sample food and mouth poses - scene["food_pose"] = self._sample_pose_in_spherical_shell() - scene["mouth_pose"] = self._sample_pose_in_spherical_shell() - - # Define derived poses - scene["home_config"] = [-1.47568, 2.92779, 1.00845, -2.0847, 1.43588, 1.32575] - scene["above_plate_pose"] = self._calculate_above_plate_pose(scene["food_pose"]) - scene["move_above_pose"] = self._calculate_move_above_pose(scene["food_pose"]) - scene["move_into_pose"] = self._calculate_move_into_pose( - scene["food_pose"], scene["move_above_pose"] - ) - scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) - scene["resting_pose"] = Pose(position=Point(x=0.4, y=-0.4, z=0.3)) - - return scene - - def _sample_pose_in_spherical_shell(self) -> Pose: - """Samples a random pose within a spherical shell in front of the robot.""" - inner_radius, outer_radius = 0.3, 0.7 - - # Sample position - r = np.random.uniform(inner_radius**3, outer_radius**3) ** (1 / 3) - theta = np.random.uniform(0, 2 * np.pi) - phi = np.random.uniform(0, np.pi / 2) - x = r * np.cos(theta) * np.sin(phi) - y = r * np.sin(theta) * np.sin(phi) - z = r * np.cos(phi) - position = Point(x=x, y=y, z=z) - - # Enforce "look-at-robot" constraint for orientation - direction_vector = -np.array([x, y, z]) - direction_vector /= np.linalg.norm(direction_vector) - - # Create rotation that aligns a frame's X-axis with this vector - # With some variability - rand_axis = np.random.randn(3) - rand_axis /= np.linalg.norm(rand_axis) - rand_angle = np.random.uniform(-np.deg2rad(30), np.deg2rad(30)) - variability_rot = R.from_rotvec(rand_angle * rand_axis) - - # Main rotation to look at origin - up_vector = np.array([0, 0, 1]) - x_axis = direction_vector - y_axis = np.cross(up_vector, x_axis) - y_axis /= np.linalg.norm(y_axis) - z_axis = np.cross(x_axis, y_axis) - - rotation_matrix = np.array([x_axis, y_axis, z_axis]).T - main_rot = R.from_matrix(rotation_matrix) - - final_rot = main_rot * variability_rot - quat = final_rot.as_quat() - - return Pose( - position=position, - orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), - ) - - def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: - """Calculate a camera pose that looks down at the food.""" - # For now, a simplified version. A real implementation would be more complex. - p = food_pose.position - pose = Pose() - pose.position = Point(x=p.x, y=p.y, z=p.z + ARTICUTOOL_LENGTH_M) - # Orientation looking down with some variability - r = R.from_euler("y", np.deg2rad(-90 + np.random.uniform(-15, 15))) - q = r.as_quat() - pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) - return pose - - def _calculate_move_above_pose(self, food_pose: Pose) -> Pose: - """Calculate the pre-acquisition pose based on ADA action schema.""" - # Parameterize with approach vector (polar and azimuthal angles) - polar_angle = np.random.uniform(np.deg2rad(30), np.deg2rad(60)) - azimuthal_angle = np.random.uniform(-np.deg2rad(45), np.deg2rad(45)) - offset_dist = 0.1 - - # Calculate offset in a frame aligned with the food pose - x_offset = offset_dist * np.sin(polar_angle) * np.cos(azimuthal_angle) - y_offset = offset_dist * np.sin(polar_angle) * np.sin(azimuthal_angle) - z_offset = offset_dist * np.cos(polar_angle) - - # Create rotation from angles - rot = R.from_euler("zy", [-azimuthal_angle, -polar_angle]) - - # Apply this transform to the food pose - food_rot = R.from_quat( - [ - food_pose.orientation.x, - food_pose.orientation.y, - food_pose.orientation.z, - food_pose.orientation.w, - ] - ) - final_rot = food_rot * rot - q = final_rot.as_quat() - - p = food_pose.position - final_pos = Point(x=p.x - x_offset, y=p.y - y_offset, z=p.z + z_offset) - - return Pose( - position=final_pos, orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) +def xacro_to_urdf_string(xacro_filename: str, logger_func=print) -> Optional[str]: + """Converts a Xacro file to a URDF XML string using ros2 run xacro.""" + if not os.path.exists(xacro_filename): + logger_func(f"Error: Xacro file not found at {xacro_filename}") + return None + try: + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", xacro_filename], + check=True, + capture_output=True, + text=True, ) - - def _calculate_move_into_pose(self, food_pose: Pose, move_above_pose: Pose) -> Pose: - """The MoveInto pose has the same orientation as MoveAbove.""" - return Pose( - position=food_pose.position, orientation=move_above_pose.orientation + return process.stdout + except Exception as e: + logger_func(f"An unexpected error occurred during XACRO processing: {e}") + return None + + +def find_ros_package_paths(xacro_file_path: str, logger_func=print) -> List[str]: + """ + Finds and returns a list of package paths for Pinocchio to search for meshes. + This function robustly finds the Colcon workspace by searching upwards from + the provided XACRO file's location. + """ + package_dirs = [] + ros_package_path = os.environ.get("ROS_PACKAGE_PATH") + if ros_package_path: + package_dirs.extend( + [p for p in ros_package_path.split(os.pathsep) if os.path.isdir(p)] ) - def _calculate_staging_pose(self, mouth_pose: Pose) -> Pose: - """Calculate the staging pose relative to the mouth.""" - offset_dist = 0.15 - - p = mouth_pose.position - q = mouth_pose.orientation - mouth_rot = R.from_quat([q.x, q.y, q.z, q.w]) - - # Offset is along the mouth's forward-facing X-axis - offset_vec = mouth_rot.apply([offset_dist, 0, 0]) - - staged_pos = Point( - x=p.x - offset_vec[0], y=p.y - offset_vec[1], z=p.z - offset_vec[2] + current_path = os.path.abspath(os.path.dirname(xacro_file_path)) + ws_root = None + while True: + if os.path.isdir(os.path.join(current_path, "src")): + ws_root = current_path + break + parent_path = os.path.dirname(current_path) + if parent_path == current_path: + break + current_path = parent_path + + if ws_root: + logger_func(f"Found workspace root by searching upwards: {ws_root}") + src_dir = os.path.join(ws_root, "src") + if os.path.isdir(src_dir) and src_dir not in package_dirs: + package_dirs.append(src_dir) + install_dir = os.path.join(ws_root, "install") + if os.path.isdir(install_dir) and install_dir not in package_dirs: + package_dirs.append(install_dir) + + unique_package_dirs = list(dict.fromkeys(package_dirs)) + logger_func(f"Using package search paths for Pinocchio: {unique_package_dirs}") + return unique_package_dirs + + +def load_pinocchio_model_from_urdf_string( + urdf_xml_string: str, package_dirs: List[str], logger_func=print +): + """Loads a Pinocchio model from a URDF XML string via a temporary file.""" + temp_urdf_path = "" + try: + with tempfile.NamedTemporaryFile( + mode="w", suffix=".urdf", delete=False + ) as temp_file: + temp_urdf_path = temp_file.name + temp_file.write(urdf_xml_string) + + model = pin.buildModelFromUrdf(temp_urdf_path) + visual_model = pin.buildGeomFromUrdf( + model, temp_urdf_path, pin.GeometryType.VISUAL, package_dirs=package_dirs ) - - # Here we would add the complex dual-orientation constraint logic - # For now, we use the same orientation as the mouth - return Pose(position=staged_pos, orientation=q) - - def _solve_articutool_ik( - self, target_vector: np.ndarray - ) -> List[Tuple[float, float]]: - """Analytical IK solver for the Articutool.""" - vx, vy, vz = target_vector - solutions = [] - asin_arg = -vx - if not (-1.0 - EPSILON <= asin_arg <= 1.0 + EPSILON): - return [] - theta_r1 = math.asin(np.clip(asin_arg, -1.0, 1.0)) - theta_r2 = (math.pi - theta_r1 + math.pi) % (2 * math.pi) - math.pi - for theta_r in list(set([theta_r1, theta_r2])): - cos_tr = math.cos(theta_r) - if math.isclose(cos_tr, 0.0, abs_tol=EPSILON): - if math.isclose(vy, 0.0, abs_tol=EPSILON) and math.isclose( - vz, 0.0, abs_tol=EPSILON - ): - solutions.append( - (0.0, (theta_r + math.pi) % (2 * math.pi) - math.pi) - ) - continue - theta_p = math.atan2(vz, vy) - solutions.append( - ( - (theta_p + math.pi) % (2 * math.pi) - math.pi, - (theta_r + math.pi) % (2 * math.pi) - math.pi, - ) - ) - return solutions - - # --- Feasibility Checking --- - def _is_config_kinematically_feasible(self, jaco_joint_config: List[float]) -> bool: - """ - Checks if the Articutool can maintain leveling at a single Jaco configuration. - This is our "oracle" for testing membership in the true feasibility manifold. - """ - if ( - self.pinocchio_model is None - or self.pinocchio_data is None - or self.jaco_ee_frame_id_pin is None - ): - return False - - q = pin.neutral(self.pinocchio_model) - for j, name in enumerate(JOINT_NAMES_FULL): - if self.pinocchio_model.existJointName(name): - joint_id = self.pinocchio_model.getJointId(name) - joint_obj = self.pinocchio_model.joints[joint_id] - if joint_obj.nq == 2 and not joint_obj.shortname().startswith( - "JointModelRX" - ): - q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ - math.cos(jaco_joint_config[j]), - math.sin(jaco_joint_config[j]), - ] - else: - q[joint_obj.idx_q] = jaco_joint_config[j] - - pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) - pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) - - ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] - target_up_in_ee_frame = ( - R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) + collision_model = pin.buildGeomFromUrdf( + model, temp_urdf_path, pin.GeometryType.COLLISION, package_dirs=package_dirs ) - solutions = self._solve_articutool_ik(target_up_in_ee_frame) - if not solutions: - return False + data = model.createData() + logger_func(f"Pinocchio model loaded. Nq: {model.nq}, Nv: {model.nv}") + return model, collision_model, visual_model, data + except Exception as e: + logger_func("\n" + "=" * 80) + logger_func("FATAL: Error loading Pinocchio model.") + logger_func(f"Original Pinocchio error: {e}") + logger_func("=" * 80 + "\n") + return None, None, None, None + finally: + if temp_urdf_path and os.path.exists(temp_urdf_path): + os.remove(temp_urdf_path) - return any( - ARTICUTOOL_PITCH_LIMITS_RAD[0] <= pitch <= ARTICUTOOL_PITCH_LIMITS_RAD[1] - and ARTICUTOOL_ROLL_LIMITS_RAD[0] <= roll <= ARTICUTOOL_ROLL_LIMITS_RAD[1] - for pitch, roll in solutions - ) - def _verify_trajectory(self, trajectory: JointTrajectory) -> float: - """Verifies a trajectory and returns the percentage of feasible waypoints.""" - if not trajectory or not trajectory.points: - return 0.0 - - feasible_waypoints = 0 - for point in trajectory.points: - if self._is_config_kinematically_feasible(point.positions): - feasible_waypoints += 1 - - return (feasible_waypoints / len(trajectory.points)) * 100.0 - - def _serialize_pose(self, pose: Pose) -> Dict[str, List[float]]: - """Converts a Pose message to a JSON-serializable dictionary.""" - return { - "position": [pose.position.x, pose.position.y, pose.position.z], - "orientation_xyzw": [ - pose.orientation.x, - pose.orientation.y, - pose.orientation.z, - pose.orientation.w, - ], - } - - def _serialize_trajectory( - self, trajectory: JointTrajectory - ) -> Optional[Dict[str, Any]]: - """Converts a JointTrajectory message to a JSON-serializable dictionary.""" - if not trajectory or not trajectory.points: - return None - - serialized_points = [] - for point in trajectory.points: - serialized_points.append( - { - "positions": list(point.positions), - "velocities": list(point.velocities), - "accelerations": list(point.accelerations), - "time_from_start_sec": point.time_from_start.sec, - "time_from_start_nanosec": point.time_from_start.nanosec, - } - ) +def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFrame: + """Loads and concatenates data from multiple benchmark JSON files.""" + all_data = [] + for file_path in file_paths: + try: + with open(file_path, "r") as f: + data = json.load(f) + valid_entries = [ + entry for entry in data if entry.get("trajectory") is not None + ] + all_data.extend(valid_entries) + except Exception as e: + logger_func(f"Warning: Could not load or parse {file_path}. Error: {e}") - return { - "joint_names": list(trajectory.joint_names), - "points": serialized_points, - } + if not all_data: + return pd.DataFrame() - # --- Planning Primitive Placeholders --- - def _plan_s1_unconstrained( - self, goal_pose: Pose, start_state: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: - LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") - self.moveit2_jaco.clear_goal_constraints() - self.moveit2_jaco.clear_path_constraints() + logger_func( + f"Successfully loaded {len(all_data)} trials with trajectories from {len(file_paths)} file(s)." + ) + return pd.DataFrame(all_data) - self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) - future = self.moveit2_jaco.plan_async(start_joint_state=start_state) - rclpy.spin_until_future_complete( - self.node, future, timeout_sec=self.planning_timeout - ) - traj = self.moveit2_jaco.get_trajectory(future) - - if not traj or not traj.points: - return TrialStatus.PLANNER_FAILURE, None - - return TrialStatus.SUCCESS, traj - - def _plan_s2_guided( - self, goal_pose: Pose, start_state: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: - LOGGER.info(" Planning with S2 (6-DOF Guided)...") - self.moveit2_jaco.clear_goal_constraints() - self.moveit2_jaco.clear_path_constraints() - - self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) - self.moveit2_jaco.set_path_orientation_constraint( - quat_xyzw=Quaternion( - x=PATH_CONSTRAINT_QUAT_XYZW[0], - y=PATH_CONSTRAINT_QUAT_XYZW[1], - z=PATH_CONSTRAINT_QUAT_XYZW[2], - w=PATH_CONSTRAINT_QUAT_XYZW[3], - ), - target_link=END_EFFECTOR_LINK_JACO, - tolerance=PATH_CONSTRAINT_TOLERANCE_XYZ_RAD, - weight=1.0, - ) +def select_trajectory(df: pd.DataFrame) -> Optional[Dict[str, Any]]: + """Interactively prompts the user to select a stage and a specific trial.""" + while True: + stages = df["stage"].unique() + print("\n--- Please Select a Stage to Visualize ---") + for i, stage in enumerate(stages): + print(f" [{i + 1}] {stage}") + print(" [q] Quit") - future = self.moveit2_jaco.plan_async( - start_joint_state=start_state, - ) - rclpy.spin_until_future_complete( - self.node, future, timeout_sec=self.planning_timeout - ) - traj = self.moveit2_jaco.get_trajectory(future) - - if not traj or not traj.points: - return TrialStatus.PLANNER_FAILURE, None, 0.0 - - feasibility_percent = self._verify_trajectory(traj) - if feasibility_percent < 99.0: - return TrialStatus.VERIFICATION_FAILURE, traj, feasibility_percent - - return TrialStatus.SUCCESS, traj, feasibility_percent - - def _plan_s3_coordinated( - self, goal_pose: Pose, start_state: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: - LOGGER.info(" Planning with S3 (8-DOF Coordinated)...") - # TODO: Implement 8-DOF IK solve and sequential planning - return TrialStatus.SKIPPED, None - - def _plan_s4_cartesian( - self, goal_pose: Pose, start_state: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: - LOGGER.info(" Planning with S4 (6-DOF Cartesian)...") - # TODO: Implement MoveIt2 call for Cartesian planning - return TrialStatus.SKIPPED, None - - # --- Main Benchmark Loop --- - def run(self): - """Main benchmark execution loop.""" - if not self.pinocchio_model: - LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") - return - self.add_articutool_bounding_cylinder() - for i in range(self.num_trials): - LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") - - # 1. Generate a new scene - scene = self._generate_scene(base_z_offset=0.1) - LOGGER.info( - f""" - --- Generated Scene Parameters for Trial {i + 1} --- - - Robot Base Z Offset: {scene["base_z_offset"]:.3f}m - - - Initial State: - - Home Config: [{", ".join(f"{j:.4f}" for j in scene["home_config"])}] - - - Core Sampled Poses: - - Food Pose: - Position: [x={scene["food_pose"].position.x:.3f}, y={scene["food_pose"].position.y:.3f}, z={scene["food_pose"].position.z:.3f}] - Orientation: [x={scene["food_pose"].orientation.x:.3f}, y={scene["food_pose"].orientation.y:.3f}, z={scene["food_pose"].orientation.z:.3f}, w={scene["food_pose"].orientation.w:.3f}] - - Mouth Pose: - Position: [x={scene["mouth_pose"].position.x:.3f}, y={scene["mouth_pose"].position.y:.3f}, z={scene["mouth_pose"].position.z:.3f}] - Orientation: [x={scene["mouth_pose"].orientation.x:.3f}, y={scene["mouth_pose"].orientation.y:.3f}, z={scene["mouth_pose"].orientation.z:.3f}, w={scene["mouth_pose"].orientation.w:.3f}] - - - Derived Poses for Feeding Cycle: - - Above Plate Pose: - Position: [x={scene["above_plate_pose"].position.x:.3f}, y={scene["above_plate_pose"].position.y:.3f}, z={scene["above_plate_pose"].position.z:.3f}] - Orientation: [x={scene["above_plate_pose"].orientation.x:.3f}, y={scene["above_plate_pose"].orientation.y:.3f}, z={scene["above_plate_pose"].orientation.z:.3f}, w={scene["above_plate_pose"].orientation.w:.3f}] - - Move Above Pose: - Position: [x={scene["move_above_pose"].position.x:.3f}, y={scene["move_above_pose"].position.y:.3f}, z={scene["move_above_pose"].position.z:.3f}] - Orientation: [x={scene["move_above_pose"].orientation.x:.3f}, y={scene["move_above_pose"].orientation.y:.3f}, z={scene["move_above_pose"].orientation.z:.3f}, w={scene["move_above_pose"].orientation.w:.3f}] - - Move Into Pose: - Position: [x={scene["move_into_pose"].position.x:.3f}, y={scene["move_into_pose"].position.y:.3f}, z={scene["move_into_pose"].position.z:.3f}] - Orientation: [x={scene["move_into_pose"].orientation.x:.3f}, y={scene["move_into_pose"].orientation.y:.3f}, z={scene["move_into_pose"].orientation.z:.3f}, w={scene["move_into_pose"].orientation.w:.3f}] - - Staging Pose: - Position: [x={scene["staging_pose"].position.x:.3f}, y={scene["staging_pose"].position.y:.3f}, z={scene["staging_pose"].position.z:.3f}] - Orientation: [x={scene["staging_pose"].orientation.x:.3f}, y={scene["staging_pose"].orientation.y:.3f}, z={scene["staging_pose"].orientation.z:.3f}, w={scene["staging_pose"].orientation.w:.3f}] - - Resting Pose: - Position: [x={scene["resting_pose"].position.x:.3f}, y={scene["resting_pose"].position.y:.3f}, z={scene["resting_pose"].position.z:.3f}] - Orientation: [x={scene["resting_pose"].orientation.x:.3f}, y={scene["resting_pose"].orientation.y:.3f}, z={scene["resting_pose"].orientation.z:.3f}, w={scene["resting_pose"].orientation.w:.3f}] - ------------------------------------------------- - """ + try: + choice_str = input(f"Enter choice (1-{len(stages)} or q): ").strip().lower() + if choice_str == "q": + return None + stage_choice = int(choice_str) - 1 + if not 0 <= stage_choice < len(stages): + raise ValueError + selected_stage = stages[stage_choice] + except (ValueError, IndexError): + print("Invalid choice.") + continue + + stage_df = df[ + (df["stage"] == selected_stage) & (df["status"] == "Success") + ].sort_values("trial_id") + + if stage_df.empty: + print(f"No successful trials found for stage: {selected_stage}") + continue + + trajectories = stage_df.to_dict("records") + print(f"\n--- Select a Successful Trial from Stage '{selected_stage}' ---") + for i, trial in enumerate(trajectories): + print( + f" [{i + 1}] Trial ID: {trial['trial_id']} (IK Attempts: {trial['ik_attempts']})" ) + print(" [m] Back to Stage Selection") - # 2. Simulate the feeding cycle state machine - food_on_tool = False - current_jaco_state = scene["home_config"] - - # --- Stage 1: Home -> AbovePlate (P1 with S1) --- - LOGGER.info("Stage 1: Home -> AbovePlate") - - # --- IK with Retries --- - max_ik_attempts = 5 - ik_attempts = 0 - above_plate_config = None - for attempt in range(max_ik_attempts): - ik_attempts += 1 - LOGGER.info( - f" Attempting IK solve ({ik_attempts}/{max_ik_attempts})..." - ) - - # Using moveit2_jaco for IK as per your latest code - config = self.moveit2_jaco.compute_ik( - position=scene["above_plate_pose"].position, - quat_xyzw=scene["above_plate_pose"].orientation, - start_joint_state=current_jaco_state, - ) - - if config: - above_plate_config = config - LOGGER.info(f" IK solution found on attempt {ik_attempts}.") - break - else: - LOGGER.warning(f" IK attempt {ik_attempts} failed.") - - # --- Planning --- - status = TrialStatus.IK_FAILURE # Default status - trajectory = None - if above_plate_config: - # IK succeeded, now attempt to plan - solution_joint_map = dict( - zip(above_plate_config.name, above_plate_config.position) - ) - ik_solution_for_planning = [ - solution_joint_map[name] for name in JOINT_NAMES_JACO - ] - - status, trajectory = self._plan_s1_unconstrained( - scene["above_plate_pose"], ik_solution_for_planning - ) - - # Create a dictionary of all generated poses for this trial - scene_poses = { - "food_pose": self._serialize_pose(scene["food_pose"]), - "mouth_pose": self._serialize_pose(scene["mouth_pose"]), - "above_plate_pose": self._serialize_pose(scene["above_plate_pose"]), - "move_above_pose": self._serialize_pose(scene["move_above_pose"]), - "move_into_pose": self._serialize_pose(scene["move_into_pose"]), - "staging_pose": self._serialize_pose(scene["staging_pose"]), - "resting_pose": self._serialize_pose(scene["resting_pose"]), - } - - # --- Result Logging for Stage 1 --- - # Consolidate all metrics for this stage into a single record. - self.results.append( - { - "trial_id": i, - "stage": "HomeToAbovePlate", - "primitive": "S1", - "status": status.value, - "ik_attempts": ik_attempts, - # Serialize the trajectory for visualization, will be None on failure - "trajectory": self._serialize_trajectory(trajectory), - "scene_poses": scene_poses, - } + try: + choice_str = ( + input(f"Enter choice (1-{len(trajectories)} or m): ").strip().lower() ) - - if status != TrialStatus.SUCCESS: - LOGGER.error( - f" Stage 1 failed with status: {status.value}. Skipping trial." - ) + if choice_str == "m": continue + traj_choice = int(choice_str) - 1 + if not 0 <= traj_choice < len(trajectories): + raise ValueError + return trajectories[traj_choice] + except (ValueError, IndexError): + print("Invalid choice.") + continue + + +def draw_scene_frames(pin_viz, scene_poses: Dict[str, Any], frame_scale: float = 0.2): + """Draws coordinate frames for all poses generated in the scene.""" + if not scene_poses: + return + + print("Drawing scene frames...") + for name, pose_data in scene_poses.items(): + position = np.array(pose_data["position"]) + quat_xyzw = pose_data["orientation_xyzw"] + quat_wxyz = np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]) + transform = pin.SE3(pin.Quaternion(quat_wxyz), position) + + frame_path = f"scene/frames/{name}" + pin_viz.viewer[frame_path].set_object(meshcat.geometry.triad(frame_scale)) + pin_viz.viewer[frame_path].set_transform(transform.homogeneous) + + +def visualization_loop(pin_viz, model, data, selected_trial, args): + """Runs the interactive UI for a single selected trajectory.""" + # Clear previous frames and draw the ones for the current trial + try: + pin_viz.viewer["scene/frames"].delete() + except KeyError: + pass # It's okay if the path doesn't exist on the first run + + if "scene_poses" in selected_trial: + draw_scene_frames(pin_viz, selected_trial["scene_poses"]) + + q = pin.neutral(model) + trajectory = selected_trial["trajectory"] + joint_names_from_traj = trajectory["joint_names"] + waypoints = trajectory["points"] + + joint_map = { + name: model.getJointId(name) + for name in joint_names_from_traj + if model.existJointName(name) + } + + current_idx = 0 + while True: + waypoint_positions = waypoints[current_idx]["positions"] + for name, pos in zip(joint_names_from_traj, waypoint_positions): + if name in joint_map: + joint_id = joint_map[name] + if model.joints[joint_id].nq == 1: + q[model.joints[joint_id].idx_q] = pos + elif model.joints[joint_id].nq == 2: + q[model.joints[joint_id].idx_q] = np.cos(pos) + q[model.joints[joint_id].idx_q + 1] = np.sin(pos) + + # Explicitly update kinematics before displaying to prevent rendering artifacts + pin.forwardKinematics(model, data, q) + pin_viz.display(q) + + print(f"\nDisplaying Frame: {current_idx + 1}/{len(waypoints)}") + print("Commands: [n]ext, [p]rev, [f]irst, [l]ast, [a]nimate, [m]enu, [q]uit") + + user_input = input("Enter command: ").strip().lower() + + if user_input == "q": + return "quit" + if user_input == "m": + return "menu" + + if user_input == "n": + current_idx = min(current_idx + 1, len(waypoints) - 1) + elif user_input == "p": + current_idx = max(current_idx - 1, 0) + elif user_input == "f": + current_idx = 0 + elif user_input == "l": + current_idx = len(waypoints) - 1 + elif user_input == "a": + print("Animating... Press Ctrl+C to stop.") + try: + for i in range(current_idx, len(waypoints)): + waypoint_positions = waypoints[i]["positions"] + for name, pos in zip(joint_names_from_traj, waypoint_positions): + if name in joint_map: + joint_id = joint_map[name] + if model.joints[joint_id].nq == 1: + q[model.joints[joint_id].idx_q] = pos + elif model.joints[joint_id].nq == 2: + q[model.joints[joint_id].idx_q] = np.cos(pos) + q[model.joints[joint_id].idx_q + 1] = np.sin(pos) + + # Explicitly update kinematics in animation loop + pin.forwardKinematics(model, data, q) + pin_viz.display(q) + print(f" Displaying frame {i + 1}/{len(waypoints)}", end="\r") + time.sleep(1.0 / args.fps) + current_idx = len(waypoints) - 1 + print("\nAnimation finished.") + except KeyboardInterrupt: + print("\nAnimation stopped.") + elif user_input: + print("Invalid command.") + + +def main(args): + """Main execution function.""" + print("--- Interactive Benchmark Trajectory Visualizer ---") + + df = load_benchmark_data(args.benchmark_files) + if df.empty: + print("No data with trajectories could be loaded. Exiting.") + return + + package_dirs = find_ros_package_paths(args.xacro_file) + urdf_string = xacro_to_urdf_string(args.xacro_file) + if not urdf_string: + return + + model, collision_model, visual_model, data = load_pinocchio_model_from_urdf_string( + urdf_xml_string=urdf_string, package_dirs=package_dirs + ) + if not model: + return - # Update state for the next stage - current_jaco_state = list(trajectory.points[-1].positions) + try: + visualizer = meshcat.Visualizer().open() + pin_viz = pin.visualize.MeshcatVisualizer(model, collision_model, visual_model) + pin_viz.initViewer(viewer=visualizer) + pin_viz.loadViewerModel(rootNodeName="robot") + print(f"\nMeshCat viewer URL: {visualizer.url()}\n") + except Exception as e: + print(f"Error initializing MeshCat: {e}") + return - self.save_results() + while True: + selected_trial = select_trajectory(df) + if selected_trial is None: + break - def save_results(self): - """Saves the comprehensive benchmark results to a single JSON file.""" - if not self.output_dir: - return - timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") - filename = os.path.join( - self.output_dir, f"end_to_end_benchmark_{timestamp}.json" - ) - try: - with open(filename, "w") as f: - json.dump(self.results, f, indent=2) - LOGGER.info(f"Benchmark results saved to {filename}") - except Exception as e: - LOGGER.error(f"Failed to save results: {e}") + # Pass the 'data' object to the visualization loop + action = visualization_loop(pin_viz, model, data, selected_trial, args) + if action == "quit": + break + print("Visualizer finished.") -def main(): + +if __name__ == "__main__": parser = argparse.ArgumentParser( - description="Run the end-to-end feeding benchmark." - ) - parser.add_argument( - "--xacro_file", - type=str, - default="/home/regulus/ada_ws/src/ada_ros2/ada_moveit/config/ada.urdf.xacro", - help="Path to the robot URDF/XACRO file.", - ) - parser.add_argument( - "--num_trials", - type=int, - default=100, - help="Number of full feeding trials to run.", + description="Visualize robot trajectories from benchmark files." ) + parser.add_argument("xacro_file", type=str, help="Path to the robot XACRO file.") parser.add_argument( - "--timeout", type=float, default=5.0, help="Planning timeout in seconds." + "benchmark_files", nargs="+", help="One or more benchmark JSON result files." ) parser.add_argument( - "--output_dir", - type=str, - default="e2e_benchmark_output", - help="Directory to save results.", + "--fps", type=int, default=30, help="Frames per second for animation." ) args = parser.parse_args() - - rclpy.init() - node = Node("end_to_end_benchmark_node") - executor = rclpy.executors.MultiThreadedExecutor() - executor.add_node(node) - executor_thread = Thread(target=executor.spin, daemon=True) - executor_thread.start() - - # Initialize MoveIt2 for the Jaco arm, Articutool, and Full - moveit2_jaco = MoveIt2( - node=node, - joint_names=JOINT_NAMES_JACO, - base_link_name=BASE_LINK_JACO, - end_effector_name=END_EFFECTOR_LINK_JACO, - group_name=PLANNING_GROUP_JACO, - callback_group=ReentrantCallbackGroup(), - ) - moveit2_atool = MoveIt2( - node=node, - joint_names=JOINT_NAMES_ATOOL, - base_link_name=BASE_LINK_ATOOL, - end_effector_name=END_EFFECTOR_LINK_ATOOL, - group_name=PLANNING_GROUP_ATOOL, - callback_group=ReentrantCallbackGroup(), - ) - moveit2_full = MoveIt2( - node=node, - joint_names=JOINT_NAMES_FULL, - base_link_name=BASE_LINK_FULL, - end_effector_name=END_EFFECTOR_LINK_FULL, - group_name=PLANNING_GROUP_FULL, - callback_group=ReentrantCallbackGroup(), - ) - - benchmark = EndToEndBenchmark( - node, - moveit2_jaco, - moveit2_atool, - moveit2_full, - args.xacro_file, - args.num_trials, - args.timeout, - args.output_dir, - ) - - try: - benchmark.run() - except KeyboardInterrupt: - LOGGER.info("Benchmark interrupted by user.") - except Exception as e: - LOGGER.error(f"An unhandled error occurred: {e}") - finally: - LOGGER.info("Shutting down.") - rclpy.shutdown() - executor_thread.join() - - -if __name__ == "__main__": - main() + main(args) From 042148324f58b24996d666cf0a6ed427559570e4 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 6 Aug 2025 17:41:26 -0700 Subject: [PATCH 167/238] Update end-to-end benchmark to use current Jaco state as start state for IK in S1, and add ft link to list of allowed collisions for cylinder --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index c66ffd76..cad85f00 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -211,6 +211,7 @@ def _update_acm_callback(self, future): "atool_motor_link", "atool_u2d2", "tool", + "ft", "j2n6s200_link_finger_1", "j2n6s200_link_finger_2", "j2n6s200_link_finger_tip_1", @@ -661,15 +662,8 @@ def run(self): trajectory = None if above_plate_config: # IK succeeded, now attempt to plan - solution_joint_map = dict( - zip(above_plate_config.name, above_plate_config.position) - ) - ik_solution_for_planning = [ - solution_joint_map[name] for name in JOINT_NAMES_JACO - ] - status, trajectory = self._plan_s1_unconstrained( - scene["above_plate_pose"], ik_solution_for_planning + scene["above_plate_pose"], current_jaco_state ) # Create a dictionary of all generated poses for this trial From c1af8272252dd79fd4df9d9755b3ea2bbd42b808 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 6 Aug 2025 17:59:01 -0700 Subject: [PATCH 168/238] Add visual markers for generated frames --- .../scripts/visualize_e2e_benchmark.py | 20 +++++++++++++------ 1 file changed, 14 insertions(+), 6 deletions(-) diff --git a/ada_feeding/scripts/visualize_e2e_benchmark.py b/ada_feeding/scripts/visualize_e2e_benchmark.py index a4364407..a023d976 100644 --- a/ada_feeding/scripts/visualize_e2e_benchmark.py +++ b/ada_feeding/scripts/visualize_e2e_benchmark.py @@ -22,6 +22,7 @@ from typing import Any, Dict, List, Optional import meshcat +import meshcat.geometry as g import numpy as np import pandas as pd import pinocchio as pin @@ -194,19 +195,29 @@ def select_trajectory(df: pd.DataFrame) -> Optional[Dict[str, Any]]: def draw_scene_frames(pin_viz, scene_poses: Dict[str, Any], frame_scale: float = 0.2): - """Draws coordinate frames for all poses generated in the scene.""" + """Draws coordinate frames and identifying markers for all poses in the scene.""" if not scene_poses: return - print("Drawing scene frames...") + print("Drawing scene frames and markers...") for name, pose_data in scene_poses.items(): position = np.array(pose_data["position"]) quat_xyzw = pose_data["orientation_xyzw"] quat_wxyz = np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]) transform = pin.SE3(pin.Quaternion(quat_wxyz), position) + # Create a parent path for the frame and its marker frame_path = f"scene/frames/{name}" - pin_viz.viewer[frame_path].set_object(meshcat.geometry.triad(frame_scale)) + + # Draw the coordinate frame (triad) + pin_viz.viewer[f"{frame_path}/triad"].set_object(g.triad(frame_scale)) + + # Draw a small sphere as a visual marker/label + # This can be clicked in the viewer to identify the frame by its name + material = g.MeshLambertMaterial(color=0x5555FF, transparent=True, opacity=0.6) + pin_viz.viewer[f"{frame_path}/marker"].set_object(g.Sphere(0.02), material) + + # Apply the same transform to the parent path pin_viz.viewer[frame_path].set_transform(transform.homogeneous) @@ -244,7 +255,6 @@ def visualization_loop(pin_viz, model, data, selected_trial, args): q[model.joints[joint_id].idx_q] = np.cos(pos) q[model.joints[joint_id].idx_q + 1] = np.sin(pos) - # Explicitly update kinematics before displaying to prevent rendering artifacts pin.forwardKinematics(model, data, q) pin_viz.display(q) @@ -280,7 +290,6 @@ def visualization_loop(pin_viz, model, data, selected_trial, args): q[model.joints[joint_id].idx_q] = np.cos(pos) q[model.joints[joint_id].idx_q + 1] = np.sin(pos) - # Explicitly update kinematics in animation loop pin.forwardKinematics(model, data, q) pin_viz.display(q) print(f" Displaying frame {i + 1}/{len(waypoints)}", end="\r") @@ -328,7 +337,6 @@ def main(args): if selected_trial is None: break - # Pass the 'data' object to the visualization loop action = visualization_loop(pin_viz, model, data, selected_trial, args) if action == "quit": break From eaf1f6df1cdc7eb7bb7fc0669db7e75aedd93f0a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 6 Aug 2025 18:20:46 -0700 Subject: [PATCH 169/238] Fix quaternion convention when visualizing generated frames --- ada_feeding/scripts/visualize_e2e_benchmark.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ada_feeding/scripts/visualize_e2e_benchmark.py b/ada_feeding/scripts/visualize_e2e_benchmark.py index a023d976..3b5df0af 100644 --- a/ada_feeding/scripts/visualize_e2e_benchmark.py +++ b/ada_feeding/scripts/visualize_e2e_benchmark.py @@ -202,9 +202,9 @@ def draw_scene_frames(pin_viz, scene_poses: Dict[str, Any], frame_scale: float = print("Drawing scene frames and markers...") for name, pose_data in scene_poses.items(): position = np.array(pose_data["position"]) - quat_xyzw = pose_data["orientation_xyzw"] - quat_wxyz = np.array([quat_xyzw[3], quat_xyzw[0], quat_xyzw[1], quat_xyzw[2]]) - transform = pin.SE3(pin.Quaternion(quat_wxyz), position) + quat_xyzw = np.array(pose_data["orientation_xyzw"]) + + transform = pin.SE3(pin.Quaternion(quat_xyzw), position) # Create a parent path for the frame and its marker frame_path = f"scene/frames/{name}" @@ -213,7 +213,6 @@ def draw_scene_frames(pin_viz, scene_poses: Dict[str, Any], frame_scale: float = pin_viz.viewer[f"{frame_path}/triad"].set_object(g.triad(frame_scale)) # Draw a small sphere as a visual marker/label - # This can be clicked in the viewer to identify the frame by its name material = g.MeshLambertMaterial(color=0x5555FF, transparent=True, opacity=0.6) pin_viz.viewer[f"{frame_path}/marker"].set_object(g.Sphere(0.02), material) From 0413fe26db6765e11ba46a96994c78515beb2731 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 6 Aug 2025 18:35:19 -0700 Subject: [PATCH 170/238] Fix logic to generate food frame such that the z-axis is always pointing up, and the x-axis is pointing towards the robot base's vertical axis, with a slight yaw offset --- .../scripts/end_to_end_feeding_benchmark.py | 52 ++++++++++++------- 1 file changed, 32 insertions(+), 20 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index cad85f00..27ab64a2 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -270,7 +270,11 @@ def _generate_scene(self, base_z_offset: float) -> Dict[str, Any]: return scene def _sample_pose_in_spherical_shell(self) -> Pose: - """Samples a random pose within a spherical shell in front of the robot.""" + """ + Samples a random pose within a spherical shell in front of the robot. + The frame's Z-axis is constrained to be world up, and its X-axis is + oriented to point towards the robot base with some random variability. + """ inner_radius, outer_radius = 0.3, 0.7 # Sample position @@ -282,27 +286,35 @@ def _sample_pose_in_spherical_shell(self) -> Pose: z = r * np.cos(phi) position = Point(x=x, y=y, z=z) - # Enforce "look-at-robot" constraint for orientation - direction_vector = -np.array([x, y, z]) - direction_vector /= np.linalg.norm(direction_vector) - - # Create rotation that aligns a frame's X-axis with this vector - # With some variability - rand_axis = np.random.randn(3) - rand_axis /= np.linalg.norm(rand_axis) - rand_angle = np.random.uniform(-np.deg2rad(30), np.deg2rad(30)) - variability_rot = R.from_rotvec(rand_angle * rand_axis) - - # Main rotation to look at origin - up_vector = np.array([0, 0, 1]) - x_axis = direction_vector - y_axis = np.cross(up_vector, x_axis) - y_axis /= np.linalg.norm(y_axis) - z_axis = np.cross(x_axis, y_axis) - - rotation_matrix = np.array([x_axis, y_axis, z_axis]).T + # --- Corrected Orientation Logic --- + # 1. Define the primary Z-axis constraint (world up). + z_axis = np.array([0.0, 0.0, 1.0]) + + # 2. Define the vector pointing from the sampled point to the robot base (origin). + look_at_vector = -np.array([x, y, z]) + + # 3. Project the look-at vector onto the XY plane to get the direction for the X-axis. + # This ensures the final X-axis will be perpendicular to the world Z-axis. + x_axis_direction = np.array([look_at_vector[0], look_at_vector[1], 0.0]) + + # Normalize the direction vector. Handle the case where the point is directly above the origin. + if np.linalg.norm(x_axis_direction) < 1e-6: + x_axis_direction = np.array( + [1.0, 0.0, 0.0] + ) # Default to pointing along world X + x_axis_direction /= np.linalg.norm(x_axis_direction) + + # 4. Create a base rotation where the X-axis points in the desired direction and Z is up. + # The Y-axis is derived from the cross product to form a right-handed frame. + y_axis = np.cross(z_axis, x_axis_direction) + rotation_matrix = np.array([x_axis_direction, y_axis, z_axis]).T main_rot = R.from_matrix(rotation_matrix) + # 5. Add a small random rotational variability around the Z-axis. + rand_yaw_angle = np.random.uniform(-np.deg2rad(30), np.deg2rad(30)) + variability_rot = R.from_euler("z", rand_yaw_angle) + + # 6. Combine the main rotation with the variability. final_rot = main_rot * variability_rot quat = final_rot.as_quat() From 7a02e8ed7dc54b700c4cb715369983ab3ac09d39 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 9 Aug 2025 11:44:42 -0700 Subject: [PATCH 171/238] Adjust logic to calculate above food pose so that the the Jaco end-effector looks at the food. The position is sampled from a spherical cap above the food pose, and the orientation is constrained to look at the food with no roll --- .../scripts/end_to_end_feeding_benchmark.py | 71 ++++++++++++++++--- 1 file changed, 61 insertions(+), 10 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 27ab64a2..cba39774 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -324,16 +324,67 @@ def _sample_pose_in_spherical_shell(self) -> Pose: ) def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: - """Calculate a camera pose that looks down at the food.""" - # For now, a simplified version. A real implementation would be more complex. - p = food_pose.position - pose = Pose() - pose.position = Point(x=p.x, y=p.y, z=p.z + ARTICUTOOL_LENGTH_M) - # Orientation looking down with some variability - r = R.from_euler("y", np.deg2rad(-90 + np.random.uniform(-15, 15))) - q = r.as_quat() - pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) - return pose + """ + Calculates a camera pose for the Jaco end-effector that looks at the food. + + The position is sampled from a spherical cap above the food pose, + and the orientation is constrained to look at the food with no roll. + """ + # --- Position Calculation using Spherical Coordinates --- + # 1. Define the spherical coordinate parameters + radial_distance = 0.3 # Constant distance from the food + + # Azimuthal angle (around Z-axis): sample from a full circle + azimuthal_angle = np.random.uniform(0, 2 * np.pi) + + # Polar angle (from Z-axis): 0 is directly above, up to 45 degrees away + polar_angle = np.random.uniform(0, np.deg2rad(45)) + + # 2. Calculate the position offset in a frame aligned with the world + x_offset = radial_distance * np.sin(polar_angle) * np.cos(azimuthal_angle) + y_offset = radial_distance * np.sin(polar_angle) * np.sin(azimuthal_angle) + z_offset = radial_distance * np.cos(polar_angle) + + # 3. Calculate the final camera position relative to the food pose + food_position = np.array( + [food_pose.position.x, food_pose.position.y, food_pose.position.z] + ) + camera_position = food_position + np.array([x_offset, y_offset, z_offset]) + + # --- Orientation Calculation (Look-at with no roll) --- + # 1. The end-effector's Z-axis must point from its position to the food's origin. + z_axis = food_position - camera_position + z_axis /= np.linalg.norm(z_axis) + + # 2. The end-effector's Y-axis should be aligned with the world's "up" to prevent roll. + world_up = np.array([0.0, 0.0, 1.0]) + + # 3. Calculate the X-axis (left) and handle the singularity when looking straight down. + if np.abs(np.dot(z_axis, world_up)) > 0.999: + # Looking straight down, the cross product is ill-defined. + # We can define the camera's "left" (X-axis) to be the world's Y-axis in this case. + x_axis = np.array([0.0, 1.0, 0.0]) + else: + x_axis = np.cross(world_up, z_axis) + x_axis /= np.linalg.norm(x_axis) + + # 4. Re-calculate the Y-axis to ensure the frame is perfectly orthonormal. + y_axis = np.cross(z_axis, x_axis) + + # 5. Construct the final rotation matrix from the basis vectors. + # Jaco EE frame convention: [x_left, y_up, z_forward] + rotation_matrix = np.array([x_axis, y_axis, z_axis]).T + rotation = R.from_matrix(rotation_matrix) + quat = rotation.as_quat() + + # Create and return the final Pose message + final_pose = Pose() + final_pose.position = Point( + x=camera_position[0], y=camera_position[1], z=camera_position[2] + ) + final_pose.orientation = Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) + + return final_pose def _calculate_move_above_pose(self, food_pose: Pose) -> Pose: """Calculate the pre-acquisition pose based on ADA action schema.""" From 43c206138135047c8f1836d40ebbd30a087be19f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 9 Aug 2025 12:49:54 -0700 Subject: [PATCH 172/238] Add logic to sample pose within a cylindrical shell instead of a spherical shell so that we can define an effective distance from the robot base's vertical axis rather than strictly the origin --- .../scripts/end_to_end_feeding_benchmark.py | 60 ++++++++++++++++++- 1 file changed, 58 insertions(+), 2 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index cba39774..c1d832be 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -254,8 +254,18 @@ def _generate_scene(self, base_z_offset: float) -> Dict[str, Any]: scene = {"base_z_offset": base_z_offset} # Sample food and mouth poses - scene["food_pose"] = self._sample_pose_in_spherical_shell() - scene["mouth_pose"] = self._sample_pose_in_spherical_shell() + scene["food_pose"] = self._sample_pose_in_cylindrical_shell( + inner_radius=0.4, + outer_radius=0.7, + min_height=0.0, + max_height=0.3, + ) + scene["mouth_pose"] = self._sample_pose_in_cylindrical_shell( + inner_radius=0.3, + outer_radius=0.6, + min_height=0.0, + max_height=0.6, + ) # Define derived poses scene["home_config"] = [-1.47568, 2.92779, 1.00845, -2.0847, 1.43588, 1.32575] @@ -269,6 +279,52 @@ def _generate_scene(self, base_z_offset: float) -> Dict[str, Any]: return scene + def _sample_pose_in_cylindrical_shell( + self, + inner_radius: float, + outer_radius: float, + min_height: float, + max_height: float, + ) -> Pose: + """ + Samples a random pose within a cylindrical shell + + The frame's Z-axis is constrained to be world up, and its X-axis is + oriented to point towards the robot base with some random variability. + """ + # --- Position Sampling in a Cylindrical Shell --- + # 1. Sample the radius and angle + radius = np.sqrt(np.random.uniform(inner_radius**2, outer_radius**2)) + theta = np.random.uniform(0, 2 * np.pi) + + # 2. Convert to Cartesian coordinates + x = radius * np.cos(theta) + y = radius * np.sin(theta) + z = np.random.uniform(min_height, max_height) + position = Point(x=x, y=y, z=z) + + # --- Orientation Calculation --- + z_axis = np.array([0.0, 0.0, 1.0]) + look_at_vector = -np.array([x, y, 0.0]) # Project to XY plane + if np.linalg.norm(look_at_vector) < 1e-6: + look_at_vector = np.array([1.0, 0.0, 0.0]) + x_axis_direction = look_at_vector / np.linalg.norm(look_at_vector) + + y_axis = np.cross(z_axis, x_axis_direction) + rotation_matrix = np.array([x_axis_direction, y_axis, z_axis]).T + main_rot = R.from_matrix(rotation_matrix) + + rand_yaw_angle = np.random.uniform(-np.deg2rad(30), np.deg2rad(30)) + variability_rot = R.from_euler("z", rand_yaw_angle) + + final_rot = main_rot * variability_rot + quat = final_rot.as_quat() + + return Pose( + position=position, + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), + ) + def _sample_pose_in_spherical_shell(self) -> Pose: """ Samples a random pose within a spherical shell in front of the robot. From f1200937012940f48b39d28bcf893e04a9db77e6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 9 Aug 2025 13:57:14 -0700 Subject: [PATCH 173/238] Adjust logic for _calculate_above_plate_pose to determine the direction from the robot base to the food frame such that the Jaco end-effector looks at the food, but only permits a constrained yaw deviation relative to a pose that aligns the end-effector's z-axis with the vector from the robot's base frame to the food --- .../scripts/end_to_end_feeding_benchmark.py | 41 +++++++++---------- 1 file changed, 20 insertions(+), 21 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index c1d832be..4746ec86 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -381,30 +381,31 @@ def _sample_pose_in_spherical_shell(self) -> Pose: def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: """ - Calculates a camera pose for the Jaco end-effector that looks at the food. - - The position is sampled from a spherical cap above the food pose, - and the orientation is constrained to look at the food with no roll. + Calculates a camera pose for the Jaco end-effector that looks at the food, + with a yaw constraint that keeps the arm aligned with the robot base. """ - # --- Position Calculation using Spherical Coordinates --- - # 1. Define the spherical coordinate parameters - radial_distance = 0.3 # Constant distance from the food + food_position = np.array( + [food_pose.position.x, food_pose.position.y, food_pose.position.z] + ) - # Azimuthal angle (around Z-axis): sample from a full circle - azimuthal_angle = np.random.uniform(0, 2 * np.pi) + # --- Position Calculation with Yaw Constraint --- + # 1. Determine the base yaw angle from the robot's origin to the food's XY position. + base_yaw_angle = np.arctan2(food_position[1], food_position[0]) - # Polar angle (from Z-axis): 0 is directly above, up to 45 degrees away - polar_angle = np.random.uniform(0, np.deg2rad(45)) + # 2. Add random variability to this base angle. + yaw_variability = np.random.uniform(-np.deg2rad(45), np.deg2rad(45)) + final_azimuthal_angle = base_yaw_angle + yaw_variability + np.pi - # 2. Calculate the position offset in a frame aligned with the world - x_offset = radial_distance * np.sin(polar_angle) * np.cos(azimuthal_angle) - y_offset = radial_distance * np.sin(polar_angle) * np.sin(azimuthal_angle) + # 3. Use spherical coordinates relative to the food pose to find the camera position. + radial_distance = 0.3 # Constant distance from the food + polar_angle = np.random.uniform(0, np.deg2rad(45)) # Angle from vertical + + # Calculate the offset from the food pose. + x_offset = radial_distance * np.sin(polar_angle) * np.cos(final_azimuthal_angle) + y_offset = radial_distance * np.sin(polar_angle) * np.sin(final_azimuthal_angle) z_offset = radial_distance * np.cos(polar_angle) - # 3. Calculate the final camera position relative to the food pose - food_position = np.array( - [food_pose.position.x, food_pose.position.y, food_pose.position.z] - ) + # The final camera position is the food position plus this offset. camera_position = food_position + np.array([x_offset, y_offset, z_offset]) # --- Orientation Calculation (Look-at with no roll) --- @@ -417,8 +418,7 @@ def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: # 3. Calculate the X-axis (left) and handle the singularity when looking straight down. if np.abs(np.dot(z_axis, world_up)) > 0.999: - # Looking straight down, the cross product is ill-defined. - # We can define the camera's "left" (X-axis) to be the world's Y-axis in this case. + # Looking straight down, define "left" relative to the world frame. x_axis = np.array([0.0, 1.0, 0.0]) else: x_axis = np.cross(world_up, z_axis) @@ -428,7 +428,6 @@ def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: y_axis = np.cross(z_axis, x_axis) # 5. Construct the final rotation matrix from the basis vectors. - # Jaco EE frame convention: [x_left, y_up, z_forward] rotation_matrix = np.array([x_axis, y_axis, z_axis]).T rotation = R.from_matrix(rotation_matrix) quat = rotation.as_quat() From c5f3e7f85192842799fb99edc1199b37476c0641 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 9 Aug 2025 21:03:39 -0700 Subject: [PATCH 174/238] Add classes for semantic acquisition schema --- .../scripts/end_to_end_feeding_benchmark.py | 41 ++++++++++++++++++- 1 file changed, 40 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 4746ec86..cc7d2c81 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -23,7 +23,8 @@ import subprocess import sys import argparse -from enum import Enum +from enum import Enum, auto +from dataclasses import dataclass # Third-party imports import numpy as np @@ -73,6 +74,44 @@ class TrialStatus(Enum): SKIPPED = "Skipped" +# --- Semantic Schema for Acquisition Actions --- +class AcquisitionStrategy(Enum): + """High-level choice of physical interaction""" + + SKEWER = auto() + SCOOP = auto() + CUT = auto() + + +class MotionAxis(Enum): + """The primary axis for the arm's linear motion""" + + MAJOR_AXIS = auto() + MINOR_AXIS = auto() + VERTICAL = auto() + + +class ToolAlignment(Enum): + """How the tool is oriented relative to the food's structure""" + + PARALLEL = auto() + PERPENDICULAR = auto() + + +@dataclass +class ActionRecipe: + """Holds the semantic parameters for a single acquisition action""" + + strategy: AcquisitionStrategy + motion_axis: MotionAxis + tool_alignment: ToolAlignment + + def __str__(self): + return ( + f"{self.strategy.name}-{self.motion_axis.name}-{self.tool_alignment.name}" + ) + + class EndToEndBenchmark: """Manages the end-to-end benchmark planning process.""" From 2a3682282d6f2d951b0840f27737f3d320e8b91f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 9 Aug 2025 21:06:13 -0700 Subject: [PATCH 175/238] Remove base_z_offset until we can programmatically modify the actual offset in the planning scene --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 7 +++---- 1 file changed, 3 insertions(+), 4 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index cc7d2c81..c231b817 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -288,9 +288,9 @@ def _update_acm_callback(self, future): self.node.get_logger().error(f"Error in ACM callback: {e}") # --- Scene Generation --- - def _generate_scene(self, base_z_offset: float) -> Dict[str, Any]: + def _generate_scene(self) -> Dict[str, Any]: """Generates a randomized, robot-centric planning scene.""" - scene = {"base_z_offset": base_z_offset} + scene = {} # Sample food and mouth poses scene["food_pose"] = self._sample_pose_in_cylindrical_shell( @@ -746,11 +746,10 @@ def run(self): LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") # 1. Generate a new scene - scene = self._generate_scene(base_z_offset=0.1) + scene = self._generate_scene) LOGGER.info( f""" --- Generated Scene Parameters for Trial {i + 1} --- - - Robot Base Z Offset: {scene["base_z_offset"]:.3f}m - Initial State: - Home Config: [{", ".join(f"{j:.4f}" for j in scene["home_config"])}] From 790bcf9b0da8ed5fc3ea88e46421a432fa1913e2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 9 Aug 2025 21:06:57 -0700 Subject: [PATCH 176/238] Fix missing parentheses --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index c231b817..b6137f09 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -746,7 +746,7 @@ def run(self): LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") # 1. Generate a new scene - scene = self._generate_scene) + scene = self._generate_scene() LOGGER.info( f""" --- Generated Scene Parameters for Trial {i + 1} --- From 5fd176dbb7293f62fe0c1239fc593b8e7adc44a4 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 9 Aug 2025 22:10:48 -0700 Subject: [PATCH 177/238] Implement skewer logic --- .../scripts/end_to_end_feeding_benchmark.py | 136 +++++++++++++----- 1 file changed, 97 insertions(+), 39 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index b6137f09..afd6f318 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -309,9 +309,16 @@ def _generate_scene(self) -> Dict[str, Any]: # Define derived poses scene["home_config"] = [-1.47568, 2.92779, 1.00845, -2.0847, 1.43588, 1.32575] scene["above_plate_pose"] = self._calculate_above_plate_pose(scene["food_pose"]) - scene["move_above_pose"] = self._calculate_move_above_pose(scene["food_pose"]) - scene["move_into_pose"] = self._calculate_move_into_pose( - scene["food_pose"], scene["move_above_pose"] + scene["move_into_pose"], approach_vector = self._calculate_move_into_pose( + food_pose=scene["food_pose"], + recipe=ActionRecipe( + AcquisitionStrategy.SKEWER, + MotionAxis.VERTICAL, + ToolAlignment.PERPENDICULAR, + ), + ) + scene["move_above_pose"] = self._calculate_move_above_pose( + move_into_pose=scene["move_into_pose"], approach_vector=approach_vector ) scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) scene["resting_pose"] = Pose(position=Point(x=0.4, y=-0.4, z=0.3)) @@ -480,45 +487,96 @@ def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: return final_pose - def _calculate_move_above_pose(self, food_pose: Pose) -> Pose: - """Calculate the pre-acquisition pose based on ADA action schema.""" - # Parameterize with approach vector (polar and azimuthal angles) - polar_angle = np.random.uniform(np.deg2rad(30), np.deg2rad(60)) - azimuthal_angle = np.random.uniform(-np.deg2rad(45), np.deg2rad(45)) - offset_dist = 0.1 - - # Calculate offset in a frame aligned with the food pose - x_offset = offset_dist * np.sin(polar_angle) * np.cos(azimuthal_angle) - y_offset = offset_dist * np.sin(polar_angle) * np.sin(azimuthal_angle) - z_offset = offset_dist * np.cos(polar_angle) - - # Create rotation from angles - rot = R.from_euler("zy", [-azimuthal_angle, -polar_angle]) - - # Apply this transform to the food pose - food_rot = R.from_quat( - [ - food_pose.orientation.x, - food_pose.orientation.y, - food_pose.orientation.z, - food_pose.orientation.w, - ] - ) - final_rot = food_rot * rot - q = final_rot.as_quat() + # --- Semantic Pose Calculation --- + def _calculate_move_into_pose( + self, food_pose: Pose, recipe: ActionRecipe + ) -> Tuple[Pose, np.ndarray]: + """ + Calculates the MoveInto tool tip pose based on a semantic ActionRecipe. + Returns the pose and the calculated approach vector (the tool's Z-axis). + """ + # Default to identity rotation and a vertical approach vector + final_rotation = R.identity() + approach_vector = np.array([0.0, 0.0, -1.0]) + + # --- SKEWER Strategy Logic --- + if recipe.strategy == AcquisitionStrategy.SKEWER: + # Get the food's principal axes from its orientation + food_orientation = R.from_quat( + [ + food_pose.orientation.x, + food_pose.orientation.y, + food_pose.orientation.z, + food_pose.orientation.w, + ] + ) + major_axis_food = food_orientation.apply( + [1.0, 0.0, 0.0] + ) # Food's X (longer) + minor_axis_food = food_orientation.apply( + [0.0, 1.0, 0.0] + ) # Food's Y (shorter) + + # 1. Align Tines: The tool's X-axis (tines) must align with the + # food's minor axis for a stable skewer. + tool_x_final = minor_axis_food + + # 2. Define Approach & Tilt: The approach is along the food's major axis, + # tilted by a random polar angle. We find the tool's Z-axis by rotating + # a downward vector around the tool's new X-axis. + sampled_polar_angle = np.random.uniform( + 0, np.deg2rad(60) + ) # Angle from vertical + rotation = R.from_rotvec(sampled_polar_angle * tool_x_final) + tool_z_final = rotation.apply( + np.array([0.0, 0.0, -1.0]) + ) # Rotate a downward vector + + # 3. Complete the Frame: The tool's Y-axis is derived from the cross product. + tool_y_final = np.cross(tool_z_final, tool_x_final) + + # 4. The final rotation is constructed from these basis vectors. + rotation_matrix = np.array([tool_x_final, tool_y_final, tool_z_final]).T + final_rotation = R.from_matrix(rotation_matrix) + approach_vector = tool_z_final + + # --- SCOOP Strategy Logic (Placeholder) --- + elif recipe.strategy == AcquisitionStrategy.SCOOP: + # This logic will be implemented next. + base_rotation = R.from_euler("y", -60, degrees=True) + final_rotation = base_rotation + # TODO: Add alignment logic based on food's principal axes. + + # --- Construct the final pose --- + q = final_rotation.as_quat() + move_into_pose = Pose() + move_into_pose.position = food_pose.position + move_into_pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + + return move_into_pose, approach_vector + + def _calculate_move_above_pose( + self, move_into_pose: Pose, approach_vector: np.ndarray + ) -> Pose: + """ + Calculates the MoveAbove pose by offsetting from MoveInto along the + calculated approach vector. + """ + # 1. Inherit the orientation directly from the target pose + final_orientation = move_into_pose.orientation - p = food_pose.position - final_pos = Point(x=p.x - x_offset, y=p.y - y_offset, z=p.z + z_offset) + # 2. The motion vector for the linear path IS the approach vector (tool's Z-axis) + motion_vector = approach_vector - return Pose( - position=final_pos, orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) - ) + # 3. Calculate the offset position + offset_dist = 0.1 # 10 cm + p = move_into_pose.position + # We add the offset because the approach_vector is already pointing "down". + # To get the "above" pose, we move in the opposite direction of the approach. + offset = motion_vector * -offset_dist + final_position = Point(x=p.x + offset[0], y=p.y + offset[1], z=p.z + offset[2]) - def _calculate_move_into_pose(self, food_pose: Pose, move_above_pose: Pose) -> Pose: - """The MoveInto pose has the same orientation as MoveAbove.""" - return Pose( - position=food_pose.position, orientation=move_above_pose.orientation - ) + return Pose(position=final_position, orientation=final_orientation) def _calculate_staging_pose(self, mouth_pose: Pose) -> Pose: """Calculate the staging pose relative to the mouth.""" From 57196557f80494a8368c9bfabc7e0fe426557260 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 10 Aug 2025 09:16:48 -0700 Subject: [PATCH 178/238] Add stage for AbovePlate to MoveAbove configurations --- .../scripts/end_to_end_feeding_benchmark.py | 88 ++++++++++++++++++- 1 file changed, 87 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index afd6f318..51e3c696 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -48,7 +48,7 @@ PLANNING_GROUP_ATOOL = "articutool" PLANNING_GROUP_FULL = "jaco_arm_with_articutool" JOINT_NAMES_JACO = [f"j2n6s200_joint_{i + 1}" for i in range(6)] -JOINT_NAMES_ATOOL = [f"atool_joint_{i + 1}" for i in range(2)] +JOINT_NAMES_ATOOL = [f"atool_joint{i + 1}" for i in range(2)] JOINT_NAMES_FULL = JOINT_NAMES_JACO + JOINT_NAMES_ATOOL BASE_LINK_JACO = "j2n6s200_link_base" BASE_LINK_ATOOL = "atool_link_base" @@ -722,6 +722,67 @@ def _serialize_trajectory( } # --- Planning Primitive Placeholders --- + def _plan_to_joint_config( + self, + moveit_planner: MoveIt2, + target_config: List[float], + start_config: List[float], + ) -> Optional[JointTrajectory]: + """Plans a joint-space trajectory for a given planner and target.""" + moveit_planner.set_joint_goal(target_config) + future = moveit_planner.plan_async(start_joint_state=start_config) + rclpy.spin_until_future_complete( + self.node, future, timeout_sec=self.planning_timeout + ) + return moveit_planner.get_trajectory(future) + + def _plan_to_move_above( + self, + move_above_pose: Pose, + start_state_jaco: List[float], + start_state_atool: List[float], + ) -> Tuple[TrialStatus, Optional[JointTrajectory], Optional[JointTrajectory]]: + """ + Plans to the MoveAbove configuration by solving 8-DOF IK and then + planning for each subgroup separately. + """ + LOGGER.info(" Solving 8-DOF IK for MoveAbove pose...") + start_state_full = start_state_jaco + start_state_atool + + # 1. Solve 8-DOF IK for the target pose + ik_solution = self.moveit2_full.compute_ik( + position=move_above_pose.position, + quat_xyzw=move_above_pose.orientation, + start_joint_state=start_state_full, + ) + + if not ik_solution: + LOGGER.warning(" IK solution for 8-DOF system not found.") + return TrialStatus.IK_FAILURE, None, None + + LOGGER.info(" 8-DOF IK solution found. Planning for subgroups.") + target_jaco_config = ik_solution[:6] + target_atool_config = ik_solution[6:] + + # 2. Plan for Jaco arm (6-DOF) + traj_jaco = self._plan_to_joint_config( + self.moveit2_jaco, target_jaco_config, start_state_jaco + ) + if not traj_jaco or not traj_jaco.points: + LOGGER.warning(" Jaco arm planning failed.") + return TrialStatus.PLANNER_FAILURE, None, None + + # 3. Plan for Articutool wrist (2-DOF) + traj_atool = self._plan_to_joint_config( + self.moveit2_atool, target_atool_config, start_state_atool + ) + if not traj_atool or not traj_atool.points: + LOGGER.warning(" Articutool planning failed.") + return TrialStatus.PLANNER_FAILURE, traj_jaco, None + + LOGGER.info(" Subgroup planning successful.") + return TrialStatus.SUCCESS, traj_jaco, traj_atool + def _plan_s1_unconstrained( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: @@ -843,6 +904,7 @@ def run(self): # 2. Simulate the feeding cycle state machine food_on_tool = False current_jaco_state = scene["home_config"] + current_atool_state = [0.0, 0.0] # Assume atool starts at zero # --- Stage 1: Home -> AbovePlate (P1 with S1) --- LOGGER.info("Stage 1: Home -> AbovePlate") @@ -914,6 +976,30 @@ def run(self): # Update state for the next stage current_jaco_state = list(trajectory.points[-1].positions) + LOGGER.info(" Stage 1 successful.") + + # --- Stage 2: AbovePlate -> MoveAbove (P2 with SX) --- + LOGGER.info("Stage 2: AbovePlate -> MoveAbove") + + # Plan to the MoveAbove configuration + status, traj_jaco_to_above, traj_atool_to_above = self._plan_to_move_above( + scene["move_above_pose"], current_jaco_state, current_atool_state + ) + + # Log the results for this stage and recipe + self.results.append( + { + "trial_id": i, + "stage": "AbovePlateToMoveAbove", + "status": status.value, + } + ) + + if status != TrialStatus.SUCCESS: + LOGGER.error( + f" Stage 2 failed with status: {status.value}. Skipping trial." + ) + continue self.save_results() From def4076b0682ef446f6e3df73bfd5bab3e1eeec0 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sun, 10 Aug 2025 09:42:38 -0700 Subject: [PATCH 179/238] Remove unused IK solve for planning from Home to AbovePlate --- .../scripts/end_to_end_feeding_benchmark.py | 37 ++----------------- 1 file changed, 3 insertions(+), 34 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 51e3c696..c1a4a295 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -909,38 +909,9 @@ def run(self): # --- Stage 1: Home -> AbovePlate (P1 with S1) --- LOGGER.info("Stage 1: Home -> AbovePlate") - # --- IK with Retries --- - max_ik_attempts = 5 - ik_attempts = 0 - above_plate_config = None - for attempt in range(max_ik_attempts): - ik_attempts += 1 - LOGGER.info( - f" Attempting IK solve ({ik_attempts}/{max_ik_attempts})..." - ) - - # Using moveit2_jaco for IK as per your latest code - config = self.moveit2_jaco.compute_ik( - position=scene["above_plate_pose"].position, - quat_xyzw=scene["above_plate_pose"].orientation, - start_joint_state=current_jaco_state, - ) - - if config: - above_plate_config = config - LOGGER.info(f" IK solution found on attempt {ik_attempts}.") - break - else: - LOGGER.warning(f" IK attempt {ik_attempts} failed.") - - # --- Planning --- - status = TrialStatus.IK_FAILURE # Default status - trajectory = None - if above_plate_config: - # IK succeeded, now attempt to plan - status, trajectory = self._plan_s1_unconstrained( - scene["above_plate_pose"], current_jaco_state - ) + status, trajectory = self._plan_s1_unconstrained( + scene["above_plate_pose"], current_jaco_state + ) # Create a dictionary of all generated poses for this trial scene_poses = { @@ -961,8 +932,6 @@ def run(self): "stage": "HomeToAbovePlate", "primitive": "S1", "status": status.value, - "ik_attempts": ik_attempts, - # Serialize the trajectory for visualization, will be None on failure "trajectory": self._serialize_trajectory(trajectory), "scene_poses": scene_poses, } From 2ced65d25667a06c144be87d5c186b4aa3190c98 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Aug 2025 19:50:41 -0700 Subject: [PATCH 180/238] Add thread locking for MoveIt2 calls to prevent deadlock conditions. Also implement joint mapping logic for IK solutions --- .../scripts/end_to_end_feeding_benchmark.py | 136 ++++++++++++------ 1 file changed, 90 insertions(+), 46 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index c1a4a295..46073f16 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -16,7 +16,7 @@ from datetime import datetime import os import time -from threading import Thread +from threading import Thread, Lock from typing import Optional, List, Dict, Tuple, Any import json import math @@ -135,6 +135,7 @@ def __init__( self.planning_timeout = planning_timeout self.output_dir = output_dir self.results: List[Dict[str, Any]] = [] + self.moveit2_lock = Lock() # Pinocchio model for feasibility checks self.pinocchio_model: Optional[pin.Model] = None @@ -729,11 +730,26 @@ def _plan_to_joint_config( start_config: List[float], ) -> Optional[JointTrajectory]: """Plans a joint-space trajectory for a given planner and target.""" - moveit_planner.set_joint_goal(target_config) - future = moveit_planner.plan_async(start_joint_state=start_config) - rclpy.spin_until_future_complete( - self.node, future, timeout_sec=self.planning_timeout - ) + + future = None + # --- Lock the MoveIt2 interaction --- + with self.moveit2_lock: + moveit_planner.set_joint_goal(target_config) + future = moveit_planner.plan_async(start_joint_state=start_config) + + # The lock is released here, while we wait for the future to complete. + # This is safe because the action goal has already been sent. + + start_time = time.time() + while rclpy.ok() and not future.done(): + if time.time() - start_time > self.planning_timeout: + LOGGER.error("Planning timed out.") + # We don't need to re-acquire the lock to cancel + future.cancel() + return None + time.sleep(0.1) + + # No lock needed to get the result return moveit_planner.get_trajectory(future) def _plan_to_move_above( @@ -748,23 +764,33 @@ def _plan_to_move_above( """ LOGGER.info(" Solving 8-DOF IK for MoveAbove pose...") start_state_full = start_state_jaco + start_state_atool - - # 1. Solve 8-DOF IK for the target pose - ik_solution = self.moveit2_full.compute_ik( - position=move_above_pose.position, - quat_xyzw=move_above_pose.orientation, - start_joint_state=start_state_full, - ) + ik_solution = None + + with self.moveit2_lock: + # 1. Solve 8-DOF IK for the target pose + ik_solution = self.moveit2_full.compute_ik( + position=move_above_pose.position, + quat_xyzw=move_above_pose.orientation, + start_joint_state=start_state_full, + ) if not ik_solution: LOGGER.warning(" IK solution for 8-DOF system not found.") return TrialStatus.IK_FAILURE, None, None - LOGGER.info(" 8-DOF IK solution found. Planning for subgroups.") - target_jaco_config = ik_solution[:6] - target_atool_config = ik_solution[6:] + LOGGER.info( + f" 8-DOF IK solution found ({ik_solution}. Planning for subgroups." + ) + + # 1. Create a dictionary mapping joint names to their solved positions. + solution_map = dict(zip(ik_solution.name, ik_solution.position)) + + # 2. Build the target list for the Jaco arm by looking up each required joint. + target_jaco_config = [solution_map[name] for name in JOINT_NAMES_JACO] + + # 3. Build the target list for the Articutool wrist. + target_atool_config = [solution_map[name] for name in JOINT_NAMES_ATOOL] - # 2. Plan for Jaco arm (6-DOF) traj_jaco = self._plan_to_joint_config( self.moveit2_jaco, target_jaco_config, start_state_jaco ) @@ -772,7 +798,7 @@ def _plan_to_move_above( LOGGER.warning(" Jaco arm planning failed.") return TrialStatus.PLANNER_FAILURE, None, None - # 3. Plan for Articutool wrist (2-DOF) + # 4. Plan for Articutool wrist (2-DOF) traj_atool = self._plan_to_joint_config( self.moveit2_atool, target_atool_config, start_state_atool ) @@ -787,15 +813,23 @@ def _plan_s1_unconstrained( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") - self.moveit2_jaco.clear_goal_constraints() - self.moveit2_jaco.clear_path_constraints() - self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) + future = None + # --- Lock the MoveIt2 interaction --- + with self.moveit2_lock: + self.moveit2_jaco.clear_goal_constraints() + self.moveit2_jaco.clear_path_constraints() + self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) + future = self.moveit2_jaco.plan_async(start_joint_state=start_state) + + # Wait for future outside the lock + start_time = time.time() + while rclpy.ok() and not future.done(): + if time.time() - start_time > self.planning_timeout: + future.cancel() + return TrialStatus.PLANNER_FAILURE, None + time.sleep(0.1) - future = self.moveit2_jaco.plan_async(start_joint_state=start_state) - rclpy.spin_until_future_complete( - self.node, future, timeout_sec=self.planning_timeout - ) traj = self.moveit2_jaco.get_trajectory(future) if not traj or not traj.points: @@ -807,28 +841,38 @@ def _plan_s2_guided( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: LOGGER.info(" Planning with S2 (6-DOF Guided)...") - self.moveit2_jaco.clear_goal_constraints() - self.moveit2_jaco.clear_path_constraints() - - self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) - self.moveit2_jaco.set_path_orientation_constraint( - quat_xyzw=Quaternion( - x=PATH_CONSTRAINT_QUAT_XYZW[0], - y=PATH_CONSTRAINT_QUAT_XYZW[1], - z=PATH_CONSTRAINT_QUAT_XYZW[2], - w=PATH_CONSTRAINT_QUAT_XYZW[3], - ), - target_link=END_EFFECTOR_LINK_JACO, - tolerance=PATH_CONSTRAINT_TOLERANCE_XYZ_RAD, - weight=1.0, - ) - future = self.moveit2_jaco.plan_async( - start_joint_state=start_state, - ) - rclpy.spin_until_future_complete( - self.node, future, timeout_sec=self.planning_timeout - ) + future = None + # --- Lock the entire MoveIt2 configuration and planning block --- + with self.moveit2_lock: + self.moveit2_jaco.clear_goal_constraints() + self.moveit2_jaco.clear_path_constraints() + + self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) + self.moveit2_jaco.set_path_orientation_constraint( + quat_xyzw=Quaternion( + x=PATH_CONSTRAINT_QUAT_XYZW[0], + y=PATH_CONSTRAINT_QUAT_XYZW[1], + z=PATH_CONSTRAINT_QUAT_XYZW[2], + w=PATH_CONSTRAINT_QUAT_XYZW[3], + ), + target_link=END_EFFECTOR_LINK_JACO, + tolerance=PATH_CONSTRAINT_TOLERANCE_XYZ_RAD, + weight=1.0, + ) + + future = self.moveit2_jaco.plan_async( + start_joint_state=start_state, + ) + + # Wait for future outside the lock + start_time = time.time() + while rclpy.ok() and not future.done(): + if time.time() - start_time > self.planning_timeout: + future.cancel() + return TrialStatus.PLANNER_FAILURE, None, 0.0 + time.sleep(0.1) + traj = self.moveit2_jaco.get_trajectory(future) if not traj or not traj.points: From b18ef27f554832bcfe6626a274b2e768e6feb2dc Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Aug 2025 20:17:37 -0700 Subject: [PATCH 181/238] Refactor to introduce MotionPlanner class that abstracts the MoveIt2 API for the purposes of this benchmark --- .../scripts/end_to_end_feeding_benchmark.py | 206 +++++++++++------- 1 file changed, 123 insertions(+), 83 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 46073f16..0ee3c662 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -35,6 +35,7 @@ from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory from geometry_msgs.msg import Pose, Point, Quaternion +from sensor_msgs.msg import JointState from scipy.spatial.transform import Rotation as R import pinocchio as pin from moveit_msgs.msg import PlanningScene, AllowedCollisionEntry, AllowedCollisionMatrix @@ -112,6 +113,99 @@ def __str__(self): ) +class MotionPlanner: + """A wrapper for MoveIt2 to provide a seamless, synchronous API for planning.""" + + def __init__( + self, + node: Node, + moveit2_jaco: MoveIt2, + moveit2_atool: MoveIt2, + moveit2_full: MoveIt2, + planning_timeout: float, + ): + self._node = node + self._planning_timeout = planning_timeout + + # Manages all moveit2 objects and the shared lock + self._moveit2_objects = { + PLANNING_GROUP_JACO: moveit2_jaco, + PLANNING_GROUP_ATOOL: moveit2_atool, + PLANNING_GROUP_FULL: moveit2_full, + } + self._lock = Lock() + + def _get_planner(self, group_name: str) -> MoveIt2: + """Selects the correct moveit2 object based on group name.""" + if group_name not in self._moveit2_objects: + raise ValueError(f"Unknown planning group: {group_name}") + return self._moveit2_objects[group_name] + + def compute_ik( + self, group_name: str, target_pose: Pose, start_joint_state: List[float] + ) -> Optional[JointState]: + """ + Computes Inverse Kinematics for a given group and target pose. + + Returns: + A JointState message on success, None on failure. + """ + planner = self._get_planner(group_name) + ik_solution = None + + with self._lock: + ik_solution = planner.compute_ik( + position=target_pose.position, + quat_xyzw=target_pose.orientation, + start_joint_state=start_joint_state, + ) + + return ik_solution + + def plan( + self, + group_name: str, + goal_pose: Optional[Pose] = None, + goal_joints: Optional[List[float]] = None, + start_state: Optional[List[float]] = None, + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + """ + Plans a trajectory for a given group to a target pose or joint state. + + Returns: + A status and the resulting trajectory, or None on failure. + """ + planner = self._get_planner(group_name) + future = None + + with self._lock: + planner.clear_goal_constraints() + planner.clear_path_constraints() + + if goal_pose: + planner.set_pose_goal(goal_pose, planner.end_effector_name) + elif goal_joints: + planner.set_joint_goal(goal_joints) + else: + return TrialStatus.IK_FAILURE, None # No goal provided + + future = planner.plan_async(start_joint_state=start_state) + + # Wait for the future to complete outside the lock + start_time = time.time() + while rclpy.ok() and not future.done(): + if time.time() - start_time > self._planning_timeout: + future.cancel() + return TrialStatus.PLANNER_FAILURE, None + time.sleep(0.1) + + traj = planner.get_trajectory(future) + if not traj or not traj.points: + return TrialStatus.PLANNER_FAILURE, None + + return TrialStatus.SUCCESS, traj + + class EndToEndBenchmark: """Manages the end-to-end benchmark planning process.""" @@ -135,7 +229,9 @@ def __init__( self.planning_timeout = planning_timeout self.output_dir = output_dir self.results: List[Dict[str, Any]] = [] - self.moveit2_lock = Lock() + self.motion_planner = MotionPlanner( + node, moveit2_jaco, moveit2_atool, moveit2_full, planning_timeout + ) # Pinocchio model for feasibility checks self.pinocchio_model: Optional[pin.Model] = None @@ -723,119 +819,63 @@ def _serialize_trajectory( } # --- Planning Primitive Placeholders --- - def _plan_to_joint_config( - self, - moveit_planner: MoveIt2, - target_config: List[float], - start_config: List[float], - ) -> Optional[JointTrajectory]: - """Plans a joint-space trajectory for a given planner and target.""" - - future = None - # --- Lock the MoveIt2 interaction --- - with self.moveit2_lock: - moveit_planner.set_joint_goal(target_config) - future = moveit_planner.plan_async(start_joint_state=start_config) - - # The lock is released here, while we wait for the future to complete. - # This is safe because the action goal has already been sent. - - start_time = time.time() - while rclpy.ok() and not future.done(): - if time.time() - start_time > self.planning_timeout: - LOGGER.error("Planning timed out.") - # We don't need to re-acquire the lock to cancel - future.cancel() - return None - time.sleep(0.1) - - # No lock needed to get the result - return moveit_planner.get_trajectory(future) - def _plan_to_move_above( self, move_above_pose: Pose, start_state_jaco: List[float], start_state_atool: List[float], ) -> Tuple[TrialStatus, Optional[JointTrajectory], Optional[JointTrajectory]]: - """ - Plans to the MoveAbove configuration by solving 8-DOF IK and then - planning for each subgroup separately. - """ LOGGER.info(" Solving 8-DOF IK for MoveAbove pose...") - start_state_full = start_state_jaco + start_state_atool - ik_solution = None - with self.moveit2_lock: - # 1. Solve 8-DOF IK for the target pose - ik_solution = self.moveit2_full.compute_ik( - position=move_above_pose.position, - quat_xyzw=move_above_pose.orientation, - start_joint_state=start_state_full, - ) + # 1. Compute IK using the planner + ik_solution = self.motion_planner.compute_ik( + group_name=PLANNING_GROUP_FULL, + target_pose=move_above_pose, + start_joint_state=(start_state_jaco + start_state_atool), + ) if not ik_solution: LOGGER.warning(" IK solution for 8-DOF system not found.") return TrialStatus.IK_FAILURE, None, None - LOGGER.info( - f" 8-DOF IK solution found ({ik_solution}. Planning for subgroups." - ) - - # 1. Create a dictionary mapping joint names to their solved positions. + LOGGER.info(f" 8-DOF IK solution found. Planning for subgroups.") solution_map = dict(zip(ik_solution.name, ik_solution.position)) - - # 2. Build the target list for the Jaco arm by looking up each required joint. target_jaco_config = [solution_map[name] for name in JOINT_NAMES_JACO] - - # 3. Build the target list for the Articutool wrist. target_atool_config = [solution_map[name] for name in JOINT_NAMES_ATOOL] - traj_jaco = self._plan_to_joint_config( - self.moveit2_jaco, target_jaco_config, start_state_jaco + # 2. Plan for the Jaco arm + status_jaco, traj_jaco = self.motion_planner.plan( + group_name=PLANNING_GROUP_JACO, + goal_joints=target_jaco_config, + start_state=start_state_jaco, ) - if not traj_jaco or not traj_jaco.points: + + if status_jaco != TrialStatus.SUCCESS: LOGGER.warning(" Jaco arm planning failed.") return TrialStatus.PLANNER_FAILURE, None, None - # 4. Plan for Articutool wrist (2-DOF) - traj_atool = self._plan_to_joint_config( - self.moveit2_atool, target_atool_config, start_state_atool + # 3. Plan for the Articutool + status_atool, traj_atool = self.motion_planner.plan( + group_name=PLANNING_GROUP_ATOOL, + goal_joints=target_atool_config, + start_state=start_state_atool, ) - if not traj_atool or not traj_atool.points: + + if status_atool != TrialStatus.SUCCESS: LOGGER.warning(" Articutool planning failed.") return TrialStatus.PLANNER_FAILURE, traj_jaco, None - LOGGER.info(" Subgroup planning successful.") return TrialStatus.SUCCESS, traj_jaco, traj_atool def _plan_s1_unconstrained( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") - - future = None - # --- Lock the MoveIt2 interaction --- - with self.moveit2_lock: - self.moveit2_jaco.clear_goal_constraints() - self.moveit2_jaco.clear_path_constraints() - self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) - future = self.moveit2_jaco.plan_async(start_joint_state=start_state) - - # Wait for future outside the lock - start_time = time.time() - while rclpy.ok() and not future.done(): - if time.time() - start_time > self.planning_timeout: - future.cancel() - return TrialStatus.PLANNER_FAILURE, None - time.sleep(0.1) - - traj = self.moveit2_jaco.get_trajectory(future) - - if not traj or not traj.points: - return TrialStatus.PLANNER_FAILURE, None - - return TrialStatus.SUCCESS, traj + return self.motion_planner.plan( + group_name=PLANNING_GROUP_JACO, + goal_pose=goal_pose, + start_state=start_state, + ) def _plan_s2_guided( self, goal_pose: Pose, start_state: Any From 962e64f3df4e20c63d2dc564b5c82185d0cfc49e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Mon, 11 Aug 2025 21:09:23 -0700 Subject: [PATCH 182/238] Add MoveIt2 constraints to MotionPlanner class --- .../scripts/end_to_end_feeding_benchmark.py | 97 ++++++++++++++++--- 1 file changed, 83 insertions(+), 14 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 0ee3c662..dd345791 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -113,6 +113,52 @@ def __str__(self): ) +class MoveIt2ConstraintType(Enum): + """Specifies the type of constraint to be applied.""" + + JOINT = "joint" + POSITION = "position" + ORIENTATION = "orientation" + POSE = "pose" + + +def create_pose_constraint( + pose: Pose, tolerance_position: float = 0.001, tolerance_orientation: float = 0.001 +) -> Tuple[MoveIt2ConstraintType, Dict]: + """Creates a standard pose goal constraint.""" + return ( + MoveIt2ConstraintType.POSE, + { + "pose": pose, + "tolerance_position": tolerance_position, + "tolerance_orientation": tolerance_orientation, + }, + ) + + +def create_orientation_path_constraint( + quat_xyzw: Tuple, tolerance_rad: Tuple +) -> Tuple[MoveIt2ConstraintType, Dict]: + """Creates an orientation path constraint, useful for keeping the tool level.""" + return ( + MoveIt2ConstraintType.ORIENTATION, + { + "quat_xyzw": Quaternion( + x=quat_xyzw[0], y=quat_xyzw[1], z=quat_xyzw[2], w=quat_xyzw[3] + ), + "tolerance": tolerance_rad, + "weight": 1.0, + }, + ) + + +def create_joint_constraint( + joint_positions: List[float], +) -> Tuple[MoveIt2ConstraintType, Dict]: + """Creates a joint goal constraint.""" + return (MoveIt2ConstraintType.JOINT, {"joint_positions": joint_positions}) + + class MotionPlanner: """A wrapper for MoveIt2 to provide a seamless, synchronous API for planning.""" @@ -165,12 +211,12 @@ def compute_ik( def plan( self, group_name: str, - goal_pose: Optional[Pose] = None, - goal_joints: Optional[List[float]] = None, - start_state: Optional[List[float]] = None, + start_state: Optional[List[float]], + goal_constraints: List[Tuple[MoveIt2ConstraintType, Dict]], + path_constraints: Optional[List[Tuple[MoveIt2ConstraintType, Dict]]] = None, ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: """ - Plans a trajectory for a given group to a target pose or joint state. + Plans a trajectory for a given group based on a list of goal and path constraints. Returns: A status and the resulting trajectory, or None on failure. @@ -178,17 +224,36 @@ def plan( planner = self._get_planner(group_name) future = None + if not goal_constraints: + LOGGER.error("Planning failed: At least one goal constraint is required.") + return TrialStatus.IK_FAILURE, None + with self._lock: planner.clear_goal_constraints() planner.clear_path_constraints() - if goal_pose: - planner.set_pose_goal(goal_pose, planner.end_effector_name) - elif goal_joints: - planner.set_joint_goal(goal_joints) - else: - return TrialStatus.IK_FAILURE, None # No goal provided - + # --- Process Goal Constraints --- + for constraint_type, kwargs in goal_constraints: + if constraint_type == MoveIt2ConstraintType.JOINT: + planner.set_joint_goal(**kwargs) + elif constraint_type == MoveIt2ConstraintType.POSITION: + planner.set_position_goal(**kwargs) + elif constraint_type == MoveIt2ConstraintType.ORIENTATION: + planner.set_orientation_goal(**kwargs) + elif constraint_type == MoveIt2ConstraintType.POSE: + planner.set_pose_goal(**kwargs) + + # --- Process Path Constraints --- + if path_constraints: + for constraint_type, kwargs in path_constraints: + if constraint_type == MoveIt2ConstraintType.JOINT: + planner.set_path_joint_constraint(**kwargs) + elif constraint_type == MoveIt2ConstraintType.POSITION: + planner.set_path_position_constraint(**kwargs) + elif constraint_type == MoveIt2ConstraintType.ORIENTATION: + planner.set_path_orientation_constraint(**kwargs) + + # --- Initiate Asynchronous Planning --- future = planner.plan_async(start_joint_state=start_state) # Wait for the future to complete outside the lock @@ -844,10 +909,11 @@ def _plan_to_move_above( target_atool_config = [solution_map[name] for name in JOINT_NAMES_ATOOL] # 2. Plan for the Jaco arm + jaco_goal_constraints = [create_joint_constraint(target_jaco_config)] status_jaco, traj_jaco = self.motion_planner.plan( group_name=PLANNING_GROUP_JACO, - goal_joints=target_jaco_config, start_state=start_state_jaco, + goal_constraints=jaco_goal_constraints, ) if status_jaco != TrialStatus.SUCCESS: @@ -855,10 +921,11 @@ def _plan_to_move_above( return TrialStatus.PLANNER_FAILURE, None, None # 3. Plan for the Articutool + atool_goal_constraints = [create_joint_constraint(target_atool_config)] status_atool, traj_atool = self.motion_planner.plan( group_name=PLANNING_GROUP_ATOOL, - goal_joints=target_atool_config, start_state=start_state_atool, + goal_constraints=atool_goal_constraints, ) if status_atool != TrialStatus.SUCCESS: @@ -871,10 +938,12 @@ def _plan_s1_unconstrained( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") + goal_constraints = [create_pose_constraint(goal_pose)] + return self.motion_planner.plan( group_name=PLANNING_GROUP_JACO, - goal_pose=goal_pose, start_state=start_state, + goal_constraints=goal_constraints, ) def _plan_s2_guided( From be0c747300d1543688d39866d485c2048ca107cb Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 12 Aug 2025 18:58:19 -0700 Subject: [PATCH 183/238] Abstract high-level trial data from granular stage data --- .../scripts/end_to_end_feeding_benchmark.py | 35 ++++++++++--------- 1 file changed, 18 insertions(+), 17 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index dd345791..10f3bb3c 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1017,7 +1017,7 @@ def run(self): for i in range(self.num_trials): LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") - # 1. Generate a new scene + # 1. Generate a new scene and create the top-level dictionary for this trial scene = self._generate_scene() LOGGER.info( f""" @@ -1053,6 +1053,19 @@ def run(self): ------------------------------------------------- """ ) + trial_data = { + "trial_id": i, + "scene_poses": { + "food_pose": self._serialize_pose(scene["food_pose"]), + "mouth_pose": self._serialize_pose(scene["mouth_pose"]), + "above_plate_pose": self._serialize_pose(scene["above_plate_pose"]), + "move_above_pose": self._serialize_pose(scene["move_above_pose"]), + "move_into_pose": self._serialize_pose(scene["move_into_pose"]), + "staging_pose": self._serialize_pose(scene["staging_pose"]), + "resting_pose": self._serialize_pose(scene["resting_pose"]), + }, + "stages": [], + } # 2. Simulate the feeding cycle state machine food_on_tool = False @@ -1066,27 +1079,14 @@ def run(self): scene["above_plate_pose"], current_jaco_state ) - # Create a dictionary of all generated poses for this trial - scene_poses = { - "food_pose": self._serialize_pose(scene["food_pose"]), - "mouth_pose": self._serialize_pose(scene["mouth_pose"]), - "above_plate_pose": self._serialize_pose(scene["above_plate_pose"]), - "move_above_pose": self._serialize_pose(scene["move_above_pose"]), - "move_into_pose": self._serialize_pose(scene["move_into_pose"]), - "staging_pose": self._serialize_pose(scene["staging_pose"]), - "resting_pose": self._serialize_pose(scene["resting_pose"]), - } - # --- Result Logging for Stage 1 --- # Consolidate all metrics for this stage into a single record. - self.results.append( + trial_data["stages"].append( { - "trial_id": i, "stage": "HomeToAbovePlate", "primitive": "S1", "status": status.value, "trajectory": self._serialize_trajectory(trajectory), - "scene_poses": scene_poses, } ) @@ -1109,9 +1109,8 @@ def run(self): ) # Log the results for this stage and recipe - self.results.append( + trial_data["stages"].append( { - "trial_id": i, "stage": "AbovePlateToMoveAbove", "status": status.value, } @@ -1123,6 +1122,8 @@ def run(self): ) continue + self.results.append(trial_data) + self.save_results() def save_results(self): From 1b11b76ec48f41b72d7c0f2ab6e7275de8eec470 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 12 Aug 2025 19:25:20 -0700 Subject: [PATCH 184/238] Add Cartesian planning --- .../scripts/end_to_end_feeding_benchmark.py | 89 ++++++++++++++++++- 1 file changed, 85 insertions(+), 4 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 10f3bb3c..650df985 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -34,7 +34,7 @@ from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node from trajectory_msgs.msg import JointTrajectory -from geometry_msgs.msg import Pose, Point, Quaternion +from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped from sensor_msgs.msg import JointState from scipy.spatial.transform import Rotation as R import pinocchio as pin @@ -42,6 +42,7 @@ from moveit_msgs.srv import GetPlanningScene from moveit_msgs.msg import CollisionObject from shape_msgs.msg import SolidPrimitive +import tf2_ros # --- Constants --- LOGGER = rclpy.logging.get_logger("end_to_end_benchmark") @@ -168,11 +169,14 @@ def __init__( moveit2_jaco: MoveIt2, moveit2_atool: MoveIt2, moveit2_full: MoveIt2, + tf_buffer: tf2_ros.Buffer, planning_timeout: float, ): self._node = node self._planning_timeout = planning_timeout + self._tf_buffer = tf_buffer + # Manages all moveit2 objects and the shared lock self._moveit2_objects = { PLANNING_GROUP_JACO: moveit2_jaco, @@ -187,6 +191,47 @@ def _get_planner(self, group_name: str) -> MoveIt2: raise ValueError(f"Unknown planning group: {group_name}") return self._moveit2_objects[group_name] + @staticmethod + def _scale_cartesian_trajectory_velocity( + traj: JointTrajectory, scale_factor: float + ): + """Scales the velocity of a Cartesian trajectory""" + for point in traj.points: + nsec = (point.time_from_start.sec * 1e9) + point.time_from_start.nanosec + nsec /= scale_factor + sec = int(math.floor(nsec / 1e9)) + point.time_from_start.sec = sec + point.time_from_start.nanosec = int(nsec - (sec * 1e9)) + for i in range(len(point.velocities)): + point.velocities[i] *= scale_factor + for i in range(len(point.accelerations)): + point.accelerations[i] *= scale_factor**2 + + def _transform_goal_to_base_link( + self, planner: MoveIt2, constraint: Tuple[MoveIt2ConstraintType, Dict] + ): + """Transforms a pose-based goal constraint to the robot's base frame.""" + constraint_type, kwargs = constraint + + # Only transform pose constraints and only if a frame_id is specified + if ( + constraint_type != MoveIt2ConstraintType.POSE + or "frame_id" not in kwargs + or kwargs["frame_id"] is None + ): + return + + pose_stamped = PoseStamped() + pose_stamped.header.frame_id = kwargs["frame_id"] + pose_stamped.pose = kwargs["pose"] + + transformed_pose = self._tf_buffer.transform( + pose_stamped, planner.base_link_name + ) + + kwargs["pose"] = transformed_pose.pose + kwargs["frame_id"] = planner.base_link_name + def compute_ik( self, group_name: str, target_pose: Pose, start_joint_state: List[float] ) -> Optional[JointState]: @@ -214,6 +259,10 @@ def plan( start_state: Optional[List[float]], goal_constraints: List[Tuple[MoveIt2ConstraintType, Dict]], path_constraints: Optional[List[Tuple[MoveIt2ConstraintType, Dict]]] = None, + cartesian: bool = False, + cartesian_max_step: float = 0.005, + cartesian_jump_threshold: float = 0.0, + cartesian_fraction_threshold: float = 0.9, ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: """ Plans a trajectory for a given group based on a list of goal and path constraints. @@ -232,6 +281,17 @@ def plan( planner.clear_goal_constraints() planner.clear_path_constraints() + # If cartesian, transform pose goals to the base link frame + if cartesian: + try: + for constraint in goal_constraints: + self._transform_goal_to_base_link(planner, constraint) + except Exception as e: + LOGGER.error( + f"Failed to transform Cartesian goal to base link: {e}" + ) + return TrialStatus.IK_FAILURE, None + # --- Process Goal Constraints --- for constraint_type, kwargs in goal_constraints: if constraint_type == MoveIt2ConstraintType.JOINT: @@ -254,7 +314,11 @@ def plan( planner.set_path_orientation_constraint(**kwargs) # --- Initiate Asynchronous Planning --- - future = planner.plan_async(start_joint_state=start_state) + future = planner.plan_async( + start_joint_state=start_state, + cartesian=cartesian, + max_step=cartesian_max_step, + ) # Wait for the future to complete outside the lock start_time = time.time() @@ -264,10 +328,20 @@ def plan( return TrialStatus.PLANNER_FAILURE, None time.sleep(0.1) - traj = planner.get_trajectory(future) + traj = planner.get_trajectory( + future, + cartesian=cartesian, + cartesian_fraction_threshold=cartesian_fraction_threshold, + ) if not traj or not traj.points: return TrialStatus.PLANNER_FAILURE, None + # Scale velocity for cartesian plans, as pymoveit2 doesn't do this automatically + if cartesian and planner.max_velocity > 0.0: + MotionPlanner._scale_cartesian_trajectory_velocity( + traj, planner.max_velocity + ) + return TrialStatus.SUCCESS, traj @@ -294,8 +368,15 @@ def __init__( self.planning_timeout = planning_timeout self.output_dir = output_dir self.results: List[Dict[str, Any]] = [] + self.tf_buffer = tf2_ros.Buffer() + self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) self.motion_planner = MotionPlanner( - node, moveit2_jaco, moveit2_atool, moveit2_full, planning_timeout + node, + moveit2_jaco, + moveit2_atool, + moveit2_full, + self.tf_buffer, + planning_timeout, ) # Pinocchio model for feasibility checks From 8d9d2e6939feda52ce2f0cfb6bc4af69363b241d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 12 Aug 2025 19:53:54 -0700 Subject: [PATCH 185/238] Naming convention update --- .../scripts/end_to_end_feeding_benchmark.py | 86 ++++++++----------- 1 file changed, 38 insertions(+), 48 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 650df985..1c2f34c4 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -552,7 +552,7 @@ def _generate_scene(self) -> Dict[str, Any]: # Define derived poses scene["home_config"] = [-1.47568, 2.92779, 1.00845, -2.0847, 1.43588, 1.32575] scene["above_plate_pose"] = self._calculate_above_plate_pose(scene["food_pose"]) - scene["move_into_pose"], approach_vector = self._calculate_move_into_pose( + scene["in_food_pose"], approach_vector = self._calculate_in_food_pose( food_pose=scene["food_pose"], recipe=ActionRecipe( AcquisitionStrategy.SKEWER, @@ -560,8 +560,8 @@ def _generate_scene(self) -> Dict[str, Any]: ToolAlignment.PERPENDICULAR, ), ) - scene["move_above_pose"] = self._calculate_move_above_pose( - move_into_pose=scene["move_into_pose"], approach_vector=approach_vector + scene["above_food_pose"] = self._calculate_above_food_pose( + in_food_pose=scene["in_food_pose"], approach_vector=approach_vector ) scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) scene["resting_pose"] = Pose(position=Point(x=0.4, y=-0.4, z=0.3)) @@ -731,11 +731,11 @@ def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: return final_pose # --- Semantic Pose Calculation --- - def _calculate_move_into_pose( + def _calculate_in_food_pose( self, food_pose: Pose, recipe: ActionRecipe ) -> Tuple[Pose, np.ndarray]: """ - Calculates the MoveInto tool tip pose based on a semantic ActionRecipe. + Calculates the InFood tool tip pose based on a semantic ActionRecipe. Returns the pose and the calculated approach vector (the tool's Z-axis). """ # Default to identity rotation and a vertical approach vector @@ -792,28 +792,28 @@ def _calculate_move_into_pose( # --- Construct the final pose --- q = final_rotation.as_quat() - move_into_pose = Pose() - move_into_pose.position = food_pose.position - move_into_pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + in_food_pose = Pose() + in_food_pose.position = food_pose.position + in_food_pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) - return move_into_pose, approach_vector + return in_food_pose, approach_vector - def _calculate_move_above_pose( - self, move_into_pose: Pose, approach_vector: np.ndarray + def _calculate_above_food_pose( + self, in_food_pose: Pose, approach_vector: np.ndarray ) -> Pose: """ - Calculates the MoveAbove pose by offsetting from MoveInto along the + Calculates the AboveFood pose by offsetting from InFood along the calculated approach vector. """ # 1. Inherit the orientation directly from the target pose - final_orientation = move_into_pose.orientation + final_orientation = in_food_pose.orientation # 2. The motion vector for the linear path IS the approach vector (tool's Z-axis) motion_vector = approach_vector # 3. Calculate the offset position offset_dist = 0.1 # 10 cm - p = move_into_pose.position + p = in_food_pose.position # We add the offset because the approach_vector is already pointing "down". # To get the "above" pose, we move in the opposite direction of the approach. offset = motion_vector * -offset_dist @@ -965,18 +965,18 @@ def _serialize_trajectory( } # --- Planning Primitive Placeholders --- - def _plan_to_move_above( + def _plan_to_above_food( self, - move_above_pose: Pose, + above_food_pose: Pose, start_state_jaco: List[float], start_state_atool: List[float], ) -> Tuple[TrialStatus, Optional[JointTrajectory], Optional[JointTrajectory]]: - LOGGER.info(" Solving 8-DOF IK for MoveAbove pose...") + LOGGER.info(" Solving 8-DOF IK for AboveFood pose...") # 1. Compute IK using the planner ik_solution = self.motion_planner.compute_ik( group_name=PLANNING_GROUP_FULL, - target_pose=move_above_pose, + target_pose=above_food_pose, start_joint_state=(start_state_jaco + start_state_atool), ) @@ -1015,7 +1015,7 @@ def _plan_to_move_above( return TrialStatus.SUCCESS, traj_jaco, traj_atool - def _plan_s1_unconstrained( + def _plan_to_above_plate( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") @@ -1119,12 +1119,12 @@ def run(self): - Above Plate Pose: Position: [x={scene["above_plate_pose"].position.x:.3f}, y={scene["above_plate_pose"].position.y:.3f}, z={scene["above_plate_pose"].position.z:.3f}] Orientation: [x={scene["above_plate_pose"].orientation.x:.3f}, y={scene["above_plate_pose"].orientation.y:.3f}, z={scene["above_plate_pose"].orientation.z:.3f}, w={scene["above_plate_pose"].orientation.w:.3f}] - - Move Above Pose: - Position: [x={scene["move_above_pose"].position.x:.3f}, y={scene["move_above_pose"].position.y:.3f}, z={scene["move_above_pose"].position.z:.3f}] - Orientation: [x={scene["move_above_pose"].orientation.x:.3f}, y={scene["move_above_pose"].orientation.y:.3f}, z={scene["move_above_pose"].orientation.z:.3f}, w={scene["move_above_pose"].orientation.w:.3f}] - - Move Into Pose: - Position: [x={scene["move_into_pose"].position.x:.3f}, y={scene["move_into_pose"].position.y:.3f}, z={scene["move_into_pose"].position.z:.3f}] - Orientation: [x={scene["move_into_pose"].orientation.x:.3f}, y={scene["move_into_pose"].orientation.y:.3f}, z={scene["move_into_pose"].orientation.z:.3f}, w={scene["move_into_pose"].orientation.w:.3f}] + - Above Food Pose: + Position: [x={scene["above_food_pose"].position.x:.3f}, y={scene["above_food_pose"].position.y:.3f}, z={scene["above_food_pose"].position.z:.3f}] + Orientation: [x={scene["above_food_pose"].orientation.x:.3f}, y={scene["above_food_pose"].orientation.y:.3f}, z={scene["above_food_pose"].orientation.z:.3f}, w={scene["above_food_pose"].orientation.w:.3f}] + - In Food Pose: + Position: [x={scene["in_food_pose"].position.x:.3f}, y={scene["in_food_pose"].position.y:.3f}, z={scene["in_food_pose"].position.z:.3f}] + Orientation: [x={scene["in_food_pose"].orientation.x:.3f}, y={scene["in_food_pose"].orientation.y:.3f}, z={scene["in_food_pose"].orientation.z:.3f}, w={scene["in_food_pose"].orientation.w:.3f}] - Staging Pose: Position: [x={scene["staging_pose"].position.x:.3f}, y={scene["staging_pose"].position.y:.3f}, z={scene["staging_pose"].position.z:.3f}] Orientation: [x={scene["staging_pose"].orientation.x:.3f}, y={scene["staging_pose"].orientation.y:.3f}, z={scene["staging_pose"].orientation.z:.3f}, w={scene["staging_pose"].orientation.w:.3f}] @@ -1140,8 +1140,8 @@ def run(self): "food_pose": self._serialize_pose(scene["food_pose"]), "mouth_pose": self._serialize_pose(scene["mouth_pose"]), "above_plate_pose": self._serialize_pose(scene["above_plate_pose"]), - "move_above_pose": self._serialize_pose(scene["move_above_pose"]), - "move_into_pose": self._serialize_pose(scene["move_into_pose"]), + "above_food_pose": self._serialize_pose(scene["above_food_pose"]), + "in_food_pose": self._serialize_pose(scene["in_food_pose"]), "staging_pose": self._serialize_pose(scene["staging_pose"]), "resting_pose": self._serialize_pose(scene["resting_pose"]), }, @@ -1149,28 +1149,22 @@ def run(self): } # 2. Simulate the feeding cycle state machine - food_on_tool = False current_jaco_state = scene["home_config"] current_atool_state = [0.0, 0.0] # Assume atool starts at zero - # --- Stage 1: Home -> AbovePlate (P1 with S1) --- + # --- Stage 1: Home -> AbovePlate --- LOGGER.info("Stage 1: Home -> AbovePlate") - - status, trajectory = self._plan_s1_unconstrained( + status, traj_jaco_to_above_plate = self._plan_to_above_plate( scene["above_plate_pose"], current_jaco_state ) - - # --- Result Logging for Stage 1 --- - # Consolidate all metrics for this stage into a single record. trial_data["stages"].append( { "stage": "HomeToAbovePlate", "primitive": "S1", "status": status.value, - "trajectory": self._serialize_trajectory(trajectory), + "traj_jaco": self._serialize_trajectory(traj_jaco_to_above_plate), } ) - if status != TrialStatus.SUCCESS: LOGGER.error( f" Stage 1 failed with status: {status.value}. Skipping trial." @@ -1178,25 +1172,21 @@ def run(self): continue # Update state for the next stage - current_jaco_state = list(trajectory.points[-1].positions) - LOGGER.info(" Stage 1 successful.") - - # --- Stage 2: AbovePlate -> MoveAbove (P2 with SX) --- - LOGGER.info("Stage 2: AbovePlate -> MoveAbove") + current_jaco_state = list(traj_jaco_to_above_plate.points[-1].positions) - # Plan to the MoveAbove configuration - status, traj_jaco_to_above, traj_atool_to_above = self._plan_to_move_above( - scene["move_above_pose"], current_jaco_state, current_atool_state + # --- Stage 2: AbovePlate -> AboveFood --- + LOGGER.info("Stage 2: AbovePlate -> AboveFood") + status, traj_jaco_to_above_food, traj_atool_to_above_food = ( + self._plan_to_above_food( + scene["above_food_pose"], current_jaco_state, current_atool_state + ) ) - - # Log the results for this stage and recipe trial_data["stages"].append( { - "stage": "AbovePlateToMoveAbove", + "stage": "AbovePlateToAboveFood", "status": status.value, } ) - if status != TrialStatus.SUCCESS: LOGGER.error( f" Stage 2 failed with status: {status.value}. Skipping trial." From c34ddb08f5679e0f988269b73665e3c0ac40566f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 12 Aug 2025 19:59:10 -0700 Subject: [PATCH 186/238] Cleanup --- .../scripts/end_to_end_feeding_benchmark.py | 40 ++++++------------- 1 file changed, 13 insertions(+), 27 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 1c2f34c4..4f956775 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -964,7 +964,19 @@ def _serialize_trajectory( "points": serialized_points, } - # --- Planning Primitive Placeholders --- + # --- Planning Primitive --- + def _plan_to_above_plate( + self, goal_pose: Pose, start_state: Any + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") + goal_constraints = [create_pose_constraint(goal_pose)] + + return self.motion_planner.plan( + group_name=PLANNING_GROUP_JACO, + start_state=start_state, + goal_constraints=goal_constraints, + ) + def _plan_to_above_food( self, above_food_pose: Pose, @@ -1015,18 +1027,6 @@ def _plan_to_above_food( return TrialStatus.SUCCESS, traj_jaco, traj_atool - def _plan_to_above_plate( - self, goal_pose: Pose, start_state: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: - LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") - goal_constraints = [create_pose_constraint(goal_pose)] - - return self.motion_planner.plan( - group_name=PLANNING_GROUP_JACO, - start_state=start_state, - goal_constraints=goal_constraints, - ) - def _plan_s2_guided( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: @@ -1074,20 +1074,6 @@ def _plan_s2_guided( return TrialStatus.SUCCESS, traj, feasibility_percent - def _plan_s3_coordinated( - self, goal_pose: Pose, start_state: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: - LOGGER.info(" Planning with S3 (8-DOF Coordinated)...") - # TODO: Implement 8-DOF IK solve and sequential planning - return TrialStatus.SKIPPED, None - - def _plan_s4_cartesian( - self, goal_pose: Pose, start_state: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: - LOGGER.info(" Planning with S4 (6-DOF Cartesian)...") - # TODO: Implement MoveIt2 call for Cartesian planning - return TrialStatus.SKIPPED, None - # --- Main Benchmark Loop --- def run(self): """Main benchmark execution loop.""" From f2d31d03a3de5f5c3d5160461c1e1a61f79c6f69 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 12 Aug 2025 20:21:39 -0700 Subject: [PATCH 187/238] Add FK and implement plan to in food pose --- .../scripts/end_to_end_feeding_benchmark.py | 80 ++++++++++++++++++- 1 file changed, 76 insertions(+), 4 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 4f956775..00eca35a 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -253,6 +253,25 @@ def compute_ik( return ik_solution + def compute_fk( + self, group_name: str, joint_state: JointState, fk_link_names: List[str] + ) -> Optional[List[PoseStamped]]: + """ + Computes Forward Kinematics for a given set of links. + + Returns: + A list of PoseStamped messages, one for each requested link. + """ + planner = self._get_planner(group_name) + fk_poses = None + + with self._lock: + fk_poses = planner.compute_fk( + joint_state=joint_state, fk_link_names=fk_link_names + ) + + return fk_poses + def plan( self, group_name: str, @@ -966,14 +985,14 @@ def _serialize_trajectory( # --- Planning Primitive --- def _plan_to_above_plate( - self, goal_pose: Pose, start_state: Any + self, above_plate_pose: Pose, start_state_jaco: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: - LOGGER.info(" Planning with S1 (6-DOF Unconstrained)...") - goal_constraints = [create_pose_constraint(goal_pose)] + LOGGER.info(" Planning to AbovePlate pose...") + goal_constraints = [create_pose_constraint(above_plate_pose)] return self.motion_planner.plan( group_name=PLANNING_GROUP_JACO, - start_state=start_state, + start_state=start_state_jaco, goal_constraints=goal_constraints, ) @@ -1027,6 +1046,58 @@ def _plan_to_above_food( return TrialStatus.SUCCESS, traj_jaco, traj_atool + def _plan_to_in_food( + self, + in_food_pose: Pose, + start_state_jaco: List[float], + start_state_atool: List[float], + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + LOGGER.info(" Solving 8-DOF IK for InFood pose...") + + # 1. Compute the 8-DOF IK solution for the target tool tip pose + ik_solution = self.motion_planner.compute_ik( + group_name=PLANNING_GROUP_FULL, + target_pose=in_food_pose, + start_joint_state=(start_state_jaco + start_state_atool), + ) + + if not ik_solution: + LOGGER.warning(" IK solution for 8-DOF system not found.") + return TrialStatus.IK_FAILURE, None + + LOGGER.info(f" 8-DOF IK solution found. Calculating wrist pose via FK.") + + # 2. Use FK to find the Jaco wrist pose from the 8-DOF solution + fk_poses = self.motion_planner.compute_fk( + group_name=PLANNING_GROUP_FULL, + joint_state=ik_solution, + fk_link_names=[END_EFFECTOR_LINK_JACO], + ) + + if not fk_poses: + LOGGER.warning(" FK calculation for Jaco wrist failed.") + return TrialStatus.IK_FAILURE, None + + # The goal for the Jaco arm is the calculated pose of its wrist + jaco_wrist_goal_pose = fk_poses[0].pose + + # 3. Create a pose constraint for the Cartesian plan + jaco_goal_constraints = [create_pose_constraint(jaco_wrist_goal_pose)] + + # 4. Plan a Cartesian motion for the Jaco arm to the wrist pose + status_jaco, traj_jaco = self.motion_planner.plan( + group_name=PLANNING_GROUP_JACO, + start_state=start_state_jaco, + goal_constraints=jaco_goal_constraints, + cartesian=True, + ) + + if status_jaco != TrialStatus.SUCCESS: + LOGGER.warning(" Jaco arm planning failed.") + return TrialStatus.PLANNER_FAILURE, None + + return TrialStatus.SUCCESS, traj_jaco + def _plan_s2_guided( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: @@ -1149,6 +1220,7 @@ def run(self): "primitive": "S1", "status": status.value, "traj_jaco": self._serialize_trajectory(traj_jaco_to_above_plate), + "traj_atool": None, } ) if status != TrialStatus.SUCCESS: From db74ec6e761f4ea405b50fdbcc1dff38c4c78180 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 14 Aug 2025 12:32:27 -0700 Subject: [PATCH 188/238] Add function to compute leveling configuration for Articutool given the current Jaco EE pose. Also implement planning for leveling configuration --- .../scripts/end_to_end_feeding_benchmark.py | 78 +++++++++++++++++++ 1 file changed, 78 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 00eca35a..823d0e63 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -889,6 +889,49 @@ def _solve_articutool_ik( ) return solutions + def _compute_leveling_joints(self, jaco_wrist_pose: Pose) -> Optional[List[float]]: + """ + Calculates the Articutool joint angles required to point the tool's Y-axis up. + This now uses the original `_solve_articutool_ik` method. + """ + # 1. Get the rotation of the Jaco wrist in the world frame + R_world_jacoee = R.from_quat( + [ + jaco_wrist_pose.orientation.x, + jaco_wrist_pose.orientation.y, + jaco_wrist_pose.orientation.z, + jaco_wrist_pose.orientation.w, + ] + ) + + # 2. Transform the world "up" vector into the wrist's frame + world_z_up_vector = np.array([0.0, 0.0, 1.0]) + target_up_in_wrist_frame = R_world_jacoee.inv().apply(world_z_up_vector) + + # 3. Solve the Articutool IK for the transformed vector using the original solver + # The solver's math is correct for this specific leveling task. + ik_solutions = self._solve_articutool_ik(target_up_in_wrist_frame) + + if not ik_solutions: + return None + + # 4. Find the first valid solution within joint limits + for theta_p, theta_r in ik_solutions: + if ( + ARTICUTOOL_PITCH_LIMITS_RAD[0] + <= theta_p + <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] + <= theta_r + <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + ): + LOGGER.info( + f" Found valid leveling solution: Pitch={math.degrees(theta_p):.1f}, Roll={math.degrees(theta_r):.1f}" + ) + return [theta_p, theta_r] + + return None + # --- Feasibility Checking --- def _is_config_kinematically_feasible(self, jaco_joint_config: List[float]) -> bool: """ @@ -1098,6 +1141,41 @@ def _plan_to_in_food( return TrialStatus.SUCCESS, traj_jaco + def _plan_to_level_articutool( + self, + jaco_wrist_pose: Pose, + start_state_atool: List[float], + ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + """ + Calculates the required joint angles for the Articutool to achieve a level + pose and plans a trajectory to that configuration. + """ + LOGGER.info(" Calculating Articutool leveling configuration...") + + # 1. Compute the target joint angles for leveling based on the wrist's pose + target_leveling_joints = self._compute_leveling_joints(jaco_wrist_pose) + + if target_leveling_joints is None: + LOGGER.warning(" Could not find an IK solution for Articutool leveling.") + return TrialStatus.IK_FAILURE, None + + # 2. Create a joint goal constraint for the Articutool + goal_constraints = [create_joint_constraint(target_leveling_joints)] + + # 3. Plan a joint-space motion for the Articutool group + status, trajectory = self.motion_planner.plan( + group_name=PLANNING_GROUP_ATOOL, + start_state=start_state_atool, + goal_constraints=goal_constraints, + ) + + if status != TrialStatus.SUCCESS: + LOGGER.warning(" Articutool leveling plan failed.") + return TrialStatus.PLANNER_FAILURE, None + + LOGGER.info(" Articutool leveling plan successful.") + return TrialStatus.SUCCESS, trajectory + def _plan_s2_guided( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: From 53adfbe6339157fb775c5cf12299f89ec35f62d6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 14 Aug 2025 13:15:59 -0700 Subject: [PATCH 189/238] Integrate planning and results for up to Stage 4 (LevelArticutool) --- .../scripts/end_to_end_feeding_benchmark.py | 89 +++++++++++++------ 1 file changed, 63 insertions(+), 26 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 823d0e63..ca9a356b 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1094,7 +1094,7 @@ def _plan_to_in_food( in_food_pose: Pose, start_state_jaco: List[float], start_state_atool: List[float], - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + ) -> Tuple[TrialStatus, Optional[JointTrajectory], Optional[Pose]]: LOGGER.info(" Solving 8-DOF IK for InFood pose...") # 1. Compute the 8-DOF IK solution for the target tool tip pose @@ -1106,7 +1106,7 @@ def _plan_to_in_food( if not ik_solution: LOGGER.warning(" IK solution for 8-DOF system not found.") - return TrialStatus.IK_FAILURE, None + return TrialStatus.IK_FAILURE, None, None LOGGER.info(f" 8-DOF IK solution found. Calculating wrist pose via FK.") @@ -1119,7 +1119,7 @@ def _plan_to_in_food( if not fk_poses: LOGGER.warning(" FK calculation for Jaco wrist failed.") - return TrialStatus.IK_FAILURE, None + return TrialStatus.IK_FAILURE, None, None # The goal for the Jaco arm is the calculated pose of its wrist jaco_wrist_goal_pose = fk_poses[0].pose @@ -1137,9 +1137,9 @@ def _plan_to_in_food( if status_jaco != TrialStatus.SUCCESS: LOGGER.warning(" Jaco arm planning failed.") - return TrialStatus.PLANNER_FAILURE, None + return TrialStatus.PLANNER_FAILURE, None, None - return TrialStatus.SUCCESS, traj_jaco + return TrialStatus.SUCCESS, traj_jaco, jaco_wrist_goal_pose def _plan_to_level_articutool( self, @@ -1230,10 +1230,11 @@ def run(self): LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") return self.add_articutool_bounding_cylinder() + for i in range(self.num_trials): LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") - # 1. Generate a new scene and create the top-level dictionary for this trial + # 1. Generate scene and create the top-level dictionary for the trial scene = self._generate_scene() LOGGER.info( f""" @@ -1283,52 +1284,88 @@ def run(self): "stages": [], } - # 2. Simulate the feeding cycle state machine + # 2. Initialize the robot's state for the trial current_jaco_state = scene["home_config"] - current_atool_state = [0.0, 0.0] # Assume atool starts at zero + current_atool_state = [0.0, 0.0] # --- Stage 1: Home -> AbovePlate --- LOGGER.info("Stage 1: Home -> AbovePlate") - status, traj_jaco_to_above_plate = self._plan_to_above_plate( + status, traj_jaco = self._plan_to_above_plate( scene["above_plate_pose"], current_jaco_state ) trial_data["stages"].append( { - "stage": "HomeToAbovePlate", - "primitive": "S1", + "stage_name": "HomeToAbovePlate", "status": status.value, - "traj_jaco": self._serialize_trajectory(traj_jaco_to_above_plate), + "traj_jaco": self._serialize_trajectory(traj_jaco), "traj_atool": None, } ) if status != TrialStatus.SUCCESS: - LOGGER.error( - f" Stage 1 failed with status: {status.value}. Skipping trial." - ) + LOGGER.error(f" Stage 1 failed. Skipping trial.") + self.results.append(trial_data) continue - - # Update state for the next stage - current_jaco_state = list(traj_jaco_to_above_plate.points[-1].positions) + current_jaco_state = list(traj_jaco.points[-1].positions) # --- Stage 2: AbovePlate -> AboveFood --- LOGGER.info("Stage 2: AbovePlate -> AboveFood") - status, traj_jaco_to_above_food, traj_atool_to_above_food = ( - self._plan_to_above_food( - scene["above_food_pose"], current_jaco_state, current_atool_state - ) + status, traj_jaco, traj_atool = self._plan_to_above_food( + scene["above_food_pose"], current_jaco_state, current_atool_state ) trial_data["stages"].append( { - "stage": "AbovePlateToAboveFood", + "stage_name": "AbovePlateToAboveFood", "status": status.value, + "traj_jaco": self._serialize_trajectory(traj_jaco), + "traj_atool": self._serialize_trajectory(traj_atool), } ) if status != TrialStatus.SUCCESS: - LOGGER.error( - f" Stage 2 failed with status: {status.value}. Skipping trial." - ) + LOGGER.error(f" Stage 2 failed. Skipping trial.") + self.results.append(trial_data) + continue + current_jaco_state = list(traj_jaco.points[-1].positions) + current_atool_state = list(traj_atool.points[-1].positions) + + # --- Stage 3: AboveFood -> InFood --- + LOGGER.info("Stage 3: AboveFood -> InFood (Cartesian)") + status, traj_jaco, jaco_wrist_pose = self._plan_to_in_food( + scene["in_food_pose"], current_jaco_state, current_atool_state + ) + trial_data["stages"].append( + { + "stage_name": "AboveFoodToInFood", + "status": status.value, + "traj_jaco": self._serialize_trajectory(traj_jaco), + "traj_atool": None, + } + ) + if status != TrialStatus.SUCCESS: + LOGGER.error(f" Stage 3 failed. Skipping trial.") + self.results.append(trial_data) + continue + current_jaco_state = list(traj_jaco.points[-1].positions) + + # --- Stage 4: InFood -> LevelArticutool --- + LOGGER.info("Stage 4: Level Articutool") + status, traj_atool = self._plan_to_level_articutool( + jaco_wrist_pose, current_atool_state + ) + trial_data["stages"].append( + { + "stage_name": "LevelArticutool", + "status": status.value, + "traj_jaco": None, + "traj_atool": self._serialize_trajectory(traj_atool), + } + ) + if status != TrialStatus.SUCCESS: + LOGGER.error(f" Stage 4 failed. Skipping trial.") + self.results.append(trial_data) continue + current_atool_state = list(traj_atool.points[-1].positions) + # At the end of a successful trial, append the complete trial data self.results.append(trial_data) self.save_results() From db8f3a4849bcae034f5779c3ea1b5201f02b163d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 14 Aug 2025 14:20:02 -0700 Subject: [PATCH 190/238] Update benchmark to log execution mode (JACO_ONLY, ATOOL_ONLY, SEQUENTIAL,...) --- .../scripts/end_to_end_feeding_benchmark.py | 15 +++++++++++++++ 1 file changed, 15 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index ca9a356b..17ac9786 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -76,6 +76,17 @@ class TrialStatus(Enum): SKIPPED = "Skipped" +class ExecutionMode(Enum): + """Defines how trajectories for a stage should be interpreted.""" + + SEQUENTIAL = "Sequential" # Jaco and Articutool move one after the other. + SYNCHRONOUS = "Synchronous" # Jaco and Articutool move at the same time. + JACO_ONLY = "Jaco Only" # Only a Jaco trajectory exists for this stage. + ATOOL_ONLY = ( + "Articutool Only" # Only an Articutool trajectory exists for this stage. + ) + + # --- Semantic Schema for Acquisition Actions --- class AcquisitionStrategy(Enum): """High-level choice of physical interaction""" @@ -1297,6 +1308,7 @@ def run(self): { "stage_name": "HomeToAbovePlate", "status": status.value, + "execution_mode": ExecutionMode.JACO_ONLY.value, "traj_jaco": self._serialize_trajectory(traj_jaco), "traj_atool": None, } @@ -1316,6 +1328,7 @@ def run(self): { "stage_name": "AbovePlateToAboveFood", "status": status.value, + "execution_mode": ExecutionMode.SEQUENTIAL.value, "traj_jaco": self._serialize_trajectory(traj_jaco), "traj_atool": self._serialize_trajectory(traj_atool), } @@ -1336,6 +1349,7 @@ def run(self): { "stage_name": "AboveFoodToInFood", "status": status.value, + "execution_mode": ExecutionMode.JACO_ONLY.value, "traj_jaco": self._serialize_trajectory(traj_jaco), "traj_atool": None, } @@ -1355,6 +1369,7 @@ def run(self): { "stage_name": "LevelArticutool", "status": status.value, + "execution_mode": ExecutionMode.ATOOL_ONLY.value, "traj_jaco": None, "traj_atool": self._serialize_trajectory(traj_atool), } From 161783e23353ec17e56284ea7c3a91bf9a483d4f Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 15 Aug 2025 14:54:16 -0700 Subject: [PATCH 191/238] Add logic to handle updated JSON structure and execution modes --- .../scripts/visualize_e2e_benchmark.py | 338 ++++++++++++++---- 1 file changed, 265 insertions(+), 73 deletions(-) diff --git a/ada_feeding/scripts/visualize_e2e_benchmark.py b/ada_feeding/scripts/visualize_e2e_benchmark.py index 3b5df0af..9a6b72b6 100644 --- a/ada_feeding/scripts/visualize_e2e_benchmark.py +++ b/ada_feeding/scripts/visualize_e2e_benchmark.py @@ -8,8 +8,11 @@ It allows a user to load a benchmark result file, choose a specific planning stage and trial, and then inspect the resulting trajectory -frame-by-frame or animate it. This version includes robust mesh path finding -and explicit kinematic updates to prevent rendering artifacts. +frame-by-frame or animate it. + +This version automatically handles different execution modes (sequential, synchronous) +and includes robust mesh path finding and explicit kinematic updates to prevent +rendering artifacts. """ # Standard imports @@ -120,75 +123,116 @@ def load_pinocchio_model_from_urdf_string( def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFrame: - """Loads and concatenates data from multiple benchmark JSON files.""" - all_data = [] + """ + Loads and flattens data from one or more benchmark JSON files into a pandas DataFrame. + The new hierarchical structure (trial -> stages) is flattened so that each + stage with a valid trajectory becomes a single row in the DataFrame. + """ + all_stages_flat = [] for file_path in file_paths: try: with open(file_path, "r") as f: - data = json.load(f) - valid_entries = [ - entry for entry in data if entry.get("trajectory") is not None - ] - all_data.extend(valid_entries) + trials = json.load(f) + for trial in trials: + for stage in trial.get("stages", []): + flat_record = { + "trial_id": trial.get("trial_id"), + "scene_poses": trial.get("scene_poses"), + "stage_name": stage.get("stage_name"), + "status": stage.get("status"), + "execution_mode": stage.get("execution_mode"), + "traj_jaco": stage.get("traj_jaco"), + "traj_atool": stage.get("traj_atool"), + } + if flat_record["traj_jaco"] or flat_record["traj_atool"]: + all_stages_flat.append(flat_record) except Exception as e: logger_func(f"Warning: Could not load or parse {file_path}. Error: {e}") - if not all_data: + if not all_stages_flat: return pd.DataFrame() - logger_func( - f"Successfully loaded {len(all_data)} trials with trajectories from {len(file_paths)} file(s)." - ) - return pd.DataFrame(all_data) + logger_func(f"Successfully loaded {len(all_stages_flat)} stages with trajectories.") + return pd.DataFrame(all_stages_flat) def select_trajectory(df: pd.DataFrame) -> Optional[Dict[str, Any]]: - """Interactively prompts the user to select a stage and a specific trial.""" + """ + Interactively prompts the user to select a stage and a trial. It then finds + the static pose of the Jaco arm from the previous stage to ensure + visual continuity. + """ + stage_order = [ + "HomeToAbovePlate", + "AbovePlateToAboveFood", + "AboveFoodToInFood", + "LevelArticutool", + ] + while True: - stages = df["stage"].unique() + stages = df["stage_name"].unique() print("\n--- Please Select a Stage to Visualize ---") for i, stage in enumerate(stages): print(f" [{i + 1}] {stage}") print(" [q] Quit") - try: choice_str = input(f"Enter choice (1-{len(stages)} or q): ").strip().lower() if choice_str == "q": return None - stage_choice = int(choice_str) - 1 - if not 0 <= stage_choice < len(stages): - raise ValueError - selected_stage = stages[stage_choice] + selected_stage_name = stages[int(choice_str) - 1] except (ValueError, IndexError): print("Invalid choice.") continue stage_df = df[ - (df["stage"] == selected_stage) & (df["status"] == "Success") + (df["stage_name"] == selected_stage_name) & (df["status"] == "Success") ].sort_values("trial_id") if stage_df.empty: - print(f"No successful trials found for stage: {selected_stage}") + print(f"No successful trials found for stage: {selected_stage_name}") continue - trajectories = stage_df.to_dict("records") - print(f"\n--- Select a Successful Trial from Stage '{selected_stage}' ---") - for i, trial in enumerate(trajectories): + trials_for_stage = stage_df.to_dict("records") + print(f"\n--- Select a Successful Trial from Stage '{selected_stage_name}' ---") + for i, trial in enumerate(trials_for_stage): print( - f" [{i + 1}] Trial ID: {trial['trial_id']} (IK Attempts: {trial['ik_attempts']})" + f" [{i + 1}] Trial ID: {trial['trial_id']} (Mode: {trial['execution_mode']})" ) print(" [m] Back to Stage Selection") - try: choice_str = ( - input(f"Enter choice (1-{len(trajectories)} or m): ").strip().lower() + input(f"Enter choice (1-{len(trials_for_stage)} or m): ") + .strip() + .lower() ) if choice_str == "m": continue - traj_choice = int(choice_str) - 1 - if not 0 <= traj_choice < len(trajectories): - raise ValueError - return trajectories[traj_choice] + + selected_record = trials_for_stage[int(choice_str) - 1] + trial_id = selected_record["trial_id"] + + try: + current_stage_index = stage_order.index(selected_stage_name) + if current_stage_index > 0: + previous_stage_name = stage_order[current_stage_index - 1] + previous_stage_df = df[ + (df["trial_id"] == trial_id) + & (df["stage_name"] == previous_stage_name) + ] + if not previous_stage_df.empty: + prev_traj_jaco = previous_stage_df.iloc[0]["traj_jaco"] + if prev_traj_jaco and prev_traj_jaco["points"]: + last_waypoint = prev_traj_jaco["points"][-1] + selected_record["static_jaco_config"] = { + "positions": last_waypoint["positions"], + "joint_names": prev_traj_jaco["joint_names"], + } + except ValueError: + print( + f"Warning: Stage '{selected_stage_name}' not in defined stage order for state carryover." + ) + + return selected_record except (ValueError, IndexError): print("Invalid choice.") continue @@ -220,46 +264,143 @@ def draw_scene_frames(pin_viz, scene_poses: Dict[str, Any], frame_scale: float = pin_viz.viewer[frame_path].set_transform(transform.homogeneous) -def visualization_loop(pin_viz, model, data, selected_trial, args): - """Runs the interactive UI for a single selected trajectory.""" - # Clear previous frames and draw the ones for the current trial +def get_joint_map(model, joint_names): + """Helper to map joint names to their indices in Pinocchio's q vector.""" + if not joint_names: + return {} + return { + name: model.getJointId(name) + for name in joint_names + if model.existJointName(name) + } + + +def update_q_from_waypoint(q, model, waypoint, joint_names, joint_map): + """Helper to update the robot configuration from a single waypoint.""" + if not waypoint: + return + for name, pos in zip(joint_names, waypoint["positions"]): + if name in joint_map: + joint_id = joint_map[name] + if model.joints[joint_id].nq == 1: + q[model.joints[joint_id].idx_q] = pos + elif model.joints[joint_id].nq == 2: + q[model.joints[joint_id].idx_q] = np.cos(pos) + q[model.joints[joint_id].idx_q + 1] = np.sin(pos) + + +def visualization_loop(pin_viz, model, data, selected_stage, args): + """Runs the interactive UI, handling different execution modes and frame-by-frame controls.""" try: pin_viz.viewer["scene/frames"].delete() except KeyError: - pass # It's okay if the path doesn't exist on the first run + pass + if "scene_poses" in selected_stage: + draw_scene_frames(pin_viz, selected_stage["scene_poses"]) - if "scene_poses" in selected_trial: - draw_scene_frames(pin_viz, selected_trial["scene_poses"]) - - q = pin.neutral(model) - trajectory = selected_trial["trajectory"] - joint_names_from_traj = trajectory["joint_names"] - waypoints = trajectory["points"] + execution_mode = selected_stage["execution_mode"] + traj_jaco = selected_stage.get("traj_jaco") + traj_atool = selected_stage.get("traj_atool") + static_jaco_config = selected_stage.get("static_jaco_config") - joint_map = { - name: model.getJointId(name) - for name in joint_names_from_traj - if model.existJointName(name) - } + jaco_joint_map = get_joint_map( + model, (traj_jaco or static_jaco_config or {}).get("joint_names", []) + ) + atool_joint_map = get_joint_map(model, (traj_atool or {}).get("joint_names", [])) + + # --- Prepare a unified list of waypoints for consistent navigation --- + waypoints = [] + if execution_mode in ["Jaco Only", "Articutool Only"]: + traj = traj_jaco if execution_mode == "Jaco Only" else traj_atool + waypoints = traj["points"] + elif execution_mode == "Sequential": + waypoints.extend(traj_jaco["points"]) + waypoints.extend(traj_atool["points"]) + elif execution_mode == "Synchronous": + waypoints = traj_jaco["points"] if traj_jaco else [] # Default to Jaco + + if not waypoints: + print("No waypoints to display for this stage.") + return "menu" current_idx = 0 + q = pin.neutral(model) + while True: - waypoint_positions = waypoints[current_idx]["positions"] - for name, pos in zip(joint_names_from_traj, waypoint_positions): - if name in joint_map: - joint_id = joint_map[name] - if model.joints[joint_id].nq == 1: - q[model.joints[joint_id].idx_q] = pos - elif model.joints[joint_id].nq == 2: - q[model.joints[joint_id].idx_q] = np.cos(pos) - q[model.joints[joint_id].idx_q + 1] = np.sin(pos) + # --- Update robot configuration `q` for the current frame --- + # 1. Start with the static Jaco pose from the previous stage, if available. + if static_jaco_config: + update_q_from_waypoint( + q, + model, + {"positions": static_jaco_config["positions"]}, + static_jaco_config["joint_names"], + jaco_joint_map, + ) + + # 2. Layer on the animated joint positions for the current waypoint. + if execution_mode == "Sequential": + # For sequential, we need to know the final pose of the first trajectory + jaco_end_waypoint = traj_jaco["points"][-1] + if current_idx >= len(traj_jaco["points"]): # We are in the Articutool part + update_q_from_waypoint( + q, + model, + jaco_end_waypoint, + traj_jaco["joint_names"], + jaco_joint_map, + ) + update_q_from_waypoint( + q, + model, + waypoints[current_idx], + traj_atool["joint_names"], + atool_joint_map, + ) + else: # We are in the Jaco part + update_q_from_waypoint( + q, + model, + waypoints[current_idx], + traj_jaco["joint_names"], + jaco_joint_map, + ) + # Keep atool at its starting position + update_q_from_waypoint( + q, + model, + traj_atool["points"][0], + traj_atool["joint_names"], + atool_joint_map, + ) + else: # For all other modes, just update from the single relevant trajectory + if traj_jaco: + update_q_from_waypoint( + q, + model, + traj_jaco["points"][min(current_idx, len(traj_jaco["points"]) - 1)], + traj_jaco["joint_names"], + jaco_joint_map, + ) + if traj_atool: + update_q_from_waypoint( + q, + model, + traj_atool["points"][ + min(current_idx, len(traj_atool["points"]) - 1) + ], + traj_atool["joint_names"], + atool_joint_map, + ) pin.forwardKinematics(model, data, q) pin_viz.display(q) print(f"\nDisplaying Frame: {current_idx + 1}/{len(waypoints)}") + print( + f"--- Stage: {selected_stage['stage_name']} | Trial: {selected_stage['trial_id']} | Mode: {execution_mode} ---" + ) print("Commands: [n]ext, [p]rev, [f]irst, [l]ast, [a]nimate, [m]enu, [q]uit") - user_input = input("Enter command: ").strip().lower() if user_input == "q": @@ -279,15 +420,68 @@ def visualization_loop(pin_viz, model, data, selected_trial, args): print("Animating... Press Ctrl+C to stop.") try: for i in range(current_idx, len(waypoints)): - waypoint_positions = waypoints[i]["positions"] - for name, pos in zip(joint_names_from_traj, waypoint_positions): - if name in joint_map: - joint_id = joint_map[name] - if model.joints[joint_id].nq == 1: - q[model.joints[joint_id].idx_q] = pos - elif model.joints[joint_id].nq == 2: - q[model.joints[joint_id].idx_q] = np.cos(pos) - q[model.joints[joint_id].idx_q + 1] = np.sin(pos) + # The same update logic as above is used for animation frames + if static_jaco_config: + update_q_from_waypoint( + q, + model, + {"positions": static_jaco_config["positions"]}, + static_jaco_config["joint_names"], + jaco_joint_map, + ) + if execution_mode == "Sequential": + jaco_end_waypoint = traj_jaco["points"][-1] + if i >= len(traj_jaco["points"]): + update_q_from_waypoint( + q, + model, + jaco_end_waypoint, + traj_jaco["joint_names"], + jaco_joint_map, + ) + update_q_from_waypoint( + q, + model, + waypoints[i], + traj_atool["joint_names"], + atool_joint_map, + ) + else: + update_q_from_waypoint( + q, + model, + waypoints[i], + traj_jaco["joint_names"], + jaco_joint_map, + ) + update_q_from_waypoint( + q, + model, + traj_atool["points"][0], + traj_atool["joint_names"], + atool_joint_map, + ) + else: + if traj_jaco: + update_q_from_waypoint( + q, + model, + traj_jaco["points"][ + min(i, len(traj_jaco["points"]) - 1) + ], + traj_jaco["joint_names"], + jaco_joint_map, + ) + if traj_atool: + update_q_from_waypoint( + q, + model, + traj_atool["points"][ + min(i, len(traj_atool["points"]) - 1) + ], + traj_atool["joint_names"], + atool_joint_map, + ) pin.forwardKinematics(model, data, q) pin_viz.display(q) @@ -332,14 +526,12 @@ def main(args): return while True: - selected_trial = select_trajectory(df) - if selected_trial is None: + selected_stage = select_trajectory(df) + if selected_stage is None: break - - action = visualization_loop(pin_viz, model, data, selected_trial, args) + action = visualization_loop(pin_viz, model, data, selected_stage, args) if action == "quit": break - print("Visualizer finished.") From 10bf3fbaed7c0c5a07f3d57a2f87a0f48131f3f9 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 15 Aug 2025 15:09:00 -0700 Subject: [PATCH 192/238] Adjust cartesian_max_step and cartesian_fraction_threshold --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 17ac9786..3804b218 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -290,9 +290,9 @@ def plan( goal_constraints: List[Tuple[MoveIt2ConstraintType, Dict]], path_constraints: Optional[List[Tuple[MoveIt2ConstraintType, Dict]]] = None, cartesian: bool = False, - cartesian_max_step: float = 0.005, + cartesian_max_step: float = 0.001, cartesian_jump_threshold: float = 0.0, - cartesian_fraction_threshold: float = 0.9, + cartesian_fraction_threshold: float = 0.92, ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: """ Plans a trajectory for a given group based on a list of goal and path constraints. From c934cac7868910a34bf5a508b2a14dca4e8ffa15 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 15 Aug 2025 15:22:41 -0700 Subject: [PATCH 193/238] Add cartesian_jump_threshold that limits the maximum distance in joint positions between two consecutive trajectory points. This avoids large jerks in Cartesian motions --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 3804b218..81fd8c7d 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -291,7 +291,7 @@ def plan( path_constraints: Optional[List[Tuple[MoveIt2ConstraintType, Dict]]] = None, cartesian: bool = False, cartesian_max_step: float = 0.001, - cartesian_jump_threshold: float = 0.0, + cartesian_jump_threshold: float = 5.0, cartesian_fraction_threshold: float = 0.92, ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: """ @@ -307,6 +307,10 @@ def plan( LOGGER.error("Planning failed: At least one goal constraint is required.") return TrialStatus.IK_FAILURE, None + if cartesian: + planner.cartesian_jump_threshold = cartesian_jump_threshold + # Note: Other properties like prismatic/revolute jump thresholds could also be set here + with self._lock: planner.clear_goal_constraints() planner.clear_path_constraints() From c5b73eda7a10083acedabfbb43b3c7ee883f0989 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 16 Aug 2025 10:37:56 -0700 Subject: [PATCH 194/238] Add synchronous Articutool trajectory for motion to InFood pose that keeps the Articutool's tool tip frame at a particular orientation --- .../scripts/end_to_end_feeding_benchmark.py | 123 ++++++++++++++++-- 1 file changed, 113 insertions(+), 10 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 81fd8c7d..37b96bcb 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -33,7 +33,7 @@ import rclpy from rclpy.callback_groups import ReentrantCallbackGroup from rclpy.node import Node -from trajectory_msgs.msg import JointTrajectory +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from geometry_msgs.msg import Pose, Point, Quaternion, PoseStamped from sensor_msgs.msg import JointState from scipy.spatial.transform import Rotation as R @@ -702,6 +702,93 @@ def _sample_pose_in_spherical_shell(self) -> Pose: orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), ) + def _generate_orientation_holding_atool_trajectory( + self, traj_jaco: JointTrajectory, desired_tool_tip_world_orientation: Quaternion + ) -> Optional[JointTrajectory]: + """ + Generates a synchronized Articutool trajectory that maintains a fixed world + orientation, including the necessary kinematic pre-rotation for the IK solver. + """ + if not traj_jaco or not traj_jaco.points: + return None + + R_World_TipTarget = R.from_quat( + [ + desired_tool_tip_world_orientation.x, + desired_tool_tip_world_orientation.y, + desired_tool_tip_world_orientation.z, + desired_tool_tip_world_orientation.w, + ] + ) + y_axis_TipTarget_InWorld = R_World_TipTarget.apply(np.array([0.0, 1.0, 0.0])) + jaco_wrist_frame_id = self.pinocchio_model.getFrameId(END_EFFECTOR_LINK_JACO) + + atool_solutions = [] + last_valid_solution = None + + for point in traj_jaco.points: + q = pin.neutral(self.pinocchio_model) + for i, name in enumerate(traj_jaco.joint_names): + joint_id = self.pinocchio_model.getJointId(name) + q[self.pinocchio_model.joints[joint_id].idx_q] = point.positions[i] + + pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) + pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) + + jaco_wrist_transform = self.pinocchio_data.oMf[jaco_wrist_frame_id] + R_World_JacoEE = R.from_matrix(jaco_wrist_transform.rotation) + + target_y_in_wrist_frame = R_World_JacoEE.inv().apply( + y_axis_TipTarget_InWorld + ) + + # The IK solver expects a vector pre-rotated from our target. + # A vector (x, y, z) becomes (-y, x, z). + x, y, z = target_y_in_wrist_frame + ik_input_vector = np.array([-y, x, z]) + + ik_solutions = self._solve_articutool_ik(ik_input_vector) + + valid_solutions = [ + np.array(sol) + for sol in ik_solutions + if ( + ARTICUTOOL_PITCH_LIMITS_RAD[0] + <= sol[0] + <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] + <= sol[1] + <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + ) + ] + + if not valid_solutions: + LOGGER.warning( + " Failed to find valid IK solution for an Articutool waypoint." + ) + return None + + if last_valid_solution is None: + chosen_solution = valid_solutions[0] + else: + distances = [ + np.linalg.norm(sol - last_valid_solution) for sol in valid_solutions + ] + chosen_solution = valid_solutions[np.argmin(distances)] + + atool_solutions.append(chosen_solution) + last_valid_solution = chosen_solution + + traj_atool = JointTrajectory() + traj_atool.joint_names = JOINT_NAMES_ATOOL + for i, pos in enumerate(atool_solutions): + point = JointTrajectoryPoint() + point.positions = pos.tolist() + point.time_from_start = traj_jaco.points[i].time_from_start + traj_atool.points.append(point) + + return traj_atool + def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: """ Calculates a camera pose for the Jaco end-effector that looks at the food, @@ -1109,7 +1196,12 @@ def _plan_to_in_food( in_food_pose: Pose, start_state_jaco: List[float], start_state_atool: List[float], - ) -> Tuple[TrialStatus, Optional[JointTrajectory], Optional[Pose]]: + ) -> Tuple[ + TrialStatus, + Optional[JointTrajectory], + Optional[JointTrajectory], + Optional[Pose], + ]: LOGGER.info(" Solving 8-DOF IK for InFood pose...") # 1. Compute the 8-DOF IK solution for the target tool tip pose @@ -1121,7 +1213,7 @@ def _plan_to_in_food( if not ik_solution: LOGGER.warning(" IK solution for 8-DOF system not found.") - return TrialStatus.IK_FAILURE, None, None + return TrialStatus.IK_FAILURE, None, None, None LOGGER.info(f" 8-DOF IK solution found. Calculating wrist pose via FK.") @@ -1134,12 +1226,12 @@ def _plan_to_in_food( if not fk_poses: LOGGER.warning(" FK calculation for Jaco wrist failed.") - return TrialStatus.IK_FAILURE, None, None + return TrialStatus.IK_FAILURE, None, None, None # The goal for the Jaco arm is the calculated pose of its wrist jaco_wrist_goal_pose = fk_poses[0].pose - # 3. Create a pose constraint for the Cartesian plan + # 3. Plan a Cartesian motion for the Jaco arm to the wrist pose jaco_goal_constraints = [create_pose_constraint(jaco_wrist_goal_pose)] # 4. Plan a Cartesian motion for the Jaco arm to the wrist pose @@ -1152,9 +1244,19 @@ def _plan_to_in_food( if status_jaco != TrialStatus.SUCCESS: LOGGER.warning(" Jaco arm planning failed.") - return TrialStatus.PLANNER_FAILURE, None, None + return TrialStatus.PLANNER_FAILURE, None, None, None + + # 4. Generate the corresponding synchronous Articutool trajectory + LOGGER.info(" Generating synchronous Articutool trajectory...") + traj_atool = self._generate_orientation_holding_atool_trajectory( + traj_jaco, in_food_pose.orientation + ) + + if traj_atool is None: + LOGGER.error(" Failed to generate synchronous Articutool trajectory.") + return TrialStatus.IK_FAILURE, traj_jaco, None, None - return TrialStatus.SUCCESS, traj_jaco, jaco_wrist_goal_pose + return TrialStatus.SUCCESS, traj_jaco, traj_atool, jaco_wrist_goal_pose def _plan_to_level_articutool( self, @@ -1346,16 +1448,16 @@ def run(self): # --- Stage 3: AboveFood -> InFood --- LOGGER.info("Stage 3: AboveFood -> InFood (Cartesian)") - status, traj_jaco, jaco_wrist_pose = self._plan_to_in_food( + status, traj_jaco, traj_atool, jaco_wrist_pose = self._plan_to_in_food( scene["in_food_pose"], current_jaco_state, current_atool_state ) trial_data["stages"].append( { "stage_name": "AboveFoodToInFood", "status": status.value, - "execution_mode": ExecutionMode.JACO_ONLY.value, + "execution_mode": ExecutionMode.SYNCHRONOUS.value, "traj_jaco": self._serialize_trajectory(traj_jaco), - "traj_atool": None, + "traj_atool": self._serialize_trajectory(traj_atool), } ) if status != TrialStatus.SUCCESS: @@ -1363,6 +1465,7 @@ def run(self): self.results.append(trial_data) continue current_jaco_state = list(traj_jaco.points[-1].positions) + current_atool_state = list(traj_atool.points[-1].positions) # --- Stage 4: InFood -> LevelArticutool --- LOGGER.info("Stage 4: Level Articutool") From 4ce5fa93e73a44902c7f387c8c085f94e4e7091a Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 16 Aug 2025 11:21:14 -0700 Subject: [PATCH 195/238] Remove pre-rotation logic --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 8 +------- 1 file changed, 1 insertion(+), 7 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 37b96bcb..ec565085 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -741,13 +741,7 @@ def _generate_orientation_holding_atool_trajectory( target_y_in_wrist_frame = R_World_JacoEE.inv().apply( y_axis_TipTarget_InWorld ) - - # The IK solver expects a vector pre-rotated from our target. - # A vector (x, y, z) becomes (-y, x, z). - x, y, z = target_y_in_wrist_frame - ik_input_vector = np.array([-y, x, z]) - - ik_solutions = self._solve_articutool_ik(ik_input_vector) + ik_solutions = self._solve_articutool_ik(target_y_in_wrist_frame) valid_solutions = [ np.array(sol) From a7c5a47673d7bce9902dc9f32b8d1362624c9edd Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 16 Aug 2025 12:30:49 -0700 Subject: [PATCH 196/238] Add unit test for analytical Articutool IK solver --- .../scripts/end_to_end_feeding_benchmark.py | 80 +++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index ec565085..3fb5a43d 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -702,6 +702,86 @@ def _sample_pose_in_spherical_shell(self) -> Pose: orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), ) + def _normalize_angle(self, angle: float) -> float: + """Normalize an angle to [-pi, pi].""" + return (angle + math.pi) % (2 * math.pi) - math.pi + + def _test_articutool_ik_solver(self): + """ + Performs a round-trip test of the Articutool's analytical IK solver. + """ + LOGGER.info("--- Running Articutool IK Solver Test ---") + + test_cases_deg = { + "Zero": (0, 0), + "Pitch_Positive": (30, 0), + "Pitch_Negative": (-45, 0), + "Roll_Positive": (0, 30), + "Roll_Negative": (0, -45), + "Combined_1": (30, 45), + "Combined_2": (-20, -60), + "Limit_Pitch": (90, 0), # A singularity case + "Limit_Roll": (0, 90), + } + + all_passed = True + for name, (pitch_deg, roll_deg) in test_cases_deg.items(): + # --- 1. START: Convert test case to radians --- + theta_p = np.deg2rad(pitch_deg) + theta_r = np.deg2rad(roll_deg) + + # --- 2. FORWARD KINEMATICS: Calculate the target Y-axis vector --- + # Note the swapped sin/cos for y_y due to kinematic conventions + y_x = np.cos(theta_p) * np.cos(theta_r) + y_y = np.sin(theta_r) + y_z = np.sin(theta_p) * np.cos(theta_r) + target_y_in_wrist_frame = np.array([y_x, y_y, y_z]) + + # --- 3. PRE-ROTATION: Apply the necessary R_z(+pi/2) rotation --- + # This is the crucial step that mimics what our trajectory generator does. + ik_input_vector = np.array( + [ + -target_y_in_wrist_frame[1], + target_y_in_wrist_frame[0], + target_y_in_wrist_frame[2], + ] + ) + + # --- 4. INVERSE KINEMATICS: Call the solver --- + ik_solutions_rad = self._solve_articutool_ik(ik_input_vector) + + # --- 5. VERIFICATION: Check if the original angles are in the solution set --- + found_match = False + for sol_p, sol_r in ik_solutions_rad: + # Check if a solution is close to the original input, accounting for angle wrapping + if np.isclose( + self._normalize_angle(sol_p), self._normalize_angle(theta_p) + ) and np.isclose( + self._normalize_angle(sol_r), self._normalize_angle(theta_r) + ): + found_match = True + break + + status = "✅ SUCCESS" if found_match else "❌ FAILURE" + if not found_match: + all_passed = False + + print(f"\n- Test Case: {name} ({pitch_deg}°, {roll_deg}°)") + print( + f" - FK Target Vector (y_axis): {np.round(target_y_in_wrist_frame, 3)}" + ) + print(f" - IK Input Vector (pre-rotated): {np.round(ik_input_vector, 3)}") + print( + f" - IK Solutions Found (deg): {[(np.rad2deg(p), np.rad2deg(r)) for p, r in ik_solutions_rad]}" + ) + print(f" - Result: {status}") + + LOGGER.info("--- IK Solver Test Finished ---") + if all_passed: + LOGGER.info("✅ All test cases passed!") + else: + LOGGER.error("❌ One or more test cases failed!") + def _generate_orientation_holding_atool_trajectory( self, traj_jaco: JointTrajectory, desired_tool_tip_world_orientation: Quaternion ) -> Optional[JointTrajectory]: From 358c17727657f584f9db28d4cf91a79879ca48c7 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 16 Aug 2025 12:59:11 -0700 Subject: [PATCH 197/238] Fix error in updating Pinocchio robot state within the Pinocchio model. Specifically, Pinocchio represents continuous joints as a 2D vector [cos(angle), sin(angle)] instead of a raw angle. --- .../scripts/end_to_end_feeding_benchmark.py | 16 +++++++++++++--- 1 file changed, 13 insertions(+), 3 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 3fb5a43d..fa937528 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -808,9 +808,19 @@ def _generate_orientation_holding_atool_trajectory( for point in traj_jaco.points: q = pin.neutral(self.pinocchio_model) - for i, name in enumerate(traj_jaco.joint_names): - joint_id = self.pinocchio_model.getJointId(name) - q[self.pinocchio_model.joints[joint_id].idx_q] = point.positions[i] + for j, name in enumerate(traj_jaco.joint_names): + if self.pinocchio_model.existJointName(name): + joint_id = self.pinocchio_model.getJointId(name) + joint_obj = self.pinocchio_model.joints[joint_id] + angle = point.positions[j] + # Check if the joint is a standard revolute joint (nq=2) + if joint_obj.nq == 2: + q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ + np.cos(angle), + np.sin(angle), + ] + else: + q[joint_obj.idx_q] = angle pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) From 6ba8f5ceda7eb2d0a493c91d96879f3cde79f795 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 16 Aug 2025 13:47:27 -0700 Subject: [PATCH 198/238] Add PinoccioModel class to make querying Pinocchio easier --- .../scripts/end_to_end_feeding_benchmark.py | 180 +++++++++++------- 1 file changed, 109 insertions(+), 71 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index fa937528..19c5f0ce 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -379,6 +379,96 @@ def plan( return TrialStatus.SUCCESS, traj +class PinocchioModel: + """A wrapper for Pinocchio to provide a seamless interface for kinematic queries.""" + + def __init__(self, xacro_file_path: str): + """Loads the robot model from a XACRO file.""" + self.model: Optional[pin.Model] = None + self.data: Optional[pin.Data] = None + self._is_ready = False + + try: + # Convert XACRO to URDF string + process = subprocess.run( + ["ros2", "run", "xacro", "xacro", xacro_file_path], + check=True, + capture_output=True, + text=True, + ) + urdf_xml_string = process.stdout + + # Load model from string + self.model = pin.buildModelFromXML(urdf_xml_string) + self.data = self.model.createData() + self._is_ready = True + LOGGER.info("Pinocchio model loaded successfully.") + except Exception as e: + LOGGER.error(f"Failed to initialize Pinocchio model: {e}", exc_info=True) + + def is_ready(self) -> bool: + """Returns True if the model was loaded successfully.""" + return self._is_ready + + def _update_configuration( + self, jaco_joints: List[float], atool_joints: Optional[List[float]] = None + ) -> np.ndarray: + """ + Populates Pinocchio's configuration vector `q` from joint lists. + Handles the sin/cos representation for revolute joints correctly using the + python `math` library to ensure type compatibility with Pinocchio's bindings. + """ + q = pin.neutral(self.model) + + all_joints = jaco_joints + (atool_joints if atool_joints is not None else []) + all_names = JOINT_NAMES_JACO + ( + JOINT_NAMES_ATOOL if atool_joints is not None else [] + ) + + for i, name in enumerate(all_names): + if self.model.existJointName(name): + joint_id = self.model.getJointId(name) + joint_obj = self.model.joints[joint_id] + angle = all_joints[i] + + idx = joint_obj.idx_q + if joint_obj.nq == 2: + q[idx : idx + 2] = [math.cos(angle), math.sin(angle)] + else: + q[idx] = angle + return q + + def get_frame_transform( + self, + frame_name: str, + jaco_joints: List[float], + atool_joints: Optional[List[float]] = None, + ) -> Optional[pin.SE3]: + """ + Performs Forward Kinematics to get the transform of a specific frame. + + Returns: + A Pinocchio SE3 transform object, or None on failure. + """ + if not self.is_ready(): + return None + + try: + # Update the model's configuration vector + q = self._update_configuration(jaco_joints, atool_joints) + + # Run Forward Kinematics + pin.forwardKinematics(self.model, self.data, q) + pin.updateFramePlacements(self.model, self.data) + + # Get and return the transform + frame_id = self.model.getFrameId(frame_name) + return self.data.oMf[frame_id] + except Exception as e: + LOGGER.error(f"Pinocchio FK failed for frame '{frame_name}': {e}") + return None + + class EndToEndBenchmark: """Manages the end-to-end benchmark planning process.""" @@ -414,34 +504,11 @@ def __init__( ) # Pinocchio model for feasibility checks - self.pinocchio_model: Optional[pin.Model] = None - self.pinocchio_data: Optional[pin.Data] = None - self.jaco_ee_frame_id_pin: Optional[int] = None - self._initialize_pinocchio() + self.kinematics_model = PinocchioModel(self.xacro_file_path) if self.output_dir: os.makedirs(self.output_dir, exist_ok=True) - def _initialize_pinocchio(self): - """Loads the robot model from a XACRO file into Pinocchio.""" - try: - process = subprocess.run( - ["ros2", "run", "xacro", "xacro", self.xacro_file_path], - check=True, - capture_output=True, - text=True, - ) - urdf_xml_string = process.stdout - self.pinocchio_model = pin.buildModelFromXML(urdf_xml_string) - self.pinocchio_data = self.pinocchio_model.createData() - self.jaco_ee_frame_id_pin = self.pinocchio_model.getFrameId( - END_EFFECTOR_LINK_FULL - ) - LOGGER.info("Pinocchio model loaded successfully.") - except Exception as e: - LOGGER.error(f"Failed to initialize Pinocchio model: {e}", exc_info=True) - self.pinocchio_model = None - def add_articutool_bounding_cylinder(self): """ Adds a cylindrical collision object to represent the Articutool @@ -801,31 +868,18 @@ def _generate_orientation_holding_atool_trajectory( ] ) y_axis_TipTarget_InWorld = R_World_TipTarget.apply(np.array([0.0, 1.0, 0.0])) - jaco_wrist_frame_id = self.pinocchio_model.getFrameId(END_EFFECTOR_LINK_JACO) atool_solutions = [] last_valid_solution = None for point in traj_jaco.points: - q = pin.neutral(self.pinocchio_model) - for j, name in enumerate(traj_jaco.joint_names): - if self.pinocchio_model.existJointName(name): - joint_id = self.pinocchio_model.getJointId(name) - joint_obj = self.pinocchio_model.joints[joint_id] - angle = point.positions[j] - # Check if the joint is a standard revolute joint (nq=2) - if joint_obj.nq == 2: - q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ - np.cos(angle), - np.sin(angle), - ] - else: - q[joint_obj.idx_q] = angle - - pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) - pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) - - jaco_wrist_transform = self.pinocchio_data.oMf[jaco_wrist_frame_id] + jaco_points = list(point.positions) + jaco_wrist_transform = self.kinematics_model.get_frame_transform( + frame_name=END_EFFECTOR_LINK_JACO, jaco_joints=jaco_points + ) + if jaco_wrist_transform is None: + LOGGER.error(" Pinocchio FK failed for a waypoint.") + return None R_World_JacoEE = R.from_matrix(jaco_wrist_transform.rotation) target_y_in_wrist_frame = R_World_JacoEE.inv().apply( @@ -1121,35 +1175,22 @@ def _compute_leveling_joints(self, jaco_wrist_pose: Pose) -> Optional[List[float # --- Feasibility Checking --- def _is_config_kinematically_feasible(self, jaco_joint_config: List[float]) -> bool: """ - Checks if the Articutool can maintain leveling at a single Jaco configuration. - This is our "oracle" for testing membership in the true feasibility manifold. + Checks if the Articutool can maintain leveling at a single Jaco configuration + by using the PinocchioModel to perform forward kinematics. """ - if ( - self.pinocchio_model is None - or self.pinocchio_data is None - or self.jaco_ee_frame_id_pin is None - ): + if not self.kinematics_model.is_ready(): return False - q = pin.neutral(self.pinocchio_model) - for j, name in enumerate(JOINT_NAMES_FULL): - if self.pinocchio_model.existJointName(name): - joint_id = self.pinocchio_model.getJointId(name) - joint_obj = self.pinocchio_model.joints[joint_id] - if joint_obj.nq == 2 and not joint_obj.shortname().startswith( - "JointModelRX" - ): - q[joint_obj.idx_q : joint_obj.idx_q + 2] = [ - math.cos(jaco_joint_config[j]), - math.sin(jaco_joint_config[j]), - ] - else: - q[joint_obj.idx_q] = jaco_joint_config[j] + # Get the Jaco end-effector's transform for the given joint configuration. + # We don't need to specify atool_joints, as they don't affect the Jaco wrist's pose. + ee_transform = self.kinematics_model.get_frame_transform( + frame_name=END_EFFECTOR_LINK_JACO, jaco_joints=jaco_joint_config + ) - pin.forwardKinematics(self.pinocchio_model, self.pinocchio_data, q) - pin.updateFramePlacements(self.pinocchio_model, self.pinocchio_data) + if ee_transform is None: + # The FK calculation failed. + return False - ee_transform = self.pinocchio_data.oMf[self.jaco_ee_frame_id_pin] target_up_in_ee_frame = ( R.from_matrix(ee_transform.rotation).inv().apply(WORLD_UP_VECTOR) ) @@ -1427,9 +1468,6 @@ def _plan_s2_guided( # --- Main Benchmark Loop --- def run(self): """Main benchmark execution loop.""" - if not self.pinocchio_model: - LOGGER.error("Benchmark cannot run because Pinocchio model failed to load.") - return self.add_articutool_bounding_cylinder() for i in range(self.num_trials): From fb821da54e62f38d7ceabfa308a2c538dc087174 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Sat, 16 Aug 2025 17:26:39 -0700 Subject: [PATCH 199/238] Update .gitignore --- .gitignore | 32 ++++++++++++++++++++++++++++++++ 1 file changed, 32 insertions(+) diff --git a/.gitignore b/.gitignore index 7c67213b..0f642014 100644 --- a/.gitignore +++ b/.gitignore @@ -66,3 +66,35 @@ trees/debug.xml ada_feeding_perception/test/food_img/ ada_feeding_perception/test/output/ ada_feeding_action_select/data + +# Debug/scratch tree +trees/debug.xml + +# =============================================================== +# Python Artifacts (Consolidated and Added) +# =============================================================== +__pycache__/ +*.pyc +.ada_venv/ +venv/ +.venv/ +env/ + +# =============================================================== +# Project-Specific Data, Plots, and Benchmark Outputs (Added) +# =============================================================== +# Ignore generated data, plots, and saved trajectories +ada_feeding/data/ + +# Ignore all benchmark and analysis output directories +ada_feeding/scripts/benchmark_analysis_plots/ +ada_feeding/scripts/e2e_benchmark_output/ +ada_feeding/scripts/holistic_benchmark_output/ +ada_feeding/scripts/manifold_data/ +ada_feeding/scripts/visualization_output/ + +# Ignore specific generated files in the scripts directory +ada_feeding/scripts/camera_poses.json + +# Ignore screenshots directory at the root +screenshots/ From dd2bdeaac8029fa91140500a932c4e8d913c07aa Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 19 Aug 2025 15:36:51 -0700 Subject: [PATCH 200/238] Add planning stage for motion to Resting pose --- .../scripts/end_to_end_feeding_benchmark.py | 129 ++++++++++++++++++ 1 file changed, 129 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 19c5f0ce..badfa1aa 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -927,6 +927,60 @@ def _generate_orientation_holding_atool_trajectory( return traj_atool + def _generate_leveling_atool_trajectory( + self, traj_jaco: JointTrajectory + ) -> Optional[JointTrajectory]: + """ + Generates a synchronized Articutool trajectory that maintains a level-to-gravity + orientation during a Jaco arm motion. + """ + if not traj_jaco or not traj_jaco.points: + return None + + atool_solutions = [] + + # 1. Iterate through each waypoint of the Jaco trajectory + for point in traj_jaco.points: + jaco_joint_config = list(point.positions) + + # 2. For each arm configuration, find the pose of the wrist using FK + jaco_wrist_transform = self.kinematics_model.get_frame_transform( + frame_name=END_EFFECTOR_LINK_JACO, jaco_joints=jaco_joint_config + ) + if jaco_wrist_transform is None: + LOGGER.error( + " Pinocchio FK failed for a waypoint while generating leveling trajectory." + ) + return None + + q = R.from_matrix(jaco_wrist_transform.rotation).as_quat() + p = jaco_wrist_transform.translation + jaco_wrist_pose = Pose( + position=Point(x=p[0], y=p[1], z=p[2]), + orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]), + ) + + # 3. Calculate the Articutool joints needed to be level at that wrist pose + atool_leveling_joints = self._compute_leveling_joints(jaco_wrist_pose) + if atool_leveling_joints is None: + LOGGER.warning( + " Failed to find leveling IK solution for an Articutool waypoint." + ) + return None + + atool_solutions.append(atool_leveling_joints) + + # 4. Compile the solutions into a JointTrajectory message + traj_atool = JointTrajectory() + traj_atool.joint_names = JOINT_NAMES_ATOOL + for i, pos in enumerate(atool_solutions): + point_msg = JointTrajectoryPoint() + point_msg.positions = pos + point_msg.time_from_start = traj_jaco.points[i].time_from_start + traj_atool.points.append(point_msg) + + return traj_atool + def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: """ Calculates a camera pose for the Jaco end-effector that looks at the food, @@ -1418,6 +1472,60 @@ def _plan_to_level_articutool( LOGGER.info(" Articutool leveling plan successful.") return TrialStatus.SUCCESS, trajectory + def _plan_to_resting( + self, + resting_wrist_pose: Pose, + start_state_jaco: List[float], + ) -> Tuple[ + TrialStatus, Optional[JointTrajectory], Optional[JointTrajectory], float + ]: + """ + Plans a 6-DOF guided motion for the Jaco arm to a resting wrist pose + and measures its leveling feasibility. + """ + LOGGER.info(" Planning to Resting pose (S2-Heuristic)...") + + # 1. Define goal and path constraints for the Jaco arm's wrist + goal_constraints = [create_pose_constraint(resting_wrist_pose)] + path_constraints = [ + create_orientation_path_constraint( + quat_xyzw=PATH_CONSTRAINT_QUAT_XYZW, + tolerance_rad=PATH_CONSTRAINT_TOLERANCE_XYZ_RAD, + ) + ] + + # 2. Plan the guided 6-DOF trajectory for the Jaco arm + status, traj_jaco = self.motion_planner.plan( + group_name=PLANNING_GROUP_JACO, + start_state=start_state_jaco, + goal_constraints=goal_constraints, + path_constraints=path_constraints, + ) + if status != TrialStatus.SUCCESS: + return TrialStatus.PLANNER_FAILURE, None, None, 0.0 + + # 3. VERIFY the Jaco trajectory for leveling feasibility + feasibility_percent = self._verify_trajectory(traj_jaco) + if feasibility_percent < 99.0: + LOGGER.warning( + f" Path to Resting failed verification ({feasibility_percent:.1f}% feasible)." + ) + return ( + TrialStatus.VERIFICATION_FAILURE, + traj_jaco, + None, + feasibility_percent, + ) + + # 4. Generate the corresponding synchronous Articutool trajectory + LOGGER.info(" Generating synchronous Articutool trajectory...") + traj_atool = self._generate_leveling_atool_trajectory(traj_jaco) + + if traj_atool is None: + LOGGER.error(" Failed to generate synchronous Articutool trajectory.") + return TrialStatus.IK_FAILURE, traj_jaco, None, feasibility_percent + return TrialStatus.SUCCESS, traj_jaco, traj_atool, feasibility_percent + def _plan_s2_guided( self, goal_pose: Pose, start_state: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: @@ -1609,6 +1717,27 @@ def run(self): continue current_atool_state = list(traj_atool.points[-1].positions) + # --- Stage 5: LevelArticutool -> Resting --- + LOGGER.info("Stage 5: Resting") + status, traj_jaco, traj_atool, leveling_feasibility = self._plan_to_resting( + scene["resting_pose"], current_jaco_state + ) + trial_data["stages"].append( + { + "stage_name": "Resting", + "status": status.value, + "execution_mode": ExecutionMode.SYNCHRONOUS.value, + "traj_jaco": traj_jaco, + "traj_atool": traj_atool, + } + ) + if status != TrialStatus.SUCCESS: + LOGGER.error(f" Stage 5 failed. Skipping trial.") + self.results.append(trial_data) + continue + current_jaco_state = list(traj_jaco.points[-1].positions) + current_atool_state = list(traj_atool.points[-1].positions) + # At the end of a successful trial, append the complete trial data self.results.append(trial_data) From 494acb8bb1c61cf52e7725d30edcd0f19eec39bb Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 19 Aug 2025 15:57:31 -0700 Subject: [PATCH 201/238] Add tool roll angle when defining InFood pose --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 12 +++++++++--- 1 file changed, 9 insertions(+), 3 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index badfa1aa..3c60abc8 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1045,7 +1045,7 @@ def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: # --- Semantic Pose Calculation --- def _calculate_in_food_pose( - self, food_pose: Pose, recipe: ActionRecipe + self, food_pose: Pose, recipe: ActionRecipe, tool_roll_angle_deg: float = 180.0 ) -> Tuple[Pose, np.ndarray]: """ Calculates the InFood tool tip pose based on a semantic ActionRecipe. @@ -1092,8 +1092,14 @@ def _calculate_in_food_pose( tool_y_final = np.cross(tool_z_final, tool_x_final) # 4. The final rotation is constructed from these basis vectors. - rotation_matrix = np.array([tool_x_final, tool_y_final, tool_z_final]).T - final_rotation = R.from_matrix(rotation_matrix) + base_rotation_matrix = np.array( + [tool_x_final, tool_y_final, tool_z_final] + ).T + base_orientation = R.from_matrix(base_rotation_matrix) + roll_rotation = R.from_rotvec( + np.deg2rad(tool_roll_angle_deg) * tool_z_final + ) + final_rotation = roll_rotation * base_orientation approach_vector = tool_z_final # --- SCOOP Strategy Logic (Placeholder) --- From c1cce56b05ff8b1031a3c7813c0dfaf054437f5d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 19 Aug 2025 17:01:37 -0700 Subject: [PATCH 202/238] Add logic to programmatically generate Resting pose, and fix issue where trajectories were not serialized before storing to results --- .../scripts/end_to_end_feeding_benchmark.py | 88 ++++++++++++++++++- 1 file changed, 85 insertions(+), 3 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 3c60abc8..700be3c7 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -665,7 +665,9 @@ def _generate_scene(self) -> Dict[str, Any]: in_food_pose=scene["in_food_pose"], approach_vector=approach_vector ) scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) - scene["resting_pose"] = Pose(position=Point(x=0.4, y=-0.4, z=0.3)) + scene["resting_pose"] = self._calculate_resting_pose( + scene["food_pose"], scene["mouth_pose"] + ) return scene @@ -1159,6 +1161,86 @@ def _calculate_staging_pose(self, mouth_pose: Pose) -> Pose: # For now, we use the same orientation as the mouth return Pose(position=staged_pos, orientation=q) + def _calculate_resting_pose( + self, + food_pose: Pose, + mouth_pose: Pose, + angular_offset_deg: float = 20, + radial_distance_m: float = 0.8, + vertical_offset_m: float = ARTICUTOOL_LENGTH_M + 0.2, + ) -> Pose: + """ + Calculates a dynamic "Resting" pose for the Jaco end-effector. + + This method implements the "Angular Standoff" strategy. It positions + the resting pose at a fixed radial distance from the robot base, but with + an angular offset from the food's position. This offset is directed + away from the mouth's position, placing the arm in a safe, clear, and + context-aware staging area. + + Args: + food_pose: The 6D pose of the food item. + mouth_pose: The 6D pose of the user's mouth. + + Returns: + The calculated 6D resting pose. + """ + + # --- Extract XY positions and create 2D vectors from the origin --- + v_food = np.array([food_pose.position.x, food_pose.position.y]) + v_mouth = np.array([mouth_pose.position.x, mouth_pose.position.y]) + + # --- 1. Determine the direction of angular offset --- + # Use the 2D cross product to find the sign of the angle between vectors. + # This tells us if the mouth is clockwise or counter-clockwise from the food. + cross_product_z = np.cross(v_food, v_mouth) + + # We apply the offset in the direction that moves away from the mouth. + if cross_product_z > 0: # Mouth is CCW from food, so we rotate CW + angle = -np.deg2rad(angular_offset_deg) + else: # Mouth is CW from food, so we rotate CCW + angle = np.deg2rad(angular_offset_deg) + + # --- 2. Calculate the new position --- + # Normalize the food vector to get its direction + v_food_dir = v_food / np.linalg.norm(v_food) + + # Create a 2D rotation matrix and apply it to the food's direction + c, s = np.cos(angle), np.sin(angle) + rotation_matrix = np.array(((c, -s), (s, c))) + v_rest_dir = rotation_matrix @ v_food_dir + + # Scale the new direction by the fixed radial distance for the final XY position + rest_position_xy = v_rest_dir * radial_distance_m + rest_position_z = food_pose.position.z + vertical_offset_m + + # --- 3. Calculate the new orientation (upright and facing outward) --- + # The z-axis (forward) points horizontally from the base to the new position + z_axis = np.array([rest_position_xy[0], rest_position_xy[1], 0.0]) + z_axis /= np.linalg.norm(z_axis) + + # The y-axis (up) is aligned with the world's Z-axis + y_axis = np.array([0.0, 0.0, 1.0]) + + # The x-axis (left) is the cross product, forming an orthonormal frame + x_axis = np.cross(y_axis, z_axis) + + # Construct the final rotation matrix and convert to a quaternion + rotation_matrix_3d = np.array([x_axis, y_axis, z_axis]).T + rotation = R.from_matrix(rotation_matrix_3d) + quat = rotation.as_quat() + + # --- 4. Assemble and return the final Pose message --- + resting_pose = Pose() + resting_pose.position = Point( + x=rest_position_xy[0], y=rest_position_xy[1], z=rest_position_z + ) + resting_pose.orientation = Quaternion( + x=quat[0], y=quat[1], z=quat[2], w=quat[3] + ) + + return resting_pose + def _solve_articutool_ik( self, target_vector: np.ndarray ) -> List[Tuple[float, float]]: @@ -1733,8 +1815,8 @@ def run(self): "stage_name": "Resting", "status": status.value, "execution_mode": ExecutionMode.SYNCHRONOUS.value, - "traj_jaco": traj_jaco, - "traj_atool": traj_atool, + "traj_jaco": self._serialize_trajectory(traj_jaco), + "traj_atool": self._serialize_trajectory(traj_atool), } ) if status != TrialStatus.SUCCESS: From 21a8e92a46dc64a186b036142d2c7472ac487eec Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 19 Aug 2025 17:23:43 -0700 Subject: [PATCH 203/238] Fix issue where JointTrajectoryPoint is not cast to list --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 700be3c7..7598c8d3 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1354,7 +1354,8 @@ def _verify_trajectory(self, trajectory: JointTrajectory) -> float: feasible_waypoints = 0 for point in trajectory.points: - if self._is_config_kinematically_feasible(point.positions): + jaco_joint_config = list(point.positions) + if self._is_config_kinematically_feasible(jaco_joint_config): feasible_waypoints += 1 return (feasible_waypoints / len(trajectory.points)) * 100.0 From 5c8ec49c66e9c34bf37e97310777c2dc6c948a5d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 19 Aug 2025 17:41:00 -0700 Subject: [PATCH 204/238] Ensure IK solution continuity when generating level-to-gravity Articutool trajectory --- .../scripts/end_to_end_feeding_benchmark.py | 54 +++++++++++++++---- 1 file changed, 44 insertions(+), 10 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 7598c8d3..b1a03b3f 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -934,12 +934,13 @@ def _generate_leveling_atool_trajectory( ) -> Optional[JointTrajectory]: """ Generates a synchronized Articutool trajectory that maintains a level-to-gravity - orientation during a Jaco arm motion. + orientation during a Jaco arm motion, ensuring solution continuity. """ if not traj_jaco or not traj_jaco.points: return None atool_solutions = [] + last_valid_solution = None # 1. Iterate through each waypoint of the Jaco trajectory for point in traj_jaco.points: @@ -955,29 +956,62 @@ def _generate_leveling_atool_trajectory( ) return None - q = R.from_matrix(jaco_wrist_transform.rotation).as_quat() p = jaco_wrist_transform.translation + q = R.from_matrix(jaco_wrist_transform.rotation).as_quat() jaco_wrist_pose = Pose( position=Point(x=p[0], y=p[1], z=p[2]), orientation=Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]), ) - # 3. Calculate the Articutool joints needed to be level at that wrist pose - atool_leveling_joints = self._compute_leveling_joints(jaco_wrist_pose) - if atool_leveling_joints is None: + # 3. Calculate all possible Articutool IK solutions for leveling + R_world_jacoee = R.from_quat([q[0], q[1], q[2], q[3]]) + target_up_in_wrist_frame = R_world_jacoee.inv().apply( + np.array([0.0, 0.0, 1.0]) + ) + ik_solutions = self._solve_articutool_ik(target_up_in_wrist_frame) + + # 4. Filter for solutions that are within joint limits + valid_solutions = [ + np.array(sol) + for sol in ik_solutions + if ( + ARTICUTOOL_PITCH_LIMITS_RAD[0] + <= sol[0] + <= ARTICUTOOL_PITCH_LIMITS_RAD[1] + and ARTICUTOOL_ROLL_LIMITS_RAD[0] + <= sol[1] + <= ARTICUTOOL_ROLL_LIMITS_RAD[1] + ) + ] + + if not valid_solutions: LOGGER.warning( - " Failed to find leveling IK solution for an Articutool waypoint." + " Failed to find any valid leveling IK solution for a waypoint." ) - return None + # As a fallback, try to re-use the last known good solution + if last_valid_solution is not None: + chosen_solution = last_valid_solution + else: + return None # Cannot proceed if the first waypoint has no solution + elif last_valid_solution is None: + # For the first waypoint, just pick the first valid solution + chosen_solution = valid_solutions[0] + else: + # For subsequent waypoints, choose the solution closest to the previous one + distances = [ + np.linalg.norm(sol - last_valid_solution) for sol in valid_solutions + ] + chosen_solution = valid_solutions[np.argmin(distances)] - atool_solutions.append(atool_leveling_joints) + atool_solutions.append(chosen_solution) + last_valid_solution = chosen_solution - # 4. Compile the solutions into a JointTrajectory message + # 5. Compile the continuous solutions into a JointTrajectory message traj_atool = JointTrajectory() traj_atool.joint_names = JOINT_NAMES_ATOOL for i, pos in enumerate(atool_solutions): point_msg = JointTrajectoryPoint() - point_msg.positions = pos + point_msg.positions = pos.tolist() point_msg.time_from_start = traj_jaco.points[i].time_from_start traj_atool.points.append(point_msg) From 46913bf7376fc6c8991d85b436a035d8ad144f1d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 19 Aug 2025 18:54:08 -0700 Subject: [PATCH 205/238] Enhance benchmark with granular metric tracking instrumentation --- .../scripts/end_to_end_feeding_benchmark.py | 390 ++++++++++++------ 1 file changed, 261 insertions(+), 129 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index b1a03b3f..92b30d9d 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -293,19 +293,19 @@ def plan( cartesian_max_step: float = 0.001, cartesian_jump_threshold: float = 5.0, cartesian_fraction_threshold: float = 0.92, - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: """ - Plans a trajectory for a given group based on a list of goal and path constraints. + Plans a trajectory and measures the planning time. Returns: - A status and the resulting trajectory, or None on failure. + A status, the resulting trajectory, and the planning time in seconds. """ planner = self._get_planner(group_name) future = None if not goal_constraints: LOGGER.error("Planning failed: At least one goal constraint is required.") - return TrialStatus.IK_FAILURE, None + return TrialStatus.IK_FAILURE, None, 0.0 if cartesian: planner.cartesian_jump_threshold = cartesian_jump_threshold @@ -324,7 +324,7 @@ def plan( LOGGER.error( f"Failed to transform Cartesian goal to base link: {e}" ) - return TrialStatus.IK_FAILURE, None + return TrialStatus.IK_FAILURE, None, 0.0 # --- Process Goal Constraints --- for constraint_type, kwargs in goal_constraints: @@ -354,13 +354,15 @@ def plan( max_step=cartesian_max_step, ) - # Wait for the future to complete outside the lock + # --- Start timing the planning operation --- start_time = time.time() while rclpy.ok() and not future.done(): if time.time() - start_time > self._planning_timeout: future.cancel() - return TrialStatus.PLANNER_FAILURE, None + return TrialStatus.PLANNER_FAILURE, None, self._planning_timeout time.sleep(0.1) + planning_time = time.time() - start_time + # --- End timing --- traj = planner.get_trajectory( future, @@ -368,7 +370,7 @@ def plan( cartesian_fraction_threshold=cartesian_fraction_threshold, ) if not traj or not traj.points: - return TrialStatus.PLANNER_FAILURE, None + return TrialStatus.PLANNER_FAILURE, None, planning_time # Scale velocity for cartesian plans, as pymoveit2 doesn't do this automatically if cartesian and planner.max_velocity > 0.0: @@ -376,7 +378,7 @@ def plan( traj, planner.max_velocity ) - return TrialStatus.SUCCESS, traj + return TrialStatus.SUCCESS, traj, planning_time class PinocchioModel: @@ -1348,6 +1350,45 @@ def _compute_leveling_joints(self, jaco_wrist_pose: Pose) -> Optional[List[float return None + def _calculate_cartesian_path_length( + self, trajectory: Optional[JointTrajectory], group_name: str + ) -> float: + """Calculates the Cartesian path length of the end-effector for a trajectory.""" + if ( + not trajectory + or not self.kinematics_model.is_ready() + or len(trajectory.points) < 2 + ): + return 0.0 + + total_length = 0.0 + last_position = None + joint_names = trajectory.joint_names + + # Determine the correct end-effector link for the planning group + if group_name == PLANNING_GROUP_JACO: + ee_link = END_EFFECTOR_LINK_JACO + else: + ee_link = END_EFFECTOR_LINK_FULL + + for point in trajectory.points: + joint_map = dict(zip(joint_names, point.positions)) + jaco_config = [joint_map.get(name, 0.0) for name in JOINT_NAMES_JACO] + atool_config = [joint_map.get(name, 0.0) for name in JOINT_NAMES_ATOOL] + + transform = self.kinematics_model.get_frame_transform( + ee_link, jaco_config, atool_config + ) + if transform is None: + continue + + current_position = transform.translation + if last_position is not None: + total_length += np.linalg.norm(current_position - last_position) + last_position = current_position + + return total_length + # --- Feasibility Checking --- def _is_config_kinematically_feasible(self, jaco_joint_config: List[float]) -> bool: """ @@ -1433,7 +1474,7 @@ def _serialize_trajectory( # --- Planning Primitive --- def _plan_to_above_plate( self, above_plate_pose: Pose, start_state_jaco: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: LOGGER.info(" Planning to AbovePlate pose...") goal_constraints = [create_pose_constraint(above_plate_pose)] @@ -1448,7 +1489,9 @@ def _plan_to_above_food( above_food_pose: Pose, start_state_jaco: List[float], start_state_atool: List[float], - ) -> Tuple[TrialStatus, Optional[JointTrajectory], Optional[JointTrajectory]]: + ) -> Tuple[ + TrialStatus, Optional[JointTrajectory], Optional[JointTrajectory], float + ]: LOGGER.info(" Solving 8-DOF IK for AboveFood pose...") # 1. Compute IK using the planner @@ -1460,7 +1503,7 @@ def _plan_to_above_food( if not ik_solution: LOGGER.warning(" IK solution for 8-DOF system not found.") - return TrialStatus.IK_FAILURE, None, None + return TrialStatus.IK_FAILURE, None, None, 0.0 LOGGER.info(f" 8-DOF IK solution found. Planning for subgroups.") solution_map = dict(zip(ik_solution.name, ik_solution.position)) @@ -1469,7 +1512,7 @@ def _plan_to_above_food( # 2. Plan for the Jaco arm jaco_goal_constraints = [create_joint_constraint(target_jaco_config)] - status_jaco, traj_jaco = self.motion_planner.plan( + status_jaco, traj_jaco, time_jaco = self.motion_planner.plan( group_name=PLANNING_GROUP_JACO, start_state=start_state_jaco, goal_constraints=jaco_goal_constraints, @@ -1477,21 +1520,22 @@ def _plan_to_above_food( if status_jaco != TrialStatus.SUCCESS: LOGGER.warning(" Jaco arm planning failed.") - return TrialStatus.PLANNER_FAILURE, None, None + return TrialStatus.PLANNER_FAILURE, None, None, time_jaco # 3. Plan for the Articutool atool_goal_constraints = [create_joint_constraint(target_atool_config)] - status_atool, traj_atool = self.motion_planner.plan( + status_atool, traj_atool, time_atool = self.motion_planner.plan( group_name=PLANNING_GROUP_ATOOL, start_state=start_state_atool, goal_constraints=atool_goal_constraints, ) + total_time = time_jaco + time_atool if status_atool != TrialStatus.SUCCESS: LOGGER.warning(" Articutool planning failed.") - return TrialStatus.PLANNER_FAILURE, traj_jaco, None + return TrialStatus.PLANNER_FAILURE, traj_jaco, None, total_time - return TrialStatus.SUCCESS, traj_jaco, traj_atool + return TrialStatus.SUCCESS, traj_jaco, traj_atool, total_time def _plan_to_in_food( self, @@ -1503,6 +1547,7 @@ def _plan_to_in_food( Optional[JointTrajectory], Optional[JointTrajectory], Optional[Pose], + float, ]: LOGGER.info(" Solving 8-DOF IK for InFood pose...") @@ -1515,7 +1560,7 @@ def _plan_to_in_food( if not ik_solution: LOGGER.warning(" IK solution for 8-DOF system not found.") - return TrialStatus.IK_FAILURE, None, None, None + return TrialStatus.IK_FAILURE, None, None, None, 0.0 LOGGER.info(f" 8-DOF IK solution found. Calculating wrist pose via FK.") @@ -1528,7 +1573,7 @@ def _plan_to_in_food( if not fk_poses: LOGGER.warning(" FK calculation for Jaco wrist failed.") - return TrialStatus.IK_FAILURE, None, None, None + return TrialStatus.IK_FAILURE, None, None, None, 0.0 # The goal for the Jaco arm is the calculated pose of its wrist jaco_wrist_goal_pose = fk_poses[0].pose @@ -1537,7 +1582,7 @@ def _plan_to_in_food( jaco_goal_constraints = [create_pose_constraint(jaco_wrist_goal_pose)] # 4. Plan a Cartesian motion for the Jaco arm to the wrist pose - status_jaco, traj_jaco = self.motion_planner.plan( + status_jaco, traj_jaco, planning_time = self.motion_planner.plan( group_name=PLANNING_GROUP_JACO, start_state=start_state_jaco, goal_constraints=jaco_goal_constraints, @@ -1546,7 +1591,7 @@ def _plan_to_in_food( if status_jaco != TrialStatus.SUCCESS: LOGGER.warning(" Jaco arm planning failed.") - return TrialStatus.PLANNER_FAILURE, None, None, None + return TrialStatus.PLANNER_FAILURE, None, None, None, planning_time # 4. Generate the corresponding synchronous Articutool trajectory LOGGER.info(" Generating synchronous Articutool trajectory...") @@ -1556,15 +1601,21 @@ def _plan_to_in_food( if traj_atool is None: LOGGER.error(" Failed to generate synchronous Articutool trajectory.") - return TrialStatus.IK_FAILURE, traj_jaco, None, None + return TrialStatus.IK_FAILURE, traj_jaco, None, None, planning_time - return TrialStatus.SUCCESS, traj_jaco, traj_atool, jaco_wrist_goal_pose + return ( + TrialStatus.SUCCESS, + traj_jaco, + traj_atool, + jaco_wrist_goal_pose, + planning_time, + ) def _plan_to_level_articutool( self, jaco_wrist_pose: Pose, start_state_atool: List[float], - ) -> Tuple[TrialStatus, Optional[JointTrajectory]]: + ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: """ Calculates the required joint angles for the Articutool to achieve a level pose and plans a trajectory to that configuration. @@ -1576,13 +1627,13 @@ def _plan_to_level_articutool( if target_leveling_joints is None: LOGGER.warning(" Could not find an IK solution for Articutool leveling.") - return TrialStatus.IK_FAILURE, None + return TrialStatus.IK_FAILURE, None, 0.0 # 2. Create a joint goal constraint for the Articutool goal_constraints = [create_joint_constraint(target_leveling_joints)] # 3. Plan a joint-space motion for the Articutool group - status, trajectory = self.motion_planner.plan( + status, trajectory, planning_time = self.motion_planner.plan( group_name=PLANNING_GROUP_ATOOL, start_state=start_state_atool, goal_constraints=goal_constraints, @@ -1590,17 +1641,21 @@ def _plan_to_level_articutool( if status != TrialStatus.SUCCESS: LOGGER.warning(" Articutool leveling plan failed.") - return TrialStatus.PLANNER_FAILURE, None + return TrialStatus.PLANNER_FAILURE, None, planning_time LOGGER.info(" Articutool leveling plan successful.") - return TrialStatus.SUCCESS, trajectory + return TrialStatus.SUCCESS, trajectory, planning_time def _plan_to_resting( self, resting_wrist_pose: Pose, start_state_jaco: List[float], ) -> Tuple[ - TrialStatus, Optional[JointTrajectory], Optional[JointTrajectory], float + TrialStatus, + Optional[JointTrajectory], + Optional[JointTrajectory], + float, + float, ]: """ Plans a 6-DOF guided motion for the Jaco arm to a resting wrist pose @@ -1618,14 +1673,14 @@ def _plan_to_resting( ] # 2. Plan the guided 6-DOF trajectory for the Jaco arm - status, traj_jaco = self.motion_planner.plan( + status, traj_jaco, planning_time = self.motion_planner.plan( group_name=PLANNING_GROUP_JACO, start_state=start_state_jaco, goal_constraints=goal_constraints, path_constraints=path_constraints, ) if status != TrialStatus.SUCCESS: - return TrialStatus.PLANNER_FAILURE, None, None, 0.0 + return TrialStatus.PLANNER_FAILURE, None, None, 0.0, planning_time # 3. VERIFY the Jaco trajectory for leveling feasibility feasibility_percent = self._verify_trajectory(traj_jaco) @@ -1638,6 +1693,7 @@ def _plan_to_resting( traj_jaco, None, feasibility_percent, + planning_time, ) # 4. Generate the corresponding synchronous Articutool trajectory @@ -1646,8 +1702,20 @@ def _plan_to_resting( if traj_atool is None: LOGGER.error(" Failed to generate synchronous Articutool trajectory.") - return TrialStatus.IK_FAILURE, traj_jaco, None, feasibility_percent - return TrialStatus.SUCCESS, traj_jaco, traj_atool, feasibility_percent + return ( + TrialStatus.IK_FAILURE, + traj_jaco, + None, + feasibility_percent, + planning_time, + ) + return ( + TrialStatus.SUCCESS, + traj_jaco, + traj_atool, + feasibility_percent, + planning_time, + ) def _plan_s2_guided( self, goal_pose: Pose, start_state: Any @@ -1698,7 +1766,7 @@ def _plan_s2_guided( # --- Main Benchmark Loop --- def run(self): - """Main benchmark execution loop.""" + """Main benchmark execution loop with granular metric collection.""" self.add_articutool_bounding_cylinder() for i in range(self.num_trials): @@ -1751,119 +1819,183 @@ def run(self): "staging_pose": self._serialize_pose(scene["staging_pose"]), "resting_pose": self._serialize_pose(scene["resting_pose"]), }, + "parameters": { # Placeholder for future parameter optimization + "angular_offset_deg": 20, + "radial_distance_m": 0.8, + }, "stages": [], + "end_to_end_success": False, # Default to False } # 2. Initialize the robot's state for the trial current_jaco_state = scene["home_config"] current_atool_state = [0.0, 0.0] + trial_failed = False # --- Stage 1: Home -> AbovePlate --- - LOGGER.info("Stage 1: Home -> AbovePlate") - status, traj_jaco = self._plan_to_above_plate( - scene["above_plate_pose"], current_jaco_state - ) - trial_data["stages"].append( - { - "stage_name": "HomeToAbovePlate", - "status": status.value, - "execution_mode": ExecutionMode.JACO_ONLY.value, - "traj_jaco": self._serialize_trajectory(traj_jaco), - "traj_atool": None, - } - ) - if status != TrialStatus.SUCCESS: - LOGGER.error(f" Stage 1 failed. Skipping trial.") - self.results.append(trial_data) - continue - current_jaco_state = list(traj_jaco.points[-1].positions) + if not trial_failed: + LOGGER.info("Stage 1: Home -> AbovePlate") + status, traj_jaco, planning_time = self._plan_to_above_plate( + scene["above_plate_pose"], current_jaco_state + ) + path_length = self._calculate_cartesian_path_length( + traj_jaco, PLANNING_GROUP_JACO + ) + trial_data["stages"].append( + { + "stage_name": "HomeToAbovePlate", + "status": status.value, + "execution_mode": ExecutionMode.JACO_ONLY.value, + "planning_time_sec": planning_time, + "trajectory_path_length_m": path_length, + "custom_metrics": {}, + "traj_jaco": self._serialize_trajectory(traj_jaco), + "traj_atool": None, + } + ) + if status != TrialStatus.SUCCESS: + LOGGER.error(f" Stage 1 failed. Skipping trial.") + trial_failed = True + else: + current_jaco_state = list(traj_jaco.points[-1].positions) # --- Stage 2: AbovePlate -> AboveFood --- - LOGGER.info("Stage 2: AbovePlate -> AboveFood") - status, traj_jaco, traj_atool = self._plan_to_above_food( - scene["above_food_pose"], current_jaco_state, current_atool_state - ) - trial_data["stages"].append( - { - "stage_name": "AbovePlateToAboveFood", - "status": status.value, - "execution_mode": ExecutionMode.SEQUENTIAL.value, - "traj_jaco": self._serialize_trajectory(traj_jaco), - "traj_atool": self._serialize_trajectory(traj_atool), - } - ) - if status != TrialStatus.SUCCESS: - LOGGER.error(f" Stage 2 failed. Skipping trial.") - self.results.append(trial_data) - continue - current_jaco_state = list(traj_jaco.points[-1].positions) - current_atool_state = list(traj_atool.points[-1].positions) + if not trial_failed: + LOGGER.info("Stage 2: AbovePlate -> AboveFood") + ( + status, + traj_jaco, + traj_atool, + planning_time, + ) = self._plan_to_above_food( + scene["above_food_pose"], current_jaco_state, current_atool_state + ) + path_length_jaco = self._calculate_cartesian_path_length( + traj_jaco, PLANNING_GROUP_JACO + ) + path_length_atool = self._calculate_cartesian_path_length( + traj_atool, PLANNING_GROUP_ATOOL + ) + trial_data["stages"].append( + { + "stage_name": "AbovePlateToAboveFood", + "status": status.value, + "execution_mode": ExecutionMode.SEQUENTIAL.value, + "planning_time_sec": planning_time, + "trajectory_path_length_m": path_length_jaco + + path_length_atool, + "custom_metrics": {}, + "traj_jaco": self._serialize_trajectory(traj_jaco), + "traj_atool": self._serialize_trajectory(traj_atool), + } + ) + if status != TrialStatus.SUCCESS: + LOGGER.error(f" Stage 2 failed. Skipping trial.") + trial_failed = True + else: + current_jaco_state = list(traj_jaco.points[-1].positions) + current_atool_state = list(traj_atool.points[-1].positions) # --- Stage 3: AboveFood -> InFood --- - LOGGER.info("Stage 3: AboveFood -> InFood (Cartesian)") - status, traj_jaco, traj_atool, jaco_wrist_pose = self._plan_to_in_food( - scene["in_food_pose"], current_jaco_state, current_atool_state - ) - trial_data["stages"].append( - { - "stage_name": "AboveFoodToInFood", - "status": status.value, - "execution_mode": ExecutionMode.SYNCHRONOUS.value, - "traj_jaco": self._serialize_trajectory(traj_jaco), - "traj_atool": self._serialize_trajectory(traj_atool), - } - ) - if status != TrialStatus.SUCCESS: - LOGGER.error(f" Stage 3 failed. Skipping trial.") - self.results.append(trial_data) - continue - current_jaco_state = list(traj_jaco.points[-1].positions) - current_atool_state = list(traj_atool.points[-1].positions) + if not trial_failed: + LOGGER.info("Stage 3: AboveFood -> InFood (Cartesian)") + ( + status, + traj_jaco, + traj_atool, + jaco_wrist_pose, + planning_time, + ) = self._plan_to_in_food( + scene["in_food_pose"], current_jaco_state, current_atool_state + ) + path_length = self._calculate_cartesian_path_length( + traj_jaco, PLANNING_GROUP_JACO + ) + trial_data["stages"].append( + { + "stage_name": "AboveFoodToInFood", + "status": status.value, + "execution_mode": ExecutionMode.SYNCHRONOUS.value, + "planning_time_sec": planning_time, + "trajectory_path_length_m": path_length, + "custom_metrics": {}, + "traj_jaco": self._serialize_trajectory(traj_jaco), + "traj_atool": self._serialize_trajectory(traj_atool), + } + ) + if status != TrialStatus.SUCCESS: + LOGGER.error(f" Stage 3 failed. Skipping trial.") + trial_failed = True + else: + current_jaco_state = list(traj_jaco.points[-1].positions) + current_atool_state = list(traj_atool.points[-1].positions) # --- Stage 4: InFood -> LevelArticutool --- - LOGGER.info("Stage 4: Level Articutool") - status, traj_atool = self._plan_to_level_articutool( - jaco_wrist_pose, current_atool_state - ) - trial_data["stages"].append( - { - "stage_name": "LevelArticutool", - "status": status.value, - "execution_mode": ExecutionMode.ATOOL_ONLY.value, - "traj_jaco": None, - "traj_atool": self._serialize_trajectory(traj_atool), - } - ) - if status != TrialStatus.SUCCESS: - LOGGER.error(f" Stage 4 failed. Skipping trial.") - self.results.append(trial_data) - continue - current_atool_state = list(traj_atool.points[-1].positions) + if not trial_failed: + LOGGER.info("Stage 4: Level Articutool") + status, traj_atool, planning_time = self._plan_to_level_articutool( + jaco_wrist_pose, current_atool_state + ) + path_length = self._calculate_cartesian_path_length( + traj_atool, PLANNING_GROUP_ATOOL + ) + trial_data["stages"].append( + { + "stage_name": "LevelArticutool", + "status": status.value, + "execution_mode": ExecutionMode.ATOOL_ONLY.value, + "planning_time_sec": planning_time, + "trajectory_path_length_m": path_length, + "custom_metrics": {}, + "traj_jaco": None, + "traj_atool": self._serialize_trajectory(traj_atool), + } + ) + if status != TrialStatus.SUCCESS: + LOGGER.error(f" Stage 4 failed. Skipping trial.") + trial_failed = True + else: + current_atool_state = list(traj_atool.points[-1].positions) # --- Stage 5: LevelArticutool -> Resting --- - LOGGER.info("Stage 5: Resting") - status, traj_jaco, traj_atool, leveling_feasibility = self._plan_to_resting( - scene["resting_pose"], current_jaco_state - ) - trial_data["stages"].append( - { - "stage_name": "Resting", - "status": status.value, - "execution_mode": ExecutionMode.SYNCHRONOUS.value, - "traj_jaco": self._serialize_trajectory(traj_jaco), - "traj_atool": self._serialize_trajectory(traj_atool), - } - ) - if status != TrialStatus.SUCCESS: - LOGGER.error(f" Stage 5 failed. Skipping trial.") - self.results.append(trial_data) - continue - current_jaco_state = list(traj_jaco.points[-1].positions) - current_atool_state = list(traj_atool.points[-1].positions) + if not trial_failed: + LOGGER.info("Stage 5: Resting") + ( + status, + traj_jaco, + traj_atool, + leveling_feasibility, + planning_time, + ) = self._plan_to_resting(scene["resting_pose"], current_jaco_state) + path_length = self._calculate_cartesian_path_length( + traj_jaco, PLANNING_GROUP_JACO + ) + trial_data["stages"].append( + { + "stage_name": "Resting", + "status": status.value, + "execution_mode": ExecutionMode.SYNCHRONOUS.value, + "planning_time_sec": planning_time, + "trajectory_path_length_m": path_length, + "custom_metrics": { + "leveling_feasibility_percent": leveling_feasibility + }, + "traj_jaco": self._serialize_trajectory(traj_jaco), + "traj_atool": self._serialize_trajectory(traj_atool), + } + ) + if status != TrialStatus.SUCCESS: + LOGGER.error(f" Stage 5 failed. Skipping trial.") + trial_failed = True + else: + current_jaco_state = list(traj_jaco.points[-1].positions) + current_atool_state = list(traj_atool.points[-1].positions) - # At the end of a successful trial, append the complete trial data - self.results.append(trial_data) + # --- Finalize Trial --- + if not trial_failed: + trial_data["end_to_end_success"] = True + self.results.append(trial_data) self.save_results() def save_results(self): From 60e51c8c605171fcc1e7f482e8b79257b001c380 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 19 Aug 2025 19:22:36 -0700 Subject: [PATCH 206/238] Add script to analyze benchmark results and produce aggregate statistics --- ada_feeding/scripts/analyze_benchmark.py | 92 ++++++++++++++++++++++++ 1 file changed, 92 insertions(+) create mode 100644 ada_feeding/scripts/analyze_benchmark.py diff --git a/ada_feeding/scripts/analyze_benchmark.py b/ada_feeding/scripts/analyze_benchmark.py new file mode 100644 index 00000000..42703ba6 --- /dev/null +++ b/ada_feeding/scripts/analyze_benchmark.py @@ -0,0 +1,92 @@ +import argparse +import json +import pandas as pd + + +def analyze_benchmark_results(file_path: str): + """ + Loads benchmark data, calculates key metrics, and prints a summary report. + """ + try: + with open(file_path, "r") as f: + data = json.load(f) + except (FileNotFoundError, json.JSONDecodeError) as e: + print(f"Error reading or parsing file: {e}") + return + + if not data: + print("No data found in the benchmark file.") + return + + # --- 1. Flatten the data for easier analysis with pandas --- + # Each row in this DataFrame will represent a single stage from a single trial. + flat_stages = [] + for trial in data: + for stage in trial.get("stages", []): + flat_record = { + "trial_id": trial.get("trial_id"), + "end_to_end_success": trial.get("end_to_end_success"), + "stage_name": stage.get("stage_name"), + "status": stage.get("status"), + "planning_time_sec": stage.get("planning_time_sec"), + "path_length_m": stage.get("trajectory_path_length_m"), + } + # Add custom metrics if they exist + if stage.get("custom_metrics"): + flat_record.update(stage["custom_metrics"]) + flat_stages.append(flat_record) + + df = pd.DataFrame(flat_stages) + + # --- 2. Calculate and Print Key Metrics --- + num_trials = len(data) + successful_trials = sum(1 for trial in data if trial.get("end_to_end_success")) + overall_success_rate = ( + (successful_trials / num_trials) * 100 if num_trials > 0 else 0 + ) + + print("\n" + "=" * 50) + print(" BENCHMARK ANALYSIS REPORT") + print("=" * 50) + print(f"File: {file_path}") + print(f"Total Trials: {num_trials}") + print( + f"End-to-End Success Rate: {overall_success_rate:.1f}% ({successful_trials}/{num_trials})" + ) + print("-" * 50) + + if not df.empty: + # --- Failure analysis by STAGE --- + failed_stages_df = df[df["status"] != "Success"] + stage_failure_counts = failed_stages_df["stage_name"].value_counts() + print("\nFailure Count by Stage:") + if not stage_failure_counts.empty: + print(stage_failure_counts.to_string()) + else: + print("No stage failures recorded.") + print("-" * 50) + + # --- Failure analysis by TYPE for the most problematic stage --- + if not stage_failure_counts.empty: + most_problematic_stage = stage_failure_counts.index[0] + print( + f"\nFailure Breakdown for Most Problematic Stage ('{most_problematic_stage}'):" + ) + problem_stage_df = failed_stages_df[ + failed_stages_df["stage_name"] == most_problematic_stage + ] + status_counts = problem_stage_df["status"].value_counts() + print(status_counts.to_string()) + print("=" * 50) + + +if __name__ == "__main__": + parser = argparse.ArgumentParser( + description="Analyze end-to-end benchmark results." + ) + parser.add_argument( + "benchmark_file", type=str, help="Path to the benchmark JSON output file." + ) + args = parser.parse_args() + + analyze_benchmark_results(args.benchmark_file) From c5aba4b180170949e3b4c7d2c9f8e328772ee0a5 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Tue, 19 Aug 2025 19:36:53 -0700 Subject: [PATCH 207/238] Comment out verbose scene log --- .../scripts/end_to_end_feeding_benchmark.py | 68 +++++++++---------- 1 file changed, 34 insertions(+), 34 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 92b30d9d..cd3be706 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1774,40 +1774,40 @@ def run(self): # 1. Generate scene and create the top-level dictionary for the trial scene = self._generate_scene() - LOGGER.info( - f""" - --- Generated Scene Parameters for Trial {i + 1} --- - - - Initial State: - - Home Config: [{", ".join(f"{j:.4f}" for j in scene["home_config"])}] - - - Core Sampled Poses: - - Food Pose: - Position: [x={scene["food_pose"].position.x:.3f}, y={scene["food_pose"].position.y:.3f}, z={scene["food_pose"].position.z:.3f}] - Orientation: [x={scene["food_pose"].orientation.x:.3f}, y={scene["food_pose"].orientation.y:.3f}, z={scene["food_pose"].orientation.z:.3f}, w={scene["food_pose"].orientation.w:.3f}] - - Mouth Pose: - Position: [x={scene["mouth_pose"].position.x:.3f}, y={scene["mouth_pose"].position.y:.3f}, z={scene["mouth_pose"].position.z:.3f}] - Orientation: [x={scene["mouth_pose"].orientation.x:.3f}, y={scene["mouth_pose"].orientation.y:.3f}, z={scene["mouth_pose"].orientation.z:.3f}, w={scene["mouth_pose"].orientation.w:.3f}] - - - Derived Poses for Feeding Cycle: - - Above Plate Pose: - Position: [x={scene["above_plate_pose"].position.x:.3f}, y={scene["above_plate_pose"].position.y:.3f}, z={scene["above_plate_pose"].position.z:.3f}] - Orientation: [x={scene["above_plate_pose"].orientation.x:.3f}, y={scene["above_plate_pose"].orientation.y:.3f}, z={scene["above_plate_pose"].orientation.z:.3f}, w={scene["above_plate_pose"].orientation.w:.3f}] - - Above Food Pose: - Position: [x={scene["above_food_pose"].position.x:.3f}, y={scene["above_food_pose"].position.y:.3f}, z={scene["above_food_pose"].position.z:.3f}] - Orientation: [x={scene["above_food_pose"].orientation.x:.3f}, y={scene["above_food_pose"].orientation.y:.3f}, z={scene["above_food_pose"].orientation.z:.3f}, w={scene["above_food_pose"].orientation.w:.3f}] - - In Food Pose: - Position: [x={scene["in_food_pose"].position.x:.3f}, y={scene["in_food_pose"].position.y:.3f}, z={scene["in_food_pose"].position.z:.3f}] - Orientation: [x={scene["in_food_pose"].orientation.x:.3f}, y={scene["in_food_pose"].orientation.y:.3f}, z={scene["in_food_pose"].orientation.z:.3f}, w={scene["in_food_pose"].orientation.w:.3f}] - - Staging Pose: - Position: [x={scene["staging_pose"].position.x:.3f}, y={scene["staging_pose"].position.y:.3f}, z={scene["staging_pose"].position.z:.3f}] - Orientation: [x={scene["staging_pose"].orientation.x:.3f}, y={scene["staging_pose"].orientation.y:.3f}, z={scene["staging_pose"].orientation.z:.3f}, w={scene["staging_pose"].orientation.w:.3f}] - - Resting Pose: - Position: [x={scene["resting_pose"].position.x:.3f}, y={scene["resting_pose"].position.y:.3f}, z={scene["resting_pose"].position.z:.3f}] - Orientation: [x={scene["resting_pose"].orientation.x:.3f}, y={scene["resting_pose"].orientation.y:.3f}, z={scene["resting_pose"].orientation.z:.3f}, w={scene["resting_pose"].orientation.w:.3f}] - ------------------------------------------------- - """ - ) + # LOGGER.info( + # f""" + # --- Generated Scene Parameters for Trial {i + 1} --- + # + # - Initial State: + # - Home Config: [{", ".join(f"{j:.4f}" for j in scene["home_config"])}] + # + # - Core Sampled Poses: + # - Food Pose: + # Position: [x={scene["food_pose"].position.x:.3f}, y={scene["food_pose"].position.y:.3f}, z={scene["food_pose"].position.z:.3f}] + # Orientation: [x={scene["food_pose"].orientation.x:.3f}, y={scene["food_pose"].orientation.y:.3f}, z={scene["food_pose"].orientation.z:.3f}, w={scene["food_pose"].orientation.w:.3f}] + # - Mouth Pose: + # Position: [x={scene["mouth_pose"].position.x:.3f}, y={scene["mouth_pose"].position.y:.3f}, z={scene["mouth_pose"].position.z:.3f}] + # Orientation: [x={scene["mouth_pose"].orientation.x:.3f}, y={scene["mouth_pose"].orientation.y:.3f}, z={scene["mouth_pose"].orientation.z:.3f}, w={scene["mouth_pose"].orientation.w:.3f}] + # + # - Derived Poses for Feeding Cycle: + # - Above Plate Pose: + # Position: [x={scene["above_plate_pose"].position.x:.3f}, y={scene["above_plate_pose"].position.y:.3f}, z={scene["above_plate_pose"].position.z:.3f}] + # Orientation: [x={scene["above_plate_pose"].orientation.x:.3f}, y={scene["above_plate_pose"].orientation.y:.3f}, z={scene["above_plate_pose"].orientation.z:.3f}, w={scene["above_plate_pose"].orientation.w:.3f}] + # - Above Food Pose: + # Position: [x={scene["above_food_pose"].position.x:.3f}, y={scene["above_food_pose"].position.y:.3f}, z={scene["above_food_pose"].position.z:.3f}] + # Orientation: [x={scene["above_food_pose"].orientation.x:.3f}, y={scene["above_food_pose"].orientation.y:.3f}, z={scene["above_food_pose"].orientation.z:.3f}, w={scene["above_food_pose"].orientation.w:.3f}] + # - In Food Pose: + # Position: [x={scene["in_food_pose"].position.x:.3f}, y={scene["in_food_pose"].position.y:.3f}, z={scene["in_food_pose"].position.z:.3f}] + # Orientation: [x={scene["in_food_pose"].orientation.x:.3f}, y={scene["in_food_pose"].orientation.y:.3f}, z={scene["in_food_pose"].orientation.z:.3f}, w={scene["in_food_pose"].orientation.w:.3f}] + # - Staging Pose: + # Position: [x={scene["staging_pose"].position.x:.3f}, y={scene["staging_pose"].position.y:.3f}, z={scene["staging_pose"].position.z:.3f}] + # Orientation: [x={scene["staging_pose"].orientation.x:.3f}, y={scene["staging_pose"].orientation.y:.3f}, z={scene["staging_pose"].orientation.z:.3f}, w={scene["staging_pose"].orientation.w:.3f}] + # - Resting Pose: + # Position: [x={scene["resting_pose"].position.x:.3f}, y={scene["resting_pose"].position.y:.3f}, z={scene["resting_pose"].position.z:.3f}] + # Orientation: [x={scene["resting_pose"].orientation.x:.3f}, y={scene["resting_pose"].orientation.y:.3f}, z={scene["resting_pose"].orientation.z:.3f}, w={scene["resting_pose"].orientation.w:.3f}] + # ------------------------------------------------- + # """ + # ) trial_data = { "trial_id": i, "scene_poses": { From 2d76149d9d054ffe1d72b57931413a992a11708c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 09:47:39 -0700 Subject: [PATCH 208/238] Major refactor to abstract scene generation into a dedicated class --- .../scripts/end_to_end_feeding_benchmark.py | 707 ++++++++++-------- 1 file changed, 395 insertions(+), 312 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index cd3be706..fd0b9dbf 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -24,7 +24,7 @@ import sys import argparse from enum import Enum, auto -from dataclasses import dataclass +from dataclasses import dataclass, asdict # Third-party imports import numpy as np @@ -125,9 +125,41 @@ def __str__(self): ) -class MoveIt2ConstraintType(Enum): - """Specifies the type of constraint to be applied.""" +@dataclass +class CylindricalSamplingParams: + """Parameters for sampling a pose within a cylindrical shell.""" + + name: str + inner_radius: float + outer_radius: float + min_height: float + max_height: float + +@dataclass +class SceneGenerationParams: + """Holds all parameters that define the random scene generation.""" + + food_sampling: CylindricalSamplingParams = CylindricalSamplingParams( + name="food", inner_radius=0.4, outer_radius=0.7, min_height=0.0, max_height=0.3 + ) + mouth_sampling: CylindricalSamplingParams = CylindricalSamplingParams( + name="mouth", inner_radius=0.3, outer_radius=0.6, min_height=0.0, max_height=0.6 + ) + above_plate_radial_dist: float = 0.3 + above_plate_polar_angle_rad_max: float = math.pi / 4 # 45 deg + above_plate_yaw_variability_rad: float = math.pi / 4 # 45 deg + skewer_polar_angle_rad_max: float = math.pi / 3 # 60 deg + in_food_tool_roll_angle_deg: float = 180.0 + above_food_offset_dist: float = 0.1 # 10 cm + staging_offset_dist: float = 0.15 # 15 cm + resting_angular_offset_deg: float = 20.0 + resting_radial_dist: float = 0.8 + resting_vertical_offset: float = ARTICUTOOL_LENGTH_M + 0.2 + + +# --- Constraint Helpers --- +class MoveIt2ConstraintType(Enum): JOINT = "joint" POSITION = "position" ORIENTATION = "orientation" @@ -171,6 +203,353 @@ def create_joint_constraint( return (MoveIt2ConstraintType.JOINT, {"joint_positions": joint_positions}) +class SceneGenerator: + """A dedicated class for procedurally generating planning scenes.""" + + def __init__(self, params: SceneGenerationParams): + self.params = params + + def generate(self) -> Tuple[Dict[str, Any], Dict[str, float]]: + """ + Generates a randomized scene and returns the scene dict and sampled parameters. + """ + scene = {} + scene_characteristics = {} + + scene["food_pose"], food_params = self._sample_pose_in_cylindrical_shell( + self.params.food_sampling + ) + scene["mouth_pose"], mouth_params = self._sample_pose_in_cylindrical_shell( + self.params.mouth_sampling + ) + scene_characteristics.update(food_params) + scene_characteristics.update(mouth_params) + + scene["home_config"] = [-1.47568, 2.92779, 1.00845, -2.0847, 1.43588, 1.32575] + + scene["above_plate_pose"], above_plate_params = ( + self._calculate_above_plate_pose(scene["food_pose"]) + ) + scene_characteristics.update(above_plate_params) + + ( + scene["in_food_pose"], + approach_vector, + in_food_params, + ) = self._calculate_in_food_pose( + food_pose=scene["food_pose"], + recipe=ActionRecipe( + AcquisitionStrategy.SKEWER, + MotionAxis.VERTICAL, + ToolAlignment.PERPENDICULAR, + ), + ) + scene_characteristics.update(in_food_params) + + scene["above_food_pose"] = self._calculate_above_food_pose( + scene["in_food_pose"], approach_vector + ) + scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) + scene["resting_pose"] = self._calculate_resting_pose( + scene["food_pose"], scene["mouth_pose"] + ) + + # Add derived characteristics for analysis + food_pos = scene["food_pose"].position + mouth_pos = scene["mouth_pose"].position + scene_characteristics["food_mouth_distance_m"] = math.sqrt( + (food_pos.x - mouth_pos.x) ** 2 + + (food_pos.y - mouth_pos.y) ** 2 + + (food_pos.z - mouth_pos.z) ** 2 + ) + + return scene, scene_characteristics + + def _sample_pose_in_cylindrical_shell( + self, sampling_params: CylindricalSamplingParams + ) -> Tuple[Pose, Dict[str, float]]: + """ + Samples a random pose within a cylindrical shell + The frame's Z-axis is constrained to be world up, and its X-axis is + oriented to point towards the robot base with some random variability. + """ + # --- Position Sampling in a Cylindrical Shell --- + # 1. Sample the radius and angle + radius = np.sqrt( + np.random.uniform( + sampling_params.inner_radius**2, sampling_params.outer_radius**2 + ) + ) + theta = np.random.uniform(0, 2 * np.pi) + + # 2. Convert to Cartesian coordinates + x = radius * np.cos(theta) + y = radius * np.sin(theta) + z = np.random.uniform(sampling_params.min_height, sampling_params.max_height) + position = Point(x=x, y=y, z=z) + + # --- Orientation Calculation --- + z_axis = np.array([0.0, 0.0, 1.0]) + look_at_vector = -np.array([x, y, 0.0]) # Project to XY plane + if np.linalg.norm(look_at_vector) < 1e-6: + look_at_vector = np.array([1.0, 0.0, 0.0]) + x_axis_direction = look_at_vector / np.linalg.norm(look_at_vector) + + y_axis = np.cross(z_axis, x_axis_direction) + rotation_matrix = np.array([x_axis_direction, y_axis, z_axis]).T + main_rot = R.from_matrix(rotation_matrix) + + rand_yaw_angle = np.random.uniform(-np.deg2rad(30), np.deg2rad(30)) + variability_rot = R.from_euler("z", rand_yaw_angle) + + final_rot = main_rot * variability_rot + quat = final_rot.as_quat() + + # --- Assemble final pose and sampled characteristics --- + final_pose = Pose( + position=position, + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), + ) + + prefix = sampling_params.name + sampled_values = { + f"{prefix}_sampled_radius": radius, + f"{prefix}_sampled_theta_rad": theta, + f"{prefix}_sampled_z": z, + } + return final_pose, sampled_values + + def _calculate_above_plate_pose( + self, food_pose: Pose + ) -> Tuple[Pose, Dict[str, float]]: + """ + Calculates a camera pose for the Jaco end-effector that looks at the food, + with a yaw constraint that keeps the arm aligned with the robot base. + """ + food_position = np.array( + [food_pose.position.x, food_pose.position.y, food_pose.position.z] + ) + + # --- Position Calculation with Yaw Constraint --- + # 1. Determine the base yaw angle from the robot's origin to the food's XY position. + base_yaw_angle = np.arctan2(food_position[1], food_position[0]) + + # 2. Add random variability to this base angle. + yaw_variability = np.random.uniform( + -self.params.above_plate_yaw_variability_rad, + self.params.above_plate_yaw_variability_rad, + ) + final_azimuthal_angle = base_yaw_angle + yaw_variability + np.pi + + # 3. Use spherical coordinates relative to the food pose to find the camera position. + radial_distance = self.params.above_plate_radial_dist + polar_angle = np.random.uniform(0, self.params.above_plate_polar_angle_rad_max) + + # Calculate the offset from the food pose. + x_offset = radial_distance * np.sin(polar_angle) * np.cos(final_azimuthal_angle) + y_offset = radial_distance * np.sin(polar_angle) * np.sin(final_azimuthal_angle) + z_offset = radial_distance * np.cos(polar_angle) + + # The final camera position is the food position plus this offset. + camera_position = food_position + np.array([x_offset, y_offset, z_offset]) + + # --- Orientation Calculation (Look-at with no roll) --- + # (This logic remains the same) + z_axis = food_position - camera_position + z_axis /= np.linalg.norm(z_axis) + world_up = np.array([0.0, 0.0, 1.0]) + if np.abs(np.dot(z_axis, world_up)) > 0.999: + x_axis = np.array([0.0, 1.0, 0.0]) + else: + x_axis = np.cross(world_up, z_axis) + x_axis /= np.linalg.norm(x_axis) + y_axis = np.cross(z_axis, x_axis) + rotation_matrix = np.array([x_axis, y_axis, z_axis]).T + rotation = R.from_matrix(rotation_matrix) + quat = rotation.as_quat() + + # Create and return the final Pose message + final_pose = Pose() + final_pose.position = Point( + x=camera_position[0], y=camera_position[1], z=camera_position[2] + ) + final_pose.orientation = Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) + + sampled_params = { + "above_plate_sampled_yaw_variability_rad": yaw_variability, + "above_plate_sampled_polar_angle_rad": polar_angle, + } + return final_pose, sampled_params + + def _calculate_in_food_pose( + self, food_pose: Pose, recipe: ActionRecipe + ) -> Tuple[Pose, np.ndarray, Dict[str, float]]: + """ + Calculates the InFood tool tip pose based on a semantic ActionRecipe. + Returns the pose, the approach vector, and sampled parameters. + """ + # Default to identity rotation and a vertical approach vector + final_rotation = R.identity() + approach_vector = np.array([0.0, 0.0, -1.0]) + sampled_polar_angle = 0.0 + + # --- SKEWER Strategy Logic --- + if recipe.strategy == AcquisitionStrategy.SKEWER: + # (Logic for finding food axes remains the same) + food_orientation = R.from_quat( + [ + food_pose.orientation.x, + food_pose.orientation.y, + food_pose.orientation.z, + food_pose.orientation.w, + ] + ) + minor_axis_food = food_orientation.apply([0.0, 1.0, 0.0]) + tool_x_final = minor_axis_food + + # Use parameter from the params object + sampled_polar_angle = np.random.uniform( + 0, self.params.skewer_polar_angle_rad_max + ) + rotation = R.from_rotvec(sampled_polar_angle * tool_x_final) + tool_z_final = rotation.apply(np.array([0.0, 0.0, -1.0])) + + tool_y_final = np.cross(tool_z_final, tool_x_final) + + base_rotation_matrix = np.array( + [tool_x_final, tool_y_final, tool_z_final] + ).T + base_orientation = R.from_matrix(base_rotation_matrix) + roll_rotation = R.from_rotvec( + np.deg2rad(self.params.in_food_tool_roll_angle_deg) * tool_z_final + ) + final_rotation = roll_rotation * base_orientation + approach_vector = tool_z_final + + # --- SCOOP Strategy Logic (Placeholder) --- + elif recipe.strategy == AcquisitionStrategy.SCOOP: + # ... + pass + + # --- Construct the final pose --- + q = final_rotation.as_quat() + in_food_pose = Pose() + in_food_pose.position = food_pose.position + in_food_pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) + + sampled_params = {"in_food_sampled_polar_angle_rad": sampled_polar_angle} + return in_food_pose, approach_vector, sampled_params + + def _calculate_above_food_pose( + self, in_food_pose: Pose, approach_vector: np.ndarray + ) -> Pose: + """ + Calculates the AboveFood pose by offsetting from InFood along the + calculated approach vector. + """ + # 1. Inherit the orientation directly from the target pose + final_orientation = in_food_pose.orientation + + # 2. The motion vector for the linear path IS the approach vector (tool's Z-axis) + motion_vector = approach_vector + + # 3. Calculate the offset position using parameter from the params object + offset_dist = self.params.above_food_offset_dist + p = in_food_pose.position + offset = motion_vector * -offset_dist + final_position = Point(x=p.x + offset[0], y=p.y + offset[1], z=p.z + offset[2]) + + return Pose(position=final_position, orientation=final_orientation) + + def _calculate_staging_pose(self, mouth_pose: Pose) -> Pose: + """Calculate the staging pose relative to the mouth.""" + offset_dist = self.params.staging_offset_dist + + p = mouth_pose.position + q = mouth_pose.orientation + mouth_rot = R.from_quat([q.x, q.y, q.z, q.w]) + + # Offset is along the mouth's forward-facing X-axis + offset_vec = mouth_rot.apply([offset_dist, 0, 0]) + + staged_pos = Point( + x=p.x - offset_vec[0], y=p.y - offset_vec[1], z=p.z - offset_vec[2] + ) + + return Pose(position=staged_pos, orientation=q) + + def _calculate_resting_pose(self, food_pose: Pose, mouth_pose: Pose) -> Pose: + """ + Calculates a dynamic "Resting" pose for the Jaco end-effector. + + This method implements the "Angular Standoff" strategy. It positions + the resting pose at a fixed radial distance from the robot base, but with + an angular offset from the food's position. This offset is directed + away from the mouth's position, placing the arm in a safe, clear, and + context-aware staging area. + + Args: + food_pose: The 6D pose of the food item. + mouth_pose: The 6D pose of the user's mouth. + + Returns: + The calculated 6D resting pose. + """ + # --- Extract XY positions and create 2D vectors from the origin --- + v_food = np.array([food_pose.position.x, food_pose.position.y]) + v_mouth = np.array([mouth_pose.position.x, mouth_pose.position.y]) + + # --- 1. Determine the direction of angular offset --- + # Use the 2D cross product to find the sign of the angle between vectors. + # This tells us if the mouth is clockwise or counter-clockwise from the food. + cross_product_z = np.cross(v_food, v_mouth) + + # We apply the offset in the direction that moves away from the mouth. + angle = ( + -np.deg2rad(self.params.resting_angular_offset_deg) + if cross_product_z > 0 + else np.deg2rad(self.params.resting_angular_offset_deg) + ) + + # --- 2. Calculate the new position --- + # Normalize the food vector to get its direction + v_food_dir = v_food / np.linalg.norm(v_food) + + # Create a 2D rotation matrix and apply it to the food's direction + c, s = np.cos(angle), np.sin(angle) + rotation_matrix = np.array(((c, -s), (s, c))) + v_rest_dir = rotation_matrix @ v_food_dir + + # Scale the new direction by the fixed radial distance for the final XY position + rest_position_xy = v_rest_dir * self.params.resting_radial_dist + rest_position_z = food_pose.position.z + self.params.resting_vertical_offset + + # --- 3. Calculate the new orientation (upright and facing outward) --- + # The z-axis (forward) points horizontally from the base to the new position + z_axis = np.array([rest_position_xy[0], rest_position_xy[1], 0.0]) + z_axis /= np.linalg.norm(z_axis) + + # The y-axis (up) is aligned with the world's Z-axis + y_axis = np.array([0.0, 0.0, 1.0]) + + # The x-axis (left) is the cross product, forming an orthonormal frame + x_axis = np.cross(y_axis, z_axis) + + # Construct the final rotation matrix and convert to a quaternion + rotation_matrix_3d = np.array([x_axis, y_axis, z_axis]).T + rotation = R.from_matrix(rotation_matrix_3d) + quat = rotation.as_quat() + + # --- 4. Assemble and return the final Pose message --- + return Pose( + position=Point( + x=rest_position_xy[0], y=rest_position_xy[1], z=rest_position_z + ), + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), + ) + + +# --- Core Benchmark Classes --- class MotionPlanner: """A wrapper for MoveIt2 to provide a seamless, synchronous API for planning.""" @@ -486,27 +865,20 @@ def __init__( output_dir: Optional[str] = None, ): self.node = node - self.moveit2_jaco = moveit2_jaco - self.moveit2_atool = moveit2_atool - self.moveit2_full = moveit2_full - self.xacro_file_path = xacro_file_path self.num_trials = num_trials - self.planning_timeout = planning_timeout self.output_dir = output_dir self.results: List[Dict[str, Any]] = [] - self.tf_buffer = tf2_ros.Buffer() - self.tf_listener = tf2_ros.TransformListener(self.tf_buffer, self.node) + self.motion_planner = MotionPlanner( node, moveit2_jaco, moveit2_atool, moveit2_full, - self.tf_buffer, + tf2_ros.Buffer(), planning_timeout, ) - # Pinocchio model for feasibility checks - self.kinematics_model = PinocchioModel(self.xacro_file_path) + self.kinematics_model = PinocchioModel(xacro_file_path) if self.output_dir: os.makedirs(self.output_dir, exist_ok=True) @@ -1019,264 +1391,6 @@ def _generate_leveling_atool_trajectory( return traj_atool - def _calculate_above_plate_pose(self, food_pose: Pose) -> Pose: - """ - Calculates a camera pose for the Jaco end-effector that looks at the food, - with a yaw constraint that keeps the arm aligned with the robot base. - """ - food_position = np.array( - [food_pose.position.x, food_pose.position.y, food_pose.position.z] - ) - - # --- Position Calculation with Yaw Constraint --- - # 1. Determine the base yaw angle from the robot's origin to the food's XY position. - base_yaw_angle = np.arctan2(food_position[1], food_position[0]) - - # 2. Add random variability to this base angle. - yaw_variability = np.random.uniform(-np.deg2rad(45), np.deg2rad(45)) - final_azimuthal_angle = base_yaw_angle + yaw_variability + np.pi - - # 3. Use spherical coordinates relative to the food pose to find the camera position. - radial_distance = 0.3 # Constant distance from the food - polar_angle = np.random.uniform(0, np.deg2rad(45)) # Angle from vertical - - # Calculate the offset from the food pose. - x_offset = radial_distance * np.sin(polar_angle) * np.cos(final_azimuthal_angle) - y_offset = radial_distance * np.sin(polar_angle) * np.sin(final_azimuthal_angle) - z_offset = radial_distance * np.cos(polar_angle) - - # The final camera position is the food position plus this offset. - camera_position = food_position + np.array([x_offset, y_offset, z_offset]) - - # --- Orientation Calculation (Look-at with no roll) --- - # 1. The end-effector's Z-axis must point from its position to the food's origin. - z_axis = food_position - camera_position - z_axis /= np.linalg.norm(z_axis) - - # 2. The end-effector's Y-axis should be aligned with the world's "up" to prevent roll. - world_up = np.array([0.0, 0.0, 1.0]) - - # 3. Calculate the X-axis (left) and handle the singularity when looking straight down. - if np.abs(np.dot(z_axis, world_up)) > 0.999: - # Looking straight down, define "left" relative to the world frame. - x_axis = np.array([0.0, 1.0, 0.0]) - else: - x_axis = np.cross(world_up, z_axis) - x_axis /= np.linalg.norm(x_axis) - - # 4. Re-calculate the Y-axis to ensure the frame is perfectly orthonormal. - y_axis = np.cross(z_axis, x_axis) - - # 5. Construct the final rotation matrix from the basis vectors. - rotation_matrix = np.array([x_axis, y_axis, z_axis]).T - rotation = R.from_matrix(rotation_matrix) - quat = rotation.as_quat() - - # Create and return the final Pose message - final_pose = Pose() - final_pose.position = Point( - x=camera_position[0], y=camera_position[1], z=camera_position[2] - ) - final_pose.orientation = Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) - - return final_pose - - # --- Semantic Pose Calculation --- - def _calculate_in_food_pose( - self, food_pose: Pose, recipe: ActionRecipe, tool_roll_angle_deg: float = 180.0 - ) -> Tuple[Pose, np.ndarray]: - """ - Calculates the InFood tool tip pose based on a semantic ActionRecipe. - Returns the pose and the calculated approach vector (the tool's Z-axis). - """ - # Default to identity rotation and a vertical approach vector - final_rotation = R.identity() - approach_vector = np.array([0.0, 0.0, -1.0]) - - # --- SKEWER Strategy Logic --- - if recipe.strategy == AcquisitionStrategy.SKEWER: - # Get the food's principal axes from its orientation - food_orientation = R.from_quat( - [ - food_pose.orientation.x, - food_pose.orientation.y, - food_pose.orientation.z, - food_pose.orientation.w, - ] - ) - major_axis_food = food_orientation.apply( - [1.0, 0.0, 0.0] - ) # Food's X (longer) - minor_axis_food = food_orientation.apply( - [0.0, 1.0, 0.0] - ) # Food's Y (shorter) - - # 1. Align Tines: The tool's X-axis (tines) must align with the - # food's minor axis for a stable skewer. - tool_x_final = minor_axis_food - - # 2. Define Approach & Tilt: The approach is along the food's major axis, - # tilted by a random polar angle. We find the tool's Z-axis by rotating - # a downward vector around the tool's new X-axis. - sampled_polar_angle = np.random.uniform( - 0, np.deg2rad(60) - ) # Angle from vertical - rotation = R.from_rotvec(sampled_polar_angle * tool_x_final) - tool_z_final = rotation.apply( - np.array([0.0, 0.0, -1.0]) - ) # Rotate a downward vector - - # 3. Complete the Frame: The tool's Y-axis is derived from the cross product. - tool_y_final = np.cross(tool_z_final, tool_x_final) - - # 4. The final rotation is constructed from these basis vectors. - base_rotation_matrix = np.array( - [tool_x_final, tool_y_final, tool_z_final] - ).T - base_orientation = R.from_matrix(base_rotation_matrix) - roll_rotation = R.from_rotvec( - np.deg2rad(tool_roll_angle_deg) * tool_z_final - ) - final_rotation = roll_rotation * base_orientation - approach_vector = tool_z_final - - # --- SCOOP Strategy Logic (Placeholder) --- - elif recipe.strategy == AcquisitionStrategy.SCOOP: - # This logic will be implemented next. - base_rotation = R.from_euler("y", -60, degrees=True) - final_rotation = base_rotation - # TODO: Add alignment logic based on food's principal axes. - - # --- Construct the final pose --- - q = final_rotation.as_quat() - in_food_pose = Pose() - in_food_pose.position = food_pose.position - in_food_pose.orientation = Quaternion(x=q[0], y=q[1], z=q[2], w=q[3]) - - return in_food_pose, approach_vector - - def _calculate_above_food_pose( - self, in_food_pose: Pose, approach_vector: np.ndarray - ) -> Pose: - """ - Calculates the AboveFood pose by offsetting from InFood along the - calculated approach vector. - """ - # 1. Inherit the orientation directly from the target pose - final_orientation = in_food_pose.orientation - - # 2. The motion vector for the linear path IS the approach vector (tool's Z-axis) - motion_vector = approach_vector - - # 3. Calculate the offset position - offset_dist = 0.1 # 10 cm - p = in_food_pose.position - # We add the offset because the approach_vector is already pointing "down". - # To get the "above" pose, we move in the opposite direction of the approach. - offset = motion_vector * -offset_dist - final_position = Point(x=p.x + offset[0], y=p.y + offset[1], z=p.z + offset[2]) - - return Pose(position=final_position, orientation=final_orientation) - - def _calculate_staging_pose(self, mouth_pose: Pose) -> Pose: - """Calculate the staging pose relative to the mouth.""" - offset_dist = 0.15 - - p = mouth_pose.position - q = mouth_pose.orientation - mouth_rot = R.from_quat([q.x, q.y, q.z, q.w]) - - # Offset is along the mouth's forward-facing X-axis - offset_vec = mouth_rot.apply([offset_dist, 0, 0]) - - staged_pos = Point( - x=p.x - offset_vec[0], y=p.y - offset_vec[1], z=p.z - offset_vec[2] - ) - - # Here we would add the complex dual-orientation constraint logic - # For now, we use the same orientation as the mouth - return Pose(position=staged_pos, orientation=q) - - def _calculate_resting_pose( - self, - food_pose: Pose, - mouth_pose: Pose, - angular_offset_deg: float = 20, - radial_distance_m: float = 0.8, - vertical_offset_m: float = ARTICUTOOL_LENGTH_M + 0.2, - ) -> Pose: - """ - Calculates a dynamic "Resting" pose for the Jaco end-effector. - - This method implements the "Angular Standoff" strategy. It positions - the resting pose at a fixed radial distance from the robot base, but with - an angular offset from the food's position. This offset is directed - away from the mouth's position, placing the arm in a safe, clear, and - context-aware staging area. - - Args: - food_pose: The 6D pose of the food item. - mouth_pose: The 6D pose of the user's mouth. - - Returns: - The calculated 6D resting pose. - """ - - # --- Extract XY positions and create 2D vectors from the origin --- - v_food = np.array([food_pose.position.x, food_pose.position.y]) - v_mouth = np.array([mouth_pose.position.x, mouth_pose.position.y]) - - # --- 1. Determine the direction of angular offset --- - # Use the 2D cross product to find the sign of the angle between vectors. - # This tells us if the mouth is clockwise or counter-clockwise from the food. - cross_product_z = np.cross(v_food, v_mouth) - - # We apply the offset in the direction that moves away from the mouth. - if cross_product_z > 0: # Mouth is CCW from food, so we rotate CW - angle = -np.deg2rad(angular_offset_deg) - else: # Mouth is CW from food, so we rotate CCW - angle = np.deg2rad(angular_offset_deg) - - # --- 2. Calculate the new position --- - # Normalize the food vector to get its direction - v_food_dir = v_food / np.linalg.norm(v_food) - - # Create a 2D rotation matrix and apply it to the food's direction - c, s = np.cos(angle), np.sin(angle) - rotation_matrix = np.array(((c, -s), (s, c))) - v_rest_dir = rotation_matrix @ v_food_dir - - # Scale the new direction by the fixed radial distance for the final XY position - rest_position_xy = v_rest_dir * radial_distance_m - rest_position_z = food_pose.position.z + vertical_offset_m - - # --- 3. Calculate the new orientation (upright and facing outward) --- - # The z-axis (forward) points horizontally from the base to the new position - z_axis = np.array([rest_position_xy[0], rest_position_xy[1], 0.0]) - z_axis /= np.linalg.norm(z_axis) - - # The y-axis (up) is aligned with the world's Z-axis - y_axis = np.array([0.0, 0.0, 1.0]) - - # The x-axis (left) is the cross product, forming an orthonormal frame - x_axis = np.cross(y_axis, z_axis) - - # Construct the final rotation matrix and convert to a quaternion - rotation_matrix_3d = np.array([x_axis, y_axis, z_axis]).T - rotation = R.from_matrix(rotation_matrix_3d) - quat = rotation.as_quat() - - # --- 4. Assemble and return the final Pose message --- - resting_pose = Pose() - resting_pose.position = Point( - x=rest_position_xy[0], y=rest_position_xy[1], z=rest_position_z - ) - resting_pose.orientation = Quaternion( - x=quat[0], y=quat[1], z=quat[2], w=quat[3] - ) - - return resting_pose - def _solve_articutool_ik( self, target_vector: np.ndarray ) -> List[Tuple[float, float]]: @@ -1772,42 +1886,13 @@ def run(self): for i in range(self.num_trials): LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") - # 1. Generate scene and create the top-level dictionary for the trial - scene = self._generate_scene() - # LOGGER.info( - # f""" - # --- Generated Scene Parameters for Trial {i + 1} --- - # - # - Initial State: - # - Home Config: [{", ".join(f"{j:.4f}" for j in scene["home_config"])}] - # - # - Core Sampled Poses: - # - Food Pose: - # Position: [x={scene["food_pose"].position.x:.3f}, y={scene["food_pose"].position.y:.3f}, z={scene["food_pose"].position.z:.3f}] - # Orientation: [x={scene["food_pose"].orientation.x:.3f}, y={scene["food_pose"].orientation.y:.3f}, z={scene["food_pose"].orientation.z:.3f}, w={scene["food_pose"].orientation.w:.3f}] - # - Mouth Pose: - # Position: [x={scene["mouth_pose"].position.x:.3f}, y={scene["mouth_pose"].position.y:.3f}, z={scene["mouth_pose"].position.z:.3f}] - # Orientation: [x={scene["mouth_pose"].orientation.x:.3f}, y={scene["mouth_pose"].orientation.y:.3f}, z={scene["mouth_pose"].orientation.z:.3f}, w={scene["mouth_pose"].orientation.w:.3f}] - # - # - Derived Poses for Feeding Cycle: - # - Above Plate Pose: - # Position: [x={scene["above_plate_pose"].position.x:.3f}, y={scene["above_plate_pose"].position.y:.3f}, z={scene["above_plate_pose"].position.z:.3f}] - # Orientation: [x={scene["above_plate_pose"].orientation.x:.3f}, y={scene["above_plate_pose"].orientation.y:.3f}, z={scene["above_plate_pose"].orientation.z:.3f}, w={scene["above_plate_pose"].orientation.w:.3f}] - # - Above Food Pose: - # Position: [x={scene["above_food_pose"].position.x:.3f}, y={scene["above_food_pose"].position.y:.3f}, z={scene["above_food_pose"].position.z:.3f}] - # Orientation: [x={scene["above_food_pose"].orientation.x:.3f}, y={scene["above_food_pose"].orientation.y:.3f}, z={scene["above_food_pose"].orientation.z:.3f}, w={scene["above_food_pose"].orientation.w:.3f}] - # - In Food Pose: - # Position: [x={scene["in_food_pose"].position.x:.3f}, y={scene["in_food_pose"].position.y:.3f}, z={scene["in_food_pose"].position.z:.3f}] - # Orientation: [x={scene["in_food_pose"].orientation.x:.3f}, y={scene["in_food_pose"].orientation.y:.3f}, z={scene["in_food_pose"].orientation.z:.3f}, w={scene["in_food_pose"].orientation.w:.3f}] - # - Staging Pose: - # Position: [x={scene["staging_pose"].position.x:.3f}, y={scene["staging_pose"].position.y:.3f}, z={scene["staging_pose"].position.z:.3f}] - # Orientation: [x={scene["staging_pose"].orientation.x:.3f}, y={scene["staging_pose"].orientation.y:.3f}, z={scene["staging_pose"].orientation.z:.3f}, w={scene["staging_pose"].orientation.w:.3f}] - # - Resting Pose: - # Position: [x={scene["resting_pose"].position.x:.3f}, y={scene["resting_pose"].position.y:.3f}, z={scene["resting_pose"].position.z:.3f}] - # Orientation: [x={scene["resting_pose"].orientation.x:.3f}, y={scene["resting_pose"].orientation.y:.3f}, z={scene["resting_pose"].orientation.z:.3f}, w={scene["resting_pose"].orientation.w:.3f}] - # ------------------------------------------------- - # """ - # ) + # 1. Create the parameter set and the generator for this trial + generation_params = SceneGenerationParams() + scene_generator = SceneGenerator(generation_params) + + # 2. Generate the scene and characteristics with a single, clean call + scene, scene_characteristics = scene_generator.generate() + trial_data = { "trial_id": i, "scene_poses": { @@ -1819,15 +1904,13 @@ def run(self): "staging_pose": self._serialize_pose(scene["staging_pose"]), "resting_pose": self._serialize_pose(scene["resting_pose"]), }, - "parameters": { # Placeholder for future parameter optimization - "angular_offset_deg": 20, - "radial_distance_m": 0.8, - }, + "scene_characteristics": scene_characteristics, + "parameters": asdict(generation_params), "stages": [], "end_to_end_success": False, # Default to False } - # 2. Initialize the robot's state for the trial + # 3. Initialize the robot's state for the trial current_jaco_state = scene["home_config"] current_atool_state = [0.0, 0.0] trial_failed = False From c5febbd31a4d622c397ff27dc77880952ce9390b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 09:58:04 -0700 Subject: [PATCH 209/238] Update analysis script to provide a quantitative overview of the performance metrics and scene parameters, breaking them down for both successful and failed trials. --- ada_feeding/scripts/analyze_benchmark.py | 65 ++++++++++++++++++++---- 1 file changed, 54 insertions(+), 11 deletions(-) diff --git a/ada_feeding/scripts/analyze_benchmark.py b/ada_feeding/scripts/analyze_benchmark.py index 42703ba6..fe8e9aba 100644 --- a/ada_feeding/scripts/analyze_benchmark.py +++ b/ada_feeding/scripts/analyze_benchmark.py @@ -1,6 +1,7 @@ import argparse import json import pandas as pd +from dataclasses import asdict def analyze_benchmark_results(file_path: str): @@ -21,7 +22,16 @@ def analyze_benchmark_results(file_path: str): # --- 1. Flatten the data for easier analysis with pandas --- # Each row in this DataFrame will represent a single stage from a single trial. flat_stages = [] + trial_characteristics = [] + for trial in data: + trial_characteristics.append( + { + "trial_id": trial.get("trial_id"), + "end_to_end_success": trial.get("end_to_end_success"), + **trial.get("scene_characteristics", {}), + } + ) for stage in trial.get("stages", []): flat_record = { "trial_id": trial.get("trial_id"), @@ -36,35 +46,35 @@ def analyze_benchmark_results(file_path: str): flat_record.update(stage["custom_metrics"]) flat_stages.append(flat_record) - df = pd.DataFrame(flat_stages) + df_stages = pd.DataFrame(flat_stages) + df_trials = pd.DataFrame(trial_characteristics) - # --- 2. Calculate and Print Key Metrics --- + # --- 2. Print High-Level Summary --- num_trials = len(data) successful_trials = sum(1 for trial in data if trial.get("end_to_end_success")) overall_success_rate = ( (successful_trials / num_trials) * 100 if num_trials > 0 else 0 ) - print("\n" + "=" * 50) - print(" BENCHMARK ANALYSIS REPORT") - print("=" * 50) + print("\n" + "=" * 60) + print(" BENCHMARK ANALYSIS REPORT") + print("=" * 60) print(f"File: {file_path}") print(f"Total Trials: {num_trials}") print( f"End-to-End Success Rate: {overall_success_rate:.1f}% ({successful_trials}/{num_trials})" ) - print("-" * 50) + print("-" * 60) - if not df.empty: - # --- Failure analysis by STAGE --- - failed_stages_df = df[df["status"] != "Success"] + if not df_stages.empty: + failed_stages_df = df_stages[df_stages["status"] != "Success"] stage_failure_counts = failed_stages_df["stage_name"].value_counts() print("\nFailure Count by Stage:") if not stage_failure_counts.empty: print(stage_failure_counts.to_string()) else: print("No stage failures recorded.") - print("-" * 50) + print("-" * 60) # --- Failure analysis by TYPE for the most problematic stage --- if not stage_failure_counts.empty: @@ -77,7 +87,40 @@ def analyze_benchmark_results(file_path: str): ] status_counts = problem_stage_df["status"].value_counts() print(status_counts.to_string()) - print("=" * 50) + print("=" * 60) + + # --- 3. NEW: Generate Comprehensive Statistical Summaries --- + if not df_trials.empty: + # Define the columns we want to summarize + characteristic_cols = [ + col + for col in df_trials.columns + if col not in ["trial_id", "end_to_end_success"] + ] + + # Separate successful and failed trials + successful_df = df_trials[df_trials["end_to_end_success"] == True] + failed_df = df_trials[df_trials["end_to_end_success"] == False] + + print("\n COMPREHENSIVE STATISTICAL SUMMARY") + print("-" * 60) + + if not successful_df.empty: + print( + f"\n--- For SUCCESSFUL End-to-End Trials ({len(successful_df)} trials) ---" + ) + # Use .describe() to get summary statistics, transpose for readability + print(successful_df[characteristic_cols].describe().transpose().to_string()) + else: + print("\n--- No SUCCESSFUL End-to-End Trials to Summarize ---") + + if not failed_df.empty: + print(f"\n--- For FAILED End-to-End Trials ({len(failed_df)} trials) ---") + print(failed_df[characteristic_cols].describe().transpose().to_string()) + else: + print("\n--- No FAILED End-to-End Trials to Summarize ---") + + print("=" * 60) if __name__ == "__main__": From e6820463516f70e75076b861958f9c99b54790ff Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 11:06:17 -0700 Subject: [PATCH 210/238] Update visualization script to take a trial-centric approach --- .../scripts/visualize_e2e_benchmark.py | 305 ++++++++++-------- 1 file changed, 172 insertions(+), 133 deletions(-) diff --git a/ada_feeding/scripts/visualize_e2e_benchmark.py b/ada_feeding/scripts/visualize_e2e_benchmark.py index 9a6b72b6..05c9e50c 100644 --- a/ada_feeding/scripts/visualize_e2e_benchmark.py +++ b/ada_feeding/scripts/visualize_e2e_benchmark.py @@ -6,13 +6,10 @@ This script interactively visualizes trajectories from end_to_end_benchmark.py output files using Pinocchio and MeshCat. -It allows a user to load a benchmark result file, choose a specific -planning stage and trial, and then inspect the resulting trajectory -frame-by-frame or animate it. - -This version automatically handles different execution modes (sequential, synchronous) -and includes robust mesh path finding and explicit kinematic updates to prevent -rendering artifacts. +This refactored version uses a "trial-centric" workflow. A user first selects +a Trial ID, which immediately displays the static scene for that trial. The user +can then choose to play back any of the available (successful) stage +trajectories from within that trial's context. """ # Standard imports @@ -144,6 +141,7 @@ def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFram "traj_jaco": stage.get("traj_jaco"), "traj_atool": stage.get("traj_atool"), } + # We only care about stages that have a trajectory to visualize if flat_record["traj_jaco"] or flat_record["traj_atool"]: all_stages_flat.append(flat_record) except Exception as e: @@ -156,86 +154,28 @@ def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFram return pd.DataFrame(all_stages_flat) -def select_trajectory(df: pd.DataFrame) -> Optional[Dict[str, Any]]: - """ - Interactively prompts the user to select a stage and a trial. It then finds - the static pose of the Jaco arm from the previous stage to ensure - visual continuity. - """ - stage_order = [ - "HomeToAbovePlate", - "AbovePlateToAboveFood", - "AboveFoodToInFood", - "LevelArticutool", - ] +def select_trial(df: pd.DataFrame) -> Optional[pd.DataFrame]: + """Interactively prompts the user to select a Trial ID.""" + unique_trials = sorted(df["trial_id"].unique()) + if not unique_trials: + print("No trials found in the loaded data.") + return None while True: - stages = df["stage_name"].unique() - print("\n--- Please Select a Stage to Visualize ---") - for i, stage in enumerate(stages): - print(f" [{i + 1}] {stage}") + print("\n--- Please Select a Trial to Visualize ---") + for i, trial_id in enumerate(unique_trials): + print(f" [{i + 1}] Trial ID: {trial_id}") print(" [q] Quit") - try: - choice_str = input(f"Enter choice (1-{len(stages)} or q): ").strip().lower() - if choice_str == "q": - return None - selected_stage_name = stages[int(choice_str) - 1] - except (ValueError, IndexError): - print("Invalid choice.") - continue - - stage_df = df[ - (df["stage_name"] == selected_stage_name) & (df["status"] == "Success") - ].sort_values("trial_id") - - if stage_df.empty: - print(f"No successful trials found for stage: {selected_stage_name}") - continue - - trials_for_stage = stage_df.to_dict("records") - print(f"\n--- Select a Successful Trial from Stage '{selected_stage_name}' ---") - for i, trial in enumerate(trials_for_stage): - print( - f" [{i + 1}] Trial ID: {trial['trial_id']} (Mode: {trial['execution_mode']})" - ) - print(" [m] Back to Stage Selection") try: choice_str = ( - input(f"Enter choice (1-{len(trials_for_stage)} or m): ") - .strip() - .lower() + input(f"Enter choice (1-{len(unique_trials)} or q): ").strip().lower() ) - if choice_str == "m": - continue - - selected_record = trials_for_stage[int(choice_str) - 1] - trial_id = selected_record["trial_id"] - - try: - current_stage_index = stage_order.index(selected_stage_name) - if current_stage_index > 0: - previous_stage_name = stage_order[current_stage_index - 1] - previous_stage_df = df[ - (df["trial_id"] == trial_id) - & (df["stage_name"] == previous_stage_name) - ] - if not previous_stage_df.empty: - prev_traj_jaco = previous_stage_df.iloc[0]["traj_jaco"] - if prev_traj_jaco and prev_traj_jaco["points"]: - last_waypoint = prev_traj_jaco["points"][-1] - selected_record["static_jaco_config"] = { - "positions": last_waypoint["positions"], - "joint_names": prev_traj_jaco["joint_names"], - } - except ValueError: - print( - f"Warning: Stage '{selected_stage_name}' not in defined stage order for state carryover." - ) - - return selected_record + if choice_str == "q": + return None + selected_trial_id = unique_trials[int(choice_str) - 1] + return df[df["trial_id"] == selected_trial_id].copy() except (ValueError, IndexError): print("Invalid choice.") - continue def draw_scene_frames(pin_viz, scene_poses: Dict[str, Any], frame_scale: float = 0.2): @@ -243,6 +183,12 @@ def draw_scene_frames(pin_viz, scene_poses: Dict[str, Any], frame_scale: float = if not scene_poses: return + # Clear any previous frames before drawing new ones + try: + pin_viz.viewer["scene/frames"].delete() + except KeyError: + pass # It's okay if it doesn't exist + print("Drawing scene frames and markers...") for name, pose_data in scene_poses.items(): position = np.array(pose_data["position"]) @@ -264,6 +210,7 @@ def draw_scene_frames(pin_viz, scene_poses: Dict[str, Any], frame_scale: float = pin_viz.viewer[frame_path].set_transform(transform.homogeneous) +# --- Helper functions for robot state updates --- def get_joint_map(model, joint_names): """Helper to map joint names to their indices in Pinocchio's q vector.""" if not joint_names: @@ -289,15 +236,11 @@ def update_q_from_waypoint(q, model, waypoint, joint_names, joint_map): q[model.joints[joint_id].idx_q + 1] = np.sin(pos) -def visualization_loop(pin_viz, model, data, selected_stage, args): - """Runs the interactive UI, handling different execution modes and frame-by-frame controls.""" - try: - pin_viz.viewer["scene/frames"].delete() - except KeyError: - pass - if "scene_poses" in selected_stage: - draw_scene_frames(pin_viz, selected_stage["scene_poses"]) - +def trajectory_playback_loop(pin_viz, model, data, selected_stage, args): + """ + Runs the interactive UI for a SINGLE trajectory. + Handles different execution modes and frame-by-frame controls. + """ execution_mode = selected_stage["execution_mode"] traj_jaco = selected_stage.get("traj_jaco") traj_atool = selected_stage.get("traj_atool") @@ -311,23 +254,24 @@ def visualization_loop(pin_viz, model, data, selected_stage, args): # --- Prepare a unified list of waypoints for consistent navigation --- waypoints = [] if execution_mode in ["Jaco Only", "Articutool Only"]: - traj = traj_jaco if execution_mode == "Jaco Only" else traj_atool - waypoints = traj["points"] - elif execution_mode == "Sequential": + waypoints = (traj_jaco or traj_atool).get("points", []) + elif execution_mode == "Sequential" and traj_jaco and traj_atool: waypoints.extend(traj_jaco["points"]) waypoints.extend(traj_atool["points"]) - elif execution_mode == "Synchronous": - waypoints = traj_jaco["points"] if traj_jaco else [] # Default to Jaco + elif execution_mode == "Synchronous" and traj_jaco: + waypoints = traj_jaco["points"] if not waypoints: print("No waypoints to display for this stage.") - return "menu" + input("Press Enter to continue...") + return "back" current_idx = 0 - q = pin.neutral(model) while True: - # --- Update robot configuration `q` for the current frame --- + # This ensures a clean slate and prevents state from bleeding over. + q = pin.neutral(model) + # 1. Start with the static Jaco pose from the previous stage, if available. if static_jaco_config: update_q_from_waypoint( @@ -342,7 +286,7 @@ def visualization_loop(pin_viz, model, data, selected_stage, args): if execution_mode == "Sequential": # For sequential, we need to know the final pose of the first trajectory jaco_end_waypoint = traj_jaco["points"][-1] - if current_idx >= len(traj_jaco["points"]): # We are in the Articutool part + if current_idx >= len(traj_jaco["points"]): # Articutool part update_q_from_waypoint( q, model, @@ -357,7 +301,7 @@ def visualization_loop(pin_viz, model, data, selected_stage, args): traj_atool["joint_names"], atool_joint_map, ) - else: # We are in the Jaco part + else: # Jaco part update_q_from_waypoint( q, model, @@ -365,15 +309,15 @@ def visualization_loop(pin_viz, model, data, selected_stage, args): traj_jaco["joint_names"], jaco_joint_map, ) - # Keep atool at its starting position - update_q_from_waypoint( - q, - model, - traj_atool["points"][0], - traj_atool["joint_names"], - atool_joint_map, - ) - else: # For all other modes, just update from the single relevant trajectory + if traj_atool and traj_atool["points"]: + update_q_from_waypoint( + q, + model, + traj_atool["points"][0], + traj_atool["joint_names"], + atool_joint_map, + ) + else: # For Synchronous, Jaco Only, Articutool Only if traj_jaco: update_q_from_waypoint( q, @@ -393,20 +337,21 @@ def visualization_loop(pin_viz, model, data, selected_stage, args): atool_joint_map, ) + # 3. Update and display the final calculated pose pin.forwardKinematics(model, data, q) pin_viz.display(q) print(f"\nDisplaying Frame: {current_idx + 1}/{len(waypoints)}") + print(f"--- Stage: {selected_stage['stage_name']} ---") print( - f"--- Stage: {selected_stage['stage_name']} | Trial: {selected_stage['trial_id']} | Mode: {execution_mode} ---" + "Commands: [n]ext, [p]rev, [f]irst, [l]ast, [a]nimate, [b]ack to stage selection, [q]uit" ) - print("Commands: [n]ext, [p]rev, [f]irst, [l]ast, [a]nimate, [m]enu, [q]uit") user_input = input("Enter command: ").strip().lower() if user_input == "q": return "quit" - if user_input == "m": - return "menu" + if user_input == "b": + return "back" if user_input == "n": current_idx = min(current_idx + 1, len(waypoints) - 1) @@ -420,27 +365,29 @@ def visualization_loop(pin_viz, model, data, selected_stage, args): print("Animating... Press Ctrl+C to stop.") try: for i in range(current_idx, len(waypoints)): - # The same update logic as above is used for animation frames + # Also reset q inside the animation loop for consistency + q_anim = pin.neutral(model) if static_jaco_config: update_q_from_waypoint( - q, + q_anim, model, {"positions": static_jaco_config["positions"]}, static_jaco_config["joint_names"], jaco_joint_map, ) + if execution_mode == "Sequential": jaco_end_waypoint = traj_jaco["points"][-1] if i >= len(traj_jaco["points"]): update_q_from_waypoint( - q, + q_anim, model, jaco_end_waypoint, traj_jaco["joint_names"], jaco_joint_map, ) update_q_from_waypoint( - q, + q_anim, model, waypoints[i], traj_atool["joint_names"], @@ -448,23 +395,24 @@ def visualization_loop(pin_viz, model, data, selected_stage, args): ) else: update_q_from_waypoint( - q, + q_anim, model, waypoints[i], traj_jaco["joint_names"], jaco_joint_map, ) - update_q_from_waypoint( - q, - model, - traj_atool["points"][0], - traj_atool["joint_names"], - atool_joint_map, - ) + if traj_atool and traj_atool["points"]: + update_q_from_waypoint( + q_anim, + model, + traj_atool["points"][0], + traj_atool["joint_names"], + atool_joint_map, + ) else: if traj_jaco: update_q_from_waypoint( - q, + q_anim, model, traj_jaco["points"][ min(i, len(traj_jaco["points"]) - 1) @@ -474,7 +422,7 @@ def visualization_loop(pin_viz, model, data, selected_stage, args): ) if traj_atool: update_q_from_waypoint( - q, + q_anim, model, traj_atool["points"][ min(i, len(traj_atool["points"]) - 1) @@ -483,20 +431,109 @@ def visualization_loop(pin_viz, model, data, selected_stage, args): atool_joint_map, ) - pin.forwardKinematics(model, data, q) - pin_viz.display(q) + pin.forwardKinematics(model, data, q_anim) + pin_viz.display(q_anim) print(f" Displaying frame {i + 1}/{len(waypoints)}", end="\r") time.sleep(1.0 / args.fps) current_idx = len(waypoints) - 1 print("\nAnimation finished.") except KeyboardInterrupt: print("\nAnimation stopped.") - elif user_input: - print("Invalid command.") + + +def trial_visualization_loop(pin_viz, model, data, trial_df, args): + """ + NEW: The main menu loop for an individual trial. + Displays the scene and lists available trajectories to visualize. + """ + trial_id = trial_df.iloc[0]["trial_id"] + scene_poses = trial_df.iloc[0]["scene_poses"] + + # 1. Immediately visualize the scene for this trial + draw_scene_frames(pin_viz, scene_poses) + + # 2. Prepare a map of all trajectories in this trial for state carry-over + stage_data_map = {row["stage_name"]: row for row in trial_df.to_dict("records")} + stage_order = [ + "HomeToAbovePlate", + "AbovePlateToAboveFood", + "AboveFoodToInFood", + "LevelArticutool", + "Resting", + ] + + while True: + print(f"\n--- Visualizing Trial ID: {trial_id} ---") + print("Scene frames are now displayed.") + print("Select a trajectory to play:") + + available_stages = [ + row for row in trial_df.to_dict("records") if row["status"] == "Success" + ] + + if not available_stages: + print("No successful trajectories available to visualize for this trial.") + input("Press Enter to return to trial selection...") + return "menu" + + for i, stage in enumerate(available_stages): + print( + f" [{i + 1}] {stage['stage_name']} (Mode: {stage['execution_mode']})" + ) + print(" [m] Back to Trial Selection") + print(" [q] Quit") + + try: + choice_str = ( + input(f"Enter choice (1-{len(available_stages)} or m/q): ") + .strip() + .lower() + ) + if choice_str == "q": + return "quit" + if choice_str == "m": + return "menu" + + selected_stage_data = available_stages[int(choice_str) - 1] + + # Find the previous stage to set the robot's static starting pose + try: + current_stage_index = stage_order.index( + selected_stage_data["stage_name"] + ) + if current_stage_index > 0: + for i in range(current_stage_index - 1, -1, -1): + prev_stage_name = stage_order[i] + if ( + prev_stage_name in stage_data_map + and stage_data_map[prev_stage_name]["traj_jaco"] + ): + prev_traj_jaco = stage_data_map[prev_stage_name][ + "traj_jaco" + ] + last_waypoint = prev_traj_jaco["points"][-1] + selected_stage_data["static_jaco_config"] = { + "positions": last_waypoint["positions"], + "joint_names": prev_traj_jaco["joint_names"], + } + break + except ValueError: + pass # Stage not in order, can't determine previous state + + # Enter the playback loop for the chosen trajectory + action = trajectory_playback_loop( + pin_viz, model, data, selected_stage_data, args + ) + if action == "quit": + return "quit" + # If 'back', the loop will continue, re-displaying this menu + + except (ValueError, IndexError): + print("Invalid choice.") def main(args): - """Main execution function.""" + """Main execution function with the new trial-centric workflow.""" print("--- Interactive Benchmark Trajectory Visualizer ---") df = load_benchmark_data(args.benchmark_files) @@ -526,12 +563,14 @@ def main(args): return while True: - selected_stage = select_trajectory(df) - if selected_stage is None: + trial_df = select_trial(df) + if trial_df is None or trial_df.empty: break - action = visualization_loop(pin_viz, model, data, selected_stage, args) + + action = trial_visualization_loop(pin_viz, model, data, trial_df, args) if action == "quit": break + print("Visualizer finished.") From 18aaf5512c5ca2681a7cde763bbe407da19705db Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 11:20:44 -0700 Subject: [PATCH 211/238] Add label during trial selection indicating the last successful stage --- .../scripts/visualize_e2e_benchmark.py | 45 ++++++++++++++++--- 1 file changed, 38 insertions(+), 7 deletions(-) diff --git a/ada_feeding/scripts/visualize_e2e_benchmark.py b/ada_feeding/scripts/visualize_e2e_benchmark.py index 05c9e50c..1c800be4 100644 --- a/ada_feeding/scripts/visualize_e2e_benchmark.py +++ b/ada_feeding/scripts/visualize_e2e_benchmark.py @@ -155,24 +155,55 @@ def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFram def select_trial(df: pd.DataFrame) -> Optional[pd.DataFrame]: - """Interactively prompts the user to select a Trial ID.""" - unique_trials = sorted(df["trial_id"].unique()) - if not unique_trials: + """ + Interactively prompts the user to select a Trial ID, showing + the last successfully completed stage for each trial. + """ + all_trial_ids = sorted(df["trial_id"].unique()) + if not all_trial_ids: print("No trials found in the loaded data.") return None + # Pre-calculate the completion status for each trial + trial_completion_info = {} + stage_order = [ + "HomeToAbovePlate", + "AbovePlateToAboveFood", + "AboveFoodToInFood", + "LevelArticutool", + "Resting", + ] + for trial_id in all_trial_ids: + trial_df = df[df["trial_id"] == trial_id] + successful_stages = trial_df[trial_df["status"] == "Success"][ + "stage_name" + ].tolist() + + last_success = "None" + last_success_idx = -1 + for stage_name in successful_stages: + if stage_name in stage_order: + last_success_idx = max(last_success_idx, stage_order.index(stage_name)) + + if last_success_idx != -1: + last_success = stage_order[last_success_idx] + trial_completion_info[trial_id] = last_success + while True: print("\n--- Please Select a Trial to Visualize ---") - for i, trial_id in enumerate(unique_trials): - print(f" [{i + 1}] Trial ID: {trial_id}") + for i, trial_id in enumerate(all_trial_ids): + completion_status = trial_completion_info.get(trial_id, "None") + print( + f" [{i + 1}] Trial ID: {trial_id} (Last Success: {completion_status})" + ) print(" [q] Quit") try: choice_str = ( - input(f"Enter choice (1-{len(unique_trials)} or q): ").strip().lower() + input(f"Enter choice (1-{len(all_trial_ids)} or q): ").strip().lower() ) if choice_str == "q": return None - selected_trial_id = unique_trials[int(choice_str) - 1] + selected_trial_id = all_trial_ids[int(choice_str) - 1] return df[df["trial_id"] == selected_trial_id].copy() except (ValueError, IndexError): print("Invalid choice.") From 9fd2c9df0e8728c9c9ef69b88be55902e4146fa8 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 11:32:48 -0700 Subject: [PATCH 212/238] Modify visualization script to show scene even if no successful trajectories were planned --- .../scripts/visualize_e2e_benchmark.py | 73 ++++++++++++------- 1 file changed, 46 insertions(+), 27 deletions(-) diff --git a/ada_feeding/scripts/visualize_e2e_benchmark.py b/ada_feeding/scripts/visualize_e2e_benchmark.py index 1c800be4..1e1e3a30 100644 --- a/ada_feeding/scripts/visualize_e2e_benchmark.py +++ b/ada_feeding/scripts/visualize_e2e_benchmark.py @@ -121,9 +121,8 @@ def load_pinocchio_model_from_urdf_string( def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFrame: """ - Loads and flattens data from one or more benchmark JSON files into a pandas DataFrame. - The new hierarchical structure (trial -> stages) is flattened so that each - stage with a valid trajectory becomes a single row in the DataFrame. + Loads data from benchmark JSON files, ensuring all trials and stages + are loaded, even if they have no successful trajectories. """ all_stages_flat = [] for file_path in file_paths: @@ -131,6 +130,18 @@ def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFram with open(file_path, "r") as f: trials = json.load(f) for trial in trials: + # If a trial has no stages, we can't do much, but we can still show the scene + if not trial.get("stages"): + all_stages_flat.append( + { + "trial_id": trial.get("trial_id"), + "scene_poses": trial.get("scene_poses"), + "stage_name": "N/A", + "status": "NoStages", + } + ) + continue + for stage in trial.get("stages", []): flat_record = { "trial_id": trial.get("trial_id"), @@ -141,23 +152,24 @@ def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFram "traj_jaco": stage.get("traj_jaco"), "traj_atool": stage.get("traj_atool"), } - # We only care about stages that have a trajectory to visualize - if flat_record["traj_jaco"] or flat_record["traj_atool"]: - all_stages_flat.append(flat_record) + # Load all stages regardless of trajectory existence + all_stages_flat.append(flat_record) except Exception as e: logger_func(f"Warning: Could not load or parse {file_path}. Error: {e}") if not all_stages_flat: return pd.DataFrame() - logger_func(f"Successfully loaded {len(all_stages_flat)} stages with trajectories.") + logger_func( + f"Successfully loaded {len(all_stages_flat)} stage records across all trials." + ) return pd.DataFrame(all_stages_flat) def select_trial(df: pd.DataFrame) -> Optional[pd.DataFrame]: """ - Interactively prompts the user to select a Trial ID, showing - the last successfully completed stage for each trial. + Prompts the user to select a Trial ID, showing the last successfully + completed stage for each trial. """ all_trial_ids = sorted(df["trial_id"].unique()) if not all_trial_ids: @@ -218,8 +230,7 @@ def draw_scene_frames(pin_viz, scene_poses: Dict[str, Any], frame_scale: float = try: pin_viz.viewer["scene/frames"].delete() except KeyError: - pass # It's okay if it doesn't exist - + pass print("Drawing scene frames and markers...") for name, pose_data in scene_poses.items(): position = np.array(pose_data["position"]) @@ -317,7 +328,7 @@ def trajectory_playback_loop(pin_viz, model, data, selected_stage, args): if execution_mode == "Sequential": # For sequential, we need to know the final pose of the first trajectory jaco_end_waypoint = traj_jaco["points"][-1] - if current_idx >= len(traj_jaco["points"]): # Articutool part + if current_idx >= len(traj_jaco["points"]): update_q_from_waypoint( q, model, @@ -332,7 +343,7 @@ def trajectory_playback_loop(pin_viz, model, data, selected_stage, args): traj_atool["joint_names"], atool_joint_map, ) - else: # Jaco part + else: update_q_from_waypoint( q, model, @@ -348,7 +359,7 @@ def trajectory_playback_loop(pin_viz, model, data, selected_stage, args): traj_atool["joint_names"], atool_joint_map, ) - else: # For Synchronous, Jaco Only, Articutool Only + else: if traj_jaco: update_q_from_waypoint( q, @@ -474,8 +485,8 @@ def trajectory_playback_loop(pin_viz, model, data, selected_stage, args): def trial_visualization_loop(pin_viz, model, data, trial_df, args): """ - NEW: The main menu loop for an individual trial. - Displays the scene and lists available trajectories to visualize. + The main menu loop for an individual trial. Handles trials with + no successful stages gracefully. """ trial_id = trial_df.iloc[0]["trial_id"] scene_poses = trial_df.iloc[0]["scene_poses"] @@ -496,7 +507,6 @@ def trial_visualization_loop(pin_viz, model, data, trial_df, args): while True: print(f"\n--- Visualizing Trial ID: {trial_id} ---") print("Scene frames are now displayed.") - print("Select a trajectory to play:") available_stages = [ row for row in trial_df.to_dict("records") if row["status"] == "Success" @@ -504,9 +514,17 @@ def trial_visualization_loop(pin_viz, model, data, trial_df, args): if not available_stages: print("No successful trajectories available to visualize for this trial.") - input("Press Enter to return to trial selection...") - return "menu" + print(" [m] Back to Trial Selection") + print(" [q] Quit") + choice_str = input(f"Enter choice (m/q): ").strip().lower() + if choice_str == "q": + return "quit" + if choice_str == "m": + return "menu" + print("Invalid choice.") + continue + print("Select a trajectory to play:") for i, stage in enumerate(available_stages): print( f" [{i + 1}] {stage['stage_name']} (Mode: {stage['execution_mode']})" @@ -542,14 +560,15 @@ def trial_visualization_loop(pin_viz, model, data, trial_df, args): prev_traj_jaco = stage_data_map[prev_stage_name][ "traj_jaco" ] - last_waypoint = prev_traj_jaco["points"][-1] - selected_stage_data["static_jaco_config"] = { - "positions": last_waypoint["positions"], - "joint_names": prev_traj_jaco["joint_names"], - } - break + if prev_traj_jaco and prev_traj_jaco.get("points"): + last_waypoint = prev_traj_jaco["points"][-1] + selected_stage_data["static_jaco_config"] = { + "positions": last_waypoint["positions"], + "joint_names": prev_traj_jaco["joint_names"], + } + break except ValueError: - pass # Stage not in order, can't determine previous state + pass # Enter the playback loop for the chosen trajectory action = trajectory_playback_loop( @@ -569,7 +588,7 @@ def main(args): df = load_benchmark_data(args.benchmark_files) if df.empty: - print("No data with trajectories could be loaded. Exiting.") + print("No data could be loaded. Exiting.") return package_dirs = find_ros_package_paths(args.xacro_file) From e9091a0a813d104df658b2e8f14395cb0f6aa4b8 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 11:43:50 -0700 Subject: [PATCH 213/238] Relax scene generation parameters to analyze a larger subset of the workspace --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index fd0b9dbf..6b784fe7 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -141,15 +141,15 @@ class SceneGenerationParams: """Holds all parameters that define the random scene generation.""" food_sampling: CylindricalSamplingParams = CylindricalSamplingParams( - name="food", inner_radius=0.4, outer_radius=0.7, min_height=0.0, max_height=0.3 + name="food", inner_radius=0.0, outer_radius=1.0, min_height=0.0, max_height=0.6 ) mouth_sampling: CylindricalSamplingParams = CylindricalSamplingParams( - name="mouth", inner_radius=0.3, outer_radius=0.6, min_height=0.0, max_height=0.6 + name="mouth", inner_radius=0.0, outer_radius=1.0, min_height=0.0, max_height=0.6 ) above_plate_radial_dist: float = 0.3 - above_plate_polar_angle_rad_max: float = math.pi / 4 # 45 deg - above_plate_yaw_variability_rad: float = math.pi / 4 # 45 deg - skewer_polar_angle_rad_max: float = math.pi / 3 # 60 deg + above_plate_polar_angle_rad_max: float = math.pi / 3 + above_plate_yaw_variability_rad: float = math.pi / 2 + skewer_polar_angle_rad_max: float = math.pi / 2 in_food_tool_roll_angle_deg: float = 180.0 above_food_offset_dist: float = 0.1 # 10 cm staging_offset_dist: float = 0.15 # 15 cm From 37b56167da88be6ce7669ad061129f6317a03a66 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 11:57:52 -0700 Subject: [PATCH 214/238] Tighten spherical parameters for generating food pose --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 6b784fe7..77054f04 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -141,7 +141,7 @@ class SceneGenerationParams: """Holds all parameters that define the random scene generation.""" food_sampling: CylindricalSamplingParams = CylindricalSamplingParams( - name="food", inner_radius=0.0, outer_radius=1.0, min_height=0.0, max_height=0.6 + name="food", inner_radius=0.4, outer_radius=0.8, min_height=0.0, max_height=0.3 ) mouth_sampling: CylindricalSamplingParams = CylindricalSamplingParams( name="mouth", inner_radius=0.0, outer_radius=1.0, min_height=0.0, max_height=0.6 From c8046d5d42da926dadf20b4c0f8abcc3b264c3bb Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 12:22:48 -0700 Subject: [PATCH 215/238] Reduce yaw variability for AbovePlate pose --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 77054f04..fb003d82 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -148,7 +148,7 @@ class SceneGenerationParams: ) above_plate_radial_dist: float = 0.3 above_plate_polar_angle_rad_max: float = math.pi / 3 - above_plate_yaw_variability_rad: float = math.pi / 2 + above_plate_yaw_variability_rad: float = math.pi / 8 skewer_polar_angle_rad_max: float = math.pi / 2 in_food_tool_roll_angle_deg: float = 180.0 above_food_offset_dist: float = 0.1 # 10 cm From 33d2d19ece970f436aac95a5117d3740cdd2cd29 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 12:37:39 -0700 Subject: [PATCH 216/238] Update analysis script to group statistics based on the specific stage of failure --- ada_feeding/scripts/analyze_benchmark.py | 49 +++++++++++++++++++----- 1 file changed, 40 insertions(+), 9 deletions(-) diff --git a/ada_feeding/scripts/analyze_benchmark.py b/ada_feeding/scripts/analyze_benchmark.py index fe8e9aba..5b076ce3 100644 --- a/ada_feeding/scripts/analyze_benchmark.py +++ b/ada_feeding/scripts/analyze_benchmark.py @@ -6,7 +6,8 @@ def analyze_benchmark_results(file_path: str): """ - Loads benchmark data, calculates key metrics, and prints a summary report. + Loads benchmark data, calculates key metrics, and prints a summary report, + including statistics grouped by the stage of failure. """ try: with open(file_path, "r") as f: @@ -51,7 +52,7 @@ def analyze_benchmark_results(file_path: str): # --- 2. Print High-Level Summary --- num_trials = len(data) - successful_trials = sum(1 for trial in data if trial.get("end_to_end_success")) + successful_trials = df_trials["end_to_end_success"].sum() overall_success_rate = ( (successful_trials / num_trials) * 100 if num_trials > 0 else 0 ) @@ -89,7 +90,7 @@ def analyze_benchmark_results(file_path: str): print(status_counts.to_string()) print("=" * 60) - # --- 3. NEW: Generate Comprehensive Statistical Summaries --- + # --- 3. Generate Comprehensive Statistical Summaries (Binary Pass/Fail) --- if not df_trials.empty: # Define the columns we want to summarize characteristic_cols = [ @@ -100,7 +101,6 @@ def analyze_benchmark_results(file_path: str): # Separate successful and failed trials successful_df = df_trials[df_trials["end_to_end_success"] == True] - failed_df = df_trials[df_trials["end_to_end_success"] == False] print("\n COMPREHENSIVE STATISTICAL SUMMARY") print("-" * 60) @@ -114,12 +114,43 @@ def analyze_benchmark_results(file_path: str): else: print("\n--- No SUCCESSFUL End-to-End Trials to Summarize ---") - if not failed_df.empty: - print(f"\n--- For FAILED End-to-End Trials ({len(failed_df)} trials) ---") - print(failed_df[characteristic_cols].describe().transpose().to_string()) - else: - print("\n--- No FAILED End-to-End Trials to Summarize ---") + # --- 4. Generate Statistical Summaries by Failure Stage --- + if not df_stages.empty and not df_trials.empty: + # Find the first failing stage for each failed trial + failed_stages_df = df_stages[df_stages["status"] != "Success"] + first_failures = failed_stages_df.loc[ + failed_stages_df.groupby("trial_id").head(1).index + ] + first_failures = first_failures.rename(columns={"stage_name": "failure_stage"}) + + # Merge this information back with the trial characteristics + trials_with_failure_stage = pd.merge( + df_trials, + first_failures[["trial_id", "failure_stage"]], + on="trial_id", + how="left", + ) + failure_stages = trials_with_failure_stage["failure_stage"].dropna().unique() + + print("\n\n" + "=" * 60) + print(" STATISTICAL SUMMARY BY STAGE OF FAILURE") + print("=" * 60) + + for stage in failure_stages: + stage_specific_failures_df = trials_with_failure_stage[ + trials_with_failure_stage["failure_stage"] == stage + ] + if not stage_specific_failures_df.empty: + print( + f"\n--- For Trials Failing at Stage: '{stage}' ({len(stage_specific_failures_df)} trials) ---" + ) + print( + stage_specific_failures_df[characteristic_cols] + .describe() + .transpose() + .to_string() + ) print("=" * 60) From b423c7e7260fc00536baaf6731f27f5099ab1686 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 12:58:40 -0700 Subject: [PATCH 217/238] Adjust resting parameters --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index fb003d82..ccb358a2 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -153,9 +153,9 @@ class SceneGenerationParams: in_food_tool_roll_angle_deg: float = 180.0 above_food_offset_dist: float = 0.1 # 10 cm staging_offset_dist: float = 0.15 # 15 cm - resting_angular_offset_deg: float = 20.0 - resting_radial_dist: float = 0.8 - resting_vertical_offset: float = ARTICUTOOL_LENGTH_M + 0.2 + resting_angular_offset_deg: float = 45.0 + resting_radial_dist: float = 0.6 + resting_vertical_offset: float = ARTICUTOOL_LENGTH_M # --- Constraint Helpers --- From 4864274e58506ade900d0cf16e0ea8dba179685e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 14:55:59 -0700 Subject: [PATCH 218/238] Add anti-roll constraint for motion to AboveFood --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index ccb358a2..02cff1d9 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1626,10 +1626,17 @@ def _plan_to_above_food( # 2. Plan for the Jaco arm jaco_goal_constraints = [create_joint_constraint(target_jaco_config)] + jaco_path_constraints = [ + create_orientation_path_constraint( + quat_xyzw=PATH_CONSTRAINT_QUAT_XYZW, + tolerance_rad=(np.pi, 2 * np.pi, 2 * np.pi), + ) + ] status_jaco, traj_jaco, time_jaco = self.motion_planner.plan( group_name=PLANNING_GROUP_JACO, start_state=start_state_jaco, goal_constraints=jaco_goal_constraints, + path_constraints=jaco_path_constraints, ) if status_jaco != TrialStatus.SUCCESS: From de153ee796b9316466c268b2e489b5ead92d2c7d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 16:30:55 -0700 Subject: [PATCH 219/238] Updates the benchmark to save results to a .jsonl file after each trial, improving robustness and memory efficiency. The analysis script has also been updated to correctly parse this new format. --- ada_feeding/scripts/analyze_benchmark.py | 63 ++++++++----------- .../scripts/end_to_end_feeding_benchmark.py | 45 +++++++------ 2 files changed, 53 insertions(+), 55 deletions(-) diff --git a/ada_feeding/scripts/analyze_benchmark.py b/ada_feeding/scripts/analyze_benchmark.py index 5b076ce3..ee552ecf 100644 --- a/ada_feeding/scripts/analyze_benchmark.py +++ b/ada_feeding/scripts/analyze_benchmark.py @@ -6,52 +6,41 @@ def analyze_benchmark_results(file_path: str): """ - Loads benchmark data, calculates key metrics, and prints a summary report, + Loads benchmark data from a .jsonl file, calculates key metrics, and prints a summary report, including statistics grouped by the stage of failure. """ try: - with open(file_path, "r") as f: - data = json.load(f) - except (FileNotFoundError, json.JSONDecodeError) as e: + # --- Load the .jsonl file directly into a main DataFrame --- + main_df = pd.read_json(file_path, lines=True) + except (FileNotFoundError, ValueError) as e: print(f"Error reading or parsing file: {e}") return - if not data: + if main_df.empty: print("No data found in the benchmark file.") return - # --- 1. Flatten the data for easier analysis with pandas --- - # Each row in this DataFrame will represent a single stage from a single trial. - flat_stages = [] - trial_characteristics = [] - - for trial in data: - trial_characteristics.append( - { - "trial_id": trial.get("trial_id"), - "end_to_end_success": trial.get("end_to_end_success"), - **trial.get("scene_characteristics", {}), - } - ) - for stage in trial.get("stages", []): - flat_record = { - "trial_id": trial.get("trial_id"), - "end_to_end_success": trial.get("end_to_end_success"), - "stage_name": stage.get("stage_name"), - "status": stage.get("status"), - "planning_time_sec": stage.get("planning_time_sec"), - "path_length_m": stage.get("trajectory_path_length_m"), - } - # Add custom metrics if they exist - if stage.get("custom_metrics"): - flat_record.update(stage["custom_metrics"]) - flat_stages.append(flat_record) - - df_stages = pd.DataFrame(flat_stages) - df_trials = pd.DataFrame(trial_characteristics) - - # --- 2. Print High-Level Summary --- - num_trials = len(data) + # --- Properly flatten the nested data into two clean DataFrames --- + + # 1. Create the trials DataFrame by flattening the 'scene_characteristics' + characteristics_df = pd.json_normalize(main_df["scene_characteristics"]) + df_trials = pd.concat( + [ + main_df.drop( + columns=["scene_characteristics", "stages", "scene_poses", "parameters"] + ), + characteristics_df, + ], + axis=1, + ) + + # 2. Create the stages DataFrame by "exploding" the list of stages + df_stages = main_df[["trial_id", "stages"]].explode("stages").reset_index(drop=True) + stage_details = pd.json_normalize(df_stages["stages"]) + df_stages = pd.concat([df_stages.drop(columns=["stages"]), stage_details], axis=1) + + # --- 3. Print High-Level Summary (No changes needed from here on) --- + num_trials = len(df_trials) successful_trials = df_trials["end_to_end_success"].sum() overall_success_rate = ( (successful_trials / num_trials) * 100 if num_trials > 0 else 0 diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 02cff1d9..fd353394 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -866,8 +866,14 @@ def __init__( ): self.node = node self.num_trials = num_trials - self.output_dir = output_dir - self.results: List[Dict[str, Any]] = [] + self.output_filename = None + if output_dir: + os.makedirs(output_dir, exist_ok=True) + timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") + # Use the .jsonl extension for JSON Lines format + self.output_filename = os.path.join( + output_dir, f"end_to_end_benchmark_{timestamp}.jsonl" + ) self.motion_planner = MotionPlanner( node, @@ -880,9 +886,6 @@ def __init__( # Pinocchio model for feasibility checks self.kinematics_model = PinocchioModel(xacro_file_path) - if self.output_dir: - os.makedirs(self.output_dir, exist_ok=True) - def add_articutool_bounding_cylinder(self): """ Adds a cylindrical collision object to represent the Articutool @@ -2085,23 +2088,29 @@ def run(self): if not trial_failed: trial_data["end_to_end_success"] = True - self.results.append(trial_data) - self.save_results() + self._save_trial_data(trial_data) + LOGGER.info(f"Trial {i} data saved to {self.output_filename}") - def save_results(self): - """Saves the comprehensive benchmark results to a single JSON file.""" - if not self.output_dir: + LOGGER.info("Benchmark finished.") + + def _save_trial_data(self, trial_data: Dict[str, Any]): + """ + Saves a single trial's data by appending it as a new line + to the output file. + """ + if not self.output_filename: return - timestamp = datetime.now().strftime("%Y%m%d-%H%M%S") - filename = os.path.join( - self.output_dir, f"end_to_end_benchmark_{timestamp}.json" - ) try: - with open(filename, "w") as f: - json.dump(self.results, f, indent=2) - LOGGER.info(f"Benchmark results saved to {filename}") + # Open the file in append mode ('a') + with open(self.output_filename, "a") as f: + # Use json.dumps to serialize the single trial dictionary + json_string = json.dumps(trial_data) + # Write the string followed by a newline character + f.write(json_string + "\n") except Exception as e: - LOGGER.error(f"Failed to save results: {e}") + LOGGER.error( + f"Failed to save trial data for trial {trial_data.get('trial_id', 'N/A')}: {e}" + ) def main(): From 6ff01ec9a113a3fcda396a0d4e007e1aafed4515 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 16:58:19 -0700 Subject: [PATCH 220/238] Re-integrate sampling poses from a spherical shell --- .../scripts/end_to_end_feeding_benchmark.py | 70 +++++++++++++++++-- 1 file changed, 65 insertions(+), 5 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index fd353394..51d38acb 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -136,6 +136,18 @@ class CylindricalSamplingParams: max_height: float +@dataclass +class SphericalSamplingParams: + """Parameters for sampling a pose within a spherical shell.""" + + name: str + inner_radius: float + outer_radius: float + # Angles in radians for theta (XY plane) and phi (from Z-axis) + theta_range: Tuple[float, float] + phi_range: Tuple[float, float] + + @dataclass class SceneGenerationParams: """Holds all parameters that define the random scene generation.""" @@ -146,6 +158,13 @@ class SceneGenerationParams: mouth_sampling: CylindricalSamplingParams = CylindricalSamplingParams( name="mouth", inner_radius=0.0, outer_radius=1.0, min_height=0.0, max_height=0.6 ) + resting_sampling: SphericalSamplingParams = SphericalSamplingParams( + name="resting", + inner_radius=0.0, + outer_radius=1.0, + theta_range=(0.0, 2 * math.pi), + phi_range=(0, math.pi / 2), + ) above_plate_radial_dist: float = 0.3 above_plate_polar_angle_rad_max: float = math.pi / 3 above_plate_yaw_variability_rad: float = math.pi / 8 @@ -153,9 +172,6 @@ class SceneGenerationParams: in_food_tool_roll_angle_deg: float = 180.0 above_food_offset_dist: float = 0.1 # 10 cm staging_offset_dist: float = 0.15 # 15 cm - resting_angular_offset_deg: float = 45.0 - resting_radial_dist: float = 0.6 - resting_vertical_offset: float = ARTICUTOOL_LENGTH_M # --- Constraint Helpers --- @@ -250,9 +266,10 @@ def generate(self) -> Tuple[Dict[str, Any], Dict[str, float]]: scene["in_food_pose"], approach_vector ) scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) - scene["resting_pose"] = self._calculate_resting_pose( - scene["food_pose"], scene["mouth_pose"] + scene["resting_pose"], resting_params = self._sample_pose_in_spherical_shell( + self.params.resting_sampling ) + scene_characteristics.update(resting_params) # Add derived characteristics for analysis food_pos = scene["food_pose"].position @@ -319,6 +336,49 @@ def _sample_pose_in_cylindrical_shell( } return final_pose, sampled_values + def _sample_pose_in_spherical_shell( + self, sampling_params: SphericalSamplingParams + ) -> Tuple[Pose, Dict[str, float]]: + """ + Samples a random pose within a spherical shell. The orientation is + kept upright and pointed towards the robot base. + """ + # Sample position + r = np.random.uniform( + sampling_params.inner_radius**3, sampling_params.outer_radius**3 + ) ** (1 / 3) + theta = np.random.uniform(*sampling_params.theta_range) + phi = np.random.uniform(*sampling_params.phi_range) + x = r * np.cos(theta) * np.sin(phi) + y = r * np.sin(theta) * np.sin(phi) + z = r * np.cos(phi) + position = Point(x=x, y=y, z=z) + + # --- Orientation Logic (upright and facing base) --- + z_axis = np.array([0.0, 0.0, 1.0]) # World up + x_axis_direction = -np.array([x, y, 0.0]) # Points toward origin + if np.linalg.norm(x_axis_direction) < 1e-6: + x_axis_direction = np.array([1.0, 0.0, 0.0]) + x_axis_direction /= np.linalg.norm(x_axis_direction) + y_axis = np.cross(z_axis, x_axis_direction) + + rotation_matrix = np.array([x_axis_direction, y_axis, z_axis]).T + rotation = R.from_matrix(rotation_matrix) + quat = rotation.as_quat() + + final_pose = Pose( + position=position, + orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), + ) + + prefix = sampling_params.name + sampled_values = { + f"{prefix}_sampled_radius": r, + f"{prefix}_sampled_theta_rad": theta, + f"{prefix}_sampled_phi_rad": phi, + } + return final_pose, sampled_values + def _calculate_above_plate_pose( self, food_pose: Pose ) -> Tuple[Pose, Dict[str, float]]: From 7886c71d22c1eff3ee76a89999e3ebe8d2e85ca6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 17:08:25 -0700 Subject: [PATCH 221/238] Update visualization script to use JSON lines --- .../scripts/visualize_e2e_benchmark.py | 49 ++++++++++--------- 1 file changed, 25 insertions(+), 24 deletions(-) diff --git a/ada_feeding/scripts/visualize_e2e_benchmark.py b/ada_feeding/scripts/visualize_e2e_benchmark.py index 1e1e3a30..3be67ab5 100644 --- a/ada_feeding/scripts/visualize_e2e_benchmark.py +++ b/ada_feeding/scripts/visualize_e2e_benchmark.py @@ -121,39 +121,40 @@ def load_pinocchio_model_from_urdf_string( def load_benchmark_data(file_paths: List[str], logger_func=print) -> pd.DataFrame: """ - Loads data from benchmark JSON files, ensuring all trials and stages - are loaded, even if they have no successful trajectories. + Loads data from benchmark .jsonl files, ensuring all trials and + stages are loaded. """ all_stages_flat = [] for file_path in file_paths: try: + # --- Read the file line by line for .jsonl format --- with open(file_path, "r") as f: - trials = json.load(f) - for trial in trials: - # If a trial has no stages, we can't do much, but we can still show the scene - if not trial.get("stages"): - all_stages_flat.append( - { - "trial_id": trial.get("trial_id"), - "scene_poses": trial.get("scene_poses"), - "stage_name": "N/A", - "status": "NoStages", - } - ) - continue + # Create a list of trial dictionaries by parsing each line + trials = [json.loads(line) for line in f if line.strip()] - for stage in trial.get("stages", []): - flat_record = { + for trial in trials: + if not trial.get("stages"): + all_stages_flat.append( + { "trial_id": trial.get("trial_id"), "scene_poses": trial.get("scene_poses"), - "stage_name": stage.get("stage_name"), - "status": stage.get("status"), - "execution_mode": stage.get("execution_mode"), - "traj_jaco": stage.get("traj_jaco"), - "traj_atool": stage.get("traj_atool"), + "stage_name": "N/A", + "status": "NoStages", } - # Load all stages regardless of trajectory existence - all_stages_flat.append(flat_record) + ) + continue + + for stage in trial.get("stages", []): + flat_record = { + "trial_id": trial.get("trial_id"), + "scene_poses": trial.get("scene_poses"), + "stage_name": stage.get("stage_name"), + "status": stage.get("status"), + "execution_mode": stage.get("execution_mode"), + "traj_jaco": stage.get("traj_jaco"), + "traj_atool": stage.get("traj_atool"), + } + all_stages_flat.append(flat_record) except Exception as e: logger_func(f"Warning: Could not load or parse {file_path}. Error: {e}") From 7b29f1d9871c42029988c63cd5fa08c35b1148f5 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Wed, 20 Aug 2025 17:52:16 -0700 Subject: [PATCH 222/238] Abstract pose sampling to first sampling positions in a geometric primitive, then sampling a task specific orientation --- .../scripts/end_to_end_feeding_benchmark.py | 141 ++++++++++-------- 1 file changed, 80 insertions(+), 61 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 51d38acb..331326dd 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -232,15 +232,31 @@ def generate(self) -> Tuple[Dict[str, Any], Dict[str, float]]: scene = {} scene_characteristics = {} - scene["food_pose"], food_params = self._sample_pose_in_cylindrical_shell( + food_position, food_params = self._sample_pose_in_cylindrical_shell( self.params.food_sampling ) - scene["mouth_pose"], mouth_params = self._sample_pose_in_cylindrical_shell( + food_orientation = self._calculate_base_facing_orientation(food_position) + scene["food_pose"] = Pose(position=food_position, orientation=food_orientation) + scene_characteristics.update(food_params) + + mouth_position, mouth_params = self._sample_pose_in_cylindrical_shell( self.params.mouth_sampling ) - scene_characteristics.update(food_params) + mouth_orientation = self._calculate_base_facing_orientation(mouth_position) + scene["mouth_pose"] = Pose( + position=mouth_position, orientation=mouth_orientation + ) scene_characteristics.update(mouth_params) + resting_position, resting_params = self._sample_pose_in_spherical_shell( + self.params.resting_sampling + ) + resting_orientation = self._calculate_jaco_ee_orientation(resting_position) + scene["resting_pose"] = Pose( + position=resting_position, orientation=resting_orientation + ) + scene_characteristics.update(resting_params) + scene["home_config"] = [-1.47568, 2.92779, 1.00845, -2.0847, 1.43588, 1.32575] scene["above_plate_pose"], above_plate_params = ( @@ -266,67 +282,43 @@ def generate(self) -> Tuple[Dict[str, Any], Dict[str, float]]: scene["in_food_pose"], approach_vector ) scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) - scene["resting_pose"], resting_params = self._sample_pose_in_spherical_shell( - self.params.resting_sampling - ) - scene_characteristics.update(resting_params) # Add derived characteristics for analysis food_pos = scene["food_pose"].position mouth_pos = scene["mouth_pose"].position + resting_pos = scene["resting_pose"].position scene_characteristics["food_mouth_distance_m"] = math.sqrt( (food_pos.x - mouth_pos.x) ** 2 + (food_pos.y - mouth_pos.y) ** 2 + (food_pos.z - mouth_pos.z) ** 2 ) + scene_characteristics["food_resting_distance_m"] = math.sqrt( + (food_pos.x - resting_pos.x) ** 2 + + (food_pos.y - resting_pos.y) ** 2 + + (food_pos.z - resting_pos.z) ** 2 + ) return scene, scene_characteristics def _sample_pose_in_cylindrical_shell( self, sampling_params: CylindricalSamplingParams - ) -> Tuple[Pose, Dict[str, float]]: + ) -> Tuple[Point, Dict[str, float]]: """ - Samples a random pose within a cylindrical shell - The frame's Z-axis is constrained to be world up, and its X-axis is - oriented to point towards the robot base with some random variability. + A general function to sample a POSITION within the given cylindrical parameters. + Returns a Point and the sampled characteristic values. """ - # --- Position Sampling in a Cylindrical Shell --- - # 1. Sample the radius and angle radius = np.sqrt( np.random.uniform( sampling_params.inner_radius**2, sampling_params.outer_radius**2 ) ) theta = np.random.uniform(0, 2 * np.pi) + z = np.random.uniform(sampling_params.min_height, sampling_params.max_height) - # 2. Convert to Cartesian coordinates x = radius * np.cos(theta) y = radius * np.sin(theta) - z = np.random.uniform(sampling_params.min_height, sampling_params.max_height) - position = Point(x=x, y=y, z=z) - - # --- Orientation Calculation --- - z_axis = np.array([0.0, 0.0, 1.0]) - look_at_vector = -np.array([x, y, 0.0]) # Project to XY plane - if np.linalg.norm(look_at_vector) < 1e-6: - look_at_vector = np.array([1.0, 0.0, 0.0]) - x_axis_direction = look_at_vector / np.linalg.norm(look_at_vector) - - y_axis = np.cross(z_axis, x_axis_direction) - rotation_matrix = np.array([x_axis_direction, y_axis, z_axis]).T - main_rot = R.from_matrix(rotation_matrix) - - rand_yaw_angle = np.random.uniform(-np.deg2rad(30), np.deg2rad(30)) - variability_rot = R.from_euler("z", rand_yaw_angle) - - final_rot = main_rot * variability_rot - quat = final_rot.as_quat() - # --- Assemble final pose and sampled characteristics --- - final_pose = Pose( - position=position, - orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), - ) + position = Point(x=x, y=y, z=z) prefix = sampling_params.name sampled_values = { @@ -334,42 +326,26 @@ def _sample_pose_in_cylindrical_shell( f"{prefix}_sampled_theta_rad": theta, f"{prefix}_sampled_z": z, } - return final_pose, sampled_values + return position, sampled_values def _sample_pose_in_spherical_shell( self, sampling_params: SphericalSamplingParams - ) -> Tuple[Pose, Dict[str, float]]: + ) -> Tuple[Point, Dict[str, float]]: """ - Samples a random pose within a spherical shell. The orientation is - kept upright and pointed towards the robot base. + A general function to sample a POSITION within a spherical shell. + Returns a Point and the sampled characteristic values. """ - # Sample position r = np.random.uniform( sampling_params.inner_radius**3, sampling_params.outer_radius**3 ) ** (1 / 3) theta = np.random.uniform(*sampling_params.theta_range) phi = np.random.uniform(*sampling_params.phi_range) + x = r * np.cos(theta) * np.sin(phi) y = r * np.sin(theta) * np.sin(phi) z = r * np.cos(phi) - position = Point(x=x, y=y, z=z) - - # --- Orientation Logic (upright and facing base) --- - z_axis = np.array([0.0, 0.0, 1.0]) # World up - x_axis_direction = -np.array([x, y, 0.0]) # Points toward origin - if np.linalg.norm(x_axis_direction) < 1e-6: - x_axis_direction = np.array([1.0, 0.0, 0.0]) - x_axis_direction /= np.linalg.norm(x_axis_direction) - y_axis = np.cross(z_axis, x_axis_direction) - - rotation_matrix = np.array([x_axis_direction, y_axis, z_axis]).T - rotation = R.from_matrix(rotation_matrix) - quat = rotation.as_quat() - final_pose = Pose( - position=position, - orientation=Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]), - ) + position = Point(x=x, y=y, z=z) prefix = sampling_params.name sampled_values = { @@ -377,7 +353,50 @@ def _sample_pose_in_spherical_shell( f"{prefix}_sampled_theta_rad": theta, f"{prefix}_sampled_phi_rad": phi, } - return final_pose, sampled_values + return position, sampled_values + + def _calculate_base_facing_orientation(self, position: Point) -> Quaternion: + """ + Calculates an orientation that is upright (Z-up) and has its X-axis + pointing towards the robot base (origin). + """ + z_axis = np.array([0.0, 0.0, 1.0]) + look_at_vector = -np.array([position.x, position.y, 0.0]) + if np.linalg.norm(look_at_vector) < 1e-6: + look_at_vector = np.array([1.0, 0.0, 0.0]) + x_axis = look_at_vector / np.linalg.norm(look_at_vector) + y_axis = np.cross(z_axis, x_axis) + + rotation_matrix = np.array([x_axis, y_axis, z_axis]).T + # Add random yaw variability + rand_yaw = np.random.uniform(-np.deg2rad(30), np.deg2rad(30)) + final_rot = R.from_matrix(rotation_matrix) * R.from_euler("z", rand_yaw) + quat = final_rot.as_quat() + + return Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) + + def _calculate_jaco_ee_orientation(self, position: Point) -> Quaternion: + """ + Calculates an orientation matching the Jaco EE convention (Y-up, Z-forward) + at the given position. + """ + # (Y-up) is the world's Z-axis + target_y_axis = np.array([0.0, 0.0, 1.0]) + + # (Z-forward) points horizontally away from the robot's base + target_z_axis_dir = np.array([position.x, position.y, 0.0]) + if np.linalg.norm(target_z_axis_dir) < 1e-6: + target_z_axis_dir = np.array([1.0, 0.0, 0.0]) + target_z_axis = target_z_axis_dir / np.linalg.norm(target_z_axis_dir) + + # (X-left) is derived from the cross product + target_x_axis = np.cross(target_y_axis, target_z_axis) + + rotation_matrix = np.array([target_x_axis, target_y_axis, target_z_axis]).T + rotation = R.from_matrix(rotation_matrix) + quat = rotation.as_quat() + + return Quaternion(x=quat[0], y=quat[1], z=quat[2], w=quat[3]) def _calculate_above_plate_pose( self, food_pose: Pose From fefeb40d6fcfc0d983fc44a17b4b662ab344c71b Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 21 Aug 2025 08:22:14 -0700 Subject: [PATCH 223/238] Add metrics for pose distance, angular offsets, and logging the target frame --- .../scripts/end_to_end_feeding_benchmark.py | 43 +++++++++++++++++++ 1 file changed, 43 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 331326dd..68a1b1fb 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -256,6 +256,9 @@ def generate(self) -> Tuple[Dict[str, Any], Dict[str, float]]: position=resting_position, orientation=resting_orientation ) scene_characteristics.update(resting_params) + scene_characteristics.update( + self._characterize_pose(scene["resting_pose"], "resting_pose") + ) scene["home_config"] = [-1.47568, 2.92779, 1.00845, -2.0847, 1.43588, 1.32575] @@ -263,6 +266,9 @@ def generate(self) -> Tuple[Dict[str, Any], Dict[str, float]]: self._calculate_above_plate_pose(scene["food_pose"]) ) scene_characteristics.update(above_plate_params) + scene_characteristics.update( + self._characterize_pose(scene["above_plate_pose"], "above_plate_pose") + ) ( scene["in_food_pose"], @@ -277,11 +283,20 @@ def generate(self) -> Tuple[Dict[str, Any], Dict[str, float]]: ), ) scene_characteristics.update(in_food_params) + scene_characteristics.update( + self._characterize_pose(scene["in_food_pose"], "in_food_pose") + ) scene["above_food_pose"] = self._calculate_above_food_pose( scene["in_food_pose"], approach_vector ) + scene_characteristics.update( + self._characterize_pose(scene["above_food_pose"], "above_food_pose") + ) scene["staging_pose"] = self._calculate_staging_pose(scene["mouth_pose"]) + scene_characteristics.update( + self._characterize_pose(scene["staging_pose"], "staging_pose") + ) # Add derived characteristics for analysis food_pos = scene["food_pose"].position @@ -300,6 +315,29 @@ def generate(self) -> Tuple[Dict[str, Any], Dict[str, float]]: return scene, scene_characteristics + def _characterize_pose(self, pose: Pose, pose_name: str) -> Dict[str, float]: + """Extracts key kinematic characteristics from a pose.""" + pos = np.array([pose.position.x, pose.position.y, pose.position.z]) + rot = R.from_quat( + [ + pose.orientation.x, + pose.orientation.y, + pose.orientation.z, + pose.orientation.w, + ] + ) + + # For a Jaco EE frame, the Z-axis is "forward" + forward_vec = rot.apply([0.0, 0.0, 1.0]) + + characteristics = { + f"{pose_name}_dist_3d": np.linalg.norm(pos), + f"{pose_name}_dist_2d": np.linalg.norm(pos[:2]), + f"{pose_name}_global_yaw_rad": np.arctan2(forward_vec[1], forward_vec[0]), + f"{pose_name}_global_pitch_rad": np.arcsin(forward_vec[2]), + } + return characteristics + def _sample_pose_in_cylindrical_shell( self, sampling_params: CylindricalSamplingParams ) -> Tuple[Point, Dict[str, float]]: @@ -2016,6 +2054,7 @@ def run(self): trial_data["stages"].append( { "stage_name": "HomeToAbovePlate", + "target_frame": END_EFFECTOR_LINK_JACO, "status": status.value, "execution_mode": ExecutionMode.JACO_ONLY.value, "planning_time_sec": planning_time, @@ -2051,6 +2090,7 @@ def run(self): trial_data["stages"].append( { "stage_name": "AbovePlateToAboveFood", + "target_frame": END_EFFECTOR_LINK_FULL, "status": status.value, "execution_mode": ExecutionMode.SEQUENTIAL.value, "planning_time_sec": planning_time, @@ -2086,6 +2126,7 @@ def run(self): trial_data["stages"].append( { "stage_name": "AboveFoodToInFood", + "target_frame": END_EFFECTOR_LINK_FULL, "status": status.value, "execution_mode": ExecutionMode.SYNCHRONOUS.value, "planning_time_sec": planning_time, @@ -2114,6 +2155,7 @@ def run(self): trial_data["stages"].append( { "stage_name": "LevelArticutool", + "target_frame": END_EFFECTOR_LINK_ATOOL, "status": status.value, "execution_mode": ExecutionMode.ATOOL_ONLY.value, "planning_time_sec": planning_time, @@ -2145,6 +2187,7 @@ def run(self): trial_data["stages"].append( { "stage_name": "Resting", + "target_frame": END_EFFECTOR_LINK_JACO, "status": status.value, "execution_mode": ExecutionMode.SYNCHRONOUS.value, "planning_time_sec": planning_time, From 7ee7580afc8ad842e948df3abd7eed60054b4f72 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 21 Aug 2025 16:34:38 -0700 Subject: [PATCH 224/238] Add unmeasured data to results --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 68a1b1fb..95c6c0b1 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -2020,6 +2020,15 @@ def run(self): # 2. Generate the scene and characteristics with a single, clean call scene, scene_characteristics = scene_generator.generate() + params_dict = asdict(generation_params) + params_dict["path_constraint_tolerance_xyz_rad"] = list( + PATH_CONSTRAINT_TOLERANCE_XYZ_RAD + ) + params_dict["articutool_pitch_limits_rad"] = list( + ARTICUTOOL_PITCH_LIMITS_RAD + ) + params_dict["articutool_roll_limits_rad"] = list(ARTICUTOOL_ROLL_LIMITS_RAD) + trial_data = { "trial_id": i, "scene_poses": { @@ -2032,7 +2041,7 @@ def run(self): "resting_pose": self._serialize_pose(scene["resting_pose"]), }, "scene_characteristics": scene_characteristics, - "parameters": asdict(generation_params), + "parameters": params_dict, "stages": [], "end_to_end_success": False, # Default to False } From 7ab6d18d6b58d7f4238a167bd9d6a07e9328409e Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Thu, 21 Aug 2025 17:19:42 -0700 Subject: [PATCH 225/238] Sample food and mouth frame from spherical shell --- .../scripts/end_to_end_feeding_benchmark.py | 24 ++++++++++++------- 1 file changed, 16 insertions(+), 8 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 95c6c0b1..7502c701 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -152,16 +152,24 @@ class SphericalSamplingParams: class SceneGenerationParams: """Holds all parameters that define the random scene generation.""" - food_sampling: CylindricalSamplingParams = CylindricalSamplingParams( - name="food", inner_radius=0.4, outer_radius=0.8, min_height=0.0, max_height=0.3 + food_sampling: SphericalSamplingParams = SphericalSamplingParams( + name="food", + inner_radius=0.4, + outer_radius=1.04, # Corresponds to the 8-DOF workspace + theta_range=(0.0, 2 * math.pi), + phi_range=(0.0, math.pi / 2), # Upper hemisphere ) - mouth_sampling: CylindricalSamplingParams = CylindricalSamplingParams( - name="mouth", inner_radius=0.0, outer_radius=1.0, min_height=0.0, max_height=0.6 + mouth_sampling: SphericalSamplingParams = SphericalSamplingParams( + name="mouth", + inner_radius=0.4, + outer_radius=1.04, # Corresponds to the 8-DOF workspace + theta_range=(0.0, 2 * math.pi), + phi_range=(0.0, math.pi / 2), ) resting_sampling: SphericalSamplingParams = SphericalSamplingParams( name="resting", - inner_radius=0.0, - outer_radius=1.0, + inner_radius=0.5, + outer_radius=0.9, # Corresponds to the 6-DOF workspace theta_range=(0.0, 2 * math.pi), phi_range=(0, math.pi / 2), ) @@ -232,14 +240,14 @@ def generate(self) -> Tuple[Dict[str, Any], Dict[str, float]]: scene = {} scene_characteristics = {} - food_position, food_params = self._sample_pose_in_cylindrical_shell( + food_position, food_params = self._sample_pose_in_spherical_shell( self.params.food_sampling ) food_orientation = self._calculate_base_facing_orientation(food_position) scene["food_pose"] = Pose(position=food_position, orientation=food_orientation) scene_characteristics.update(food_params) - mouth_position, mouth_params = self._sample_pose_in_cylindrical_shell( + mouth_position, mouth_params = self._sample_pose_in_spherical_shell( self.params.mouth_sampling ) mouth_orientation = self._calculate_base_facing_orientation(mouth_position) From 27e8cf0235ee2cc29c7d374bbe3e766fac1880e2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 10:43:00 -0700 Subject: [PATCH 226/238] Temporarily disable Articutool bounding cylinder used for conservative collision detection --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 7502c701..316379fa 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -2016,7 +2016,8 @@ def _plan_s2_guided( # --- Main Benchmark Loop --- def run(self): """Main benchmark execution loop with granular metric collection.""" - self.add_articutool_bounding_cylinder() + # TODO: Decide whether we should keep the bounding cylinder active + # self.add_articutool_bounding_cylinder() for i in range(self.num_trials): LOGGER.info(f"--- Running Trial {i + 1}/{self.num_trials} ---") From 74539acad3fa8bf0dc04b9495cb18a7517c3816c Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 11:33:05 -0700 Subject: [PATCH 227/238] Add position constraint to motion planning interface --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 316379fa..519c4d39 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -227,6 +227,19 @@ def create_joint_constraint( return (MoveIt2ConstraintType.JOINT, {"joint_positions": joint_positions}) +def create_position_constraint( + position: Point, tolerance_position: float = 0.001 +) -> Tuple[MoveIt2ConstraintType, Dict]: + """Creates a standard position goal constraint.""" + return ( + MoveIt2ConstraintType.POSITION, + { + "position": position, + "tolerance_position": tolerance_position, + }, + ) + + class SceneGenerator: """A dedicated class for procedurally generating planning scenes.""" From 8c048c7d9a29b197b014e7b3eb88a2778f71c7d9 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 11:33:56 -0700 Subject: [PATCH 228/238] Loosen orientation constraint for AbovePlate pose --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 519c4d39..8b473981 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1731,7 +1731,9 @@ def _plan_to_above_plate( self, above_plate_pose: Pose, start_state_jaco: Any ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: LOGGER.info(" Planning to AbovePlate pose...") - goal_constraints = [create_pose_constraint(above_plate_pose)] + goal_constraints = [ + create_pose_constraint(above_plate_pose, tolerance_orientation=0.2) + ] return self.motion_planner.plan( group_name=PLANNING_GROUP_JACO, From fd97986e410051308a8ca3df21e888681e5e5340 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 11:40:47 -0700 Subject: [PATCH 229/238] Use position constraint for Resting stage --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 8b473981..b60e6c8f 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -235,7 +235,8 @@ def create_position_constraint( MoveIt2ConstraintType.POSITION, { "position": position, - "tolerance_position": tolerance_position, + "tolerance": tolerance_position, + "weight": 1.0, }, ) @@ -1928,7 +1929,7 @@ def _plan_to_resting( LOGGER.info(" Planning to Resting pose (S2-Heuristic)...") # 1. Define goal and path constraints for the Jaco arm's wrist - goal_constraints = [create_pose_constraint(resting_wrist_pose)] + goal_constraints = [create_position_constraint(resting_wrist_pose.position)] path_constraints = [ create_orientation_path_constraint( quat_xyzw=PATH_CONSTRAINT_QUAT_XYZW, From 1459e57cc058dacca3313cbecccabab2c91364c6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 12:14:20 -0700 Subject: [PATCH 230/238] Major refactor of analysis script to focus on generating granular visualizations of key scene frames for different stages of the feeding cycle, and whether they resulted in success or failure --- ada_feeding/scripts/analyze_benchmark.py | 307 ++++++++++++++++------- 1 file changed, 213 insertions(+), 94 deletions(-) diff --git a/ada_feeding/scripts/analyze_benchmark.py b/ada_feeding/scripts/analyze_benchmark.py index ee552ecf..bae7b2e2 100644 --- a/ada_feeding/scripts/analyze_benchmark.py +++ b/ada_feeding/scripts/analyze_benchmark.py @@ -1,16 +1,181 @@ import argparse import json +import os import pandas as pd -from dataclasses import asdict +import plotly.graph_objects as go +from plotly.subplots import make_subplots + + +def generate_visualizations(df_trials: pd.DataFrame, file_path: str): + """ + Generates interactive Plotly visualizations with a dropdown for stage-specific analysis. + """ + if df_trials.empty: + print("Skipping visualization, no trial data to plot.") + return + + # --- 1. Load and Prepare Granular Stage Data --- + try: + with open(file_path, "r") as f: + full_data = [json.loads(line) for line in f if line.strip()] + except (FileNotFoundError, ValueError) as e: + print(f"Error reading or parsing file for visualization: {e}") + return + + # Create a DataFrame with one row per stage to get granular status + stages_records = [] + for trial in full_data: + for stage in trial.get("stages", []): + stages_records.append( + { + "trial_id": trial["trial_id"], + "stage_name": stage["stage_name"], + "status": ( + 1 if stage["status"] == "Success" else 0 + ), # Use 1 for success, 0 for failure + } + ) + df_stages = pd.DataFrame(stages_records) + + # Pivot the data so each trial has a column for each stage's status + df_stage_status = df_stages.pivot( + index="trial_id", columns="stage_name", values="status" + ) + df_stage_status.columns = [ + f"status_{col}" for col in df_stage_status.columns + ] # Rename columns + + # --- 2. Prepare Data for 3D Workspace Plot --- + plot_data = [] + for trial in full_data: + trial_id = trial.get("trial_id") + # Merge trial characteristics and stage statuses + characteristics = df_trials[df_trials["trial_id"] == trial_id] + if characteristics.empty: + continue + + trial_info = characteristics.iloc[0].to_dict() + if trial_id in df_stage_status.index: + trial_info.update(df_stage_status.loc[trial_id].to_dict()) + + for pose_name, pose_data in trial.get("scene_poses", {}).items(): + if pose_name in ["food_pose", "mouth_pose", "resting_pose"]: + pos = pose_data.get("position") + if not isinstance(pos, list) or len(pos) != 3: + continue + + # Add all info to the record for plotting + record = {"x": pos[0], "y": pos[1], "z": pos[2], "pose_name": pose_name} + record.update(trial_info) + plot_data.append(record) + + if not plot_data: + print("No valid pose data found to generate visualizations.") + return + vis_df = pd.DataFrame(plot_data) + + # --- 3. Create Figure and Traces --- + fig = make_subplots( + rows=1, + cols=1, + specs=[[{"type": "scatter3d"}]], + subplot_titles=("3D Workspace Visualization",), + ) + + # Define all possible status columns we can filter by + status_cols = {"End-to-End": "end_to_end_success"} + for col in df_stage_status.columns: + stage_name = col.replace("status_", "") + status_cols[f"Stage: {stage_name}"] = col + + symbols = {"food_pose": "circle", "mouth_pose": "square", "resting_pose": "diamond"} + + # Create all traces upfront, most will be hidden initially + for status_name, status_col in status_cols.items(): + for pose_name, symbol in symbols.items(): + for success_val, color in zip([1, 0], ["green", "red"]): + is_success = success_val == 1 + df_subset = vis_df[ + (vis_df["pose_name"] == pose_name) + & (vis_df[status_col] == success_val) + ] + if not df_subset.empty: + fig.add_trace( + go.Scatter3d( + x=df_subset["x"], + y=df_subset["y"], + z=df_subset["z"], + mode="markers", + marker=dict( + color=color, symbol=symbol, size=5, opacity=0.7 + ), + # Make only the default view (End-to-End) visible initially + visible=(status_name == "End-to-End"), + name=f"{'Success' if is_success else 'Failure'} ({pose_name.replace('_pose', '')})", + customdata=df_subset.to_dict("records"), + hovertemplate="Trial ID: %{customdata.trial_id}
Pose: %{customdata.pose_name}
Success: " + + ("Yes" if is_success else "No") + + "
Distance: %{customdata.food_mouth_distance_m:.2f}m", + ) + ) + + # --- 4. Create the Dropdown Menu --- + buttons = [] + for i, (status_name, status_col) in enumerate(status_cols.items()): + # Create a visibility mask for the traces + # Each status type has 6 traces (3 poses x 2 outcomes) + visibility = [False] * len(fig.data) + start_index = i * 6 + for j in range(6): + if (start_index + j) < len(visibility): + visibility[start_index + j] = True + + buttons.append( + dict(label=status_name, method="update", args=[{"visible": visibility}]) + ) + + fig.update_layout( + updatemenus=[ + dict( + active=0, + buttons=buttons, + direction="down", + pad={"r": 10, "t": 10}, + showactive=True, + x=0.1, + xanchor="left", + y=1.1, + yanchor="top", + ) + ], + margin=dict(l=0, r=0, b=0, t=40), + scene=dict( + xaxis_title="X (m)", + yaxis_title="Y (m)", + zaxis_title="Z (m)", + aspectmode="data", + ), + legend_title="Legend", + ) + + # --- 5. Save to File --- + output_filename = "benchmark_visualization.html" + try: + fig.write_html(output_filename) + print( + f"\nInteractive visualization saved to: {os.path.abspath(output_filename)}" + ) + except Exception as e: + print(f"\nError saving visualization file: {e}") def analyze_benchmark_results(file_path: str): """ - Loads benchmark data from a .jsonl file, calculates key metrics, and prints a summary report, - including statistics grouped by the stage of failure. + Loads benchmark data from a .jsonl file, calculates key metrics, prints a summary report, + and generates interactive visualizations. """ try: - # --- Load the .jsonl file directly into a main DataFrame --- + # Load the .jsonl file directly into a main DataFrame main_df = pd.read_json(file_path, lines=True) except (FileNotFoundError, ValueError) as e: print(f"Error reading or parsing file: {e}") @@ -22,24 +187,25 @@ def analyze_benchmark_results(file_path: str): # --- Properly flatten the nested data into two clean DataFrames --- - # 1. Create the trials DataFrame by flattening the 'scene_characteristics' + # 1. Create the trials DataFrame by flattening 'scene_characteristics' characteristics_df = pd.json_normalize(main_df["scene_characteristics"]) df_trials = pd.concat( [ main_df.drop( - columns=["scene_characteristics", "stages", "scene_poses", "parameters"] + columns=[ + "scene_characteristics", + "stages", + "scene_poses", + "parameters", + ], + errors="ignore", ), characteristics_df, ], axis=1, ) - # 2. Create the stages DataFrame by "exploding" the list of stages - df_stages = main_df[["trial_id", "stages"]].explode("stages").reset_index(drop=True) - stage_details = pd.json_normalize(df_stages["stages"]) - df_stages = pd.concat([df_stages.drop(columns=["stages"]), stage_details], axis=1) - - # --- 3. Print High-Level Summary (No changes needed from here on) --- + # --- Print High-Level Summary --- num_trials = len(df_trials) successful_trials = df_trials["end_to_end_success"].sum() overall_success_rate = ( @@ -56,99 +222,52 @@ def analyze_benchmark_results(file_path: str): ) print("-" * 60) - if not df_stages.empty: - failed_stages_df = df_stages[df_stages["status"] != "Success"] - stage_failure_counts = failed_stages_df["stage_name"].value_counts() - print("\nFailure Count by Stage:") - if not stage_failure_counts.empty: - print(stage_failure_counts.to_string()) - else: - print("No stage failures recorded.") - print("-" * 60) - - # --- Failure analysis by TYPE for the most problematic stage --- - if not stage_failure_counts.empty: - most_problematic_stage = stage_failure_counts.index[0] - print( - f"\nFailure Breakdown for Most Problematic Stage ('{most_problematic_stage}'):" - ) - problem_stage_df = failed_stages_df[ - failed_stages_df["stage_name"] == most_problematic_stage - ] - status_counts = problem_stage_df["status"].value_counts() - print(status_counts.to_string()) - print("=" * 60) - - # --- 3. Generate Comprehensive Statistical Summaries (Binary Pass/Fail) --- - if not df_trials.empty: - # Define the columns we want to summarize - characteristic_cols = [ - col - for col in df_trials.columns - if col not in ["trial_id", "end_to_end_success"] - ] - - # Separate successful and failed trials - successful_df = df_trials[df_trials["end_to_end_success"] == True] - - print("\n COMPREHENSIVE STATISTICAL SUMMARY") - print("-" * 60) - - if not successful_df.empty: - print( - f"\n--- For SUCCESSFUL End-to-End Trials ({len(successful_df)} trials) ---" - ) - # Use .describe() to get summary statistics, transpose for readability - print(successful_df[characteristic_cols].describe().transpose().to_string()) - else: - print("\n--- No SUCCESSFUL End-to-End Trials to Summarize ---") - - # --- 4. Generate Statistical Summaries by Failure Stage --- - if not df_stages.empty and not df_trials.empty: - # Find the first failing stage for each failed trial - failed_stages_df = df_stages[df_stages["status"] != "Success"] - first_failures = failed_stages_df.loc[ - failed_stages_df.groupby("trial_id").head(1).index - ] - first_failures = first_failures.rename(columns={"stage_name": "failure_stage"}) - - # Merge this information back with the trial characteristics - trials_with_failure_stage = pd.merge( - df_trials, - first_failures[["trial_id", "failure_stage"]], - on="trial_id", - how="left", + # 2. Create the stages DataFrame by "exploding" the list of stages + if "stages" in main_df.columns and not main_df["stages"].isnull().all(): + df_stages = ( + main_df[["trial_id", "stages"]].explode("stages").reset_index(drop=True) + ) + # Filter out rows where stages might be null after exploding + df_stages.dropna(subset=["stages"], inplace=True) + stage_details = pd.json_normalize(df_stages["stages"]) + df_stages = pd.concat( + [df_stages.drop(columns=["stages"]), stage_details], axis=1 ) - failure_stages = trials_with_failure_stage["failure_stage"].dropna().unique() - - print("\n\n" + "=" * 60) - print(" STATISTICAL SUMMARY BY STAGE OF FAILURE") - print("=" * 60) + if not df_stages.empty: + failed_stages_df = df_stages[df_stages["status"] != "Success"] + stage_failure_counts = failed_stages_df["stage_name"].value_counts() + print("\nFailure Count by Stage:") + if not stage_failure_counts.empty: + print(stage_failure_counts.to_string()) + else: + print("No stage failures recorded.") + print("-" * 60) - for stage in failure_stages: - stage_specific_failures_df = trials_with_failure_stage[ - trials_with_failure_stage["failure_stage"] == stage - ] - if not stage_specific_failures_df.empty: + # --- Failure analysis by TYPE for the most problematic stage --- + if not stage_failure_counts.empty: + most_problematic_stage = stage_failure_counts.index[0] print( - f"\n--- For Trials Failing at Stage: '{stage}' ({len(stage_specific_failures_df)} trials) ---" + f"\nFailure Breakdown for Most Problematic Stage ('{most_problematic_stage}'):" ) - print( - stage_specific_failures_df[characteristic_cols] - .describe() - .transpose() - .to_string() - ) - print("=" * 60) + problem_stage_df = failed_stages_df[ + failed_stages_df["stage_name"] == most_problematic_stage + ] + status_counts = problem_stage_df["status"].value_counts() + print(status_counts.to_string()) + print("=" * 60) + + # --- Generate Visualizations --- + # Call the new function to generate and save the plots + generate_visualizations(df_trials, file_path) if __name__ == "__main__": parser = argparse.ArgumentParser( - description="Analyze end-to-end benchmark results." + description="Analyze end-to-end benchmark results and generate visualizations." ) parser.add_argument( - "benchmark_file", type=str, help="Path to the benchmark JSON output file." + "benchmark_file", type=str, help="Path to the benchmark JSONL output file." ) args = parser.parse_args() From 9519c48fba2007f3e554150c74514077cb8585f6 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 12:21:30 -0700 Subject: [PATCH 231/238] Use a relevance map to only show frames relevant to a particular stage when visualizing --- ada_feeding/scripts/analyze_benchmark.py | 54 ++++++++++++++---------- 1 file changed, 32 insertions(+), 22 deletions(-) diff --git a/ada_feeding/scripts/analyze_benchmark.py b/ada_feeding/scripts/analyze_benchmark.py index bae7b2e2..18bb367a 100644 --- a/ada_feeding/scripts/analyze_benchmark.py +++ b/ada_feeding/scripts/analyze_benchmark.py @@ -8,7 +8,8 @@ def generate_visualizations(df_trials: pd.DataFrame, file_path: str): """ - Generates interactive Plotly visualizations with a dropdown for stage-specific analysis. + Generates interactive Plotly visualizations with a context-aware dropdown + for stage-specific analysis. """ if df_trials.empty: print("Skipping visualization, no trial data to plot.") @@ -75,22 +76,26 @@ def generate_visualizations(df_trials: pd.DataFrame, file_path: str): vis_df = pd.DataFrame(plot_data) # --- 3. Create Figure and Traces --- - fig = make_subplots( - rows=1, - cols=1, - specs=[[{"type": "scatter3d"}]], - subplot_titles=("3D Workspace Visualization",), - ) + fig = go.Figure() # Use go.Figure for more control - # Define all possible status columns we can filter by + # Define all possible status columns and their relevant poses for filtering status_cols = {"End-to-End": "end_to_end_success"} for col in df_stage_status.columns: stage_name = col.replace("status_", "") status_cols[f"Stage: {stage_name}"] = col + relevance_map = { + "End-to-End": ["food_pose", "mouth_pose", "resting_pose"], + "Stage: HomeToAbovePlate": ["food_pose"], + "Stage: AbovePlateToAboveFood": ["food_pose"], + "Stage: AboveFoodToInFood": ["food_pose"], + "Stage: LevelArticutool": ["food_pose"], + "Stage: Resting": ["food_pose", "resting_pose"], + } + symbols = {"food_pose": "circle", "mouth_pose": "square", "resting_pose": "diamond"} - # Create all traces upfront, most will be hidden initially + # Create all traces upfront for status_name, status_col in status_cols.items(): for pose_name, symbol in symbols.items(): for success_val, color in zip([1, 0], ["green", "red"]): @@ -113,28 +118,33 @@ def generate_visualizations(df_trials: pd.DataFrame, file_path: str): visible=(status_name == "End-to-End"), name=f"{'Success' if is_success else 'Failure'} ({pose_name.replace('_pose', '')})", customdata=df_subset.to_dict("records"), - hovertemplate="Trial ID: %{customdata.trial_id}
Pose: %{customdata.pose_name}
Success: " - + ("Yes" if is_success else "No") + hovertemplate="Trial ID: %{customdata.trial_id}
Pose: %{customdata.pose_name}
Status: " + + ("Success" if is_success else "Failure") + "
Distance: %{customdata.food_mouth_distance_m:.2f}m", ) ) - # --- 4. Create the Dropdown Menu --- + # --- 4. Create the Dropdown Menu with Filtering Logic --- buttons = [] - for i, (status_name, status_col) in enumerate(status_cols.items()): - # Create a visibility mask for the traces - # Each status type has 6 traces (3 poses x 2 outcomes) - visibility = [False] * len(fig.data) - start_index = i * 6 - for j in range(6): - if (start_index + j) < len(visibility): - visibility[start_index + j] = True + for status_name_key in status_cols.keys(): + visibility = [] + # This nested loop must match the trace creation order exactly + for status_name_trace in status_cols.keys(): + for pose_name_trace in symbols.keys(): + for _ in [1, 0]: # Success and Failure traces + is_visible = False + if status_name_key == status_name_trace: + relevant_poses = relevance_map.get(status_name_key, []) + if pose_name_trace in relevant_poses: + is_visible = True + visibility.append(is_visible) buttons.append( - dict(label=status_name, method="update", args=[{"visible": visibility}]) + dict(label=status_name_key, method="update", args=[{"visible": visibility}]) ) fig.update_layout( + title="3D Workspace Visualization", updatemenus=[ dict( active=0, @@ -142,7 +152,7 @@ def generate_visualizations(df_trials: pd.DataFrame, file_path: str): direction="down", pad={"r": 10, "t": 10}, showactive=True, - x=0.1, + x=0.05, xanchor="left", y=1.1, yanchor="top", From 6bf7443751511a59da15b04f3f292eb48834e332 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 12:40:29 -0700 Subject: [PATCH 232/238] Add min/max height parameters to spherical sampling --- .../scripts/end_to_end_feeding_benchmark.py | 45 +++++++++++-------- 1 file changed, 27 insertions(+), 18 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index b60e6c8f..c629378b 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -146,6 +146,8 @@ class SphericalSamplingParams: # Angles in radians for theta (XY plane) and phi (from Z-axis) theta_range: Tuple[float, float] phi_range: Tuple[float, float] + min_height: Optional[float] = None + max_height: Optional[float] = None @dataclass @@ -394,26 +396,33 @@ def _sample_pose_in_spherical_shell( """ A general function to sample a POSITION within a spherical shell. Returns a Point and the sampled characteristic values. + Uses rejection sampling if min/max height is specified. """ - r = np.random.uniform( - sampling_params.inner_radius**3, sampling_params.outer_radius**3 - ) ** (1 / 3) - theta = np.random.uniform(*sampling_params.theta_range) - phi = np.random.uniform(*sampling_params.phi_range) + max_attempts = 100 + for _ in range(max_attempts): + r = np.random.uniform( + sampling_params.inner_radius**3, sampling_params.outer_radius**3 + ) ** (1 / 3) + theta = np.random.uniform(*sampling_params.theta_range) + phi = np.random.uniform(*sampling_params.phi_range) + + x = r * np.cos(theta) * np.sin(phi) + y = r * np.sin(theta) * np.sin(phi) + z = r * np.cos(phi) - x = r * np.cos(theta) * np.sin(phi) - y = r * np.sin(theta) * np.sin(phi) - z = r * np.cos(phi) - - position = Point(x=x, y=y, z=z) - - prefix = sampling_params.name - sampled_values = { - f"{prefix}_sampled_radius": r, - f"{prefix}_sampled_theta_rad": theta, - f"{prefix}_sampled_phi_rad": phi, - } - return position, sampled_values + if ( + sampling_params.min_height is None or z >= sampling_params.min_height + ) and ( + sampling_params.max_height is None or z <= sampling_params.max_height + ): + position = Point(x=x, y=y, z=z) + prefix = sampling_params.name + sampled_values = { + f"{prefix}_sampled_radius": r, + f"{prefix}_sampled_theta_rad": theta, + f"{prefix}_sampled_phi_rad": phi, + } + return position, sampled_values def _calculate_base_facing_orientation(self, position: Point) -> Quaternion: """ From 7ed054918fe1c73ba4542d690909a96a1a0fb2ca Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 12:59:57 -0700 Subject: [PATCH 233/238] Add min/max height spherical sampling parameters for food, mouth, and resting frames --- ada_feeding/scripts/end_to_end_feeding_benchmark.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index c629378b..05b4fa55 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -160,6 +160,8 @@ class SceneGenerationParams: outer_radius=1.04, # Corresponds to the 8-DOF workspace theta_range=(0.0, 2 * math.pi), phi_range=(0.0, math.pi / 2), # Upper hemisphere + min_height=0.1, + max_height=0.4, ) mouth_sampling: SphericalSamplingParams = SphericalSamplingParams( name="mouth", @@ -167,6 +169,8 @@ class SceneGenerationParams: outer_radius=1.04, # Corresponds to the 8-DOF workspace theta_range=(0.0, 2 * math.pi), phi_range=(0.0, math.pi / 2), + min_height=0.3, + max_height=0.5, ) resting_sampling: SphericalSamplingParams = SphericalSamplingParams( name="resting", @@ -174,6 +178,8 @@ class SceneGenerationParams: outer_radius=0.9, # Corresponds to the 6-DOF workspace theta_range=(0.0, 2 * math.pi), phi_range=(0, math.pi / 2), + min_height=0.2, + max_height=0.4, ) above_plate_radial_dist: float = 0.3 above_plate_polar_angle_rad_max: float = math.pi / 3 From fe8adc4e6cd711710bb8c413d5eefdf76e5deb99 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 13:47:42 -0700 Subject: [PATCH 234/238] Set up the initial default state of the plot correctly from the start, rather than trying to trigger a control action in the code --- ada_feeding/scripts/analyze_benchmark.py | 128 ++++++++++++++--------- 1 file changed, 77 insertions(+), 51 deletions(-) diff --git a/ada_feeding/scripts/analyze_benchmark.py b/ada_feeding/scripts/analyze_benchmark.py index 18bb367a..15796cd0 100644 --- a/ada_feeding/scripts/analyze_benchmark.py +++ b/ada_feeding/scripts/analyze_benchmark.py @@ -42,9 +42,9 @@ def generate_visualizations(df_trials: pd.DataFrame, file_path: str): df_stage_status = df_stages.pivot( index="trial_id", columns="stage_name", values="status" ) - df_stage_status.columns = [ - f"status_{col}" for col in df_stage_status.columns - ] # Rename columns + # Fill NaN for trials that didn't reach certain stages + df_stage_status = df_stage_status.fillna(0) + df_stage_status.columns = [f"status_{col}" for col in df_stage_status.columns] # --- 2. Prepare Data for 3D Workspace Plot --- plot_data = [] @@ -75,15 +75,14 @@ def generate_visualizations(df_trials: pd.DataFrame, file_path: str): return vis_df = pd.DataFrame(plot_data) - # --- 3. Create Figure and Traces --- - fig = go.Figure() # Use go.Figure for more control - - # Define all possible status columns and their relevant poses for filtering - status_cols = {"End-to-End": "end_to_end_success"} - for col in df_stage_status.columns: - stage_name = col.replace("status_", "") - status_cols[f"Stage: {stage_name}"] = col + # --- 3. Create Figure and Six Persistent Traces --- + fig = go.Figure() + symbols = {"food_pose": "circle", "mouth_pose": "square", "resting_pose": "diamond"} + colors = {"Success": "green", "Failure": "red"} + # Define the default view + default_status_name = "End-to-End" + default_status_col = "end_to_end_success" relevance_map = { "End-to-End": ["food_pose", "mouth_pose", "resting_pose"], "Stage: HomeToAbovePlate": ["food_pose"], @@ -93,55 +92,82 @@ def generate_visualizations(df_trials: pd.DataFrame, file_path: str): "Stage: Resting": ["food_pose", "resting_pose"], } - symbols = {"food_pose": "circle", "mouth_pose": "square", "resting_pose": "diamond"} + # Create the 6 persistent traces, populating them with data for the default view + for pose_name, symbol in symbols.items(): + for outcome, color in colors.items(): + success_val = 1 if outcome == "Success" else 0 + + # Filter data for this trace's default view + df_subset = vis_df[ + (vis_df["pose_name"] == pose_name) + & (vis_df[default_status_col] == success_val) + ] + + x_data, y_data, z_data, custom_data, hover_template = ( + [], + [], + [], + None, + "none", + ) + if pose_name in relevance_map.get(default_status_name, []): + x_data = df_subset["x"] + y_data = df_subset["y"] + z_data = df_subset["z"] + custom_data = df_subset.to_dict("records") + hover_template = ( + "Trial ID: %{customdata.trial_id}
Pose: %{customdata.pose_name}
Status: " + + outcome + + "
Distance: %{customdata.food_mouth_distance_m:.2f}m" + ) + + fig.add_trace( + go.Scatter3d( + x=x_data, + y=y_data, + z=z_data, + customdata=custom_data, + hovertemplate=hover_template, + mode="markers", + marker=dict(color=color, symbol=symbol, size=5, opacity=0.7), + name=f"{outcome} ({pose_name.replace('_pose', '')})", + ) + ) + + # --- 4. Create the Dropdown Menu with 'restyle' Logic --- + status_cols = {"End-to-End": "end_to_end_success"} + stage_names = sorted([c.replace("status_", "") for c in df_stage_status.columns]) + for stage_name in stage_names: + status_cols[f"Stage: {stage_name}"] = f"status_{stage_name}" + buttons = [] # Create all traces upfront for status_name, status_col in status_cols.items(): - for pose_name, symbol in symbols.items(): - for success_val, color in zip([1, 0], ["green", "red"]): - is_success = success_val == 1 + update_args = {"x": [], "y": [], "z": [], "customdata": [], "hovertemplate": []} + + for pose_name in symbols.keys(): + for outcome in ["Success", "Failure"]: + success_val = 1 if outcome == "Success" else 0 df_subset = vis_df[ (vis_df["pose_name"] == pose_name) & (vis_df[status_col] == success_val) ] - if not df_subset.empty: - fig.add_trace( - go.Scatter3d( - x=df_subset["x"], - y=df_subset["y"], - z=df_subset["z"], - mode="markers", - marker=dict( - color=color, symbol=symbol, size=5, opacity=0.7 - ), - # Make only the default view (End-to-End) visible initially - visible=(status_name == "End-to-End"), - name=f"{'Success' if is_success else 'Failure'} ({pose_name.replace('_pose', '')})", - customdata=df_subset.to_dict("records"), - hovertemplate="Trial ID: %{customdata.trial_id}
Pose: %{customdata.pose_name}
Status: " - + ("Success" if is_success else "Failure") - + "
Distance: %{customdata.food_mouth_distance_m:.2f}m", - ) + + if pose_name in relevance_map.get(status_name, []): + update_args["x"].append(df_subset["x"]) + update_args["y"].append(df_subset["y"]) + update_args["z"].append(df_subset["z"]) + update_args["customdata"].append(df_subset.to_dict("records")) + update_args["hovertemplate"].append( + "Trial ID: %{customdata.trial_id}
Pose: %{customdata.pose_name}
Status: " + + outcome + + "
Distance: %{customdata.food_mouth_distance_m:.2f}m" ) + else: + for key in ["x", "y", "z", "customdata", "hovertemplate"]: + update_args[key].append(None) # Use None to clear data - # --- 4. Create the Dropdown Menu with Filtering Logic --- - buttons = [] - for status_name_key in status_cols.keys(): - visibility = [] - # This nested loop must match the trace creation order exactly - for status_name_trace in status_cols.keys(): - for pose_name_trace in symbols.keys(): - for _ in [1, 0]: # Success and Failure traces - is_visible = False - if status_name_key == status_name_trace: - relevant_poses = relevance_map.get(status_name_key, []) - if pose_name_trace in relevant_poses: - is_visible = True - visibility.append(is_visible) - - buttons.append( - dict(label=status_name_key, method="update", args=[{"visible": visibility}]) - ) + buttons.append(dict(label=status_name, method="restyle", args=[update_args])) fig.update_layout( title="3D Workspace Visualization", From 7dd71f468746ef89a59a4e1644393a67b492d9cd Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 15:52:19 -0700 Subject: [PATCH 235/238] Remove unused function --- .../scripts/end_to_end_feeding_benchmark.py | 47 ------------------- 1 file changed, 47 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 05b4fa55..11c217f9 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1997,53 +1997,6 @@ def _plan_to_resting( planning_time, ) - def _plan_s2_guided( - self, goal_pose: Pose, start_state: Any - ) -> Tuple[TrialStatus, Optional[JointTrajectory], float]: - LOGGER.info(" Planning with S2 (6-DOF Guided)...") - - future = None - # --- Lock the entire MoveIt2 configuration and planning block --- - with self.moveit2_lock: - self.moveit2_jaco.clear_goal_constraints() - self.moveit2_jaco.clear_path_constraints() - - self.moveit2_jaco.set_pose_goal(goal_pose, END_EFFECTOR_LINK_JACO) - self.moveit2_jaco.set_path_orientation_constraint( - quat_xyzw=Quaternion( - x=PATH_CONSTRAINT_QUAT_XYZW[0], - y=PATH_CONSTRAINT_QUAT_XYZW[1], - z=PATH_CONSTRAINT_QUAT_XYZW[2], - w=PATH_CONSTRAINT_QUAT_XYZW[3], - ), - target_link=END_EFFECTOR_LINK_JACO, - tolerance=PATH_CONSTRAINT_TOLERANCE_XYZ_RAD, - weight=1.0, - ) - - future = self.moveit2_jaco.plan_async( - start_joint_state=start_state, - ) - - # Wait for future outside the lock - start_time = time.time() - while rclpy.ok() and not future.done(): - if time.time() - start_time > self.planning_timeout: - future.cancel() - return TrialStatus.PLANNER_FAILURE, None, 0.0 - time.sleep(0.1) - - traj = self.moveit2_jaco.get_trajectory(future) - - if not traj or not traj.points: - return TrialStatus.PLANNER_FAILURE, None, 0.0 - - feasibility_percent = self._verify_trajectory(traj) - if feasibility_percent < 99.0: - return TrialStatus.VERIFICATION_FAILURE, traj, feasibility_percent - - return TrialStatus.SUCCESS, traj, feasibility_percent - # --- Main Benchmark Loop --- def run(self): """Main benchmark execution loop with granular metric collection.""" From a2cfcd5f7af48c4de88d7e7b49ddffc37d82db9d Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 16:44:21 -0700 Subject: [PATCH 236/238] Add stage to re-orient wrist to a kinematically advantageous orientation before motion to Resting stage --- .../scripts/end_to_end_feeding_benchmark.py | 117 +++++++++++++++++- 1 file changed, 114 insertions(+), 3 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 11c217f9..8c21d815 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1891,6 +1891,86 @@ def _plan_to_in_food( planning_time, ) + def _plan_to_reorient_wrist( + self, start_state_jaco: List[float] + ) -> Tuple[ + TrialStatus, Optional[JointTrajectory], Optional[JointTrajectory], float, float + ]: + """ + Plans a motion to reorient the Jaco EE to a neutral "level" pose (Y-axis up) while maintaining its current position. + This acts as a preparatory step before the long-distance transport. + """ + LOGGER.info(" Planning to Reorient Wrist to a level posture...") + + # 1. Get the current EE pose via FK + start_joint_state = JointState() + start_joint_state.name = JOINT_NAMES_JACO + start_joint_state.position = start_state_jaco + fk_poses = self.motion_planner.compute_fk( + group_name=PLANNING_GROUP_JACO, + joint_state=start_joint_state, + fk_link_names=[END_EFFECTOR_LINK_JACO], + ) + if not fk_poses: + LOGGER.warning(" FK failed, cannot determine current wrist pose.") + return TrialStatus.IK_FAILURE, None, 0.0 + + current_ee_pose = fk_poses[0].pose + current_position = current_ee_pose.position + + # 2. Construct a goal constraint that keeps the Jaco EE roughly in the same position, but level against gravity + goal_constraints = [ + create_position_constraint(current_position, 0.1), + create_orientation_path_constraint( + PATH_CONSTRAINT_QUAT_XYZW, (0.1, 2 * math.pi, 0.1) + ), + ] + + # 3. Plan the 6-DOF trajectory for the Jaco arm + status, traj_jaco, planning_time = self.motion_planner.plan( + group_name=PLANNING_GROUP_JACO, + start_state=start_state_jaco, + goal_constraints=goal_constraints, + cartesian=True, + ) + if status != TrialStatus.SUCCESS: + return TrialStatus.PLANNER_FAILURE, None, None, 0.0, planning_time + + # 4. VERIFY the Jaco trajectory for leveling feasibility + feasibility_percent = self._verify_trajectory(traj_jaco) + if feasibility_percent < 99.0: + LOGGER.warning( + f" Path to Resting failed verification ({feasibility_percent:.1f}% feasible)." + ) + return ( + TrialStatus.VERIFICATION_FAILURE, + traj_jaco, + None, + feasibility_percent, + planning_time, + ) + + # 5. Generate the corresponding synchronous Articutool trajectory + LOGGER.info(" Generating synchronous Articutool trajectory...") + traj_atool = self._generate_leveling_atool_trajectory(traj_jaco) + + if traj_atool is None: + LOGGER.error(" Failed to generate synchronous Articutool trajectory.") + return ( + TrialStatus.IK_FAILURE, + traj_jaco, + None, + feasibility_percent, + planning_time, + ) + return ( + TrialStatus.SUCCESS, + traj_jaco, + traj_atool, + feasibility_percent, + planning_time, + ) + def _plan_to_level_articutool( self, jaco_wrist_pose: Pose, @@ -2173,9 +2253,40 @@ def run(self): else: current_atool_state = list(traj_atool.points[-1].positions) - # --- Stage 5: LevelArticutool -> Resting --- + # --- Stage 5: LevelArticutool -> Reorient Wrist --- if not trial_failed: - LOGGER.info("Stage 5: Resting") + LOGGER.info("Stage 5: Reorient Wrist") + (status, traj_jaco, traj_atool, leveling_feasibility, planning_time) = ( + self._plan_to_reorient_wrist(current_jaco_state) + ) + path_length = self._calculate_cartesian_path_length( + traj_jaco, PLANNING_GROUP_JACO + ) + trial_data["stages"].append( + { + "stage_name": "ReorientWrist", + "target_frame": END_EFFECTOR_LINK_JACO, + "status": status.value, + "execution_mode": ExecutionMode.SYNCHRONOUS.value, + "planning_time_sec": planning_time, + "trajectory_path_length_m": path_length, + "custom_metrics": { + "leveling_feasibility_percent": leveling_feasibility + }, + "traj_jaco": self._serialize_trajectory(traj_jaco), + "traj_atool": self._serialize_trajectory(traj_atool), + } + ) + if status != TrialStatus.SUCCESS: + LOGGER.error(f" Stage 5 failed. Skipping trial.") + trial_failed = True + else: + current_jaco_state = list(traj_jaco.points[-1].positions) + current_atool_state = list(traj_atool.points[-1].positions) + + # --- Stage 6: LevelArticutool -> Resting --- + if not trial_failed: + LOGGER.info("Stage 6: Resting") ( status, traj_jaco, @@ -2202,7 +2313,7 @@ def run(self): } ) if status != TrialStatus.SUCCESS: - LOGGER.error(f" Stage 5 failed. Skipping trial.") + LOGGER.error(f" Stage 6 failed. Skipping trial.") trial_failed = True else: current_jaco_state = list(traj_jaco.points[-1].positions) From 6f0f01ff301e69ab73c879f383e2e5df7c1a0956 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 17:05:19 -0700 Subject: [PATCH 237/238] Add retry logic for HomeToAbovePlate motion to improve robustness to probabilistic planning failures --- .../scripts/end_to_end_feeding_benchmark.py | 20 +++++++++++++++---- 1 file changed, 16 insertions(+), 4 deletions(-) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index 8c21d815..e5ca6590 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -2127,9 +2127,21 @@ def run(self): # --- Stage 1: Home -> AbovePlate --- if not trial_failed: LOGGER.info("Stage 1: Home -> AbovePlate") - status, traj_jaco, planning_time = self._plan_to_above_plate( - scene["above_plate_pose"], current_jaco_state - ) + max_attempts = 3 + total_planning_time = 0.0 + status, traj_jaco = TrialStatus.PLANNER_FAILURE, None + for attempt in range(max_attempts): + LOGGER.info(f" Attempt {attempt + 1}/{max_attempts}...") + status, traj_jaco, planning_time = self._plan_to_above_plate( + scene["above_plate_pose"], current_jaco_state + ) + total_planning_time += planning_time + if status == TrialStatus.SUCCESS: + LOGGER.info(f" Success on attempt {attempt + 1}.") + break + LOGGER.warning( + f" Attempt {attempt + 1} failed with status: {status.value}" + ) path_length = self._calculate_cartesian_path_length( traj_jaco, PLANNING_GROUP_JACO ) @@ -2139,7 +2151,7 @@ def run(self): "target_frame": END_EFFECTOR_LINK_JACO, "status": status.value, "execution_mode": ExecutionMode.JACO_ONLY.value, - "planning_time_sec": planning_time, + "planning_time_sec": total_planning_time, "trajectory_path_length_m": path_length, "custom_metrics": {}, "traj_jaco": self._serialize_trajectory(traj_jaco), From be3e9346c519e31af9fd26bfcb5eb43ce19bc5f2 Mon Sep 17 00:00:00 2001 From: Jose Jaime Date: Fri, 22 Aug 2025 17:13:29 -0700 Subject: [PATCH 238/238] Add method to Pinocchio wrapper to compute the Jacobian for a frame at any configuration --- .../scripts/end_to_end_feeding_benchmark.py | 30 +++++++++++++++++++ 1 file changed, 30 insertions(+) diff --git a/ada_feeding/scripts/end_to_end_feeding_benchmark.py b/ada_feeding/scripts/end_to_end_feeding_benchmark.py index e5ca6590..1bf3d265 100644 --- a/ada_feeding/scripts/end_to_end_feeding_benchmark.py +++ b/ada_feeding/scripts/end_to_end_feeding_benchmark.py @@ -1003,6 +1003,36 @@ def get_frame_transform( LOGGER.error(f"Pinocchio FK failed for frame '{frame_name}': {e}") return None + def get_frame_jacobian( + self, + frame_name: str, + jaco_joints: List[float], + atool_joints: Optional[List[float]] = None, + ) -> Optional[np.ndarray]: + """ + Computes the full Jacobian for a specific frame in the world frame. + + Returns: + A 6xN numpy array representing the Jacobian, or None on failure. + """ + if not self.is_ready(): + return None + + try: + q = self._update_configuration(jaco_joints, atool_joints) + frame_id = self.model.getFrameId(frame_name) + + # Compute the Jacobian for the frame. + # The result is stored in self.data.J + pin.computeFrameJacobian(self.model, self.data, q, frame_id) + + return self.data.J + except Exception as e: + LOGGER.error( + f"Pinocchio Jacobian calculation failed for frame '{frame_name}': {e}" + ) + return None + class EndToEndBenchmark: """Manages the end-to-end benchmark planning process."""