fix(added lab scene for pickup):converting geom pose to robot_base frame and detecting an ik failure

Author: suman1209

python/examples/grasp_demo.py

@@ -24,7 +24,10 @@ def get_object_pose(self, geom_name) -> Pose:
         data = self.env.get_wrapper_attr("sim").data
 
         geom_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_GEOM, geom_name)
-        return Pose(translation=data.geom_xpos[geom_id], rotation=data.geom_xmat[geom_id].reshape(3, 3))
+        obj_pose_world_coordinates = Pose(translation=data.geom_xpos[geom_id],
+                                          rotation=data.geom_xmat[geom_id].reshape(3, 3))
+        obj_pose_robot_coordinates = self.env.unwrapped.robot.to_pose_in_robot_coordinates(obj_pose_world_coordinates)
+        return obj_pose_robot_coordinates
 
     def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: int) -> list[Pose]:
         waypoints = []
@@ -36,7 +39,7 @@ def generate_waypoints(self, start_pose: Pose, end_pose: Pose, num_waypoints: in
     def step(self, action: np.ndarray) -> dict:
         re = self.env.step(action)
         s: FR3State = self.env.unwrapped.robot.get_state()
-        return re[0]
+        return re
 
     def plan_linear_motion(self, geom_name: str, delta_up: float, num_waypoints: int = 20) -> list[Pose]:
         end_eff_pose = self.env.unwrapped.robot.get_cartesian_position()
@@ -55,6 +58,14 @@ def execute_motion(self, waypoints: list[Pose], gripper: float = GripperWrapper.
             # calculate delta action
             delta_action = waypoints[i] * waypoints[i - 1].inverse()
             obs = self.step(self._action(delta_action, gripper))
+            ik_success = obs[-1]['ik_success']
+            if not obs[-1]['ik_success']:
+                trans_source, rot_source = waypoints[i - 1].translation(), waypoints[i - 1].rotation_rpy().as_vector()
+                trans_dest, rot_des = waypoints[i].translation(), waypoints[i].rotation_rpy().as_vector()
+                print(f"ik success: {ik_success} when attempting to move from trans: {trans_source}, rot: {rot_source}\n"
+                      f" to trans: {trans_dest} rot: {rot_des}!")
+                print(f"aborting motion!")
+                exit(-1)
         return obs
 
     def approach(self, geom_name: str):
@@ -84,7 +95,7 @@ def pickup(self, geom_name: str):
 
 def main():
     # available envs: "rcs/SimplePickUpSim-v0", "rcs/FR3LabPickUpSimDigitHand-v0", "SimplePickUpSimDigitHand-v0"
-    env = gym.make("rcs/FR3LabPickUpSimDigitHand-v0", render_mode="human", delta_actions=True)
+    env = gym.make("rcs/SimplePickUpSim-v0", render_mode="human", delta_actions=True)
     env.reset()
     controller = PickUpDemo(env)
     controller.pickup("yellow_box_geom")