From 64f3edf161bede6bddca6570bdd51eae62e50270 Mon Sep 17 00:00:00 2001 From: Jimmy Yang <55121504+jimmytyyang@users.noreply.github.com> Date: Thu, 15 Aug 2024 09:26:15 -0400 Subject: [PATCH 01/82] Open close tracking with sam2 (#183) * add tracking * refine * tracking working * polish * 5 * polish * find the path automatically * add readme * enable tracking for mobile gaze * fix bug * increase the confidnece bound * reset tracking * Open close tracking with sam2 fremont finetuning (#186) * finetuning fixes * better grasp detection * fix bug * polish * add cab * fix cab --------- Co-authored-by: Tushar Sangam * polish * polish the text --------- Co-authored-by: Jimmy Yang Co-authored-by: Tushar Sangam --- README.md | 21 +- bd_spot_wrapper/spot_wrapper/spot.py | 17 +- spot_rl_experiments/configs/config.yaml | 2 +- .../skill_test/test_open_close_drawer.py | 2 + spot_rl_experiments/spot_rl/envs/base_env.py | 5 + .../spot_rl/envs/open_close_drawer_env.py | 130 ++++++++----- spot_rl_experiments/spot_rl/models/owlvit.py | 25 ++- .../spot_rl/utils/img_publishers.py | 94 ++++++++- .../spot_rl/utils/tracking_service.py | 182 ++++++++++++++++++ 9 files changed, 420 insertions(+), 58 deletions(-) create mode 100644 spot_rl_experiments/spot_rl/utils/tracking_service.py diff --git a/README.md b/README.md index 03af6035..f5c4e13d 100644 --- a/README.md +++ b/README.md @@ -184,7 +184,7 @@ git checkout main #### Step6. [Optional] Pick with Pose estimation (uses NVIDIA's FoundationPose Model) - Ensure [FoundationPoseForSpotSim2real](https://github.com/tusharsangam/FoundationPoseForSpotSim2Real) is setup as submodule in third_party folder please follow instructions in [SETUP_INSTRUCTIONS.mds](./installation/SETUP_INSTRUCTIONS.md) - Currently we only support pose estimation for bottle, penguine plush toy & paper cup found in FB offices' microktichen -- New Meshes can be added using 360 video of the object from any camera (iphone, android), entire process will be described in the above repo's README +- New Meshes can be added using 360 video of the object from any camera (iphone, android), entire process will be described in the above repo's README - Pose estimation model [FoundationPose](https://nvlabs.github.io/FoundationPose/) runs as a microservice & can be communicated through pyzmq socket - The [Step 1](#step1-run-the-local-launch-executable) should also start the pose estimation service & no other step is required to start this microservice - How to use Pose Estimation ? @@ -194,6 +194,19 @@ git checkout main - Enabling pose estimation can help in two major way - informs grasp api how to approach the object viz. topdown or side which increases the grasp success probability & correct object orientation before place is ran. +#### Step7. [Optional] Object detection with tracking (uses Meta's SAM2 Model) +- Follow the instruction to setup SAM2 [here](https://github.com/jimmytyyang/sam2_skills.git). You can create a new conda environment, different from spot conda environment. The reason for not merging these two environments is that spot-sim2real currently use a lower python version. In addition, we motify the orginal sam2 so that now it can take raw RGB images, rather than a video path. +- Once finishing the installation, run tracking service on its own conda environment by +```bash +python spot_rl_experiments/spot_rl/utils/tracking_service.py +``` +- We support open/close skills using SAM2's tracking. It is done by setting +```python +import rospy +rospy.set_param("enable_tracking", True) +``` + + ### Using Spot Data-logger All logs will get stored inside `data/data_logs` directory @@ -233,7 +246,7 @@ This will record data in a while loop, press `Ctrl+c` to spot the logger. That w Warning : This logger will cause motion blur as camera data is logged while the robot moves. Currently we do not support Spot-Record-Go protocol to log #### Log replay -It is also possible to replay the logged data (essentially the camera streams that have been logged) using the following command : +It is also possible to replay the logged data (essentially the camera streams that have been logged) using the following command: ```bash python -m spot_wrapper.data_logger --replay=".pkl" ``` @@ -247,11 +260,11 @@ We provide an function that can call skills in seperate conda environment. And t #### Step1. Follow Running instructions section to setup image client in spot_ros conda environment #### Step2. Run ```skill_executor.py``` to listen to which skill to use. This will run on the background. ```bash -python spot_rl_experiments/spot_rl/utils/skill_executor.py +python spot_rl_experiments/spot_rl/utils/skill_executor.py ``` #### Step3. Use ROS to use skill in your application. Now you can call skills in non-blocking way. ```python -# In your application, you import rospy for calling which skill to use +# In your application, you import rospy for calling which skill to use import rospy # This is the only package you need to install in your environment rospy.set_param("skill_name_input", "Navigate,desk") # Call navigation skills to navigate to the desk. This is a non-blocking call. ``` diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index 6537f5d6..337ac67d 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -404,6 +404,10 @@ def move_gripper_to_points( ) cmd_id = self.command_client.robot_command(command) success_status = self.block_until_arm_arrives(cmd_id, timeout_sec=timeout_sec) + + # Set the robot base velocity to reset the base motion after calling body movement. + # Without this, calling the move gripper function casues the base to move. + self.set_base_velocity(x_vel=0, y_vel=0, ang_vel=0, vel_time=0.8) return success_status def move_gripper_to_point( @@ -599,6 +603,7 @@ def move_arm_to_point_with_body_follow( gripper_pose_quats: List[List[float]], seconds: int = 5, allow_body_follow: bool = True, + body_offset_from_hand: List[float] = [0.55, 0, 0.25], ) -> bool: """Move the arm to the given point in the body frame and the given gripper poses, and allow the body to follow the arm.""" @@ -613,7 +618,11 @@ def move_arm_to_point_with_body_follow( # Tell the robot's body to follow the arm mobility_command = mobility_command_pb2.MobilityCommand.Request( follow_arm_request=basic_command_pb2.FollowArmCommand.Request( - body_offset_from_hand=Vec3(x=0.5, y=0, z=0) + body_offset_from_hand=Vec3( + x=body_offset_from_hand[0], + y=body_offset_from_hand[1], + z=body_offset_from_hand[2], + ) ) ) synchronized_command = synchronized_command_pb2.SynchronizedCommand.Request( @@ -632,8 +641,12 @@ def move_arm_to_point_with_body_follow( # Send the request move_command_id = self.command_client.robot_command(command) self.robot.logger.info("Moving arm to position.") + msg = self.block_until_arm_arrives(move_command_id, seconds + 1) - return self.block_until_arm_arrives(move_command_id, seconds + 1) + # Set the robot base velocity to reset the base motion after calling body movement. + # Without this, calling the move gripper function casues the base to move. + self.set_base_velocity(x_vel=0, y_vel=0, ang_vel=0, vel_time=0.8) + return msg def grasp_point_in_image_with_IK( self, diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index be039b07..147a67f1 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -103,7 +103,7 @@ SEMANTIC_PLACE_JOINT_BLACKLIST: [3] MAX_LIN_DIST_OPEN_CLOSE_DRAWER: 0.0 MAX_ANG_DIST_OPEN_CLOSE_DRAWER: 3.73 # degrees OPEN_CLOSE_DRAWER_ACTION_SPACE_LENGTH: 8 -OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.3 +OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.75 # Semantic Pick IMG_SRC: 0 # 1 for intel & 0 for gripper diff --git a/spot_rl_experiments/experiments/skill_test/test_open_close_drawer.py b/spot_rl_experiments/experiments/skill_test/test_open_close_drawer.py index 75750823..bc367342 100644 --- a/spot_rl_experiments/experiments/skill_test/test_open_close_drawer.py +++ b/spot_rl_experiments/experiments/skill_test/test_open_close_drawer.py @@ -4,6 +4,7 @@ import numpy as np +import rospy from spot_rl.envs.skill_manager import SpotSkillManager if __name__ == "__main__": @@ -15,6 +16,7 @@ # Using while loop contnue = True while contnue: + rospy.set_param("enable_tracking", False) spotskillmanager.opendrawer() close_drawer = map_user_input_to_boolean( "Do you want to close the drawer ? Y/N " diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index fdc0761e..d9659058 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -173,6 +173,9 @@ def __init__( self._max_lin_dist_scale = self.config[max_lin_dist_key] self._max_ang_dist_scale = self.config[max_ang_dist_key] + # Tracking paramters reset + rospy.set_param("enable_tracking", False) + # Text-to-speech self.tts_pub = rospy.Publisher(rt.TEXT_TO_SPEECH, String, queue_size=1) @@ -265,6 +268,7 @@ def reset(self, target_obj_name=None, *args, **kwargs): self.prev_base_moved = False self.should_end = False rospy.set_param("is_whiten_black", True) + rospy.set_param("enable_tracking", False) observations = self.get_observations() return observations @@ -684,6 +688,7 @@ def get_gripper_images(self, save_image=False): break if self.detection_timestamp is None: raise RuntimeError("Could not correctly synchronize gaze observations") + self.detections_str_synced, filtered_hand_depth = ( self.detections_buffer["detections"][self.detection_timestamp], self.detections_buffer["filtered_depth"][self.detection_timestamp], diff --git a/spot_rl_experiments/spot_rl/envs/open_close_drawer_env.py b/spot_rl_experiments/spot_rl/envs/open_close_drawer_env.py index f902eee6..41197229 100644 --- a/spot_rl_experiments/spot_rl/envs/open_close_drawer_env.py +++ b/spot_rl_experiments/spot_rl/envs/open_close_drawer_env.py @@ -7,12 +7,26 @@ import time from typing import Any, Dict +import cv2 import magnum as mn import numpy as np import quaternion import rospy + +try: + import sophuspy as sp +except Exception as e: + print(f"Cannot import sophuspy due to {e}. Import sophus instead") + import sophus as sp +from bosdyn.client.frame_helpers import GRAV_ALIGNED_BODY_FRAME_NAME +from bosdyn.client.math_helpers import Quat, SE3Pose from spot_rl.envs.base_env import SpotBaseEnv -from spot_rl.utils.heuristic_nav import get_3d_point +from spot_rl.utils.pixel_to_3d_conversion_utils import ( + get_3d_point, + project_3d_to_pixel_uv, + sample_patch_around_point, +) +from spot_rl.utils.rospy_light_detection import detect_with_rospy_subscriber from spot_wrapper.spot import Spot, image_response_to_cv2, scale_depth_img from spot_wrapper.utils import angle_between_quat @@ -100,6 +114,7 @@ def reset(self, goal_dict=None, *args, **kwargs): # Update target object name as provided in config observations = super().reset(target_obj_name="drawer handle", *args, **kwargs) rospy.set_param("object_target", self.target_obj_name) + rospy.set_param("enable_tracking", True) # Flag for done self._success = False @@ -149,8 +164,9 @@ def open_drawer(self): # Get the transformation of the gripper vision_T_hand = self.spot.get_magnum_Matrix4_spot_a_T_b("vision", "hand") + # Get the location that we want to move to for retracting/moving forward the arm. Pull/push the drawer by 20 cm - pull_push_distance = -0.2 if self._mode == "open" else 0.25 + pull_push_distance = -0.2 if self._mode == "open" else 0.4 move_target = vision_T_hand.transform_point( mn.Vector3([pull_push_distance, 0, 0]) ) @@ -159,7 +175,9 @@ def open_drawer(self): # Retract the arm based on the current gripper location self.spot.move_gripper_to_point( - move_target, [ee_rotation.w, ee_rotation.x, ee_rotation.y, ee_rotation.z] + move_target, + [ee_rotation.w, ee_rotation.x, ee_rotation.y, ee_rotation.z], + seconds_to_goal=5, ) def open_cabinet(self): @@ -213,48 +231,60 @@ def open_cabinet(self): vel_time=0.8, ) - def approach_handle_and_grasp(self, z, pixel_x, pixel_y): + def approach_handle_and_grasp(self): """This method does IK to approach the handle and close the gripper to grasp the handle.""" ######################################################## ### Step 1: Get the location of handle in hand frame ### ######################################################## + # Get the image imgs = self.spot.get_hand_image() + depth_raw = image_response_to_cv2(imgs[1]) + + # Always get the latest detection + x1, y1, x2, y2 = detect_with_rospy_subscriber( + rospy.get_param("/object_target", "drawer handle"), self.config.IMAGE_SCALE + ) + pixel_x, pixel_y = (x1 + x2) // 2, (y1 + y2) // 2 + + # Get the depth z + z = sample_patch_around_point(pixel_x, pixel_y, depth_raw) * 1e-3 # Get the camera intrinsics cam_intrinsics = imgs[0].source.pinhole.intrinsics - # Get the transformation - vision_T_base = self.spot.get_magnum_Matrix4_spot_a_T_b("vision", "body") - - # Get the 3D point in the hand RGB frame point_in_hand_image_3d = get_3d_point(cam_intrinsics, (pixel_x, pixel_y), z) - # Get the vision to hand - vision_T_hand_image: mn.Matrix4 = self.spot.get_magnum_Matrix4_spot_a_T_b( - "vision", "hand_color_image_sensor", imgs[0].shot.transforms_snapshot + body_T_hand: mn.Matrix4 = self.spot.get_magnum_Matrix4_spot_a_T_b( + GRAV_ALIGNED_BODY_FRAME_NAME, + "link_wr1", + ) + hand_T_gripper: mn.Matrix4 = self.spot.get_magnum_Matrix4_spot_a_T_b( + "arm0.link_wr1", + "hand_color_image_sensor", + imgs[0].shot.transforms_snapshot, ) - point_in_global_3d = vision_T_hand_image.transform_point( + body_T_gripper: mn.Matrix4 = body_T_hand @ hand_T_gripper + point_in_base_3d = body_T_gripper.transform_point( mn.Vector3(*point_in_hand_image_3d) ) - # Get the transformation of the gripper - vision_T_hand = self.spot.get_magnum_Matrix4_spot_a_T_b("vision", "hand") - # Get the location relative to the gripper - point_in_hand_3d = vision_T_hand.inverted().transform_point(point_in_global_3d) - # Offset the x and z direction in hand frame - ee_offset_x = 0.05 if self._rep_type == "drawer" else 0.05 - ee_offset_y = 0.0 if self._rep_type == "drawer" else 0.01 - ee_offset_z = -0.05 if self._rep_type == "drawer" else 0.02 - point_in_hand_3d[0] += ee_offset_x - point_in_hand_3d[1] += ee_offset_y - point_in_hand_3d[2] += ee_offset_z - # Make it back to global frame - point_in_global_3d = vision_T_hand.transform_point(point_in_hand_3d) - - # Get the point in the base frame - point_in_base_3d = vision_T_base.inverted().transform_point(point_in_global_3d) + print( + f"Drawer point in gripper 3D {point_in_hand_image_3d}, point in body {point_in_base_3d}" + ) + + # Useful Debug Code -- verify point in image, comment later + # image_rgb = image_response_to_cv2(imgs[0]) + # image_rgb = cv2.circle( + # image_rgb, (int(pixel_x), int(pixel_y)), radius=4, color=(0, 0, 255) + # ) + # cv2.namedWindow("Handle", cv2.WINDOW_NORMAL) + # cv2.imshow("Handle", image_rgb) + # cv2.waitKey(0) + # cv2.destroyAllWindows() + + point_in_base_3d = mn.Vector3(*point_in_base_3d) # Make it to be numpy point_in_base_3d = np.array( @@ -269,28 +299,42 @@ def approach_handle_and_grasp(self, z, pixel_x, pixel_y): ### Step 2: Move the gripper to that handle ### ############################################### - # Get the current ee rotation in body frame - ee_rotation = self.spot.get_ee_quaternion_in_body_frame() - - # For the cabnet part: rotation the gripper by 90 degree + # Move the gripper to target using current gripper pose in the body frame + # while maintaining the gripper orientation + # For the cabinet part: rotation the gripper by 90 degree if self._rep_type == "cabinet": - self.spot.move_gripper_to_point( - point_in_base_3d, - [np.pi / 2, 0, 0], + # Rotate the gripper by 90 degree + ee_rotation = quaternion.quaternion( + 0.71277446, 0.70131284, 0.00662719, 0.00830902 ) elif self._rep_type == "drawer": - # Move the gripper to target using current gripper pose in the body frame - # while maintaining the gripper orientation - self.spot.move_gripper_to_point( - point_in_base_3d, - [ee_rotation.w, ee_rotation.x, ee_rotation.y, ee_rotation.z], + # Do not rotate the gripper + ee_rotation = quaternion.quaternion( + 0.99998224, 0.00505713, 0.00285832, 0.00132725 ) + # Check if the point is reachable without mobility + is_point_reachable_without_mobility = ( + self.spot.query_IK_reachability_of_gripper( + SE3Pose( + *point_in_base_3d, + Quat(ee_rotation.w, ee_rotation.x, ee_rotation.y, ee_rotation.z), + ) + ) + ) + + # Move the gripper + self.spot.move_arm_to_point_with_body_follow( + [point_in_base_3d], + [[ee_rotation.w, ee_rotation.x, ee_rotation.y, ee_rotation.z]], + seconds=5, + allow_body_follow=not is_point_reachable_without_mobility, + body_offset_from_hand=[0.9, 0, 0.35], + ) + ################################# ### Step 3: Close the gripper ### ################################# - - # Close the gripper self.spot.close_gripper() # Pause a bit to ensure the gripper grapes the handle time.sleep(2) @@ -346,7 +390,7 @@ def step(self, action_dict: Dict[str, Any]): # We close gripper here if z != 0 and z < self._dis_threshold_ee_to_handle: # Do IK to approach the target - self.approach_handle_and_grasp(z, pixel_x, pixel_y) + self.approach_handle_and_grasp() # If we can do IK, then we call it successful done = self._success diff --git a/spot_rl_experiments/spot_rl/models/owlvit.py b/spot_rl_experiments/spot_rl/models/owlvit.py index fd91d398..0d12c56c 100644 --- a/spot_rl_experiments/spot_rl/models/owlvit.py +++ b/spot_rl_experiments/spot_rl/models/owlvit.py @@ -130,13 +130,13 @@ def run_inference_and_return_img(self, img, vis_img_required=True): return ( self.get_most_confident_bounding_box_per_label(results), - self.create_img_with_bounding_box(img, results) + self.create_img_with_bounding_box_no_ranking(img, results) if vis_img_required else None, ) def show_img_with_overlaid_bounding_boxes(self, img, results): - img = self.create_img_with_bounding_box(img, results) + img = self.create_img_with_bounding_box_no_ranking(img, results) cv2.imshow("img", img) cv2.waitKey(1) @@ -320,6 +320,27 @@ def create_img_with_bounding_box(self, img, results): return img + def create_img_with_bounding_box_no_ranking(self, img, results): + """ + Returns an image with all bounding boxes above the threshold overlaid. + Each class has only one bounding box. + """ + + results = self.get_most_confident_bounding_box_per_label(results) + font = cv2.FONT_HERSHEY_SIMPLEX + + for label, score, box in results: + img = cv2.rectangle(img, box[:2], box[2:], (255, 0, 0), 5) + if box[3] + 25 > 768: + y = box[3] - 10 + else: + y = box[3] + 25 + img = cv2.putText( + img, label, (box[0], y), font, 1, (255, 0, 0), 2, cv2.LINE_AA + ) + + return img + def update_label(self, labels: List[List[str]]): """Update labels that need to be detected diff --git a/spot_rl_experiments/spot_rl/utils/img_publishers.py b/spot_rl_experiments/spot_rl/utils/img_publishers.py index 4b7aea67..abd58416 100644 --- a/spot_rl_experiments/spot_rl/utils/img_publishers.py +++ b/spot_rl_experiments/spot_rl/utils/img_publishers.py @@ -17,6 +17,7 @@ from cv_bridge import CvBridge from sensor_msgs.msg import Image from spot_rl.utils.depth_map_utils import filter_depth +from spot_rl.utils.tracking_service import tracking_with_socket from spot_wrapper.spot import Spot from spot_wrapper.spot import SpotCamIds as Cam from spot_wrapper.spot import image_response_to_cv2, scale_depth_img @@ -316,6 +317,7 @@ def __init__(self, model): self.detection_topic, String, queue_size=1, tcp_nodelay=True ) self.viz_topic = rt.MASK_RCNN_VIZ_TOPIC + self._reset_tracking_params() def preprocess_image(self, img): if self.image_scale != 1.0: @@ -337,17 +339,96 @@ def preprocess_image(self, img): return img + def _reset_tracking_params(self): + self._tracking_first_time = ( + True # flag to indicate if it is the first time tracking + ) + self._tracking_images = [] # cache the tracking images + self._tracking_bboxs = [] # cache the tracking bboxs + def _publish(self): stopwatch = Stopwatch() header = self.img_msg.header timestamp = header.stamp - hand_rgb = self.msg_to_cv2(self.img_msg) - # Internal model + # size of hand_rgb: (480, 640, 3) + # size of hand_rgb_preprocessed: (336, 448, 3) + + hand_rgb = self.msg_to_cv2(self.img_msg) hand_rgb_preprocessed = self.preprocess_image(hand_rgb) - bbox_data, viz_img = self.model.inference( - hand_rgb_preprocessed, timestamp, stopwatch - ) + + # Copy the hand_rgb_preprocessed + tracking_rgb = hand_rgb_preprocessed.copy().astype("uint8") + + # Fetch the tracking flag + enable_tracking = rospy.get_param("enable_tracking", False) + + # Cache the vis image + viz_img = hand_rgb_preprocessed + + if enable_tracking: + # Start tracking using video segmentation model + images = np.expand_dims( + tracking_rgb, axis=0 + ) # The size should (1, H, W, C) + self._tracking_images.append(images) + + if len(self._tracking_images) == 1 and self._tracking_first_time: + # To see if there is a bounding box in the first frame + self._tracking_first_time = False + # Get the first detection + bbox_data, viz_img = self.model.inference( + hand_rgb_preprocessed, timestamp, stopwatch + ) + _, detection_str = bbox_data.split("|") + if detection_str == "None": + # Do not see the object yet, so restart the detection + self._reset_tracking_params() + else: + # See the object at the first frame + cur_bbox_str = detection_str.split(",")[2:] + cur_bbox = [int(v) for v in cur_bbox_str] + self._tracking_bboxs.append(cur_bbox) + elif len(self._tracking_images) >= 2: + # This means that there is one object being detected in the previous frame (first frame) + input_images = np.concatenate(self._tracking_images, axis=0) + # Get the previous anchor + bbox = self._tracking_bboxs[-1] + cur_bbox = tracking_with_socket(input_images, bbox) + if cur_bbox is None: + self._tracking_bboxs.append(None) + bbox_data = f"{str(timestamp)}|None" + else: + cur_bbox = [cur_bbox[1], cur_bbox[0], cur_bbox[3], cur_bbox[2]] + self._tracking_bboxs.append(cur_bbox) + object_label = rospy.get_param("object_target") + bbox_data = f"{str(timestamp)}|{object_label},{1.0}," + ",".join( + [str(v) for v in cur_bbox] + ) + # Apply the red bounding box to showcase tracking on the image + viz_img = cv2.rectangle( + viz_img, cur_bbox[:2], cur_bbox[2:], (0, 0, 255), 5 + ) + + if len(self._tracking_images) >= 2: + # Prune the data + # Find the frame that has tracking + suc_frame = [] + for i, v in enumerate(self._tracking_bboxs): + if v is not None: + suc_frame.append(i) + + # Only keep the latest frame that has tracking to keep + # buffer size 2 + self._tracking_images = [self._tracking_images[suc_frame[-1]]] + self._tracking_bboxs = [self._tracking_bboxs[suc_frame[-1]]] + else: + # Normal detection mode using detection model + bbox_data, viz_img = self.model.inference( + hand_rgb_preprocessed, timestamp, stopwatch + ) + time_stamp, detection_str = bbox_data.split("|") + self._reset_tracking_params() # publish data self.publish_bbox_data(bbox_data) @@ -365,7 +446,7 @@ def publish_viz_img(self, viz_img, header): class OWLVITModel: - def __init__(self, score_threshold=0.05, show_img=False): + def __init__(self, score_threshold=0.1, show_img=False): self.config = config = construct_config() self.owlvit = OwlVit([["ball"]], score_threshold, show_img, 2) self.image_scale = config.IMAGE_SCALE @@ -455,6 +536,7 @@ def inference(self, hand_rgb, timestamp, stopwatch): elif owlvit: # TODO dynamic label rospy.set_param("object_target", "ball") + rospy.set_param("enable_tracking", False) model = OWLVITModel() node = SpotBoundingBoxPublisher(model) elif decompress: diff --git a/spot_rl_experiments/spot_rl/utils/tracking_service.py b/spot_rl_experiments/spot_rl/utils/tracking_service.py new file mode 100644 index 00000000..6ffefefe --- /dev/null +++ b/spot_rl_experiments/spot_rl/utils/tracking_service.py @@ -0,0 +1,182 @@ +import os +import time + +import matplotlib.pyplot as plt +import numpy as np +import torch +import zmq +from PIL import Image + +LOAD_SAM2 = False +try: + from sam2.build_sam import build_sam2_video_predictor + + LOAD_SAM2 = True +except Exception: + print("Cannot import SAM2. SAM2 needs SAM2 conda env. Skipping...") + LOAD_SAM2 = False + +if LOAD_SAM2: + # use bfloat16 for the entire notebook + torch.autocast(device_type="cuda", dtype=torch.bfloat16).__enter__() + + if torch.cuda.get_device_properties(0).major >= 8: + torch.backends.cuda.matmul.allow_tf32 = True + torch.backends.cudnn.allow_tf32 = True + + # SAM2_CKPT = "segment-anything-2/checkpoints/sam2_hiera_large.pt" + SAM2_CKPT = None + for root, dirs, files in os.walk("/home/"): + if "sam2_hiera_large.pt" in files: + SAM2_CKPT = os.path.join(root, "sam2_hiera_large.pt") + break + + if SAM2_CKPT is None: + print("Cannot import sam2. Please provide sam2 checkpoint") + raise Exception("Cannot import sam2. Please provide sam2 checkpoint") + MODEL_CFG = "sam2_hiera_l.yaml" + + +class Track: + def __init__(self): + self.predictor = build_sam2_video_predictor(MODEL_CFG, SAM2_CKPT) + self.cur_imgs = None + self.video_segments = {} + + def init_state(self, imgs): + self.cur_imgs = imgs + self.inference_state = self.predictor.init_state_with_images(imgs) + self.reset_state() + + def reset_state(self): + self.predictor.reset_state(self.inference_state) + + def add_bbox(self, bbox): + # a box at (x_min, y_min, x_max, y_max) to get started + _, out_obj_ids, out_mask_logits = self.predictor.add_new_points_or_box( + inference_state=self.inference_state, + frame_idx=0, + obj_id=1, + box=np.array(bbox, dtype=np.float32), + ) + + def track(self): + # run propagation throughout the video and collect the results in a dict + self.video_segments = ( + {} + ) # video_segments contains the per-frame segmentation results + for ( + out_frame_idx, + out_obj_ids, + out_mask_logits, + ) in self.predictor.propagate_in_video(self.inference_state): + self.video_segments[out_frame_idx] = { + out_obj_id: (out_mask_logits[i] > 0.0).cpu().numpy() + for i, out_obj_id in enumerate(out_obj_ids) + } + final_masks = None + + # Get the final mask + result = [(k, v) for k, v in self.video_segments[out_frame_idx].items()] + out_mask = result[-1][1] + if np.sum(out_mask) != 0: + xmax, ymax = np.max(np.where(out_mask[0] == 1), 1) + xmin, ymin = np.min(np.where(out_mask[0] == 1), 1) + final_masks = [xmin, ymin, xmax, ymax] + return final_masks + + def vis( + self, + ): + # render the segmentation results every few frames + frame_names = ["prev_frame", "cur_frame"] + plt.close("all") + for out_frame_idx in range(0, len(frame_names)): + plt.figure(figsize=(6, 4)) + plt.title(f"{out_frame_idx}") + plt.imshow(self.cur_imgs[out_frame_idx]) + for out_obj_id, out_mask in self.video_segments[out_frame_idx].items(): + show_mask(out_mask, plt.gca(), obj_id=out_obj_id) + plt.show() + + +def connect_socket(port): + context = zmq.Context() + socket = context.socket(zmq.REQ) + socket.connect(f"tcp://localhost:{port}") + return socket + + +def tracking_with_socket(images, bbox, port=21002): + # images with the size of [2, H, W, 3] + # bbox with the format of [x_min, y_min, x_max, y_max] + socket = connect_socket(port) + socket.send_pyobj((images, bbox)) + return socket.recv_pyobj() + + +def show_mask(mask, ax, obj_id=None, random_color=False): + if random_color: + color = np.concatenate([np.random.random(3), np.array([0.6])], axis=0) + else: + cmap = plt.get_cmap("tab10") + cmap_idx = 0 if obj_id is None else obj_id + color = np.array([*cmap(cmap_idx)[:3], 0.6]) + h, w = mask.shape[-2:] + mask_image = mask.reshape(h, w, 1) * color.reshape(1, 1, -1) + ax.imshow(mask_image) + + +if __name__ == "__main__": + port = "21002" + context = zmq.Context() + socket = context.socket(zmq.REP) + socket.bind(f"tcp://*:{port}") + print(f"Tracking Server Listening on port {port}") + + sam2 = Track() + while True: + """A service for running tracking service, send request using zmq socket""" + images, bbox = socket.recv_pyobj() + start_time = time.time() + sam2.init_state(images) + cur_bbox = bbox + sam2.add_bbox(cur_bbox) + new_bbox = sam2.track() + # sam2.vis() # debug + socket.send_pyobj(new_bbox) + +# Debug code +# sam2 = Track() + +# # Load the images +# from sam2.utils.misc import load_video_frames_light + +# images, video_height, video_width = load_video_frames_light( +# video_path="/home/jmmy/research/segment-anything-2/notebooks/videos/handle", +# image_size=sam2.predictor.image_size, +# offload_video_to_cpu=False, +# async_loading_frames=False, +# ) +# images = (images.permute(0, 2, 3, 1).cpu().numpy() * 255).astype("uint8") +# images = images[80:, :, :, :] + +# cur_bbox = [200, 340, 300, 350] # (x_min, y_min, x_max, y_max) +# # Loop over images +# for i in range(len(images) - 1): +# # images with the shape of torch.Size([# of frames, 1024, 1024, 3]) +# # Init the predictor +# start_time = time.time() +# sam2.init_state(images[i : i + 2]) +# # Add the bbox for the first frame +# sam2.add_bbox(cur_bbox) +# # Track the object +# try: +# raw_cur_bbox = sam2.track() +# print("time taken to track one frame:", time.time() - start_time, "sec") +# sam2.vis() +# except Exception as e: +# # Visualize the results +# sam2.vis() +# # For img coordinate +# cur_bbox = [raw_cur_bbox[1], raw_cur_bbox[0], raw_cur_bbox[3], raw_cur_bbox[2]] From 5e73910aac94372979644cf54ba9e00bb7493b45 Mon Sep 17 00:00:00 2001 From: Jimmy Yang <55121504+jimmytyyang@users.noreply.github.com> Date: Fri, 16 Aug 2024 11:14:03 -0400 Subject: [PATCH 02/82] fix (#192) Co-authored-by: Jimmy Yang --- spot_rl_experiments/spot_rl/envs/skill_manager.py | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index d51e6966..f9e90d70 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -475,6 +475,15 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray message = f"Failed - place target {place_target} not found - use the exact name" conditional_print(message=message, verbose=self.verbose) return False, message + + if self.use_semantic_place: + # Convert HOME frame coordinates into body frame + place_target_location = ( + self.place_controller.env.get_target_in_base_frame( + mn.Vector3(*place_target_location.astype(np.float64).tolist()) + ) + ) + is_local = True else: message = "No place target specified, estimating point through heuristic" conditional_print(message=message, verbose=self.verbose) From 66211f45805d61c3b7c53fe60f5da806a40ae569 Mon Sep 17 00:00:00 2001 From: Jimmy Yang <55121504+jimmytyyang@users.noreply.github.com> Date: Sat, 17 Aug 2024 16:14:05 -0400 Subject: [PATCH 03/82] spot-sim2real for hab-llm (#187) * load the newly trained checkpoints * port semantic pick * tidy * appraching * update checkpoint and make the MAX_JOINT_MOVEMENT_SEMANTIC_GAZE large and it works well * pose estimation (#166) * load the newly trained checkpoints port semantic pick tidy appraching update checkpoint and make the MAX_JOINT_MOVEMENT_SEMANTIC_GAZE large and it works well smooth arm actions pose estimation pose correction with discretization Orientation Correction Working pose correction finetuning adding more grasp orientation & object correction pose estimation with object facing camera creating new pick api force control for object grasping PR ready to pr PR ready changes Starting pose estimation service from spot-sim2real PR hot fixes submodule init instruction update add todo and fix pre-commit issue PR review 1 PR fixes 2 fix precommit fix precommit v2 * add trest srcipt for nn basew * PR review final --------- Co-authored-by: Jimmy Yang first commit force control first commit parameter adjustment Generalization to grasp some changes remaining arm command follow pose estimation fix for grasping video shoot body follow fix some changes caliberation fix Resolving calibartaion error with minimal changes gitignore backup benchmark files ros_communications ros_communications reducing file size Bug fixes n support for Float32MultiArray update make it work find human action tool is in rospy node add nav target update the nav skill execution polish detected translation polish the location in 3D improve object detection use local place skills and change the nav threshold add spot benchmark fix suc condition for dynamic yaw round handle the case for not getting spot transformation add exploration fix split bug add semantic place improve print polish detection make exploration works fix the bug of getting none transformaion fix the bug of getting none transformaion change it 100 steps fix the search table issue of not getting snapshot transformation * polish remove files refine skill mgr fix bug polish remove fix bug change config skills add polish polish --------- Co-authored-by: Tushar Sangam Co-authored-by: Jimmy Yang --- bd_spot_wrapper/spot_wrapper/spot.py | 8 +- spot_rl_experiments/configs/config.yaml | 5 +- .../configs/ros_topic_names.yaml | 2 + .../skill_test/test_dynamic_yaw_nav.py | 100 +++++++ .../test_dynamic_yaw_nav_math_simulation.py | 261 ++++++++++++++++++ spot_rl_experiments/spot_rl/envs/base_env.py | 8 +- spot_rl_experiments/spot_rl/envs/nav_env.py | 64 ++++- .../spot_rl/envs/skill_manager.py | 32 ++- .../spot_rl/skills/atomic_skills.py | 40 ++- .../spot_rl/utils/gripper_T_intel.npy | Bin 256 -> 0 bytes .../spot_rl/utils/img_publishers.py | 222 +++++++++++++++ .../spot_rl/utils/skill_executor.py | 39 ++- 12 files changed, 747 insertions(+), 34 deletions(-) create mode 100644 spot_rl_experiments/experiments/skill_test/test_dynamic_yaw_nav.py create mode 100644 spot_rl_experiments/experiments/skill_test/test_dynamic_yaw_nav_math_simulation.py delete mode 100644 spot_rl_experiments/spot_rl/utils/gripper_T_intel.npy diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index 337ac67d..fe6b6d70 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -1221,19 +1221,21 @@ def select_hand_image(self, is_rgb=True, img_src: List[str] = []): ) return img_resp - def get_hand_image(self, is_rgb=True): + def get_hand_image(self, is_rgb=True, force_get_gripper=False): """ Gets hand raw rgb & depth, returns List[rgbimage, unscaleddepthimage] image object is BD source image object which has kinematic snapshot & camera intrinsics along with pixel data If is_gripper_blocked is True then returns intel realsense images If hand_image_sources are passed then above condition is ignored & will send image & depth for each source - Thus if you send hand_image_sources=["gripper", "intelrealsense"] then 4 image resps should be returned + Thus if you send hand_image_sources=["gripper", "intelrealsense"] then 4 image resps should be returned. + In addition, the flag force_get_gripper allows you to get gripper images even if is_gripper_blocked is True. + This is useful when you want to get gripper camera transformation when the gripper is blocked. """ realsense_img_srcs: List[str] = [ SpotCamIds.INTEL_REALSENSE_COLOR, SpotCamIds.INTEL_REALSENSE_DEPTH, ] - if self.is_gripper_blocked: # return intelrealsense + if self.is_gripper_blocked and not force_get_gripper: return self.select_hand_image(img_src=realsense_img_srcs) else: return self.select_hand_image(is_rgb=is_rgb) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 147a67f1..ba8eaf46 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -59,11 +59,13 @@ PARALLEL_INFERENCE_MODE: True # General env params CTRL_HZ: 2.0 -MAX_EPISODE_STEPS: 500 +MAX_EPISODE_STEPS: 100 # Nav env SUCCESS_DISTANCE: 0.3 SUCCESS_ANGLE_DIST: 5 +SUCCESS_DISTANCE_FOR_DYNAMIC_YAW_NAV: 1.10 +SUCCESS_ANGLE_DIST_FOR_DYNAMIC_YAW_NAV: 20 DISABLE_OBSTACLE_AVOIDANCE: False USE_OA_FOR_NAV: True USE_HEAD_CAMERA: True @@ -134,6 +136,7 @@ INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_LEFT_HAND: [0, -180, 180, 0, 0, -90] # T INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_TOP_DOWN: [-0.91, -92.87, 91.21, 0, 90.01, 0] # The initial orientation of the arm for top down grasping # The old gaze ready angle: [0, -170, 120, 0, 75, 0] GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 75, 0] #[0, -160, 100, 0, 90, 0] +SEMANTIC_PLACE_ARM_JOINT_ANGLES: [0, -125, 80, 0, 85, 0] PLACE_ARM_JOINT_ANGLES: [0, -170, 120, 0, 75, 0] ARM_LOWER_LIMITS: [-45, -180, 0, 0, -90, 0] ARM_UPPER_LIMITS: [45, 0, 180, 0, 90, 0] diff --git a/spot_rl_experiments/configs/ros_topic_names.yaml b/spot_rl_experiments/configs/ros_topic_names.yaml index 11dbdf28..701d48da 100644 --- a/spot_rl_experiments/configs/ros_topic_names.yaml +++ b/spot_rl_experiments/configs/ros_topic_names.yaml @@ -12,6 +12,8 @@ MASK_RCNN_VIZ_TOPIC: "/mask_rcnn_visualizations" DETECTIONS_TOPIC: "/mask_rcnn_detections" IMAGE_SCALE: "/image_scale" +OPEN_VOC_OBJECT_DETECTOR_TOPIC: "/open_voc_object_detector" + ROBOT_STATE: "/robot_state" TEXT_TO_SPEECH: "/text_to_speech" diff --git a/spot_rl_experiments/experiments/skill_test/test_dynamic_yaw_nav.py b/spot_rl_experiments/experiments/skill_test/test_dynamic_yaw_nav.py new file mode 100644 index 00000000..f1a35b81 --- /dev/null +++ b/spot_rl_experiments/experiments/skill_test/test_dynamic_yaw_nav.py @@ -0,0 +1,100 @@ +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import argparse +import os +import os.path as osp +import time + +import magnum as mn +import numpy as np +import rospy +from spot_rl.envs.skill_manager import SpotSkillManager +from spot_rl.utils.utils import ros_topics as rt +from spot_wrapper.utils import get_angle_between_two_vectors + +NUM_REPEAT = 5 +WAYPOINT_TEST = [[1.8, 1.1]] * NUM_REPEAT # x, y + + +class SpotRosSkillExecutor: + """This class reads the ros butter to execute skills""" + + def __init__(self): + self.spotskillmanager = None + self._cur_skill_name_input = None + + def compute_metrics(self, traj, target_point): + """ "Compute the metrics""" + num_steps = len(traj) + final_point = np.array(traj[-1]["pose"][0:2]) + distance = np.linalg.norm(target_point - final_point) + # Compute the angle + vector_robot_to_target = target_point - final_point + vector_robot_to_target = vector_robot_to_target / np.linalg.norm( + vector_robot_to_target + ) + vector_forward_robot = np.array( + self.spotskillmanager.get_env().curr_transform.transform_vector( + mn.Vector3(1, 0, 0) + ) + )[[0, 1]] + vector_forward_robot = vector_forward_robot / np.linalg.norm( + vector_forward_robot + ) + dot_product_facing_target = abs( + np.dot(vector_robot_to_target, vector_forward_robot) + ) + angle_facing_target = abs( + get_angle_between_two_vectors(vector_robot_to_target, vector_forward_robot) + ) + return { + "num_steps": num_steps, + "distance": distance, + "dot_product_facing_target": dot_product_facing_target, + "angle_facing_target": angle_facing_target, + } + + def benchmark(self): + """ "Run the benchmark code to test skills""" + + metrics_list = [] + for waypoint in WAYPOINT_TEST: + # Call the skill manager + self.spotskillmanager = SpotSkillManager( + use_mobile_pick=True, use_semantic_place=False + ) + x, y = waypoint + suc, _ = self.spotskillmanager.nav(x, y) + # Compute the metrics + traj = ( + self.spotskillmanager.nav_controller.get_most_recent_result_log().get( + "robot_trajectory" + ) + ) + metrics = self.compute_metrics(traj, np.array([x, y])) + metrics["suc"] = suc + metrics_list.append(metrics) + # Reset + self.spotskillmanager.dock() + + # Compute the final number + for mm in metrics: + data = [metrics[mm] for metrics in metrics_list] + _mean = round(np.mean(data), 2) + _std = round(np.std(data), 2) + print(f"{mm}: {_mean} +- {_std}") + + +def main(): + parser = argparse.ArgumentParser() + parser.add_argument("--useful_parameters", action="store_true") + _ = parser.parse_args() + + executor = SpotRosSkillExecutor() + executor.benchmark() + + +if __name__ == "__main__": + main() diff --git a/spot_rl_experiments/experiments/skill_test/test_dynamic_yaw_nav_math_simulation.py b/spot_rl_experiments/experiments/skill_test/test_dynamic_yaw_nav_math_simulation.py new file mode 100644 index 00000000..c333267f --- /dev/null +++ b/spot_rl_experiments/experiments/skill_test/test_dynamic_yaw_nav_math_simulation.py @@ -0,0 +1,261 @@ +import math + +import matplotlib as mpl +import matplotlib.animation as animation +import matplotlib.pyplot as plt +import numpy as np +from svgpath2mpl import parse_path +from svgpathtools import svg2paths + +"""This is a script to debug dynamic yaw point nav skill""" + +axis_size = 14 + + +def wrap_heading(heading): + """Ensures input heading is between -180 an 180; can be float or np.ndarray""" + return (heading + np.pi) % (2 * np.pi) - np.pi + + +def gen_arrow_head_marker(rot): + """generate a marker to plot with matplotlib scatter, plot, ... + https://matplotlib.org/stable/api/markers_api.html#module-matplotlib.markers + rot=0: positive x direction + Parameters + ---------- + rot : float + rotation in rad + 0 is positive x direction + Returns + ------- + arrow_head_marker : Path + use this path for marker argument of plt.scatter + scale : float + multiply a argument of plt.scatter with this factor got get markers + with the same size independent of their rotation. + Paths are autoscaled to a box of size -1 <= x, y <= 1 by plt.scatter + """ + arr = np.array([[0.1, 0.3], [0.1, -0.3], [1, 0], [0.1, 0.3]]) # arrow shape + angle = rot + rot_mat = np.array( + [[np.cos(angle), np.sin(angle)], [-np.sin(angle), np.cos(angle)]] + ) + arr = np.matmul(arr, rot_mat) # rotates the arrow + + # scale + x0 = np.amin(arr[:, 0]) + x1 = np.amax(arr[:, 0]) + y0 = np.amin(arr[:, 1]) + y1 = np.amax(arr[:, 1]) + scale = np.amax(np.abs([x0, x1, y0, y1])) + codes = [ + mpl.path.Path.MOVETO, + mpl.path.Path.LINETO, + mpl.path.Path.LINETO, + mpl.path.Path.CLOSEPOLY, + ] + arrow_head_marker = mpl.path.Path(arr, codes) + return arrow_head_marker, scale + + +def point_in_circle(h, k, r, theta): + return h + np.cos(theta) * r, k + np.sin(theta) * r + + +class robot_simulation: + def __init__(self, goal_xy): + self.x, self.y, self.yaw = [0, 0, 0] # x, y, theta + self._goal_xy = goal_xy + self._goal_heading_list = [] + self._xyyaw_list = [] + + def robot_new_xyyaw(self, xyyaw): + self.x, self.y, self.yaw = xyyaw + + def get_current_angle_for_target_facing(self): + vector_robot_to_target = self._goal_xy - np.array([self.x, self.y]) + vector_robot_to_target = vector_robot_to_target / np.linalg.norm( + vector_robot_to_target + ) + vector_forward_robot = np.array([np.cos(self.yaw), np.sin(self.yaw)]) + vector_forward_robot = vector_forward_robot / np.linalg.norm( + vector_forward_robot + ) + + return vector_robot_to_target, vector_forward_robot + + def compute_angle(self): + ( + vector_robot_to_target, + vector_forward_robot, + ) = self.get_current_angle_for_target_facing() + x1 = ( + vector_robot_to_target[1] * vector_forward_robot[0] + - vector_robot_to_target[0] * vector_forward_robot[1] + ) + x2 = ( + vector_robot_to_target[0] * vector_forward_robot[0] + + vector_robot_to_target[1] * vector_forward_robot[1] + ) + rotation_delta = np.arctan2(x1, x2) + goal_heading = wrap_heading(self.yaw + rotation_delta) + return goal_heading + + def goal_heading_delta(self, goal_heading): + goal_heading_delta = -wrap_heading(goal_heading - self.yaw) + return goal_heading_delta + + def animate(self, xyyaw_list, save_name): + self._goal_heading_list = [] + self._goal_heading_delta_list = [] + self._xyyaw_list = xyyaw_list + for xyyaw in xyyaw_list: + self.robot_new_xyyaw(xyyaw) + self._goal_heading_list.append(self.compute_angle()) + self._goal_heading_delta_list.append( + self.goal_heading_delta(self._goal_heading_list[-1]) + ) + + plt.close("all") + fig = plt.figure() + self.ax = fig.add_subplot(1, 1, 1) + + # Move left y-axis and bottom x-axis to centre, passing through (0,0) + self.ax.spines["left"].set_position("center") + self.ax.spines["bottom"].set_position("center") + + # Eliminate upper and right axes + self.ax.spines["right"].set_color("none") + self.ax.spines["top"].set_color("none") + + # Show ticks in the left and lower axes only + self.ax.xaxis.set_ticks_position("bottom") + self.ax.yaxis.set_ticks_position("left") + self.ax.set_xlim([-axis_size, axis_size]) + self.ax.set_ylim([-axis_size, axis_size]) + plt.gca().invert_xaxis() + + # Make arrows and x,y labels + self.ax.plot( + (0), + (0), + ls="", + marker="<", + ms=10, + color="k", + transform=self.ax.get_yaxis_transform(), + clip_on=False, + ) + self.ax.plot( + (0), + (1), + ls="", + marker="^", + ms=10, + color="k", + transform=self.ax.get_xaxis_transform(), + clip_on=False, + ) + self.ax.set_xlabel("$Y$", size=14, labelpad=-24, x=-0.02) + self.ax.set_ylabel("$X$", size=14, labelpad=-21, y=1.02, rotation=0) + + # Animation + ani = animation.FuncAnimation( + fig=fig, + func=self.animation_update, + frames=len(self._goal_heading_list) + 1, + interval=1, + ) + + # Add legend + self.ax.scatter([], [], marker="*", s=25, c="red", label="target goal") + marker, scale = gen_arrow_head_marker(np.pi / 2) + self.ax.scatter( + [], + [], + marker=marker, + s=(25 * scale) ** 1.5, + c="b", + label="robot pose (x, y, yaw) w/ goal_heading; delta_heading", + ) + plt.legend(loc=1) + + ani.save(filename=save_name, writer="pillow") + + def animation_update(self, frame): + # for each frame, update the data stored on each artist. + print(frame, len(self._goal_heading_list)) + if frame == 0: + return + goal_headings = self._goal_heading_list[:frame] + xyyaws = self._xyyaw_list[:frame] + goal_headings_delta = self._goal_heading_delta_list[:frame] + # update the scatter plot: + x_data = [v[1] for v in xyyaws] + y_data = [v[0] for v in xyyaws] + yaw_data = [v[2] for v in xyyaws] + # Plot the point + for i in range(len(x_data)): + marker, scale = gen_arrow_head_marker( + yaw_data[i] + np.pi / 2 + ) # for ploting, add 90 deg + self.ax.scatter( + x_data[i], + y_data[i], + marker=marker, + s=(25 * scale) ** 1.5, + c="b", + label="robot pose", + ) + # Add the goal heading text + self.ax.annotate( + str(int(np.rad2deg(goal_headings[i]))) + ";", + (x_data[i] - 0.1, y_data[i]), + fontsize=6, + color="black", + ) + # Add the delta heading text + self.ax.annotate( + str(round(np.rad2deg(goal_headings_delta[i]), 1)), + (x_data[i] - 0.8, y_data[i]), + fontsize=6, + color="green", + ) + + # Plot target goal + self.ax.scatter( + self._goal_xy[1], + self._goal_xy[0], + marker="*", + s=25, + c="red", + label="target goal", + ) + + +if __name__ == "__main__": + """ + X + 0 + ^ + | + | + | + Y pi/2<-------------- -pi/2 + | + | + | + """ + # Robot rotation in place + print("======Robot ratates in place========") + + for i, goal_xy in enumerate([[1, -1], [-1, 1], [-1, -1], [1, 1]]): + robot_sim = robot_simulation(goal_xy) + animation_list = [] + for x in range(-8, 12, 8): + for y in range(-8, 12, 8): + for yaw in range(-180, 180, 45): + _x, _y = point_in_circle(x, y, 1.5, np.deg2rad(yaw)) + animation_list.append([_x, _y, np.deg2rad(yaw)]) + + robot_sim.animate(animation_list, f"debug_{i}.gif") diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index d9659058..451eb7ad 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -599,8 +599,12 @@ def get_nav_success(observations, success_distance, success_angle): # Is the agent at the goal? dist_to_goal, _ = observations["target_point_goal_gps_and_compass_sensor"] at_goal = dist_to_goal < success_distance - good_heading = abs(observations["goal_heading"][0]) < success_angle - return at_goal and good_heading + goal_heading = abs(observations["goal_heading"][0]) + good_heading_suc = goal_heading < success_angle + print( + f"nav dis.: {success_distance} {dist_to_goal}; nav delta heading: {success_angle} {goal_heading}" + ) + return at_goal and good_heading_suc def print_nav_stats(self, observations): rho, theta = observations["target_point_goal_gps_and_compass_sensor"] diff --git a/spot_rl_experiments/spot_rl/envs/nav_env.py b/spot_rl_experiments/spot_rl/envs/nav_env.py index 2ffbd3ec..ce4098cf 100644 --- a/spot_rl_experiments/spot_rl/envs/nav_env.py +++ b/spot_rl_experiments/spot_rl/envs/nav_env.py @@ -3,6 +3,7 @@ # LICENSE file in the root directory of this source tree. +import magnum as mn import numpy as np from bosdyn.client.frame_helpers import get_a_tform_b from bosdyn.client.math_helpers import quat_to_eulerZYX @@ -15,6 +16,7 @@ def __init__(self, config, spot: Spot): super().__init__(config, spot) self._goal_xy = None self._enable_nav_by_hand = False + self._enable_dynamic_yaw = False self.goal_heading = None self.succ_distance = config.SUCCESS_DISTANCE self.succ_angle = np.deg2rad(config.SUCCESS_ANGLE_DIST) @@ -40,17 +42,27 @@ def disable_nav_by_hand(self): f"{self.node_name} Disabling nav goal change get_nav_observation by base fn restored" ) - def reset(self, goal_xy, goal_heading): + def reset(self, goal_xy, goal_heading, dynamic_yaw=False): self._goal_xy = np.array(goal_xy, dtype=np.float32) self.goal_heading = goal_heading observations = super().reset() + self._enable_dynamic_yaw = dynamic_yaw assert len(self._goal_xy) == 2 + if self._enable_dynamic_yaw: + self.succ_distance = self.config.SUCCESS_DISTANCE_FOR_DYNAMIC_YAW_NAV + self.succ_angle = np.deg2rad( + self.config.SUCCESS_ANGLE_DIST_FOR_DYNAMIC_YAW_NAV + ) + else: + self.succ_distance = self.config.SUCCESS_DISTANCE + self.succ_angle = np.deg2rad(self.config.SUCCESS_ANGLE_DIST) + return observations - def get_success(self, observations): + def get_success(self, observations, succ_set_base=True): succ = self.get_nav_success(observations, self.succ_distance, self.succ_angle) - if succ: + if succ and succ_set_base: self.spot.set_base_velocity(0.0, 0.0, 0.0, 1 / self.ctrl_hz) return succ @@ -94,5 +106,51 @@ def get_nav_observation_by_hand(self, goal_xy, goal_heading): return observations + def get_current_angle_for_target_facing(self): + vector_robot_to_target = self._goal_xy - np.array([self.x, self.y]) + vector_robot_to_target = vector_robot_to_target / np.linalg.norm( + vector_robot_to_target + ) + vector_forward_robot = np.array( + self.curr_transform.transform_vector(mn.Vector3(1, 0, 0)) + )[[0, 1]] + vector_forward_robot = vector_forward_robot / np.linalg.norm( + vector_forward_robot + ) + + return vector_robot_to_target, vector_forward_robot + def get_observations(self): + if self._enable_dynamic_yaw: + # Modify the goal_heading here based on the current robot orientation + ( + vector_robot_to_target, + vector_forward_robot, + ) = self.get_current_angle_for_target_facing() + x1 = ( + vector_robot_to_target[1] * vector_forward_robot[0] + - vector_robot_to_target[0] * vector_forward_robot[1] + ) + x2 = ( + vector_robot_to_target[0] * vector_forward_robot[0] + + vector_robot_to_target[1] * vector_forward_robot[1] + ) + rotation_delta = np.arctan2(x1, x2) + self.goal_heading = wrap_heading(self.yaw + rotation_delta) + return self.get_nav_observation(self._goal_xy, self.goal_heading) + + def step(self, *args, **kwargs): + observations, reward, done, info = super().step(*args, **kwargs) + + # Slow the base down if we are close to the nav target to slow down the the heading changes + dist_to_goal, _ = observations["target_point_goal_gps_and_compass_sensor"] + abs_good_heading = abs(observations["goal_heading"][0]) + + if self._enable_dynamic_yaw: + if dist_to_goal < 1.5 and abs_good_heading < np.rad2deg(45): + self.slowdown_base = 0.5 + else: + self.slowdown_base = -1 + + return observations, reward, done, info diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index f9e90d70..ebf92282 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -3,7 +3,7 @@ # LICENSE file in the root directory of this source tree. -from typing import Any, Dict, Tuple +from typing import Any, Dict, List, Tuple import magnum as mn import numpy as np @@ -231,8 +231,8 @@ def reset(self): # Reset the policies and environments via the controllers raise NotImplementedError - @multimethod # type: ignore - def nav(self, nav_target: str = None) -> Tuple[bool, str]: # type: ignore + @multimethod + def nav(self, nav_target: str = None) -> Tuple[bool, str]: """ Perform the nav action on the navigation target specified as a known string @@ -275,7 +275,7 @@ def nav( # noqa self, x: float, y: float, - theta=float, + theta: float, reset_current_receptacle_name: bool = True, ) -> Tuple[bool, str]: """ @@ -301,6 +301,26 @@ def nav( # noqa conditional_print(message=message, verbose=self.verbose) return status, message + @multimethod # type: ignore + def nav(self, x: float, y: float) -> Tuple[bool, str]: # noqa + """ + Perform the nav action on the navigation target with yaw specified as a metric location + Args: + x (float): x coordinate of the nav target (in meters) specified in the world frame + y (float): y coordinate of the nav target (in meters) specified in the world frame + Returns: + bool: True if navigation was successful, False otherwise + str: Message indicating the status of the navigation + """ + theta = 0.0 + goal_dict = { + "nav_target": (x, y, theta), + "dynamic_yaw": True, + } # type: Dict[str, Any] + status, message = self.nav_controller.execute(goal_dict=goal_dict) + conditional_print(message=message, verbose=self.verbose) + return status, message + def heuristic_mobile_gaze( self, x: float, @@ -496,7 +516,7 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray _, ) = detect_place_point_by_pcd_method( self.spot, - self.pick_config.GAZE_ARM_JOINT_ANGLES, + self.pick_config.SEMANTIC_PLACE_ARM_JOINT_ANGLES, percentile=0 if visualize else 70, visualize=visualize, height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, @@ -558,7 +578,7 @@ def contrainedplace(self, object_target: str = None, ee_orientation_at_grasping: object_target, proposition, self.spot, - self.pick_config.GAZE_ARM_JOINT_ANGLES, + self.pick_config.SEMANTIC_PLACE_ARM_JOINT_ANGLES, percentile=70, visualize=visualize, height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index 3fca6eb1..f7d2f1ae 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -343,9 +343,10 @@ def reset_skill(self, goal_dict: Dict[str, Any]) -> Any: # Reset the env and policy (goal_x, goal_y, goal_heading) = nav_target - observations = self.env.reset((goal_x, goal_y), goal_heading) + dynamic_yaw = goal_dict.get("dynamic_yaw") + dynamic_yaw = False if dynamic_yaw is None else dynamic_yaw + observations = self.env.reset((goal_x, goal_y), goal_heading, dynamic_yaw) self.policy.reset() - # Logging and Debug self.env.say(f"Navigating to {goal_dict['nav_target']}") @@ -358,19 +359,28 @@ def update_and_check_status(self, goal_dict: Dict[str, Any]) -> Tuple[bool, str] """Refer to class Skill for documentation""" self.env.say("Navigation Skill finished .. checking status") - nav_target = goal_dict["nav_target"] # safe to access as sanity check passed - # Make the angle from rad to deg - _nav_target_pose_deg = ( - nav_target[0], - nav_target[1], - np.rad2deg(nav_target[2]), - ) - check_navigation_success = is_pose_within_bounds( - self.skill_result_log.get("robot_trajectory")[-1].get("pose"), - _nav_target_pose_deg, - self.config.SUCCESS_DISTANCE, - self.config.SUCCESS_ANGLE_DIST, - ) + dynamic_yaw = goal_dict.get("dynamic_yaw") + dynamic_yaw = False if dynamic_yaw is None else dynamic_yaw + + if dynamic_yaw: + obs = self.env.get_observations() + check_navigation_success = self.env.get_success(obs, False) + else: + nav_target = goal_dict[ + "nav_target" + ] # safe to access as sanity check passed + # Make the angle from rad to deg + _nav_target_pose_deg = ( + nav_target[0], + nav_target[1], + np.rad2deg(nav_target[2]), + ) + check_navigation_success = is_pose_within_bounds( + self.skill_result_log.get("robot_trajectory")[-1].get("pose"), + _nav_target_pose_deg, + self.config.SUCCESS_DISTANCE, + self.config.SUCCESS_ANGLE_DIST, + ) # Update result log self.skill_result_log["time_taken"] = time.time() - self.start_time diff --git a/spot_rl_experiments/spot_rl/utils/gripper_T_intel.npy b/spot_rl_experiments/spot_rl/utils/gripper_T_intel.npy deleted file mode 100644 index f4f4ccf78776949cc3b2253b0a6ee51db38da492..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 256 zcmbR27wQ`j$;eQ~P_3SlTAW;@Zl$1ZlV+i=qoAIaUsO_*m=~X4l#&V(4=E~51qv5u zBo?Fsxf&)q3MQI53bhL41FrTPHwvBKythC7H)`)?z6JZwfBNmYc-{K_3{cQm6k8lDQ0*xBem0bG&(@eG!k$uD2|g?IrD-;?GvTvxn)wFm1{% ciK7$ttJ~+yPz%~-Kf!HnU;69iQ27t`0AZ9*h5!Hn diff --git a/spot_rl_experiments/spot_rl/utils/img_publishers.py b/spot_rl_experiments/spot_rl/utils/img_publishers.py index abd58416..e7ae2ac0 100644 --- a/spot_rl_experiments/spot_rl/utils/img_publishers.py +++ b/spot_rl_experiments/spot_rl/utils/img_publishers.py @@ -12,6 +12,7 @@ import blosc import cv2 +import magnum as mn import numpy as np import rospy from cv_bridge import CvBridge @@ -41,6 +42,10 @@ # owlvit from spot_rl.models import OwlVit +from spot_rl.utils.pixel_to_3d_conversion_utils import ( + get_3d_point, + sample_patch_around_point, +) from spot_rl.utils.stopwatch import Stopwatch from spot_rl.utils.utils import construct_config from spot_rl.utils.utils import ros_topics as rt @@ -48,6 +53,15 @@ MAX_PUBLISH_FREQ = 20 MAX_DEPTH = 3.5 MAX_HAND_DEPTH = 1.7 +DETECTIONS_BUFFER_LEN = 30 +LEFT_CROP = 124 +RIGHT_CROP = 60 +NEW_WIDTH = 228 +NEW_HEIGHT = 240 +ORIG_WIDTH = 640 +ORIG_HEIGHT = 480 +WIDTH_SCALE = 0.5 +HEIGHT_SCALE = 0.5 class SpotImagePublisher: @@ -445,6 +459,182 @@ def publish_viz_img(self, viz_img, header): self.pubs[self.viz_topic].publish(viz_img_msg) +class SpotOpenVocObjectDetectorPublisher(SpotProcessedImagesPublisher): + + name = "spot_open_voc_object_detector_publisher" + subscriber_topic = rt.HAND_RGB + # TODO: spot-sim2real: this is a hack since it publishes images in SpotProcessedImagesPublisher + publisher_topics = ["temp"] + + def __init__(self, model, spot): + super().__init__() + self.model = model + self.spot = spot + self.detection_topic = rt.OPEN_VOC_OBJECT_DETECTOR_TOPIC + + self.config = config = construct_config() + self.image_scale = config.IMAGE_SCALE + self.deblur_gan = get_deblurgan_model(config) + self.grayscale = self.config.GRAYSCALE_MASK_RCNN + + self.pubs[self.detection_topic] = rospy.Publisher( + self.detection_topic, String, queue_size=1, tcp_nodelay=True + ) + + # For the depth images + self.img_msg_depth = None + rospy.Subscriber(rt.HAND_DEPTH, Image, self.depth_cb, queue_size=1) + rospy.loginfo(f"[{self.name}]: is waiting for images...") + while self.img_msg_depth is None: + pass + rospy.loginfo(f"[{self.name}]: has received images!") + + def depth_cb(self, msg: Image): + self.img_msg_depth = msg + + def preprocess_image(self, img): + if self.image_scale != 1.0: + img = cv2.resize( + img, + (0, 0), + fx=self.image_scale, + fy=self.image_scale, + interpolation=cv2.INTER_AREA, + ) + + if self.deblur_gan is not None: + img = self.deblur_gan(cv2.cvtColor(img, cv2.COLOR_BGR2RGB)) + img = cv2.cvtColor(img, cv2.COLOR_RGB2BGR) + + if self.grayscale: + img = cv2.cvtColor(img, cv2.COLOR_BGR2GRAY) + img = cv2.cvtColor(img, cv2.COLOR_GRAY2BGR) + + return img + + def affordance_prediction( + self, + depth_raw: np.ndarray, + mask: np.ndarray, + camera_intrinsics, + center_pixel: np.ndarray, + ) -> np.ndarray: + """ + Accepts + depth_raw: np.array HXW, 0.-2000. + mask: HXW, bool mask + camera_intrinsics:spot camera intrinsic object + center_pixel: np.array of length 2 + Returns: Suitable point on object to grasp + """ + + mask = np.where(mask > 0, 1, 0).astype(depth_raw.dtype) + depth_image_masked = depth_raw * mask[...].astype(depth_raw.dtype) + + non_zero_indices = np.nonzero(depth_image_masked) + # Calculate the bounding box coordinates + y_min, y_max = non_zero_indices[0].min(), non_zero_indices[0].max() + x_min, x_max = non_zero_indices[1].min(), non_zero_indices[1].max() + cx, cy = (x_min + x_max) / 2.0, (y_min + y_max) / 2.0 + Z = float(sample_patch_around_point(int(cx), int(cy), depth_raw) * 1.0) + point_in_gripper = get_3d_point(camera_intrinsics, center_pixel, Z) + + return point_in_gripper + + def _publish(self): + stopwatch = Stopwatch() + header = self.img_msg.header + timestamp = header.stamp + hand_rgb = self.msg_to_cv2(self.img_msg) + arm_depth = self.msg_to_cv2(self.img_msg_depth) + arm_depth = arm_depth[:, LEFT_CROP:-RIGHT_CROP] + arm_depth = cv2.resize( + arm_depth, (NEW_WIDTH, NEW_HEIGHT), interpolation=cv2.INTER_AREA + ) + + # Get camera pose of view and the location of the robot + # These two should be fast and limited delay + imgs = self.spot.get_hand_image( + force_get_gripper=True + ) # for getting gripper intrinsics + # Get the camera intrinsics + cam_intrinsics = imgs[0].source.pinhole.intrinsics + + # Get the vision to hand + try: + vision_T_hand_image: mn.Matrix4 = self.spot.get_magnum_Matrix4_spot_a_T_b( + "vision", "hand_color_image_sensor", imgs[0].shot.transforms_snapshot + ) + except Exception as e: + print("ee:", imgs[0].shot.transforms_snapshot is None) + print(f"Cannot get Spot T due to {e}. Online detection might be off") + return + + # Internal model + hand_rgb_preprocessed = self.preprocess_image(hand_rgb) + new_detection, viz_img = self.model.inference( + hand_rgb_preprocessed, timestamp, stopwatch + ) + # Split the detection + timestamp, new_detections = new_detection.split("|") + new_detections = new_detections.split(";") + object_info = [] + for detection_str in new_detections: + if detection_str == "None": + continue + class_label, score, x1, y1, x2, y2 = detection_str.split(",") + # Compute the center pixel + x1, y1, x2, y2 = [ + int(float(i) / self.image_scale) for i in [x1, y1, x2, y2] + ] + pixel_x = int(np.mean([x1, x2])) + pixel_y = int(np.mean([y1, y2])) + # Get the distance between arm and the object + x1_distance = max(int(float(x1 - LEFT_CROP) * WIDTH_SCALE), 0) + x2_distance = max(int(float(x2 - LEFT_CROP) * WIDTH_SCALE), 0) + y1_distance = int(float(y1) * HEIGHT_SCALE) + y2_distance = int(float(y2) * HEIGHT_SCALE) + arm_depth_bbox = np.zeros_like(arm_depth, dtype=np.float32) + arm_depth_bbox[y1_distance:y2_distance, x1_distance:x2_distance] = 1.0 + # Estimate distance from the gripper to the object + depth_box = arm_depth[y1_distance:y2_distance, x1_distance:x2_distance] + if depth_box.size == 0: + continue + z = np.median(depth_box) / 255.0 * MAX_HAND_DEPTH + try: + point_in_gripper = self.affordance_prediction( + depth_raw=arm_depth / 255.0 * MAX_HAND_DEPTH, + mask=arm_depth_bbox, + camera_intrinsics=cam_intrinsics, + center_pixel=np.array([pixel_x, pixel_y]), + ) + except Exception as e: + print(f"Issue of predicting location: {e}") + continue + + if np.isnan(point_in_gripper).any(): + continue + + point_in_global_3d = vision_T_hand_image.transform_point( + mn.Vector3(*point_in_gripper) + ) + + object_info.append( + f"{class_label},{point_in_global_3d[0]},{point_in_global_3d[1]},{point_in_global_3d[2]}" + ) + print( + f"{class_label}: {point_in_global_3d} {x1} {y1} {x2} {y2} {pixel_x} {pixel_y} {z}" + ) + + # publish data + self.publish_new_detection(";".join(object_info)) + + stopwatch.print_stats() + + def publish_new_detection(self, new_object): + self.pubs[self.detection_topic].publish(new_object) + + class OWLVITModel: def __init__(self, score_threshold=0.1, show_img=False): self.config = config = construct_config() @@ -470,6 +660,30 @@ def inference(self, hand_rgb, timestamp, stopwatch): return detections_str, viz_img +class OWLVITModelMultiClasses(OWLVITModel): + def inference(self, hand_rgb, timestamp, stopwatch): + # Add new classes here to the model + # We decide to hardcode these classes first as this will be more robust + # and gives us the way to control the detection + # multi_classes = [["ball", "cup", "table", "cabinet", "chair", "sofa"]] + multi_classes = [["glass bottle"]] + self.owlvit.update_label(multi_classes) + # TODO: spot-sim2real: right now for each class, we only return the most confident detection + bbox_xy, viz_img = self.owlvit.run_inference_and_return_img(hand_rgb) + # bbox_xy is a list of [label_without_prefix, target_scores[label], [x1, y1, x2, y2]] + if bbox_xy is not None and bbox_xy != []: + detections = [] + for detection in bbox_xy: + str_det = f'{detection[0]},{detection[1]},{",".join([str(i) for i in detection[2]])}' + detections.append(str_det) + bbox_xy_string = ";".join(detections) + else: + bbox_xy_string = "None" + detections_str = f"{str(timestamp)}|{bbox_xy_string}" + + return detections_str, viz_img + + class MRCNNModel: def __init__(self): self.config = config = construct_config() @@ -494,6 +708,7 @@ def inference(self, hand_rgb, timestamp, stopwatch): parser.add_argument("--raw", action="store_true") parser.add_argument("--compress", action="store_true") parser.add_argument("--owlvit", action="store_true") + parser.add_argument("--open-voc", action="store_true") parser.add_argument("--mrcnn", action="store_true") parser.add_argument("--core", action="store_true", help="running on the Core") parser.add_argument("--listen", action="store_true", help="listening to Core") @@ -523,6 +738,7 @@ def inference(self, hand_rgb, timestamp, stopwatch): bounding_box_detector = args.bounding_box_detector mrcnn = args.mrcnn owlvit = args.owlvit + open_voc = args.open_voc node = None # type: Any model = None # type: Any @@ -539,6 +755,11 @@ def inference(self, hand_rgb, timestamp, stopwatch): rospy.set_param("enable_tracking", False) model = OWLVITModel() node = SpotBoundingBoxPublisher(model) + elif open_voc: + # Add open voc object detector here + spot = Spot("SpotOpenVocObjectDetectorPublisher") + model = OWLVITModelMultiClasses() + node = SpotOpenVocObjectDetectorPublisher(model, spot) elif decompress: node = SpotDecompressingRawImagesPublisher() elif raw or compress: @@ -564,6 +785,7 @@ def inference(self, hand_rgb, timestamp, stopwatch): flags.append("--decompress") elif local: flags.append("--raw") + flags.append("--open-voc") else: raise RuntimeError("This should be impossible.") cmds = [f"python {osp.abspath(__file__)} {flag}" for flag in flags] diff --git a/spot_rl_experiments/spot_rl/utils/skill_executor.py b/spot_rl_experiments/spot_rl/utils/skill_executor.py index d4a10b6b..e9b5f6d5 100644 --- a/spot_rl_experiments/spot_rl/utils/skill_executor.py +++ b/spot_rl_experiments/spot_rl/utils/skill_executor.py @@ -36,32 +36,64 @@ def execute_skills(self): # Get the current skill name skill_name, skill_input = get_skill_name_and_input_from_ros() - print(f"current skill_name {skill_name} skill_input {skill_input}") # Select the skill from the ros buffer and call the skill if skill_name == "nav": + print(f"current skill_name {skill_name} skill_input {skill_input}") # Reset the skill message self.reset_skill_msg() + # For navigation target + nav_target_xyz = rospy.get_param("nav_target_xyz", "None,None,None|") # Call the skill - succeded, msg = self.spotskillmanager.nav(skill_input) + if "None" not in nav_target_xyz: + nav_target_xyz = nav_target_xyz.split("|")[0:-1] + for nav_i, nav_target in enumerate(nav_target_xyz): + _nav_target = nav_target.split(",") + # This z and y are flipped due to hab convention + x, y = ( + float(_nav_target[0]), + float(_nav_target[2]), + ) + print(f"nav to {x} {y}, {nav_i+1}/{len(nav_target_xyz)}") + succeded, msg = self.spotskillmanager.nav(x, y) + if not succeded: + break + else: + succeded, msg = self.spotskillmanager.nav(skill_input) # Reset skill name and input and publish message self.reset_skill_name_input(skill_name, succeded, msg) + # Reset the navigation target + rospy.set_param("nav_target_xyz", "None,None,None|") elif skill_name == "pick": + print(f"current skill_name {skill_name} skill_input {skill_input}") self.reset_skill_msg() succeded, msg = self.spotskillmanager.pick(skill_input) self.reset_skill_name_input(skill_name, succeded, msg) elif skill_name == "place": + print(f"current skill_name {skill_name} skill_input {skill_input}") self.reset_skill_msg() - succeded, msg = self.spotskillmanager.place(skill_input) + # Use the following for the hardcode waypoint place + # succeded, msg = self.spotskillmanager.place(0.6, 0.0, 0.4, is_local=True) + # Call semantic place skills + succeded, msg = self.spotskillmanager.place( + None, is_local=True, visualize=False + ) self.reset_skill_name_input(skill_name, succeded, msg) elif skill_name == "opendrawer": + print(f"current skill_name {skill_name} skill_input {skill_input}") self.reset_skill_msg() succeded, msg = self.spotskillmanager.opendrawer() self.reset_skill_name_input(skill_name, succeded, msg) elif skill_name == "closedrawer": + print(f"current skill_name {skill_name} skill_input {skill_input}") self.reset_skill_msg() succeded, msg = self.spotskillmanager.closedrawer() self.reset_skill_name_input(skill_name, succeded, msg) + elif skill_name == "findagentaction": + print(f"current skill_name {skill_name} skill_input {skill_input}") + self.reset_skill_msg() + succeded, msg = True, rospy.get_param("human_state", "standing") + self.reset_skill_name_input(skill_name, succeded, msg) def main(): @@ -75,7 +107,6 @@ def main(): # Call the skill manager spotskillmanager = SpotSkillManager(use_mobile_pick=True, use_semantic_place=True) - executor = None try: executor = SpotRosSkillExecutor(spotskillmanager) From 6990b8f9f010b34b4e366c8a57a09c841d20b199 Mon Sep 17 00:00:00 2001 From: Tushar Sangam Date: Sat, 17 Aug 2024 13:17:21 -0700 Subject: [PATCH 04/82] torque based collision detection (#194) * torque visualizations * ,taking backup --- .../spot_rl/utils/collision_detector.py | 104 ++++++++++++++++++ 1 file changed, 104 insertions(+) create mode 100644 spot_rl_experiments/spot_rl/utils/collision_detector.py diff --git a/spot_rl_experiments/spot_rl/utils/collision_detector.py b/spot_rl_experiments/spot_rl/utils/collision_detector.py new file mode 100644 index 00000000..e3eb0b17 --- /dev/null +++ b/spot_rl_experiments/spot_rl/utils/collision_detector.py @@ -0,0 +1,104 @@ +import atexit + +import matplotlib.pyplot as plt +import numpy as np +import rospy +from matplotlib.animation import FuncAnimation + +spot = None +recorded_data = None +save_path: str = "joint_states_recording.npy" +len_of_torque_data: int = 0 + + +def get_arm_joints_from_spot(rename=True): + global spot + assert spot is not None, "Spot object not initialized" + joint_states = ( + spot.robot_state_client.get_robot_state().kinematic_state.joint_states + ) + arm_joint_names, arm_torque_values = [], [] + i = 1 + for jstate in joint_states: + if "arm0" in str(jstate.name): + jointname = str(jstate.name).split(".")[-1] + arm_joint_names.append("J_" + str(i)) if rename else arm_joint_names.append( + jointname + ) + arm_torque_values.append(float(jstate.load.value)) + i += 1 + assert len(arm_joint_names) > 0, "No arm joints were found in given joint_states" + return arm_joint_names, arm_torque_values + + +def dump_recorded_data(): + global recorded_data + global save_path + if recorded_data: + torques = np.array(list(recorded_data.values()), dtype=np.float32).reshape( + 8, -1 + ) + np.save(save_path, torques) + print(f"Data saved at {save_path}") + + +atexit.register(dump_recorded_data) + +if __name__ == "__main__": + from spot_wrapper.spot import Spot + + spot = Spot("arm collision detector") + + # Create the figure and axis objects + fig, axes = plt.subplots(nrows=9, ncols=1, figsize=(12, 12)) + + # Initialize the plot with some default data + joint_names, torques = get_arm_joints_from_spot(False) + bars = axes[0].bar(joint_names, torques) + + # Set the labels + axes[0].set_xlabel("Joint Name") + axes[0].set_ylabel("Torque (N·m)") + axes[0].set_ylim(0, 20) # Set an appropriate limit for your torques + torque_data = {i: [] for i in range(8)} # type: ignore + + max_len = 1000 + + for i in range(1, 8): + axes[i].set_xlabel("Time (s)") + axes[i].set_ylabel(f"{joint_names[i-1]}") + + # Function to update the plot + def update(frame): + # Plot the instant torque + global recorded_data + global len_of_torque_data + + _, torques = get_arm_joints_from_spot(False) + is_recording_on: bool = rospy.get_param("record_joints", False) + for bar, torque in zip(bars, torques): + bar.set_height(torque) + + # Plot the history of the troque + for i in range(len(torques)): + axes[i + 1].cla() + if len_of_torque_data > max_len: + torque_data[i] = [] + len_of_torque_data = 0 + torque_data[i].append(torques[i]) + len_of_torque_data += 1 + # torque_data[i] = torque_data[i][-max_len:] + index = [i for i in range(len(torque_data[i]))] + axes[i + 1].plot(index, torque_data[i]) + axes[i + 1].set_xlim([0, max_len]) + + fig.tight_layout() + + if is_recording_on: + recorded_data = torque_data + + # Create an animation + ani = FuncAnimation(fig, update, frames=range(100), blit=False, interval=100) + + # Display the plot + plt.show() From ceabab49d63006e6e88df394e204ea6c41c8742e Mon Sep 17 00:00:00 2001 From: Kavit Shah <59990215+KavitShah1998@users.noreply.github.com> Date: Wed, 28 Aug 2024 17:25:19 -0700 Subject: [PATCH 05/82] Load gripper_T_intel only for spot with intel service enabled (#198) --- bd_spot_wrapper/spot_wrapper/spot.py | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index fe6b6d70..27cd3311 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -215,9 +215,14 @@ def __init__(self, client_name_prefix): self.intelrealsense_image_client = robot.ensure_client( "intel-realsense-image-service" ) + self.gripper_T_intel: sp.SE3 = sp.SE3(np.load(GRIPPER_T_INTEL_PATH)) + print(f"Loaded gripper_T_intel (sp.SE3) as {self.gripper_T_intel.matrix()}") + except Exception: print("There is no intel-realsense-image_service. Using gripper cameras") self.intelrealsense_image_client = None + self.gripper_T_intel = None + print(f"Loaded gripper_T_intel (sp.SE3) as {self.gripper_T_intel}") self.manipulation_api_client = robot.ensure_client( ManipulationApiClient.default_service_name @@ -229,10 +234,6 @@ def __init__(self, client_name_prefix): InverseKinematicsClient.default_service_name ) - # TODO: Add safety net - self.gripper_T_intel: sp.SE3 = sp.SE3(np.load(GRIPPER_T_INTEL_PATH)) - print(f"Loaded gripper_T_intel (sp.SE3) as {self.gripper_T_intel.matrix()}") - # Used to re-center origin of global frame if osp.isfile(HOME_TXT): with open(HOME_TXT) as f: From 76319fee79ed782f09120043b27da4caf97663c7 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Thu, 29 Aug 2024 18:38:39 -0700 Subject: [PATCH 06/82] Heights obtained from Concept graph Json --- spot_rl_experiments/configs/config.yaml | 15 +- .../experiments/skill_test/test_sem_place.py | 62 +- .../spot_rl/envs/cfslam_object_relations.json | 1262 +++++++++++++++++ .../spot_rl/envs/skill_manager.py | 85 +- .../spot_rl/envs/test_cfslam.py | 29 + 5 files changed, 1410 insertions(+), 43 deletions(-) create mode 100644 spot_rl_experiments/spot_rl/envs/cfslam_object_relations.json create mode 100644 spot_rl_experiments/spot_rl/envs/test_cfslam.py diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index ba8eaf46..a032652a 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -19,6 +19,9 @@ WEIGHTS_TORCHSCRIPT: # Semantic place SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" + # Semantic place EE + SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" + # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -41,6 +44,9 @@ WEIGHTS: # Semantic place SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" + # Semantic place EE + SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" + # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -64,8 +70,6 @@ MAX_EPISODE_STEPS: 100 # Nav env SUCCESS_DISTANCE: 0.3 SUCCESS_ANGLE_DIST: 5 -SUCCESS_DISTANCE_FOR_DYNAMIC_YAW_NAV: 1.10 -SUCCESS_ANGLE_DIST_FOR_DYNAMIC_YAW_NAV: 20 DISABLE_OBSTACLE_AVOIDANCE: False USE_OA_FOR_NAV: True USE_HEAD_CAMERA: True @@ -101,11 +105,14 @@ PLACE_ACTION_SPACE_LENGTH: 4 SEMANTIC_PLACE_ACTION_SPACE_LENGTH: 9 SEMANTIC_PLACE_JOINT_BLACKLIST: [3] +# Semantic Place EE +SEMANTIC_PLACE_EE_ACTION_SPACE_LENGTH: 10 + # Open Close Drawer env MAX_LIN_DIST_OPEN_CLOSE_DRAWER: 0.0 MAX_ANG_DIST_OPEN_CLOSE_DRAWER: 3.73 # degrees OPEN_CLOSE_DRAWER_ACTION_SPACE_LENGTH: 8 -OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.75 +OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.3 # Semantic Pick IMG_SRC: 0 # 1 for intel & 0 for gripper @@ -136,7 +143,7 @@ INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_LEFT_HAND: [0, -180, 180, 0, 0, -90] # T INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_TOP_DOWN: [-0.91, -92.87, 91.21, 0, 90.01, 0] # The initial orientation of the arm for top down grasping # The old gaze ready angle: [0, -170, 120, 0, 75, 0] GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 75, 0] #[0, -160, 100, 0, 90, 0] -SEMANTIC_PLACE_ARM_JOINT_ANGLES: [0, -125, 80, 0, 85, 0] +GAZE_ARM_JOINT_ANGLES2: [0, -120, 60, 0, 88, 0] #[0, -160, 100, 0, 90, 0] PLACE_ARM_JOINT_ANGLES: [0, -170, 120, 0, 75, 0] ARM_LOWER_LIMITS: [-45, -180, 0, 0, -90, 0] ARM_UPPER_LIMITS: [45, 0, 180, 0, 90, 0] diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index 58743e77..e413d72a 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -1,6 +1,13 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. +# mypy: ignore-errors +# black: ignore-errors +import json +import time +from datetime import datetime + +import numpy as np import rospy from perception_and_utils.utils.generic_utils import map_user_input_to_boolean from spot_rl.envs.skill_manager import SpotSkillManager @@ -14,25 +21,74 @@ if in_fre_lab: # at FRE - place_target = "coffee_table" + place_target = "office chair" else: # at NYC place_target = "test_desk" spotskillmanager = SpotSkillManager(use_mobile_pick=False, use_semantic_place=True) + spot_pos1 = spotskillmanager.spot.get_arm_joint_positions(as_array=True) + + # is_local = False + # if enable_estimation_before_place: + # place_target = None + # is_local = True + # # Start testing + # contnue = True + # while contnue: + # rospy.set_param("is_gripper_blocked", 0) + # spotskillmanager.place(place_target, is_local=is_local, visualize=True) + # contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") is_local = False if enable_estimation_before_place: + height_target = place_target place_target = None is_local = True # Start testing contnue = True + INITIAL_ARM_JOINT_ANGLES = [0, -180, 180, 90, 0, -90] + episode_ctr = 0 + # Get EE Pose Initial + spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() + # Set Orientation as Zero + spot_ort = np.zeros(3) while contnue: + # Open Gripper + spotskillmanager.spot.open_gripper() + input("Place an object in Spot's gripper and press Enter to continue...") + # Place Object and Close Gripper rospy.set_param("is_gripper_blocked", 0) - spotskillmanager.place(place_target, is_local=is_local, visualize=True) - contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") + episode_log = {"actions": []} # mypy: ignore-errors + spotskillmanager.spot.close_gripper() + input("waiting for user to get ready with camera") + if place_target is None: + spotskillmanager.place( + place_target, height_target, is_local=is_local, visualize=True + ) + else: + spotskillmanager.place( + place_target, None, is_local=is_local, visualize=True + ) + skill_log = spotskillmanager.place_controller.skill_result_log + if "num_steps" not in skill_log: + + skill_log["num_steps"] = 0 + episode_log["actions"].append({"place": skill_log}) + curr_date = datetime.today().strftime("%m-%d-%y") + file_path = ( + f"logs/semantic_place/{curr_date}/episode_sem_pl_run2_{episode_ctr}.json" + ) + with open(file_path, "w") as file: + json.dump(episode_log, file, indent=4) + print(f"Saved log: {file_path}") + episode_ctr += 1 + contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") + # Return the arm to the original position + spot_pos = spotskillmanager.spot.get_ee_pos_in_body_frame()[0] + spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) # The following is a helpful tip to debug the arm # We get Spot class # spot = spotskillmanager.spot diff --git a/spot_rl_experiments/spot_rl/envs/cfslam_object_relations.json b/spot_rl_experiments/spot_rl/envs/cfslam_object_relations.json new file mode 100644 index 00000000..3afecefa --- /dev/null +++ b/spot_rl_experiments/spot_rl/envs/cfslam_object_relations.json @@ -0,0 +1,1262 @@ +[ + { + "object1": { + "id": 0, + "bbox_extent": [ + 0.8, + 0.2, + 0.2 + ], + "bbox_center": [ + 2.6, + -1.1, + -0.1 + ], + "object_tag": "fabric", + "category_tag": "object" + }, + "object2": { + "id": 5, + "bbox_extent": [ + 0.5, + 0.2, + 0.2 + ], + "bbox_center": [ + 2.3, + -1.5, + -0.1 + ], + "object_tag": "furniture", + "category_tag": "furniture" + }, + "object_relation": "a next to b", + "reason": "The objects are placed close to each other on the floor, with their centers at similar heights. Object1 is a fabric object and object2 is a furniture, so it's likely they're placed next to each other in a living room setting.", + "room_region": "living room" + }, + { + "object1": { + "id": 1, + "bbox_extent": [ + 1.1, + 1.1, + 0.6 + ], + "bbox_center": [ + 2.1, + -1.6, + 0.3 + ], + "object_tag": "white dresser", + "category_tag": "furniture" + }, + "object2": { + "id": 4, + "bbox_extent": [ + 0.2, + 0.1, + 0.1 + ], + "bbox_center": [ + 2.1, + -1.5, + -0.1 + ], + "object_tag": "piece of wood", + "category_tag": "object" + }, + "object_relation": "b on a", + "reason": "The object_relation is 'b on a' because the center of object2 (a piece of wood) is below and close to the center of object1 (a white dresser), suggesting it might be placed on top. The room_region is 'bedroom' because the bbox_center of both objects are near the floor (z-coordinate is low) and the object_tag of object1 suggests a bedroom furniture.", + "room_region": "bedroom" + }, + { + "object1": { + "id": 1, + "bbox_extent": [ + 1.1, + 1.1, + 0.6 + ], + "bbox_center": [ + 2.1, + -1.6, + 0.3 + ], + "object_tag": "white dresser", + "category_tag": "furniture" + }, + "object2": { + "id": 5, + "bbox_extent": [ + 0.5, + 0.2, + 0.2 + ], + "bbox_center": [ + 2.3, + -1.5, + -0.1 + ], + "object_tag": "furniture", + "category_tag": "furniture" + }, + "object_relation": "a next to b", + "reason": "Object 1 is a dresser which typically stands on the floor, and object 2's category tag is also furniture. Their bounding box centers are close in x and y coordinates but have different z coordinates, indicating they might be placed side by side. The room region is likely a bedroom as dressers are commonly found there.", + "room_region": "bedroom" + }, + { + "object1": { + "id": 2, + "bbox_extent": [ + 1.1, + 0.7, + 0.3 + ], + "bbox_center": [ + 2.4, + -1.2, + -0.3 + ], + "object_tag": "skateboard", + "category_tag": "object" + }, + "object2": { + "id": 5, + "bbox_extent": [ + 0.5, + 0.2, + 0.2 + ], + "bbox_center": [ + 2.3, + -1.5, + -0.1 + ], + "object_tag": "furniture", + "category_tag": "furniture" + }, + "object_relation": "a next to b", + "reason": "Object 1 is a skateboard and object 2 is furniture, suggesting they are not related by containment. Their centers are close together in the X-Y plane but have different heights (z-coordinates), indicating they might be placed side-by-side. The object tags do not suggest a specific room region, so we default to 'living room' as it is a common area where both objects could be found.", + "room_region": "living room" + }, + { + "object1": { + "id": 2, + "bbox_extent": [ + 1.1, + 0.7, + 0.3 + ], + "bbox_center": [ + 2.4, + -1.2, + -0.3 + ], + "object_tag": "skateboard", + "category_tag": "object" + }, + "object2": { + "id": 34, + "bbox_extent": [ + 1.3, + 0.4, + 0.3 + ], + "bbox_center": [ + 2.6, + -0.6, + -0.0 + ], + "object_tag": "black fabric", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The two objects are placed close together, but not on top of or inside each other. The 'skateboard' and 'black fabric' tags suggest a living room setting where these items may be found.", + "room_region": "living room" + }, + { + "object1": { + "id": 3, + "bbox_extent": [ + 0.8, + 0.2, + 0.2 + ], + "bbox_center": [ + 2.1, + -1.5, + 0.4 + ], + "object_tag": "drawer", + "category_tag": "furniture" + }, + "object2": { + "id": 33, + "bbox_extent": [ + 0.9, + 0.7, + 0.3 + ], + "bbox_center": [ + 2.6, + -1.1, + 0.2 + ], + "object_tag": "black curtain", + "category_tag": "object" + }, + "object_relation": "none of these", + "reason": "Object1 is a drawer and Object2 is a black curtain. Since the bbox_center of both objects are close to each other (z-coordinate is low, indicating they are near the floor), it's likely they are in the same room. The object_tag 'drawer' suggests that it might be part of a furniture like a dresser or cabinet, which can be found in a living room. The relationship between them doesn't fit any of the described options.", + "room_region": "living room" + }, + { + "object1": { + "id": 7, + "bbox_extent": [ + 1.1, + 0.5, + 0.2 + ], + "bbox_center": [ + 1.8, + -1.4, + -0.2 + ], + "object_tag": "black suitcase", + "category_tag": "object" + }, + "object2": { + "id": 5, + "bbox_extent": [ + 0.5, + 0.2, + 0.2 + ], + "bbox_center": [ + 2.3, + -1.5, + -0.1 + ], + "object_tag": "furniture", + "category_tag": "furniture" + }, + "object_relation": "a next to b", + "reason": "Object 1 is a black suitcase and object 2 is a furniture, which are commonly found in bedrooms. Their bounding box centers are also close to each other, suggesting they are likely placed next to each other.", + "room_region": "bedroom" + }, + { + "object1": { + "id": 8, + "bbox_extent": [ + 0.6, + 0.4, + 0.2 + ], + "bbox_center": [ + 1.8, + -1.7, + -0.1 + ], + "object_tag": "carpet", + "category_tag": "furniture" + }, + "object2": { + "id": 2, + "bbox_extent": [ + 1.1, + 0.7, + 0.3 + ], + "bbox_center": [ + 2.4, + -1.2, + -0.3 + ], + "object_tag": "skateboard", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The 'carpet' and 'skateboard' are in close proximity, suggesting they may be placed side by side. Given the object tags, it's likely they're in a living room where such objects are commonly found.", + "room_region": "living room" + }, + { + "object1": { + "id": 10, + "bbox_extent": [ + 1.0, + 0.3, + 0.2 + ], + "bbox_center": [ + 1.0, + -1.5, + -0.2 + ], + "object_tag": "yellow tag", + "category_tag": "object" + }, + "object2": { + "id": 7, + "bbox_extent": [ + 1.1, + 0.5, + 0.2 + ], + "bbox_center": [ + 1.8, + -1.4, + -0.2 + ], + "object_tag": "black suitcase", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "Object1 (yellow tag) and object2 (black suitcase) are both objects, not furniture. They are close in proximity but not on top of or inside each other, so they must be next to each other. Their centers are near the floor (-1.5 and -1.4 respectively), which is common for living room objects.", + "room_region": "living room" + }, + { + "object1": { + "id": 10, + "bbox_extent": [ + 1.0, + 0.3, + 0.2 + ], + "bbox_center": [ + 1.0, + -1.5, + -0.2 + ], + "object_tag": "yellow tag", + "category_tag": "object" + }, + "object2": { + "id": 12, + "bbox_extent": [ + 1.0, + 0.5, + 0.2 + ], + "bbox_center": [ + 0.5, + -1.4, + -0.2 + ], + "object_tag": "suitcase", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The objects are not on top of each other and their bounding box centers are close but not inside each other. They are likely placed side by side, which is common in a hallway. The object tags 'yellow tag' and 'suitcase' do not provide any specific room region information, so the hallway is chosen as it's a common area where objects can be placed next to each other.", + "room_region": "hallway" + }, + { + "object1": { + "id": 12, + "bbox_extent": [ + 1.0, + 0.5, + 0.2 + ], + "bbox_center": [ + 0.5, + -1.4, + -0.2 + ], + "object_tag": "suitcase", + "category_tag": "object" + }, + "object2": { + "id": 9, + "bbox_extent": [ + 0.6, + 0.3, + 0.2 + ], + "bbox_center": [ + 1.1, + -1.5, + -0.2 + ], + "object_tag": "yellow tag", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The objects are both categorized as 'objects', and their bounding box centers suggest they are placed near each other, but not in a typical on-top or inside relationship. Their tags also don't imply any specific room region, so I'm defaulting to the hallway.", + "room_region": "hallway" + }, + { + "object1": { + "id": 31, + "bbox_extent": [ + 0.7, + 0.2, + 0.1 + ], + "bbox_center": [ + 2.1, + -1.5, + 0.4 + ], + "object_tag": "drawer with gold handles", + "category_tag": "furniture" + }, + "object2": { + "id": 33, + "bbox_extent": [ + 0.9, + 0.7, + 0.3 + ], + "bbox_center": [ + 2.6, + -1.1, + 0.2 + ], + "object_tag": "black curtain", + "category_tag": "object" + }, + "object_relation": "none of these", + "reason": "Object1 is a drawer with gold handles and object2 is a black curtain, they do not fit any specific relation. The center coordinates of the two objects are close to each other, which suggests they might be in the same room. The height of the objects (0.4 and 0.2) indicates that they are likely on the floor or very close to it, which is common in a living room.", + "room_region": "living room" + }, + { + "object1": { + "id": 33, + "bbox_extent": [ + 0.9, + 0.7, + 0.3 + ], + "bbox_center": [ + 2.6, + -1.1, + 0.2 + ], + "object_tag": "black curtain", + "category_tag": "object" + }, + "object2": { + "id": 2, + "bbox_extent": [ + 1.1, + 0.7, + 0.3 + ], + "bbox_center": [ + 2.4, + -1.2, + -0.3 + ], + "object_tag": "skateboard", + "category_tag": "object" + }, + "object_relation": "none of these", + "reason": "The relationship between a black curtain and a skateboard is unclear as they are not typically placed in relation to each other. Additionally, the room region cannot be determined with certainty due to the lack of context.", + "room_region": "unknown" + }, + { + "object1": { + "id": 33, + "bbox_extent": [ + 0.9, + 0.7, + 0.3 + ], + "bbox_center": [ + 2.6, + -1.1, + 0.2 + ], + "object_tag": "black curtain", + "category_tag": "object" + }, + "object2": { + "id": 30, + "bbox_extent": [ + 1.1, + 0.8, + 0.4 + ], + "bbox_center": [ + 2.2, + -1.6, + 0.2 + ], + "object_tag": "white dresser", + "category_tag": "furniture" + }, + "object_relation": "none of these", + "reason": "The object relation is none of these because the black curtain and white dresser are not typically placed on or inside each other. The room region is bedroom because both objects have a low z-coordinate in their bbox_center, indicating they are close to the ground, and the white dresser suggests it might be used for storing clothes.", + "room_region": "bedroom" + }, + { + "object1": { + "id": 33, + "bbox_extent": [ + 0.9, + 0.7, + 0.3 + ], + "bbox_center": [ + 2.6, + -1.1, + 0.2 + ], + "object_tag": "black curtain", + "category_tag": "object" + }, + "object2": { + "id": 32, + "bbox_extent": [ + 0.8, + 0.5, + 0.3 + ], + "bbox_center": [ + 2.1, + -1.5, + 0.4 + ], + "object_tag": "white drawer", + "category_tag": "furniture" + }, + "object_relation": "none of these", + "reason": "Object1 is a curtain and Object2 is a drawer, but they don't have any typical spatial relationship. They are both in the bedroom region because the object tags suggest it.", + "room_region": "bedroom" + }, + { + "object1": { + "id": 13, + "bbox_extent": [ + 0.7, + 0.4, + 0.3 + ], + "bbox_center": [ + -0.6, + -1.6, + 0.4 + ], + "object_tag": "office chair", + "category_tag": "furniture" + }, + "object2": { + "id": 56, + "bbox_extent": [ + 0.2, + 0.1, + 0.1 + ], + "bbox_center": [ + -0.5, + -1.5, + 0.4 + ], + "object_tag": "yellowed paper", + "category_tag": "object" + }, + "object_relation": "b on a", + "reason": "The object2 'yellowed paper' is much smaller than the object1 'office chair', and their centers are very close, suggesting that the paper could be placed on top of or near the chair. The room region is determined as an office due to the presence of an office chair.", + "room_region": "office" + }, + { + "object1": { + "id": 14, + "bbox_extent": [ + 0.6, + 0.6, + 0.4 + ], + "bbox_center": [ + -0.6, + -1.6, + 0.4 + ], + "object_tag": "black office chair", + "category_tag": "furniture" + }, + "object2": { + "id": 53, + "bbox_extent": [ + 0.7, + 0.7, + 0.5 + ], + "bbox_center": [ + -0.6, + -1.5, + 0.0 + ], + "object_tag": "black office chair", + "category_tag": "furniture" + }, + "object_relation": "none of these", + "reason": "Both objects are black office chairs and their bounding box centers are very close, but there is no indication that one is placed on top or inside the other. The room region is likely an office since both objects are office chairs.", + "room_region": "office" + }, + { + "object1": { + "id": 14, + "bbox_extent": [ + 0.6, + 0.6, + 0.4 + ], + "bbox_center": [ + -0.6, + -1.6, + 0.4 + ], + "object_tag": "black office chair", + "category_tag": "furniture" + }, + "object2": { + "id": 56, + "bbox_extent": [ + 0.2, + 0.1, + 0.1 + ], + "bbox_center": [ + -0.5, + -1.5, + 0.4 + ], + "object_tag": "yellowed paper", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "Object1 is a chair and object2 is a piece of paper, which are often placed next to each other in an office setting. The bbox centers of both objects are close to the ground (z=0.4), indicating they are on the floor or on a low surface, which supports this relationship.", + "room_region": "office" + }, + { + "object1": { + "id": 15, + "bbox_extent": [ + 0.2, + 0.2, + 0.2 + ], + "bbox_center": [ + -0.6, + -0.1, + -0.1 + ], + "object_tag": "small electronic device", + "category_tag": "object" + }, + "object2": { + "id": 17, + "bbox_extent": [ + 0.3, + 0.3, + 0.2 + ], + "bbox_center": [ + -0.6, + -0.1, + -0.0 + ], + "object_tag": "small electronic device", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "Object 1 and Object 2 are both small electronic devices with similar sizes and centers close together, suggesting they are placed side by side. Their centers' z-coordinates indicate they are on a surface around 0-10 cm above the ground, which is common for living room furniture or shelves.", + "room_region": "living room" + }, + { + "object1": { + "id": 16, + "bbox_extent": [ + 0.5, + 0.2, + 0.1 + ], + "bbox_center": [ + -1.3, + -0.5, + -0.2 + ], + "object_tag": "twisted electrical wire", + "category_tag": "object" + }, + "object2": { + "id": 55, + "bbox_extent": [ + 0.9, + 0.7, + 0.3 + ], + "bbox_center": [ + -1.4, + -0.9, + 0.2 + ], + "object_tag": "pole or wand", + "category_tag": "object" + }, + "object_relation": "none of these", + "reason": "The objects are both categorized as objects and have no clear spatial relationship, with their bounding box centers not indicating a specific room region.", + "room_region": "unknown" + }, + { + "object1": { + "id": 18, + "bbox_extent": [ + 1.1, + 0.4, + 0.1 + ], + "bbox_center": [ + -1.4, + -1.2, + 0.3 + ], + "object_tag": "decorative stick", + "category_tag": "object" + }, + "object2": { + "id": 55, + "bbox_extent": [ + 0.9, + 0.7, + 0.3 + ], + "bbox_center": [ + -1.4, + -0.9, + 0.2 + ], + "object_tag": "pole or wand", + "category_tag": "object" + }, + "object_relation": "none of these", + "reason": "The objects are both categorized as 'objects' and have similar heights off the ground, but their object tags do not suggest a common relationship or room region. The bounding box centers and extents also do not provide any clear indication of their relationship or room region.", + "room_region": "unknown" + }, + { + "object1": { + "id": 35, + "bbox_extent": [ + 1.3, + 0.2, + 0.1 + ], + "bbox_center": [ + -1.4, + -0.2, + -0.2 + ], + "object_tag": "monitor", + "category_tag": "object" + }, + "object2": { + "id": 52, + "bbox_extent": [ + 0.8, + 0.4, + 0.1 + ], + "bbox_center": [ + -1.3, + -0.3, + -0.3 + ], + "object_tag": "electrical cord", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The object1 is a monitor and the object2 is an electrical cord, which are commonly placed together near each other. The bbox_center of both objects has negative x, y, and z coordinates indicating they are likely on the floor or on a table in a living room.", + "room_region": "living room" + }, + { + "object1": { + "id": 35, + "bbox_extent": [ + 1.3, + 0.2, + 0.1 + ], + "bbox_center": [ + -1.4, + -0.2, + -0.2 + ], + "object_tag": "monitor", + "category_tag": "object" + }, + "object2": { + "id": 55, + "bbox_extent": [ + 0.9, + 0.7, + 0.3 + ], + "bbox_center": [ + -1.4, + -0.9, + 0.2 + ], + "object_tag": "pole or wand", + "category_tag": "object" + }, + "object_relation": "none of these", + "reason": "The objects 'monitor' and 'pole or wand' do not have a clear relationship, as they are not typically placed on or inside each other. Additionally, their bounding box centers do not suggest a specific room region.", + "room_region": "unknown" + }, + { + "object1": { + "id": 54, + "bbox_extent": [ + 0.6, + 0.2, + 0.1 + ], + "bbox_center": [ + -1.4, + -1.4, + -0.3 + ], + "object_tag": "blue line", + "category_tag": "object" + }, + "object2": { + "id": 55, + "bbox_extent": [ + 0.9, + 0.7, + 0.3 + ], + "bbox_center": [ + -1.4, + -0.9, + 0.2 + ], + "object_tag": "pole or wand", + "category_tag": "object" + }, + "object_relation": "none of these", + "reason": "The objects' bounding box centers are close in the XY plane but not in the Z axis. Object1 is an object and Object2 is also an object, but they do not fit any specific relationship category. Their object tags 'blue line' and 'pole or wand' do not give any clear indication of their usage or placement in a room.", + "room_region": "unknown" + }, + { + "object1": { + "id": 19, + "bbox_extent": [ + 0.6, + 0.5, + 0.1 + ], + "bbox_center": [ + -0.5, + 0.3, + -0.3 + ], + "object_tag": "computer monitor", + "category_tag": "object" + }, + "object2": { + "id": 22, + "bbox_extent": [ + 1.3, + 1.1, + 0.3 + ], + "bbox_center": [ + -0.6, + 0.7, + -0.2 + ], + "object_tag": "collection of cords", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "Object1 (computer monitor) is commonly placed on a table or desk, and object2 (collection of cords) is often placed beside it. The bbox_center coordinates suggest they are close to each other on the floor plane, indicating they might be in a living room.", + "room_region": "living room" + }, + { + "object1": { + "id": 19, + "bbox_extent": [ + 0.6, + 0.5, + 0.1 + ], + "bbox_center": [ + -0.5, + 0.3, + -0.3 + ], + "object_tag": "computer monitor", + "category_tag": "object" + }, + "object2": { + "id": 36, + "bbox_extent": [ + 0.6, + 0.3, + 0.2 + ], + "bbox_center": [ + -0.2, + 0.1, + -0.2 + ], + "object_tag": "small electronic device", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The objects are both categorized as objects and their centers are relatively close, suggesting they might be placed next to each other. The object tags 'computer monitor' and 'small electronic device' hint at a possible location in the living room.", + "room_region": "living room" + }, + { + "object1": { + "id": 20, + "bbox_extent": [ + 0.2, + 0.1, + 0.1 + ], + "bbox_center": [ + -0.2, + -0.1, + -0.1 + ], + "object_tag": "electronic device", + "category_tag": "object" + }, + "object2": { + "id": 36, + "bbox_extent": [ + 0.6, + 0.3, + 0.2 + ], + "bbox_center": [ + -0.2, + 0.1, + -0.2 + ], + "object_tag": "small electronic device", + "category_tag": "object" + }, + "object_relation": "a on b", + "reason": "Object1 is a small electronic device and object2 is also an electronic device, so it's likely that object1 is placed on top of object2. Both objects have their centers close to the ground (z-coordinate), which suggests they are in a living room.", + "room_region": "living room" + }, + { + "object1": { + "id": 21, + "bbox_extent": [ + 0.4, + 0.2, + 0.1 + ], + "bbox_center": [ + -1.0, + 0.7, + -0.3 + ], + "object_tag": "yellow wire", + "category_tag": "object" + }, + "object2": { + "id": 22, + "bbox_extent": [ + 1.3, + 1.1, + 0.3 + ], + "bbox_center": [ + -0.6, + 0.7, + -0.2 + ], + "object_tag": "collection of cords", + "category_tag": "object" + }, + "object_relation": "none of these", + "reason": "Since both objects are categorized as 'object' and have similar height (z-coordinate), it's unclear what their relationship is or which room region they belong to. Object1 has a smaller extent, but its center is farther away from the origin than object2. Without more context, we cannot determine their relation or room region.", + "room_region": "unknown" + }, + { + "object1": { + "id": 23, + "bbox_extent": [ + 0.6, + 0.4, + 0.1 + ], + "bbox_center": [ + -0.1, + 0.8, + 0.5 + ], + "object_tag": "yellow object", + "category_tag": "object" + }, + "object2": { + "id": 43, + "bbox_extent": [ + 0.7, + 0.3, + 0.1 + ], + "bbox_center": [ + 0.2, + 0.9, + 0.6 + ], + "object_tag": "wall", + "category_tag": "furniture" + }, + "object_relation": "none of these", + "reason": "The object tags and categories indicate that 'yellow object' is an object and 'wall' is a furniture. The bbox_center coordinates suggest that they are both at a certain height off the ground, but there's no clear indication of one being on or in the other. Based on their positions, it seems like they could be in a living room.", + "room_region": "living room" + }, + { + "object1": { + "id": 42, + "bbox_extent": [ + 0.7, + 0.5, + 0.2 + ], + "bbox_center": [ + -0.3, + 0.8, + 0.7 + ], + "object_tag": "yellow object", + "category_tag": "object" + }, + "object2": { + "id": 43, + "bbox_extent": [ + 0.7, + 0.3, + 0.1 + ], + "bbox_center": [ + 0.2, + 0.9, + 0.6 + ], + "object_tag": "wall", + "category_tag": "furniture" + }, + "object_relation": "none of these", + "reason": "Object1 is an object and Object2 is a wall, which are not commonly placed in specific relationships with each other. The room region cannot be determined as the objects do not provide clear indication of the room they are in.", + "room_region": "unknown" + }, + { + "object1": { + "id": 24, + "bbox_extent": [ + 0.4, + 0.3, + 0.2 + ], + "bbox_center": [ + 2.2, + 0.7, + 0.1 + ], + "object_tag": "suitcase", + "category_tag": "object" + }, + "object2": { + "id": 25, + "bbox_extent": [ + 0.7, + 0.6, + 0.4 + ], + "bbox_center": [ + 2.3, + 0.6, + 0.5 + ], + "object_tag": "black case", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The objects are both categorized as 'objects' and their centers are close in the x-y plane, indicating they may be placed side by side. The z-coordinates of their centers are relatively low, suggesting they are on or near the floor. Additionally, the object tags 'suitcase' and 'black case' are commonly found in bedrooms.", + "room_region": "bedroom" + }, + { + "object1": { + "id": 24, + "bbox_extent": [ + 0.4, + 0.3, + 0.2 + ], + "bbox_center": [ + 2.2, + 0.7, + 0.1 + ], + "object_tag": "suitcase", + "category_tag": "object" + }, + "object2": { + "id": 26, + "bbox_extent": [ + 0.6, + 0.3, + 0.2 + ], + "bbox_center": [ + 2.3, + 0.6, + 0.1 + ], + "object_tag": "black suitcase", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The two objects are both suitcases and have similar sizes, with centers very close to each other (difference in x-coordinate is 0.1). They also have the same height off of the ground (z-coordinate of 0.1), which suggests they are placed side by side on the floor. Given their object tags, it's likely they're both located in a hallway.", + "room_region": "hallway" + }, + { + "object1": { + "id": 25, + "bbox_extent": [ + 0.7, + 0.6, + 0.4 + ], + "bbox_center": [ + 2.3, + 0.6, + 0.5 + ], + "object_tag": "black case", + "category_tag": "object" + }, + "object2": { + "id": 38, + "bbox_extent": [ + 0.6, + 0.5, + 0.3 + ], + "bbox_center": [ + 2.4, + 0.6, + 0.9 + ], + "object_tag": "black case/speaker", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The centers of the objects are very close in x and y coordinates, indicating they are likely placed side by side. The z coordinates are also relatively low, suggesting they are on or near the floor. Additionally, both object tags mention 'case', which is often associated with living room electronics.", + "room_region": "living room" + }, + { + "object1": { + "id": 27, + "bbox_extent": [ + 1.0, + 0.2, + 0.1 + ], + "bbox_center": [ + 2.6, + 0.1, + 0.2 + ], + "object_tag": "black pole", + "category_tag": "object" + }, + "object2": { + "id": 29, + "bbox_extent": [ + 0.2, + 0.1, + 0.1 + ], + "bbox_center": [ + 2.6, + 0.0, + 0.5 + ], + "object_tag": "chair", + "category_tag": "furniture" + }, + "object_relation": "none of these", + "reason": "Object1 is a black pole and object2 is a chair. There are no obvious signs that one object is on, in or next to the other. The bbox_center coordinates do not provide any specific information about the room region.", + "room_region": "unknown" + }, + { + "object1": { + "id": 46, + "bbox_extent": [ + 0.3, + 0.2, + 0.0 + ], + "bbox_center": [ + -0.7, + -3.5, + 0.0 + ], + "object_tag": "spider web", + "category_tag": "object" + }, + "object2": { + "id": 48, + "bbox_extent": [ + 0.6, + 0.3, + 0.2 + ], + "bbox_center": [ + -0.8, + -3.4, + -0.0 + ], + "object_tag": "wooden furniture", + "category_tag": "furniture" + }, + "object_relation": "none of these", + "reason": "The object1 is a spider web and the object2 is wooden furniture, they do not have any direct relationship with each other. The bbox_center of both objects indicates that they are close to the floor and in the same general area, but their categories and tags do not suggest a specific relation. Based on the bbox_center, it seems likely that they are in a living room.", + "room_region": "living room" + }, + { + "object1": { + "id": 49, + "bbox_extent": [ + 0.2, + 0.2, + 0.1 + ], + "bbox_center": [ + 1.7, + -2.6, + 0.3 + ], + "object_tag": "button", + "category_tag": "object" + }, + "object2": { + "id": 50, + "bbox_extent": [ + 0.3, + 0.2, + 0.1 + ], + "bbox_center": [ + 1.8, + -2.6, + 0.4 + ], + "object_tag": "black leather case", + "category_tag": "object" + }, + "object_relation": "a next to b", + "reason": "The objects are close in proximity (bbox_center of object1: [1.7, -2.6, 0.3], bbox_center of object2: [1.8, -2.6, 0.4]) and are both categorized as objects, suggesting they might be placed next to each other on a surface such as a table or shelf in a living room.", + "room_region": "living room" + } +] \ No newline at end of file diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index ebf92282..84e8fff5 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -3,20 +3,23 @@ # LICENSE file in the root directory of this source tree. -from typing import Any, Dict, List, Tuple +from typing import Any, Dict, Tuple import magnum as mn import numpy as np import rospy from multimethod import multimethod from perception_and_utils.utils.generic_utils import conditional_print +from spot_rl.envs.test_cfslam import calculate_height from spot_rl.skills.atomic_skills import ( + MobilePickEE, Navigation, OpenCloseDrawer, Pick, Place, SemanticPick, SemanticPlace, + SemanticPlaceEE, ) from spot_rl.utils.construct_configs import ( construct_config_for_gaze, @@ -102,6 +105,8 @@ def __init__( open_close_drawer_config=None, use_mobile_pick: bool = False, use_semantic_place: bool = False, + use_pick_ee: bool = False, + use_place_ee: bool = False, verbose: bool = True, use_policies: bool = True, ): @@ -113,7 +118,10 @@ def __init__( # Process the meta parameters self._use_mobile_pick = use_mobile_pick + print("USE MOBILE PICK VARIABLE IS SET TO :", self._use_mobile_pick) self.use_semantic_place = use_semantic_place + self.use_pick_ee = use_pick_ee + self.use_place_ee = use_place_ee # Create the spot object, init lease, and construct configs self.__init_spot( @@ -174,6 +182,7 @@ def __init_spot( ) if place_config is None: + # TODO: overwrite the config self.place_config = ( construct_config_for_semantic_place() if self.use_semantic_place @@ -203,15 +212,30 @@ def __initiate_controllers(self, use_policies: bool = True): spot=self.spot, config=self.nav_config, ) - self.gaze_controller = Pick( - spot=self.spot, - config=self.pick_config, - use_mobile_pick=self._use_mobile_pick, - ) - if self.use_semantic_place: - self.place_controller = SemanticPlace( - spot=self.spot, config=self.place_config + if self.use_pick_ee: + # print("GOING INSIDE GAZE EE ENV") + self.gaze_controller = MobilePickEE( + spot=self.spot, + config=self.pick_config, + use_mobile_pick=self._use_mobile_pick, + ) + else: + self.gaze_controller = Pick( + spot=self.spot, + config=self.pick_config, + use_mobile_pick=self._use_mobile_pick, ) + if self.use_semantic_place: + + if self.use_place_ee: + print("GOING INSIDE SEM EE ENV") + self.place_controller = SemanticPlaceEE( + spot=self.spot, config=self.place_config + ) + else: + self.place_controller = SemanticPlace( + spot=self.spot, config=self.place_config + ) else: self.place_controller = Place( spot=self.spot, @@ -231,8 +255,8 @@ def reset(self): # Reset the policies and environments via the controllers raise NotImplementedError - @multimethod - def nav(self, nav_target: str = None) -> Tuple[bool, str]: + @multimethod # type: ignore + def nav(self, nav_target: str = None) -> Tuple[bool, str]: # type: ignore """ Perform the nav action on the navigation target specified as a known string @@ -275,7 +299,7 @@ def nav( # noqa self, x: float, y: float, - theta: float, + theta=float, reset_current_receptacle_name: bool = True, ) -> Tuple[bool, str]: """ @@ -301,26 +325,6 @@ def nav( # noqa conditional_print(message=message, verbose=self.verbose) return status, message - @multimethod # type: ignore - def nav(self, x: float, y: float) -> Tuple[bool, str]: # noqa - """ - Perform the nav action on the navigation target with yaw specified as a metric location - Args: - x (float): x coordinate of the nav target (in meters) specified in the world frame - y (float): y coordinate of the nav target (in meters) specified in the world frame - Returns: - bool: True if navigation was successful, False otherwise - str: Message indicating the status of the navigation - """ - theta = 0.0 - goal_dict = { - "nav_target": (x, y, theta), - "dynamic_yaw": True, - } # type: Dict[str, Any] - status, message = self.nav_controller.execute(goal_dict=goal_dict) - conditional_print(message=message, verbose=self.verbose) - return status, message - def heuristic_mobile_gaze( self, x: float, @@ -467,7 +471,7 @@ def semanticpick( return status, message @multimethod # type: ignore - def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray = None, is_local: bool = False, visualize: bool = False) -> Tuple[bool, str]: # type: ignore + def place(self, place_target: str = None, height_target: str = None, ee_orientation_at_grasping: np.ndarray = None, is_local: bool = False, visualize: bool = False) -> Tuple[bool, str]: # type: ignore """ Perform the place action on the place target specified as known string @@ -495,7 +499,6 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray message = f"Failed - place target {place_target} not found - use the exact name" conditional_print(message=message, verbose=self.verbose) return False, message - if self.use_semantic_place: # Convert HOME frame coordinates into body frame place_target_location = ( @@ -506,9 +509,18 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray is_local = True else: message = "No place target specified, estimating point through heuristic" + height = calculate_height(height_target) + if height < 0.8128: + print("lower than 20 in") + self.angle = self.pick_config.GAZE_ARM_JOINT_ANGLES + else: + print("higher than 20 inc") + self.angle = self.pick_config.GAZE_ARM_JOINT_ANGLES2 + conditional_print(message=message, verbose=self.verbose) is_local = True # estimate waypoint + visualize = True try: ( place_target_location, @@ -516,12 +528,13 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray _, ) = detect_place_point_by_pcd_method( self.spot, - self.pick_config.SEMANTIC_PLACE_ARM_JOINT_ANGLES, + self.angle, percentile=0 if visualize else 70, visualize=visualize, height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, ) print(f"Estimate Place xyz: {place_target_location}") + breakpoint() if visualize: plot_place_point_in_gripper_image( self.spot, place_target_in_gripper_camera @@ -578,7 +591,7 @@ def contrainedplace(self, object_target: str = None, ee_orientation_at_grasping: object_target, proposition, self.spot, - self.pick_config.SEMANTIC_PLACE_ARM_JOINT_ANGLES, + self.pick_config.GAZE_ARM_JOINT_ANGLES, percentile=70, visualize=visualize, height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, diff --git a/spot_rl_experiments/spot_rl/envs/test_cfslam.py b/spot_rl_experiments/spot_rl/envs/test_cfslam.py new file mode 100644 index 00000000..b80ee35b --- /dev/null +++ b/spot_rl_experiments/spot_rl/envs/test_cfslam.py @@ -0,0 +1,29 @@ +import json +import os + + +def calculate_height(object_tag): + # Iterate through each dictionary in the world_graph + os.chdir( + "/home/tusharsangam/Desktop/spot-sim2real/spot_rl_experiments/spot_rl/envs" + ) + with open("cfslam_object_relations.json") as f: + world_graph = json.load(f) + print("THE OBJECT I GOT IS", object_tag) + for rel in world_graph: + # Iterate through all keys in the dictionary + for key, value in rel.items(): + if isinstance(value, dict) and value.get("object_tag") == object_tag: + object_node = value + # Extract the height + if "bbox_center" in object_node and "bbox_extent" in object_node: + bbox_center = object_node["bbox_center"] + bbox_extent = object_node["bbox_extent"] + # Calculate the height + height = bbox_center[2] + bbox_extent[2] + return height + else: + raise ValueError( + f"Object with tag '{object_tag}' is missing required properties" + ) + raise ValueError(f"Object with tag '{object_tag}' not found in world_graph") From 625f3392bc37fbe6dd73807af5d598b627779828 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Thu, 29 Aug 2024 18:46:58 -0700 Subject: [PATCH 07/82] Minor skills manager changes --- .../spot_rl/envs/skill_manager.py | 39 +++++-------------- 1 file changed, 9 insertions(+), 30 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 84e8fff5..d4007c8b 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -1,7 +1,7 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. - +# black: ignore-errors from typing import Any, Dict, Tuple @@ -12,14 +12,12 @@ from perception_and_utils.utils.generic_utils import conditional_print from spot_rl.envs.test_cfslam import calculate_height from spot_rl.skills.atomic_skills import ( - MobilePickEE, Navigation, OpenCloseDrawer, Pick, Place, SemanticPick, SemanticPlace, - SemanticPlaceEE, ) from spot_rl.utils.construct_configs import ( construct_config_for_gaze, @@ -105,8 +103,6 @@ def __init__( open_close_drawer_config=None, use_mobile_pick: bool = False, use_semantic_place: bool = False, - use_pick_ee: bool = False, - use_place_ee: bool = False, verbose: bool = True, use_policies: bool = True, ): @@ -120,8 +116,6 @@ def __init__( self._use_mobile_pick = use_mobile_pick print("USE MOBILE PICK VARIABLE IS SET TO :", self._use_mobile_pick) self.use_semantic_place = use_semantic_place - self.use_pick_ee = use_pick_ee - self.use_place_ee = use_place_ee # Create the spot object, init lease, and construct configs self.__init_spot( @@ -212,30 +206,15 @@ def __initiate_controllers(self, use_policies: bool = True): spot=self.spot, config=self.nav_config, ) - if self.use_pick_ee: - # print("GOING INSIDE GAZE EE ENV") - self.gaze_controller = MobilePickEE( - spot=self.spot, - config=self.pick_config, - use_mobile_pick=self._use_mobile_pick, - ) - else: - self.gaze_controller = Pick( - spot=self.spot, - config=self.pick_config, - use_mobile_pick=self._use_mobile_pick, - ) + self.gaze_controller = Pick( + spot=self.spot, + config=self.pick_config, + use_mobile_pick=self._use_mobile_pick, + ) if self.use_semantic_place: - - if self.use_place_ee: - print("GOING INSIDE SEM EE ENV") - self.place_controller = SemanticPlaceEE( - spot=self.spot, config=self.place_config - ) - else: - self.place_controller = SemanticPlace( - spot=self.spot, config=self.place_config - ) + self.place_controller = SemanticPlace( + spot=self.spot, config=self.place_config + ) else: self.place_controller = Place( spot=self.spot, From 2d9d2fa225791685cb328c49b2dc9fc9cfeb1fbb Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Thu, 29 Aug 2024 19:05:28 -0700 Subject: [PATCH 08/82] Renamed Gaze Angle Variables in Config --- spot_rl_experiments/configs/config.yaml | 6 ++++-- spot_rl_experiments/spot_rl/envs/skill_manager.py | 6 +++--- 2 files changed, 7 insertions(+), 5 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index a032652a..b194b551 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -142,8 +142,10 @@ INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE: [0, -180, 180, 0, 0, 90] # The initial INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_LEFT_HAND: [0, -180, 180, 0, 0, -90] # The initial orientation of the arm for side grasping (gripper is in object's left hand side) INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_TOP_DOWN: [-0.91, -92.87, 91.21, 0, 90.01, 0] # The initial orientation of the arm for top down grasping # The old gaze ready angle: [0, -170, 120, 0, 75, 0] -GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 75, 0] #[0, -160, 100, 0, 90, 0] -GAZE_ARM_JOINT_ANGLES2: [0, -120, 60, 0, 88, 0] #[0, -160, 100, 0, 90, 0] +GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 75, 0] +GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES: [0, -160, 100, 0, 75, 0] #[0, -160, 100, 0, 90, 0] +GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES: [0, -120, 60, 0, 88, 0] #[0, -160, 100, 0, 90, 0] +HEIGHT_THRESHOLD : 0.8128 PLACE_ARM_JOINT_ANGLES: [0, -170, 120, 0, 75, 0] ARM_LOWER_LIMITS: [-45, -180, 0, 0, -90, 0] ARM_UPPER_LIMITS: [45, 0, 180, 0, 90, 0] diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index d4007c8b..e20909ea 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -489,12 +489,12 @@ def place(self, place_target: str = None, height_target: str = None, ee_orientat else: message = "No place target specified, estimating point through heuristic" height = calculate_height(height_target) - if height < 0.8128: + if height < self.place_config.HEIGHT_THRESHOLD: print("lower than 20 in") - self.angle = self.pick_config.GAZE_ARM_JOINT_ANGLES + self.angle = self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES else: print("higher than 20 inc") - self.angle = self.pick_config.GAZE_ARM_JOINT_ANGLES2 + self.angle = self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES conditional_print(message=message, verbose=self.verbose) is_local = True From bcac26d5504ddcb1e8e1e861ab45eac59dfe0f22 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Thu, 29 Aug 2024 19:20:52 -0700 Subject: [PATCH 09/82] removed breakpoint --- spot_rl_experiments/spot_rl/envs/skill_manager.py | 1 - spot_rl_experiments/spot_rl/envs/test_cfslam.py | 8 +++++--- 2 files changed, 5 insertions(+), 4 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index e20909ea..4ec01387 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -513,7 +513,6 @@ def place(self, place_target: str = None, height_target: str = None, ee_orientat height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, ) print(f"Estimate Place xyz: {place_target_location}") - breakpoint() if visualize: plot_place_point_in_gripper_image( self.spot, place_target_in_gripper_camera diff --git a/spot_rl_experiments/spot_rl/envs/test_cfslam.py b/spot_rl_experiments/spot_rl/envs/test_cfslam.py index b80ee35b..d7b3a42d 100644 --- a/spot_rl_experiments/spot_rl/envs/test_cfslam.py +++ b/spot_rl_experiments/spot_rl/envs/test_cfslam.py @@ -1,3 +1,7 @@ +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. +# black: ignore-errors import json import os @@ -23,7 +27,5 @@ def calculate_height(object_tag): height = bbox_center[2] + bbox_extent[2] return height else: - raise ValueError( - f"Object with tag '{object_tag}' is missing required properties" - ) + raise ValueError(f"Object with tag '{object_tag}' is missing") raise ValueError(f"Object with tag '{object_tag}' not found in world_graph") From bccc55aa3af5ab6e6c71c44c1072dcb66ebfa7c1 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Fri, 30 Aug 2024 11:00:04 -0700 Subject: [PATCH 10/82] Removed CG Json --- .../spot_rl/envs/cfslam_object_relations.json | 1262 ----------------- 1 file changed, 1262 deletions(-) delete mode 100644 spot_rl_experiments/spot_rl/envs/cfslam_object_relations.json diff --git a/spot_rl_experiments/spot_rl/envs/cfslam_object_relations.json b/spot_rl_experiments/spot_rl/envs/cfslam_object_relations.json deleted file mode 100644 index 3afecefa..00000000 --- a/spot_rl_experiments/spot_rl/envs/cfslam_object_relations.json +++ /dev/null @@ -1,1262 +0,0 @@ -[ - { - "object1": { - "id": 0, - "bbox_extent": [ - 0.8, - 0.2, - 0.2 - ], - "bbox_center": [ - 2.6, - -1.1, - -0.1 - ], - "object_tag": "fabric", - "category_tag": "object" - }, - "object2": { - "id": 5, - "bbox_extent": [ - 0.5, - 0.2, - 0.2 - ], - "bbox_center": [ - 2.3, - -1.5, - -0.1 - ], - "object_tag": "furniture", - "category_tag": "furniture" - }, - "object_relation": "a next to b", - "reason": "The objects are placed close to each other on the floor, with their centers at similar heights. Object1 is a fabric object and object2 is a furniture, so it's likely they're placed next to each other in a living room setting.", - "room_region": "living room" - }, - { - "object1": { - "id": 1, - "bbox_extent": [ - 1.1, - 1.1, - 0.6 - ], - "bbox_center": [ - 2.1, - -1.6, - 0.3 - ], - "object_tag": "white dresser", - "category_tag": "furniture" - }, - "object2": { - "id": 4, - "bbox_extent": [ - 0.2, - 0.1, - 0.1 - ], - "bbox_center": [ - 2.1, - -1.5, - -0.1 - ], - "object_tag": "piece of wood", - "category_tag": "object" - }, - "object_relation": "b on a", - "reason": "The object_relation is 'b on a' because the center of object2 (a piece of wood) is below and close to the center of object1 (a white dresser), suggesting it might be placed on top. The room_region is 'bedroom' because the bbox_center of both objects are near the floor (z-coordinate is low) and the object_tag of object1 suggests a bedroom furniture.", - "room_region": "bedroom" - }, - { - "object1": { - "id": 1, - "bbox_extent": [ - 1.1, - 1.1, - 0.6 - ], - "bbox_center": [ - 2.1, - -1.6, - 0.3 - ], - "object_tag": "white dresser", - "category_tag": "furniture" - }, - "object2": { - "id": 5, - "bbox_extent": [ - 0.5, - 0.2, - 0.2 - ], - "bbox_center": [ - 2.3, - -1.5, - -0.1 - ], - "object_tag": "furniture", - "category_tag": "furniture" - }, - "object_relation": "a next to b", - "reason": "Object 1 is a dresser which typically stands on the floor, and object 2's category tag is also furniture. Their bounding box centers are close in x and y coordinates but have different z coordinates, indicating they might be placed side by side. The room region is likely a bedroom as dressers are commonly found there.", - "room_region": "bedroom" - }, - { - "object1": { - "id": 2, - "bbox_extent": [ - 1.1, - 0.7, - 0.3 - ], - "bbox_center": [ - 2.4, - -1.2, - -0.3 - ], - "object_tag": "skateboard", - "category_tag": "object" - }, - "object2": { - "id": 5, - "bbox_extent": [ - 0.5, - 0.2, - 0.2 - ], - "bbox_center": [ - 2.3, - -1.5, - -0.1 - ], - "object_tag": "furniture", - "category_tag": "furniture" - }, - "object_relation": "a next to b", - "reason": "Object 1 is a skateboard and object 2 is furniture, suggesting they are not related by containment. Their centers are close together in the X-Y plane but have different heights (z-coordinates), indicating they might be placed side-by-side. The object tags do not suggest a specific room region, so we default to 'living room' as it is a common area where both objects could be found.", - "room_region": "living room" - }, - { - "object1": { - "id": 2, - "bbox_extent": [ - 1.1, - 0.7, - 0.3 - ], - "bbox_center": [ - 2.4, - -1.2, - -0.3 - ], - "object_tag": "skateboard", - "category_tag": "object" - }, - "object2": { - "id": 34, - "bbox_extent": [ - 1.3, - 0.4, - 0.3 - ], - "bbox_center": [ - 2.6, - -0.6, - -0.0 - ], - "object_tag": "black fabric", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The two objects are placed close together, but not on top of or inside each other. The 'skateboard' and 'black fabric' tags suggest a living room setting where these items may be found.", - "room_region": "living room" - }, - { - "object1": { - "id": 3, - "bbox_extent": [ - 0.8, - 0.2, - 0.2 - ], - "bbox_center": [ - 2.1, - -1.5, - 0.4 - ], - "object_tag": "drawer", - "category_tag": "furniture" - }, - "object2": { - "id": 33, - "bbox_extent": [ - 0.9, - 0.7, - 0.3 - ], - "bbox_center": [ - 2.6, - -1.1, - 0.2 - ], - "object_tag": "black curtain", - "category_tag": "object" - }, - "object_relation": "none of these", - "reason": "Object1 is a drawer and Object2 is a black curtain. Since the bbox_center of both objects are close to each other (z-coordinate is low, indicating they are near the floor), it's likely they are in the same room. The object_tag 'drawer' suggests that it might be part of a furniture like a dresser or cabinet, which can be found in a living room. The relationship between them doesn't fit any of the described options.", - "room_region": "living room" - }, - { - "object1": { - "id": 7, - "bbox_extent": [ - 1.1, - 0.5, - 0.2 - ], - "bbox_center": [ - 1.8, - -1.4, - -0.2 - ], - "object_tag": "black suitcase", - "category_tag": "object" - }, - "object2": { - "id": 5, - "bbox_extent": [ - 0.5, - 0.2, - 0.2 - ], - "bbox_center": [ - 2.3, - -1.5, - -0.1 - ], - "object_tag": "furniture", - "category_tag": "furniture" - }, - "object_relation": "a next to b", - "reason": "Object 1 is a black suitcase and object 2 is a furniture, which are commonly found in bedrooms. Their bounding box centers are also close to each other, suggesting they are likely placed next to each other.", - "room_region": "bedroom" - }, - { - "object1": { - "id": 8, - "bbox_extent": [ - 0.6, - 0.4, - 0.2 - ], - "bbox_center": [ - 1.8, - -1.7, - -0.1 - ], - "object_tag": "carpet", - "category_tag": "furniture" - }, - "object2": { - "id": 2, - "bbox_extent": [ - 1.1, - 0.7, - 0.3 - ], - "bbox_center": [ - 2.4, - -1.2, - -0.3 - ], - "object_tag": "skateboard", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The 'carpet' and 'skateboard' are in close proximity, suggesting they may be placed side by side. Given the object tags, it's likely they're in a living room where such objects are commonly found.", - "room_region": "living room" - }, - { - "object1": { - "id": 10, - "bbox_extent": [ - 1.0, - 0.3, - 0.2 - ], - "bbox_center": [ - 1.0, - -1.5, - -0.2 - ], - "object_tag": "yellow tag", - "category_tag": "object" - }, - "object2": { - "id": 7, - "bbox_extent": [ - 1.1, - 0.5, - 0.2 - ], - "bbox_center": [ - 1.8, - -1.4, - -0.2 - ], - "object_tag": "black suitcase", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "Object1 (yellow tag) and object2 (black suitcase) are both objects, not furniture. They are close in proximity but not on top of or inside each other, so they must be next to each other. Their centers are near the floor (-1.5 and -1.4 respectively), which is common for living room objects.", - "room_region": "living room" - }, - { - "object1": { - "id": 10, - "bbox_extent": [ - 1.0, - 0.3, - 0.2 - ], - "bbox_center": [ - 1.0, - -1.5, - -0.2 - ], - "object_tag": "yellow tag", - "category_tag": "object" - }, - "object2": { - "id": 12, - "bbox_extent": [ - 1.0, - 0.5, - 0.2 - ], - "bbox_center": [ - 0.5, - -1.4, - -0.2 - ], - "object_tag": "suitcase", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The objects are not on top of each other and their bounding box centers are close but not inside each other. They are likely placed side by side, which is common in a hallway. The object tags 'yellow tag' and 'suitcase' do not provide any specific room region information, so the hallway is chosen as it's a common area where objects can be placed next to each other.", - "room_region": "hallway" - }, - { - "object1": { - "id": 12, - "bbox_extent": [ - 1.0, - 0.5, - 0.2 - ], - "bbox_center": [ - 0.5, - -1.4, - -0.2 - ], - "object_tag": "suitcase", - "category_tag": "object" - }, - "object2": { - "id": 9, - "bbox_extent": [ - 0.6, - 0.3, - 0.2 - ], - "bbox_center": [ - 1.1, - -1.5, - -0.2 - ], - "object_tag": "yellow tag", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The objects are both categorized as 'objects', and their bounding box centers suggest they are placed near each other, but not in a typical on-top or inside relationship. Their tags also don't imply any specific room region, so I'm defaulting to the hallway.", - "room_region": "hallway" - }, - { - "object1": { - "id": 31, - "bbox_extent": [ - 0.7, - 0.2, - 0.1 - ], - "bbox_center": [ - 2.1, - -1.5, - 0.4 - ], - "object_tag": "drawer with gold handles", - "category_tag": "furniture" - }, - "object2": { - "id": 33, - "bbox_extent": [ - 0.9, - 0.7, - 0.3 - ], - "bbox_center": [ - 2.6, - -1.1, - 0.2 - ], - "object_tag": "black curtain", - "category_tag": "object" - }, - "object_relation": "none of these", - "reason": "Object1 is a drawer with gold handles and object2 is a black curtain, they do not fit any specific relation. The center coordinates of the two objects are close to each other, which suggests they might be in the same room. The height of the objects (0.4 and 0.2) indicates that they are likely on the floor or very close to it, which is common in a living room.", - "room_region": "living room" - }, - { - "object1": { - "id": 33, - "bbox_extent": [ - 0.9, - 0.7, - 0.3 - ], - "bbox_center": [ - 2.6, - -1.1, - 0.2 - ], - "object_tag": "black curtain", - "category_tag": "object" - }, - "object2": { - "id": 2, - "bbox_extent": [ - 1.1, - 0.7, - 0.3 - ], - "bbox_center": [ - 2.4, - -1.2, - -0.3 - ], - "object_tag": "skateboard", - "category_tag": "object" - }, - "object_relation": "none of these", - "reason": "The relationship between a black curtain and a skateboard is unclear as they are not typically placed in relation to each other. Additionally, the room region cannot be determined with certainty due to the lack of context.", - "room_region": "unknown" - }, - { - "object1": { - "id": 33, - "bbox_extent": [ - 0.9, - 0.7, - 0.3 - ], - "bbox_center": [ - 2.6, - -1.1, - 0.2 - ], - "object_tag": "black curtain", - "category_tag": "object" - }, - "object2": { - "id": 30, - "bbox_extent": [ - 1.1, - 0.8, - 0.4 - ], - "bbox_center": [ - 2.2, - -1.6, - 0.2 - ], - "object_tag": "white dresser", - "category_tag": "furniture" - }, - "object_relation": "none of these", - "reason": "The object relation is none of these because the black curtain and white dresser are not typically placed on or inside each other. The room region is bedroom because both objects have a low z-coordinate in their bbox_center, indicating they are close to the ground, and the white dresser suggests it might be used for storing clothes.", - "room_region": "bedroom" - }, - { - "object1": { - "id": 33, - "bbox_extent": [ - 0.9, - 0.7, - 0.3 - ], - "bbox_center": [ - 2.6, - -1.1, - 0.2 - ], - "object_tag": "black curtain", - "category_tag": "object" - }, - "object2": { - "id": 32, - "bbox_extent": [ - 0.8, - 0.5, - 0.3 - ], - "bbox_center": [ - 2.1, - -1.5, - 0.4 - ], - "object_tag": "white drawer", - "category_tag": "furniture" - }, - "object_relation": "none of these", - "reason": "Object1 is a curtain and Object2 is a drawer, but they don't have any typical spatial relationship. They are both in the bedroom region because the object tags suggest it.", - "room_region": "bedroom" - }, - { - "object1": { - "id": 13, - "bbox_extent": [ - 0.7, - 0.4, - 0.3 - ], - "bbox_center": [ - -0.6, - -1.6, - 0.4 - ], - "object_tag": "office chair", - "category_tag": "furniture" - }, - "object2": { - "id": 56, - "bbox_extent": [ - 0.2, - 0.1, - 0.1 - ], - "bbox_center": [ - -0.5, - -1.5, - 0.4 - ], - "object_tag": "yellowed paper", - "category_tag": "object" - }, - "object_relation": "b on a", - "reason": "The object2 'yellowed paper' is much smaller than the object1 'office chair', and their centers are very close, suggesting that the paper could be placed on top of or near the chair. The room region is determined as an office due to the presence of an office chair.", - "room_region": "office" - }, - { - "object1": { - "id": 14, - "bbox_extent": [ - 0.6, - 0.6, - 0.4 - ], - "bbox_center": [ - -0.6, - -1.6, - 0.4 - ], - "object_tag": "black office chair", - "category_tag": "furniture" - }, - "object2": { - "id": 53, - "bbox_extent": [ - 0.7, - 0.7, - 0.5 - ], - "bbox_center": [ - -0.6, - -1.5, - 0.0 - ], - "object_tag": "black office chair", - "category_tag": "furniture" - }, - "object_relation": "none of these", - "reason": "Both objects are black office chairs and their bounding box centers are very close, but there is no indication that one is placed on top or inside the other. The room region is likely an office since both objects are office chairs.", - "room_region": "office" - }, - { - "object1": { - "id": 14, - "bbox_extent": [ - 0.6, - 0.6, - 0.4 - ], - "bbox_center": [ - -0.6, - -1.6, - 0.4 - ], - "object_tag": "black office chair", - "category_tag": "furniture" - }, - "object2": { - "id": 56, - "bbox_extent": [ - 0.2, - 0.1, - 0.1 - ], - "bbox_center": [ - -0.5, - -1.5, - 0.4 - ], - "object_tag": "yellowed paper", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "Object1 is a chair and object2 is a piece of paper, which are often placed next to each other in an office setting. The bbox centers of both objects are close to the ground (z=0.4), indicating they are on the floor or on a low surface, which supports this relationship.", - "room_region": "office" - }, - { - "object1": { - "id": 15, - "bbox_extent": [ - 0.2, - 0.2, - 0.2 - ], - "bbox_center": [ - -0.6, - -0.1, - -0.1 - ], - "object_tag": "small electronic device", - "category_tag": "object" - }, - "object2": { - "id": 17, - "bbox_extent": [ - 0.3, - 0.3, - 0.2 - ], - "bbox_center": [ - -0.6, - -0.1, - -0.0 - ], - "object_tag": "small electronic device", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "Object 1 and Object 2 are both small electronic devices with similar sizes and centers close together, suggesting they are placed side by side. Their centers' z-coordinates indicate they are on a surface around 0-10 cm above the ground, which is common for living room furniture or shelves.", - "room_region": "living room" - }, - { - "object1": { - "id": 16, - "bbox_extent": [ - 0.5, - 0.2, - 0.1 - ], - "bbox_center": [ - -1.3, - -0.5, - -0.2 - ], - "object_tag": "twisted electrical wire", - "category_tag": "object" - }, - "object2": { - "id": 55, - "bbox_extent": [ - 0.9, - 0.7, - 0.3 - ], - "bbox_center": [ - -1.4, - -0.9, - 0.2 - ], - "object_tag": "pole or wand", - "category_tag": "object" - }, - "object_relation": "none of these", - "reason": "The objects are both categorized as objects and have no clear spatial relationship, with their bounding box centers not indicating a specific room region.", - "room_region": "unknown" - }, - { - "object1": { - "id": 18, - "bbox_extent": [ - 1.1, - 0.4, - 0.1 - ], - "bbox_center": [ - -1.4, - -1.2, - 0.3 - ], - "object_tag": "decorative stick", - "category_tag": "object" - }, - "object2": { - "id": 55, - "bbox_extent": [ - 0.9, - 0.7, - 0.3 - ], - "bbox_center": [ - -1.4, - -0.9, - 0.2 - ], - "object_tag": "pole or wand", - "category_tag": "object" - }, - "object_relation": "none of these", - "reason": "The objects are both categorized as 'objects' and have similar heights off the ground, but their object tags do not suggest a common relationship or room region. The bounding box centers and extents also do not provide any clear indication of their relationship or room region.", - "room_region": "unknown" - }, - { - "object1": { - "id": 35, - "bbox_extent": [ - 1.3, - 0.2, - 0.1 - ], - "bbox_center": [ - -1.4, - -0.2, - -0.2 - ], - "object_tag": "monitor", - "category_tag": "object" - }, - "object2": { - "id": 52, - "bbox_extent": [ - 0.8, - 0.4, - 0.1 - ], - "bbox_center": [ - -1.3, - -0.3, - -0.3 - ], - "object_tag": "electrical cord", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The object1 is a monitor and the object2 is an electrical cord, which are commonly placed together near each other. The bbox_center of both objects has negative x, y, and z coordinates indicating they are likely on the floor or on a table in a living room.", - "room_region": "living room" - }, - { - "object1": { - "id": 35, - "bbox_extent": [ - 1.3, - 0.2, - 0.1 - ], - "bbox_center": [ - -1.4, - -0.2, - -0.2 - ], - "object_tag": "monitor", - "category_tag": "object" - }, - "object2": { - "id": 55, - "bbox_extent": [ - 0.9, - 0.7, - 0.3 - ], - "bbox_center": [ - -1.4, - -0.9, - 0.2 - ], - "object_tag": "pole or wand", - "category_tag": "object" - }, - "object_relation": "none of these", - "reason": "The objects 'monitor' and 'pole or wand' do not have a clear relationship, as they are not typically placed on or inside each other. Additionally, their bounding box centers do not suggest a specific room region.", - "room_region": "unknown" - }, - { - "object1": { - "id": 54, - "bbox_extent": [ - 0.6, - 0.2, - 0.1 - ], - "bbox_center": [ - -1.4, - -1.4, - -0.3 - ], - "object_tag": "blue line", - "category_tag": "object" - }, - "object2": { - "id": 55, - "bbox_extent": [ - 0.9, - 0.7, - 0.3 - ], - "bbox_center": [ - -1.4, - -0.9, - 0.2 - ], - "object_tag": "pole or wand", - "category_tag": "object" - }, - "object_relation": "none of these", - "reason": "The objects' bounding box centers are close in the XY plane but not in the Z axis. Object1 is an object and Object2 is also an object, but they do not fit any specific relationship category. Their object tags 'blue line' and 'pole or wand' do not give any clear indication of their usage or placement in a room.", - "room_region": "unknown" - }, - { - "object1": { - "id": 19, - "bbox_extent": [ - 0.6, - 0.5, - 0.1 - ], - "bbox_center": [ - -0.5, - 0.3, - -0.3 - ], - "object_tag": "computer monitor", - "category_tag": "object" - }, - "object2": { - "id": 22, - "bbox_extent": [ - 1.3, - 1.1, - 0.3 - ], - "bbox_center": [ - -0.6, - 0.7, - -0.2 - ], - "object_tag": "collection of cords", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "Object1 (computer monitor) is commonly placed on a table or desk, and object2 (collection of cords) is often placed beside it. The bbox_center coordinates suggest they are close to each other on the floor plane, indicating they might be in a living room.", - "room_region": "living room" - }, - { - "object1": { - "id": 19, - "bbox_extent": [ - 0.6, - 0.5, - 0.1 - ], - "bbox_center": [ - -0.5, - 0.3, - -0.3 - ], - "object_tag": "computer monitor", - "category_tag": "object" - }, - "object2": { - "id": 36, - "bbox_extent": [ - 0.6, - 0.3, - 0.2 - ], - "bbox_center": [ - -0.2, - 0.1, - -0.2 - ], - "object_tag": "small electronic device", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The objects are both categorized as objects and their centers are relatively close, suggesting they might be placed next to each other. The object tags 'computer monitor' and 'small electronic device' hint at a possible location in the living room.", - "room_region": "living room" - }, - { - "object1": { - "id": 20, - "bbox_extent": [ - 0.2, - 0.1, - 0.1 - ], - "bbox_center": [ - -0.2, - -0.1, - -0.1 - ], - "object_tag": "electronic device", - "category_tag": "object" - }, - "object2": { - "id": 36, - "bbox_extent": [ - 0.6, - 0.3, - 0.2 - ], - "bbox_center": [ - -0.2, - 0.1, - -0.2 - ], - "object_tag": "small electronic device", - "category_tag": "object" - }, - "object_relation": "a on b", - "reason": "Object1 is a small electronic device and object2 is also an electronic device, so it's likely that object1 is placed on top of object2. Both objects have their centers close to the ground (z-coordinate), which suggests they are in a living room.", - "room_region": "living room" - }, - { - "object1": { - "id": 21, - "bbox_extent": [ - 0.4, - 0.2, - 0.1 - ], - "bbox_center": [ - -1.0, - 0.7, - -0.3 - ], - "object_tag": "yellow wire", - "category_tag": "object" - }, - "object2": { - "id": 22, - "bbox_extent": [ - 1.3, - 1.1, - 0.3 - ], - "bbox_center": [ - -0.6, - 0.7, - -0.2 - ], - "object_tag": "collection of cords", - "category_tag": "object" - }, - "object_relation": "none of these", - "reason": "Since both objects are categorized as 'object' and have similar height (z-coordinate), it's unclear what their relationship is or which room region they belong to. Object1 has a smaller extent, but its center is farther away from the origin than object2. Without more context, we cannot determine their relation or room region.", - "room_region": "unknown" - }, - { - "object1": { - "id": 23, - "bbox_extent": [ - 0.6, - 0.4, - 0.1 - ], - "bbox_center": [ - -0.1, - 0.8, - 0.5 - ], - "object_tag": "yellow object", - "category_tag": "object" - }, - "object2": { - "id": 43, - "bbox_extent": [ - 0.7, - 0.3, - 0.1 - ], - "bbox_center": [ - 0.2, - 0.9, - 0.6 - ], - "object_tag": "wall", - "category_tag": "furniture" - }, - "object_relation": "none of these", - "reason": "The object tags and categories indicate that 'yellow object' is an object and 'wall' is a furniture. The bbox_center coordinates suggest that they are both at a certain height off the ground, but there's no clear indication of one being on or in the other. Based on their positions, it seems like they could be in a living room.", - "room_region": "living room" - }, - { - "object1": { - "id": 42, - "bbox_extent": [ - 0.7, - 0.5, - 0.2 - ], - "bbox_center": [ - -0.3, - 0.8, - 0.7 - ], - "object_tag": "yellow object", - "category_tag": "object" - }, - "object2": { - "id": 43, - "bbox_extent": [ - 0.7, - 0.3, - 0.1 - ], - "bbox_center": [ - 0.2, - 0.9, - 0.6 - ], - "object_tag": "wall", - "category_tag": "furniture" - }, - "object_relation": "none of these", - "reason": "Object1 is an object and Object2 is a wall, which are not commonly placed in specific relationships with each other. The room region cannot be determined as the objects do not provide clear indication of the room they are in.", - "room_region": "unknown" - }, - { - "object1": { - "id": 24, - "bbox_extent": [ - 0.4, - 0.3, - 0.2 - ], - "bbox_center": [ - 2.2, - 0.7, - 0.1 - ], - "object_tag": "suitcase", - "category_tag": "object" - }, - "object2": { - "id": 25, - "bbox_extent": [ - 0.7, - 0.6, - 0.4 - ], - "bbox_center": [ - 2.3, - 0.6, - 0.5 - ], - "object_tag": "black case", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The objects are both categorized as 'objects' and their centers are close in the x-y plane, indicating they may be placed side by side. The z-coordinates of their centers are relatively low, suggesting they are on or near the floor. Additionally, the object tags 'suitcase' and 'black case' are commonly found in bedrooms.", - "room_region": "bedroom" - }, - { - "object1": { - "id": 24, - "bbox_extent": [ - 0.4, - 0.3, - 0.2 - ], - "bbox_center": [ - 2.2, - 0.7, - 0.1 - ], - "object_tag": "suitcase", - "category_tag": "object" - }, - "object2": { - "id": 26, - "bbox_extent": [ - 0.6, - 0.3, - 0.2 - ], - "bbox_center": [ - 2.3, - 0.6, - 0.1 - ], - "object_tag": "black suitcase", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The two objects are both suitcases and have similar sizes, with centers very close to each other (difference in x-coordinate is 0.1). They also have the same height off of the ground (z-coordinate of 0.1), which suggests they are placed side by side on the floor. Given their object tags, it's likely they're both located in a hallway.", - "room_region": "hallway" - }, - { - "object1": { - "id": 25, - "bbox_extent": [ - 0.7, - 0.6, - 0.4 - ], - "bbox_center": [ - 2.3, - 0.6, - 0.5 - ], - "object_tag": "black case", - "category_tag": "object" - }, - "object2": { - "id": 38, - "bbox_extent": [ - 0.6, - 0.5, - 0.3 - ], - "bbox_center": [ - 2.4, - 0.6, - 0.9 - ], - "object_tag": "black case/speaker", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The centers of the objects are very close in x and y coordinates, indicating they are likely placed side by side. The z coordinates are also relatively low, suggesting they are on or near the floor. Additionally, both object tags mention 'case', which is often associated with living room electronics.", - "room_region": "living room" - }, - { - "object1": { - "id": 27, - "bbox_extent": [ - 1.0, - 0.2, - 0.1 - ], - "bbox_center": [ - 2.6, - 0.1, - 0.2 - ], - "object_tag": "black pole", - "category_tag": "object" - }, - "object2": { - "id": 29, - "bbox_extent": [ - 0.2, - 0.1, - 0.1 - ], - "bbox_center": [ - 2.6, - 0.0, - 0.5 - ], - "object_tag": "chair", - "category_tag": "furniture" - }, - "object_relation": "none of these", - "reason": "Object1 is a black pole and object2 is a chair. There are no obvious signs that one object is on, in or next to the other. The bbox_center coordinates do not provide any specific information about the room region.", - "room_region": "unknown" - }, - { - "object1": { - "id": 46, - "bbox_extent": [ - 0.3, - 0.2, - 0.0 - ], - "bbox_center": [ - -0.7, - -3.5, - 0.0 - ], - "object_tag": "spider web", - "category_tag": "object" - }, - "object2": { - "id": 48, - "bbox_extent": [ - 0.6, - 0.3, - 0.2 - ], - "bbox_center": [ - -0.8, - -3.4, - -0.0 - ], - "object_tag": "wooden furniture", - "category_tag": "furniture" - }, - "object_relation": "none of these", - "reason": "The object1 is a spider web and the object2 is wooden furniture, they do not have any direct relationship with each other. The bbox_center of both objects indicates that they are close to the floor and in the same general area, but their categories and tags do not suggest a specific relation. Based on the bbox_center, it seems likely that they are in a living room.", - "room_region": "living room" - }, - { - "object1": { - "id": 49, - "bbox_extent": [ - 0.2, - 0.2, - 0.1 - ], - "bbox_center": [ - 1.7, - -2.6, - 0.3 - ], - "object_tag": "button", - "category_tag": "object" - }, - "object2": { - "id": 50, - "bbox_extent": [ - 0.3, - 0.2, - 0.1 - ], - "bbox_center": [ - 1.8, - -2.6, - 0.4 - ], - "object_tag": "black leather case", - "category_tag": "object" - }, - "object_relation": "a next to b", - "reason": "The objects are close in proximity (bbox_center of object1: [1.7, -2.6, 0.3], bbox_center of object2: [1.8, -2.6, 0.4]) and are both categorized as objects, suggesting they might be placed next to each other on a surface such as a table or shelf in a living room.", - "room_region": "living room" - } -] \ No newline at end of file From 1d2d4abf59a1beb1608daa0b28c035d1e9b66772 Mon Sep 17 00:00:00 2001 From: Achuthankrishna <74654704+Achuthankrishna@users.noreply.github.com> Date: Fri, 30 Aug 2024 11:32:48 -0700 Subject: [PATCH 11/82] Update config.yaml Updates based on PR Review --- spot_rl_experiments/configs/config.yaml | 7 +------ 1 file changed, 1 insertion(+), 6 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index b194b551..e18d3e0b 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -19,9 +19,6 @@ WEIGHTS_TORCHSCRIPT: # Semantic place SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" - # Semantic place EE - SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" - # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -70,6 +67,7 @@ MAX_EPISODE_STEPS: 100 # Nav env SUCCESS_DISTANCE: 0.3 SUCCESS_ANGLE_DIST: 5 +SUCCESS_DISTANCE_FOR_DYNAMIC_YAW_NAV: 1.10 DISABLE_OBSTACLE_AVOIDANCE: False USE_OA_FOR_NAV: True USE_HEAD_CAMERA: True @@ -105,9 +103,6 @@ PLACE_ACTION_SPACE_LENGTH: 4 SEMANTIC_PLACE_ACTION_SPACE_LENGTH: 9 SEMANTIC_PLACE_JOINT_BLACKLIST: [3] -# Semantic Place EE -SEMANTIC_PLACE_EE_ACTION_SPACE_LENGTH: 10 - # Open Close Drawer env MAX_LIN_DIST_OPEN_CLOSE_DRAWER: 0.0 MAX_ANG_DIST_OPEN_CLOSE_DRAWER: 3.73 # degrees From 5867b53dccb45aa9ee32331b91c6397036d0a1ce Mon Sep 17 00:00:00 2001 From: Achuthankrishna <74654704+Achuthankrishna@users.noreply.github.com> Date: Fri, 30 Aug 2024 12:03:56 -0700 Subject: [PATCH 12/82] Update test_sem_place.py --- .../experiments/skill_test/test_sem_place.py | 16 +--------------- 1 file changed, 1 insertion(+), 15 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index e413d72a..6d2149c0 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -27,19 +27,6 @@ place_target = "test_desk" spotskillmanager = SpotSkillManager(use_mobile_pick=False, use_semantic_place=True) - spot_pos1 = spotskillmanager.spot.get_arm_joint_positions(as_array=True) - - # is_local = False - # if enable_estimation_before_place: - # place_target = None - # is_local = True - - # # Start testing - # contnue = True - # while contnue: - # rospy.set_param("is_gripper_blocked", 0) - # spotskillmanager.place(place_target, is_local=is_local, visualize=True) - # contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") is_local = False if enable_estimation_before_place: height_target = place_target @@ -50,7 +37,7 @@ contnue = True INITIAL_ARM_JOINT_ANGLES = [0, -180, 180, 90, 0, -90] episode_ctr = 0 - # Get EE Pose Initial + # Get EE Pose Initial in rest position spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() # Set Orientation as Zero spot_ort = np.zeros(3) @@ -87,7 +74,6 @@ episode_ctr += 1 contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") # Return the arm to the original position - spot_pos = spotskillmanager.spot.get_ee_pos_in_body_frame()[0] spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) # The following is a helpful tip to debug the arm # We get Spot class From ce5da6a6c4218124b82ae23abc37db1384584f35 Mon Sep 17 00:00:00 2001 From: Achuthankrishna <74654704+Achuthankrishna@users.noreply.github.com> Date: Fri, 30 Aug 2024 12:14:50 -0700 Subject: [PATCH 13/82] Updated skill_manager.py Minor changes as mentioned on PR --- .../spot_rl/envs/skill_manager.py | 27 +++++++++++++++---- 1 file changed, 22 insertions(+), 5 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 4ec01387..229b27dc 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -303,6 +303,26 @@ def nav( # noqa status, message = self.nav_controller.execute(goal_dict=goal_dict) conditional_print(message=message, verbose=self.verbose) return status, message + + @multimethod # type: ignore + def nav(self, x: float, y: float) -> Tuple[bool, str]: # noqa + """ + Perform the nav action on the navigation target with yaw specified as a metric location + Args: + x (float): x coordinate of the nav target (in meters) specified in the world frame + y (float): y coordinate of the nav target (in meters) specified in the world frame + Returns: + bool: True if navigation was successful, False otherwise + str: Message indicating the status of the navigation + """ + theta = 0.0 + goal_dict = { + "nav_target": (x, y, theta), + "dynamic_yaw": True, + } # type: Dict[str, Any] + status, message = self.nav_controller.execute(goal_dict=goal_dict) + conditional_print(message=message, verbose=self.verbose) + return status, message def heuristic_mobile_gaze( self, @@ -490,16 +510,13 @@ def place(self, place_target: str = None, height_target: str = None, ee_orientat message = "No place target specified, estimating point through heuristic" height = calculate_height(height_target) if height < self.place_config.HEIGHT_THRESHOLD: - print("lower than 20 in") - self.angle = self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES + self.arm_joint_angles = self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES else: - print("higher than 20 inc") - self.angle = self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES + self.arm_joint_angles = self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES conditional_print(message=message, verbose=self.verbose) is_local = True # estimate waypoint - visualize = True try: ( place_target_location, From 85ee3e474544091486e72f84494d9b32e827a5b5 Mon Sep 17 00:00:00 2001 From: Achuthankrishna <74654704+Achuthankrishna@users.noreply.github.com> Date: Fri, 30 Aug 2024 12:30:07 -0700 Subject: [PATCH 14/82] Update skill_manager.py --- spot_rl_experiments/spot_rl/envs/skill_manager.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 229b27dc..05338180 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -524,7 +524,7 @@ def place(self, place_target: str = None, height_target: str = None, ee_orientat _, ) = detect_place_point_by_pcd_method( self.spot, - self.angle, + self.arm_joint_angles, percentile=0 if visualize else 70, visualize=visualize, height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, From 4f5bf887df1b0375ff2a6bae2f95b5b6440144b3 Mon Sep 17 00:00:00 2001 From: Kavit Shah <59990215+KavitShah1998@users.noreply.github.com> Date: Tue, 3 Sep 2024 11:52:03 -0700 Subject: [PATCH 15/82] Add Nexus UI Server code as a third-party (#207) --- .gitmodules | 3 +++ third_party/meta-fair-siro-phase-h1-server | 1 + 2 files changed, 4 insertions(+) create mode 160000 third_party/meta-fair-siro-phase-h1-server diff --git a/.gitmodules b/.gitmodules index 75aa2589..00f5ab3f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -18,3 +18,6 @@ url = https://github.com/tusharsangam/FoundationPoseForSpotSim2Real branch = main ignore = dirty +[submodule "third_party/meta-fair-siro-phase-h1-server"] + path = third_party/meta-fair-siro-phase-h1-server + url = git@bitbucket.org:nexusproductions/meta-fair-siro-phase-h1-server.git diff --git a/third_party/meta-fair-siro-phase-h1-server b/third_party/meta-fair-siro-phase-h1-server new file mode 160000 index 00000000..7f5ad23e --- /dev/null +++ b/third_party/meta-fair-siro-phase-h1-server @@ -0,0 +1 @@ +Subproject commit 7f5ad23eaa18dec15a8befcf968134547a01208d From d991e4a892644b21e6b2e8d6e9aad95cac705631 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 3 Sep 2024 16:26:05 -0700 Subject: [PATCH 16/82] Added wapoint estimator flags and cleanup --- .../experiments/skill_test/test_sem_place.py | 18 +++++----- .../spot_rl/envs/skill_manager.py | 35 ++++++++++++++----- .../spot_rl/envs/test_cfslam.py | 31 ---------------- spot_rl_experiments/spot_rl/utils/utils.py | 27 ++++++++++++++ 4 files changed, 63 insertions(+), 48 deletions(-) delete mode 100644 spot_rl_experiments/spot_rl/envs/test_cfslam.py diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index 6d2149c0..c106e771 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -28,11 +28,6 @@ spotskillmanager = SpotSkillManager(use_mobile_pick=False, use_semantic_place=True) is_local = False - if enable_estimation_before_place: - height_target = place_target - place_target = None - is_local = True - # Start testing contnue = True INITIAL_ARM_JOINT_ANGLES = [0, -180, 180, 90, 0, -90] @@ -50,13 +45,20 @@ episode_log = {"actions": []} # mypy: ignore-errors spotskillmanager.spot.close_gripper() input("waiting for user to get ready with camera") - if place_target is None: + if enable_estimation_before_place: + is_local = True spotskillmanager.place( - place_target, height_target, is_local=is_local, visualize=True + place_target, + is_local=is_local, + visualize=True, + enable_waypoint_estimation=True, ) else: spotskillmanager.place( - place_target, None, is_local=is_local, visualize=True + place_target, + is_local=is_local, + visualize=True, + enable_waypoint_estimation=False, ) skill_log = spotskillmanager.place_controller.skill_result_log diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 05338180..4c5603db 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -10,7 +10,6 @@ import rospy from multimethod import multimethod from perception_and_utils.utils.generic_utils import conditional_print -from spot_rl.envs.test_cfslam import calculate_height from spot_rl.skills.atomic_skills import ( Navigation, OpenCloseDrawer, @@ -37,6 +36,7 @@ plot_place_point_in_gripper_image, ) from spot_rl.utils.utils import ( + calculate_height, get_waypoint_yaml, nav_target_from_waypoint, place_target_from_waypoint, @@ -278,7 +278,7 @@ def nav( # noqa self, x: float, y: float, - theta=float, + theta: float, reset_current_receptacle_name: bool = True, ) -> Tuple[bool, str]: """ @@ -303,7 +303,7 @@ def nav( # noqa status, message = self.nav_controller.execute(goal_dict=goal_dict) conditional_print(message=message, verbose=self.verbose) return status, message - + @multimethod # type: ignore def nav(self, x: float, y: float) -> Tuple[bool, str]: # noqa """ @@ -470,7 +470,7 @@ def semanticpick( return status, message @multimethod # type: ignore - def place(self, place_target: str = None, height_target: str = None, ee_orientation_at_grasping: np.ndarray = None, is_local: bool = False, visualize: bool = False) -> Tuple[bool, str]: # type: ignore + def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray = None, is_local: bool = False, visualize: bool = False, enable_waypoint_estimation: bool = False) -> Tuple[bool, str]: # type: ignore """ Perform the place action on the place target specified as known string @@ -478,6 +478,7 @@ def place(self, place_target: str = None, height_target: str = None, ee_orientat place_target (str): Name of the place target (as stored in waypoints.yaml) ee_orientation_at_grasping (list): The ee orientation at grasping. If users specifiy, the robot will place the object in the desired pose This is only used for the semantic place skills. + enable_waypoint_estimation (bool) : This flag determines whether the place policy must use waypoint estimation or not Returns: bool: True if place was successful, False otherwise @@ -488,7 +489,7 @@ def place(self, place_target: str = None, height_target: str = None, ee_orientat verbose=self.verbose, ) - if place_target is not None: + if enable_waypoint_estimation is False: # Get the place target coordinates try: place_target_location = place_target_from_waypoint( @@ -508,11 +509,16 @@ def place(self, place_target: str = None, height_target: str = None, ee_orientat is_local = True else: message = "No place target specified, estimating point through heuristic" - height = calculate_height(height_target) + height = calculate_height(place_target) if height < self.place_config.HEIGHT_THRESHOLD: - self.arm_joint_angles = self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES + print("HERE") + self.arm_joint_angles = ( + self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES + ) else: - self.arm_joint_angles = self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES + self.arm_joint_angles = ( + self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES + ) conditional_print(message=message, verbose=self.verbose) is_local = True @@ -577,6 +583,17 @@ def contrainedplace(self, object_target: str = None, ee_orientation_at_grasping: "next-to", ], f"Place skill does not support proposition of {proposition}" + height = calculate_height(object_target) + if height < self.place_config.HEIGHT_THRESHOLD: + print("HERE") + self.arm_joint_angles = ( + self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES + ) + else: + self.arm_joint_angles = ( + self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES + ) + # Esitmate the waypoint ( place_target_location, @@ -586,7 +603,7 @@ def contrainedplace(self, object_target: str = None, ee_orientation_at_grasping: object_target, proposition, self.spot, - self.pick_config.GAZE_ARM_JOINT_ANGLES, + self.arm_joint_angles, percentile=70, visualize=visualize, height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, diff --git a/spot_rl_experiments/spot_rl/envs/test_cfslam.py b/spot_rl_experiments/spot_rl/envs/test_cfslam.py deleted file mode 100644 index d7b3a42d..00000000 --- a/spot_rl_experiments/spot_rl/envs/test_cfslam.py +++ /dev/null @@ -1,31 +0,0 @@ -# Copyright (c) Meta Platforms, Inc. and its affiliates. -# This source code is licensed under the MIT license found in the -# LICENSE file in the root directory of this source tree. -# black: ignore-errors -import json -import os - - -def calculate_height(object_tag): - # Iterate through each dictionary in the world_graph - os.chdir( - "/home/tusharsangam/Desktop/spot-sim2real/spot_rl_experiments/spot_rl/envs" - ) - with open("cfslam_object_relations.json") as f: - world_graph = json.load(f) - print("THE OBJECT I GOT IS", object_tag) - for rel in world_graph: - # Iterate through all keys in the dictionary - for key, value in rel.items(): - if isinstance(value, dict) and value.get("object_tag") == object_tag: - object_node = value - # Extract the height - if "bbox_center" in object_node and "bbox_extent" in object_node: - bbox_center = object_node["bbox_center"] - bbox_extent = object_node["bbox_extent"] - # Calculate the height - height = bbox_center[2] + bbox_extent[2] - return height - else: - raise ValueError(f"Object with tag '{object_tag}' is missing") - raise ValueError(f"Object with tag '{object_tag}' not found in world_graph") diff --git a/spot_rl_experiments/spot_rl/utils/utils.py b/spot_rl_experiments/spot_rl/utils/utils.py index 13fecb35..c9dad265 100644 --- a/spot_rl_experiments/spot_rl/utils/utils.py +++ b/spot_rl_experiments/spot_rl/utils/utils.py @@ -4,6 +4,8 @@ import argparse +import json +import os import os.path as osp from collections import OrderedDict @@ -157,6 +159,31 @@ def arr2str(arr): return +def calculate_height(object_tag): + # Iterate through each dictionary in the world_graph + script_dir = os.path.dirname(__file__) # Directory of the script + json_file_path = os.path.join(script_dir, "cfslam_object_relations.json") + + with open(json_file_path) as f: + world_graph = json.load(f) + print("THE OBJECT I GOT IS", object_tag) + for rel in world_graph: + # Iterate through all keys in the dictionary + for key, value in rel.items(): + if isinstance(value, dict) and value.get("object_tag") == object_tag: + object_node = value + # Extract the height + if "bbox_center" in object_node and "bbox_extent" in object_node: + bbox_center = object_node["bbox_center"] + bbox_extent = object_node["bbox_extent"] + # Calculate the height + height = bbox_center[2] + bbox_extent[2] + return height + else: + raise ValueError(f"Object with tag '{object_tag}' is missing") + raise ValueError(f"Object with tag '{object_tag}' not found in world_graph") + + class FixSizeOrderedDict(OrderedDict): def __init__(self, *args, maxlen=0, **kwargs): self._maxlen = maxlen From f6200b80304dcc2e94899bf09a0f1d63fda867ab Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 3 Sep 2024 16:26:32 -0700 Subject: [PATCH 17/82] Added wapoint estimator flags and cleanup --- .../experiments/skill_test/test_sem_place.py | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index c106e771..4be7f6ca 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -77,19 +77,3 @@ contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") # Return the arm to the original position spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) -# The following is a helpful tip to debug the arm -# We get Spot class -# spot = spotskillmanager.spot -# We can move the gripper to a point with x,y,z and roll, pitch, yaw -# spot.move_gripper_to_point((0.55, 0., 0.26), np.deg2rad(np.array([0,0,0]))) -# We can also set the robot arm joints -# config = construct_config() -# spot.set_arm_joint_positions(np.deg2rad(config.INITIAL_ARM_JOINT_ANGLES)) - -# In addition, if you want to use semantic place skill based on the grasping orientation, you can do -# spotskillmanager.nav("black_case") -# spotskillmanager.pick("bottle") -# # Fetch the arm joint at grasping location -# ee_orientation_at_grasping = spotskillmanager.gaze_controller.env.ee_orientation_at_grasping -# spotskillmanager.nav("test_desk") -# spotskillmanager.place("test_desk", orientation_at_grasping) # This controls the arm initial orientation From 345ba68babbaf5c48e666e5c5345ad3e7ad88b29 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 3 Sep 2024 16:28:06 -0700 Subject: [PATCH 18/82] Updated Config --- spot_rl_experiments/configs/config.yaml | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index e18d3e0b..e5bd21a9 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -40,10 +40,7 @@ WEIGHTS: # Semantic place SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" - - # Semantic place EE - SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" - + # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" From 3c11ef802333c485e6d995114c1b189202b99e9f Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 3 Sep 2024 16:29:28 -0700 Subject: [PATCH 19/82] Updated Config --- spot_rl_experiments/configs/config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index e5bd21a9..4f3d6c41 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -40,7 +40,7 @@ WEIGHTS: # Semantic place SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" - + # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -104,7 +104,7 @@ SEMANTIC_PLACE_JOINT_BLACKLIST: [3] MAX_LIN_DIST_OPEN_CLOSE_DRAWER: 0.0 MAX_ANG_DIST_OPEN_CLOSE_DRAWER: 3.73 # degrees OPEN_CLOSE_DRAWER_ACTION_SPACE_LENGTH: 8 -OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.3 +OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.75 # Semantic Pick IMG_SRC: 0 # 1 for intel & 0 for gripper From b3bb5681c9edb29e0cd5159bf485d0c970bf666d Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 3 Sep 2024 16:30:26 -0700 Subject: [PATCH 20/82] Updated Config --- spot_rl_experiments/configs/config.yaml | 1 + 1 file changed, 1 insertion(+) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 4f3d6c41..df883485 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -65,6 +65,7 @@ MAX_EPISODE_STEPS: 100 SUCCESS_DISTANCE: 0.3 SUCCESS_ANGLE_DIST: 5 SUCCESS_DISTANCE_FOR_DYNAMIC_YAW_NAV: 1.10 +SUCCESS_ANGLE_DIST_FOR_DYNAMIC_YAW_NAV: 20 DISABLE_OBSTACLE_AVOIDANCE: False USE_OA_FOR_NAV: True USE_HEAD_CAMERA: True From 894b66ddbf260fa9c0cde534a7e0d8d53af132f7 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 3 Sep 2024 17:25:10 -0700 Subject: [PATCH 21/82] Further cleanup and using flag for waypt estimation --- .../skill_test/test_nav_pick_nav_place_skills.py | 4 +++- .../spot_rl/utils/skill_executor.py | 2 +- spot_rl_experiments/spot_rl/utils/utils.py | 14 ++++++++++++-- 3 files changed, 16 insertions(+), 4 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_nav_pick_nav_place_skills.py b/spot_rl_experiments/experiments/skill_test/test_nav_pick_nav_place_skills.py index b7a18457..0c258044 100644 --- a/spot_rl_experiments/experiments/skill_test/test_nav_pick_nav_place_skills.py +++ b/spot_rl_experiments/experiments/skill_test/test_nav_pick_nav_place_skills.py @@ -19,7 +19,9 @@ enable_force_control=True, ) spotskillmanager.nav("kitchen_counter") - spotskillmanager.place(None, is_local=True, visualize=False) + spotskillmanager.place( + "", is_local=True, visualize=False, enable_waypoint_estimation=True + ) contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") # Navigate to dock and shutdown diff --git a/spot_rl_experiments/spot_rl/utils/skill_executor.py b/spot_rl_experiments/spot_rl/utils/skill_executor.py index e9b5f6d5..6dc1050c 100644 --- a/spot_rl_experiments/spot_rl/utils/skill_executor.py +++ b/spot_rl_experiments/spot_rl/utils/skill_executor.py @@ -76,7 +76,7 @@ def execute_skills(self): # succeded, msg = self.spotskillmanager.place(0.6, 0.0, 0.4, is_local=True) # Call semantic place skills succeded, msg = self.spotskillmanager.place( - None, is_local=True, visualize=False + None, is_local=True, visualize=False, enable_waypoint_estimation=True ) self.reset_skill_name_input(skill_name, succeded, msg) elif skill_name == "opendrawer": diff --git a/spot_rl_experiments/spot_rl/utils/utils.py b/spot_rl_experiments/spot_rl/utils/utils.py index c9dad265..ef58e13c 100644 --- a/spot_rl_experiments/spot_rl/utils/utils.py +++ b/spot_rl_experiments/spot_rl/utils/utils.py @@ -12,6 +12,7 @@ import numpy as np import rospy import yaml +from spot_rl.utils.construct_configs import construct_config_for_semantic_place from yacs.config import CfgNode as CN this_dir = osp.dirname(osp.abspath(__file__)) @@ -167,6 +168,10 @@ def calculate_height(object_tag): with open(json_file_path) as f: world_graph = json.load(f) print("THE OBJECT I GOT IS", object_tag) + # getting default height as threshold height in config + default_config = construct_config_for_semantic_place() + default_height = default_config.HEIGHT_THRESHOLD + for rel in world_graph: # Iterate through all keys in the dictionary for key, value in rel.items(): @@ -180,8 +185,13 @@ def calculate_height(object_tag): height = bbox_center[2] + bbox_extent[2] return height else: - raise ValueError(f"Object with tag '{object_tag}' is missing") - raise ValueError(f"Object with tag '{object_tag}' not found in world_graph") + print( + f"Object with tag '{object_tag}' missing bbox property!!! Returning Default Height" + ) + return default_height + # If the object tag is empty, we return the threhold height + print(f"Object with tag '{object_tag}' not found in world_graph") + return default_height class FixSizeOrderedDict(OrderedDict): From 75fe79512668e0e615a1272479475a3d63bf1975 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Fri, 6 Sep 2024 20:25:48 -0700 Subject: [PATCH 22/82] Parsing objectid from object_tag for concept graph --- .../experiments/skill_test/test_sem_place.py | 4 +- .../spot_rl/envs/skill_manager.py | 1 - .../spot_rl/utils/search_table_location.py | 1 + spot_rl_experiments/spot_rl/utils/utils.py | 49 ++++++++++++------- 4 files changed, 35 insertions(+), 20 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index 4be7f6ca..42caf431 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -21,7 +21,7 @@ if in_fre_lab: # at FRE - place_target = "office chair" + place_target = "160_teddy bear" else: # at NYC place_target = "test_desk" @@ -50,7 +50,7 @@ spotskillmanager.place( place_target, is_local=is_local, - visualize=True, + visualize=False, enable_waypoint_estimation=True, ) else: diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 4c5603db..de2d4a94 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -511,7 +511,6 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray message = "No place target specified, estimating point through heuristic" height = calculate_height(place_target) if height < self.place_config.HEIGHT_THRESHOLD: - print("HERE") self.arm_joint_angles = ( self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES ) diff --git a/spot_rl_experiments/spot_rl/utils/search_table_location.py b/spot_rl_experiments/spot_rl/utils/search_table_location.py index 646e5b21..8c43560f 100644 --- a/spot_rl_experiments/spot_rl/utils/search_table_location.py +++ b/spot_rl_experiments/spot_rl/utils/search_table_location.py @@ -410,6 +410,7 @@ def detect_place_point_by_pcd_method( print(f"Intel point {selected_point}, Gripper Point {selected_point_in_gripper}") img_with_bbox = None + visualize = True if visualize: img_with_bbox = img.copy() for xy in corners_xys: diff --git a/spot_rl_experiments/spot_rl/utils/utils.py b/spot_rl_experiments/spot_rl/utils/utils.py index ef58e13c..2db9e1d0 100644 --- a/spot_rl_experiments/spot_rl/utils/utils.py +++ b/spot_rl_experiments/spot_rl/utils/utils.py @@ -167,30 +167,45 @@ def calculate_height(object_tag): with open(json_file_path) as f: world_graph = json.load(f) - print("THE OBJECT I GOT IS", object_tag) + # Get Object id from string object_tag + try: + object_id_str, object_tag = object_tag.split("_", 1) + object_id = int(object_id_str) # Convert ID to integer + except ValueError: + print(f"Invalid object_tag format: '{object_tag}'") + # Return default height if the format is invalid + default_config = construct_config_for_semantic_place() + default_height = default_config.HEIGHT_THRESHOLD + return default_height + # getting default height as threshold height in config default_config = construct_config_for_semantic_place() default_height = default_config.HEIGHT_THRESHOLD - for rel in world_graph: # Iterate through all keys in the dictionary for key, value in rel.items(): - if isinstance(value, dict) and value.get("object_tag") == object_tag: - object_node = value - # Extract the height - if "bbox_center" in object_node and "bbox_extent" in object_node: - bbox_center = object_node["bbox_center"] - bbox_extent = object_node["bbox_extent"] - # Calculate the height - height = bbox_center[2] + bbox_extent[2] - return height - else: - print( - f"Object with tag '{object_tag}' missing bbox property!!! Returning Default Height" - ) - return default_height + if isinstance(value, dict): + if ( + value.get("id") == object_id + and value.get("object_tag") == object_tag + ): + object_node = value + # Extract the height + if "bbox_center" in object_node and "bbox_extent" in object_node: + bbox_center = object_node["bbox_center"] + bbox_extent = object_node["bbox_extent"] + # Calculate the height + height = bbox_center[2] + bbox_extent[2] + return height + else: + print( + f"Object with ID '{object_id}' and tag '{object_tag}' missing bbox properties! Returning Default Height" + ) + return default_height # If the object tag is empty, we return the threhold height - print(f"Object with tag '{object_tag}' not found in world_graph") + print( + f"Object with ID '{object_id}' and tag '{object_tag}' not found in world_graph" + ) return default_height From 37636c30e9af52e0095e2fd3b88782babfe6789f Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 11 Sep 2024 13:05:22 -0700 Subject: [PATCH 23/82] Altered functionalities for getting joint angles and removed comments --- .../test_nav_pick_nav_place_skills.py | 2 +- .../experiments/skill_test/test_sem_place.py | 23 +++-------- .../spot_rl/envs/skill_manager.py | 39 ++++++++----------- .../spot_rl/utils/search_table_location.py | 1 - 4 files changed, 24 insertions(+), 41 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_nav_pick_nav_place_skills.py b/spot_rl_experiments/experiments/skill_test/test_nav_pick_nav_place_skills.py index 0c258044..7b231d01 100644 --- a/spot_rl_experiments/experiments/skill_test/test_nav_pick_nav_place_skills.py +++ b/spot_rl_experiments/experiments/skill_test/test_nav_pick_nav_place_skills.py @@ -20,7 +20,7 @@ ) spotskillmanager.nav("kitchen_counter") spotskillmanager.place( - "", is_local=True, visualize=False, enable_waypoint_estimation=True + None, is_local=True, visualize=False, enable_waypoint_estimation=True ) contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index 42caf431..d5e64cd9 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -30,7 +30,6 @@ is_local = False # Start testing contnue = True - INITIAL_ARM_JOINT_ANGLES = [0, -180, 180, 90, 0, -90] episode_ctr = 0 # Get EE Pose Initial in rest position spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() @@ -45,22 +44,12 @@ episode_log = {"actions": []} # mypy: ignore-errors spotskillmanager.spot.close_gripper() input("waiting for user to get ready with camera") - if enable_estimation_before_place: - is_local = True - spotskillmanager.place( - place_target, - is_local=is_local, - visualize=False, - enable_waypoint_estimation=True, - ) - else: - spotskillmanager.place( - place_target, - is_local=is_local, - visualize=True, - enable_waypoint_estimation=False, - ) - + spotskillmanager.place( + place_target, + is_local=enable_estimation_before_place, + visualize=False, + enable_waypoint_estimation=enable_estimation_before_place, + ) skill_log = spotskillmanager.place_controller.skill_result_log if "num_steps" not in skill_log: diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index de2d4a94..ddf2a0dc 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -114,7 +114,6 @@ def __init__( # Process the meta parameters self._use_mobile_pick = use_mobile_pick - print("USE MOBILE PICK VARIABLE IS SET TO :", self._use_mobile_pick) self.use_semantic_place = use_semantic_place # Create the spot object, init lease, and construct configs @@ -176,7 +175,6 @@ def __init_spot( ) if place_config is None: - # TODO: overwrite the config self.place_config = ( construct_config_for_semantic_place() if self.use_semantic_place @@ -469,6 +467,18 @@ def semanticpick( conditional_print(message=message, verbose=self.verbose) return status, message + def set_arm_joint_angles(self, place_target: str = None): + height = calculate_height(place_target) + if height < self.place_config.HEIGHT_THRESHOLD: + self.arm_joint_angles = ( + self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES + ) + else: + self.arm_joint_angles = ( + self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES + ) + return self.arm_joint_angles + @multimethod # type: ignore def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray = None, is_local: bool = False, visualize: bool = False, enable_waypoint_estimation: bool = False) -> Tuple[bool, str]: # type: ignore """ @@ -489,7 +499,7 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray verbose=self.verbose, ) - if enable_waypoint_estimation is False: + if not enable_waypoint_estimation: # Get the place target coordinates try: place_target_location = place_target_from_waypoint( @@ -509,15 +519,8 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray is_local = True else: message = "No place target specified, estimating point through heuristic" - height = calculate_height(place_target) - if height < self.place_config.HEIGHT_THRESHOLD: - self.arm_joint_angles = ( - self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES - ) - else: - self.arm_joint_angles = ( - self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES - ) + # Estimate Joint angles from height of the object using Concept graph + self.arm_joint_angles = self.set_arm_joint_angles(place_target) conditional_print(message=message, verbose=self.verbose) is_local = True @@ -582,16 +585,8 @@ def contrainedplace(self, object_target: str = None, ee_orientation_at_grasping: "next-to", ], f"Place skill does not support proposition of {proposition}" - height = calculate_height(object_target) - if height < self.place_config.HEIGHT_THRESHOLD: - print("HERE") - self.arm_joint_angles = ( - self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES - ) - else: - self.arm_joint_angles = ( - self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES - ) + # Estimate Joint angles from height of the object using Concept graph + self.arm_joint_angles = self.set_arm_joint_angles(object_target) # Esitmate the waypoint ( diff --git a/spot_rl_experiments/spot_rl/utils/search_table_location.py b/spot_rl_experiments/spot_rl/utils/search_table_location.py index 8c43560f..646e5b21 100644 --- a/spot_rl_experiments/spot_rl/utils/search_table_location.py +++ b/spot_rl_experiments/spot_rl/utils/search_table_location.py @@ -410,7 +410,6 @@ def detect_place_point_by_pcd_method( print(f"Intel point {selected_point}, Gripper Point {selected_point_in_gripper}") img_with_bbox = None - visualize = True if visualize: img_with_bbox = img.copy() for xy in corners_xys: From 04e75b2bcd0a14cfff06fdc4e4d19544aef98209 Mon Sep 17 00:00:00 2001 From: Achuthankrishna <74654704+Achuthankrishna@users.noreply.github.com> Date: Wed, 11 Sep 2024 13:28:11 -0700 Subject: [PATCH 24/82] Update skill_manager.py --- spot_rl_experiments/spot_rl/envs/skill_manager.py | 9 ++------- 1 file changed, 2 insertions(+), 7 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index ddf2a0dc..a55f5c54 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -470,14 +470,9 @@ def semanticpick( def set_arm_joint_angles(self, place_target: str = None): height = calculate_height(place_target) if height < self.place_config.HEIGHT_THRESHOLD: - self.arm_joint_angles = ( - self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES - ) + return self.place_config.GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES else: - self.arm_joint_angles = ( - self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES - ) - return self.arm_joint_angles + return self.place_config.GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES @multimethod # type: ignore def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray = None, is_local: bool = False, visualize: bool = False, enable_waypoint_estimation: bool = False) -> Tuple[bool, str]: # type: ignore From 14dc13ac05e214b74d3e0bd0aaf9aa6ff3928b01 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 11 Sep 2024 14:12:28 -0700 Subject: [PATCH 25/82] Moved CG File call to config --- spot_rl_experiments/configs/config.yaml | 2 ++ spot_rl_experiments/spot_rl/utils/utils.py | 4 ++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index df883485..605d4db5 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -107,6 +107,8 @@ MAX_ANG_DIST_OPEN_CLOSE_DRAWER: 3.73 # degrees OPEN_CLOSE_DRAWER_ACTION_SPACE_LENGTH: 8 OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.75 +# CG Json file +CONCEPT_GRAPH_FILE : "cfslam_object_relations.json" # Semantic Pick IMG_SRC: 0 # 1 for intel & 0 for gripper SEG_PORT: 21001 # port for segmentation service diff --git a/spot_rl_experiments/spot_rl/utils/utils.py b/spot_rl_experiments/spot_rl/utils/utils.py index 2db9e1d0..79cd8a9b 100644 --- a/spot_rl_experiments/spot_rl/utils/utils.py +++ b/spot_rl_experiments/spot_rl/utils/utils.py @@ -162,8 +162,9 @@ def arr2str(arr): def calculate_height(object_tag): # Iterate through each dictionary in the world_graph + default_config = construct_config_for_semantic_place() script_dir = os.path.dirname(__file__) # Directory of the script - json_file_path = os.path.join(script_dir, "cfslam_object_relations.json") + json_file_path = os.path.join(script_dir, default_config.CONCEPT_GRAPH_FILE) with open(json_file_path) as f: world_graph = json.load(f) @@ -179,7 +180,6 @@ def calculate_height(object_tag): return default_height # getting default height as threshold height in config - default_config = construct_config_for_semantic_place() default_height = default_config.HEIGHT_THRESHOLD for rel in world_graph: # Iterate through all keys in the dictionary From 76f9226a745a6340124dcc451ea5814a16b1e106 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 11 Sep 2024 14:34:12 -0700 Subject: [PATCH 26/82] minor changes in config --- spot_rl_experiments/configs/config.yaml | 6 ++++-- spot_rl_experiments/spot_rl/utils/utils.py | 9 ++------- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 605d4db5..6d0e7cb7 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -52,6 +52,9 @@ WEIGHTS: MRCNN_50: "weights/ikea_apricot_r50_normal_100_output_model_0003599.pth" DEBLURGAN: "weights/fpn_inception.h5" SAM: "weights/sam_vit_h_4b8939.pth" + +# CG Json file +CONCEPT_GRAPH_FILE : "cfslam_object_relations.json" DEVICE: "cuda:0" USE_REMOTE_SPOT: False @@ -107,8 +110,7 @@ MAX_ANG_DIST_OPEN_CLOSE_DRAWER: 3.73 # degrees OPEN_CLOSE_DRAWER_ACTION_SPACE_LENGTH: 8 OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.75 -# CG Json file -CONCEPT_GRAPH_FILE : "cfslam_object_relations.json" + # Semantic Pick IMG_SRC: 0 # 1 for intel & 0 for gripper SEG_PORT: 21001 # port for segmentation service diff --git a/spot_rl_experiments/spot_rl/utils/utils.py b/spot_rl_experiments/spot_rl/utils/utils.py index 79cd8a9b..24077d9e 100644 --- a/spot_rl_experiments/spot_rl/utils/utils.py +++ b/spot_rl_experiments/spot_rl/utils/utils.py @@ -161,28 +161,23 @@ def arr2str(arr): def calculate_height(object_tag): - # Iterate through each dictionary in the world_graph default_config = construct_config_for_semantic_place() - script_dir = os.path.dirname(__file__) # Directory of the script + script_dir = os.path.dirname(__file__) json_file_path = os.path.join(script_dir, default_config.CONCEPT_GRAPH_FILE) with open(json_file_path) as f: world_graph = json.load(f) - # Get Object id from string object_tag try: object_id_str, object_tag = object_tag.split("_", 1) - object_id = int(object_id_str) # Convert ID to integer + object_id = int(object_id_str) except ValueError: print(f"Invalid object_tag format: '{object_tag}'") - # Return default height if the format is invalid default_config = construct_config_for_semantic_place() default_height = default_config.HEIGHT_THRESHOLD return default_height - # getting default height as threshold height in config default_height = default_config.HEIGHT_THRESHOLD for rel in world_graph: - # Iterate through all keys in the dictionary for key, value in rel.items(): if isinstance(value, dict): if ( From cce721d19a3ad2357b211e5f34bf341ae918fe0e Mon Sep 17 00:00:00 2001 From: Kavit Shah <59990215+KavitShah1998@users.noreply.github.com> Date: Tue, 3 Sep 2024 11:52:03 -0700 Subject: [PATCH 27/82] Add Nexus UI Server code as a third-party (#207) --- .gitmodules | 3 +++ third_party/meta-fair-siro-phase-h1-server | 1 + 2 files changed, 4 insertions(+) create mode 160000 third_party/meta-fair-siro-phase-h1-server diff --git a/.gitmodules b/.gitmodules index 75aa2589..00f5ab3f 100644 --- a/.gitmodules +++ b/.gitmodules @@ -18,3 +18,6 @@ url = https://github.com/tusharsangam/FoundationPoseForSpotSim2Real branch = main ignore = dirty +[submodule "third_party/meta-fair-siro-phase-h1-server"] + path = third_party/meta-fair-siro-phase-h1-server + url = git@bitbucket.org:nexusproductions/meta-fair-siro-phase-h1-server.git diff --git a/third_party/meta-fair-siro-phase-h1-server b/third_party/meta-fair-siro-phase-h1-server new file mode 160000 index 00000000..7f5ad23e --- /dev/null +++ b/third_party/meta-fair-siro-phase-h1-server @@ -0,0 +1 @@ +Subproject commit 7f5ad23eaa18dec15a8befcf968134547a01208d From 8069e8288d209b44dee3dd80db91cfb89a2d1579 Mon Sep 17 00:00:00 2001 From: Kavit Shah <59990215+KavitShah1998@users.noreply.github.com> Date: Fri, 13 Sep 2024 11:31:26 -0700 Subject: [PATCH 28/82] Reset robot home at undock & dock given the operations are successful (#210) --- bd_spot_wrapper/spot_wrapper/home_robot.py | 2 +- bd_spot_wrapper/spot_wrapper/spot.py | 107 ++++++++++++++---- .../spot_rl/envs/skill_manager.py | 6 +- 3 files changed, 87 insertions(+), 28 deletions(-) diff --git a/bd_spot_wrapper/spot_wrapper/home_robot.py b/bd_spot_wrapper/spot_wrapper/home_robot.py index efd1b8d5..ac90b571 100644 --- a/bd_spot_wrapper/spot_wrapper/home_robot.py +++ b/bd_spot_wrapper/spot_wrapper/home_robot.py @@ -7,4 +7,4 @@ if __name__ == "__main__": spot = Spot("NavPoseMonitor") - spot.home_robot(write_to_file=True) + spot.write_home_robot() diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index 27cd3311..b311ab31 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -14,6 +14,7 @@ import os import os.path as osp import time +import traceback from collections import OrderedDict from typing import Any, Dict, List, Tuple @@ -235,15 +236,8 @@ def __init__(self, client_name_prefix): ) # Used to re-center origin of global frame - if osp.isfile(HOME_TXT): - with open(HOME_TXT) as f: - data = f.read() - self.global_T_home = np.array([float(d) for d in data.split(", ")[:9]]) - self.global_T_home = self.global_T_home.reshape([3, 3]) - self.robot_recenter_yaw = float(data.split(", ")[-1]) - else: - self.global_T_home = None - self.robot_recenter_yaw = None + # Read home.txt file and port its contents. + (self.global_T_home, self.robot_recenter_yaw) = self.read_home_robot() # Print the battery charge level of the robot self.loginfo(f"Current battery charge: {self.get_battery_charge()}%") @@ -1126,19 +1120,45 @@ def _get_local_T_global(self, x=None, y=None, yaw=None): ) return local_T_global - def home_robot(self, write_to_file: bool = False): - print(f"Updating robot pose w.r.t home. write_to_file={write_to_file}") - x, y, yaw = self.get_xy_yaw(use_boot_origin=True) + def write_home_robot(self): + """ + Updates home frame to current robot frame. Spot-sim2real uses home frame as its global frame for all skills + """ + # Init local variables + global_T_home = None + robot_recenter_yaw = None + + print("Updating robot pose w.r.t home in home.txt") + _, _, yaw = self.get_xy_yaw(use_boot_origin=True) # vision_T_body local_T_global = self._get_local_T_global() - self.global_T_home = np.linalg.inv(local_T_global) - self.robot_recenter_yaw = yaw + global_T_home = np.linalg.inv(local_T_global) + robot_recenter_yaw = yaw - if write_to_file: - as_string = list(self.global_T_home.flatten()) + [yaw] + try: + as_string = list(global_T_home.flatten()) + [robot_recenter_yaw] as_string = f"{as_string}"[1:-1] # [1:-1] removes brackets with open(HOME_TXT, "w") as f: f.write(as_string) - self.loginfo(f"Wrote:\n{as_string}\nto: {HOME_TXT}") + print(f"Wrote: \n{as_string}\nto: {HOME_TXT}") + except Exception: + print( + "Encountered exception while persisting global_T_home into home.txt file", + traceback.print_exc(), + ) + + def read_home_robot(self): + """Returns - Tuple of global_T_home & robot_recenter_yaw. Both will be None if Home.txt file doesn't exist""" + print("Reading robot pose w.r.t home from home.txt") + global_T_home = None + robot_recenter_yaw = None + if osp.isfile(HOME_TXT): + with open(HOME_TXT) as f: + data = f.read() + global_T_home = np.array([float(d) for d in data.split(", ")[:9]]) + global_T_home = global_T_home.reshape([3, 3]) + robot_recenter_yaw = float(data.split(", ")[-1]) + + return (global_T_home, robot_recenter_yaw) def get_base_transform_to(self, child_frame): kin_state = self.robot_state_client.get_robot_state().kinematic_state @@ -1147,7 +1167,7 @@ def get_base_transform_to(self, child_frame): ).parent_tform_child return kin_state.position, kin_state.rotation - def dock(self, dock_id: int = DOCK_ID, home_robot: bool = False) -> None: + def dock(self, dock_id: int = DOCK_ID, home_robot: bool = True) -> None: """ Dock the robot to the specified dock `blocking_dock_robot` will also move the robot to the dock if the dock is in view @@ -1157,11 +1177,18 @@ def dock(self, dock_id: int = DOCK_ID, home_robot: bool = False) -> None: dock_id: The dock to dock to home_robot: Whether to reset home the robot after docking """ + # If spot is NOT on docking station, blocking_dock_robot will raise an exception + # and home_txt will not get updated blocking_dock_robot(self.robot, dock_id) if home_robot: - self.home_robot(write_to_file=True) + print("Will Home Robot") + self.write_home_robot() def undock(self): + """ + DO NOT USE THIS FUNCTION DIRECTLY. + USE def power_robot() INSTEAD. + """ blocking_undock(self.robot) def power_robot(self): @@ -1170,32 +1197,64 @@ def power_robot(self): """ self.power_on() + most_recent_global_T_home_and_yaw_tuple = self.read_home_robot() + exception_free_undocking: bool = True + # Undock if docked, otherwise stand up try: self.undock() except Exception: print("Undocking failed: just standing up instead...") + exception_free_undocking = False self.blocking_stand() - def shutdown(self, should_dock: bool = False) -> None: + if exception_free_undocking: + print( + "Robot undocked from docking station without exceptions, will update self.global_T_home" + ) + ( + self.global_T_home, + self.robot_recenter_yaw, + ) = most_recent_global_T_home_and_yaw_tuple + else: + print("Exception occured, will NOT update self.global_T_home") + + def shutdown(self, should_dock: bool = False, home_robot=True) -> None: """ Stops the robot and docks it if should_dock is True else sits the robot down Args: should_dock: bool indicating whether to dock the robot or not + home_robot: bool indicating whether home.txt file should be updated or not """ + print(f"Called spot.shutdown() with should_dock={should_dock}") try: + successful_docking = True if should_dock: - print("Executing automatic docking") + print( + f"Executing automatic docking for Dock_ID={DOCK_ID}, home_robot={home_robot}" + ) dock_start_time = time.time() while time.time() - dock_start_time < 2: try: - self.dock(dock_id=DOCK_ID, home_robot=True) + print("Searching for dock") + self.dock(dock_id=DOCK_ID, home_robot=home_robot) + successful_docking = True except Exception: + print( + "Exception occured while docking : ", traceback.print_exc() + ) print("Dock not found... trying again") + successful_docking = False time.sleep(0.1) - else: - print("Will sit down here") + + print( + f"Docking status - should_dock={should_dock} , successful_docking={successful_docking}" + ) + if not (should_dock and successful_docking): + print( + "Did not reset home due as didn't dock on base. \nWill sit down here." + ) self.sit() finally: self.power_off() diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index a55f5c54..0935291e 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -733,7 +733,7 @@ def get_env(self): "Get the env for the ease of the access" return self.nav_controller.env - def dock(self): + def dock(self, home_robot=True): # Stow back the arm self.get_env().reset_arm() @@ -744,7 +744,7 @@ def dock(self): status, message = self.nav("dock") # Dock - self.spot.shutdown(should_dock=True) + self.spot.shutdown(should_dock=True, home_robot=home_robot) except Exception: message = "Error encountered while docking" conditional_print(message=message, verbose=self.verbose) @@ -777,4 +777,4 @@ def dock(self): spotskillmanager.place("test_place_left") # Navigate to dock and shutdown - spotskillmanager.dock() + spotskillmanager.dock(home_robot=True) From f78c3caa9441bb044dc26bc1fbb74be016fe8959 Mon Sep 17 00:00:00 2001 From: Joanne Truong Date: Mon, 16 Sep 2024 10:04:25 -0700 Subject: [PATCH 29/82] add fix for if cg file doesn't exist --- spot_rl_experiments/spot_rl/utils/utils.py | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/spot_rl_experiments/spot_rl/utils/utils.py b/spot_rl_experiments/spot_rl/utils/utils.py index 24077d9e..763321d4 100644 --- a/spot_rl_experiments/spot_rl/utils/utils.py +++ b/spot_rl_experiments/spot_rl/utils/utils.py @@ -164,19 +164,21 @@ def calculate_height(object_tag): default_config = construct_config_for_semantic_place() script_dir = os.path.dirname(__file__) json_file_path = os.path.join(script_dir, default_config.CONCEPT_GRAPH_FILE) + default_height = default_config.HEIGHT_THRESHOLD - with open(json_file_path) as f: - world_graph = json.load(f) + if osp.isfile(json_file_path): + with open(json_file_path) as f: + world_graph = json.load(f) + else: + print(f"Concept Graph File does not exist. Using default height: {default_height}") + return default_height try: object_id_str, object_tag = object_tag.split("_", 1) object_id = int(object_id_str) except ValueError: print(f"Invalid object_tag format: '{object_tag}'") - default_config = construct_config_for_semantic_place() - default_height = default_config.HEIGHT_THRESHOLD return default_height - default_height = default_config.HEIGHT_THRESHOLD for rel in world_graph: for key, value in rel.items(): if isinstance(value, dict): From 999d8ba67f99616aa0c46cbf181b0d5e6d995569 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Mon, 16 Sep 2024 14:02:11 -0400 Subject: [PATCH 30/82] fix try except --- spot_rl_experiments/spot_rl/utils/utils.py | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) diff --git a/spot_rl_experiments/spot_rl/utils/utils.py b/spot_rl_experiments/spot_rl/utils/utils.py index 763321d4..05ed3589 100644 --- a/spot_rl_experiments/spot_rl/utils/utils.py +++ b/spot_rl_experiments/spot_rl/utils/utils.py @@ -170,13 +170,15 @@ def calculate_height(object_tag): with open(json_file_path) as f: world_graph = json.load(f) else: - print(f"Concept Graph File does not exist. Using default height: {default_height}") + print( + f"Concept Graph File does not exist. Using default height: {default_height}" + ) return default_height try: object_id_str, object_tag = object_tag.split("_", 1) object_id = int(object_id_str) - except ValueError: - print(f"Invalid object_tag format: '{object_tag}'") + except Exception as e: + print(f"Invalid object_tag format: {object_tag} due to {e}") return default_height for rel in world_graph: From 2107c0d35ae0f9754a5892c5a10b4d5d06a483df Mon Sep 17 00:00:00 2001 From: Kavit Shah <59990215+KavitShah1998@users.noreply.github.com> Date: Mon, 16 Sep 2024 13:35:59 -0700 Subject: [PATCH 31/82] Add time profiler class (#203) --- .../utils/time_profiler.py | 111 ++++++++++++++++++ third_party/meta-fair-siro-phase-h1-server | 2 +- 2 files changed, 112 insertions(+), 1 deletion(-) create mode 100644 perception_and_utils_root/perception_and_utils/utils/time_profiler.py diff --git a/perception_and_utils_root/perception_and_utils/utils/time_profiler.py b/perception_and_utils_root/perception_and_utils/utils/time_profiler.py new file mode 100644 index 00000000..4d786449 --- /dev/null +++ b/perception_and_utils_root/perception_and_utils/utils/time_profiler.py @@ -0,0 +1,111 @@ +import time + +WINDOW_SIZE = 5 + + +class TimeProfiler: + def __init__(self, window_size=WINDOW_SIZE): + """ + A class to measure and calculate frame rates. + + The class uses a timer to track the time elapsed between frames, + and calculates the instantaneous and average frame rates based on this data. + The instantaneous frame rate is the frame rate at the current moment, + while the average frame rate is the average frame rate since the start of the timer. + + The class also provides a windowed average frame rate, + which is the average frame rate over a specified number of frames. + This can be useful for smoothing out fluctuations in the frame rate + and getting a more accurate picture of the application's performance. + + How to Use: + 1. Create an instance of the TimeProfiler class, passing a window size as an argument (optional). + 2. Call the start method to start the timer. + 3. Perform some operation that you want to measure the frame rate for. + 4. Call the stop method to stop the timer and get the instantaneous and average frame rates. + 5. If you want to reset the profiler, call the reset method. + + Example: + profiler = TimeProfiler() + + while (condition): + # Start the profiler + profiler.start() + + # Perform some operation here + + # Stop the profiler and get the frame rates + instantaneous_frame_rate, average_frame_rate_from_start, average_frame_rate_over_window = profiler.stop() + + """ + self.start_time = None + self.total_time_ns = 0 + self.total_frames = 0 + self.window_size = window_size + self.window = [0.0] * self.window_size + self.window_index = 0 + self.start_index = 0 + + def start(self): + self.start_time = time.perf_counter_ns() + + def stop(self): + """ + Stop the timer and return the frame rates (instantaneous, and window average) + Returns: + instantaneous_frame_rate: float + average_frame_rate_from_start: float + average_frame_rate_over_window: float + """ + end_time = time.perf_counter_ns() + elapsed_time_ns = end_time - self.start_time + if elapsed_time_ns > 0: + self.total_frames += 1 + self.total_time_ns += elapsed_time_ns + + # Add the current frame time to the window + self.window[self.window_index] = elapsed_time_ns + self.window_index = (self.window_index + 1) % self.window_size + + # If the window is full, increment the start index + if self.window_index == self.start_index: + self.start_index = (self.start_index + 1) % self.window_size + + # Compute instant. rate + instantaneous_frame_rate = 1 / (elapsed_time_ns * 1e-9) + + # Compute average frame rate from start + average_frame_rate_from_start = self.total_frames / ( + self.total_time_ns * 1e-9 + ) + + # Compute average frame rate over the window + average_frame_rate_over_window = 0.0 + if self.window_index >= self.start_index: + average_frame_rate_over_window = ( + self.window_index - self.start_index + 1 + ) / (sum(self.window[self.start_index : self.window_index + 1]) * 1e-9) + else: + average_frame_rate_over_window = ( + self.window_size - self.start_index + self.window_index + 1 + ) / ( + sum( + self.window[self.start_index :] + + self.window[: self.window_index + 1] + ) + * 1e-9 + ) + + return ( + instantaneous_frame_rate, + average_frame_rate_from_start, + average_frame_rate_over_window, + ) + + def reset(self): + self.start_time = None + self.total_time_ns = 0 + self.total_frames = 0 + self.window_index = 0 + self.start_index = 0 + self.window = [0] * self.window_size diff --git a/third_party/meta-fair-siro-phase-h1-server b/third_party/meta-fair-siro-phase-h1-server index 7f5ad23e..28e2e8a3 160000 --- a/third_party/meta-fair-siro-phase-h1-server +++ b/third_party/meta-fair-siro-phase-h1-server @@ -1 +1 @@ -Subproject commit 7f5ad23eaa18dec15a8befcf968134547a01208d +Subproject commit 28e2e8a388fc4515b8f81dc61e1485fba041ac6c From 803d7a3d02185e15e67c5777fb5490d776481336 Mon Sep 17 00:00:00 2001 From: Kavit Shah <59990215+KavitShah1998@users.noreply.github.com> Date: Mon, 16 Sep 2024 14:03:56 -0700 Subject: [PATCH 32/82] Revert "Add time profiler class (#203)" (#215) This reverts commit 2107c0d35ae0f9754a5892c5a10b4d5d06a483df. --- .../utils/time_profiler.py | 111 ------------------ third_party/meta-fair-siro-phase-h1-server | 2 +- 2 files changed, 1 insertion(+), 112 deletions(-) delete mode 100644 perception_and_utils_root/perception_and_utils/utils/time_profiler.py diff --git a/perception_and_utils_root/perception_and_utils/utils/time_profiler.py b/perception_and_utils_root/perception_and_utils/utils/time_profiler.py deleted file mode 100644 index 4d786449..00000000 --- a/perception_and_utils_root/perception_and_utils/utils/time_profiler.py +++ /dev/null @@ -1,111 +0,0 @@ -import time - -WINDOW_SIZE = 5 - - -class TimeProfiler: - def __init__(self, window_size=WINDOW_SIZE): - """ - A class to measure and calculate frame rates. - - The class uses a timer to track the time elapsed between frames, - and calculates the instantaneous and average frame rates based on this data. - The instantaneous frame rate is the frame rate at the current moment, - while the average frame rate is the average frame rate since the start of the timer. - - The class also provides a windowed average frame rate, - which is the average frame rate over a specified number of frames. - This can be useful for smoothing out fluctuations in the frame rate - and getting a more accurate picture of the application's performance. - - How to Use: - 1. Create an instance of the TimeProfiler class, passing a window size as an argument (optional). - 2. Call the start method to start the timer. - 3. Perform some operation that you want to measure the frame rate for. - 4. Call the stop method to stop the timer and get the instantaneous and average frame rates. - 5. If you want to reset the profiler, call the reset method. - - Example: - profiler = TimeProfiler() - - while (condition): - # Start the profiler - profiler.start() - - # Perform some operation here - - # Stop the profiler and get the frame rates - instantaneous_frame_rate, average_frame_rate_from_start, average_frame_rate_over_window = profiler.stop() - - """ - self.start_time = None - self.total_time_ns = 0 - self.total_frames = 0 - self.window_size = window_size - self.window = [0.0] * self.window_size - self.window_index = 0 - self.start_index = 0 - - def start(self): - self.start_time = time.perf_counter_ns() - - def stop(self): - """ - Stop the timer and return the frame rates (instantaneous, and window average) - Returns: - instantaneous_frame_rate: float - average_frame_rate_from_start: float - average_frame_rate_over_window: float - """ - end_time = time.perf_counter_ns() - elapsed_time_ns = end_time - self.start_time - if elapsed_time_ns > 0: - self.total_frames += 1 - self.total_time_ns += elapsed_time_ns - - # Add the current frame time to the window - self.window[self.window_index] = elapsed_time_ns - self.window_index = (self.window_index + 1) % self.window_size - - # If the window is full, increment the start index - if self.window_index == self.start_index: - self.start_index = (self.start_index + 1) % self.window_size - - # Compute instant. rate - instantaneous_frame_rate = 1 / (elapsed_time_ns * 1e-9) - - # Compute average frame rate from start - average_frame_rate_from_start = self.total_frames / ( - self.total_time_ns * 1e-9 - ) - - # Compute average frame rate over the window - average_frame_rate_over_window = 0.0 - if self.window_index >= self.start_index: - average_frame_rate_over_window = ( - self.window_index - self.start_index + 1 - ) / (sum(self.window[self.start_index : self.window_index + 1]) * 1e-9) - else: - average_frame_rate_over_window = ( - self.window_size - self.start_index + self.window_index + 1 - ) / ( - sum( - self.window[self.start_index :] - + self.window[: self.window_index + 1] - ) - * 1e-9 - ) - - return ( - instantaneous_frame_rate, - average_frame_rate_from_start, - average_frame_rate_over_window, - ) - - def reset(self): - self.start_time = None - self.total_time_ns = 0 - self.total_frames = 0 - self.window_index = 0 - self.start_index = 0 - self.window = [0] * self.window_size diff --git a/third_party/meta-fair-siro-phase-h1-server b/third_party/meta-fair-siro-phase-h1-server index 28e2e8a3..7f5ad23e 160000 --- a/third_party/meta-fair-siro-phase-h1-server +++ b/third_party/meta-fair-siro-phase-h1-server @@ -1 +1 @@ -Subproject commit 28e2e8a388fc4515b8f81dc61e1485fba041ac6c +Subproject commit 7f5ad23eaa18dec15a8befcf968134547a01208d From 39a6199236f4000ada568225813d412321ed942c Mon Sep 17 00:00:00 2001 From: Kavit Shah <59990215+KavitShah1998@users.noreply.github.com> Date: Mon, 16 Sep 2024 14:58:15 -0700 Subject: [PATCH 33/82] Add time profiler class (#216) --- .../utils/time_profiler.py | 111 ++++++++++++++++++ 1 file changed, 111 insertions(+) create mode 100644 perception_and_utils_root/perception_and_utils/utils/time_profiler.py diff --git a/perception_and_utils_root/perception_and_utils/utils/time_profiler.py b/perception_and_utils_root/perception_and_utils/utils/time_profiler.py new file mode 100644 index 00000000..4d786449 --- /dev/null +++ b/perception_and_utils_root/perception_and_utils/utils/time_profiler.py @@ -0,0 +1,111 @@ +import time + +WINDOW_SIZE = 5 + + +class TimeProfiler: + def __init__(self, window_size=WINDOW_SIZE): + """ + A class to measure and calculate frame rates. + + The class uses a timer to track the time elapsed between frames, + and calculates the instantaneous and average frame rates based on this data. + The instantaneous frame rate is the frame rate at the current moment, + while the average frame rate is the average frame rate since the start of the timer. + + The class also provides a windowed average frame rate, + which is the average frame rate over a specified number of frames. + This can be useful for smoothing out fluctuations in the frame rate + and getting a more accurate picture of the application's performance. + + How to Use: + 1. Create an instance of the TimeProfiler class, passing a window size as an argument (optional). + 2. Call the start method to start the timer. + 3. Perform some operation that you want to measure the frame rate for. + 4. Call the stop method to stop the timer and get the instantaneous and average frame rates. + 5. If you want to reset the profiler, call the reset method. + + Example: + profiler = TimeProfiler() + + while (condition): + # Start the profiler + profiler.start() + + # Perform some operation here + + # Stop the profiler and get the frame rates + instantaneous_frame_rate, average_frame_rate_from_start, average_frame_rate_over_window = profiler.stop() + + """ + self.start_time = None + self.total_time_ns = 0 + self.total_frames = 0 + self.window_size = window_size + self.window = [0.0] * self.window_size + self.window_index = 0 + self.start_index = 0 + + def start(self): + self.start_time = time.perf_counter_ns() + + def stop(self): + """ + Stop the timer and return the frame rates (instantaneous, and window average) + Returns: + instantaneous_frame_rate: float + average_frame_rate_from_start: float + average_frame_rate_over_window: float + """ + end_time = time.perf_counter_ns() + elapsed_time_ns = end_time - self.start_time + if elapsed_time_ns > 0: + self.total_frames += 1 + self.total_time_ns += elapsed_time_ns + + # Add the current frame time to the window + self.window[self.window_index] = elapsed_time_ns + self.window_index = (self.window_index + 1) % self.window_size + + # If the window is full, increment the start index + if self.window_index == self.start_index: + self.start_index = (self.start_index + 1) % self.window_size + + # Compute instant. rate + instantaneous_frame_rate = 1 / (elapsed_time_ns * 1e-9) + + # Compute average frame rate from start + average_frame_rate_from_start = self.total_frames / ( + self.total_time_ns * 1e-9 + ) + + # Compute average frame rate over the window + average_frame_rate_over_window = 0.0 + if self.window_index >= self.start_index: + average_frame_rate_over_window = ( + self.window_index - self.start_index + 1 + ) / (sum(self.window[self.start_index : self.window_index + 1]) * 1e-9) + else: + average_frame_rate_over_window = ( + self.window_size - self.start_index + self.window_index + 1 + ) / ( + sum( + self.window[self.start_index :] + + self.window[: self.window_index + 1] + ) + * 1e-9 + ) + + return ( + instantaneous_frame_rate, + average_frame_rate_from_start, + average_frame_rate_over_window, + ) + + def reset(self): + self.start_time = None + self.total_time_ns = 0 + self.total_frames = 0 + self.window_index = 0 + self.start_index = 0 + self.window = [0] * self.window_size From 71f9c7b800b989bbbe511c008f2195f098261faa Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 14 Aug 2024 15:49:31 -0400 Subject: [PATCH 34/82] add semantic place ee --- bd_spot_wrapper/spot_wrapper/spot.py | 35 +++- spot_rl_experiments/configs/config.yaml | 9 + .../skill_test/test_sem_place_ee.py | 36 ++++ spot_rl_experiments/spot_rl/envs/base_env.py | 40 +++++ spot_rl_experiments/spot_rl/envs/place_env.py | 159 ++++++++++++++++++ .../spot_rl/envs/skill_manager.py | 17 +- spot_rl_experiments/spot_rl/real_policy.py | 57 +++++++ .../spot_rl/skills/atomic_skills.py | 37 +++- .../utils/cparamssemanticplaceee.yaml | 19 +++ .../utils/pytorch_to_torchscript.py | 1 - 10 files changed, 403 insertions(+), 7 deletions(-) create mode 100644 spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py create mode 100644 spot_rl_experiments/utils/cparamssemanticplaceee.yaml diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index b311ab31..8d986b83 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -406,7 +406,7 @@ def move_gripper_to_points( return success_status def move_gripper_to_point( - self, point, rotation, seconds_to_goal=3.0, timeout_sec=10 + self, point, rotation, seconds_to_goal=3.0, timeout_sec=10, return_cmd=False ): """ Moves EE to a point relative to body frame @@ -450,6 +450,9 @@ def move_gripper_to_point( command = robot_command_pb2.RobotCommand( synchronized_command=synchronized_command ) + if return_cmd: + return command + cmd_id = self.command_client.robot_command(command) success_status = self.block_until_arm_arrives(cmd_id, timeout_sec=timeout_sec) @@ -1080,6 +1083,36 @@ def set_base_vel_and_arm_pos( ) return cmd_id + def set_base_vel_and_arm_ee_pos( + self, + x_vel, + y_vel, + ang_vel, + arm_ee_action, + travel_time, + disable_obstacle_avoidance=False, + ): + print("in set_base_vel_and_arm_ee_pos") + base_cmd = self.set_base_velocity( + x_vel, + y_vel, + ang_vel, + vel_time=travel_time, + disable_obstacle_avoidance=disable_obstacle_avoidance, + return_cmd=True, + ) + arm_cmd = self.move_gripper_to_point( + point=arm_ee_action[0:3], + rotation=list(arm_ee_action[3:]), + seconds_to_goal=travel_time, + return_cmd=True, + ) + synchro_command = RobotCommandBuilder.build_synchro_command(base_cmd, arm_cmd) + cmd_id = self.command_client.robot_command( + synchro_command, end_time_secs=time.time() + travel_time + ) + return cmd_id + def get_xy_yaw(self, use_boot_origin=False, robot_state=None): """ Returns the relative x and y distance from start, as well as relative heading diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 6d0e7cb7..bca51051 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -19,6 +19,9 @@ WEIGHTS_TORCHSCRIPT: # Semantic place SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" + # Semantic place EE + SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" + # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -41,6 +44,9 @@ WEIGHTS: # Semantic place SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" + # Semantic place EE + SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" + # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -104,6 +110,9 @@ PLACE_ACTION_SPACE_LENGTH: 4 SEMANTIC_PLACE_ACTION_SPACE_LENGTH: 9 SEMANTIC_PLACE_JOINT_BLACKLIST: [3] +# Semantic Place EE +SEMANTIC_PLACE_EE_ACTION_SPACE_LENGTH: 10 + # Open Close Drawer env MAX_LIN_DIST_OPEN_CLOSE_DRAWER: 0.0 MAX_ANG_DIST_OPEN_CLOSE_DRAWER: 3.73 # degrees diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py new file mode 100644 index 00000000..60e214df --- /dev/null +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -0,0 +1,36 @@ +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. +import rospy +from perception_and_utils.utils.generic_utils import map_user_input_to_boolean +from spot_rl.envs.skill_manager import SpotSkillManager + +if __name__ == "__main__": + # Know which location we are doing experiments + in_fre_lab = map_user_input_to_boolean("Are you JT in FRE? Y/N ") + enable_estimation_before_place = map_user_input_to_boolean( + "Enable estimation before place? Y/N " + ) + + if in_fre_lab: + # at FRE + place_target = "coffee_table" + else: + # at NYC + place_target = "test_desk" + + spotskillmanager = SpotSkillManager( + use_mobile_pick=False, use_semantic_place=True, use_semantic_place_ee=True + ) + + is_local = False + if enable_estimation_before_place: + place_target = None + is_local = True + + # Start testing + contnue = True + while contnue: + rospy.set_param("is_gripper_blocked", 0) + spotskillmanager.place(place_target, is_local=is_local, visualize=True) + contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 451eb7ad..f47843d3 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -102,6 +102,22 @@ def rescale_actions(actions, action_thresh=0.05, silence_only=False): return actions +def rescale_arm_ee_actions(actions, action_thresh=0.05, silence_only=False): + actions = np.clip(actions, -1, 1) + # Silence low actions + actions[np.abs(actions) < action_thresh] = 0.0 + if silence_only: + return actions + + # Remap action scaling to compensate for silenced values + action_offsets = np.ones_like(actions) * action_thresh + action_offsets[actions < 0] = -action_offsets[actions < 0] + action_offsets[actions == 0] = 0 + actions = (actions - np.array(action_offsets)) / (1.0 - action_thresh) + + return actions + + class SpotBaseEnv(SpotRobotSubscriberMixin, gym.Env): node_name = "spot_reality_gym" no_raw = True @@ -291,6 +307,7 @@ def step( # noqa :param base_action: np.array of velocities (linear, angular) :param arm_action: np.array of radians denoting how each joint is to be moved + :param arm_ee_action: np.array of IK control based on x, y, z, roll, pitch, yaw :param grasp: whether to call the grasp_hand_depth() method :param place: whether to call the open_gripper() method :param semantic_place: whether to call the open_gripper() method, but distable turning wrist behavior @@ -301,6 +318,7 @@ def step( # noqa # Get Base & Arm actions, grasp and place from action dictionary base_action = action_dict.get("base_action", None) arm_action = action_dict.get("arm_action", None) + arm_ee_action = action_dict.get("arm_ee_action", None) semantic_place = action_dict.get("semantic_place", False) grasp = action_dict.get("grasp", False) place = action_dict.get("place", False) @@ -405,6 +423,19 @@ def step( # noqa else: arm_action = None + if arm_ee_action is not None: + arm_ee_action = rescale_arm_ee_actions(arm_ee_action) + if np.count_nonzero(arm_ee_action) > 0: + # TODO: semantic place ee: move this to config + arm_ee_action *= 0.1 + xyz, rpy = self.spot.get_ee_pos_in_body_frame() + cur_ee_pose = np.concatenate((xyz, rpy), axis=0) + arm_ee_action += cur_ee_pose + # TODO: semantic place ee: move this to config + arm_ee_action = np.clip(arm_ee_action, 0.5, 0.5) + else: + arm_ee_action = None + if not (grasp or place): if self.slowdown_base > -1 and base_action is not None: # self.ctrl_hz = self.slowdown_base @@ -421,6 +452,15 @@ def step( # noqa travel_time=self.config.ARM_TRAJECTORY_TIME_IN_SECONDS, disable_obstacle_avoidance=disable_oa, ) + elif base_action is not None and arm_ee_action is not None: + print("input base_action velocity:", arr2str(base_action)) + print("input arm_ee_action:", arr2str(arm_ee_action)) + self.spot.set_base_vel_and_arm_ee_pos( + *base_action, + arm_ee_action, + travel_time=self.config.ARM_TRAJECTORY_TIME_IN_SECONDS, + disable_obstacle_avoidance=disable_oa, + ) elif base_action is not None: self.spot.set_base_velocity( *base_action, diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index 07a0b23b..196f9d5e 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -220,3 +220,162 @@ def get_observations(self): "is_holding": np.ones((1,)), } return observations + + +class SpotSemanticPlaceEEEnv(SpotBaseEnv): + """This is Spot semantic place class""" + + def __init__(self, config, spot: Spot): + # We set the initial arm joints + config.INITIAL_ARM_JOINT_ANGLES = copy.deepcopy( + config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE + ) + super().__init__(config, spot) + + # Define the place variables + self.place_target = None + self.place_target_is_local = False + self.placed = False + + # Set gripper variables + self.ee_gripper_offset = mn.Vector3(config.EE_GRIPPER_OFFSET) + + # Define the observation variables + self.initial_ee_pose = None + self.target_object_pose = None + + # Overwrite joint limits for semantic_place skills + self.arm_lower_limits = np.deg2rad(config.ARM_LOWER_LIMITS_SEMANTIC_PLACE) + self.arm_upper_limits = np.deg2rad(config.ARM_UPPER_LIMITS_SEMANTIC_PLACE) + + # Place steps + self._time_step = 0 + + def decide_init_arm_joint(self, ee_orientation_at_grasping): + """Decide the place location""" + + # User does not set the gripper orientation + if ee_orientation_at_grasping is None: + self.initial_arm_joint_angles = np.deg2rad( + self.config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE + ) + else: + # Get the pitch and yaw + pitch = ee_orientation_at_grasping[1] + yaw = ee_orientation_at_grasping[2] + print("ee_orientation_at_grasping:", ee_orientation_at_grasping) + if abs(pitch) <= 1.309: # 75 degree in pitch + if yaw > 0: # gripper is in object's right hand side + self.initial_arm_joint_angles = np.deg2rad( + self.config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE + ) + else: # gripper is in object's left hand side + self.initial_arm_joint_angles = np.deg2rad( + self.config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_LEFT_HAND + ) + else: + self.initial_arm_joint_angles = np.deg2rad( + self.config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_TOP_DOWN + ) + + def reset(self, place_target, target_is_local=False, *args, **kwargs): + assert place_target is not None + self.place_target = np.array(place_target) + self.place_target_is_local = target_is_local + + self._time_step = 0 + + # Decide the reset arm angle and then reset the arm + self.decide_init_arm_joint(kwargs["ee_orientation_at_grasping"]) + self.reset_arm() + + # We wait for a second to let the arm in the placing + # ready location + time.sleep(1.0) + # Sometimes, there will be a bit of mistchmatch of joints after resetting + # So we can reset the arm again here using the following + # ee_position, ee_orientation = self.spot.get_ee_pos_in_body_frame() + # self.spot.move_gripper_to_point(ee_position, [np.pi / 2, 0, 0]) + + # Set the initial ee pose + self.initial_ee_pose = self.spot.get_ee_quaternion_in_body_frame() + # Set the target pose + self.target_object_pose = self.spot.get_ee_quaternion_in_body_frame() + # Automatically use intelrealsense camera + rospy.set_param("is_gripper_blocked", 1) + observations = super().reset() + rospy.set_param("is_whiten_black", False) + self.placed = False + return observations + + def step(self, action_dict: Dict[str, Any], *args, **kwargs): + + place = False + + # Place command is issued if the place action is smaller than zero + place = action_dict.get("grip_action", None) <= 0.0 + + # If the time steps have been passed for 50 steps and gripper is in the desired place location + cur_place_sensor_xyz = self.get_place_sensor(True) + if ( + abs(cur_place_sensor_xyz[2]) < 0.05 + and np.linalg.norm( + np.array([cur_place_sensor_xyz[0], cur_place_sensor_xyz[1]]) + ) + < 0.25 + and self._time_step >= 50 + ): + place = True + + # If the time steps have been passed for 75 steps, we will just place the object + if self._time_step >= 75: + place = True + + self._time_step += 1 + + # Write into action dict + action_dict["place"] = place + action_dict["semantic_place"] = place + + # Set the travel time scale so that the arm movement is smooth + return super().step( + action_dict, travel_time_scale=1.0 / 0.9 * 1.75, *args, **kwargs + ) + + def get_success(self, observations): + return self.place_attempted + + def get_observations(self): + assert self.initial_ee_pose is not None + assert self.target_object_pose is not None + + # Get the gaol sensor + obj_goal_sensor = self.get_place_sensor(True) + + # Get the delta ee orientation + current_gripper_orientation = self.spot.get_ee_quaternion_in_body_frame() + delta_ee = angle_between_quat(self.initial_ee_pose, current_gripper_orientation) + delta_ee = np.array([delta_ee], dtype=np.float32) + + # Get the delta object orientation + delta_obj = angle_between_quat( + self.target_object_pose, current_gripper_orientation + ) + # remove the offset from the base to object + delta_obj = np.array( + [delta_obj - abs(self.spot.get_cur_ee_pose_offset())], dtype=np.float32 + ) + # Get the jaw image + arm_depth, _ = self.get_gripper_images() + + # Get ee's pose -- x,y,z + xyz, _ = self.spot.get_ee_pos_in_body_frame() + observations = { + "obj_goal_sensor": obj_goal_sensor, + "relative_initial_ee_orientation": delta_ee, + "relative_target_object_orientation": delta_obj, + "articulated_agent_jaw_depth": arm_depth, + "ee_pos": xyz, + "is_holding": np.ones((1,)), + } + return observations diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 0935291e..b033df1d 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -17,6 +17,7 @@ Place, SemanticPick, SemanticPlace, + SemanticPlaceEE, ) from spot_rl.utils.construct_configs import ( construct_config_for_gaze, @@ -103,6 +104,7 @@ def __init__( open_close_drawer_config=None, use_mobile_pick: bool = False, use_semantic_place: bool = False, + use_semantic_place_ee: bool = False, verbose: bool = True, use_policies: bool = True, ): @@ -115,6 +117,7 @@ def __init__( # Process the meta parameters self._use_mobile_pick = use_mobile_pick self.use_semantic_place = use_semantic_place + self.use_semantic_place_ee = use_semantic_place_ee # Create the spot object, init lease, and construct configs self.__init_spot( @@ -175,9 +178,10 @@ def __init_spot( ) if place_config is None: + # TODO: overwrite the config self.place_config = ( construct_config_for_semantic_place() - if self.use_semantic_place + if self.use_semantic_place or self.use_semantic_place_ee else construct_config_for_place() ) else: @@ -210,9 +214,14 @@ def __initiate_controllers(self, use_policies: bool = True): use_mobile_pick=self._use_mobile_pick, ) if self.use_semantic_place: - self.place_controller = SemanticPlace( - spot=self.spot, config=self.place_config - ) + if self.use_semantic_place_ee: + self.place_controller = SemanticPlaceEE( + spot=self.spot, config=self.place_config + ) + else: + self.place_controller = SemanticPlace( + spot=self.spot, config=self.place_config + ) else: self.place_controller = Place( spot=self.spot, diff --git a/spot_rl_experiments/spot_rl/real_policy.py b/spot_rl_experiments/spot_rl/real_policy.py index 9223f0ab..23e6c989 100644 --- a/spot_rl_experiments/spot_rl/real_policy.py +++ b/spot_rl_experiments/spot_rl/real_policy.py @@ -342,6 +342,63 @@ def __init__(self, checkpoint_path, device, config: CN = CN()): ) +class SemanticPlaceEEPolicy(RealPolicy): + def __init__(self, checkpoint_path, device, config: CN = CN()): + observation_space = SpaceDict( + { + "obj_goal_sensor": spaces.Box( + shape=[ + 3, + ], + low=np.finfo(np.float32).min, + high=np.finfo(np.float32).max, + dtype=np.float32, + ), + "relative_initial_ee_orientation": spaces.Box( + shape=[ + 1, + ], + low=np.finfo(np.float32).min, + high=np.finfo(np.float32).max, + dtype=np.float32, + ), + "relative_target_object_orientation": spaces.Box( + shape=[ + 1, + ], + low=np.finfo(np.float32).min, + high=np.finfo(np.float32).max, + dtype=np.float32, + ), + "articulated_agent_jaw_depth": spaces.Box( + shape=[240, 228, 1], low=0.0, high=1.0, dtype=np.float32 + ), + "ee_pos": spaces.Box( + shape=[ + 3, + ], + low=np.finfo(np.float32).min, + high=np.finfo(np.float32).max, + dtype=np.float32, + ), + "is_holding": spaces.Box( + shape=[ + 1, + ], + low=0, + high=1, + dtype=np.float32, + ), + } + ) + action_space = spaces.Box( + -1.0, 1.0, (config.get("SEMANTIC_PLACE_EE_ACTION_SPACE_LENGTH", 10),) + ) + super().__init__( + checkpoint_path, observation_space, action_space, device, config=config + ) + + class NavPolicy(RealPolicy): def __init__(self, checkpoint_path, device, config: CN = CN()): observation_space = SpaceDict( diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index f7d2f1ae..3bfc5860 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -17,7 +17,11 @@ # Import Envs from spot_rl.envs.nav_env import SpotNavEnv from spot_rl.envs.open_close_drawer_env import SpotOpenCloseDrawerEnv -from spot_rl.envs.place_env import SpotPlaceEnv, SpotSemanticPlaceEnv +from spot_rl.envs.place_env import ( + SpotPlaceEnv, + SpotSemanticPlaceEEEnv, + SpotSemanticPlaceEnv, +) # Import policies from spot_rl.real_policy import ( @@ -27,6 +31,7 @@ OpenCloseDrawerPolicy, PlacePolicy, SemanticGazePolicy, + SemanticPlaceEEPolicy, SemanticPlacePolicy, ) @@ -969,6 +974,36 @@ def update_and_check_status(self, goal_dict: Dict[str, Any]) -> Tuple[bool, str] return status, message +class SemanticPlaceEE(SemanticPlace): + """ + Semantic place ee controller is used to execute place for given place targets + """ + + def __init__(self, spot: Spot, config): + if not config: + config = construct_config_for_semantic_place() + super().__init__(spot, config) + + self.policy = SemanticPlaceEEPolicy( + config.WEIGHTS.SEMANTIC_PLACE_EE, device=config.DEVICE, config=config + ) + self.policy.reset() + + self.env = SpotSemanticPlaceEEEnv(config, spot) + + def split_action(self, action: np.ndarray) -> Dict[str, Any]: + """Refer to class Skill for documentation""" + # action size is 10 + # TODO: semantic place ee: check the order + action_dict = { + "arm_ee_action": action[:6], + "base_action": action[6:8], + "grip_action": action[8], + } + + return action_dict + + class OpenCloseDrawer(Skill): """ Open close drawer controller is used to execute open/close drawers diff --git a/spot_rl_experiments/utils/cparamssemanticplaceee.yaml b/spot_rl_experiments/utils/cparamssemanticplaceee.yaml new file mode 100644 index 00000000..928c747e --- /dev/null +++ b/spot_rl_experiments/utils/cparamssemanticplaceee.yaml @@ -0,0 +1,19 @@ +# Define the model class name +MODEL_CLASS_NAME : "habitat_baselines.rl.ddppo.policy.resnet_policy.PointNavResNetPolicy" +# Define the observation dict +OBSERVATIONS_DICT: + obj_goal_sensor: [[3,], 'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] + relative_initial_ee_orientation: [[1,],'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] + relative_target_object_orientation: [[1,],'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] + articulated_agent_jaw_depth: [[240, 228, 1], '0.0', '1.0', 'np.float32'] + ee_pos: [[3,], 'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] + is_holding: [[1,], '0', '1', 'np.float32'] + +# Define the action space output length +ACTION_SPACE_LENGTH: 10 +# The path to load and save the models +TARGET_HAB3_POLICY_PATH: "/home/jmmy/research/spot-sim2real/spot_rl_experiments/weights/semantic_place_ee/sp_jt_ckpt.16_3.pth" +OUTPUT_COMBINED_NET_SAVE_PATH: "/home/jmmy/research/spot-sim2real/spot_rl_experiments/weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" +# If we want to use stereo pair camera for mobile gaze +USE_STEREO_PAIR_CAMERA: False +NEW_HABITAT_LAB_POLICY_OR_OLD: 'new' \ No newline at end of file diff --git a/spot_rl_experiments/utils/pytorch_to_torchscript.py b/spot_rl_experiments/utils/pytorch_to_torchscript.py index be20cf89..25058193 100644 --- a/spot_rl_experiments/utils/pytorch_to_torchscript.py +++ b/spot_rl_experiments/utils/pytorch_to_torchscript.py @@ -119,7 +119,6 @@ def __init__( ): print("Loading policy...") self.device = torch.device(device) - # Load the checkpoint checkpoint = torch.load(checkpoint_path, map_location="cpu") From 9cb31bf7e82faf7526fe048b734f0e2dc9caf4d7 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 14 Aug 2024 16:04:24 -0400 Subject: [PATCH 35/82] fix --- spot_rl_experiments/spot_rl/envs/base_env.py | 7 ++++--- spot_rl_experiments/spot_rl/skills/atomic_skills.py | 1 + 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index f47843d3..4b1c7abd 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -427,12 +427,13 @@ def step( # noqa arm_ee_action = rescale_arm_ee_actions(arm_ee_action) if np.count_nonzero(arm_ee_action) > 0: # TODO: semantic place ee: move this to config - arm_ee_action *= 0.1 + arm_ee_action[0:3] *= 0.015 + arm_ee_action[3:6] *= 0.0125 xyz, rpy = self.spot.get_ee_pos_in_body_frame() cur_ee_pose = np.concatenate((xyz, rpy), axis=0) + # Wrap the heading arm_ee_action += cur_ee_pose - # TODO: semantic place ee: move this to config - arm_ee_action = np.clip(arm_ee_action, 0.5, 0.5) + arm_ee_action[3:] = wrap_heading(arm_ee_action[3:]) else: arm_ee_action = None diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index 3bfc5860..e6307fc7 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -995,6 +995,7 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: """Refer to class Skill for documentation""" # action size is 10 # TODO: semantic place ee: check the order + # TODO: semantic place ee: check roll pitch yaw action_dict = { "arm_ee_action": action[:6], "base_action": action[6:8], From 3f71d4b815a6f768cbe2969b4b8101b83ba0c30f Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Wed, 14 Aug 2024 16:41:07 -0400 Subject: [PATCH 36/82] fix bugs --- bd_spot_wrapper/spot_wrapper/spot.py | 22 ++++++++++--------- spot_rl_experiments/spot_rl/envs/base_env.py | 4 ++-- spot_rl_experiments/spot_rl/envs/place_env.py | 1 + .../spot_rl/skills/atomic_skills.py | 5 +++-- 4 files changed, 18 insertions(+), 14 deletions(-) diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index 8d986b83..dc41b556 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -1092,22 +1092,24 @@ def set_base_vel_and_arm_ee_pos( travel_time, disable_obstacle_avoidance=False, ): - print("in set_base_vel_and_arm_ee_pos") - base_cmd = self.set_base_velocity( - x_vel, - y_vel, - ang_vel, - vel_time=travel_time, - disable_obstacle_avoidance=disable_obstacle_avoidance, - return_cmd=True, - ) + print(f"in set_base_vel_and_arm_ee_pos: {arm_ee_action} {travel_time}") + # TODO: semantic place ee: temp hack to distance the base vel + # base_cmd = self.set_base_velocity( + # x_vel, + # y_vel, + # ang_vel, + # vel_time=travel_time, + # disable_obstacle_avoidance=disable_obstacle_avoidance, + # return_cmd=True, + # ) arm_cmd = self.move_gripper_to_point( point=arm_ee_action[0:3], rotation=list(arm_ee_action[3:]), seconds_to_goal=travel_time, return_cmd=True, ) - synchro_command = RobotCommandBuilder.build_synchro_command(base_cmd, arm_cmd) + # synchro_command = RobotCommandBuilder.build_synchro_command(base_cmd, arm_cmd) + synchro_command = RobotCommandBuilder.build_synchro_command(arm_cmd) cmd_id = self.command_client.robot_command( synchro_command, end_time_secs=time.time() + travel_time ) diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 4b1c7abd..c02a5521 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -427,8 +427,8 @@ def step( # noqa arm_ee_action = rescale_arm_ee_actions(arm_ee_action) if np.count_nonzero(arm_ee_action) > 0: # TODO: semantic place ee: move this to config - arm_ee_action[0:3] *= 0.015 - arm_ee_action[3:6] *= 0.0125 + arm_ee_action[0:3] *= 0.1 # 0.015 + arm_ee_action[3:6] *= 0.1 # 0.0125 xyz, rpy = self.spot.get_ee_pos_in_body_frame() cur_ee_pose = np.concatenate((xyz, rpy), axis=0) # Wrap the heading diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index 196f9d5e..f048bd96 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -313,6 +313,7 @@ def step(self, action_dict: Dict[str, Any], *args, **kwargs): place = False # Place command is issued if the place action is smaller than zero + # TODO: semantic place ee: check how grip_action behaves! place = action_dict.get("grip_action", None) <= 0.0 # If the time steps have been passed for 50 steps and gripper is in the desired place location diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index e6307fc7..801ed480 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -996,10 +996,11 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: # action size is 10 # TODO: semantic place ee: check the order # TODO: semantic place ee: check roll pitch yaw + print(f"action: {action}") action_dict = { "arm_ee_action": action[:6], - "base_action": action[6:8], - "grip_action": action[8], + "base_action": action[7:9], + "grip_action": action[6], } return action_dict From 0aa4ab908b40e5c7e2de8eda70ca233479b857a8 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Thu, 15 Aug 2024 10:39:11 -0400 Subject: [PATCH 37/82] command out the base moving --- spot_rl_experiments/spot_rl/envs/base_env.py | 40 ++++++++++---------- 1 file changed, 21 insertions(+), 19 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index c02a5521..73bd9ac3 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -323,7 +323,7 @@ def step( # noqa grasp = action_dict.get("grasp", False) place = action_dict.get("place", False) - target_yaw = None + # target_yaw = None if disable_oa is None: disable_oa = self.config.DISABLE_OBSTACLE_AVOIDANCE grasp = grasp or self.config.GRASP_EVERY_STEP @@ -399,13 +399,13 @@ def step( # noqa lin_dist *= self._max_lin_dist_scale ang_dist *= np.deg2rad(self._max_ang_dist_scale) - target_yaw = wrap_heading(self.yaw + ang_dist) + # target_yaw = wrap_heading(self.yaw + ang_dist) # No horizontal velocity ctrl_period = 1 / self.ctrl_hz # Don't even bother moving if it's just for a bit of distance if abs(lin_dist) < 0.05 and abs(ang_dist) < np.deg2rad(3): base_action = None - target_yaw = None + # target_yaw = None else: base_action = [lin_dist / ctrl_period, 0, ang_dist / ctrl_period] self.prev_base_moved = True @@ -480,22 +480,24 @@ def step( # noqa print(f"base_action: {arr2str(base_action)}\tarm_action: {arr2str(arm_action)}") # Spin until enough time has passed during this step - start_time = time.time() - if base_action is not None or arm_action is not None: - while time.time() < start_time + 1 / self.ctrl_hz: - if target_yaw is not None and abs( - wrap_heading(self.yaw - target_yaw) - ) < np.deg2rad(3): - # Prevent overshooting of angular velocity - self.spot.set_base_velocity(base_action[0], 0, 0, MAX_CMD_DURATION) - target_yaw = None - elif not (grasp or place): - self.say("!!!! NO ACTIONS CALLED: moving to next step !!!!") - self.num_steps -= 1 - - self.stopwatch.record("run_actions") - if base_action is not None: - self.spot.set_base_velocity(0, 0, 0, 0.5) + # TODO: semantic place ee: temp hack to command this out + # for not using base velocity + # start_time = time.time() + # if base_action is not None or arm_action is not None: + # while time.time() < start_time + 1 / self.ctrl_hz: + # if target_yaw is not None and abs( + # wrap_heading(self.yaw - target_yaw) + # ) < np.deg2rad(3): + # # Prevent overshooting of angular velocity + # self.spot.set_base_velocity(base_action[0], 0, 0, MAX_CMD_DURATION) + # target_yaw = None + # elif not (grasp or place): + # self.say("!!!! NO ACTIONS CALLED: moving to next step !!!!") + # self.num_steps -= 1 + + # self.stopwatch.record("run_actions") + # if base_action is not None: + # self.spot.set_base_velocity(0, 0, 0, 0.5) observations = self.get_observations() self.stopwatch.record("get_observations") From 9b4b2ce65938a8014c26a24d7d4ae49d2ade82ac Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Thu, 15 Aug 2024 19:38:52 -0400 Subject: [PATCH 38/82] fix --- spot_rl_experiments/spot_rl/envs/base_env.py | 40 ++++++++++--------- spot_rl_experiments/spot_rl/envs/place_env.py | 1 + 2 files changed, 22 insertions(+), 19 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 73bd9ac3..aafde413 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -323,7 +323,7 @@ def step( # noqa grasp = action_dict.get("grasp", False) place = action_dict.get("place", False) - # target_yaw = None + target_yaw = None if disable_oa is None: disable_oa = self.config.DISABLE_OBSTACLE_AVOIDANCE grasp = grasp or self.config.GRASP_EVERY_STEP @@ -399,13 +399,13 @@ def step( # noqa lin_dist *= self._max_lin_dist_scale ang_dist *= np.deg2rad(self._max_ang_dist_scale) - # target_yaw = wrap_heading(self.yaw + ang_dist) + target_yaw = wrap_heading(self.yaw + ang_dist) # No horizontal velocity ctrl_period = 1 / self.ctrl_hz # Don't even bother moving if it's just for a bit of distance if abs(lin_dist) < 0.05 and abs(ang_dist) < np.deg2rad(3): base_action = None - # target_yaw = None + target_yaw = None else: base_action = [lin_dist / ctrl_period, 0, ang_dist / ctrl_period] self.prev_base_moved = True @@ -482,22 +482,24 @@ def step( # noqa # Spin until enough time has passed during this step # TODO: semantic place ee: temp hack to command this out # for not using base velocity - # start_time = time.time() - # if base_action is not None or arm_action is not None: - # while time.time() < start_time + 1 / self.ctrl_hz: - # if target_yaw is not None and abs( - # wrap_heading(self.yaw - target_yaw) - # ) < np.deg2rad(3): - # # Prevent overshooting of angular velocity - # self.spot.set_base_velocity(base_action[0], 0, 0, MAX_CMD_DURATION) - # target_yaw = None - # elif not (grasp or place): - # self.say("!!!! NO ACTIONS CALLED: moving to next step !!!!") - # self.num_steps -= 1 - - # self.stopwatch.record("run_actions") - # if base_action is not None: - # self.spot.set_base_velocity(0, 0, 0, 0.5) + start_time = time.time() + if arm_ee_action is not None: + print("do not move the base") + elif base_action is not None or arm_action is not None: + while time.time() < start_time + 1 / self.ctrl_hz: + if target_yaw is not None and abs( + wrap_heading(self.yaw - target_yaw) + ) < np.deg2rad(3): + # Prevent overshooting of angular velocity + self.spot.set_base_velocity(base_action[0], 0, 0, MAX_CMD_DURATION) + target_yaw = None + elif not (grasp or place): + self.say("!!!! NO ACTIONS CALLED: moving to next step !!!!") + self.num_steps -= 1 + + self.stopwatch.record("run_actions") + if base_action is not None: + self.spot.set_base_velocity(0, 0, 0, 0.5) observations = self.get_observations() self.stopwatch.record("get_observations") diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index f048bd96..c65e8763 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -219,6 +219,7 @@ def get_observations(self): "joint": self.get_arm_joints(self.config.SEMANTIC_PLACE_JOINT_BLACKLIST), "is_holding": np.ones((1,)), } + print("---") return observations From bb862796d6795c09edab6964b040d5332bf817a6 Mon Sep 17 00:00:00 2001 From: Tushar Sangam Date: Tue, 20 Aug 2024 13:20:19 -0700 Subject: [PATCH 39/82] adding logging stuff --- .gitignore | 1 + .../skill_test/episode_configs/pick_32.yaml | 69 +++++++++ .../experiments/skill_test/parse_logs.py | 93 +++++++++++++ .../experiments/skill_test/run_episode.py | 131 ++++++++++++++++++ spot_rl_experiments/spot_rl/envs/base_env.py | 4 +- .../spot_rl/skills/atomic_skills.py | 71 ++++++---- .../spot_rl/utils/helper_nodes.py | 73 +++++++--- 7 files changed, 393 insertions(+), 49 deletions(-) create mode 100644 spot_rl_experiments/experiments/skill_test/episode_configs/pick_32.yaml create mode 100644 spot_rl_experiments/experiments/skill_test/parse_logs.py create mode 100644 spot_rl_experiments/experiments/skill_test/run_episode.py diff --git a/.gitignore b/.gitignore index 002b5af4..44619c5a 100644 --- a/.gitignore +++ b/.gitignore @@ -26,3 +26,4 @@ spot_rl_experiments/experiments/skill_test/logs/* spot_rl_experiments/experiments/skill_test/table_detections/* ros_tcp/ros_communication_client.egg-info/** *.npy +*.csv diff --git a/spot_rl_experiments/experiments/skill_test/episode_configs/pick_32.yaml b/spot_rl_experiments/experiments/skill_test/episode_configs/pick_32.yaml new file mode 100644 index 00000000..364149b2 --- /dev/null +++ b/spot_rl_experiments/experiments/skill_test/episode_configs/pick_32.yaml @@ -0,0 +1,69 @@ +# episode21: +# spotskillmanagerargs: +# use_mobile_pick: False +# use_semantic_place_ee: True +# actions: +# - nav: +# - "fremont_lab_white_desk" +# - pick: +# - "frosted flakes cup" +# # - nav: +# # - "fremont_lab_white_desk" +# - place: +# - null +# episode22: +# spotskillmanagerargs: +# use_mobile_pick: False +# use_semantic_place_ee: True +# actions: +# - nav: +# - "fremont_lab_white_desk" +# - pick: +# - "ball" +# # - nav: +# # - "fremont_lab_white_desk" +# - place: +# - null +episode33: + spotskillmanagerargs: + use_mobile_pick: False + use_semantic_place_ee: True + actions: + - nav: + - "fremont_lab_white_desk" + - pick: + - "cereal box" + # - nav: + # - "nav_desk_32_in" + - place: + - null +# episode24: +# spotskillmanagerargs: +# use_mobile_pick: False +# use_semantic_place_ee: True +# actions: +# # - nav:Semantic Place with waypoint estimator +# # - "lab_desk" + +# - nav: +# - "fremont_lab_white_desk" +# - pick: +# - "bottle" +# # - nav: +# # - "nav_desk_32_in" +# - place: +# - null + +# episode25: +# spotskillmanagerargs: +# use_mobile_pick: False +# use_semantic_place_ee: True +# actions: +# - nav: +# - "fremont_lab_white_desk" +# - pick: +# - "water can" +# # - nav: +# # - "nav_desk_32_in" +# - place: +# - null diff --git a/spot_rl_experiments/experiments/skill_test/parse_logs.py b/spot_rl_experiments/experiments/skill_test/parse_logs.py new file mode 100644 index 00000000..bee5a11b --- /dev/null +++ b/spot_rl_experiments/experiments/skill_test/parse_logs.py @@ -0,0 +1,93 @@ +import argparse +import csv +import json + + +def parse_json_file(file_path): + try: + with open(file_path, "r") as file: + data = json.load(file) + return data + except FileNotFoundError: + print(f"Error: File '{file_path}' not found.") + return None + except json.JSONDecodeError: + print(f"Error: '{file_path}' is not a valid JSON file.") + return None + + +def format_value(key, value): + if isinstance(value, (int, float)): + value = round(value, 3) + return str(value) + + +def export_to_csv(results_dict, output_file): + with open(output_file, mode="w", newline="") as file: + writer = csv.writer(file) + # Write header (keys of the dictionary) + writer.writerow(results_dict.keys()) + # Write data (values of the dictionary) + writer.writerow(results_dict.values()) + + +def main(): + parser = argparse.ArgumentParser(description="Parse a JSON file") + parser.add_argument("file", help="Path to the JSON file to parse") + parser.add_argument("output_csv", help="Path to the output CSV file") + args = parser.parse_args() + + parsed_data = parse_json_file(args.file) + if parsed_data is None: + return + + results_dict = { + "overall_success": " ", + "total_time": parsed_data["total_time"], + "total_num_steps": parsed_data["total_steps"], + "0_nav_success": " ", + "0_nav_time_taken": " ", + "0_nav_num_steps": " ", + "0_nav_distance_travelled": " ", + "0_nav_distance_to_goal_linear": " ", + "0_nav_distance_to_goal_angular": " ", + "1_pick_success": " ", + "1_pick_time_taken": " ", + "1_pick_num_steps": " ", + "2_nav_success": " ", + "2_nav_time_taken": " ", + "2_nav_num_steps": " ", + "2_nav_distance_travelled": " ", + "2_nav_distance_to_goal_linear": " ", + "2_nav_distance_to_goal_angular": " ", + "3_place_success": " ", + "3_place_time_taken": " ", + "3_place_num_steps": " ", + } + + success_ctr = 0 + for idx, action in enumerate(parsed_data["actions"]): + for action_type in action.keys(): + for k, v in action[action_type].items(): + if ( + action_type == "pick" or action_type == "place" + ) and k == "distance_travelled": + pass + elif k == "robot_trajectory": + pass + elif k == "distance_to_goal": + for bk, bv in action[action_type][k].items(): + match_str = f"{idx}_{action_type}_{k}_{bk}" + results_dict[match_str] = bv + else: + match_str = f"{idx}_{action_type}_{k}" + results_dict[match_str] = v + if "success" in k and v is True: + success_ctr += 1 + results_dict["overall_success"] = success_ctr / 4 + + export_to_csv(results_dict, args.output_csv) + + +if __name__ == "__main__": + main() diff --git a/spot_rl_experiments/experiments/skill_test/run_episode.py b/spot_rl_experiments/experiments/skill_test/run_episode.py new file mode 100644 index 00000000..6bfce974 --- /dev/null +++ b/spot_rl_experiments/experiments/skill_test/run_episode.py @@ -0,0 +1,131 @@ +# mypy: ignore-errors +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + +import atexit +import json +import os.path as osp +import signal +import sys + +import yaml +from perception_and_utils.utils.generic_utils import map_user_input_to_boolean +from spot_rl.envs.skill_manager import SpotSkillManager + +controller_dict = { + "nav": "nav_controller", + "pick": "gaze_controller", + "place": "place_controller", +} +config_dict = {"nav": "nav_config", "pick": "pick_config"} +episode_name, episode_log, final_success, total_time, total_steps = ( + None, + None, + None, + None, + None, +) + + +def save_logs_as_json(): + global final_success, total_time, total_steps, episode_log, episode_name + try: + if episode_name is not None and episode_log is not None: + episode_log["total_time"] = total_time + episode_log["final_success"] = final_success + episode_log["total_steps"] = total_steps + file_path = f"logs/integration_test/8-15-24/{episode_name}.json" + with open(file_path, "w") as file: + json.dump(episode_log, file, indent=4) + print(f"Saved log: {file_path}") + except Exception: + pass + + +atexit.register(save_logs_as_json) +# Register the signal handler for termination signals +signal.signal(signal.SIGINT, save_logs_as_json) +signal.signal(signal.SIGTERM, save_logs_as_json) + + +def parse_yaml(file_path): + global final_success, total_time, total_steps, episode_log, episode_name + with open(file_path, "r") as file: + episodes = yaml.safe_load(file) + + for episode, details in episodes.items(): + print(f"Executing {episode}") + # Instantiate ActionTaker with provided args + action_taker_args = details.pop("spotskillmanagerargs") + action_taker = SpotSkillManager(**action_taker_args) + episode_name, episode_log = episode, {"actions": []} + actions = details["actions"] + final_success, total_time, total_steps = True, 0, 0 + terminate_episode = False + for action_dict in actions: + if terminate_episode: + break + for action, args in action_dict.items(): + method = getattr(action_taker, action, None) + if method: + # breakpoint() + status, _ = method(*args) + controller = getattr(action_taker, controller_dict[action], None) + if controller: + skill_log = controller.skill_result_log + final_success = final_success and skill_log["success"] + # check if episode failed. If so, terminate the episode + max_episode_steps = getattr( + action_taker, f"{action}_config" + ).MAX_EPISODE_STEPS + print(f"EPISODE STEPS{max_episode_steps}") + max_episode_steps = ( + 75 if action == "place" else max_episode_steps + ) + if ( + skill_log["num_steps"] >= max_episode_steps + and not skill_log["success"] + ): + terminate_episode = True + print("status: ", status, skill_log["success"]) + print(skill_log["time_taken"], total_time) + total_time += skill_log["time_taken"] + total_steps += skill_log.get("num_steps", 0) + episode_log["actions"].append({f"{action}": skill_log}) + else: + print( + f"{controller_dict[action]} not found in SpotSkillManager" + ) + else: + print(f"Action {action} not found in SpotSkillManager") + save_logs_as_json() + contnu = map_user_input_to_boolean("Should I dock y/n ?") + if contnu: + action_taker.dock() + else: + # spotskillmanager.spot. reset arm + action_taker.spot.sit() + contnu = map_user_input_to_boolean( + f"Finished {episode}. Press y/n to continue to the next episode..." + ) + if not contnu: + break + + +if __name__ == "__main__": + + current_script_path = osp.dirname(osp.abspath(__file__)) + yaml_file_root = osp.join(current_script_path, "episode_configs") + if len(sys.argv) != 2: + print("Usage: python run_episode.py episode1.yaml") + sys.exit(1) + yaml_file_path = osp.join(yaml_file_root, sys.argv[1]) + assert osp.exists( + yaml_file_path + ), f"{yaml_file_path} doesn't exists please recheck the path" + continu = map_user_input_to_boolean( + f"Using the yaml file {yaml_file_path} should I continue" + ) + if continu: + parse_yaml(yaml_file_path) diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index aafde413..f7144990 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -427,8 +427,8 @@ def step( # noqa arm_ee_action = rescale_arm_ee_actions(arm_ee_action) if np.count_nonzero(arm_ee_action) > 0: # TODO: semantic place ee: move this to config - arm_ee_action[0:3] *= 0.1 # 0.015 - arm_ee_action[3:6] *= 0.1 # 0.0125 + arm_ee_action[0:3] *= 0.2 # 0.015 + arm_ee_action[3:6] *= 0.2 # 0.0125 xyz, rpy = self.spot.get_ee_pos_in_body_frame() cur_ee_pose = np.concatenate((xyz, rpy), axis=0) # Wrap the heading diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index 801ed480..24a1f6f6 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -46,10 +46,12 @@ # Import Utils from spot_rl.utils.geometry_utils import ( + euclidean, generate_intermediate_point, get_RPY_from_vector, is_pose_within_bounds, is_position_within_bounds, + wrap_angle_deg, ) from spot_rl.utils.utils import get_skill_name_and_input_from_ros @@ -161,8 +163,15 @@ def execute_rl_loop(self, goal_dict: Dict[str, Any]) -> Tuple[bool, str]: while not done: action = self.policy.act(observations) # type: ignore action_dict = self.split_action(action) - observations, _, done, _ = self.env.step(action_dict=action_dict) # type: ignore - + prev_pose = [ + self.env.x, # type: ignore + self.env.y, # type: ignore + ] + observations, _, done, info = self.env.step(action_dict=action_dict) # type: ignore + curr_pose = [ + self.env.x, # type: ignore + self.env.y, # type: ignore + ] # Record trajectories at every step self.skill_result_log["robot_trajectory"].append( { @@ -174,6 +183,12 @@ def execute_rl_loop(self, goal_dict: Dict[str, Any]) -> Tuple[bool, str]: ], } ) + self.skill_result_log["num_steps"] = info["num_steps"] + if "distance_travelled" not in self.skill_result_log: + self.skill_result_log["distance_travelled"] = 0 + self.skill_result_log["distance_travelled"] += euclidean( + curr_pose, prev_pose + ) # Check if we still want to use the same skill # We terminate the skill if skill name or input is differnt from the one at the beginning. @@ -364,30 +379,30 @@ def update_and_check_status(self, goal_dict: Dict[str, Any]) -> Tuple[bool, str] """Refer to class Skill for documentation""" self.env.say("Navigation Skill finished .. checking status") - dynamic_yaw = goal_dict.get("dynamic_yaw") - dynamic_yaw = False if dynamic_yaw is None else dynamic_yaw - - if dynamic_yaw: - obs = self.env.get_observations() - check_navigation_success = self.env.get_success(obs, False) - else: - nav_target = goal_dict[ - "nav_target" - ] # safe to access as sanity check passed - # Make the angle from rad to deg - _nav_target_pose_deg = ( - nav_target[0], - nav_target[1], - np.rad2deg(nav_target[2]), - ) - check_navigation_success = is_pose_within_bounds( - self.skill_result_log.get("robot_trajectory")[-1].get("pose"), - _nav_target_pose_deg, - self.config.SUCCESS_DISTANCE, - self.config.SUCCESS_ANGLE_DIST, - ) + nav_target = goal_dict["nav_target"] # safe to access as sanity check passed + # Make the angle from rad to deg + _nav_target_pose_deg = ( + nav_target[0], + nav_target[1], + np.rad2deg(nav_target[2]), + ) + current_pose = self.skill_result_log.get("robot_trajectory")[-1].get("pose") + check_navigation_success = is_pose_within_bounds( + current_pose, + _nav_target_pose_deg, + self.config.SUCCESS_DISTANCE, + self.config.SUCCESS_ANGLE_DIST, + ) # Update result log + + self.skill_result_log["distance_to_goal"] = { + "linear": euclidean(current_pose[:2], _nav_target_pose_deg[:2]), + "angular": abs( + wrap_angle_deg(current_pose[2]) + - wrap_angle_deg(_nav_target_pose_deg[2]) + ), + } self.skill_result_log["time_taken"] = time.time() - self.start_time self.skill_result_log["success"] = check_navigation_success @@ -475,10 +490,14 @@ def __init__(self, spot, config=None, use_mobile_pick=False) -> None: self.env = SpotGazeEnv(self.config, spot, use_mobile_pick) def set_pose_estimation_flags( - self, enable_pose_estimation: bool = False, enable_pose_correction: bool = False + self, + enable_pose_estimation: bool = False, + enable_pose_correction: bool = False, + mesh_name: str = "", ) -> None: self.enable_pose_estimation = enable_pose_estimation self.enable_pose_correction = enable_pose_correction + self.mesh_name = mesh_name def set_force_control(self, enable_force_control: bool = False): self.enable_force_control = enable_force_control @@ -570,6 +589,7 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: "enable_pose_correction": self.enable_pose_correction, "enable_force_control": self.enable_force_control, "grasp_mode": self.grasp_mode, + "mesh_name": self.mesh_name, } else: action_dict = { @@ -579,6 +599,7 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: "enable_pose_correction": self.enable_pose_correction, "enable_force_control": self.enable_force_control, "grasp_mode": self.grasp_mode, + "mesh_name": self.mesh_name, } return action_dict diff --git a/spot_rl_experiments/spot_rl/utils/helper_nodes.py b/spot_rl_experiments/spot_rl/utils/helper_nodes.py index 573f50a1..f39f6690 100644 --- a/spot_rl_experiments/spot_rl/utils/helper_nodes.py +++ b/spot_rl_experiments/spot_rl/utils/helper_nodes.py @@ -1,8 +1,8 @@ +# mypy: ignore-errors # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. - import argparse import time @@ -15,6 +15,7 @@ from spot_wrapper.utils import say from std_msgs.msg import Float32MultiArray, String from tf2_ros import StaticTransformBroadcaster +from visualization_msgs.msg import Marker, MarkerArray NAV_POSE_BUFFER_LEN = 1 @@ -24,28 +25,36 @@ def __init__(self, spot): rospy.init_node("spot_ros_proprioception_node", disable_signals=True) self.spot = spot - # Instantiate filtered image publishers + # Publishers self.pub = rospy.Publisher(rt.ROBOT_STATE, Float32MultiArray, queue_size=1) - self.last_publish = time.time() - rospy.loginfo("[spot_ros_proprioception_node]: Publishing has started.") + self.odom_broadcaster = rospy.Publisher( + rt.ODOM_TOPIC, Odometry, queue_size=1, latch=True + ) + self.marker_pub = rospy.Publisher( + "gripper_trajectory_marker_array", MarkerArray, queue_size=1 + ) + self.last_publish = time.time() self.nav_pose_buff = None self.buff_idx = 0 - # Instantiate static transform broadcaster for publishing spot to spotWorld transform + # Static transform broadcaster self.static_tf_broadcaster = StaticTransformBroadcaster() - # broadcast spot odometry to visualize path in rviz - self.odom_broadcaster = rospy.Publisher( - rt.ODOM_TOPIC, Odometry, queue_size=1, latch=True - ) + self.gripper_traj = [] def publish_msgs(self): st = time.time() robot_state = self.spot.get_robot_state() - robot_kinematic_snapshot_tree = robot_state.kinematic_state.transforms_snapshot + position, rotation = self.spot.get_base_transform_to("link_wr1") + ee_pose = np.array([position.x, position.y, position.z]) + + self.gripper_traj.append(ee_pose) + if len(self.gripper_traj) > 100: + self.gripper_traj.pop(0) + msg = Float32MultiArray() xy_yaw = self.spot.get_xy_yaw(robot_state=robot_state, use_boot_origin=True) if self.nav_pose_buff is None: @@ -56,8 +65,6 @@ def publish_msgs(self): xy_yaw = np.mean(self.nav_pose_buff, axis=0) joints = self.spot.get_arm_proprioception(robot_state=robot_state).values() - - position, rotation = self.spot.get_base_transform_to("link_wr1") gripper_transform = [position.x, position.y, position.z] + [ rotation.x, rotation.y, @@ -70,32 +77,54 @@ def publish_msgs(self): dtype=np.float32, ) - # Limit publishing to 10 Hz max if time.time() - self.last_publish > 1 / 10: self.pub.publish(msg) - # publish spot to spotWorld transform as ROS TF self.static_tf_broadcaster.sendTransform( self.spot.get_ros_TransformStamped_vision_T_body( robot_kinematic_snapshot_tree ) ) - # publish pose as ROS Odometry - msg = Odometry() pose = self.spot.get_ros_Pose_vision_T_body(robot_kinematic_snapshot_tree) + msg = Odometry() msg.pose.pose = pose msg.header.stamp = rospy.Time.now() msg.child_frame_id = rf.SPOT msg.header.frame_id = rf.SPOT_WORLD self.odom_broadcaster.publish(msg) - # Log publish rate + + self.publish_ee_trajectory() + rospy.loginfo( - f"[spot_ros_proprioception_node]: " - "Proprioception retrieval / publish time: " - f"{1/(time.time() - st):.4f} / " - f"{1/(time.time() - self.last_publish):.4f} Hz" + f"[spot_ros_proprioception_node]: Proprioception retrieval / publish time: {1/(time.time() - st):.4f} / {1/(time.time() - self.last_publish):.4f} Hz" ) self.last_publish = time.time() + def publish_ee_trajectory(self): + marker_array = MarkerArray() + for i, pose in enumerate(self.gripper_traj): + marker = Marker() + marker.header.frame_id = "spot_world" + marker.header.stamp = rospy.Time.now() + marker.ns = "ee_trajectory" + marker.id = i + marker.type = Marker.SPHERE + marker.action = Marker.ADD + marker.pose.position.x = pose[0] + marker.pose.position.y = pose[1] + marker.pose.position.z = pose[2] + marker.pose.orientation.w = 1.0 + marker.scale.x = 0.05 + marker.scale.y = 0.05 + marker.scale.z = 0.05 + marker.color.r = 1.0 + marker.color.g = 0.0 + marker.color.b = 0.0 + marker.color.a = 1.0 + marker.lifetime = rospy.Duration(0) + marker_array.markers.append(marker) + + self.marker_pub.publish(marker_array) + if __name__ == "__main__": parser = argparse.ArgumentParser() @@ -104,7 +133,7 @@ def publish_msgs(self): args = parser.parse_args() if args.text_to_speech: - tts_callback = lambda msg: say(msg.data) # noqa + tts_callback = lambda msg: say(msg.data) # noqa: E731 rospy.init_node("spot_ros_tts_node", disable_signals=True) rospy.Subscriber(rt.TEXT_TO_SPEECH, String, tts_callback, queue_size=1) rospy.loginfo("[spot_ros_tts_node]: Listening for text to dictate.") From 1abcad37606e5f21f14686a6e22ae0b220f00dbc Mon Sep 17 00:00:00 2001 From: Joanne Truong Date: Wed, 21 Aug 2024 20:12:54 -0700 Subject: [PATCH 40/82] indent use_semantic_place --- spot_rl_experiments/spot_rl/envs/skill_manager.py | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index b033df1d..c1a6904d 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -515,10 +515,8 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray return False, message if self.use_semantic_place: # Convert HOME frame coordinates into body frame - place_target_location = ( - self.place_controller.env.get_target_in_base_frame( - mn.Vector3(*place_target_location.astype(np.float64).tolist()) - ) + place_target_location = self.place_controller.env.get_target_in_base_frame( + mn.Vector3(*place_target_location.astype(np.float64).tolist()) ) is_local = True else: From 70d07b60aa9e182b55847f54c3d3e8d42cb7bd5c Mon Sep 17 00:00:00 2001 From: Joanne Truong Date: Wed, 21 Aug 2024 20:25:42 -0700 Subject: [PATCH 41/82] add mobile_gaze_ee and semantic_place_ee torchscript cparams files --- .../utils/cparamsmobilegazeee.yaml | 16 ++++++++++++++++ .../utils/cparamssemanticplaceee.yaml | 6 +++--- 2 files changed, 19 insertions(+), 3 deletions(-) create mode 100644 spot_rl_experiments/utils/cparamsmobilegazeee.yaml diff --git a/spot_rl_experiments/utils/cparamsmobilegazeee.yaml b/spot_rl_experiments/utils/cparamsmobilegazeee.yaml new file mode 100644 index 00000000..f279ce8b --- /dev/null +++ b/spot_rl_experiments/utils/cparamsmobilegazeee.yaml @@ -0,0 +1,16 @@ +# Define the model class name +MODEL_CLASS_NAME : "habitat_baselines.rl.ddppo.policy.resnet_policy.PointNavResNetPolicy" +# Define the observation dict +OBSERVATIONS_DICT: + arm_depth_bbox_sensor: [[240, 228, 1], 'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] + articulated_agent_arm_depth: [[240, 228, 1], '0.0', '1.0', 'np.float32'] + spot_head_stereo_depth_sensor: [[240, 228, 1], '0.0', '1.0', 'np.float32'] + ee_pose: [[6,], 'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] +# Define the action space output length +ACTION_SPACE_LENGTH: 9 +# The path to load and save the models +TARGET_HAB3_POLICY_PATH: "/Users/jtruong/Downloads/mg_55k_mobile_30_ee_rot_0.1_250_steps_EEPoseSensor_kinematic_acfreq_1_cam_no_end_emb_unocc_snap_ckpt_11.pth" +OUTPUT_COMBINED_NET_SAVE_PATH: "/Users/jtruong/Downloads/mg_55k_mobile_30_ee_rot_0.1_250_steps_EEPoseSensor_kinematic_acfreq_1_cam_no_end_emb_unocc_snap_ckpt_11.pth.torchscript" +# If we want to use stereo pair camera for mobile gaze +USE_STEREO_PAIR_CAMERA: False +NEW_HABITAT_LAB_POLICY_OR_OLD: 'new' diff --git a/spot_rl_experiments/utils/cparamssemanticplaceee.yaml b/spot_rl_experiments/utils/cparamssemanticplaceee.yaml index 928c747e..4c88cb4b 100644 --- a/spot_rl_experiments/utils/cparamssemanticplaceee.yaml +++ b/spot_rl_experiments/utils/cparamssemanticplaceee.yaml @@ -6,14 +6,14 @@ OBSERVATIONS_DICT: relative_initial_ee_orientation: [[1,],'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] relative_target_object_orientation: [[1,],'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] articulated_agent_jaw_depth: [[240, 228, 1], '0.0', '1.0', 'np.float32'] - ee_pos: [[3,], 'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] + ee_pose: [[6,], 'np.finfo(np.float32).min', 'np.finfo(np.float32).max', 'np.float32'] is_holding: [[1,], '0', '1', 'np.float32'] # Define the action space output length ACTION_SPACE_LENGTH: 10 # The path to load and save the models -TARGET_HAB3_POLICY_PATH: "/home/jmmy/research/spot-sim2real/spot_rl_experiments/weights/semantic_place_ee/sp_jt_ckpt.16_3.pth" -OUTPUT_COMBINED_NET_SAVE_PATH: "/home/jmmy/research/spot-sim2real/spot_rl_experiments/weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" +TARGET_HAB3_POLICY_PATH: "/Users/jtruong/Downloads/ckpt.50.pth" +OUTPUT_COMBINED_NET_SAVE_PATH: "/Users/jtruong/Downloads/ckpt.50.pth.torchscript" # If we want to use stereo pair camera for mobile gaze USE_STEREO_PAIR_CAMERA: False NEW_HABITAT_LAB_POLICY_OR_OLD: 'new' \ No newline at end of file From 3ff954fbe0971e6a19dcb04791fefc683cdb64af Mon Sep 17 00:00:00 2001 From: Joanne Truong Date: Wed, 21 Aug 2024 21:24:21 -0700 Subject: [PATCH 42/82] add mobile_gaze_ee env and policy --- spot_rl_experiments/spot_rl/envs/gaze_env.py | 50 +++++++++++++++++++ .../spot_rl/envs/skill_manager.py | 26 ++++++---- spot_rl_experiments/spot_rl/real_policy.py | 27 ++++++++++ .../spot_rl/skills/atomic_skills.py | 39 ++++++++++++++- 4 files changed, 132 insertions(+), 10 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/gaze_env.py b/spot_rl_experiments/spot_rl/envs/gaze_env.py index 8a3057ec..d0dafb9a 100644 --- a/spot_rl_experiments/spot_rl/envs/gaze_env.py +++ b/spot_rl_experiments/spot_rl/envs/gaze_env.py @@ -110,6 +110,56 @@ def get_observations(self): def get_success(self, observations): return self.grasp_attempted +class SpotGazeEEEnv(SpotGazeEnv): + def __init__(self, config, spot: Spot, use_mobile_pick: bool = False): + super().__init__( + config, + spot, + use_mobile_pick=use_mobile_pick, + ) + + def step(self, action_dict: Dict[str, Any]): + grasp = self.should_grasp() + + # Update the action_dict with grasp and place flags + action_dict["grasp"] = grasp + action_dict["place"] = False # TODO: Why is gaze getting flag for place? + + observations, reward, done, info = super().step( + action_dict=action_dict, + ) + return observations, reward, done, info + + def remap_observation_keys_for_hab3(self, observations): + """ + Change observation keys as per hab3. + + @INFO: Policies trained on older hab versions DON'T need remapping + """ + semantic_gaze_observations = {} + semantic_gaze_observations["arm_depth_bbox_sensor"] = observations[ + "arm_depth_bbox" + ] + semantic_gaze_observations["articulated_agent_arm_depth"] = observations[ + "arm_depth" + ] + semantic_gaze_observations["ee_pose"] = observations["ee_pose"] + return semantic_gaze_observations + + def get_observations(self): + arm_depth, arm_depth_bbox = self.get_gripper_images() + xyz, rpy = self.spot.get_ee_pos_in_body_frame() + observations = { + "ee_pose": np.concatenate([xyz, rpy]), + "arm_depth": arm_depth, + "arm_depth_bbox": arm_depth_bbox, + } + + # Remap observation keys for mobile gaze as it was trained with Habitat version3 + if self._use_mobile_pick: + observations = self.remap_observation_keys_for_hab3(observations) + + return observations class SpotSemanticGazeEnv(SpotBaseEnv): def __init__(self, config, spot: Spot): diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index c1a6904d..ea30f5ab 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -16,6 +16,7 @@ Pick, Place, SemanticPick, + MobilePickEE, SemanticPlace, SemanticPlaceEE, ) @@ -104,7 +105,7 @@ def __init__( open_close_drawer_config=None, use_mobile_pick: bool = False, use_semantic_place: bool = False, - use_semantic_place_ee: bool = False, + use_ee: bool = False, verbose: bool = True, use_policies: bool = True, ): @@ -117,7 +118,7 @@ def __init__( # Process the meta parameters self._use_mobile_pick = use_mobile_pick self.use_semantic_place = use_semantic_place - self.use_semantic_place_ee = use_semantic_place_ee + self.use_ee = use_ee # Create the spot object, init lease, and construct configs self.__init_spot( @@ -181,7 +182,7 @@ def __init_spot( # TODO: overwrite the config self.place_config = ( construct_config_for_semantic_place() - if self.use_semantic_place or self.use_semantic_place_ee + if self.use_semantic_place else construct_config_for_place() ) else: @@ -208,13 +209,20 @@ def __initiate_controllers(self, use_policies: bool = True): spot=self.spot, config=self.nav_config, ) - self.gaze_controller = Pick( - spot=self.spot, - config=self.pick_config, - use_mobile_pick=self._use_mobile_pick, - ) + if self.use_ee: + self.gaze_controller = Pick( + spot=self.spot, + config=self.pick_config, + use_mobile_pick=self._use_mobile_pick, + ) + else: + self.gaze_controller = MobilePickEE( + spot=self.spot, + config=self.pick_config, + use_mobile_pick=self._use_mobile_pick, + ) if self.use_semantic_place: - if self.use_semantic_place_ee: + if self.use_ee: self.place_controller = SemanticPlaceEE( spot=self.spot, config=self.place_config ) diff --git a/spot_rl_experiments/spot_rl/real_policy.py b/spot_rl_experiments/spot_rl/real_policy.py index 23e6c989..370829dd 100644 --- a/spot_rl_experiments/spot_rl/real_policy.py +++ b/spot_rl_experiments/spot_rl/real_policy.py @@ -231,6 +231,33 @@ def __init__(self, checkpoint_path, device, config: CN = CN()): checkpoint_path, observation_space, action_space, device, config=config ) +class MobileGazeEEPolicy(RealPolicy): + def __init__(self, checkpoint_path, device, config: CN = CN()): + observation_space = SpaceDict( + { + "arm_depth_bbox_sensor": spaces.Box( + low=np.finfo(np.float32).min, + high=np.finfo(np.float32).max, + shape=(240, 228, 1), + dtype=np.float32, + ), + "articulated_agent_arm_depth": spaces.Box( + low=0.0, high=1.0, shape=(240, 228, 1), dtype=np.float32 + ), + "ee_pose": spaces.Box( + low=np.finfo(np.float32).min, + high=np.finfo(np.float32).max, + shape=(6,), + dtype=np.float32, + ), + } + ) + action_space = spaces.Box( + -1.0, 1.0, (9,) + ) + super().__init__( + checkpoint_path, observation_space, action_space, device, config=config + ) class SemanticGazePolicy(RealPolicy): def __init__(self, checkpoint_path, device, config: CN = CN()): diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index 24a1f6f6..36f66014 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -12,7 +12,7 @@ conditional_print, map_user_input_to_boolean, ) -from spot_rl.envs.gaze_env import SpotGazeEnv, SpotSemanticGazeEnv +from spot_rl.envs.gaze_env import SpotGazeEnv, SpotSemanticGazeEnv, SpotGazeEEEnv # Import Envs from spot_rl.envs.nav_env import SpotNavEnv @@ -27,6 +27,7 @@ from spot_rl.real_policy import ( GazePolicy, MobileGazePolicy, + MobileGazeEEPolicy, NavPolicy, OpenCloseDrawerPolicy, PlacePolicy, @@ -703,6 +704,42 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: } return action_dict +class MobilePickEE(Pick): + """ + Semantic place ee controller is used to execute place for given place targets + """ + + def __init__(self, spot: Spot, config, use_mobile_pick=True): + if not config: + config = construct_config_for_gaze() + super().__init__(spot, config, use_mobile_pick=True) + + self.policy = MobileGazeEEPolicy( + self.config.WEIGHTS.MOBILE_GAZE, + device=self.config.DEVICE, + config=self.config, + ) + self.policy.reset() + + self.env = SpotGazeEEEnv(config, spot) + + def split_action(self, action: np.ndarray) -> Dict[str, Any]: + """Refer to class Skill for documentation""" + # action size is 9 + # TODO: semantic place ee: check the order + # TODO: semantic place ee: check roll pitch yaw + print(f"action: {action}") + action_dict = { + "arm_ee_action": action[:6], + "base_action": action[7:9], + "enable_pose_estimation": self.enable_pose_estimation, + "enable_pose_correction": self.enable_pose_correction, + "enable_force_control": self.enable_force_control, + "grasp_mode": self.grasp_mode, + "mesh_name": self.mesh_name, + } + + return action_dict class Place(Skill): """ From 6e5d05b85ffd862c47563b498e04949da80fb9ff Mon Sep 17 00:00:00 2001 From: Joanne Truong Date: Thu, 22 Aug 2024 09:49:12 -0700 Subject: [PATCH 43/82] add test scripts --- .../skill_test/test_mobile_gaze_ee.py | 20 +++++++++++++++++++ .../skill_test/test_sem_place_ee.py | 15 ++++---------- 2 files changed, 24 insertions(+), 11 deletions(-) create mode 100644 spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py diff --git a/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py b/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py new file mode 100644 index 00000000..b2841897 --- /dev/null +++ b/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py @@ -0,0 +1,20 @@ +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. + + +import numpy as np +from spot_rl.envs.skill_manager import SpotSkillManager + +if __name__ == "__main__": + from perception_and_utils.utils.generic_utils import map_user_input_to_boolean + + spotskillmanager = SpotSkillManager(use_mobile_pick=True, use_ee=True) + contnue = True + while contnue: + spotskillmanager.pick("cup") + spotskillmanager.spot.open_gripper() + contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") + + # Navigate to dock and shutdown + spotskillmanager.dock() diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py index 60e214df..f8e5eedf 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -7,20 +7,13 @@ if __name__ == "__main__": # Know which location we are doing experiments - in_fre_lab = map_user_input_to_boolean("Are you JT in FRE? Y/N ") enable_estimation_before_place = map_user_input_to_boolean( - "Enable estimation before place? Y/N " + "Use waypoint estimator? Y/N " ) - if in_fre_lab: - # at FRE - place_target = "coffee_table" - else: - # at NYC - place_target = "test_desk" - + place_target = "coffee_table" spotskillmanager = SpotSkillManager( - use_mobile_pick=False, use_semantic_place=True, use_semantic_place_ee=True + use_mobile_pick=False, use_semantic_place=True, use_ee=True ) is_local = False @@ -32,5 +25,5 @@ contnue = True while contnue: rospy.set_param("is_gripper_blocked", 0) - spotskillmanager.place(place_target, is_local=is_local, visualize=True) + spotskillmanager.place(place_target, is_local=is_local, visualize=False) contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") From 599d7c668a277de62202306d8574a9c8e8607198 Mon Sep 17 00:00:00 2001 From: Joanne Truong Date: Fri, 23 Aug 2024 12:25:19 -0400 Subject: [PATCH 44/82] add logging and open gripper to semantic place ee test --- .../experiments/skill_test/test_sem_place_ee.py | 17 +++++++++++++++++ 1 file changed, 17 insertions(+) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py index f8e5eedf..e357ea16 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -4,6 +4,8 @@ import rospy from perception_and_utils.utils.generic_utils import map_user_input_to_boolean from spot_rl.envs.skill_manager import SpotSkillManager +from datetime import datetime +import json if __name__ == "__main__": # Know which location we are doing experiments @@ -23,7 +25,22 @@ # Start testing contnue = True + episode_ctr = 0 while contnue: + spotskillmanager.spot.open_gripper() + input('Place an object in Spot\'s gripper and press Enter to continue...') rospy.set_param("is_gripper_blocked", 0) + episode_log = {"actions": []} + spotskillmanager.place(place_target, is_local=is_local, visualize=False) + skill_log = spotskillmanager.place_controller.skill_result_log + if "num_steps" not in skill_log: + skill_log["num_steps"] = 0 + episode_log["actions"].append({f"place": skill_log}) + curr_date = datetime.today().strftime('%m-%d-%y') + file_path = f"logs/semantic_place_ee/{curr_date}/episode_{episode_ctr}.json" + with open(file_path, "w") as file: + json.dump(episode_log, file, indent=4) + print(f"Saved log: {file_path}") + episode_ctr +=1 contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") From cd928f89c6908f187cce4f98944ec5b3bcfb2adf Mon Sep 17 00:00:00 2001 From: Tushar Sangam Date: Mon, 26 Aug 2024 16:20:03 -0700 Subject: [PATCH 45/82] Minor chagnges for new policy weights --- .../experiments/skill_test/parse_logs_csv.py | 110 ++++++++++++++++++ .../skill_test/test_mobile_gaze_ee.py | 4 +- .../experiments/skill_test/test_sem_place.py | 62 ++++++++++ .../skill_test/test_sem_place_ee.py | 14 +-- .../skill_test/test_static_place.py | 63 ++++++++++ spot_rl_experiments/spot_rl/envs/place_env.py | 9 +- .../spot_rl/envs/skill_manager.py | 26 +++-- .../spot_rl/skills/atomic_skills.py | 2 +- 8 files changed, 266 insertions(+), 24 deletions(-) create mode 100644 spot_rl_experiments/experiments/skill_test/parse_logs_csv.py create mode 100644 spot_rl_experiments/experiments/skill_test/test_static_place.py diff --git a/spot_rl_experiments/experiments/skill_test/parse_logs_csv.py b/spot_rl_experiments/experiments/skill_test/parse_logs_csv.py new file mode 100644 index 00000000..244bac21 --- /dev/null +++ b/spot_rl_experiments/experiments/skill_test/parse_logs_csv.py @@ -0,0 +1,110 @@ +import argparse +import csv +import json +import os + + +def parse_json_file(file_path): + try: + with open(file_path, "r") as file: + data = json.load(file) + return data + except FileNotFoundError: + print(f"Error: File '{file_path}' not found.") + return None + except json.JSONDecodeError: + print(f"Error: '{file_path}' is not a valid JSON file.") + return None + + +def format_value(key, value): + if isinstance(value, (int, float)): + value = round(value, 3) + return str(value) + + +def export_to_csv(results_dict, output_file): + with open(output_file, mode="w", newline="") as file: + writer = csv.writer(file) + # Write header (keys of the dictionary) + writer.writerow(results_dict.keys()) + # Write data (values of the dictionary) + writer.writerow(results_dict.values()) + + +def process_json_file(input_file, output_file): + parsed_data = parse_json_file(input_file) + if parsed_data is None: + return + + results_dict = { + "overall_success": " ", + "total_time": parsed_data.get("total_time", " "), + "total_num_steps": parsed_data.get("total_steps", " "), + "0_nav_success": " ", + "0_nav_time_taken": " ", + "0_nav_num_steps": " ", + "0_nav_distance_travelled": " ", + "0_nav_distance_to_goal_linear": " ", + "0_nav_distance_to_goal_angular": " ", + "1_pick_success": " ", + "1_pick_time_taken": " ", + "1_pick_num_steps": " ", + "2_nav_success": " ", + "2_nav_time_taken": " ", + "2_nav_num_steps": " ", + "2_nav_distance_travelled": " ", + "2_nav_distance_to_goal_linear": " ", + "2_nav_distance_to_goal_angular": " ", + "3_place_success": " ", + "3_place_time_taken": " ", + "3_place_num_steps": " ", + } + + success_ctr = 0 + for idx, action in enumerate(parsed_data.get("actions", [])): + for action_type in action.keys(): + for k, v in action[action_type].items(): + if ( + action_type == "pick" or action_type == "place" + ) and k == "distance_travelled": + pass + elif k == "robot_trajectory": + pass + elif k == "distance_to_goal": + for bk, bv in action[action_type][k].items(): + match_str = f"{idx}_{action_type}_{k}_{bk}" + results_dict[match_str] = bv + else: + match_str = f"{idx}_{action_type}_{k}" + results_dict[match_str] = v + if "success" in k and v is True: + success_ctr += 1 + results_dict["overall_success"] = success_ctr / 4 + + export_to_csv(results_dict, output_file) + + +def main(): + parser = argparse.ArgumentParser(description="Convert JSON files in a folder to CSV") + parser.add_argument("input_folder", help="Path to the folder containing JSON files") + parser.add_argument("output_folder", help="Path to the folder where CSV files will be saved") + args = parser.parse_args() + + if not os.path.exists(args.input_folder): + print(f"Error: Input folder '{args.input_folder}' does not exist.") + return + + if not os.path.exists(args.output_folder): + os.makedirs(args.output_folder) + + for filename in os.listdir(args.input_folder): + if filename.endswith(".json"): + input_file = os.path.join(args.input_folder, filename) + output_file = os.path.join(args.output_folder, f"{os.path.splitext(filename)[0]}.csv") + process_json_file(input_file, output_file) + print(f"Processed {filename} into {output_file}") + + +if __name__ == "__main__": + main() diff --git a/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py b/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py index b2841897..5318f242 100644 --- a/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py @@ -9,10 +9,10 @@ if __name__ == "__main__": from perception_and_utils.utils.generic_utils import map_user_input_to_boolean - spotskillmanager = SpotSkillManager(use_mobile_pick=True, use_ee=True) + spotskillmanager = SpotSkillManager(use_mobile_pick=False) contnue = True while contnue: - spotskillmanager.pick("cup") + spotskillmanager.pick("Frosted Flakes Cup") spotskillmanager.spot.open_gripper() contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index d5e64cd9..3a0dd0a5 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -27,9 +27,26 @@ place_target = "test_desk" spotskillmanager = SpotSkillManager(use_mobile_pick=False, use_semantic_place=True) +<<<<<<< HEAD +======= + spot_pos1 = spotskillmanager.spot.get_arm_joint_positions(as_array=True) + + # is_local = False + # if enable_estimation_before_place: + # place_target = None + # is_local = True + + # # Start testing + # contnue = True + # while contnue: + # rospy.set_param("is_gripper_blocked", 0) + # spotskillmanager.place(place_target, is_local=is_local, visualize=True) + # contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") +>>>>>>> 41ba97e (Minor chagnges for new policy weights) is_local = False # Start testing contnue = True +<<<<<<< HEAD episode_ctr = 0 # Get EE Pose Initial in rest position spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() @@ -55,6 +72,29 @@ skill_log["num_steps"] = 0 episode_log["actions"].append({"place": skill_log}) +======= + INITIAL_ARM_JOINT_ANGLES = [0, -180, 180, 90, 0, -90] + episode_ctr = 0 + #Get EE Pose Initial + spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() + #Set Orientation as Zero + spot_ort = np.zeros(3) + while contnue: + #Open Gripper + spotskillmanager.spot.open_gripper() + input("Place an object in Spot's gripper and press Enter to continue...") + #Place Object and Close Gripper + rospy.set_param("is_gripper_blocked", 0) + episode_log = {"actions": []} + spotskillmanager.spot.close_gripper() + input("waiting for user to get ready with camera") + + spotskillmanager.place(place_target, is_local=is_local, visualize=False) + skill_log = spotskillmanager.place_controller.skill_result_log + if "num_steps" not in skill_log: + skill_log["num_steps"] = 0 + episode_log["actions"].append({f"place": skill_log}) +>>>>>>> 41ba97e (Minor chagnges for new policy weights) curr_date = datetime.today().strftime("%m-%d-%y") file_path = ( f"logs/semantic_place/{curr_date}/episode_sem_pl_run2_{episode_ctr}.json" @@ -64,5 +104,27 @@ print(f"Saved log: {file_path}") episode_ctr += 1 contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") +<<<<<<< HEAD # Return the arm to the original position spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) +======= + #Return the arm to the original position + spot_pos = spotskillmanager.spot.get_ee_pos_in_body_frame()[0] + spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) +# The following is a helpful tip to debug the arm +# We get Spot class +# spot = spotskillmanager.spot +# We can move the gripper to a point with x,y,z and roll, pitch, yaw +# spot.move_gripper_to_point((0.55, 0., 0.26), np.deg2rad(np.array([0,0,0]))) +# We can also set the robot arm joints +# config = construct_config() +# spot.set_arm_joint_positions(np.deg2rad(config.INITIAL_ARM_JOINT_ANGLES)) + +# In addition, if you want to use semantic place skill based on the grasping orientation, you can do +# spotskillmanager.nav("black_case") +# spotskillmanager.pick("bottle") +# # Fetch the arm joint at grasping location +# ee_orientation_at_grasping = spotskillmanager.gaze_controller.env.ee_orientation_at_grasping +# spotskillmanager.nav("test_desk") +# spotskillmanager.place("test_desk", orientation_at_grasping) # This controls the arm initial orientation +>>>>>>> 41ba97e (Minor chagnges for new policy weights) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py index e357ea16..3c2e2fed 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -1,23 +1,21 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. +import json +from datetime import datetime import rospy from perception_and_utils.utils.generic_utils import map_user_input_to_boolean from spot_rl.envs.skill_manager import SpotSkillManager -from datetime import datetime -import json if __name__ == "__main__": # Know which location we are doing experiments enable_estimation_before_place = map_user_input_to_boolean( "Use waypoint estimator? Y/N " ) - place_target = "coffee_table" spotskillmanager = SpotSkillManager( use_mobile_pick=False, use_semantic_place=True, use_ee=True ) - is_local = False if enable_estimation_before_place: place_target = None @@ -28,7 +26,7 @@ episode_ctr = 0 while contnue: spotskillmanager.spot.open_gripper() - input('Place an object in Spot\'s gripper and press Enter to continue...') + input("Place an object in Spot's gripper and press Enter to continue...") rospy.set_param("is_gripper_blocked", 0) episode_log = {"actions": []} @@ -37,10 +35,10 @@ if "num_steps" not in skill_log: skill_log["num_steps"] = 0 episode_log["actions"].append({f"place": skill_log}) - curr_date = datetime.today().strftime('%m-%d-%y') + curr_date = datetime.today().strftime("%m-%d-%y") file_path = f"logs/semantic_place_ee/{curr_date}/episode_{episode_ctr}.json" with open(file_path, "w") as file: json.dump(episode_log, file, indent=4) print(f"Saved log: {file_path}") - episode_ctr +=1 - contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") + episode_ctr += 1 + contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") \ No newline at end of file diff --git a/spot_rl_experiments/experiments/skill_test/test_static_place.py b/spot_rl_experiments/experiments/skill_test/test_static_place.py new file mode 100644 index 00000000..3e015cbc --- /dev/null +++ b/spot_rl_experiments/experiments/skill_test/test_static_place.py @@ -0,0 +1,63 @@ +# Copyright (c) Meta Platforms, Inc. and its affiliates. +# This source code is licensed under the MIT license found in the +# LICENSE file in the root directory of this source tree. +import rospy +from perception_and_utils.utils.generic_utils import map_user_input_to_boolean +from spot_rl.envs.skill_manager import SpotSkillManager +from datetime import datetime +import json +import time +import numpy as np + +if __name__ == "__main__": + + # Know which location we are doing experiments + enable_estimation_before_place = map_user_input_to_boolean( + "Use waypoint estimator? Y/N " + ) + + place_target = "new_place_waypoint" + spotskillmanager = SpotSkillManager( + use_mobile_pick=False, use_semantic_place=False, use_place_ee=False + ) + spot_pos1= spotskillmanager.spot.get_arm_joint_positions(as_array=True) + print("CURR POSE",spot_pos1) + + + is_local = False + if enable_estimation_before_place: + place_target = None + is_local = True + + # Start testing + contnue = True + INITIAL_ARM_JOINT_ANGLES=[0, -180, 180, 90, 0, -90] + episode_ctr = 1 + spot_pos,spot_ort=spotskillmanager.spot.get_ee_pos_in_body_frame() + spot_ort=np.zeros(3) + while contnue: + + spotskillmanager.spot.open_gripper() + input('Place an object in Spot\'s gripper and press Enter to continue...') + rospy.set_param("is_gripper_blocked", 0) + episode_log = {"actions": []} + spotskillmanager.spot.close_gripper() + input("waiting for user to get ready with camera") + + spotskillmanager.place(place_target, is_local=is_local, visualize=False) + skill_log = spotskillmanager.place_controller.skill_result_log + if "num_steps" not in skill_log: + skill_log["num_steps"] = 0 + episode_log["actions"].append({f"place": skill_log}) + curr_date = datetime.today().strftime('%m-%d-%y') + file_path = f"logs/static_place/{curr_date}/episode_run2_{episode_ctr}.json" + with open(file_path, "w") as file: + json.dump(episode_log, file, indent=4) + print(f"Saved log: {file_path}") + episode_ctr +=1 + contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") + + spot_pos=spotskillmanager.spot.get_ee_pos_in_body_frame()[0] + spotskillmanager.spot.move_gripper_to_point(spot_pos,spot_ort) + + # spotskillmanager.spot.set_arm_joint_positions((np.deg2rad(INITIAL_ARM_JOINT_ANGLES))) diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index c65e8763..f9b699d6 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -371,13 +371,14 @@ def get_observations(self): arm_depth, _ = self.get_gripper_images() # Get ee's pose -- x,y,z - xyz, _ = self.spot.get_ee_pos_in_body_frame() + xyz, rpy = self.spot.get_ee_pos_in_body_frame() observations = { "obj_goal_sensor": obj_goal_sensor, "relative_initial_ee_orientation": delta_ee, "relative_target_object_orientation": delta_obj, "articulated_agent_jaw_depth": arm_depth, - "ee_pos": xyz, - "is_holding": np.ones((1,)), + "ee_pose": np.concatenate([xyz, rpy]), + # "ee_pos":xyz, + "is_holding": np.ones((1,)), } - return observations + return observations \ No newline at end of file diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index ea30f5ab..48e135e4 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -11,12 +11,12 @@ from multimethod import multimethod from perception_and_utils.utils.generic_utils import conditional_print from spot_rl.skills.atomic_skills import ( + MobilePickEE, Navigation, OpenCloseDrawer, Pick, Place, SemanticPick, - MobilePickEE, SemanticPlace, SemanticPlaceEE, ) @@ -105,7 +105,8 @@ def __init__( open_close_drawer_config=None, use_mobile_pick: bool = False, use_semantic_place: bool = False, - use_ee: bool = False, + use_pick_ee: bool = False, + use_place_ee: bool = False, verbose: bool = True, use_policies: bool = True, ): @@ -117,8 +118,10 @@ def __init__( # Process the meta parameters self._use_mobile_pick = use_mobile_pick + print("USE MOBILE PICK VARIABLE IS SET TO :", self._use_mobile_pick) self.use_semantic_place = use_semantic_place - self.use_ee = use_ee + self.use_pick_ee = use_pick_ee + self.use_place_ee = use_place_ee # Create the spot object, init lease, and construct configs self.__init_spot( @@ -209,20 +212,23 @@ def __initiate_controllers(self, use_policies: bool = True): spot=self.spot, config=self.nav_config, ) - if self.use_ee: - self.gaze_controller = Pick( + if self.use_pick_ee: + # print("GOING INSIDE GAZE EE ENV") + self.gaze_controller = MobilePickEE( spot=self.spot, config=self.pick_config, use_mobile_pick=self._use_mobile_pick, ) else: - self.gaze_controller = MobilePickEE( + self.gaze_controller = Pick( spot=self.spot, config=self.pick_config, use_mobile_pick=self._use_mobile_pick, ) if self.use_semantic_place: - if self.use_ee: + + if self.use_place_ee: + print("GOING INSIDE SEM EE ENV") self.place_controller = SemanticPlaceEE( spot=self.spot, config=self.place_config ) @@ -523,8 +529,10 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray return False, message if self.use_semantic_place: # Convert HOME frame coordinates into body frame - place_target_location = self.place_controller.env.get_target_in_base_frame( - mn.Vector3(*place_target_location.astype(np.float64).tolist()) + place_target_location = ( + self.place_controller.env.get_target_in_base_frame( + mn.Vector3(*place_target_location.astype(np.float64).tolist()) + ) ) is_local = True else: diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index 36f66014..08dab812 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -721,7 +721,7 @@ def __init__(self, spot: Spot, config, use_mobile_pick=True): ) self.policy.reset() - self.env = SpotGazeEEEnv(config, spot) + self.env = SpotGazeEEEnv(config, spot, use_mobile_pick) def split_action(self, action: np.ndarray) -> Dict[str, Any]: """Refer to class Skill for documentation""" From b601868ee21415b0d2e69f789830f290faeb234c Mon Sep 17 00:00:00 2001 From: Joanne Truong Date: Wed, 28 Aug 2024 21:00:03 -0700 Subject: [PATCH 46/82] fix distance threshold for gaze ee policy --- spot_rl_experiments/spot_rl/envs/gaze_env.py | 56 ++++---------------- 1 file changed, 11 insertions(+), 45 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/gaze_env.py b/spot_rl_experiments/spot_rl/envs/gaze_env.py index d0dafb9a..c92e764b 100644 --- a/spot_rl_experiments/spot_rl/envs/gaze_env.py +++ b/spot_rl_experiments/spot_rl/envs/gaze_env.py @@ -38,6 +38,7 @@ def __init__(self, config, spot: Spot, use_mobile_pick: bool = False): max_lin_dist_key=max_lin_dist_key, max_ang_dist_key=max_ang_dist_key, ) + self.grasp_dist_threshold = 1.5 self.target_obj_name = None self._use_mobile_pick = use_mobile_pick self.initial_arm_joint_angles = np.deg2rad(config.GAZE_ARM_JOINT_ANGLES) @@ -66,7 +67,7 @@ def reset(self, target_obj_name, *args, **kwargs): return observations def step(self, action_dict: Dict[str, Any]): - grasp = self.should_grasp() + grasp = self.should_grasp(self.grasp_dist_threshold) # Update the action_dict with grasp and place flags action_dict["grasp"] = grasp @@ -83,15 +84,14 @@ def remap_observation_keys_for_hab3(self, observations): @INFO: Policies trained on older hab versions DON'T need remapping """ - mobile_gaze_observations = {} - mobile_gaze_observations["arm_depth_bbox_sensor"] = observations[ + new_observations = observations.copy() + new_observations["arm_depth_bbox_sensor"] = observations[ "arm_depth_bbox" ] - mobile_gaze_observations["articulated_agent_arm_depth"] = observations[ + new_observations["articulated_agent_arm_depth"] = observations[ "arm_depth" ] - mobile_gaze_observations["joint"] = observations["joint"] - return mobile_gaze_observations + return new_observations def get_observations(self): arm_depth, arm_depth_bbox = self.get_gripper_images() @@ -117,48 +117,14 @@ def __init__(self, config, spot: Spot, use_mobile_pick: bool = False): spot, use_mobile_pick=use_mobile_pick, ) - - def step(self, action_dict: Dict[str, Any]): - grasp = self.should_grasp() - - # Update the action_dict with grasp and place flags - action_dict["grasp"] = grasp - action_dict["place"] = False # TODO: Why is gaze getting flag for place? - - observations, reward, done, info = super().step( - action_dict=action_dict, - ) - return observations, reward, done, info - - def remap_observation_keys_for_hab3(self, observations): - """ - Change observation keys as per hab3. - - @INFO: Policies trained on older hab versions DON'T need remapping - """ - semantic_gaze_observations = {} - semantic_gaze_observations["arm_depth_bbox_sensor"] = observations[ - "arm_depth_bbox" - ] - semantic_gaze_observations["articulated_agent_arm_depth"] = observations[ - "arm_depth" - ] - semantic_gaze_observations["ee_pose"] = observations["ee_pose"] - return semantic_gaze_observations + self.grasp_dist_threshold = 0.7 def get_observations(self): - arm_depth, arm_depth_bbox = self.get_gripper_images() + observations = super().get_observations() + if "joint" in observations: + del observations["joint"] xyz, rpy = self.spot.get_ee_pos_in_body_frame() - observations = { - "ee_pose": np.concatenate([xyz, rpy]), - "arm_depth": arm_depth, - "arm_depth_bbox": arm_depth_bbox, - } - - # Remap observation keys for mobile gaze as it was trained with Habitat version3 - if self._use_mobile_pick: - observations = self.remap_observation_keys_for_hab3(observations) - + observations["ee_pose"] = np.concatenate([xyz, rpy]) return observations class SpotSemanticGazeEnv(SpotBaseEnv): From 59ff34f03ee74c838df4d82959c66464b583b5e6 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Fri, 30 Aug 2024 15:56:32 -0700 Subject: [PATCH 47/82] Addition of Scaling Variables in Config and Cleanup --- spot_rl_experiments/configs/config.yaml | 14 +++++++++-- .../skill_test/test_mobile_gaze_ee.py | 4 ++-- .../skill_test/test_sem_place_ee.py | 10 ++++---- spot_rl_experiments/spot_rl/envs/base_env.py | 15 ++++++++---- spot_rl_experiments/spot_rl/envs/gaze_env.py | 12 +++++----- spot_rl_experiments/spot_rl/envs/place_env.py | 24 +++++++++++++++---- .../spot_rl/envs/skill_manager.py | 4 +++- .../spot_rl/skills/atomic_skills.py | 20 +++++++++------- 8 files changed, 70 insertions(+), 33 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index bca51051..54f1e371 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -20,7 +20,7 @@ WEIGHTS_TORCHSCRIPT: SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" # Semantic place EE - SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" + SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_55k_mobile_0.1_30_ee_0.1_rot_drop_no_term_250_steps_EEPoseSensor_kinematic_ee_dist_ckpt_50.pth.torchscript" # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -45,7 +45,7 @@ WEIGHTS: SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" # Semantic place EE - SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_jt_ckpt.16.torchscript" + SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_55k_mobile_0.1_30_ee_0.1_rot_drop_no_term_250_steps_EEPoseSensor_kinematic_ee_dist_ckpt_50.pth.torchscript" # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -136,6 +136,16 @@ RECEPTACLES: MAX_LIN_DIST: 0.25 # meters MAX_ANG_DIST: 15.0 # degrees +# Base action params for sem place EE +MAX_LIN_DIST_SEMANTIC_PLACE: 0.1 # meters +MAX_ANG_DIST_SEMANTIC_PLACE: 5.73 # degrees + +# SEMANTIC EE POLICY SCALE PARAMETERS +EE_DIST_SCALE_SEMANTIC_PLACE : 0.3 +EE_ROT_SCALE_SEMANTIC_PLACE : 0.3 +EE_DIST_SCALE_MOBILE_GAZE : 0.1 +EE_ROT_SCALE_MOBILE_GAZE : 0.0125 + # Arm action params MAX_JOINT_MOVEMENT: 0.0872665 # Gaze arm speed (5 deg) MAX_JOINT_MOVEMENT_2: 0.174533 # Place arm speed (10 deg) diff --git a/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py b/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py index 5318f242..3b3f7378 100644 --- a/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py @@ -1,7 +1,7 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. - +# black: ignore-errors import numpy as np from spot_rl.envs.skill_manager import SpotSkillManager @@ -9,7 +9,7 @@ if __name__ == "__main__": from perception_and_utils.utils.generic_utils import map_user_input_to_boolean - spotskillmanager = SpotSkillManager(use_mobile_pick=False) + spotskillmanager = SpotSkillManager(use_mobile_pick=True, use_pick_ee=True) contnue = True while contnue: spotskillmanager.pick("Frosted Flakes Cup") diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py index 3c2e2fed..98bd2fab 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -1,8 +1,10 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. +# mypy: ignore-errors import json from datetime import datetime + import rospy from perception_and_utils.utils.generic_utils import map_user_input_to_boolean from spot_rl.envs.skill_manager import SpotSkillManager @@ -12,9 +14,9 @@ enable_estimation_before_place = map_user_input_to_boolean( "Use waypoint estimator? Y/N " ) - place_target = "coffee_table" + place_target = "new_place_waypoint" spotskillmanager = SpotSkillManager( - use_mobile_pick=False, use_semantic_place=True, use_ee=True + use_mobile_pick=False, use_semantic_place=True, use_place_ee=True ) is_local = False if enable_estimation_before_place: @@ -34,11 +36,11 @@ skill_log = spotskillmanager.place_controller.skill_result_log if "num_steps" not in skill_log: skill_log["num_steps"] = 0 - episode_log["actions"].append({f"place": skill_log}) + episode_log["actions"].append({"place": skill_log}) curr_date = datetime.today().strftime("%m-%d-%y") file_path = f"logs/semantic_place_ee/{curr_date}/episode_{episode_ctr}.json" with open(file_path, "w") as file: json.dump(episode_log, file, indent=4) print(f"Saved log: {file_path}") episode_ctr += 1 - contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") \ No newline at end of file + contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index f7144990..f1abeabd 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -427,8 +427,13 @@ def step( # noqa arm_ee_action = rescale_arm_ee_actions(arm_ee_action) if np.count_nonzero(arm_ee_action) > 0: # TODO: semantic place ee: move this to config - arm_ee_action[0:3] *= 0.2 # 0.015 - arm_ee_action[3:6] *= 0.2 # 0.0125 + if place is not None: + arm_ee_action[0:3] *= self.arm_ee_dist_scale # 0.015 + arm_ee_action[3:6] *= self.arm_ee_rot_scale # 0.0125 + else: + arm_ee_action[0:3] *= self.config.EE_DIST_SCALE_MOBILE_GAZE # 0.015 + arm_ee_action[3:6] *= self.config.EE_ROT_SCALE_MOBILE_GAZE # 0.0125 + xyz, rpy = self.spot.get_ee_pos_in_body_frame() cur_ee_pose = np.concatenate((xyz, rpy), axis=0) # Wrap the heading @@ -483,9 +488,9 @@ def step( # noqa # TODO: semantic place ee: temp hack to command this out # for not using base velocity start_time = time.time() - if arm_ee_action is not None: - print("do not move the base") - elif base_action is not None or arm_action is not None: + # if arm_ee_action is not None: + # print("do not move the base") + if base_action is not None or arm_action is not None: while time.time() < start_time + 1 / self.ctrl_hz: if target_yaw is not None and abs( wrap_heading(self.yaw - target_yaw) diff --git a/spot_rl_experiments/spot_rl/envs/gaze_env.py b/spot_rl_experiments/spot_rl/envs/gaze_env.py index c92e764b..09e0ddbe 100644 --- a/spot_rl_experiments/spot_rl/envs/gaze_env.py +++ b/spot_rl_experiments/spot_rl/envs/gaze_env.py @@ -85,12 +85,8 @@ def remap_observation_keys_for_hab3(self, observations): @INFO: Policies trained on older hab versions DON'T need remapping """ new_observations = observations.copy() - new_observations["arm_depth_bbox_sensor"] = observations[ - "arm_depth_bbox" - ] - new_observations["articulated_agent_arm_depth"] = observations[ - "arm_depth" - ] + new_observations["arm_depth_bbox_sensor"] = observations["arm_depth_bbox"] + new_observations["articulated_agent_arm_depth"] = observations["arm_depth"] return new_observations def get_observations(self): @@ -110,6 +106,7 @@ def get_observations(self): def get_success(self, observations): return self.grasp_attempted + class SpotGazeEEEnv(SpotGazeEnv): def __init__(self, config, spot: Spot, use_mobile_pick: bool = False): super().__init__( @@ -117,6 +114,8 @@ def __init__(self, config, spot: Spot, use_mobile_pick: bool = False): spot, use_mobile_pick=use_mobile_pick, ) + self.arm_ee_dist_scale = self.config.EE_DIST_SCALE_MOBILE_GAZE + self.arm_ee_rot_scale = self.config.EE_ROT_SCALE_MOBILE_GAZE self.grasp_dist_threshold = 0.7 def get_observations(self): @@ -127,6 +126,7 @@ def get_observations(self): observations["ee_pose"] = np.concatenate([xyz, rpy]) return observations + class SpotSemanticGazeEnv(SpotBaseEnv): def __init__(self, config, spot: Spot): # Select suitable keys diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index f9b699d6..0cd40c0c 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -68,12 +68,24 @@ def get_observations(self): class SpotSemanticPlaceEnv(SpotBaseEnv): """This is Spot semantic place class""" - def __init__(self, config, spot: Spot): + def __init__(self, config, spot: Spot, use_semantic_place: bool = False): # We set the initial arm joints config.INITIAL_ARM_JOINT_ANGLES = copy.deepcopy( config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE ) - super().__init__(config, spot) + max_lin_dist_sem_place = ( + "MAX_LIN_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_LIN_DIST" + ) + max_ang_dist_sem_place = ( + "MAX_ANG_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_ANG_DIST" + ) + + super().__init__( + config, + spot, + max_lin_dist_key=max_lin_dist_sem_place, + max_ang_dist_key=max_ang_dist_sem_place, + ) # Define the place variables self.place_target = None @@ -245,6 +257,10 @@ def __init__(self, config, spot: Spot): self.initial_ee_pose = None self.target_object_pose = None + # Define End Effector Policy Scale Values + self.arm_ee_dist_scale = self.config.EE_DIST_SCALE_SEMANTIC_PLACE + self.arm_ee_rot_scale = self.config.EE_ROT_SCALE_SEMANTIC_PLACE + # Overwrite joint limits for semantic_place skills self.arm_lower_limits = np.deg2rad(config.ARM_LOWER_LIMITS_SEMANTIC_PLACE) self.arm_upper_limits = np.deg2rad(config.ARM_UPPER_LIMITS_SEMANTIC_PLACE) @@ -379,6 +395,6 @@ def get_observations(self): "articulated_agent_jaw_depth": arm_depth, "ee_pose": np.concatenate([xyz, rpy]), # "ee_pos":xyz, - "is_holding": np.ones((1,)), + "is_holding": np.ones((1,)), } - return observations \ No newline at end of file + return observations diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 48e135e4..5ec2b5fb 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -234,7 +234,9 @@ def __initiate_controllers(self, use_policies: bool = True): ) else: self.place_controller = SemanticPlace( - spot=self.spot, config=self.place_config + spot=self.spot, + config=self.place_config, + use_semantic_place=self.use_semantic_place, ) else: self.place_controller = Place( diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index 08dab812..a760e187 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -1,7 +1,7 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. - +# black : ignore-errors import time from typing import Any, Dict, List, Tuple @@ -12,7 +12,7 @@ conditional_print, map_user_input_to_boolean, ) -from spot_rl.envs.gaze_env import SpotGazeEnv, SpotSemanticGazeEnv, SpotGazeEEEnv +from spot_rl.envs.gaze_env import SpotGazeEEEnv, SpotGazeEnv, SpotSemanticGazeEnv # Import Envs from spot_rl.envs.nav_env import SpotNavEnv @@ -26,8 +26,8 @@ # Import policies from spot_rl.real_policy import ( GazePolicy, - MobileGazePolicy, MobileGazeEEPolicy, + MobileGazePolicy, NavPolicy, OpenCloseDrawerPolicy, PlacePolicy, @@ -704,6 +704,7 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: } return action_dict + class MobilePickEE(Pick): """ Semantic place ee controller is used to execute place for given place targets @@ -741,6 +742,7 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: return action_dict + class Place(Skill): """ Place controller is used to execute place for given place targets @@ -975,17 +977,17 @@ class SemanticPlace(Place): Semantic place controller is used to execute place for given place targets """ - def __init__(self, spot: Spot, config): + def __init__(self, spot: Spot, config, use_semantic_place=False): if not config: config = construct_config_for_semantic_place() super().__init__(spot, config) - - self.policy = SemanticPlacePolicy( - config.WEIGHTS.SEMANTIC_PLACE, device=config.DEVICE, config=config - ) + if use_semantic_place: + self.policy = SemanticPlacePolicy( + config.WEIGHTS.SEMANTIC_PLACE, device=config.DEVICE, config=config + ) self.policy.reset() - self.env = SpotSemanticPlaceEnv(config, spot) + self.env = SpotSemanticPlaceEnv(config, spot, use_semantic_place) def execute_rl_loop(self, goal_dict: Dict[str, Any]) -> Tuple[bool, str]: # Set the robot inital pose From 190366b2afb13880f97a6e903056f42a45bfd59b Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Fri, 30 Aug 2024 16:05:51 -0700 Subject: [PATCH 48/82] Cleanup Changes --- .../experiments/skill_test/test_mobile_gaze_ee.py | 2 -- .../experiments/skill_test/test_sem_place_ee.py | 2 ++ spot_rl_experiments/spot_rl/envs/base_env.py | 4 ---- spot_rl_experiments/spot_rl/skills/atomic_skills.py | 2 -- 4 files changed, 2 insertions(+), 8 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py b/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py index 3b3f7378..fb2faa88 100644 --- a/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_mobile_gaze_ee.py @@ -1,8 +1,6 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. -# black: ignore-errors - import numpy as np from spot_rl.envs.skill_manager import SpotSkillManager diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py index 98bd2fab..1bdab823 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -31,6 +31,8 @@ input("Place an object in Spot's gripper and press Enter to continue...") rospy.set_param("is_gripper_blocked", 0) episode_log = {"actions": []} + spotskillmanager.spot.close_gripper() + input("waiting for user to get ready with camera") spotskillmanager.place(place_target, is_local=is_local, visualize=False) skill_log = spotskillmanager.place_controller.skill_result_log diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index f1abeabd..bbea30a2 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -430,10 +430,6 @@ def step( # noqa if place is not None: arm_ee_action[0:3] *= self.arm_ee_dist_scale # 0.015 arm_ee_action[3:6] *= self.arm_ee_rot_scale # 0.0125 - else: - arm_ee_action[0:3] *= self.config.EE_DIST_SCALE_MOBILE_GAZE # 0.015 - arm_ee_action[3:6] *= self.config.EE_ROT_SCALE_MOBILE_GAZE # 0.0125 - xyz, rpy = self.spot.get_ee_pos_in_body_frame() cur_ee_pose = np.concatenate((xyz, rpy), axis=0) # Wrap the heading diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index a760e187..2b715cf3 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -1,8 +1,6 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. -# black : ignore-errors - import time from typing import Any, Dict, List, Tuple From b0c412b248ef26d020750f4c1a8bbb7d8bc36644 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Fri, 30 Aug 2024 19:47:15 -0700 Subject: [PATCH 49/82] Minor bug fixes in place_env --- bd_spot_wrapper/spot_wrapper/spot.py | 20 ++++++------- .../experiments/skill_test/test_sem_place.py | 26 ++++++++++++++--- .../skill_test/test_sem_place_ee.py | 6 ++-- spot_rl_experiments/spot_rl/envs/base_env.py | 5 ++-- spot_rl_experiments/spot_rl/envs/place_env.py | 28 +++++++++++-------- .../spot_rl/envs/skill_manager.py | 5 ++-- .../spot_rl/skills/atomic_skills.py | 16 +++++------ 7 files changed, 65 insertions(+), 41 deletions(-) diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index dc41b556..d322c897 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -1094,22 +1094,22 @@ def set_base_vel_and_arm_ee_pos( ): print(f"in set_base_vel_and_arm_ee_pos: {arm_ee_action} {travel_time}") # TODO: semantic place ee: temp hack to distance the base vel - # base_cmd = self.set_base_velocity( - # x_vel, - # y_vel, - # ang_vel, - # vel_time=travel_time, - # disable_obstacle_avoidance=disable_obstacle_avoidance, - # return_cmd=True, - # ) + base_cmd = self.set_base_velocity( + x_vel, + y_vel, + ang_vel, + vel_time=travel_time, + disable_obstacle_avoidance=disable_obstacle_avoidance, + return_cmd=True, + ) arm_cmd = self.move_gripper_to_point( point=arm_ee_action[0:3], rotation=list(arm_ee_action[3:]), seconds_to_goal=travel_time, return_cmd=True, ) - # synchro_command = RobotCommandBuilder.build_synchro_command(base_cmd, arm_cmd) - synchro_command = RobotCommandBuilder.build_synchro_command(arm_cmd) + synchro_command = RobotCommandBuilder.build_synchro_command(base_cmd, arm_cmd) + # synchro_command = RobotCommandBuilder.build_synchro_command(arm_cmd) cmd_id = self.command_client.robot_command( synchro_command, end_time_secs=time.time() + travel_time ) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index 3a0dd0a5..6772cfa5 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -2,7 +2,10 @@ # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. # mypy: ignore-errors +<<<<<<< HEAD # black: ignore-errors +======= +>>>>>>> abd65c7 ( Minor bug fixes in place_env) import json import time from datetime import datetime @@ -21,7 +24,11 @@ if in_fre_lab: # at FRE +<<<<<<< HEAD place_target = "160_teddy bear" +======= + place_target = "new_place_waypoint" +>>>>>>> abd65c7 ( Minor bug fixes in place_env) else: # at NYC place_target = "test_desk" @@ -31,6 +38,7 @@ ======= spot_pos1 = spotskillmanager.spot.get_arm_joint_positions(as_array=True) +<<<<<<< HEAD # is_local = False # if enable_estimation_before_place: # place_target = None @@ -43,6 +51,8 @@ # spotskillmanager.place(place_target, is_local=is_local, visualize=True) # contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") >>>>>>> 41ba97e (Minor chagnges for new policy weights) +======= +>>>>>>> abd65c7 ( Minor bug fixes in place_env) is_local = False # Start testing contnue = True @@ -75,15 +85,15 @@ ======= INITIAL_ARM_JOINT_ANGLES = [0, -180, 180, 90, 0, -90] episode_ctr = 0 - #Get EE Pose Initial + # Get EE Pose Initial spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() - #Set Orientation as Zero + # Set Orientation as Zero spot_ort = np.zeros(3) while contnue: - #Open Gripper + # Open Gripper spotskillmanager.spot.open_gripper() input("Place an object in Spot's gripper and press Enter to continue...") - #Place Object and Close Gripper + # Place Object and Close Gripper rospy.set_param("is_gripper_blocked", 0) episode_log = {"actions": []} spotskillmanager.spot.close_gripper() @@ -93,8 +103,12 @@ skill_log = spotskillmanager.place_controller.skill_result_log if "num_steps" not in skill_log: skill_log["num_steps"] = 0 +<<<<<<< HEAD episode_log["actions"].append({f"place": skill_log}) >>>>>>> 41ba97e (Minor chagnges for new policy weights) +======= + episode_log["actions"].append({"place": skill_log}) +>>>>>>> abd65c7 ( Minor bug fixes in place_env) curr_date = datetime.today().strftime("%m-%d-%y") file_path = ( f"logs/semantic_place/{curr_date}/episode_sem_pl_run2_{episode_ctr}.json" @@ -104,11 +118,15 @@ print(f"Saved log: {file_path}") episode_ctr += 1 contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") +<<<<<<< HEAD <<<<<<< HEAD # Return the arm to the original position spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) ======= #Return the arm to the original position +======= + # Return the arm to the original position +>>>>>>> abd65c7 ( Minor bug fixes in place_env) spot_pos = spotskillmanager.spot.get_ee_pos_in_body_frame()[0] spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) # The following is a helpful tip to debug the arm diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py index 1bdab823..d6c22b12 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -14,7 +14,7 @@ enable_estimation_before_place = map_user_input_to_boolean( "Use waypoint estimator? Y/N " ) - place_target = "new_place_waypoint" + place_target = "new_pl_pt" spotskillmanager = SpotSkillManager( use_mobile_pick=False, use_semantic_place=True, use_place_ee=True ) @@ -25,7 +25,7 @@ # Start testing contnue = True - episode_ctr = 0 + episode_ctr = 1 while contnue: spotskillmanager.spot.open_gripper() input("Place an object in Spot's gripper and press Enter to continue...") @@ -40,7 +40,7 @@ skill_log["num_steps"] = 0 episode_log["actions"].append({"place": skill_log}) curr_date = datetime.today().strftime("%m-%d-%y") - file_path = f"logs/semantic_place_ee/{curr_date}/episode_{episode_ctr}.json" + file_path = f"logs/semantic_place_ee/{curr_date}/0.5m_minus_20deg_episode_{episode_ctr}.json" with open(file_path, "w") as file: json.dump(episode_log, file, indent=4) print(f"Saved log: {file_path}") diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index bbea30a2..266d06bb 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -427,9 +427,8 @@ def step( # noqa arm_ee_action = rescale_arm_ee_actions(arm_ee_action) if np.count_nonzero(arm_ee_action) > 0: # TODO: semantic place ee: move this to config - if place is not None: - arm_ee_action[0:3] *= self.arm_ee_dist_scale # 0.015 - arm_ee_action[3:6] *= self.arm_ee_rot_scale # 0.0125 + arm_ee_action[0:3] *= self.arm_ee_dist_scale # 0.015 + arm_ee_action[3:6] *= self.arm_ee_rot_scale # 0.0125 xyz, rpy = self.spot.get_ee_pos_in_body_frame() cur_ee_pose = np.concatenate((xyz, rpy), axis=0) # Wrap the heading diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index 0cd40c0c..0196c65b 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -68,23 +68,15 @@ def get_observations(self): class SpotSemanticPlaceEnv(SpotBaseEnv): """This is Spot semantic place class""" - def __init__(self, config, spot: Spot, use_semantic_place: bool = False): + def __init__(self, config, spot: Spot): # We set the initial arm joints config.INITIAL_ARM_JOINT_ANGLES = copy.deepcopy( config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE ) - max_lin_dist_sem_place = ( - "MAX_LIN_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_LIN_DIST" - ) - max_ang_dist_sem_place = ( - "MAX_ANG_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_ANG_DIST" - ) super().__init__( config, spot, - max_lin_dist_key=max_lin_dist_sem_place, - max_ang_dist_key=max_ang_dist_sem_place, ) # Define the place variables @@ -238,12 +230,26 @@ def get_observations(self): class SpotSemanticPlaceEEEnv(SpotBaseEnv): """This is Spot semantic place class""" - def __init__(self, config, spot: Spot): + def __init__(self, config, spot: Spot, use_semantic_place: bool = False): # We set the initial arm joints config.INITIAL_ARM_JOINT_ANGLES = copy.deepcopy( config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE ) - super().__init__(config, spot) + max_lin_dist_sem_place = ( + "MAX_LIN_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_LIN_DIST" + ) + max_ang_dist_sem_place = ( + "MAX_ANG_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_ANG_DIST" + ) + + print("THE VAR", use_semantic_place) + + super().__init__( + config, + spot, + max_lin_dist_key=max_lin_dist_sem_place, + max_ang_dist_key=max_ang_dist_sem_place, + ) # Define the place variables self.place_target = None diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 5ec2b5fb..3382a6d2 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -230,13 +230,14 @@ def __initiate_controllers(self, use_policies: bool = True): if self.use_place_ee: print("GOING INSIDE SEM EE ENV") self.place_controller = SemanticPlaceEE( - spot=self.spot, config=self.place_config + spot=self.spot, + config=self.place_config, + use_semantic_place=self.use_semantic_place, ) else: self.place_controller = SemanticPlace( spot=self.spot, config=self.place_config, - use_semantic_place=self.use_semantic_place, ) else: self.place_controller = Place( diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index 2b715cf3..1f765786 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -975,17 +975,17 @@ class SemanticPlace(Place): Semantic place controller is used to execute place for given place targets """ - def __init__(self, spot: Spot, config, use_semantic_place=False): + def __init__(self, spot: Spot, config): if not config: config = construct_config_for_semantic_place() super().__init__(spot, config) - if use_semantic_place: - self.policy = SemanticPlacePolicy( - config.WEIGHTS.SEMANTIC_PLACE, device=config.DEVICE, config=config - ) + + self.policy = SemanticPlacePolicy( + config.WEIGHTS.SEMANTIC_PLACE, device=config.DEVICE, config=config + ) self.policy.reset() - self.env = SpotSemanticPlaceEnv(config, spot, use_semantic_place) + self.env = SpotSemanticPlaceEnv(config, spot) def execute_rl_loop(self, goal_dict: Dict[str, Any]) -> Tuple[bool, str]: # Set the robot inital pose @@ -1037,7 +1037,7 @@ class SemanticPlaceEE(SemanticPlace): Semantic place ee controller is used to execute place for given place targets """ - def __init__(self, spot: Spot, config): + def __init__(self, spot: Spot, config, use_semantic_place=False): if not config: config = construct_config_for_semantic_place() super().__init__(spot, config) @@ -1047,7 +1047,7 @@ def __init__(self, spot: Spot, config): ) self.policy.reset() - self.env = SpotSemanticPlaceEEEnv(config, spot) + self.env = SpotSemanticPlaceEEEnv(config, spot, use_semantic_place) def split_action(self, action: np.ndarray) -> Dict[str, Any]: """Refer to class Skill for documentation""" From 3573f69da7823eaa3bdefee392da9dd52f9ad1de Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 4 Sep 2024 10:29:50 -0700 Subject: [PATCH 50/82] log filre renamed --- spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py index d6c22b12..98291298 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -14,7 +14,7 @@ enable_estimation_before_place = map_user_input_to_boolean( "Use waypoint estimator? Y/N " ) - place_target = "new_pl_pt" + place_target = "fre_lab_40in_sep" spotskillmanager = SpotSkillManager( use_mobile_pick=False, use_semantic_place=True, use_place_ee=True ) From 94def9f2d61851ebdf92cbbc417ef9e052db3f49 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 4 Sep 2024 10:47:55 -0700 Subject: [PATCH 51/82] Removed Base movement for sem place ee --- bd_spot_wrapper/spot_wrapper/spot.py | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index d322c897..dc41b556 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -1094,22 +1094,22 @@ def set_base_vel_and_arm_ee_pos( ): print(f"in set_base_vel_and_arm_ee_pos: {arm_ee_action} {travel_time}") # TODO: semantic place ee: temp hack to distance the base vel - base_cmd = self.set_base_velocity( - x_vel, - y_vel, - ang_vel, - vel_time=travel_time, - disable_obstacle_avoidance=disable_obstacle_avoidance, - return_cmd=True, - ) + # base_cmd = self.set_base_velocity( + # x_vel, + # y_vel, + # ang_vel, + # vel_time=travel_time, + # disable_obstacle_avoidance=disable_obstacle_avoidance, + # return_cmd=True, + # ) arm_cmd = self.move_gripper_to_point( point=arm_ee_action[0:3], rotation=list(arm_ee_action[3:]), seconds_to_goal=travel_time, return_cmd=True, ) - synchro_command = RobotCommandBuilder.build_synchro_command(base_cmd, arm_cmd) - # synchro_command = RobotCommandBuilder.build_synchro_command(arm_cmd) + # synchro_command = RobotCommandBuilder.build_synchro_command(base_cmd, arm_cmd) + synchro_command = RobotCommandBuilder.build_synchro_command(arm_cmd) cmd_id = self.command_client.robot_command( synchro_command, end_time_secs=time.time() + travel_time ) From 9ca3f30f65ea44a5c9e95dc1d1f6d40de63a8302 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 4 Sep 2024 11:01:56 -0700 Subject: [PATCH 52/82] pre commit checks --- .../experiments/skill_test/parse_logs_csv.py | 12 ++++-- .../skill_test/test_static_place.py | 39 +++++++++---------- 2 files changed, 27 insertions(+), 24 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/parse_logs_csv.py b/spot_rl_experiments/experiments/skill_test/parse_logs_csv.py index 244bac21..43addbc3 100644 --- a/spot_rl_experiments/experiments/skill_test/parse_logs_csv.py +++ b/spot_rl_experiments/experiments/skill_test/parse_logs_csv.py @@ -86,9 +86,13 @@ def process_json_file(input_file, output_file): def main(): - parser = argparse.ArgumentParser(description="Convert JSON files in a folder to CSV") + parser = argparse.ArgumentParser( + description="Convert JSON files in a folder to CSV" + ) parser.add_argument("input_folder", help="Path to the folder containing JSON files") - parser.add_argument("output_folder", help="Path to the folder where CSV files will be saved") + parser.add_argument( + "output_folder", help="Path to the folder where CSV files will be saved" + ) args = parser.parse_args() if not os.path.exists(args.input_folder): @@ -101,7 +105,9 @@ def main(): for filename in os.listdir(args.input_folder): if filename.endswith(".json"): input_file = os.path.join(args.input_folder, filename) - output_file = os.path.join(args.output_folder, f"{os.path.splitext(filename)[0]}.csv") + output_file = os.path.join( + args.output_folder, f"{os.path.splitext(filename)[0]}.csv" + ) process_json_file(input_file, output_file) print(f"Processed {filename} into {output_file}") diff --git a/spot_rl_experiments/experiments/skill_test/test_static_place.py b/spot_rl_experiments/experiments/skill_test/test_static_place.py index 3e015cbc..148a5645 100644 --- a/spot_rl_experiments/experiments/skill_test/test_static_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_static_place.py @@ -1,16 +1,18 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. -import rospy -from perception_and_utils.utils.generic_utils import map_user_input_to_boolean -from spot_rl.envs.skill_manager import SpotSkillManager -from datetime import datetime +# mypy: ignore-errors import json import time +from datetime import datetime + import numpy as np +import rospy +from perception_and_utils.utils.generic_utils import map_user_input_to_boolean +from spot_rl.envs.skill_manager import SpotSkillManager if __name__ == "__main__": - + # Know which location we are doing experiments enable_estimation_before_place = map_user_input_to_boolean( "Use waypoint estimator? Y/N " @@ -20,9 +22,8 @@ spotskillmanager = SpotSkillManager( use_mobile_pick=False, use_semantic_place=False, use_place_ee=False ) - spot_pos1= spotskillmanager.spot.get_arm_joint_positions(as_array=True) - print("CURR POSE",spot_pos1) - + spot_pos1 = spotskillmanager.spot.get_arm_joint_positions(as_array=True) + print("CURR POSE", spot_pos1) is_local = False if enable_estimation_before_place: @@ -31,14 +32,14 @@ # Start testing contnue = True - INITIAL_ARM_JOINT_ANGLES=[0, -180, 180, 90, 0, -90] + INITIAL_ARM_JOINT_ANGLES = [0, -180, 180, 90, 0, -90] episode_ctr = 1 - spot_pos,spot_ort=spotskillmanager.spot.get_ee_pos_in_body_frame() - spot_ort=np.zeros(3) + spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() + spot_ort = np.zeros(3) while contnue: - + spotskillmanager.spot.open_gripper() - input('Place an object in Spot\'s gripper and press Enter to continue...') + input("Place an object in Spot's gripper and press Enter to continue...") rospy.set_param("is_gripper_blocked", 0) episode_log = {"actions": []} spotskillmanager.spot.close_gripper() @@ -48,16 +49,12 @@ skill_log = spotskillmanager.place_controller.skill_result_log if "num_steps" not in skill_log: skill_log["num_steps"] = 0 - episode_log["actions"].append({f"place": skill_log}) - curr_date = datetime.today().strftime('%m-%d-%y') + episode_log["actions"].append({"place": skill_log}) + curr_date = datetime.today().strftime("%m-%d-%y") file_path = f"logs/static_place/{curr_date}/episode_run2_{episode_ctr}.json" with open(file_path, "w") as file: json.dump(episode_log, file, indent=4) print(f"Saved log: {file_path}") - episode_ctr +=1 + episode_ctr += 1 contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") - - spot_pos=spotskillmanager.spot.get_ee_pos_in_body_frame()[0] - spotskillmanager.spot.move_gripper_to_point(spot_pos,spot_ort) - - # spotskillmanager.spot.set_arm_joint_positions((np.deg2rad(INITIAL_ARM_JOINT_ANGLES))) + spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) From d62bb06d3ed6d955c3021ad6a0aa685687763d66 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 4 Sep 2024 11:17:21 -0700 Subject: [PATCH 53/82] pre commit checks --- bd_spot_wrapper/spot_wrapper/spot.py | 22 ++++++++++++++++++++ spot_rl_experiments/spot_rl/envs/base_env.py | 21 +++++++++++-------- 2 files changed, 34 insertions(+), 9 deletions(-) diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index dc41b556..bf40d4d2 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -1115,6 +1115,28 @@ def set_base_vel_and_arm_ee_pos( ) return cmd_id + def set_arm_ee_pos( + self, + x_vel, + y_vel, + ang_vel, + arm_ee_action, + travel_time, + disable_obstacle_avoidance=False, + ): + print(f"in set_arm_ee_pos: {arm_ee_action} {travel_time}") + arm_cmd = self.move_gripper_to_point( + point=arm_ee_action[0:3], + rotation=list(arm_ee_action[3:]), + seconds_to_goal=travel_time, + return_cmd=True, + ) + synchro_command = RobotCommandBuilder.build_synchro_command(arm_cmd) + cmd_id = self.command_client.robot_command( + synchro_command, end_time_secs=time.time() + travel_time + ) + return cmd_id + def get_xy_yaw(self, use_boot_origin=False, robot_state=None): """ Returns the relative x and y distance from start, as well as relative heading diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 266d06bb..5560b1d1 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -456,12 +456,15 @@ def step( # noqa elif base_action is not None and arm_ee_action is not None: print("input base_action velocity:", arr2str(base_action)) print("input arm_ee_action:", arr2str(arm_ee_action)) - self.spot.set_base_vel_and_arm_ee_pos( - *base_action, - arm_ee_action, - travel_time=self.config.ARM_TRAJECTORY_TIME_IN_SECONDS, - disable_obstacle_avoidance=disable_oa, - ) + if self._max_lin_dist_scale == 0.0: + self.spot.set_arm_ee_pos() + else: + self.spot.set_base_vel_and_arm_ee_pos( + *base_action, + arm_ee_action, + travel_time=self.config.ARM_TRAJECTORY_TIME_IN_SECONDS, + disable_obstacle_avoidance=disable_oa, + ) elif base_action is not None: self.spot.set_base_velocity( *base_action, @@ -483,9 +486,9 @@ def step( # noqa # TODO: semantic place ee: temp hack to command this out # for not using base velocity start_time = time.time() - # if arm_ee_action is not None: - # print("do not move the base") - if base_action is not None or arm_action is not None: + if arm_ee_action is not None: + print("do not move the base") + elif base_action is not None or arm_action is not None: while time.time() < start_time + 1 / self.ctrl_hz: if target_yaw is not None and abs( wrap_heading(self.yaw - target_yaw) From bffc0b8073d7d2610d11360c8ba7bd96540c281e Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 4 Sep 2024 11:19:14 -0700 Subject: [PATCH 54/82] separate function for arm_ee w/o base vel --- bd_spot_wrapper/spot_wrapper/spot.py | 19 +++++++++---------- 1 file changed, 9 insertions(+), 10 deletions(-) diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index bf40d4d2..7d6c30c9 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -1094,22 +1094,21 @@ def set_base_vel_and_arm_ee_pos( ): print(f"in set_base_vel_and_arm_ee_pos: {arm_ee_action} {travel_time}") # TODO: semantic place ee: temp hack to distance the base vel - # base_cmd = self.set_base_velocity( - # x_vel, - # y_vel, - # ang_vel, - # vel_time=travel_time, - # disable_obstacle_avoidance=disable_obstacle_avoidance, - # return_cmd=True, - # ) + base_cmd = self.set_base_velocity( + x_vel, + y_vel, + ang_vel, + vel_time=travel_time, + disable_obstacle_avoidance=disable_obstacle_avoidance, + return_cmd=True, + ) arm_cmd = self.move_gripper_to_point( point=arm_ee_action[0:3], rotation=list(arm_ee_action[3:]), seconds_to_goal=travel_time, return_cmd=True, ) - # synchro_command = RobotCommandBuilder.build_synchro_command(base_cmd, arm_cmd) - synchro_command = RobotCommandBuilder.build_synchro_command(arm_cmd) + synchro_command = RobotCommandBuilder.build_synchro_command(base_cmd, arm_cmd) cmd_id = self.command_client.robot_command( synchro_command, end_time_secs=time.time() + travel_time ) From ada79947d96e02c62a3dd7d0aebab09ee4bb7bf5 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 4 Sep 2024 11:21:40 -0700 Subject: [PATCH 55/82] removed unused variables --- bd_spot_wrapper/spot_wrapper/spot.py | 4 ---- 1 file changed, 4 deletions(-) diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index 7d6c30c9..1bef6948 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -1116,12 +1116,8 @@ def set_base_vel_and_arm_ee_pos( def set_arm_ee_pos( self, - x_vel, - y_vel, - ang_vel, arm_ee_action, travel_time, - disable_obstacle_avoidance=False, ): print(f"in set_arm_ee_pos: {arm_ee_action} {travel_time}") arm_cmd = self.move_gripper_to_point( From e74488a71ad31de59353c4d4cbd796c7e314bea6 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 4 Sep 2024 11:25:37 -0700 Subject: [PATCH 56/82] pre commit failure cleanup --- spot_rl_experiments/spot_rl/real_policy.py | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/spot_rl_experiments/spot_rl/real_policy.py b/spot_rl_experiments/spot_rl/real_policy.py index 370829dd..8a4c9ed8 100644 --- a/spot_rl_experiments/spot_rl/real_policy.py +++ b/spot_rl_experiments/spot_rl/real_policy.py @@ -231,8 +231,10 @@ def __init__(self, checkpoint_path, device, config: CN = CN()): checkpoint_path, observation_space, action_space, device, config=config ) + class MobileGazeEEPolicy(RealPolicy): def __init__(self, checkpoint_path, device, config: CN = CN()): + observation_space = SpaceDict( { "arm_depth_bbox_sensor": spaces.Box( @@ -252,13 +254,12 @@ def __init__(self, checkpoint_path, device, config: CN = CN()): ), } ) - action_space = spaces.Box( - -1.0, 1.0, (9,) - ) + action_space = spaces.Box(-1.0, 1.0, (9,)) super().__init__( checkpoint_path, observation_space, action_space, device, config=config ) + class SemanticGazePolicy(RealPolicy): def __init__(self, checkpoint_path, device, config: CN = CN()): observation_space = SpaceDict( From 5d5513a9a305d4e0c3eb43743fa0260816f29683 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 4 Sep 2024 12:33:57 -0700 Subject: [PATCH 57/82] removed redeundant print statements --- spot_rl_experiments/spot_rl/envs/place_env.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index 0196c65b..c66c2ed9 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -241,9 +241,6 @@ def __init__(self, config, spot: Spot, use_semantic_place: bool = False): max_ang_dist_sem_place = ( "MAX_ANG_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_ANG_DIST" ) - - print("THE VAR", use_semantic_place) - super().__init__( config, spot, From 511ed3eb519a6c649da7a2cc07c1278cbadd05ba Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Thu, 5 Sep 2024 16:35:13 -0700 Subject: [PATCH 58/82] Cleaned up SemanticPlaceEE Class and Removed debug comments --- spot_rl_experiments/configs/config.yaml | 3 +- .../experiments/skill_test/test_sem_place.py | 62 +------- spot_rl_experiments/spot_rl/envs/base_env.py | 22 +-- .../spot_rl/envs/open_close_drawer_env.py | 2 +- spot_rl_experiments/spot_rl/envs/place_env.py | 135 ++---------------- .../spot_rl/envs/skill_manager.py | 2 - 6 files changed, 18 insertions(+), 208 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 54f1e371..50966cb8 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -117,8 +117,7 @@ SEMANTIC_PLACE_EE_ACTION_SPACE_LENGTH: 10 MAX_LIN_DIST_OPEN_CLOSE_DRAWER: 0.0 MAX_ANG_DIST_OPEN_CLOSE_DRAWER: 3.73 # degrees OPEN_CLOSE_DRAWER_ACTION_SPACE_LENGTH: 8 -OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.75 - +OPEN_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.3 # Semantic Pick IMG_SRC: 0 # 1 for intel & 0 for gripper diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index 6772cfa5..cf4ba01e 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -2,10 +2,7 @@ # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. # mypy: ignore-errors -<<<<<<< HEAD # black: ignore-errors -======= ->>>>>>> abd65c7 ( Minor bug fixes in place_env) import json import time from datetime import datetime @@ -24,21 +21,14 @@ if in_fre_lab: # at FRE -<<<<<<< HEAD place_target = "160_teddy bear" -======= - place_target = "new_place_waypoint" ->>>>>>> abd65c7 ( Minor bug fixes in place_env) else: # at NYC place_target = "test_desk" spotskillmanager = SpotSkillManager(use_mobile_pick=False, use_semantic_place=True) -<<<<<<< HEAD -======= spot_pos1 = spotskillmanager.spot.get_arm_joint_positions(as_array=True) -<<<<<<< HEAD # is_local = False # if enable_estimation_before_place: # place_target = None @@ -50,50 +40,20 @@ # rospy.set_param("is_gripper_blocked", 0) # spotskillmanager.place(place_target, is_local=is_local, visualize=True) # contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") ->>>>>>> 41ba97e (Minor chagnges for new policy weights) -======= ->>>>>>> abd65c7 ( Minor bug fixes in place_env) is_local = False # Start testing contnue = True -<<<<<<< HEAD - episode_ctr = 0 - # Get EE Pose Initial in rest position - spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() - # Set Orientation as Zero - spot_ort = np.zeros(3) - while contnue: - # Open Gripper - spotskillmanager.spot.open_gripper() - input("Place an object in Spot's gripper and press Enter to continue...") - # Place Object and Close Gripper - rospy.set_param("is_gripper_blocked", 0) - episode_log = {"actions": []} # mypy: ignore-errors - spotskillmanager.spot.close_gripper() - input("waiting for user to get ready with camera") - spotskillmanager.place( - place_target, - is_local=enable_estimation_before_place, - visualize=False, - enable_waypoint_estimation=enable_estimation_before_place, - ) - skill_log = spotskillmanager.place_controller.skill_result_log - if "num_steps" not in skill_log: - - skill_log["num_steps"] = 0 - episode_log["actions"].append({"place": skill_log}) -======= INITIAL_ARM_JOINT_ANGLES = [0, -180, 180, 90, 0, -90] episode_ctr = 0 - # Get EE Pose Initial + #Get EE Pose Initial spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() - # Set Orientation as Zero + #Set Orientation as Zero spot_ort = np.zeros(3) while contnue: - # Open Gripper + #Open Gripper spotskillmanager.spot.open_gripper() input("Place an object in Spot's gripper and press Enter to continue...") - # Place Object and Close Gripper + #Place Object and Close Gripper rospy.set_param("is_gripper_blocked", 0) episode_log = {"actions": []} spotskillmanager.spot.close_gripper() @@ -103,12 +63,7 @@ skill_log = spotskillmanager.place_controller.skill_result_log if "num_steps" not in skill_log: skill_log["num_steps"] = 0 -<<<<<<< HEAD episode_log["actions"].append({f"place": skill_log}) ->>>>>>> 41ba97e (Minor chagnges for new policy weights) -======= - episode_log["actions"].append({"place": skill_log}) ->>>>>>> abd65c7 ( Minor bug fixes in place_env) curr_date = datetime.today().strftime("%m-%d-%y") file_path = ( f"logs/semantic_place/{curr_date}/episode_sem_pl_run2_{episode_ctr}.json" @@ -118,15 +73,7 @@ print(f"Saved log: {file_path}") episode_ctr += 1 contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") -<<<<<<< HEAD -<<<<<<< HEAD - # Return the arm to the original position - spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) -======= #Return the arm to the original position -======= - # Return the arm to the original position ->>>>>>> abd65c7 ( Minor bug fixes in place_env) spot_pos = spotskillmanager.spot.get_ee_pos_in_body_frame()[0] spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) # The following is a helpful tip to debug the arm @@ -145,4 +92,3 @@ # ee_orientation_at_grasping = spotskillmanager.gaze_controller.env.ee_orientation_at_grasping # spotskillmanager.nav("test_desk") # spotskillmanager.place("test_desk", orientation_at_grasping) # This controls the arm initial orientation ->>>>>>> 41ba97e (Minor chagnges for new policy weights) diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 5560b1d1..efd39dca 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -102,22 +102,6 @@ def rescale_actions(actions, action_thresh=0.05, silence_only=False): return actions -def rescale_arm_ee_actions(actions, action_thresh=0.05, silence_only=False): - actions = np.clip(actions, -1, 1) - # Silence low actions - actions[np.abs(actions) < action_thresh] = 0.0 - if silence_only: - return actions - - # Remap action scaling to compensate for silenced values - action_offsets = np.ones_like(actions) * action_thresh - action_offsets[actions < 0] = -action_offsets[actions < 0] - action_offsets[actions == 0] = 0 - actions = (actions - np.array(action_offsets)) / (1.0 - action_thresh) - - return actions - - class SpotBaseEnv(SpotRobotSubscriberMixin, gym.Env): node_name = "spot_reality_gym" no_raw = True @@ -424,7 +408,7 @@ def step( # noqa arm_action = None if arm_ee_action is not None: - arm_ee_action = rescale_arm_ee_actions(arm_ee_action) + arm_ee_action = rescale_actions(arm_ee_action) if np.count_nonzero(arm_ee_action) > 0: # TODO: semantic place ee: move this to config arm_ee_action[0:3] *= self.arm_ee_dist_scale # 0.015 @@ -445,8 +429,6 @@ def step( # noqa ) # / self.ctrl_hz print("Slow down...") if base_action is not None and arm_action is not None: - print("input base_action velocity:", arr2str(base_action)) - print("input arm_action:", arr2str(arm_action)) self.spot.set_base_vel_and_arm_pos( *base_action, arm_action, @@ -454,8 +436,6 @@ def step( # noqa disable_obstacle_avoidance=disable_oa, ) elif base_action is not None and arm_ee_action is not None: - print("input base_action velocity:", arr2str(base_action)) - print("input arm_ee_action:", arr2str(arm_ee_action)) if self._max_lin_dist_scale == 0.0: self.spot.set_arm_ee_pos() else: diff --git a/spot_rl_experiments/spot_rl/envs/open_close_drawer_env.py b/spot_rl_experiments/spot_rl/envs/open_close_drawer_env.py index 41197229..da658a1c 100644 --- a/spot_rl_experiments/spot_rl/envs/open_close_drawer_env.py +++ b/spot_rl_experiments/spot_rl/envs/open_close_drawer_env.py @@ -67,7 +67,7 @@ def __init__(self, config, spot: Spot): # Distance threshold to call IK to approach the drawers self._dis_threshold_ee_to_handle = ( - config.OPEM_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE + config.OPEN_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE ) # Flag for using Boston Dynamics API to open the cabinet or not diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index c66c2ed9..8aa21732 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -68,17 +68,23 @@ def get_observations(self): class SpotSemanticPlaceEnv(SpotBaseEnv): """This is Spot semantic place class""" - def __init__(self, config, spot: Spot): + def __init__(self, config, spot: Spot, use_semantic_place: bool = False): # We set the initial arm joints config.INITIAL_ARM_JOINT_ANGLES = copy.deepcopy( config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE ) - + max_lin_dist_sem_place = ( + "MAX_LIN_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_LIN_DIST" + ) + max_ang_dist_sem_place = ( + "MAX_ANG_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_ANG_DIST" + ) super().__init__( config, spot, + max_lin_dist_key=max_lin_dist_sem_place, + max_ang_dist_key=max_ang_dist_sem_place, ) - # Define the place variables self.place_target = None self.place_target_is_local = False @@ -227,7 +233,7 @@ def get_observations(self): return observations -class SpotSemanticPlaceEEEnv(SpotBaseEnv): +class SpotSemanticPlaceEEEnv(SpotSemanticPlaceEnv): """This is Spot semantic place class""" def __init__(self, config, spot: Spot, use_semantic_place: bool = False): @@ -235,134 +241,15 @@ def __init__(self, config, spot: Spot, use_semantic_place: bool = False): config.INITIAL_ARM_JOINT_ANGLES = copy.deepcopy( config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE ) - max_lin_dist_sem_place = ( - "MAX_LIN_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_LIN_DIST" - ) - max_ang_dist_sem_place = ( - "MAX_ANG_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_ANG_DIST" - ) - super().__init__( - config, - spot, - max_lin_dist_key=max_lin_dist_sem_place, - max_ang_dist_key=max_ang_dist_sem_place, - ) - - # Define the place variables - self.place_target = None - self.place_target_is_local = False - self.placed = False - - # Set gripper variables - self.ee_gripper_offset = mn.Vector3(config.EE_GRIPPER_OFFSET) - - # Define the observation variables - self.initial_ee_pose = None - self.target_object_pose = None + super().__init__(config, spot) # Define End Effector Policy Scale Values self.arm_ee_dist_scale = self.config.EE_DIST_SCALE_SEMANTIC_PLACE self.arm_ee_rot_scale = self.config.EE_ROT_SCALE_SEMANTIC_PLACE - # Overwrite joint limits for semantic_place skills - self.arm_lower_limits = np.deg2rad(config.ARM_LOWER_LIMITS_SEMANTIC_PLACE) - self.arm_upper_limits = np.deg2rad(config.ARM_UPPER_LIMITS_SEMANTIC_PLACE) - # Place steps self._time_step = 0 - def decide_init_arm_joint(self, ee_orientation_at_grasping): - """Decide the place location""" - - # User does not set the gripper orientation - if ee_orientation_at_grasping is None: - self.initial_arm_joint_angles = np.deg2rad( - self.config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE - ) - else: - # Get the pitch and yaw - pitch = ee_orientation_at_grasping[1] - yaw = ee_orientation_at_grasping[2] - print("ee_orientation_at_grasping:", ee_orientation_at_grasping) - if abs(pitch) <= 1.309: # 75 degree in pitch - if yaw > 0: # gripper is in object's right hand side - self.initial_arm_joint_angles = np.deg2rad( - self.config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE - ) - else: # gripper is in object's left hand side - self.initial_arm_joint_angles = np.deg2rad( - self.config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_LEFT_HAND - ) - else: - self.initial_arm_joint_angles = np.deg2rad( - self.config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_TOP_DOWN - ) - - def reset(self, place_target, target_is_local=False, *args, **kwargs): - assert place_target is not None - self.place_target = np.array(place_target) - self.place_target_is_local = target_is_local - - self._time_step = 0 - - # Decide the reset arm angle and then reset the arm - self.decide_init_arm_joint(kwargs["ee_orientation_at_grasping"]) - self.reset_arm() - - # We wait for a second to let the arm in the placing - # ready location - time.sleep(1.0) - # Sometimes, there will be a bit of mistchmatch of joints after resetting - # So we can reset the arm again here using the following - # ee_position, ee_orientation = self.spot.get_ee_pos_in_body_frame() - # self.spot.move_gripper_to_point(ee_position, [np.pi / 2, 0, 0]) - - # Set the initial ee pose - self.initial_ee_pose = self.spot.get_ee_quaternion_in_body_frame() - # Set the target pose - self.target_object_pose = self.spot.get_ee_quaternion_in_body_frame() - # Automatically use intelrealsense camera - rospy.set_param("is_gripper_blocked", 1) - observations = super().reset() - rospy.set_param("is_whiten_black", False) - self.placed = False - return observations - - def step(self, action_dict: Dict[str, Any], *args, **kwargs): - - place = False - - # Place command is issued if the place action is smaller than zero - # TODO: semantic place ee: check how grip_action behaves! - place = action_dict.get("grip_action", None) <= 0.0 - - # If the time steps have been passed for 50 steps and gripper is in the desired place location - cur_place_sensor_xyz = self.get_place_sensor(True) - if ( - abs(cur_place_sensor_xyz[2]) < 0.05 - and np.linalg.norm( - np.array([cur_place_sensor_xyz[0], cur_place_sensor_xyz[1]]) - ) - < 0.25 - and self._time_step >= 50 - ): - place = True - - # If the time steps have been passed for 75 steps, we will just place the object - if self._time_step >= 75: - place = True - - self._time_step += 1 - - # Write into action dict - action_dict["place"] = place - action_dict["semantic_place"] = place - - # Set the travel time scale so that the arm movement is smooth - return super().step( - action_dict, travel_time_scale=1.0 / 0.9 * 1.75, *args, **kwargs - ) - def get_success(self, observations): return self.place_attempted diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 3382a6d2..6ad34b83 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -118,7 +118,6 @@ def __init__( # Process the meta parameters self._use_mobile_pick = use_mobile_pick - print("USE MOBILE PICK VARIABLE IS SET TO :", self._use_mobile_pick) self.use_semantic_place = use_semantic_place self.use_pick_ee = use_pick_ee self.use_place_ee = use_place_ee @@ -213,7 +212,6 @@ def __initiate_controllers(self, use_policies: bool = True): config=self.nav_config, ) if self.use_pick_ee: - # print("GOING INSIDE GAZE EE ENV") self.gaze_controller = MobilePickEE( spot=self.spot, config=self.pick_config, From 0ceb08db6c413358f7618561616a13b2d236857f Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Thu, 5 Sep 2024 16:37:17 -0700 Subject: [PATCH 59/82] removed debug comments --- spot_rl_experiments/spot_rl/skills/atomic_skills.py | 3 --- 1 file changed, 3 deletions(-) diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index 1f765786..9e5dcb58 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -724,9 +724,6 @@ def __init__(self, spot: Spot, config, use_mobile_pick=True): def split_action(self, action: np.ndarray) -> Dict[str, Any]: """Refer to class Skill for documentation""" - # action size is 9 - # TODO: semantic place ee: check the order - # TODO: semantic place ee: check roll pitch yaw print(f"action: {action}") action_dict = { "arm_ee_action": action[:6], From a8336b00d8c55f2857228611cec5936b81596863 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Fri, 6 Sep 2024 11:22:28 -0700 Subject: [PATCH 60/82] Removed comments and shifted Threshold Variable in config --- spot_rl_experiments/configs/config.yaml | 4 + .../experiments/skill_test/parse_logs.py | 93 ------------------- spot_rl_experiments/spot_rl/envs/base_env.py | 5 +- spot_rl_experiments/spot_rl/envs/gaze_env.py | 4 +- spot_rl_experiments/spot_rl/envs/place_env.py | 38 ++------ .../spot_rl/envs/skill_manager.py | 1 - .../spot_rl/skills/atomic_skills.py | 5 - 7 files changed, 16 insertions(+), 134 deletions(-) delete mode 100644 spot_rl_experiments/experiments/skill_test/parse_logs.py diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 50966cb8..4ef1c8f2 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -119,6 +119,10 @@ MAX_ANG_DIST_OPEN_CLOSE_DRAWER: 3.73 # degrees OPEN_CLOSE_DRAWER_ACTION_SPACE_LENGTH: 8 OPEN_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.3 +#GRASP DISTANCE THRESHOLD +GRASP_DISTANCE_THRESHOLD: 0.7 +GRASP_DISTANCE_THRESHOLD_EE: 1.5 + # Semantic Pick IMG_SRC: 0 # 1 for intel & 0 for gripper SEG_PORT: 21001 # port for segmentation service diff --git a/spot_rl_experiments/experiments/skill_test/parse_logs.py b/spot_rl_experiments/experiments/skill_test/parse_logs.py deleted file mode 100644 index bee5a11b..00000000 --- a/spot_rl_experiments/experiments/skill_test/parse_logs.py +++ /dev/null @@ -1,93 +0,0 @@ -import argparse -import csv -import json - - -def parse_json_file(file_path): - try: - with open(file_path, "r") as file: - data = json.load(file) - return data - except FileNotFoundError: - print(f"Error: File '{file_path}' not found.") - return None - except json.JSONDecodeError: - print(f"Error: '{file_path}' is not a valid JSON file.") - return None - - -def format_value(key, value): - if isinstance(value, (int, float)): - value = round(value, 3) - return str(value) - - -def export_to_csv(results_dict, output_file): - with open(output_file, mode="w", newline="") as file: - writer = csv.writer(file) - # Write header (keys of the dictionary) - writer.writerow(results_dict.keys()) - # Write data (values of the dictionary) - writer.writerow(results_dict.values()) - - -def main(): - parser = argparse.ArgumentParser(description="Parse a JSON file") - parser.add_argument("file", help="Path to the JSON file to parse") - parser.add_argument("output_csv", help="Path to the output CSV file") - args = parser.parse_args() - - parsed_data = parse_json_file(args.file) - if parsed_data is None: - return - - results_dict = { - "overall_success": " ", - "total_time": parsed_data["total_time"], - "total_num_steps": parsed_data["total_steps"], - "0_nav_success": " ", - "0_nav_time_taken": " ", - "0_nav_num_steps": " ", - "0_nav_distance_travelled": " ", - "0_nav_distance_to_goal_linear": " ", - "0_nav_distance_to_goal_angular": " ", - "1_pick_success": " ", - "1_pick_time_taken": " ", - "1_pick_num_steps": " ", - "2_nav_success": " ", - "2_nav_time_taken": " ", - "2_nav_num_steps": " ", - "2_nav_distance_travelled": " ", - "2_nav_distance_to_goal_linear": " ", - "2_nav_distance_to_goal_angular": " ", - "3_place_success": " ", - "3_place_time_taken": " ", - "3_place_num_steps": " ", - } - - success_ctr = 0 - for idx, action in enumerate(parsed_data["actions"]): - for action_type in action.keys(): - for k, v in action[action_type].items(): - if ( - action_type == "pick" or action_type == "place" - ) and k == "distance_travelled": - pass - elif k == "robot_trajectory": - pass - elif k == "distance_to_goal": - for bk, bv in action[action_type][k].items(): - match_str = f"{idx}_{action_type}_{k}_{bk}" - results_dict[match_str] = bv - else: - match_str = f"{idx}_{action_type}_{k}" - results_dict[match_str] = v - if "success" in k and v is True: - success_ctr += 1 - results_dict["overall_success"] = success_ctr / 4 - - export_to_csv(results_dict, args.output_csv) - - -if __name__ == "__main__": - main() diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index efd39dca..69be9775 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -462,11 +462,8 @@ def step( # noqa print(f"base_action: {arr2str(base_action)}\tarm_action: {arr2str(arm_action)}") - # Spin until enough time has passed during this step - # TODO: semantic place ee: temp hack to command this out - # for not using base velocity start_time = time.time() - if arm_ee_action is not None: + if arm_ee_action is not None and self._max_lin_dist_scale == 0.0: print("do not move the base") elif base_action is not None or arm_action is not None: while time.time() < start_time + 1 / self.ctrl_hz: diff --git a/spot_rl_experiments/spot_rl/envs/gaze_env.py b/spot_rl_experiments/spot_rl/envs/gaze_env.py index 09e0ddbe..fce25312 100644 --- a/spot_rl_experiments/spot_rl/envs/gaze_env.py +++ b/spot_rl_experiments/spot_rl/envs/gaze_env.py @@ -38,7 +38,7 @@ def __init__(self, config, spot: Spot, use_mobile_pick: bool = False): max_lin_dist_key=max_lin_dist_key, max_ang_dist_key=max_ang_dist_key, ) - self.grasp_dist_threshold = 1.5 + self.grasp_dist_threshold = self.config.GRASP_DISTANCE_THRESHOLD self.target_obj_name = None self._use_mobile_pick = use_mobile_pick self.initial_arm_joint_angles = np.deg2rad(config.GAZE_ARM_JOINT_ANGLES) @@ -116,7 +116,7 @@ def __init__(self, config, spot: Spot, use_mobile_pick: bool = False): ) self.arm_ee_dist_scale = self.config.EE_DIST_SCALE_MOBILE_GAZE self.arm_ee_rot_scale = self.config.EE_ROT_SCALE_MOBILE_GAZE - self.grasp_dist_threshold = 0.7 + self.grasp_dist_threshold = self.config.GRASP_DISTANCE_THRESHOLD_EE def get_observations(self): observations = super().get_observations() diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index 8aa21732..fe8f0d89 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -68,17 +68,13 @@ def get_observations(self): class SpotSemanticPlaceEnv(SpotBaseEnv): """This is Spot semantic place class""" - def __init__(self, config, spot: Spot, use_semantic_place: bool = False): + def __init__(self, config, spot: Spot): # We set the initial arm joints config.INITIAL_ARM_JOINT_ANGLES = copy.deepcopy( config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE ) - max_lin_dist_sem_place = ( - "MAX_LIN_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_LIN_DIST" - ) - max_ang_dist_sem_place = ( - "MAX_ANG_DIST_SEMANTIC_PLACE" if use_semantic_place else "MAX_ANG_DIST" - ) + max_lin_dist_sem_place = "MAX_LIN_DIST_SEMANTIC_PLACE" + max_ang_dist_sem_place = "MAX_ANG_DIST_SEMANTIC_PLACE" super().__init__( config, spot, @@ -237,29 +233,17 @@ class SpotSemanticPlaceEEEnv(SpotSemanticPlaceEnv): """This is Spot semantic place class""" def __init__(self, config, spot: Spot, use_semantic_place: bool = False): - # We set the initial arm joints - config.INITIAL_ARM_JOINT_ANGLES = copy.deepcopy( - config.INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE - ) + super().__init__(config, spot) # Define End Effector Policy Scale Values self.arm_ee_dist_scale = self.config.EE_DIST_SCALE_SEMANTIC_PLACE self.arm_ee_rot_scale = self.config.EE_ROT_SCALE_SEMANTIC_PLACE - # Place steps - self._time_step = 0 - - def get_success(self, observations): - return self.place_attempted - def get_observations(self): assert self.initial_ee_pose is not None assert self.target_object_pose is not None - # Get the gaol sensor - obj_goal_sensor = self.get_place_sensor(True) - # Get the delta ee orientation current_gripper_orientation = self.spot.get_ee_quaternion_in_body_frame() delta_ee = angle_between_quat(self.initial_ee_pose, current_gripper_orientation) @@ -278,13 +262,9 @@ def get_observations(self): # Get ee's pose -- x,y,z xyz, rpy = self.spot.get_ee_pos_in_body_frame() - observations = { - "obj_goal_sensor": obj_goal_sensor, - "relative_initial_ee_orientation": delta_ee, - "relative_target_object_orientation": delta_obj, - "articulated_agent_jaw_depth": arm_depth, - "ee_pose": np.concatenate([xyz, rpy]), - # "ee_pos":xyz, - "is_holding": np.ones((1,)), - } + observations = super().get_observations() + if "joint" in observations: + del observations["joint"] + xyz, rpy = self.spot.get_ee_pos_in_body_frame() + observations["ee_pose"] = np.concatenate([xyz, rpy]) return observations diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 6ad34b83..129e3b95 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -226,7 +226,6 @@ def __initiate_controllers(self, use_policies: bool = True): if self.use_semantic_place: if self.use_place_ee: - print("GOING INSIDE SEM EE ENV") self.place_controller = SemanticPlaceEE( spot=self.spot, config=self.place_config, diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index 9e5dcb58..ea6b2ea9 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -724,7 +724,6 @@ def __init__(self, spot: Spot, config, use_mobile_pick=True): def split_action(self, action: np.ndarray) -> Dict[str, Any]: """Refer to class Skill for documentation""" - print(f"action: {action}") action_dict = { "arm_ee_action": action[:6], "base_action": action[7:9], @@ -1048,10 +1047,6 @@ def __init__(self, spot: Spot, config, use_semantic_place=False): def split_action(self, action: np.ndarray) -> Dict[str, Any]: """Refer to class Skill for documentation""" - # action size is 10 - # TODO: semantic place ee: check the order - # TODO: semantic place ee: check roll pitch yaw - print(f"action: {action}") action_dict = { "arm_ee_action": action[:6], "base_action": action[7:9], From ce3a0f7a90a533afebb6606b461d9a6f30a589d0 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Fri, 6 Sep 2024 11:48:41 -0700 Subject: [PATCH 61/82] Switched parse_logs_csv into utils --- .../utils}/parse_logs_csv.py | 32 ++++++++++++------- 1 file changed, 21 insertions(+), 11 deletions(-) rename spot_rl_experiments/{experiments/skill_test => spot_rl/utils}/parse_logs_csv.py (78%) diff --git a/spot_rl_experiments/experiments/skill_test/parse_logs_csv.py b/spot_rl_experiments/spot_rl/utils/parse_logs_csv.py similarity index 78% rename from spot_rl_experiments/experiments/skill_test/parse_logs_csv.py rename to spot_rl_experiments/spot_rl/utils/parse_logs_csv.py index 43addbc3..76f2723f 100644 --- a/spot_rl_experiments/experiments/skill_test/parse_logs_csv.py +++ b/spot_rl_experiments/spot_rl/utils/parse_logs_csv.py @@ -95,21 +95,31 @@ def main(): ) args = parser.parse_args() - if not os.path.exists(args.input_folder): - print(f"Error: Input folder '{args.input_folder}' does not exist.") - return - - if not os.path.exists(args.output_folder): - os.makedirs(args.output_folder) - - for filename in os.listdir(args.input_folder): + if os.path.isfile(args.input_folder): + filename = args.input_folder + filename = os.path.basename(filename) if filename.endswith(".json"): - input_file = os.path.join(args.input_folder, filename) output_file = os.path.join( args.output_folder, f"{os.path.splitext(filename)[0]}.csv" ) - process_json_file(input_file, output_file) - print(f"Processed {filename} into {output_file}") + process_json_file(args.input_folder, output_file) + + elif os.path.isdir(args.input_folder): + if not os.path.exists(args.input_folder): + print(f"Error: Input folder '{args.input_folder}' does not exist.") + return + + if not os.path.exists(args.output_folder): + os.makedirs(args.output_folder) + + for filename in os.listdir(args.input_folder): + if filename.endswith(".json"): + input_file = os.path.join(args.input_folder, filename) + output_file = os.path.join( + args.output_folder, f"{os.path.splitext(filename)[0]}.csv" + ) + process_json_file(input_file, output_file) + print(f"Processed {filename} into {output_file}") if __name__ == "__main__": From e6906960154e4e9578f57bede68f4307bf79218c Mon Sep 17 00:00:00 2001 From: Achuthankrishna <74654704+Achuthankrishna@users.noreply.github.com> Date: Fri, 6 Sep 2024 11:59:57 -0700 Subject: [PATCH 62/82] Updated place_env --- spot_rl_experiments/spot_rl/envs/place_env.py | 21 ------------------- 1 file changed, 21 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index fe8f0d89..dd341132 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -241,27 +241,6 @@ def __init__(self, config, spot: Spot, use_semantic_place: bool = False): self.arm_ee_rot_scale = self.config.EE_ROT_SCALE_SEMANTIC_PLACE def get_observations(self): - assert self.initial_ee_pose is not None - assert self.target_object_pose is not None - - # Get the delta ee orientation - current_gripper_orientation = self.spot.get_ee_quaternion_in_body_frame() - delta_ee = angle_between_quat(self.initial_ee_pose, current_gripper_orientation) - delta_ee = np.array([delta_ee], dtype=np.float32) - - # Get the delta object orientation - delta_obj = angle_between_quat( - self.target_object_pose, current_gripper_orientation - ) - # remove the offset from the base to object - delta_obj = np.array( - [delta_obj - abs(self.spot.get_cur_ee_pose_offset())], dtype=np.float32 - ) - # Get the jaw image - arm_depth, _ = self.get_gripper_images() - - # Get ee's pose -- x,y,z - xyz, rpy = self.spot.get_ee_pos_in_body_frame() observations = super().get_observations() if "joint" in observations: del observations["joint"] From 1758855b88a99a9cebd27a4f46c0ca1fc31b6c4f Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Mon, 9 Sep 2024 10:27:41 -0700 Subject: [PATCH 63/82] Disabled Base movement by default and added varibale for set_arm_ee_pos --- spot_rl_experiments/configs/config.yaml | 2 +- .../experiments/skill_test/test_sem_place_ee.py | 6 ++++-- spot_rl_experiments/spot_rl/envs/base_env.py | 5 ++++- 3 files changed, 9 insertions(+), 4 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 4ef1c8f2..081ac017 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -140,7 +140,7 @@ MAX_LIN_DIST: 0.25 # meters MAX_ANG_DIST: 15.0 # degrees # Base action params for sem place EE -MAX_LIN_DIST_SEMANTIC_PLACE: 0.1 # meters +MAX_LIN_DIST_SEMANTIC_PLACE: 0.0 # meters MAX_ANG_DIST_SEMANTIC_PLACE: 5.73 # degrees # SEMANTIC EE POLICY SCALE PARAMETERS diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py index 98291298..79128174 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -14,7 +14,7 @@ enable_estimation_before_place = map_user_input_to_boolean( "Use waypoint estimator? Y/N " ) - place_target = "fre_lab_40in_sep" + place_target = "fre_lab_32in_sep" spotskillmanager = SpotSkillManager( use_mobile_pick=False, use_semantic_place=True, use_place_ee=True ) @@ -40,7 +40,9 @@ skill_log["num_steps"] = 0 episode_log["actions"].append({"place": skill_log}) curr_date = datetime.today().strftime("%m-%d-%y") - file_path = f"logs/semantic_place_ee/{curr_date}/0.5m_minus_20deg_episode_{episode_ctr}.json" + file_path = ( + f"logs/semantic_place_ee/{curr_date}/32in_place_episode_{episode_ctr}.json" + ) with open(file_path, "w") as file: json.dump(episode_log, file, indent=4) print(f"Saved log: {file_path}") diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 69be9775..fad96507 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -437,7 +437,10 @@ def step( # noqa ) elif base_action is not None and arm_ee_action is not None: if self._max_lin_dist_scale == 0.0: - self.spot.set_arm_ee_pos() + self.spot.set_arm_ee_pos( + arm_ee_action, + travel_time=self.config.ARM_TRAJECTORY_TIME_IN_SECONDS, + ) else: self.spot.set_base_vel_and_arm_ee_pos( *base_action, From e1d1067f6e29005364dfd0e1cc7be38c09814aa1 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Mon, 9 Sep 2024 10:35:10 -0700 Subject: [PATCH 64/82] Removed comments and restored debug method --- bd_spot_wrapper/spot_wrapper/spot.py | 5 +---- .../experiments/skill_test/test_sem_place.py | 15 +++++++++++++++ spot_rl_experiments/spot_rl/envs/base_env.py | 2 +- 3 files changed, 17 insertions(+), 5 deletions(-) diff --git a/bd_spot_wrapper/spot_wrapper/spot.py b/bd_spot_wrapper/spot_wrapper/spot.py index 1bef6948..7c05ce5a 100644 --- a/bd_spot_wrapper/spot_wrapper/spot.py +++ b/bd_spot_wrapper/spot_wrapper/spot.py @@ -1092,8 +1092,6 @@ def set_base_vel_and_arm_ee_pos( travel_time, disable_obstacle_avoidance=False, ): - print(f"in set_base_vel_and_arm_ee_pos: {arm_ee_action} {travel_time}") - # TODO: semantic place ee: temp hack to distance the base vel base_cmd = self.set_base_velocity( x_vel, y_vel, @@ -1119,7 +1117,6 @@ def set_arm_ee_pos( arm_ee_action, travel_time, ): - print(f"in set_arm_ee_pos: {arm_ee_action} {travel_time}") arm_cmd = self.move_gripper_to_point( point=arm_ee_action[0:3], rotation=list(arm_ee_action[3:]), @@ -1320,7 +1317,7 @@ def select_hand_image(self, is_rgb=True, img_src: List[str] = []): img_src if img_src else [SpotCamIds.HAND_COLOR, SpotCamIds.HAND_DEPTH_IN_HAND_COLOR_FRAME] - ) # default img_src to gripper + ) pixel_format_rgb = ( image_pb2.Image.PIXEL_FORMAT_RGB_U8 diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index cf4ba01e..daaff4fe 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -58,7 +58,22 @@ episode_log = {"actions": []} spotskillmanager.spot.close_gripper() input("waiting for user to get ready with camera") + # The following is a helpful tip to debug the arm + # We get Spot class + # spot = spotskillmanager.spot + # We can move the gripper to a point with x,y,z and roll, pitch, yaw + # spot.move_gripper_to_point((0.55, 0., 0.26), np.deg2rad(np.array([0,0,0]))) + # We can also set the robot arm joints + # config = construct_config() + # spot.set_arm_joint_positions(np.deg2rad(config.INITIAL_ARM_JOINT_ANGLES)) + # In addition, if you want to use semantic place skill based on the grasping orientation, you can do + # spotskillmanager.nav("black_case") + # spotskillmanager.pick("bottle") + # # Fetch the arm joint at grasping location + # ee_orientation_at_grasping = spotskillmanager.gaze_controller.env.ee_orientation_at_grasping + # spotskillmanager.nav("test_desk") + # spotskillmanager.place("test_desk", orientation_at_grasping) # This controls the arm initial orientation spotskillmanager.place(place_target, is_local=is_local, visualize=False) skill_log = spotskillmanager.place_controller.skill_result_log if "num_steps" not in skill_log: diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index fad96507..d0593223 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -467,7 +467,7 @@ def step( # noqa start_time = time.time() if arm_ee_action is not None and self._max_lin_dist_scale == 0.0: - print("do not move the base") + pass elif base_action is not None or arm_action is not None: while time.time() < start_time + 1 / self.ctrl_hz: if target_yaw is not None and abs( From c03cff1b4e397f6f2868306dd38279d1682babe2 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 10 Sep 2024 16:42:07 -0700 Subject: [PATCH 65/82] bug fixes --- spot_rl_experiments/experiments/skill_test/test_sem_place.py | 1 - spot_rl_experiments/spot_rl/envs/base_env.py | 1 - spot_rl_experiments/spot_rl/envs/place_env.py | 1 - spot_rl_experiments/spot_rl/envs/skill_manager.py | 1 - spot_rl_experiments/utils/pytorch_to_torchscript.py | 1 + 5 files changed, 1 insertion(+), 4 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index daaff4fe..df6168cf 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -43,7 +43,6 @@ is_local = False # Start testing contnue = True - INITIAL_ARM_JOINT_ANGLES = [0, -180, 180, 90, 0, -90] episode_ctr = 0 #Get EE Pose Initial spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index d0593223..0a34f2bf 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -410,7 +410,6 @@ def step( # noqa if arm_ee_action is not None: arm_ee_action = rescale_actions(arm_ee_action) if np.count_nonzero(arm_ee_action) > 0: - # TODO: semantic place ee: move this to config arm_ee_action[0:3] *= self.arm_ee_dist_scale # 0.015 arm_ee_action[3:6] *= self.arm_ee_rot_scale # 0.0125 xyz, rpy = self.spot.get_ee_pos_in_body_frame() diff --git a/spot_rl_experiments/spot_rl/envs/place_env.py b/spot_rl_experiments/spot_rl/envs/place_env.py index dd341132..787ac483 100644 --- a/spot_rl_experiments/spot_rl/envs/place_env.py +++ b/spot_rl_experiments/spot_rl/envs/place_env.py @@ -225,7 +225,6 @@ def get_observations(self): "joint": self.get_arm_joints(self.config.SEMANTIC_PLACE_JOINT_BLACKLIST), "is_holding": np.ones((1,)), } - print("---") return observations diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 129e3b95..1f40b123 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -181,7 +181,6 @@ def __init_spot( ) if place_config is None: - # TODO: overwrite the config self.place_config = ( construct_config_for_semantic_place() if self.use_semantic_place diff --git a/spot_rl_experiments/utils/pytorch_to_torchscript.py b/spot_rl_experiments/utils/pytorch_to_torchscript.py index 25058193..be20cf89 100644 --- a/spot_rl_experiments/utils/pytorch_to_torchscript.py +++ b/spot_rl_experiments/utils/pytorch_to_torchscript.py @@ -119,6 +119,7 @@ def __init__( ): print("Loading policy...") self.device = torch.device(device) + # Load the checkpoint checkpoint = torch.load(checkpoint_path, map_location="cpu") From f50180978d5015956eef2c49120c3a8ede9ccf61 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 11 Sep 2024 10:05:15 -0700 Subject: [PATCH 66/82] removed comments --- spot_rl_experiments/spot_rl/envs/base_env.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 0a34f2bf..8c13b126 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -410,8 +410,8 @@ def step( # noqa if arm_ee_action is not None: arm_ee_action = rescale_actions(arm_ee_action) if np.count_nonzero(arm_ee_action) > 0: - arm_ee_action[0:3] *= self.arm_ee_dist_scale # 0.015 - arm_ee_action[3:6] *= self.arm_ee_rot_scale # 0.0125 + arm_ee_action[0:3] *= self.arm_ee_dist_scale + arm_ee_action[3:6] *= self.arm_ee_rot_scale xyz, rpy = self.spot.get_ee_pos_in_body_frame() cur_ee_pose = np.concatenate((xyz, rpy), axis=0) # Wrap the heading From d6de5a64e0d6899f87fa401d5c74bb69226642b0 Mon Sep 17 00:00:00 2001 From: Achuthankrishna <74654704+Achuthankrishna@users.noreply.github.com> Date: Fri, 13 Sep 2024 10:53:49 -0700 Subject: [PATCH 67/82] updated to older value in config for semantic place joint angles. --- spot_rl_experiments/configs/config.yaml | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 081ac017..3d54c94c 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -161,10 +161,15 @@ INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE: [0, -180, 180, 0, 0, 90] # The initial INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_LEFT_HAND: [0, -180, 180, 0, 0, -90] # The initial orientation of the arm for side grasping (gripper is in object's left hand side) INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_TOP_DOWN: [-0.91, -92.87, 91.21, 0, 90.01, 0] # The initial orientation of the arm for top down grasping # The old gaze ready angle: [0, -170, 120, 0, 75, 0] +<<<<<<< HEAD GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 75, 0] GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES: [0, -160, 100, 0, 75, 0] #[0, -160, 100, 0, 90, 0] GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES: [0, -120, 60, 0, 88, 0] #[0, -160, 100, 0, 90, 0] HEIGHT_THRESHOLD : 0.8128 +======= +GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 75, 0] #[0, -160, 100, 0, 90, 0] +SEMANTIC_PLACE_ARM_JOINT_ANGLES: [0, -125, 80, 0, 85, 0] +>>>>>>> 9fc4546 (updated to older value in config for semantic place joint angles.) PLACE_ARM_JOINT_ANGLES: [0, -170, 120, 0, 75, 0] ARM_LOWER_LIMITS: [-45, -180, 0, 0, -90, 0] ARM_UPPER_LIMITS: [45, 0, 180, 0, 90, 0] From 8a6a381daa2e1ea8b4b07a235a25f9c160762d0a Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Fri, 13 Sep 2024 11:12:23 -0700 Subject: [PATCH 68/82] updated gitignore --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 44619c5a..6395a86e 100644 --- a/.gitignore +++ b/.gitignore @@ -25,5 +25,6 @@ data/* spot_rl_experiments/experiments/skill_test/logs/* spot_rl_experiments/experiments/skill_test/table_detections/* ros_tcp/ros_communication_client.egg-info/** +spot_rl_experiments/experiments/skill_test/episode_configs/* *.npy *.csv From 07a1d97ec98eacd9ccc9ef4fe50d897ee27f566b Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Fri, 13 Sep 2024 11:15:06 -0700 Subject: [PATCH 69/82] Updated episode configs --- .../skill_test/episode_configs/pick_32.yaml | 69 ------------------- 1 file changed, 69 deletions(-) delete mode 100644 spot_rl_experiments/experiments/skill_test/episode_configs/pick_32.yaml diff --git a/spot_rl_experiments/experiments/skill_test/episode_configs/pick_32.yaml b/spot_rl_experiments/experiments/skill_test/episode_configs/pick_32.yaml deleted file mode 100644 index 364149b2..00000000 --- a/spot_rl_experiments/experiments/skill_test/episode_configs/pick_32.yaml +++ /dev/null @@ -1,69 +0,0 @@ -# episode21: -# spotskillmanagerargs: -# use_mobile_pick: False -# use_semantic_place_ee: True -# actions: -# - nav: -# - "fremont_lab_white_desk" -# - pick: -# - "frosted flakes cup" -# # - nav: -# # - "fremont_lab_white_desk" -# - place: -# - null -# episode22: -# spotskillmanagerargs: -# use_mobile_pick: False -# use_semantic_place_ee: True -# actions: -# - nav: -# - "fremont_lab_white_desk" -# - pick: -# - "ball" -# # - nav: -# # - "fremont_lab_white_desk" -# - place: -# - null -episode33: - spotskillmanagerargs: - use_mobile_pick: False - use_semantic_place_ee: True - actions: - - nav: - - "fremont_lab_white_desk" - - pick: - - "cereal box" - # - nav: - # - "nav_desk_32_in" - - place: - - null -# episode24: -# spotskillmanagerargs: -# use_mobile_pick: False -# use_semantic_place_ee: True -# actions: -# # - nav:Semantic Place with waypoint estimator -# # - "lab_desk" - -# - nav: -# - "fremont_lab_white_desk" -# - pick: -# - "bottle" -# # - nav: -# # - "nav_desk_32_in" -# - place: -# - null - -# episode25: -# spotskillmanagerargs: -# use_mobile_pick: False -# use_semantic_place_ee: True -# actions: -# - nav: -# - "fremont_lab_white_desk" -# - pick: -# - "water can" -# # - nav: -# # - "nav_desk_32_in" -# - place: -# - null From b19522e5be8d84754a98e0eb2a48d961250c2832 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 17 Sep 2024 12:53:03 -0400 Subject: [PATCH 70/82] remove mypy --- spot_rl_experiments/experiments/skill_test/run_episode.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/run_episode.py b/spot_rl_experiments/experiments/skill_test/run_episode.py index 6bfce974..66b71c45 100644 --- a/spot_rl_experiments/experiments/skill_test/run_episode.py +++ b/spot_rl_experiments/experiments/skill_test/run_episode.py @@ -1,4 +1,3 @@ -# mypy: ignore-errors # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. @@ -45,8 +44,8 @@ def save_logs_as_json(): atexit.register(save_logs_as_json) # Register the signal handler for termination signals -signal.signal(signal.SIGINT, save_logs_as_json) -signal.signal(signal.SIGTERM, save_logs_as_json) +signal.signal(signal.SIGINT, save_logs_as_json) # type: ignore +signal.signal(signal.SIGTERM, save_logs_as_json) # type: ignore def parse_yaml(file_path): From 2e23a8ab4df1e4ecbe87e404eba283399b77c83e Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 17 Sep 2024 12:54:53 -0400 Subject: [PATCH 71/82] polish --- spot_rl_experiments/experiments/skill_test/run_episode.py | 1 - spot_rl_experiments/experiments/skill_test/test_sem_place.py | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/run_episode.py b/spot_rl_experiments/experiments/skill_test/run_episode.py index 66b71c45..3de5bde3 100644 --- a/spot_rl_experiments/experiments/skill_test/run_episode.py +++ b/spot_rl_experiments/experiments/skill_test/run_episode.py @@ -68,7 +68,6 @@ def parse_yaml(file_path): for action, args in action_dict.items(): method = getattr(action_taker, action, None) if method: - # breakpoint() status, _ = method(*args) controller = getattr(action_taker, controller_dict[action], None) if controller: diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index df6168cf..c9085a99 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -54,7 +54,7 @@ input("Place an object in Spot's gripper and press Enter to continue...") #Place Object and Close Gripper rospy.set_param("is_gripper_blocked", 0) - episode_log = {"actions": []} + episode_log = {"actions": []} # type: ignore spotskillmanager.spot.close_gripper() input("waiting for user to get ready with camera") # The following is a helpful tip to debug the arm From 5985dc656774d0349ca73a4ab14ab2fd794bf5e8 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 17 Sep 2024 12:57:45 -0400 Subject: [PATCH 72/82] polish --- .../experiments/skill_test/test_sem_place.py | 18 ++---------------- .../skill_test/test_sem_place_ee.py | 3 +-- 2 files changed, 3 insertions(+), 18 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index c9085a99..ca4bdaa8 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -52,27 +52,13 @@ #Open Gripper spotskillmanager.spot.open_gripper() input("Place an object in Spot's gripper and press Enter to continue...") - #Place Object and Close Gripper + + # Place Object and Close Gripper rospy.set_param("is_gripper_blocked", 0) episode_log = {"actions": []} # type: ignore spotskillmanager.spot.close_gripper() input("waiting for user to get ready with camera") - # The following is a helpful tip to debug the arm - # We get Spot class - # spot = spotskillmanager.spot - # We can move the gripper to a point with x,y,z and roll, pitch, yaw - # spot.move_gripper_to_point((0.55, 0., 0.26), np.deg2rad(np.array([0,0,0]))) - # We can also set the robot arm joints - # config = construct_config() - # spot.set_arm_joint_positions(np.deg2rad(config.INITIAL_ARM_JOINT_ANGLES)) - # In addition, if you want to use semantic place skill based on the grasping orientation, you can do - # spotskillmanager.nav("black_case") - # spotskillmanager.pick("bottle") - # # Fetch the arm joint at grasping location - # ee_orientation_at_grasping = spotskillmanager.gaze_controller.env.ee_orientation_at_grasping - # spotskillmanager.nav("test_desk") - # spotskillmanager.place("test_desk", orientation_at_grasping) # This controls the arm initial orientation spotskillmanager.place(place_target, is_local=is_local, visualize=False) skill_log = spotskillmanager.place_controller.skill_result_log if "num_steps" not in skill_log: diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py index 79128174..96d357dc 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place_ee.py @@ -1,7 +1,6 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. -# mypy: ignore-errors import json from datetime import datetime @@ -30,7 +29,7 @@ spotskillmanager.spot.open_gripper() input("Place an object in Spot's gripper and press Enter to continue...") rospy.set_param("is_gripper_blocked", 0) - episode_log = {"actions": []} + episode_log = {"actions": []} # type: ignore spotskillmanager.spot.close_gripper() input("waiting for user to get ready with camera") From 019078b56e0333eb80f364391a6f61a7e4c7cc46 Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 17 Sep 2024 12:58:59 -0400 Subject: [PATCH 73/82] polish --- .../experiments/skill_test/test_static_place.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_static_place.py b/spot_rl_experiments/experiments/skill_test/test_static_place.py index 148a5645..45ed6de9 100644 --- a/spot_rl_experiments/experiments/skill_test/test_static_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_static_place.py @@ -1,7 +1,7 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. -# mypy: ignore-errors + import json import time from datetime import datetime @@ -41,7 +41,7 @@ spotskillmanager.spot.open_gripper() input("Place an object in Spot's gripper and press Enter to continue...") rospy.set_param("is_gripper_blocked", 0) - episode_log = {"actions": []} + episode_log = {"actions": []} # type: ignore spotskillmanager.spot.close_gripper() input("waiting for user to get ready with camera") From 69a0c0d4d1290de4f203caa2f5218dc95640d19e Mon Sep 17 00:00:00 2001 From: Jimmy Yang Date: Tue, 17 Sep 2024 13:10:08 -0400 Subject: [PATCH 74/82] polish --- spot_rl_experiments/spot_rl/envs/base_env.py | 1 + spot_rl_experiments/spot_rl/utils/helper_nodes.py | 1 - 2 files changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 8c13b126..0c6fd516 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -464,6 +464,7 @@ def step( # noqa print(f"base_action: {arr2str(base_action)}\tarm_action: {arr2str(arm_action)}") + # Spin until enough time has passed during this step start_time = time.time() if arm_ee_action is not None and self._max_lin_dist_scale == 0.0: pass diff --git a/spot_rl_experiments/spot_rl/utils/helper_nodes.py b/spot_rl_experiments/spot_rl/utils/helper_nodes.py index f39f6690..40c3f632 100644 --- a/spot_rl_experiments/spot_rl/utils/helper_nodes.py +++ b/spot_rl_experiments/spot_rl/utils/helper_nodes.py @@ -1,4 +1,3 @@ -# mypy: ignore-errors # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. From 09a7226c00e909cef0c31dbfdd855003f94cb96a Mon Sep 17 00:00:00 2001 From: Achuthankrishna <74654704+Achuthankrishna@users.noreply.github.com> Date: Tue, 17 Sep 2024 10:37:07 -0700 Subject: [PATCH 75/82] Update config with new weights --- spot_rl_experiments/configs/config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 3d54c94c..86a909da 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -20,7 +20,7 @@ WEIGHTS_TORCHSCRIPT: SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" # Semantic place EE - SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_55k_mobile_0.1_30_ee_0.1_rot_drop_no_term_250_steps_EEPoseSensor_kinematic_ee_dist_ckpt_50.pth.torchscript" + SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_55k_mobile_0.1_30_ee_0.1_rot_drop_no_term_250_steps_EEPoseSensor_kinematic_ee_dist_emb_unocc_snap_seed1_ckpt.35.pth.torchscript" # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -45,7 +45,7 @@ WEIGHTS: SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" # Semantic place EE - SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_55k_mobile_0.1_30_ee_0.1_rot_drop_no_term_250_steps_EEPoseSensor_kinematic_ee_dist_ckpt_50.pth.torchscript" + SEMANTIC_PLACE_EE: "weights/semantic_place_ee/ssp_55k_mobile_0.1_30_ee_0.1_rot_drop_no_term_250_steps_EEPoseSensor_kinematic_ee_dist_emb_unocc_snap_seed1_ckpt.35.pth.torchscript" # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" From 34ddaeb2e6438a726cf93300820b6359299f94ce Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 17 Sep 2024 12:36:02 -0700 Subject: [PATCH 76/82] Recent changes and cleanup --- spot_rl_experiments/configs/config.yaml | 5 ----- spot_rl_experiments/experiments/skill_test/test_sem_place.py | 2 +- spot_rl_experiments/spot_rl/envs/base_env.py | 2 +- spot_rl_experiments/spot_rl/real_policy.py | 2 +- spot_rl_experiments/spot_rl/skills/atomic_skills.py | 5 ----- 5 files changed, 3 insertions(+), 13 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index 86a909da..c1cabc46 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -161,15 +161,10 @@ INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE: [0, -180, 180, 0, 0, 90] # The initial INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_LEFT_HAND: [0, -180, 180, 0, 0, -90] # The initial orientation of the arm for side grasping (gripper is in object's left hand side) INITIAL_ARM_JOINT_ANGLES_SEMANTIC_PLACE_TOP_DOWN: [-0.91, -92.87, 91.21, 0, 90.01, 0] # The initial orientation of the arm for top down grasping # The old gaze ready angle: [0, -170, 120, 0, 75, 0] -<<<<<<< HEAD GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 75, 0] GAZE_ARM_JOINT_ANGLES_LOW_RECEPTACLES: [0, -160, 100, 0, 75, 0] #[0, -160, 100, 0, 90, 0] GAZE_ARM_JOINT_ANGLES_HIGH_RECEPTACLES: [0, -120, 60, 0, 88, 0] #[0, -160, 100, 0, 90, 0] HEIGHT_THRESHOLD : 0.8128 -======= -GAZE_ARM_JOINT_ANGLES: [0, -160, 100, 0, 75, 0] #[0, -160, 100, 0, 90, 0] -SEMANTIC_PLACE_ARM_JOINT_ANGLES: [0, -125, 80, 0, 85, 0] ->>>>>>> 9fc4546 (updated to older value in config for semantic place joint angles.) PLACE_ARM_JOINT_ANGLES: [0, -170, 120, 0, 75, 0] ARM_LOWER_LIMITS: [-45, -180, 0, 0, -90, 0] ARM_UPPER_LIMITS: [45, 0, 180, 0, 90, 0] diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index ca4bdaa8..95a03c61 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -63,7 +63,7 @@ skill_log = spotskillmanager.place_controller.skill_result_log if "num_steps" not in skill_log: skill_log["num_steps"] = 0 - episode_log["actions"].append({f"place": skill_log}) + episode_log["actions"].append({"place": skill_log}) curr_date = datetime.today().strftime("%m-%d-%y") file_path = ( f"logs/semantic_place/{curr_date}/episode_sem_pl_run2_{episode_ctr}.json" diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 0c6fd516..32154350 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -434,7 +434,7 @@ def step( # noqa travel_time=self.config.ARM_TRAJECTORY_TIME_IN_SECONDS, disable_obstacle_avoidance=disable_oa, ) - elif base_action is not None and arm_ee_action is not None: + elif arm_ee_action is not None: if self._max_lin_dist_scale == 0.0: self.spot.set_arm_ee_pos( arm_ee_action, diff --git a/spot_rl_experiments/spot_rl/real_policy.py b/spot_rl_experiments/spot_rl/real_policy.py index 8a4c9ed8..e931d066 100644 --- a/spot_rl_experiments/spot_rl/real_policy.py +++ b/spot_rl_experiments/spot_rl/real_policy.py @@ -403,7 +403,7 @@ def __init__(self, checkpoint_path, device, config: CN = CN()): ), "ee_pos": spaces.Box( shape=[ - 3, + 6, ], low=np.finfo(np.float32).min, high=np.finfo(np.float32).max, diff --git a/spot_rl_experiments/spot_rl/skills/atomic_skills.py b/spot_rl_experiments/spot_rl/skills/atomic_skills.py index ea6b2ea9..107fdfb0 100644 --- a/spot_rl_experiments/spot_rl/skills/atomic_skills.py +++ b/spot_rl_experiments/spot_rl/skills/atomic_skills.py @@ -492,11 +492,9 @@ def set_pose_estimation_flags( self, enable_pose_estimation: bool = False, enable_pose_correction: bool = False, - mesh_name: str = "", ) -> None: self.enable_pose_estimation = enable_pose_estimation self.enable_pose_correction = enable_pose_correction - self.mesh_name = mesh_name def set_force_control(self, enable_force_control: bool = False): self.enable_force_control = enable_force_control @@ -588,7 +586,6 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: "enable_pose_correction": self.enable_pose_correction, "enable_force_control": self.enable_force_control, "grasp_mode": self.grasp_mode, - "mesh_name": self.mesh_name, } else: action_dict = { @@ -598,7 +595,6 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: "enable_pose_correction": self.enable_pose_correction, "enable_force_control": self.enable_force_control, "grasp_mode": self.grasp_mode, - "mesh_name": self.mesh_name, } return action_dict @@ -731,7 +727,6 @@ def split_action(self, action: np.ndarray) -> Dict[str, Any]: "enable_pose_correction": self.enable_pose_correction, "enable_force_control": self.enable_force_control, "grasp_mode": self.grasp_mode, - "mesh_name": self.mesh_name, } return action_dict From 92973b22e3156df0500478e56056d05618849db6 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 17 Sep 2024 12:40:01 -0700 Subject: [PATCH 77/82] code polish --- spot_rl_experiments/experiments/skill_test/test_sem_place.py | 1 + 1 file changed, 1 insertion(+) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index 95a03c61..f3597e2f 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -1,6 +1,7 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. + # mypy: ignore-errors # black: ignore-errors import json From f1f9e6d2082d3b1ec386d4410fd19795d3812306 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 17 Sep 2024 12:41:28 -0700 Subject: [PATCH 78/82] code polish --- spot_rl_experiments/experiments/skill_test/test_sem_place.py | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index f3597e2f..28d7d476 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -1,9 +1,8 @@ # Copyright (c) Meta Platforms, Inc. and its affiliates. # This source code is licensed under the MIT license found in the # LICENSE file in the root directory of this source tree. - -# mypy: ignore-errors # black: ignore-errors +# mypy: ignore-errors import json import time from datetime import datetime From d3e171fc60c0d5cb0e40d4fa71f1cb0e4027708f Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 17 Sep 2024 12:48:33 -0700 Subject: [PATCH 79/82] code polish --- .../experiments/skill_test/test_sem_place.py | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/spot_rl_experiments/experiments/skill_test/test_sem_place.py b/spot_rl_experiments/experiments/skill_test/test_sem_place.py index 28d7d476..2ac593df 100644 --- a/spot_rl_experiments/experiments/skill_test/test_sem_place.py +++ b/spot_rl_experiments/experiments/skill_test/test_sem_place.py @@ -44,12 +44,12 @@ # Start testing contnue = True episode_ctr = 0 - #Get EE Pose Initial + # Get EE Pose Initial spot_pos, spot_ort = spotskillmanager.spot.get_ee_pos_in_body_frame() - #Set Orientation as Zero + # Set Orientation as Zero spot_ort = np.zeros(3) while contnue: - #Open Gripper + # Open Gripper spotskillmanager.spot.open_gripper() input("Place an object in Spot's gripper and press Enter to continue...") @@ -73,7 +73,7 @@ print(f"Saved log: {file_path}") episode_ctr += 1 contnue = map_user_input_to_boolean("Do you want to do it again ? Y/N ") - #Return the arm to the original position + # Return the arm to the original position spot_pos = spotskillmanager.spot.get_ee_pos_in_body_frame()[0] spotskillmanager.spot.move_gripper_to_point(spot_pos, spot_ort) # The following is a helpful tip to debug the arm From 326bf44fd8baded3ec091f6198a82c35a1706403 Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Tue, 17 Sep 2024 12:53:53 -0700 Subject: [PATCH 80/82] recent updates for new weights --- spot_rl_experiments/spot_rl/real_policy.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/spot_rl_experiments/spot_rl/real_policy.py b/spot_rl_experiments/spot_rl/real_policy.py index e931d066..75a8fe77 100644 --- a/spot_rl_experiments/spot_rl/real_policy.py +++ b/spot_rl_experiments/spot_rl/real_policy.py @@ -401,7 +401,7 @@ def __init__(self, checkpoint_path, device, config: CN = CN()): "articulated_agent_jaw_depth": spaces.Box( shape=[240, 228, 1], low=0.0, high=1.0, dtype=np.float32 ), - "ee_pos": spaces.Box( + "ee_pose": spaces.Box( shape=[ 6, ], From 35c7f57f769610cb3ed73720b891bebe86fd7e2c Mon Sep 17 00:00:00 2001 From: Achuthankrishna Date: Wed, 18 Sep 2024 11:15:46 -0700 Subject: [PATCH 81/82] Changes in waypt estimation --- spot_rl_experiments/spot_rl/envs/base_env.py | 2 +- spot_rl_experiments/spot_rl/envs/skill_manager.py | 12 ++++++++++-- 2 files changed, 11 insertions(+), 3 deletions(-) diff --git a/spot_rl_experiments/spot_rl/envs/base_env.py b/spot_rl_experiments/spot_rl/envs/base_env.py index 32154350..c573b9c1 100644 --- a/spot_rl_experiments/spot_rl/envs/base_env.py +++ b/spot_rl_experiments/spot_rl/envs/base_env.py @@ -435,7 +435,7 @@ def step( # noqa disable_obstacle_avoidance=disable_oa, ) elif arm_ee_action is not None: - if self._max_lin_dist_scale == 0.0: + if base_action is None: self.spot.set_arm_ee_pos( arm_ee_action, travel_time=self.config.ARM_TRAJECTORY_TIME_IN_SECONDS, diff --git a/spot_rl_experiments/spot_rl/envs/skill_manager.py b/spot_rl_experiments/spot_rl/envs/skill_manager.py index 1f40b123..7477f7b3 100644 --- a/spot_rl_experiments/spot_rl/envs/skill_manager.py +++ b/spot_rl_experiments/spot_rl/envs/skill_manager.py @@ -541,6 +541,14 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray conditional_print(message=message, verbose=self.verbose) is_local = True + percentile = 0 if visualize else 70 + if self.use_semantic_place and self.use_place_ee: + height_adjustment_threshold = 0.1 + percentile = 0 if visualize else 50 + elif self.use_semantic_place: + height_adjustment_threshold = 0.05 + else: + height_adjustment_threshold = 0.23 # estimate waypoint try: ( @@ -550,9 +558,9 @@ def place(self, place_target: str = None, ee_orientation_at_grasping: np.ndarray ) = detect_place_point_by_pcd_method( self.spot, self.arm_joint_angles, - percentile=0 if visualize else 70, + percentile=percentile, visualize=visualize, - height_adjustment_offset=0.10 if self.use_semantic_place else 0.23, + height_adjustment_offset=height_adjustment_threshold, ) print(f"Estimate Place xyz: {place_target_location}") if visualize: From 6500058bb5d65a19d64a36f217fc6fa04d1cf6c2 Mon Sep 17 00:00:00 2001 From: Achuthankrishna <74654704+Achuthankrishna@users.noreply.github.com> Date: Wed, 18 Sep 2024 15:56:13 -0700 Subject: [PATCH 82/82] Update config.yaml for grasp_threshold --- spot_rl_experiments/configs/config.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/spot_rl_experiments/configs/config.yaml b/spot_rl_experiments/configs/config.yaml index c1cabc46..ccdc3586 100644 --- a/spot_rl_experiments/configs/config.yaml +++ b/spot_rl_experiments/configs/config.yaml @@ -45,7 +45,7 @@ WEIGHTS: SEMANTIC_PLACE: "weights/semantic_place/sp12lhb6v10_12_ckpt.199.torchscript" # Semantic place EE - SEMANTIC_PLACE_EE: "weights/semantic_place_ee/ssp_55k_mobile_0.1_30_ee_0.1_rot_drop_no_term_250_steps_EEPoseSensor_kinematic_ee_dist_emb_unocc_snap_seed1_ckpt.35.pth.torchscript" + SEMANTIC_PLACE_EE: "weights/semantic_place_ee/sp_55k_mobile_0.1_30_ee_0.1_rot_drop_no_term_250_steps_EEPoseSensor_kinematic_ee_dist_emb_unocc_snap_seed1_ckpt.35.pth.torchscript" # Open close drawer OPEN_CLOSE_DRAWER: "weights/open_close_drawer/od15lr_56_ckpt.59.combined_net.torchscript" @@ -120,7 +120,7 @@ OPEN_CLOSE_DRAWER_ACTION_SPACE_LENGTH: 8 OPEN_CLOSE_DRAWER_DISTANCE_BETWEEN_EE_HANDLE: 0.3 #GRASP DISTANCE THRESHOLD -GRASP_DISTANCE_THRESHOLD: 0.7 +GRASP_DISTANCE_THRESHOLD: 1.5 GRASP_DISTANCE_THRESHOLD_EE: 1.5 # Semantic Pick