From d5f1122ff3898080712ab42f3fc0781f0fd9bd91 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Mon, 7 Oct 2024 10:12:43 -0400 Subject: [PATCH 01/19] Compute relative motions dynamically and send them to the robot as global positions --- src/stretch/agent/zmq_client.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index 1e139082..a0a08e99 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -29,7 +29,7 @@ from stretch.core.robot import AbstractRobotClient from stretch.motion import PlanResult from stretch.motion.kinematics import HelloStretchIdx, HelloStretchKinematics -from stretch.utils.geometry import angle_difference +from stretch.utils.geometry import angle_difference, xyt_base_to_global from stretch.utils.image import Camera from stretch.utils.memory import lookup_address from stretch.utils.point_cloud import show_point_cloud @@ -468,7 +468,14 @@ def navigate_to( else: _xyt = xyt assert len(_xyt) == 3, "xyt must be a vector of size 3" - next_action = {"xyt": _xyt, "nav_relative": relative, "nav_blocking": blocking} + # If it's relative, compute the relative position right now - this helps handle network issues + if relative: + current_xyt = self.get_base_pose() + _xyt = xyt_base_to_global(_xyt, current_xyt) + + # We never send a relative motion over wireless - this is because we can run into timing issues. + # Instead, we always send the absolute position and let the robot handle the motions itself. + next_action = {"xyt": _xyt, "nav_relative": False, "nav_blocking": blocking} if self._rerun: self._rerun.update_nav_goal(_xyt) self.send_action(next_action, timeout=timeout, verbose=verbose) From 3ba3c180901db4777bd86678a4800b0890131e77 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Mon, 7 Oct 2024 10:24:33 -0400 Subject: [PATCH 02/19] wait for arm --- src/stretch/agent/zmq_client.py | 39 +++++++++++++++++++++++++++++++++ 1 file changed, 39 insertions(+) diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index a0a08e99..3b58316f 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -577,6 +577,7 @@ def move_to_nav_posture(self): self.send_action(next_action) self._wait_for_head(constants.STRETCH_NAVIGATION_Q, resend_action={"posture": "navigation"}) self._wait_for_mode("navigation") + self._wait_for_arm(constants.STRETCH_NAVIGATION_Q) assert self.in_navigation_mode() def move_to_manip_posture(self): @@ -648,6 +649,44 @@ def _wait_for_head( break time.sleep(0.01) + def _wait_for_arm( + self, q: np.ndarray, timeout: float = 10.0, resend_action: Optional[dict] = None + ) -> bool: + """Wait for the arm to move to a particular configuration. + + Args: + q(np.ndarray): The target joint angles + timeout(float): How long to wait for the arm to move + resend_action(dict): The action to resend if the arm is not moving. If none, do not resend. + + Returns: + bool: Whether the arm successfully moved to the target configuration + """ + t0 = timeit.default_timer() + while not self._finish: + joint_positions, joint_velocities, _ = self.get_joint_state() + if joint_positions is None: + continue + + arm_diff = np.abs(joint_positions[HelloStretchIdx.ARM] - q[HelloStretchIdx.ARM]) + lift_diff = np.abs(joint_positions[HelloStretchIdx.LIFT] - q[HelloStretchIdx.LIFT]) + + if arm_diff < self._arm_joint_tolerance and lift_diff < self._lift_joint_tolerance: + return True + + if resend_action is not None: + self.send_socket.send_pyobj(resend_action) + + t1 = timeit.default_timer() + if t1 - t0 > timeout: + logger.error( + f"Timeout waiting for arm to move to arm={q[HelloStretchIdx.ARM]}, lift={q[HelloStretchIdx.LIFT]}" + ) + return False + + # This should never happen + return False + def _wait_for_mode(self, mode, verbose: bool = False, timeout: float = 20.0): t0 = timeit.default_timer() while True: From 7106b7d57c2103af3f3ddeb9bbe13b1284ec8a90 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Mon, 7 Oct 2024 10:25:19 -0400 Subject: [PATCH 03/19] Make sure it properly waits for arm to arrive at configuration before releasing from "posture" commands --- src/stretch/agent/zmq_client.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index 3b58316f..756c536f 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -586,6 +586,7 @@ def move_to_manip_posture(self): time.sleep(0.1) self._wait_for_head(constants.STRETCH_PREGRASP_Q, resend_action={"posture": "manipulation"}) self._wait_for_mode("manipulation") + self._wait_for_arm(constants.STRETCH_PREGRASP_Q) assert self.in_manipulation_mode() def _wait_for_head( From 3c74bbfb22919d89f11033988c9d31eea7ffc99d Mon Sep 17 00:00:00 2001 From: Charlie Kemp Date: Mon, 7 Oct 2024 16:45:44 -0400 Subject: [PATCH 04/19] example of simple nav commands This app provides an example of running simple navigation commands. It is based on the example given at the start of the main Stretch AI documentation in the Quickstart section. Currently, this code does not run as expected. I am adding it to help us test these basic navigation commands. --- src/stretch/app/simple_navigation.py | 64 ++++++++++++++++++++++++++++ src/stretch/app/view_images.py | 2 +- 2 files changed, 65 insertions(+), 1 deletion(-) create mode 100644 src/stretch/app/simple_navigation.py diff --git a/src/stretch/app/simple_navigation.py b/src/stretch/app/simple_navigation.py new file mode 100644 index 00000000..b1d6500f --- /dev/null +++ b/src/stretch/app/simple_navigation.py @@ -0,0 +1,64 @@ +# Copyright (c) Hello Robot, Inc. +# All rights reserved. +# +# This source code is licensed under the license found in the LICENSE file in the root directory +# of this source tree. +# +# Some code may be adapted from other open-source works with their respective licenses. Original +# license information maybe found below, if so. + +from stretch.agent import RobotClient + +print("Initialize the robot client") +print('robot = RobotClient(robot_ip="192.168.86.47")') +robot = RobotClient(robot_ip="192.168.86.47") # Replace with your robot's IP +print("Done") +# On future connection attempts, the IP address can be left blank + +# Turn head towards robot's hand +print("Turn head towards robot's hand") +print("robot.move_to_manip_posture()") +robot.move_to_manip_posture() +print("Done") + +# Move forward 0.1 along robot x axis in maniplation mode, and move arm to 0.5 meter height +print("Move forward 0.1 along robot x axis in maniplation mode, and move arm to 0.5 meter height") +print("robot.arm_to([0.1, 0.5, 0, 0, 0, 0], blocking=True)") +robot.arm_to([0.1, 0.5, 0, 0, 0, 0], blocking=True) +print("Done") + +# Turn head towards robot's base and switch base to navigation mode +# In navigation mode, we can stream velocity commands to the base for smooth motions, and base +# rotations are enabled +print("Turn head towards robot's base and switch base to navigation mode") +print("robot.move_to_nav_posture()") +robot.move_to_nav_posture() +print("Done") + +# Move the robot back to origin +# navigate_to() is only allowed in navigation mode +print("Move the robot back to origin") +print("robot.navigate_to([0, 0, 0])") +robot.navigate_to([0, 0, 0]) +print("Done") + +# Move the robot 0.5m forward +print("Move the robot 0.5m forward") +print("robot.navigate_to([0.5, 0, 0], relative=True, blocking=True)") +robot.navigate_to([0.5, 0, 0], relative=True, blocking=True) +print("Done") + +# Rotate the robot 90 degrees to the left +print("Rotate the robot 90 degrees to the left") +print("robot.navigate_to([0, 0, 3.14159/2], relative=True, blocking=True)") +robot.navigate_to([0, 0, 3.14159 / 2], relative=True, blocking=True) +print("Done") + +# And to the right +print("Rotate the robot 90 degrees to the right") +print("robot.navigate_to([0, 0, -3.14159/2], relative=True, blocking=True)") +robot.navigate_to([0, 0, -3.14159 / 2], relative=True, blocking=True) + +print("Stop the robot") +print("robot.stop()") +robot.stop() diff --git a/src/stretch/app/view_images.py b/src/stretch/app/view_images.py index 3cdd4d87..f0e00c12 100644 --- a/src/stretch/app/view_images.py +++ b/src/stretch/app/view_images.py @@ -74,7 +74,7 @@ def main( aruco_detector = GripperArucoDetector() else: aruco_detector = None - + print("This code is running from a file outside of the Docker image!") print("Starting the robot...") robot.start() robot.move_to_manip_posture() From 2ab35bbb7c3b52d88b77047a9d94db8686c503eb Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Mon, 7 Oct 2024 17:01:47 -0400 Subject: [PATCH 05/19] waiting for observations --- src/stretch/app/view_images.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/stretch/app/view_images.py b/src/stretch/app/view_images.py index 3cdd4d87..78a9f24f 100644 --- a/src/stretch/app/view_images.py +++ b/src/stretch/app/view_images.py @@ -93,8 +93,12 @@ def main( # Get image from robot obs = robot.get_observation() if obs is None: + print("Waiting for observation...") + time.sleep(0.1) continue if obs.rgb is None: + print("Waiting for RGB image...") + time.sleep(0.1) continue # Low res images used for visual servoing and ML servo = robot.get_servo_observation() From 5daf1d36a7784f27c2188371edf5700d95b436e8 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Mon, 7 Oct 2024 17:13:07 -0400 Subject: [PATCH 06/19] updates --- src/stretch/app/simple_navigation.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/stretch/app/simple_navigation.py b/src/stretch/app/simple_navigation.py index b1d6500f..394dc0e7 100644 --- a/src/stretch/app/simple_navigation.py +++ b/src/stretch/app/simple_navigation.py @@ -10,8 +10,8 @@ from stretch.agent import RobotClient print("Initialize the robot client") -print('robot = RobotClient(robot_ip="192.168.86.47")') -robot = RobotClient(robot_ip="192.168.86.47") # Replace with your robot's IP +print("robot = RobotClient()") +robot = RobotClient() print("Done") # On future connection attempts, the IP address can be left blank From 502cb0fcf888cce3ba7c661c3501d296890d52d3 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Mon, 7 Oct 2024 17:58:00 -0400 Subject: [PATCH 07/19] update to catch posegraph missing --- src/stretch/agent/zmq_client.py | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index a9b452a8..b1b9a67b 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -852,7 +852,8 @@ def _update_obs(self, obs): def _update_pose_graph(self, obs): """Update internal pose graph""" with self._obs_lock: - self._pose_graph = obs["pose_graph"] + if "pose_graph" in obs: + self._pose_graph = obs["pose_graph"] def _update_state(self, state: dict) -> None: """Update state internally with lock. This is expected to be much more responsive than using full observations, which should be reserved for higher level control. From 8dc342cc92cb823ece45c0df8f1892c17f02fd4d Mon Sep 17 00:00:00 2001 From: Charlie Kemp Date: Tue, 8 Oct 2024 18:42:18 -0400 Subject: [PATCH 08/19] fixed blocking issue & reduced distance Navigating to the origin now uses a blocking command. Using a non-blocking command resulted in unintuitive behavior. This commit also reduces the distance of the relative translation command from half a meter to a quarter of a meter to reduce the likelihood of colliding with something. --- src/stretch/app/simple_navigation.py | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/stretch/app/simple_navigation.py b/src/stretch/app/simple_navigation.py index 394dc0e7..7f3bc789 100644 --- a/src/stretch/app/simple_navigation.py +++ b/src/stretch/app/simple_navigation.py @@ -39,12 +39,12 @@ # navigate_to() is only allowed in navigation mode print("Move the robot back to origin") print("robot.navigate_to([0, 0, 0])") -robot.navigate_to([0, 0, 0]) +robot.navigate_to([0, 0, 0], blocking=True) print("Done") # Move the robot 0.5m forward -print("Move the robot 0.5m forward") -print("robot.navigate_to([0.5, 0, 0], relative=True, blocking=True)") +print("Move the robot 0.25m forward") +print("robot.navigate_to([0.25, 0, 0], relative=True, blocking=True)") robot.navigate_to([0.5, 0, 0], relative=True, blocking=True) print("Done") From 6c62aa3c474be3616611e058b520395241201b1b Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 12:51:18 -0400 Subject: [PATCH 09/19] Rename navigate_to to move_base_to --- src/stretch/agent/operations/navigate.py | 6 ++--- src/stretch/agent/robot_agent.py | 24 +++++++++---------- src/stretch/agent/zmq_client.py | 6 ++--- src/stretch/app/goto_and_execute_skill.py | 4 ++-- src/stretch/app/grasp_object.py | 2 +- src/stretch/app/keyboard_teleop.py | 2 +- src/stretch/core/robot.py | 2 +- src/stretch/utils/dummy_stretch_client.py | 2 +- src/stretch_ros2_bridge/README.md | 2 +- .../stretch_ros2_bridge/remote/api.py | 4 ++-- .../stretch_ros2_bridge/remote/modules/nav.py | 8 +++---- .../stretch_ros2_bridge/remote/server.py | 2 +- 12 files changed, 32 insertions(+), 32 deletions(-) diff --git a/src/stretch/agent/operations/navigate.py b/src/stretch/agent/operations/navigate.py index f6a8a423..3bba2f2d 100644 --- a/src/stretch/agent/operations/navigate.py +++ b/src/stretch/agent/operations/navigate.py @@ -79,7 +79,7 @@ def run(self): theta = math.atan2( xyz[1] - self.robot.get_base_pose()[1], xyz[0] - self.robot.get_base_pose()[0] ) - self.robot.navigate_to( + self.robot.move_base_to( np.array([start_xyz[0], start_xyz[1], theta + np.pi / 2]), blocking=True, timeout=30.0, @@ -94,10 +94,10 @@ def run(self): # Orient the robot towards the object and use the end effector camera to pick it up xyt = self.plan.trajectory[-1].state - # self.robot.navigate_to(xyt + np.array([0, 0, np.pi / 2]), blocking=True, timeout=30.0) + # self.robot.move_base_to(xyt + np.array([0, 0, np.pi / 2]), blocking=True, timeout=30.0) if self.be_precise: self.warn("Moving again to make sure we're close enough to the goal.") - self.robot.navigate_to(xyt, blocking=True, timeout=30.0) + self.robot.move_base_to(xyt, blocking=True, timeout=30.0) def was_successful(self): """This will be successful if we got within a reasonable distance of the target object.""" diff --git a/src/stretch/agent/robot_agent.py b/src/stretch/agent/robot_agent.py index a6dae0f7..5f03a679 100644 --- a/src/stretch/agent/robot_agent.py +++ b/src/stretch/agent/robot_agent.py @@ -406,7 +406,7 @@ def rotate_in_place( steps += 1 while i < steps: t0 = timeit.default_timer() - self.robot.navigate_to( + self.robot.move_base_to( [x, y, theta + (i * step_size)], relative=False, blocking=True, @@ -989,11 +989,11 @@ def recover_from_invalid_start(self) -> bool: if self.space.is_valid(xyt_goal_backward, verbose=True): logger.warning("Trying to move backwards...") # Compute the position forward or backward from the robot - self.robot.navigate_to(xyt_goal_backward, relative=False) + self.robot.move_base_to(xyt_goal_backward, relative=False) elif self.space.is_valid(xyt_goal_forward, verbose=True): logger.warning("Trying to move forward...") # Compute the position forward or backward from the robot - self.robot.navigate_to(xyt_goal_forward, relative=False) + self.robot.move_base_to(xyt_goal_forward, relative=False) else: logger.warning("Could not recover from invalid start state!") return False @@ -1074,15 +1074,15 @@ def move_to_any_instance( print("ROBOT IS STUCK! Move back!") r = np.random.randint(3) if r == 0: - self.robot.navigate_to([-0.1, 0, 0], relative=True, blocking=True) + self.robot.move_base_to([-0.1, 0, 0], relative=True, blocking=True) elif r == 1: - self.robot.navigate_to([0, 0, np.pi / 4], relative=True, blocking=True) + self.robot.move_base_to([0, 0, np.pi / 4], relative=True, blocking=True) elif r == 2: - self.robot.navigate_to([0, 0, -np.pi / 4], relative=True, blocking=True) + self.robot.move_base_to([0, 0, -np.pi / 4], relative=True, blocking=True) return False time.sleep(1.0) - self.robot.navigate_to([0, 0, np.pi / 2], relative=True) + self.robot.move_base_to([0, 0, np.pi / 2], relative=True) self.robot.move_to_manip_posture() return True @@ -1445,7 +1445,7 @@ def run_exploration( if not start_is_valid: print("Start not valid. back up a bit.") print(f"robot base pose: {self.robot.get_base_pose()}") - self.robot.navigate_to([-0.1, 0, 0], relative=True) + self.robot.move_base_to([-0.1, 0, 0], relative=True) continue # Now actually plan to the frontier @@ -1503,11 +1503,11 @@ def run_exploration( r = np.random.randint(3) if r == 0: - self.robot.navigate_to([-0.1, 0, 0], relative=True, blocking=True) + self.robot.move_base_to([-0.1, 0, 0], relative=True, blocking=True) elif r == 1: - self.robot.navigate_to([0, 0, np.pi / 4], relative=True, blocking=True) + self.robot.move_base_to([0, 0, np.pi / 4], relative=True, blocking=True) elif r == 2: - self.robot.navigate_to([0, 0, -np.pi / 4], relative=True, blocking=True) + self.robot.move_base_to([0, 0, -np.pi / 4], relative=True, blocking=True) # Append latest observations if not self._realtime_updates: @@ -1564,7 +1564,7 @@ def move_closed_loop(self, goal: np.ndarray, max_time: float = 10.0) -> bool: t0 = timeit.default_timer() self.robot.move_to_nav_posture() while True: - self.robot.navigate_to(goal, blocking=False, timeout=30.0) + self.robot.move_base_to(goal, blocking=False, timeout=30.0) if self.robot.last_motion_failed(): return False position = self.robot.get_base_pose() diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index 995c90e0..736b339f 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -474,7 +474,7 @@ def arm_to( return False return True - def navigate_to( + def move_base_to( self, xyt: Union[ContinuousNavigationAction, np.ndarray], relative=False, @@ -889,9 +889,9 @@ def execute_trajectory( len(pt) == 3 or len(pt) == 2 ), "base trajectory needs to be 2-3 dimensions: x, y, and (optionally) theta" # just_xy = len(pt) == 2 - # self.navigate_to(pt, relative, position_only=just_xy, blocking=False) + # self.move_base_to(pt, relative, position_only=just_xy, blocking=False) last_waypoint = i == len(trajectory) - 1 - self.navigate_to( + self.move_base_to( pt, relative, blocking=last_waypoint, diff --git a/src/stretch/app/goto_and_execute_skill.py b/src/stretch/app/goto_and_execute_skill.py index 9b57b54a..2e96a557 100644 --- a/src/stretch/app/goto_and_execute_skill.py +++ b/src/stretch/app/goto_and_execute_skill.py @@ -309,7 +309,7 @@ def demo_main( blocking=True, ) robot.switch_to_navigation_mode() - robot.navigate_to([0.20, 0, 1.1], relative=True) + robot.move_base_to([0.20, 0, 1.1], relative=True) robot.switch_to_manipulation_mode() robot.arm_to( @@ -324,7 +324,7 @@ def demo_main( blocking=True, ) robot.switch_to_navigation_mode() - robot.navigate_to([0, 0, -1.5], relative=True) + robot.move_base_to([0, 0, -1.5], relative=True) print("- Task finished, going home...") res = planner.plan(robot.get_base_pose(), start_location) diff --git a/src/stretch/app/grasp_object.py b/src/stretch/app/grasp_object.py index 3ade650c..414e4e0d 100644 --- a/src/stretch/app/grasp_object.py +++ b/src/stretch/app/grasp_object.py @@ -107,7 +107,7 @@ def main( if reset: robot.move_to_nav_posture() - robot.navigate_to([0.0, 0.0, 0.0], blocking=True, timeout=30.0) + robot.move_base_to([0.0, 0.0, 0.0], blocking=True, timeout=30.0) robot.stop() diff --git a/src/stretch/app/keyboard_teleop.py b/src/stretch/app/keyboard_teleop.py index 786a611e..24838cd6 100644 --- a/src/stretch/app/keyboard_teleop.py +++ b/src/stretch/app/keyboard_teleop.py @@ -46,7 +46,7 @@ def key_pressed(robot: HomeRobotZmqClient, key): goal_xyt[2] = 0.2 elif key == "d": goal_xyt[2] = -0.2 - robot.navigate_to(goal_xyt, relative=True) + robot.move_base_to(goal_xyt, relative=True) def getch(): diff --git a/src/stretch/core/robot.py b/src/stretch/core/robot.py index 0dccd680..ae5894d8 100644 --- a/src/stretch/core/robot.py +++ b/src/stretch/core/robot.py @@ -37,7 +37,7 @@ def __init__(self): self._base_control_mode = ControlMode.IDLE @abstractmethod - def navigate_to( + def move_base_to( self, xyt: Union[Iterable[float], ContinuousNavigationAction], relative=False, diff --git a/src/stretch/utils/dummy_stretch_client.py b/src/stretch/utils/dummy_stretch_client.py index 7111b71b..3dba5f35 100644 --- a/src/stretch/utils/dummy_stretch_client.py +++ b/src/stretch/utils/dummy_stretch_client.py @@ -56,7 +56,7 @@ def __init__( self._robot_model = self self.dof = 3 + 2 + 4 + 2 - def navigate_to(self, xyt, relative=False, blocking=False): + def move_base_to(self, xyt, relative=False, blocking=False): """Move to xyt in global coordinates or relative coordinates.""" raise NotImplementedError() diff --git a/src/stretch_ros2_bridge/README.md b/src/stretch_ros2_bridge/README.md index 7488c585..1b18465a 100644 --- a/src/stretch_ros2_bridge/README.md +++ b/src/stretch_ros2_bridge/README.md @@ -67,7 +67,7 @@ r.manip.goto_joint_positions(state) # Navigation r.switch_to_navigation_mode() -r.nav.navigate_to([0.5, 0, 0]) # For Navigation +r.nav.move_base_to([0.5, 0, 0]) # For Navigation ``` ### Running with ORB_SLAM3 diff --git a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/api.py b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/api.py index ac37a1f4..4355602d 100644 --- a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/api.py +++ b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/api.py @@ -282,7 +282,7 @@ def execute_trajectory(self, *args, **kwargs): """Open-loop trajectory execution wrapper. Executes a multi-step trajectory; this is always blocking since it waits to reach each one in turn.""" return self.nav.execute_trajectory(*args, **kwargs) - def navigate_to( + def move_base_to( self, xyt: Iterable[float], relative: bool = False, @@ -291,7 +291,7 @@ def navigate_to( """ Move to xyt in global coordinates or relative coordinates. Cannot be used in manipulation mode. """ - return self.nav.navigate_to(xyt, relative=relative, blocking=blocking) + return self.nav.move_base_to(xyt, relative=relative, blocking=blocking) def get_observation( self, diff --git a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/modules/nav.py b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/modules/nav.py index 6bdc709d..674df516 100644 --- a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/modules/nav.py +++ b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/modules/nav.py @@ -138,7 +138,7 @@ def execute_trajectory( len(pt) == 3 or len(pt) == 2 ), "base trajectory needs to be 2-3 dimensions: x, y, and (optionally) theta" just_xy = len(pt) == 2 - self.navigate_to(pt, relative, position_only=just_xy, blocking=False) + self.move_base_to(pt, relative, position_only=just_xy, blocking=False) self.wait_for_waypoint( pt, pos_err_threshold=pos_err_threshold, @@ -147,7 +147,7 @@ def execute_trajectory( verbose=verbose, timeout=per_waypoint_timeout, ) - self.navigate_to(pt, blocking=True) + self.move_base_to(pt, blocking=True) @enforce_enabled def set_velocity(self, v, w): @@ -162,7 +162,7 @@ def set_velocity(self, v, w): self._ros_client.velocity_pub.publish(msg) @enforce_enabled - def navigate_to( + def move_base_to( self, xyt: List[float], relative: bool = False, @@ -214,7 +214,7 @@ def navigate_to( @enforce_enabled def home(self): - self.navigate_to([0.0, 0.0, 0.0], blocking=True) + self.move_base_to([0.0, 0.0, 0.0], blocking=True) # Helper methods diff --git a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/server.py b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/server.py index fcdc9aa0..a8b8db83 100755 --- a/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/server.py +++ b/src/stretch_ros2_bridge/stretch_ros2_bridge/remote/server.py @@ -157,7 +157,7 @@ def handle_action(self, action: Dict[str, Any]): self.client.in_navigation_mode(), ) print(f"{action['xyt']} {action['nav_relative']} {action['nav_blocking']}") - self.client.navigate_to( + self.client.move_base_to( action["xyt"], relative=action["nav_relative"], ) From 5cafe2408bca33074491540c7b421b25f33d85a0 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 12:52:40 -0400 Subject: [PATCH 10/19] default to blocking in navigate_to --- src/stretch/agent/zmq_client.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index 736b339f..5425bb54 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -477,8 +477,8 @@ def arm_to( def move_base_to( self, xyt: Union[ContinuousNavigationAction, np.ndarray], - relative=False, - blocking=False, + relative: bool = False, + blocking: bool = True, timeout: float = 10.0, verbose: bool = False, ): From afce3d23491e0beff8feb511c5f7d47598f80606 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 12:53:32 -0400 Subject: [PATCH 11/19] update navigate to in readme --- README.md | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/README.md b/README.md index 803141dc..dd98c575 100644 --- a/README.md +++ b/README.md @@ -54,17 +54,17 @@ robot.arm_to([0.1, 0.5, 0, 0, 0, 0]) robot.move_to_nav_posture() # Move the robot back to origin -# navigate_to() is only allowed in navigation mode -robot.navigate_to([0, 0, 0]) +# move_base_to() is only allowed in navigation mode +robot.move_base_to([0, 0, 0]) # Move the robot 0.5m forward -robot.navigate_to([0.5, 0, 0], relative=True) +robot.move_base_to([0.5, 0, 0], relative=True) # Rotate the robot 90 degrees to the left -robot.navigate_to([0, 0, 3.14159/2], relative=True) +robot.move_base_to([0, 0, 3.14159/2], relative=True) # And to the right -robot.navigate_to([0, 0, -3.14159/2], relative=True) +robot.move_base_to([0, 0, -3.14159/2], relative=True) ``` ## Apps From d69ef97b46c5c648db55854f1ce32abfa43ba631 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 12:57:38 -0400 Subject: [PATCH 12/19] update --- .../app => examples}/simple_navigation.py | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) rename {src/stretch/app => examples}/simple_navigation.py (75%) diff --git a/src/stretch/app/simple_navigation.py b/examples/simple_navigation.py similarity index 75% rename from src/stretch/app/simple_navigation.py rename to examples/simple_navigation.py index 7f3bc789..0ae04364 100644 --- a/src/stretch/app/simple_navigation.py +++ b/examples/simple_navigation.py @@ -36,28 +36,28 @@ print("Done") # Move the robot back to origin -# navigate_to() is only allowed in navigation mode +# move_base_to() is only allowed in navigation mode print("Move the robot back to origin") -print("robot.navigate_to([0, 0, 0])") -robot.navigate_to([0, 0, 0], blocking=True) +print("robot.move_base_to([0, 0, 0])") +robot.move_base_to([0, 0, 0]) print("Done") # Move the robot 0.5m forward print("Move the robot 0.25m forward") -print("robot.navigate_to([0.25, 0, 0], relative=True, blocking=True)") -robot.navigate_to([0.5, 0, 0], relative=True, blocking=True) +print("robot.move_base_to([0.25, 0, 0], relative=True)") +robot.move_base_to([0.5, 0, 0], relative=True) print("Done") # Rotate the robot 90 degrees to the left print("Rotate the robot 90 degrees to the left") -print("robot.navigate_to([0, 0, 3.14159/2], relative=True, blocking=True)") -robot.navigate_to([0, 0, 3.14159 / 2], relative=True, blocking=True) +print("robot.move_base_to([0, 0, 3.14159/2], relative=True)") +robot.move_base_to([0, 0, 3.14159 / 2], relative=True) print("Done") # And to the right print("Rotate the robot 90 degrees to the right") -print("robot.navigate_to([0, 0, -3.14159/2], relative=True, blocking=True)") -robot.navigate_to([0, 0, -3.14159 / 2], relative=True, blocking=True) +print("robot.move_base_to([0, 0, -3.14159/2], relative=True, blocking=True)") +robot.move_base_to([0, 0, -3.14159 / 2], relative=True) print("Stop the robot") print("robot.stop()") From 55a9c2f6023b2a341c537eff80b4ad7cd9273093 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 12:58:30 -0400 Subject: [PATCH 13/19] link to example + run --- README.md | 2 ++ src/stretch/agent/zmq_client.py | 2 +- 2 files changed, 3 insertions(+), 1 deletion(-) diff --git a/README.md b/README.md index dd98c575..830411f0 100644 --- a/README.md +++ b/README.md @@ -67,6 +67,8 @@ robot.move_base_to([0, 0, 3.14159/2], relative=True) robot.move_base_to([0, 0, -3.14159/2], relative=True) ``` +You can find this code in [examples/simple_navigation.py](examples/simple_navigation.py). + ## Apps After [installation](#installation), on the robot, run the server: diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index 82aecee3..b489428e 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -347,7 +347,7 @@ def arm_to( joint_angles: Optional[np.ndarray] = None, gripper: float = None, head: Optional[np.ndarray] = None, - blocking: bool = False, + blocking: bool = True, timeout: float = 10.0, verbose: bool = False, min_time: float = 2.0, From c27b6d5ca5a7bdb4827fb155e80d15eee5ec985a Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 13:40:00 -0400 Subject: [PATCH 14/19] closing things and making it work better --- src/stretch/agent/robot_agent.py | 15 ++++++++++++--- src/stretch/agent/zmq_client.py | 5 +++++ 2 files changed, 17 insertions(+), 3 deletions(-) diff --git a/src/stretch/agent/robot_agent.py b/src/stretch/agent/robot_agent.py index 5f03a679..9aba573a 100644 --- a/src/stretch/agent/robot_agent.py +++ b/src/stretch/agent/robot_agent.py @@ -135,6 +135,9 @@ def __init__( self.reset_object_plans() + # Is this still running? + self._running = True + # Store the current scene graph computed from detected objects self.scene_graph = None @@ -164,8 +167,13 @@ def __init__( self.sub_socket = self.context.socket(zmq.SUB) self.sub_socket.connect(f"tcp://localhost:{obs_sub_port}") self.sub_socket.setsockopt_string(zmq.SUBSCRIBE, "") + else: + self._update_map_thread = None + self._get_observations_thread = None def __del__(self): + if self._update_map_thread is not None and self._update_map_thread.is_alive(): + self._update_map_thread.join() self._update_map_thread.join() def _create_voxel_map(self, parameters: Parameters) -> SparseVoxelMap: @@ -459,7 +467,8 @@ def show_map(self): ) def get_observations_loop(self): - while True: + """Threaded function that gets observations in real-time.""" + while self.robot.running and self._running: obs = None t0 = timeit.default_timer() @@ -615,8 +624,8 @@ def update_map_with_pose_graph(self): self._obs_history_lock.release() def update_map_loop(self): - """Threaded function that updates our voxel map in real-time""" - while True: + """Threaded function that updates our voxel map in real-time.""" + while self.robot.running and self._running: with self._robot_lock: self.update_map_with_pose_graph() time.sleep(0.5) diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index b489428e..19cff5f5 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -175,6 +175,8 @@ def __init__( recv_servo_port, robot_ip, use_remote_computer, message_type="visual servoing data" ) + # Create this pub obs socket to send observations locally + # TODO: do we actually need this? self.pub_obs_socket = self._create_pub_obs_socket(pub_obs_port) # SEnd actions back to the robot for execution @@ -1301,6 +1303,9 @@ def stop(self): if self._rerun_thread is not None: self._rerun_thread.join() + # Delete pub obs socket + self.pub_obs_socket.close() + # Close the sockets and context self.recv_socket.close() self.recv_state_socket.close() From 6bb82af5dfb4db5408869540ea45524b9e7cacfd Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 17:04:37 -0400 Subject: [PATCH 15/19] remove blockng and set robot ip addr script --- ...simple_navigation.py => simple_motions.py} | 2 +- scripts/set_robot_ip.sh | 29 +++++++++++++++++++ 2 files changed, 30 insertions(+), 1 deletion(-) rename examples/{simple_navigation.py => simple_motions.py} (97%) create mode 100755 scripts/set_robot_ip.sh diff --git a/examples/simple_navigation.py b/examples/simple_motions.py similarity index 97% rename from examples/simple_navigation.py rename to examples/simple_motions.py index 0ae04364..87a028ea 100644 --- a/examples/simple_navigation.py +++ b/examples/simple_motions.py @@ -24,7 +24,7 @@ # Move forward 0.1 along robot x axis in maniplation mode, and move arm to 0.5 meter height print("Move forward 0.1 along robot x axis in maniplation mode, and move arm to 0.5 meter height") print("robot.arm_to([0.1, 0.5, 0, 0, 0, 0], blocking=True)") -robot.arm_to([0.1, 0.5, 0, 0, 0, 0], blocking=True) +robot.arm_to([0.1, 0.5, 0, 0, 0, 0]) print("Done") # Turn head towards robot's base and switch base to navigation mode diff --git a/scripts/set_robot_ip.sh b/scripts/set_robot_ip.sh new file mode 100755 index 00000000..b0c85e96 --- /dev/null +++ b/scripts/set_robot_ip.sh @@ -0,0 +1,29 @@ +#!/bin/bash + +## Check if an IP address is provided +if [ $# -eq 0 ]; then + echo "Error: Please provide an IP address as an argument." + echo "Usage: $0 " + exit 1 +fi + +## Validate IP address format +if ! [[ $1 =~ ^[0-9]+\.[0-9]+\.[0-9]+\.[0-9]+$ ]]; then + echo "Error: Invalid IP address format. Please use xxx.xxx.xxx.xxx" + exit 1 +fi + +## Create directory if it doesn't exist +mkdir -p ~/.stretch + +## Write IP address to file +echo "$1" > ~/.stretch/robot_ip.txt + +## Verify file creation and content +if [ -f ~/.stretch/robot_ip.txt ]; then + echo "Success: IP address $1 has been written to ~/.stretch/robot_ip.txt" +else + echo "Error: Failed to create or write to ~/.stretch/robot_ip.txt" + exit 1 +fi + From f357310a155c359308021bebcf112d24ba46de7b Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 17:17:48 -0400 Subject: [PATCH 16/19] updates --- examples/simple_motions.py | 4 ++-- src/stretch/agent/zmq_client.py | 4 ++++ 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/examples/simple_motions.py b/examples/simple_motions.py index 87a028ea..95297f41 100644 --- a/examples/simple_motions.py +++ b/examples/simple_motions.py @@ -51,13 +51,13 @@ # Rotate the robot 90 degrees to the left print("Rotate the robot 90 degrees to the left") print("robot.move_base_to([0, 0, 3.14159/2], relative=True)") -robot.move_base_to([0, 0, 3.14159 / 2], relative=True) +robot.move_base_to([0, 0, 3.14159 / 2], relative=True, verbose=True) print("Done") # And to the right print("Rotate the robot 90 degrees to the right") print("robot.move_base_to([0, 0, -3.14159/2], relative=True, blocking=True)") -robot.move_base_to([0, 0, -3.14159 / 2], relative=True) +robot.move_base_to([0, 0, -3.14159 / 2], relative=True, verbose=True) print("Stop the robot") print("robot.stop()") diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index 19cff5f5..83af7fd1 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -493,7 +493,11 @@ def move_base_to( # If it's relative, compute the relative position right now - this helps handle network issues if relative: current_xyt = self.get_base_pose() + if verbose: + print("Current pose", current_xyt) _xyt = xyt_base_to_global(_xyt, current_xyt) + if verbose: + print("Goal pose in global coordinates", _xyt) # We never send a relative motion over wireless - this is because we can run into timing issues. # Instead, we always send the absolute position and let the robot handle the motions itself. From f6bf5d3beb14306a197b8c6814e940619eb2db14 Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 18:42:21 -0400 Subject: [PATCH 17/19] some minor changes for readability --- src/stretch/agent/zmq_client.py | 32 +++++++++++++++++--------------- 1 file changed, 17 insertions(+), 15 deletions(-) diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index 83af7fd1..a3a03f4d 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -504,7 +504,21 @@ def move_base_to( next_action = {"xyt": _xyt, "nav_relative": False, "nav_blocking": blocking} if self._rerun: self._rerun.update_nav_goal(_xyt) - self.send_action(next_action, timeout=timeout, verbose=verbose) + action = self.send_action(next_action, timeout=timeout, verbose=verbose) + + # Make sure we had time to read + if blocking: + block_id = action["step"] + time.sleep(0.1) + # Now, wait for the command to finish + self._wait_for_action( + block_id, + goal_angle=_xyt[2], + verbose=verbose, + timeout=timeout, + # resend_action=action, + # resend_action=current_action, + ) def reset(self): """Reset everything in the robot's internal state""" @@ -785,8 +799,8 @@ def _wait_for_action( t0 = timeit.default_timer() continue - moved_dist = np.linalg.norm(pos - last_pos) if last_pos is not None else 0 - angle_dist = angle_difference(ang, last_ang) if last_ang is not None else 0 + moved_dist = np.linalg.norm(pos - last_pos) if last_pos is not None else float("inf") + angle_dist = angle_difference(ang, last_ang) if last_ang is not None else float("inf") if goal_angle is not None: angle_dist_to_goal = angle_difference(ang, goal_angle) at_goal = angle_dist_to_goal < goal_angle_threshold @@ -1058,18 +1072,6 @@ def send_action( # Empty it out for the next one current_action = next_action - # Make sure we had time to read - # time.sleep(0.1) - if blocking: - # Wait for the command to finish - self._wait_for_action( - block_id, - goal_angle=goal_angle, - verbose=verbose, - timeout=timeout, - # resend_action=current_action, - ) - # Returns the current action in case we want to do something with it like resend return current_action From 3d3304253b2549442a007b9493862616493c9e3a Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 18:47:15 -0400 Subject: [PATCH 18/19] increase default "not moving" steps --- src/stretch/config/default_planner.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/stretch/config/default_planner.yaml b/src/stretch/config/default_planner.yaml index 786242ef..f9706cfe 100644 --- a/src/stretch/config/default_planner.yaml +++ b/src/stretch/config/default_planner.yaml @@ -48,7 +48,7 @@ filters: motion: moving_threshold: 0.001 # How much the robot has to move to be considered "moving" angle_threshold: 0.01 # How much the robot has to rotate to be considered "rotating" - min_steps_not_moving: 2 # How many steps the robot has to not move before we consider it "stopped" + min_steps_not_moving: 5 # How many steps the robot has to not move before we consider it "stopped" joint_tolerance: arm: 0.05 base_x: 0.05 From 85ba86216524e03c587982da858b73fc01c003dd Mon Sep 17 00:00:00 2001 From: Chris Paxton Date: Thu, 10 Oct 2024 18:50:07 -0400 Subject: [PATCH 19/19] update --- src/stretch/agent/zmq_client.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/stretch/agent/zmq_client.py b/src/stretch/agent/zmq_client.py index a3a03f4d..7249a46e 100644 --- a/src/stretch/agent/zmq_client.py +++ b/src/stretch/agent/zmq_client.py @@ -750,7 +750,7 @@ def _wait_for_action( timeout: float = 10.0, moving_threshold: Optional[float] = None, angle_threshold: Optional[float] = None, - min_steps_not_moving: Optional[int] = 1, + min_steps_not_moving: Optional[int] = None, goal_angle: Optional[float] = None, goal_angle_threshold: Optional[float] = 0.15, resend_action: Optional[dict] = None,