diff --git a/spot_tools/src/spot_skills/arm_utils.py b/spot_tools/src/spot_skills/arm_utils.py index 141c464..8c9f35c 100644 --- a/spot_tools/src/spot_skills/arm_utils.py +++ b/spot_tools/src/spot_skills/arm_utils.py @@ -127,8 +127,8 @@ 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, - y=-0.5, + x=0.8, + 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 30f473c..a6f4e35 100644 --- a/spot_tools/src/spot_skills/grasp_utils.py +++ b/spot_tools/src/spot_skills/grasp_utils.py @@ -144,7 +144,18 @@ def object_place(spot, semantic_class="bag", position=None): # drop_object(spot) stow_arm(spot) close_gripper(spot) - time.sleep(0.25) + execute_recovery_action( + spot, + 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, + relative_pose=math_helpers.SE2Pose(x=0.0, y=1.0, angle=0), + ) + time.sleep(1) return True diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index e80911e..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, ) @@ -31,68 +27,20 @@ def navigate_to_relative_pose( 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, -) -> None: - """Execute a relative move. - - The pose is dx, dy, dyaw relative to the robot's body. - """ - # Get the robot's current state. +): + # 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) != "" - # 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], - ), + vision_tform_body = get_se2_a_tform_b( + transforms, VISION_FRAME_NAME, BODY_FRAME_NAME ) - mobility_params = spot_command_pb2.MobilityParams(vel_limit=speed_limit) + vision_tform_goal = vision_tform_body * body_tform_goal - 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_AT_GOAL - 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!") + navigate_to_absolute_pose(spot, waypoint=vision_tform_goal) def navigate_to_absolute_pose( diff --git a/spot_tools_ros/examples/test_spot_executor_ros.py b/spot_tools_ros/examples/test_spot_executor_ros.py index 488315a..d6f54c9 100644 --- a/spot_tools_ros/examples/test_spot_executor_ros.py +++ b/spot_tools_ros/examples/test_spot_executor_ros.py @@ -47,13 +47,13 @@ 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( [ [3.8, 0], - [7.8, 0], + [5.8, 0], ] )