diff --git a/spot_tools/src/spot_executor/spot_executor.py b/spot_tools/src/spot_executor/spot_executor.py index aa25f4b..b8a1e98 100644 --- a/spot_tools/src/spot_executor/spot_executor.py +++ b/spot_tools/src/spot_executor/spot_executor.py @@ -275,7 +275,7 @@ def execute_pick(self, command, feedback): success = object_grasp( self.spot_interface, self.detector, - image_source="hand_color_image", + image_source="frontleft_fisheye_image", user_input=False, semantic_class=command.object_class, feedback=feedback, diff --git a/spot_tools/src/spot_skills/arm_utils.py b/spot_tools/src/spot_skills/arm_utils.py index 8c9f35c..1c72e0d 100644 --- a/spot_tools/src/spot_skills/arm_utils.py +++ b/spot_tools/src/spot_skills/arm_utils.py @@ -40,7 +40,9 @@ def move_hand_to_relative_pose(spot, body_tform_goal: math_helpers.SE3Pose) -> N 2.0, ) # Send the request. - cmd_id = robot_command_client.robot_command(cmd) + full_cmd = RobotCommandBuilder.build_synchro_command(cmd) + cmd_id = robot_command_client.robot_command(full_cmd) + # Wait until the arm arrives at the goal. block_until_arm_arrives(robot_command_client, cmd_id, 2.0) diff --git a/spot_tools/src/spot_skills/grasp_utils.py b/spot_tools/src/spot_skills/grasp_utils.py index ba26a60..d954d41 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 ( close_gripper, + move_hand_to_relative_pose, open_gripper, stow_arm, ) @@ -113,34 +114,12 @@ def object_place(spot, semantic_class="bag", position=None): return True time.sleep(0.25) - # arm_to_carry(spot) - # stow_arm(spot) - # arm_to_drop(spot) - - # odom_T_task = get_root_T_ground_body( - # robot_state=spot.get_state(), root_frame_name=ODOM_FRAME_NAME - # ) - # wr1_T_tool = math_helpers.SE3Pose( - # 0.23589, 0, -0.03943, math_helpers.Quat.from_pitch(-np.pi / 2) - # ) - # force_dir_rt_task = math_helpers.Vec3(0, 0, -1) # adjust downward force here - # robot_cmd = apply_force_at_current_position( - # force_dir_rt_task_in=force_dir_rt_task, - # force_magnitude=8, - # robot_state=spot.get_state(), - # root_frame_name=ODOM_FRAME_NAME, - # root_T_task=odom_T_task, - # wr1_T_tool_nom=wr1_T_tool, - # ) - - # Execute the impedance command - # cmd_id = spot.command_client.robot_command(robot_cmd) - # spot.robot.logger.info("Impedance command issued") - # block_until_arm_arrives(spot.command_client, cmd_id, 10.0) - # input("Did impedance work") + + carry_cmd = RobotCommandBuilder.arm_carry_command() + spot.command_client.robot_command(carry_cmd) + time.sleep(1) open_gripper(spot) - # drop_object(spot) stow_arm(spot) close_gripper(spot) execute_recovery_action( @@ -167,6 +146,7 @@ def object_grasp( grasp_constraint=None, debug=False, feedback=None, + carry_high=True, ): debug_images = [] @@ -340,6 +320,19 @@ def object_grasp( ) feedback.print("INFO", f"POST-LOOP STATE: {current_state}") + # If the robot was not successful, send it back to the location it started the skill at + if not success: + execute_recovery_action( + spot, + recover_arm=True, + relative_pose=math_helpers.SE2Pose( + x=np.random.uniform(-1, -0.5), y=0.0, angle=0.0 + ), + ) + if debug: + return success, debug_images + return success + close_cmd = RobotCommandBuilder.claw_gripper_close_command( build_on_command=None, max_acc=None, @@ -351,30 +344,24 @@ def object_grasp( time.sleep(0.25) # Move the arm to a carry position. - print("") - print("Grasp finished, search for a person...") + print("Grasp finished, carrying object.") carry_cmd = RobotCommandBuilder.arm_carry_command() spot.command_client.robot_command(carry_cmd) - - # Wait for the carry command to finish - time.sleep(0.75) + time.sleep(1) print("Force stowing arm!") - # stow_arm(spot) force_stow_arm(manipulation_api_client, robot_state_client, spot.command_client) time.sleep(1) - print("Finished grasp.") - - # If the robot was not successful, send it back to the location it started the skill at - if not success: - execute_recovery_action( - spot, - recover_arm=False, - relative_pose=math_helpers.SE2Pose( - x=np.random.uniform(-1, -0.5), y=0.0, angle=0.0 - ), + if carry_high: + print("Carrying high") + body_tform_goal = math_helpers.SE3Pose( + x=0.6, y=0.0, z=0.6, rot=math_helpers.Quat() ) + move_hand_to_relative_pose(spot, body_tform_goal) + time.sleep(1) + + print("Finished grasp.") if debug: return success, debug_images diff --git a/spot_tools/src/spot_skills/navigation_utils.py b/spot_tools/src/spot_skills/navigation_utils.py index 6d4917c..d76b264 100644 --- a/spot_tools/src/spot_skills/navigation_utils.py +++ b/spot_tools/src/spot_skills/navigation_utils.py @@ -60,13 +60,30 @@ def navigate_to_absolute_pose( ) vel_limit = geometry_pb2.SE2VelocityLimit(max_vel=max_vel_se2) params.vel_limit.CopyFrom(vel_limit) - robot_cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command( - goal_x=waypoint.x, - goal_y=waypoint.y, - goal_heading=waypoint.angle, - frame_name=frame_name, - params=params, - ) + + state = spot.get_state() + manipulator_state = state.manipulator_state + + gripper_holding = manipulator_state.is_gripper_holding_item + + if gripper_holding: + arm_joint_freeze_command = RobotCommandBuilder.arm_joint_freeze_command() + robot_cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command( + goal_x=waypoint.x, + goal_y=waypoint.y, + goal_heading=waypoint.angle, + frame_name=frame_name, + params=params, + build_on_command=arm_joint_freeze_command, + ) + else: + robot_cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command( + goal_x=waypoint.x, + goal_y=waypoint.y, + goal_heading=waypoint.angle, + frame_name=frame_name, + params=params, + ) end_time = 10.0 cmd_id = robot_command_client.robot_command( lease=None, command=robot_cmd, end_time_secs=time.time() + end_time diff --git a/spot_tools_ros/examples/test_spot_executor_ros.py b/spot_tools_ros/examples/test_spot_executor_ros.py index 9782c97..4c262d9 100644 --- a/spot_tools_ros/examples/test_spot_executor_ros.py +++ b/spot_tools_ros/examples/test_spot_executor_ros.py @@ -31,7 +31,7 @@ def __init__(self): path = np.array( [ [0.0, 0], - [3.8, 0], + [2.8, 0], # [3.0, 5], # [5.0, 5], ] @@ -52,7 +52,7 @@ def __init__(self): path = np.array( [ - [3.8, 0], + [2.8, 0], [5.8, 0], ] ) 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 2595859..ba088eb 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 @@ -242,7 +242,7 @@ def register_publishers(self, node): self.logger.info(f"Logging to: {self.output_dir}") if not os.path.exists(self.output_dir): self.logger.info(f"Making {self.output_dir}") - os.mkdir(self.output_dir) + os.makedirs(self.output_dir) log_fn = os.path.join(self.output_dir, "lease_log.txt") with open(log_fn, "w") as fo: fo.write("time,event\n")