Final cleanup before review

spot_rl_experiments/spot_rl/envs/mobile_manipulation_env.py

@@ -325,7 +325,7 @@ def step(self, base_action, arm_action, *args, **kwargs):
             rospy.set_param("viz_place", waypoint_name)
             self.place_target = place_target_from_waypoint(
                 waypoint_name, get_waypoint_yaml()
-            )  # can you make this cleaner????????
+            )
             self.goal_xy, self.goal_heading = (waypoint[:2], waypoint[2])
             self.navigating_to_place = True
             info["grasp_success"] = True

spot_rl_experiments/spot_rl/envs/place_env.py

@@ -13,7 +13,7 @@
 from spot_rl.utils.generate_place_goal import get_global_place_target
 from spot_rl.utils.geometry_utils import (
     generate_intermediate_point,
-    get_RPY_from_dir_vector,
+    get_RPY_from_vector,
 )
 from spot_rl.utils.utils import (
     construct_config,
@@ -133,7 +133,7 @@ def execute(self, place_target_list, is_local=False):
                 intr_ee_pos = generate_intermediate_point(curr_ee_pos, goal_ee_pos)
 
                 # Get direction vector from current ee position to goal ee position for EE orientation
-                dir_rpy_to_goal = get_RPY_from_dir_vector(goal_ee_pos - curr_ee_pos)
+                dir_rpy_to_goal = get_RPY_from_vector(goal_ee_pos - curr_ee_pos)
 
                 # Go to intermediate point
                 cmd_id = self.spot.move_gripper_to_point(intr_ee_pos, dir_rpy_to_goal)
@@ -143,7 +143,6 @@ def execute(self, place_target_list, is_local=False):
                 cmd_id = self.spot.move_gripper_to_point(goal_ee_pos, dir_rpy_to_goal)
                 self.spot.block_until_arm_arrives(cmd_id, timeout_sec=10)
 
-            # pdb.set_trace()
             success_list.append(
                 {
                     # "place_object": "place_target",
@@ -195,7 +194,7 @@ def get_place_target_in_base_frame(self):
     def shutdown(self, should_dock=False):
         try:
             if should_dock:
-                # self.place_env.say("Executing automatic docking")
+                self.place_env.say("Executing automatic docking")
                 dock_start_time = time.time()
                 while time.time() - dock_start_time < 2:
                     try:
@@ -204,7 +203,7 @@ def shutdown(self, should_dock=False):
                         print("Dock not found... trying again")
                         time.sleep(0.1)
             else:
-                # self.place_env.say("Will sit down here")
+                self.place_env.say("Will sit down here")
                 self.spot.sit()
         finally:
             self.spot.power_off()
@@ -220,7 +219,7 @@ def __init__(self, config, spot: Spot):
         self.placed = False
 
     def reset(self, place_target, target_is_local=False, *args, **kwargs):
-        # assert place_target is not None
+        assert place_target is not None
         self.place_target = np.array(place_target)
         self.place_target_is_local = target_is_local
 

spot_rl_experiments/spot_rl/utils/geometry_utils.py

@@ -230,9 +230,9 @@ def generate_intermediate_point(point1, point2):
     return intermediate_point
 
 
-def get_RPY_from_dir_vector(vect):
+def get_RPY_from_vector(vect):
     """
-    Get the roll, pitch, yaw angles from a direction vector
+    Get the roll, pitch, yaw angles from a vector
 
     Args:
         vect (np.array): The direction vector