Skills benchmarking without pose estimation

.gitignore

@@ -2,6 +2,7 @@
 .vscode
 *.jpg
 *.png
+*.json
 !intel_realsense_payload_for_spotsim2real/images_for_readme/*.jpg
 !intel_realsense_payload_for_spotsim2real/images_for_readme/*.png
 **/**.egg-info/**
@@ -13,8 +14,16 @@ bd_spot_wrapper/spot_wrapper/home.txt
 spot_rl_experiments/spot_rl.egg-info/**
 spot_rl_experiments/configs/waypoints.yaml
 
+
+spot_rl_experiments/spot_rl/utils/gripper_T_intel.npy
+intel_realsense_payload_for_spotsim2real/gripper_T_intel.npy
+
 **/grasp_visualizations/**
 **/temp_recordings/**
 
 **/**/.ipynb_checkpoints
-*capture_the_moment.py
+data/*
+spot_rl_experiments/experiments/skill_test/logs/*
+spot_rl_experiments/experiments/skill_test/table_detections/*
+ros_tcp/ros_communication_client.egg-info/**
+*.npy

.gitmodules

@@ -13,3 +13,8 @@
 	url = https://github.com/naokiyokoyama/mask_rcnn_detectron2
 	branch = main
 	ignore = dirty
+[submodule "third_party/FoundationPoseForSpotSim2Real"]
+	path = third_party/FoundationPoseForSpotSim2Real
+	url = https://github.com/tusharsangam/FoundationPoseForSpotSim2Real
+	branch = main
+	ignore = dirty 

README.md

@@ -181,6 +181,19 @@ git checkout main
 
 - If you are done with demo of one of the above code and want to run another code, you do not need to re-run other sessions and nodes. Running a new command in the same terminal will work just fine. But **make sure to bring robot at home location and reset its home** using `spot_reset_home` in the same terminal
 
+#### 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 
+- 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
+- <b>How to use Pose Estimation ?</b>
+    - You can pass two flags `enable_pose_estimation` & `enable_pose_correction` with `pick` skill as `skillmanager.pick(enable_pose_estimation=True, enable_pose_correction=True)`
+    - If you enable pose correction, spot will first manually correct the object pose for eg. rotate horizontal object to be vertical etc & place the corrected the object at the same place.
+    - Our `orientationsolver` can also correct the object to face the camera but it incurs additional pick attempt before place can be run thus is kept to be false by default
+    - <b>Enabling pose estimation can help in two major way</b> - 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.
+
+
 ### Using Spot Data-logger
 All logs will get stored inside `data/data_logs` directory
 

aria_data_loaders/aria_data_utils/aria_live_streamer.py

@@ -13,7 +13,12 @@
 import numpy as np
 import projectaria_tools.core as aria_core
 import rospy
-import sophus as sp
+
+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 aria_data_utils.aria_sdk_utils import update_iptables
 from geometry_msgs.msg import Pose, PoseStamped
 from nav_msgs.msg import Odometry

aria_data_loaders/aria_data_utils/aria_streamer.py

@@ -11,7 +11,12 @@
 import cv2
 import numpy as np
 import rospy
-import sophus as sp
+
+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 fairotag.scene import Scene
 from matplotlib import pyplot as plt
 from perception_and_utils.perception.detector_wrappers.april_tag_detector import (

aria_data_loaders/aria_data_utils/episodic_memory_robotic_fetch.py

@@ -9,7 +9,12 @@
 import click
 import numpy as np
 import rospy
-import sophus as sp
+
+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 geometry_msgs.msg import PoseStamped
 from perception_and_utils.utils.conversions import (
     ros_PoseStamped_to_sophus_SE3,