changes to task definition (pushing and lifting)

calvin_env/envs/play_table_env.py

@@ -23,7 +23,8 @@
 
 # A logger for this file
 log = logging.getLogger(__name__)
-
+from rich.traceback import install
+install(show_locals=True)
 
 class PlayTableSimEnv(gym.Env):
     def __init__(
@@ -152,11 +153,12 @@ def render(self, mode="human"):
         """render is gym compatibility function"""
         rgb_obs, depth_obs = self.get_camera_obs()
         if mode == "human":
-            if "rgb_static" not in rgb_obs:
-                log.warning("Environment does not have static camera")
-                return
-            img = rgb_obs["rgb_static"][:, :, ::-1].copy()
-            cv2.imshow("simulation cam", cv2.resize(img, (500, 500)))
+            if "rgb_static" in rgb_obs:
+                img = rgb_obs["rgb_static"][:, :, ::-1]
+                cv2.imshow("simulation cam", cv2.resize(img, (500, 500)))
+            if "rgb_gripper" in rgb_obs:
+                img2 = rgb_obs["rgb_gripper"][:, :, ::-1]
+                cv2.imshow("gripper cam", cv2.resize(img2, (500, 500)))
             cv2.waitKey(1)
         elif mode == "rgb_array":
             assert "rgb_static" in rgb_obs, "Environment does not have static camera"
@@ -292,7 +294,7 @@ def run_env(cfg):
 
     env.reset()
     while True:
-        env.step(np.array((0.,0,0, 0,0,0, 1)))
+        env.step(np.array((0.,0, 0,0,0, 1)))
         # env.render()
         time.sleep(0.01)
 

calvin_env/envs/tasks.py

@@ -54,8 +54,7 @@ def num_tasks(self):
 
     @staticmethod
     def rotate_object(
-            obj_name, z_degrees, x_y_threshold=30, z_threshold=180, movement_threshold=0.1, start_info=None,
-            end_info=None
+        obj_name, z_degrees, x_y_threshold=30, z_threshold=180, movement_threshold=0.1, start_info=None, end_info=None
     ):
         """
         Returns True if the object with obj_name was rotated more than z_degrees degrees around the z-axis while not
@@ -101,14 +100,13 @@ def push_object(obj_name, x_direction, y_direction, start_info, end_info):
         end_pos = np.array(obj_end_info["current_pos"])
         pos_diff = end_pos - start_pos
 
-        start_contacts = set(c[2] for c in obj_start_info["contacts"])
-        end_contacts = set(c[2] for c in obj_end_info["contacts"])
-        robot_uid = {start_info["robot_info"]["uid"]}
+        robot_uid = start_info["robot_info"]["uid"]
+        # contacts excluding robot
+        start_contacts = set((c[2], c[4]) for c in obj_start_info["contacts"] if c[2] != robot_uid)
+        end_contacts = set((c[2], c[4]) for c in obj_end_info["contacts"] if c[2] != robot_uid)
 
         # computing set difference to check if object had surface contact (excluding robot) at both times
-        surface_contact = len(start_contacts - robot_uid) > 0 and (start_contacts - robot_uid) == (
-            end_contacts - robot_uid
-        )
+        surface_contact = len(start_contacts) > 0 and len(end_contacts) > 0 and start_contacts <= end_contacts
         if not surface_contact:
             return False
 
@@ -152,7 +150,7 @@ def lift_object(obj_name, z_direction, surface_body=None, surface_link=None, sta
 
         return (
             z_diff > z_direction
-            and robot_uid not in start_contacts
+            # and robot_uid not in start_contacts
             and robot_uid in end_contacts
             and len(end_contacts) == 1
             and surface_criterion

calvin_env/robot/robot.py

@@ -33,6 +33,8 @@ def __init__(
         max_velocity,
         use_ik_fast,
         euler_obs,
+        lower_joint_limits=(-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973),
+        upper_joint_limits=(2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973),
         max_rel_pos=0.02,
         max_rel_orn=0.05,
         magic_scaling_factor_pos=1,
@@ -61,8 +63,8 @@ def __init__(
         self.end_effector_link_id = end_effector_link_id
         self.gripper_action = 1
         self.ll = self.ul = self.jr = self.rp = None
-        self.ll_real = np.array([-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973])
-        self.ul_real = np.array([2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973])
+        self.ll_real = np.array(lower_joint_limits)
+        self.ul_real = np.array(upper_joint_limits)
         self.mixed_ik = None
         self.euler_obs = euler_obs
         self.max_rel_pos = max_rel_pos

conf/robot/panda.yaml

@@ -6,12 +6,14 @@ initial_joint_positions: ${scene.robot_initial_joint_positions}
 max_joint_force: 200.0
 gripper_force: 200
 arm_joint_ids: [0, 1, 2, 3, 4, 5, 6]
+lower_joint_limits: [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973]
+upper_joint_limits: [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973]
 gripper_joint_ids: [9, 10]
 gripper_joint_limits: [0, 0.04]
 tcp_link_id: 13
 end_effector_link_id: 7
 gripper_cam_link: 12
-use_nullspace: false
+use_nullspace: true
 max_velocity: 2
 use_ik_fast: false
 magic_scaling_factor_pos: 1 # 1.6