Remove impedance control since it seems to break anything (#28)

Author: y-veys

spot_tools/examples/test_spot.py

@@ -440,20 +440,20 @@ def save_images_constant_rate(spot, object_name, folder_name, rate=1.0):
     spot.stand()
     # stow_arm(spot)
 
-    input("Make sure it's in a good position")
-    pose = math_helpers.SE3Pose(
-        x=0.2,
-        y=0,
-        z=0,
-        rot=math_helpers.Quat.from_pitch(0) * math_helpers.Quat.from_roll(0),
-    )
+    # input("Make sure it's in a good position")
+    # pose = math_helpers.SE3Pose(
+    #     x=0.2,
+    #     y=0,
+    #     z=0,
+    #     rot=math_helpers.Quat.from_pitch(0) * math_helpers.Quat.from_roll(0),
+    # )
 
-    move_hand_to_relative_pose(spot, pose)
+    # move_hand_to_relative_pose(spot, pose)
 
     # _run_open_door_test(spot, yoloworld_model_path)
     # _run_walking_test(spot)
     # _run_gaze_test(spot)
-    # _run_place_test(spot)
+    _run_place_test(spot)
     # _run_traj_test(spot)
     # _run_grasp_test(spot)
     # _run_segment_test(spot)

spot_tools/src/spot_skills/grasp_utils.py

@@ -12,24 +12,18 @@
 import bosdyn.client.lease
 import bosdyn.client.util
 import cv2
-import numpy as np
 from bosdyn.api import (
     geometry_pb2,
     manipulation_api_pb2,
     robot_state_pb2,
 )
 from bosdyn.client.frame_helpers import (
-    ODOM_FRAME_NAME,
     VISION_FRAME_NAME,
     get_vision_tform_body,
     math_helpers,
 )
 from bosdyn.client.robot_command import RobotCommandBuilder, block_until_arm_arrives
 
-from spot_skills.arm_impedance_control_helpers import (
-    apply_force_at_current_position,
-    get_root_T_ground_body,
-)
 from spot_skills.arm_utils import (
     arm_to_carry,
     arm_to_drop,
@@ -123,26 +117,26 @@ def object_place(spot, semantic_class="bag", position=None):
     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,
-    )
+    # 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)
+    # 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")
 
     open_gripper(spot)