Fix orientation

examples/point_robot_full_sensor.py

@@ -25,7 +25,7 @@ def run_point_robot_with_full_sensor(n_steps=10, render=False, obstacles=True, g
     env.add_goal(goal1)
 
     # add sensor
-    sensor = FullSensor(['position'], ['position', 'size', 'type'], variance=0.0)
+    sensor = FullSensor(['position'], ['position', 'size', 'type', 'orientation'], variance=0.0)
     env.add_sensor(sensor, [0])
     # Set spaces AFTER all components have been added.
     env.set_spaces()

pyproject.toml

@@ -1,6 +1,6 @@
 [tool.poetry]
 name = "urdfenvs"
-version = "0.8.17"
+version = "0.8.18"
 description = "Simple simulation environment for robots, based on the urdf files."
 authors = ["Max Spahn <m.spahn@tudelft.nl>"]
 maintainers = [

urdfenvs/scene_examples/obstacles.py

@@ -56,6 +56,7 @@
     'type': 'box',
     'geometry': {
         'position' : [2.0, 0.0, 2.0],
+        'orientation': [0.923, 0, 0, -0.38],
         'width': 0.2,
         'height': 0.2,
         'length': 0.2,

urdfenvs/sensors/full_sensor.py

@@ -29,6 +29,12 @@ def get_observation_space(self, obstacles: dict, goals: dict):
                     high=np.array([50, 50, 50]),
                     dtype=float,
                 )
+            if "orientation" in self._obstacle_mask:
+                observation_space_obstacle["orientation"] = spaces.Box(
+                    low=np.array([-1, -1, -1, -1]),
+                    high=np.array([1, 1, 1, 1]),
+                    dtype=float,
+                )
             if "velocity" in self._obstacle_mask:
                 observation_space_obstacle["velocity"] = spaces.Box(
                     low=np.array([-50, -50, -50]),
@@ -111,6 +117,9 @@ def sense(self, robot, obstacles: dict, goals: dict, t: float):
             for mask_item in self._obstacle_mask:
                 if mask_item == "position":
                     value, _ = p.getBasePositionAndOrientation(obst_id)
+                elif mask_item == "orientation":
+                    _, value_raw = p.getBasePositionAndOrientation(obst_id)
+                    value = value_raw[3:] + value_raw[:3]
 
                 elif mask_item == "velocity":
                     value, _ = p.getBaseVelocity(obst_id)

urdfenvs/urdf_common/helpers.py

@@ -199,9 +199,9 @@ def add_shape(
     if color is None:
         color = [1.0, 1.0, 1.0, 1.0]
     if orientation is None:
-        base_orientation = (0.0, 0.0, 0.0, 1.0)
+        base_orientation = [0.0, 0.0, 0.0, 1.0]
     else:
-        base_orientation = orientation
+        base_orientation = orientation[1:] + orientation[:1]
     if position is None:
         base_position = (0.0, 0.0, 0.0)
     else:
@@ -219,7 +219,7 @@ def add_shape(
 
     elif shape_type == "box":
         half_extens = [s / 2 for s in size]
-        base_position = tuple(base_position[i] - size[i] for i in range(3))
+        base_position = tuple(base_position[i] for i in range(3))
         shape_id = pybullet.createCollisionShape(
             pybullet.GEOM_BOX, halfExtents=half_extens
         )

urdfenvs/urdf_common/urdf_env.py

@@ -321,7 +321,8 @@ def update_obstacles(self):
             try:
                 pos = obst.position(t=self.t()).tolist()
                 vel = obst.velocity(t=self.t()).tolist()
-                ori = [0, 0, 0, 1]
+                ori = obst.orientation(t=self.t()).tolist()
+                ori = ori[1:] + ori[:1]
                 p.resetBasePositionAndOrientation(obst_id, pos, ori)
                 p.resetBaseVelocity(obst_id, linearVelocity=vel)
             except Exception:
@@ -386,7 +387,8 @@ def add_obstacle(self, obst: CollisionObstacle) -> None:
                 obst.type(),
                 obst.size(),
                 obst.rgba().tolist(),
-                position=obst.position(),
+                position=obst.position().tolist(),
+                orientation=obst.orientation().tolist(),
                 movable=obst.movable(),
             )
         self._obsts[obst_id] = obst
@@ -401,7 +403,8 @@ def reset_obstacles(self) -> None:
             else:
                 pos = obstacle.position(t=0).tolist()
                 vel = obstacle.velocity(t=0).tolist()
-            ori = [0, 0, 0, 1]
+                ori = obstacle.orientation(t=0).tolist()
+            ori = ori[1:] + ori[:1]
             p.resetBasePositionAndOrientation(obst_id, pos, ori)
             p.resetBaseVelocity(obst_id, linearVelocity=vel)