From 6afd96973773bfe999af17c3d886b8785d2ba4b4 Mon Sep 17 00:00:00 2001 From: y-veys Date: Tue, 21 Oct 2025 18:27:52 -0400 Subject: [PATCH 01/13] Fix the placing so that it doesn't go to the side --- spot_tools/src/spot_skills/grasp_utils.py | 16 ++++++++--- .../src/spot_skills/navigation_utils.py | 21 +++++++++++++-- .../examples/test_spot_executor_ros.py | 2 +- .../src/spot_tools_ros/spot_executor_ros.py | 27 +++++++++++++++++++ 4 files changed, 60 insertions(+), 6 deletions(-) diff --git a/spot_tools/src/spot_skills/grasp_utils.py b/spot_tools/src/spot_skills/grasp_utils.py index 30f473c..c060499 100644 --- a/spot_tools/src/spot_skills/grasp_utils.py +++ b/spot_tools/src/spot_skills/grasp_utils.py @@ -26,7 +26,7 @@ from bosdyn.client.robot_command import RobotCommandBuilder, block_until_arm_arrives from spot_skills.arm_utils import ( - arm_to_drop, + arm_to_carry, close_gripper, open_gripper, stow_arm, @@ -114,9 +114,9 @@ def object_place(spot, semantic_class="bag", position=None): return True time.sleep(0.25) - # arm_to_carry(spot) + arm_to_carry(spot) # stow_arm(spot) - arm_to_drop(spot) + # arm_to_drop(spot) # odom_T_task = get_root_T_ground_body( # robot_state=spot.get_state(), root_frame_name=ODOM_FRAME_NAME @@ -144,6 +144,16 @@ def object_place(spot, semantic_class="bag", position=None): # drop_object(spot) stow_arm(spot) close_gripper(spot) + execute_recovery_action( + spot, + recover_arm=False, + relative_pose=math_helpers.SE2Pose(x=-1.0, y=0.0, angle=0), + ) + execute_recovery_action( + spot, + recover_arm=False, + relative_pose=math_helpers.SE2Pose(x=0.0, y=1.0, angle=0), + ) time.sleep(0.25) return True diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index e80911e..1dcbdf0 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/spot_tools/src/spot_skills/navigation_utils.py @@ -30,7 +30,7 @@ def navigate_to_relative_pose( body_tform_goal: math_helpers.SE2Pose, max_xytheta_vel: Tuple[float, float, float] = (2.0, 2.0, 1.0), min_xytheta_vel: Tuple[float, float, float] = (-2.0, -2.0, -1.0), - timeout: float = 20.0, + timeout: float = 10.0, ) -> None: """Execute a relative move. @@ -87,7 +87,7 @@ def navigate_to_relative_pose( return traj_feedback = mobility_feedback.se2_trajectory_feedback if ( - traj_feedback.status == traj_feedback.STATUS_AT_GOAL + traj_feedback.status == traj_feedback.STATUS_STOPPED and traj_feedback.body_movement_status == traj_feedback.BODY_STATUS_SETTLED ): return @@ -95,6 +95,23 @@ def navigate_to_relative_pose( spot.robot.logger.info("Timed out waiting for movement to execute!") +def navigate_to_relative_pose_new( + spot, + body_tform_goal: math_helpers.SE2Pose, + max_xytheta_vel: Tuple[float, float, float] = (2.0, 2.0, 1.0), + min_xytheta_vel: Tuple[float, float, float] = (-2.0, -2.0, -1.0), + timeout: float = 20.0, +): + current_pose = spot.get_pose() # Returns a np array + waypoint = math_helpers.SE2Pose( + x=current_pose[0] + body_tform_goal.x, + y=current_pose[1] + body_tform_goal.y, + angle=current_pose[2] + body_tform_goal.angle, + ) + + navigate_to_absolute_pose(spot, waypoint=waypoint) + + def navigate_to_absolute_pose( spot, waypoint, frame_name=VISION_FRAME_NAME, stairs=False ): diff --git a/spot_tools_ros/examples/test_spot_executor_ros.py b/spot_tools_ros/examples/test_spot_executor_ros.py index 488315a..123419e 100644 --- a/spot_tools_ros/examples/test_spot_executor_ros.py +++ b/spot_tools_ros/examples/test_spot_executor_ros.py @@ -47,7 +47,7 @@ def __init__(self): # ) pick_cmd = Pick( - "hamilton/odom", "cone", np.array([5.0, 5, 0]), np.array([7.0, 7, 0]) + "hamilton/odom", "bag", np.array([5.0, 5, 0]), np.array([7.0, 7, 0]) ) path = np.array( diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index 1e63b49..a312af3 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -396,6 +396,33 @@ def tf_lookup_fn(parent, child): timer_period_s, self.hb_callback, callback_group=heartbeat_timer_group ) + self.feedback_collector.print("INFO", self.spot_interface.get_pose()) + matrix = self.SE2_to_matrix(self.spot_interface.get_pose()) + self.feedback_collector.print("INFO", matrix) + self.feedback_collector.print("INFO", self.matrix_to_SE2(matrix)) + + def SE2_to_matrix(self, SE2_pose): + x = SE2_pose[0] + y = SE2_pose[1] + theta = SE2_pose[2] + + A = np.array( + [ + [np.cos(theta), -np.sin(theta), x], + [np.sin(theta), np.cos(theta), y], + [0, 0, 1], + ] + ) + + return A + + def matrix_to_SE2(self, A): + x = A[0, 2] + y = A[1, 2] + + theta = np.arctan2(A[1, 0], A[1, 1]) + return np.array([x, y, theta]) + def hb_callback(self): msg = NodeInfoMsg() msg.nickname = "spot_executor" From 2a2922a6b5669a8feb0a11637165cad287ddd666 Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 00:30:18 -0400 Subject: [PATCH 02/13] Update navigate_to_relative_pose to use navigate_to_absolute_pose --- .../src/spot_skills/navigation_utils.py | 21 +++++++----- .../src/spot_tools_ros/spot_executor_ros.py | 32 ++++--------------- 2 files changed, 20 insertions(+), 33 deletions(-) diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index 1dcbdf0..2fc7d4e 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/spot_tools/src/spot_skills/navigation_utils.py @@ -25,7 +25,7 @@ MAX_ROTATION_VEL = 0.65 -def navigate_to_relative_pose( +def navigate_to_relative_pose_old( spot, body_tform_goal: math_helpers.SE2Pose, max_xytheta_vel: Tuple[float, float, float] = (2.0, 2.0, 1.0), @@ -95,21 +95,26 @@ def navigate_to_relative_pose( spot.robot.logger.info("Timed out waiting for movement to execute!") -def navigate_to_relative_pose_new( +def navigate_to_relative_pose( spot, body_tform_goal: math_helpers.SE2Pose, max_xytheta_vel: Tuple[float, float, float] = (2.0, 2.0, 1.0), min_xytheta_vel: Tuple[float, float, float] = (-2.0, -2.0, -1.0), timeout: float = 20.0, ): - current_pose = spot.get_pose() # Returns a np array - waypoint = math_helpers.SE2Pose( - x=current_pose[0] + body_tform_goal.x, - y=current_pose[1] + body_tform_goal.y, - angle=current_pose[2] + body_tform_goal.angle, + # Get an SE2 pose for vision_tform_body to convert the body-based command to a non-moving frame + # that can be issued to the robot. + robot_state = spot.get_state() + transforms = robot_state.kinematic_state.transforms_snapshot + + assert str(transforms) != "" + + vision_tform_body = get_se2_a_tform_b( + transforms, VISION_FRAME_NAME, BODY_FRAME_NAME ) + vision_tform_goal = vision_tform_body * body_tform_goal - navigate_to_absolute_pose(spot, waypoint=waypoint) + navigate_to_absolute_pose(spot, waypoint=vision_tform_goal) def navigate_to_absolute_pose( diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index a312af3..9c39a59 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -9,6 +9,7 @@ import tf2_ros import tf_transformations import yaml +from bosdyn.client import math_helpers from cv_bridge import CvBridge from nav_msgs.msg import Path from rclpy.callback_groups import MutuallyExclusiveCallbackGroup @@ -396,32 +397,13 @@ def tf_lookup_fn(parent, child): timer_period_s, self.hb_callback, callback_group=heartbeat_timer_group ) - self.feedback_collector.print("INFO", self.spot_interface.get_pose()) - matrix = self.SE2_to_matrix(self.spot_interface.get_pose()) + pose = self.spot_interface.get_pose() + self.feedback_collector.print("INFO", pose) + SE2_pose = math_helpers.SE2Pose(x=pose[0], y=pose[1], angle=pose[2]) + self.feedback_collector.print("INFO", SE2_pose) + matrix = math_helpers.SE2Pose.to_matrix(SE2_pose) self.feedback_collector.print("INFO", matrix) - self.feedback_collector.print("INFO", self.matrix_to_SE2(matrix)) - - def SE2_to_matrix(self, SE2_pose): - x = SE2_pose[0] - y = SE2_pose[1] - theta = SE2_pose[2] - - A = np.array( - [ - [np.cos(theta), -np.sin(theta), x], - [np.sin(theta), np.cos(theta), y], - [0, 0, 1], - ] - ) - - return A - - def matrix_to_SE2(self, A): - x = A[0, 2] - y = A[1, 2] - - theta = np.arctan2(A[1, 0], A[1, 1]) - return np.array([x, y, theta]) + self.feedback_collector.print("INFO", SE2_pose.from_matrix(matrix)) def hb_callback(self): msg = NodeInfoMsg() From bc400c2a505ad3260ace61ccd2bcd75fcccc6632 Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 00:39:10 -0400 Subject: [PATCH 03/13] Prepare for tomorrow's test in the morning --- spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index 9c39a59..fe94632 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -23,6 +23,7 @@ from spot_executor.fake_spot import FakeSpot from spot_executor.spot import Spot from spot_skills.detection_utils import YOLODetector +from spot_skills.navigation_utils import navigate_to_relative_pose from std_msgs.msg import Bool from visualization_msgs.msg import Marker, MarkerArray @@ -405,6 +406,10 @@ def tf_lookup_fn(parent, child): self.feedback_collector.print("INFO", matrix) self.feedback_collector.print("INFO", SE2_pose.from_matrix(matrix)) + navigate_to_relative_pose( + self.spot_interface, math_helpers.SE2Pose(x=1.0, y=0.0, angle=0.0) + ) + def hb_callback(self): msg = NodeInfoMsg() msg.nickname = "spot_executor" From 7e958bae8146ebeb92a8b2d2abf490a93e031bfe Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 09:16:13 -0400 Subject: [PATCH 04/13] Add standing --- spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py | 1 + 1 file changed, 1 insertion(+) diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index fe94632..208b2c6 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -406,6 +406,7 @@ def tf_lookup_fn(parent, child): self.feedback_collector.print("INFO", matrix) self.feedback_collector.print("INFO", SE2_pose.from_matrix(matrix)) + self.spot_interface.stand() navigate_to_relative_pose( self.spot_interface, math_helpers.SE2Pose(x=1.0, y=0.0, angle=0.0) ) From a92d76cdd36d23dcbb43341c121f98b58543e9f2 Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 09:20:57 -0400 Subject: [PATCH 05/13] Remove test --- spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py | 9 ++++----- 1 file changed, 4 insertions(+), 5 deletions(-) diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index 208b2c6..1bcfed4 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -23,7 +23,6 @@ from spot_executor.fake_spot import FakeSpot from spot_executor.spot import Spot from spot_skills.detection_utils import YOLODetector -from spot_skills.navigation_utils import navigate_to_relative_pose from std_msgs.msg import Bool from visualization_msgs.msg import Marker, MarkerArray @@ -406,10 +405,10 @@ def tf_lookup_fn(parent, child): self.feedback_collector.print("INFO", matrix) self.feedback_collector.print("INFO", SE2_pose.from_matrix(matrix)) - self.spot_interface.stand() - navigate_to_relative_pose( - self.spot_interface, math_helpers.SE2Pose(x=1.0, y=0.0, angle=0.0) - ) + # self.spot_interface.stand() + # navigate_to_relative_pose( + # self.spot_interface, math_helpers.SE2Pose(x=1.0, y=0.0, angle=0.0) + # ) def hb_callback(self): msg = NodeInfoMsg() From 0718054641a777ffdcf0373224f48fd5afabae2d Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 09:27:20 -0400 Subject: [PATCH 06/13] Reduce path length for inside test --- spot_tools_ros/examples/test_spot_executor_ros.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_tools_ros/examples/test_spot_executor_ros.py b/spot_tools_ros/examples/test_spot_executor_ros.py index 123419e..d6f54c9 100644 --- a/spot_tools_ros/examples/test_spot_executor_ros.py +++ b/spot_tools_ros/examples/test_spot_executor_ros.py @@ -53,7 +53,7 @@ def __init__(self): path = np.array( [ [3.8, 0], - [7.8, 0], + [5.8, 0], ] ) From e981bfa6475b9eb75772b71af263f3946df2e7db Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 09:30:39 -0400 Subject: [PATCH 07/13] Add sleep between moving backwards --- spot_tools/src/spot_skills/grasp_utils.py | 1 + 1 file changed, 1 insertion(+) diff --git a/spot_tools/src/spot_skills/grasp_utils.py b/spot_tools/src/spot_skills/grasp_utils.py index c060499..186cb06 100644 --- a/spot_tools/src/spot_skills/grasp_utils.py +++ b/spot_tools/src/spot_skills/grasp_utils.py @@ -149,6 +149,7 @@ def object_place(spot, semantic_class="bag", position=None): recover_arm=False, relative_pose=math_helpers.SE2Pose(x=-1.0, y=0.0, angle=0), ) + time.sleep(1) execute_recovery_action( spot, recover_arm=False, From a6456c0332f63d42ad6810d93f05a6323c03763c Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 09:37:37 -0400 Subject: [PATCH 08/13] Move arm to drop so that the object falls out --- spot_tools/src/spot_skills/arm_utils.py | 2 +- spot_tools/src/spot_skills/grasp_utils.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/spot_tools/src/spot_skills/arm_utils.py b/spot_tools/src/spot_skills/arm_utils.py index 141c464..381f9e4 100644 --- a/spot_tools/src/spot_skills/arm_utils.py +++ b/spot_tools/src/spot_skills/arm_utils.py @@ -128,7 +128,7 @@ def arm_to_drop(spot, duration: float = 2.0) -> None: looking_down_and_right_and_rotated_right_pose = math_helpers.SE3Pose( x=0.2, - y=-0.5, + y=0.0, z=0.1, rot=math_helpers.Quat.from_pitch(np.pi / 2) * math_helpers.Quat.from_roll(np.pi / 2), diff --git a/spot_tools/src/spot_skills/grasp_utils.py b/spot_tools/src/spot_skills/grasp_utils.py index 186cb06..4e71091 100644 --- a/spot_tools/src/spot_skills/grasp_utils.py +++ b/spot_tools/src/spot_skills/grasp_utils.py @@ -27,6 +27,7 @@ from spot_skills.arm_utils import ( arm_to_carry, + arm_to_drop, close_gripper, open_gripper, stow_arm, @@ -116,7 +117,7 @@ def object_place(spot, semantic_class="bag", position=None): time.sleep(0.25) arm_to_carry(spot) # stow_arm(spot) - # arm_to_drop(spot) + arm_to_drop(spot) # odom_T_task = get_root_T_ground_body( # robot_state=spot.get_state(), root_frame_name=ODOM_FRAME_NAME From d789a4e1d2bb56085982db31ac2f7ac337c6a14e Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 09:43:24 -0400 Subject: [PATCH 09/13] Move arm to drop so that the object falls out with a sane x-dir --- spot_tools/src/spot_skills/arm_utils.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_tools/src/spot_skills/arm_utils.py b/spot_tools/src/spot_skills/arm_utils.py index 381f9e4..6f33ef1 100644 --- a/spot_tools/src/spot_skills/arm_utils.py +++ b/spot_tools/src/spot_skills/arm_utils.py @@ -127,7 +127,7 @@ def arm_to_drop(spot, duration: float = 2.0) -> None: """Stow the spot arm.""" looking_down_and_right_and_rotated_right_pose = math_helpers.SE3Pose( - x=0.2, + x=0.6, y=0.0, z=0.1, rot=math_helpers.Quat.from_pitch(np.pi / 2) From b9bccd72bb20b0001938e7896875ac5a6762995a Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 09:46:08 -0400 Subject: [PATCH 10/13] Move arm to drop so that the object falls out with a sane x-dir --- spot_tools/src/spot_skills/arm_utils.py | 2 +- spot_tools/src/spot_skills/grasp_utils.py | 3 +-- 2 files changed, 2 insertions(+), 3 deletions(-) diff --git a/spot_tools/src/spot_skills/arm_utils.py b/spot_tools/src/spot_skills/arm_utils.py index 6f33ef1..8c9f35c 100644 --- a/spot_tools/src/spot_skills/arm_utils.py +++ b/spot_tools/src/spot_skills/arm_utils.py @@ -127,7 +127,7 @@ def arm_to_drop(spot, duration: float = 2.0) -> None: """Stow the spot arm.""" looking_down_and_right_and_rotated_right_pose = math_helpers.SE3Pose( - x=0.6, + x=0.8, y=0.0, z=0.1, rot=math_helpers.Quat.from_pitch(np.pi / 2) diff --git a/spot_tools/src/spot_skills/grasp_utils.py b/spot_tools/src/spot_skills/grasp_utils.py index 4e71091..023a665 100644 --- a/spot_tools/src/spot_skills/grasp_utils.py +++ b/spot_tools/src/spot_skills/grasp_utils.py @@ -26,7 +26,6 @@ from bosdyn.client.robot_command import RobotCommandBuilder, block_until_arm_arrives from spot_skills.arm_utils import ( - arm_to_carry, arm_to_drop, close_gripper, open_gripper, @@ -115,7 +114,7 @@ def object_place(spot, semantic_class="bag", position=None): return True time.sleep(0.25) - arm_to_carry(spot) + # arm_to_carry(spot) # stow_arm(spot) arm_to_drop(spot) From e1ed462661988ee67f5f338eb657ab00b91fd777 Mon Sep 17 00:00:00 2001 From: y-veys Date: Wed, 22 Oct 2025 12:14:50 -0400 Subject: [PATCH 11/13] Remove test in spot executor constructor --- spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index 1bcfed4..740e867 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -405,9 +405,9 @@ def tf_lookup_fn(parent, child): self.feedback_collector.print("INFO", matrix) self.feedback_collector.print("INFO", SE2_pose.from_matrix(matrix)) - # self.spot_interface.stand() + self.spot_interface.stand() # navigate_to_relative_pose( - # self.spot_interface, math_helpers.SE2Pose(x=1.0, y=0.0, angle=0.0) + # self.spot_interface, math_helpers.SE2Pose(x=0.0, y=-2.0, angle=np.pi) # ) def hb_callback(self): From 5764f60fa90feee339b829a7bd578575e8cda704 Mon Sep 17 00:00:00 2001 From: Robot Account Date: Wed, 22 Oct 2025 16:32:38 -0400 Subject: [PATCH 12/13] Remove stand from spot executor constructor and add sleep after the robot moves leftwards post place --- spot_tools/src/spot_skills/grasp_utils.py | 2 +- spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_tools/src/spot_skills/grasp_utils.py b/spot_tools/src/spot_skills/grasp_utils.py index 023a665..a6f4e35 100644 --- a/spot_tools/src/spot_skills/grasp_utils.py +++ b/spot_tools/src/spot_skills/grasp_utils.py @@ -155,7 +155,7 @@ def object_place(spot, semantic_class="bag", position=None): recover_arm=False, relative_pose=math_helpers.SE2Pose(x=0.0, y=1.0, angle=0), ) - time.sleep(0.25) + time.sleep(1) return True diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index 740e867..5bff9eb 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -405,7 +405,7 @@ def tf_lookup_fn(parent, child): self.feedback_collector.print("INFO", matrix) self.feedback_collector.print("INFO", SE2_pose.from_matrix(matrix)) - self.spot_interface.stand() + # self.spot_interface.stand() # navigate_to_relative_pose( # self.spot_interface, math_helpers.SE2Pose(x=0.0, y=-2.0, angle=np.pi) # ) From ec4f8557be87e1ceefc792858051b0147bd67248 Mon Sep 17 00:00:00 2001 From: y-veys Date: Thu, 23 Oct 2025 08:42:05 -0400 Subject: [PATCH 13/13] Cleanup --- .../src/spot_skills/navigation_utils.py | 74 ------------------- .../src/spot_tools_ros/spot_executor_ros.py | 14 ---- 2 files changed, 88 deletions(-) diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index 2fc7d4e..3d93938 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/spot_tools/src/spot_skills/navigation_utils.py @@ -6,14 +6,10 @@ import numpy as np import shapely from bosdyn.api import geometry_pb2 -from bosdyn.api.basic_command_pb2 import RobotCommandFeedbackStatus -from bosdyn.api.geometry_pb2 import SE2Velocity, SE2VelocityLimit, Vec2 from bosdyn.api.spot import robot_command_pb2 -from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2 from bosdyn.client import math_helpers from bosdyn.client.frame_helpers import ( BODY_FRAME_NAME, - ODOM_FRAME_NAME, VISION_FRAME_NAME, get_se2_a_tform_b, ) @@ -25,76 +21,6 @@ MAX_ROTATION_VEL = 0.65 -def navigate_to_relative_pose_old( - spot, - body_tform_goal: math_helpers.SE2Pose, - max_xytheta_vel: Tuple[float, float, float] = (2.0, 2.0, 1.0), - min_xytheta_vel: Tuple[float, float, float] = (-2.0, -2.0, -1.0), - timeout: float = 10.0, -) -> None: - """Execute a relative move. - - The pose is dx, dy, dyaw relative to the robot's body. - """ - # Get the robot's current state. - robot_state = spot.get_state() - transforms = robot_state.kinematic_state.transforms_snapshot - - assert str(transforms) != "" - - # We do not want to command this goal in body frame because the body will - # move, thus shifting our goal. Instead, we transform this offset to get - # the goal position in the output frame (odometry). - out_tform_body = get_se2_a_tform_b(transforms, ODOM_FRAME_NAME, BODY_FRAME_NAME) - out_tform_goal = out_tform_body * body_tform_goal - - # Command the robot to go to the goal point in the specified - # frame. The command will stop at the new position. - # Constrain the robot not to turn, forcing it to strafe laterally. - speed_limit = SE2VelocityLimit( - max_vel=SE2Velocity( - linear=Vec2(x=max_xytheta_vel[0], y=max_xytheta_vel[1]), - angular=max_xytheta_vel[2], - ), - min_vel=SE2Velocity( - linear=Vec2(x=min_xytheta_vel[0], y=min_xytheta_vel[1]), - angular=min_xytheta_vel[2], - ), - ) - mobility_params = spot_command_pb2.MobilityParams(vel_limit=speed_limit) - - robot_command_client = spot.robot.ensure_client( - RobotCommandClient.default_service_name - ) - robot_cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command( - goal_x=out_tform_goal.x, - goal_y=out_tform_goal.y, - goal_heading=out_tform_goal.angle, - frame_name=ODOM_FRAME_NAME, - params=mobility_params, - ) - cmd_id = robot_command_client.robot_command( - lease=None, command=robot_cmd, end_time_secs=time.time() + timeout - ) - start_time = time.perf_counter() - while (time.perf_counter() - start_time) <= timeout: - feedback = robot_command_client.robot_command_feedback(cmd_id) - mobility_feedback = ( - feedback.feedback.synchronized_feedback.mobility_command_feedback - ) - if mobility_feedback.status != RobotCommandFeedbackStatus.STATUS_PROCESSING: # pylint: disable=no-member,line-too-long - spot.robot.logger.info("Failed to reach the goal") - return - traj_feedback = mobility_feedback.se2_trajectory_feedback - if ( - traj_feedback.status == traj_feedback.STATUS_STOPPED - and traj_feedback.body_movement_status == traj_feedback.BODY_STATUS_SETTLED - ): - return - if (time.perf_counter() - start_time) > timeout: - spot.robot.logger.info("Timed out waiting for movement to execute!") - - def navigate_to_relative_pose( spot, body_tform_goal: math_helpers.SE2Pose, diff --git a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py index 5bff9eb..1e63b49 100755 --- a/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py +++ b/spot_tools_ros/src/spot_tools_ros/spot_executor_ros.py @@ -9,7 +9,6 @@ import tf2_ros import tf_transformations import yaml -from bosdyn.client import math_helpers from cv_bridge import CvBridge from nav_msgs.msg import Path from rclpy.callback_groups import MutuallyExclusiveCallbackGroup @@ -397,19 +396,6 @@ def tf_lookup_fn(parent, child): timer_period_s, self.hb_callback, callback_group=heartbeat_timer_group ) - pose = self.spot_interface.get_pose() - self.feedback_collector.print("INFO", pose) - SE2_pose = math_helpers.SE2Pose(x=pose[0], y=pose[1], angle=pose[2]) - self.feedback_collector.print("INFO", SE2_pose) - matrix = math_helpers.SE2Pose.to_matrix(SE2_pose) - self.feedback_collector.print("INFO", matrix) - self.feedback_collector.print("INFO", SE2_pose.from_matrix(matrix)) - - # self.spot_interface.stand() - # navigate_to_relative_pose( - # self.spot_interface, math_helpers.SE2Pose(x=0.0, y=-2.0, angle=np.pi) - # ) - def hb_callback(self): msg = NodeInfoMsg() msg.nickname = "spot_executor"