Merge pull request #25 from ARISE-Initiative/update-tests

Update Tests and Fix Sawyer IK EEF Bug

robosuite/controllers/ik.py

@@ -518,9 +518,10 @@ def bullet_base_pose_to_world_pose(self, pose_in_base):
         """
         pose_in_base = T.pose2mat(pose_in_base)
 
-        base_pos_in_world, base_orn_in_world = np.array(
-            p.getBasePositionAndOrientation(self.ik_robot, physicsClientId=self.bullet_server_id)
+        base_pos_in_world, base_orn_in_world = p.getBasePositionAndOrientation(
+            self.ik_robot, physicsClientId=self.bullet_server_id
         )
+        base_pos_in_world, base_orn_in_world = np.array(base_pos_in_world), np.array(base_orn_in_world)
 
         base_pose_in_world = T.pose2mat((base_pos_in_world, base_orn_in_world))
 

robosuite/environments/base.py

@@ -96,6 +96,9 @@ def __init__(
         renderer="mujoco",
         renderer_config=None,
     ):
+        # If you're using an onscreen renderer, you must be also using an offscreen renderer!
+        if has_renderer and not has_offscreen_renderer:
+            has_offscreen_renderer = True
 
         # Rendering-specific attributes
         self.has_renderer = has_renderer

robosuite/models/grippers/gripper_model.py

@@ -4,7 +4,8 @@
 import numpy as np
 
 from robosuite.models.base import MujocoXMLModel
-from robosuite.utils.mjcf_utils import GRIPPER_COLLISION_COLOR
+from robosuite.utils.mjcf_utils import GRIPPER_COLLISION_COLOR, find_elements, string_to_array
+import robosuite.utils.transform_utils as T
 
 
 class GripperModel(MujocoXMLModel):
@@ -23,9 +24,16 @@ def __init__(self, fname, idn=0):
         self.current_action = np.zeros(self.dof)
 
         # Grab gripper offset (string -> np.array -> elements [1, 2, 3, 0] (x, y, z, w))
-        self.rotation_offset = np.fromstring(
+        # This is the comopunded rotation with the base body and the eef body as well!
+        base_quat = np.fromstring(
             self.worldbody[0].attrib.get("quat", "1 0 0 0"), dtype=np.float64, sep=" "
         )[[1, 2, 3, 0]]
+        eef_element = find_elements(
+            root=self.root, tags="body", attribs={"name": self.correct_naming("eef")}, return_first=True
+        )
+        eef_relative_quat = string_to_array(eef_element.get("quat", "1 0 0 0"))[[1, 2, 3, 0]]
+        self.rotation_offset = T.quat_multiply(eef_relative_quat, base_quat)
+
 
     def format_action(self, action):
         """

robosuite/models/grippers/gripper_tester.py

@@ -117,6 +117,10 @@ def start_simulation(self):
 
         if self.render:
             self.viewer = OpenCVRenderer(self.sim)
+            # We also need to add the offscreen context
+            if self.sim._render_context_offscreen is None:
+                render_context = MjRenderContextOffscreen(self.sim, device_id=-1)
+                self.sim.add_render_context(render_context)
         self.sim_state = self.sim.get_state()
 
         # For gravity correction

robosuite/utils/binding_utils.py

@@ -120,6 +120,11 @@ def update_offscreen_size(self, width, height):
             del self.con
             self._set_mujoco_context_and_buffers()
 
+    def upload_texture(self, tex_id):
+        """Uploads given texture to the GPU"""
+        self.gl_ctx.make_current()
+        mujoco.mjr_uploadTexture(self.model, self.con, tex_id)
+
     def render(self, width, height, camera_id=None, segmentation=False):
         viewport = mujoco.MjrRect(0, 0, width, height)
 

robosuite/utils/mjcf_utils.py

@@ -275,7 +275,7 @@ def string_to_array(string):
     Returns:
         np.array: Numerical array equivalent of @string
     """
-    return np.array([float(x) for x in string.split(" ")])
+    return np.array([float(x) for x in string.strip().split(" ")])
 
 
 def convert_to_string(inp):

robosuite/utils/mjmod.py

@@ -14,6 +14,7 @@
 
 import robosuite
 import robosuite.utils.transform_utils as trans
+from robosuite.utils.binding_utils import MjRenderContextOffscreen
 
 
 class BaseModder:
@@ -1249,8 +1250,10 @@ def upload_texture(self, name):
             name (str): name of geom
         """
         texture = self.get_texture(name)
+        if self.sim._render_context_offscreen is None:
+            render_context = MjRenderContextOffscreen(self.sim, device_id=self.render_gpu_device_id)
         if not self.sim.render_contexts:
-            raise NotImplementedError("Need to implement logic for dm binding")
+            MjRenderContextOffscreen(self.sim)
         for render_context in self.sim.render_contexts:
             render_context.upload_texture(texture.id)
 

tests/test_controllers/test_linear_interpolator.py

@@ -140,10 +140,10 @@ def test_linear_interpolator():
 
                 # Keep track of state of robot eef (pos, ori (euler)) and torques
                 current_torques = np.zeros(7)
-                initial_state = [env.robots[0]._hand_pos, T.mat2euler(env.robots[0]._hand_orn)]
+                initial_state = [env.robots[0]._hand_pos, T.mat2quat(env.robots[0]._hand_orn)]
                 dstate = [
                     env.robots[0]._hand_pos - initial_state[0],
-                    T.mat2euler(env.robots[0]._hand_orn) - initial_state[1],
+                    T.mat2euler(T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn), initial_state[1]))),
                 ]
 
                 # Define the uniform trajectory action
@@ -172,12 +172,12 @@ def test_linear_interpolator():
                     timesteps[j] += 1
                     dstate = [
                         env.robots[0]._hand_pos - initial_state[0],
-                        T.mat2euler(env.robots[0]._hand_orn) - initial_state[1],
+                        T.mat2euler(T.quat2mat(T.quat_distance(T.mat2quat(env.robots[0]._hand_orn), initial_state[1]))),
                     ]
 
                 # When finished, print out the timestep results
                 print(
-                    "Completed trajectory. Total summed absolute delta torques: {}".format(summed_abs_delta_torques[j])
+                    "Completed trajectory. Avg per-step absolute delta torques: {}".format(summed_abs_delta_torques[j] / timesteps[j])
                 )
 
                 # Shut down this env before starting the next test

tests/test_environments/test_camera_transforms.py

@@ -48,11 +48,12 @@ def test_camera_transforms():
 
     # unnormalized depth map
     depth_map = obs_dict["{}_depth".format(camera_name)][::-1]
-    depth_map = CU.get_real_depth_map(sim=sim, depth_map=depth_map)
+
+    depth_map = CU.get_real_depth_map(sim=env.sim, depth_map=depth_map)
 
     # get camera matrices
     world_to_camera = CU.get_camera_transform_matrix(
-        sim=sim,
+        sim=env.sim,
         camera_name=camera_name,
         camera_height=camera_height,
         camera_width=camera_width,