fix[urdf_env]: Also consider the initial orientation.

Author: Gralerfics

urdfenvs/urdf_common/urdf_env.py

@@ -396,8 +396,8 @@ def reset_obstacles(self) -> None:
         for obst_id, obstacle in self._obsts.items():
             if obstacle.type() == "urdf":
                 pos = obstacle.position()
-                vel = obstacle.velocity()
-                ori = [1.0, 0.0, 0.0, 0.0]
+                vel = obstacle.velocity() # [0.0, 0.0, 0.0]
+                ori = obstacle.orientation().tolist()
             else:
                 pos = obstacle.position(t=0).tolist()
                 vel = obstacle.velocity(t=0).tolist()