From cb20b914efde958f63d153b3fa93e2733a0534c5 Mon Sep 17 00:00:00 2001 From: Kavit Shah Date: Thu, 17 Aug 2023 18:36:13 -0700 Subject: [PATCH] Final cleanup before review --- .../spot_rl/envs/mobile_manipulation_env.py | 2 +- spot_rl_experiments/spot_rl/envs/place_env.py | 11 +++++------ spot_rl_experiments/spot_rl/utils/geometry_utils.py | 4 ++-- 3 files changed, 8 insertions(+), 9 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/mobile_manipulation_env.py b/spot_rl_experiments/spot_rl/envs/mobile_manipulation_env.py index 5677aa6e..3f472b97 100644 --- a/spot_rl_experiments/spot_rl/envs/mobile_manipulation_env.py +++ b/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 diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index 775c3b53..605dd9c4 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/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 diff --git a/spot_rl_experiments/spot_rl/utils/geometry_utils.py b/spot_rl_experiments/spot_rl/utils/geometry_utils.py index 80756fa2..5079ef3d 100644 --- a/spot_rl_experiments/spot_rl/utils/geometry_utils.py +++ b/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