fix[bicycle]: Fixes orientation error of bicycle model.

tests/test_acc_envs.py

@@ -169,7 +169,7 @@ def test_allBicycleModel(allBicycleModelEnvs):
         env = gym.make("urdf-env-v0", robots=[setup[0]], render=False, dt=0.01)
         ob, _ = env.reset(pos=setup[1], vel=setup[2])
         action = np.random.random(env.n()) * 0.1
-        np.testing.assert_array_almost_equal(ob['robot_0']['joint_state']['position'][0:2], setup[1][0:2], decimal=2)
+        np.testing.assert_array_almost_equal(ob['robot_0']['joint_state']['position'], setup[1], decimal=2)
         ob, *_ = env.step(action)
         assert isinstance(ob['robot_0'], dict)
         assert isinstance(ob['robot_0']['joint_state']['position'], np.ndarray)

tests/test_vel_envs.py

@@ -175,7 +175,7 @@ def test_allBicycleModel(allBicycleModelEnvs):
         env = gym.make("urdf-env-v0", robots=[setup[0]], render=False, dt=0.01)
         ob, _ = env.reset(pos=setup[1], vel=setup[2])
         action = np.random.random(env.n()) * 0.1
-        np.testing.assert_array_almost_equal(ob['robot_0']['joint_state']['position'][0:2], setup[1][0:2], decimal=2)
+        np.testing.assert_array_almost_equal(ob['robot_0']['joint_state']['position'], setup[1], decimal=2)
         ob, *_ = env.step(action)
         assert isinstance(ob['robot_0'], dict)
         assert isinstance(ob['robot_0']['joint_state']['position'], np.ndarray)

urdfenvs/assets/panda/panda_scene_temp.xml

@@ -0,0 +1,24 @@
+<mujoco model="panda scene">
+
+  <include file="panda.xml" />
+
+  <statistic center="0.3 0 0.4" extent="1" />
+
+  <visual>
+    <headlight diffuse="0.6 0.6 0.6" ambient="0.3 0.3 0.3" specular="0 0 0" />
+    <rgba haze="0.15 0.25 0.35 1" />
+    <global azimuth="120" elevation="-20" />
+  </visual>
+
+  <asset>
+    <texture type="skybox" builtin="gradient" rgb1="0.3 0.5 0.7" rgb2="0 0 0" width="512" height="3072" />
+    <texture type="2d" name="groundplane" builtin="checker" mark="edge" rgb1="0.2 0.3 0.4" rgb2="0.1 0.2 0.3" markrgb="0.8 0.8 0.8" width="300" height="300" />
+    <material name="groundplane" texture="groundplane" texuniform="true" texrepeat="5 5" reflectance="0.2" />
+  </asset>
+
+  <worldbody>
+    <light pos="0 0 1.5" dir="0 0 -1" directional="true" />
+    <geom name="floor" size="0 0 0.05" type="plane" material="groundplane" />
+  <geom name="sphere_1" type="sphere" rgba="0.3 0.5 0.6 1.0" pos="2.0 2.0 1.0" size="0.5" /><geom name="sphere_2" type="sphere" rgba="0.0 0.0 0.0 1.0" pos="2.0 -0.0 0.0" size="0.1" /><geom name="cylinder_obstacle" type="cylinder" rgba="0.1 0.3 0.3 1.0" pos="2.0 -3.0 0.0" size="0.25 1.0" /><geom name="wall_0" type="box" rgba="0.0 0.0 0.0 1.0" pos="5.0 0.0 0.4" size="0.05 5.0 0.4" /><geom name="wall_1" type="box" rgba="0.0 0.0 0.0 1.0" pos="0.0 5.0 0.4" size="5.0 0.05 0.4" /><geom name="wall_2" type="box" rgba="0.0 0.0 0.0 1.0" pos="0.0 -5.0 0.4" size="5.0 0.05 0.4" /><geom name="wall_3" type="box" rgba="0.0 0.0 0.0 1.0" pos="-5.0 0.0 0.4" size="0.05 5.0 0.4" /></worldbody>
+
+</mujoco>

urdfenvs/urdf_common/bicycle_model.py

@@ -255,6 +255,8 @@ def correct_base_orientation(self, pos_base: np.ndarray) -> np.ndarray:
             pos_base[2] += np.pi
         if pos_base[2] < -np.pi:
             pos_base[2] += 2 * np.pi
+        if pos_base[2] > np.pi:
+            pos_base[2] -= 2 * np.pi
         return pos_base
 
     def update_state(self) -> None: