Feature/giraffe mode

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,

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)
 

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

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

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],
             ]
         )

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")