From 39f044cd1bacf12972317648a0de489e7f57ae6f Mon Sep 17 00:00:00 2001 From: Max Date: Wed, 6 Dec 2023 19:37:22 +0100 Subject: [PATCH 1/3] fix[scene]: Adds orientation to add_obstacle. Fixes base position when adding box obstacles. --- urdfenvs/scene_examples/obstacles.py | 1 + urdfenvs/urdf_common/helpers.py | 6 +++--- urdfenvs/urdf_common/urdf_env.py | 9 ++++++--- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/urdfenvs/scene_examples/obstacles.py b/urdfenvs/scene_examples/obstacles.py index b1cc1ad9..0efc523d 100644 --- a/urdfenvs/scene_examples/obstacles.py +++ b/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, diff --git a/urdfenvs/urdf_common/helpers.py b/urdfenvs/urdf_common/helpers.py index 31c127e5..7d428beb 100644 --- a/urdfenvs/urdf_common/helpers.py +++ b/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 ) diff --git a/urdfenvs/urdf_common/urdf_env.py b/urdfenvs/urdf_common/urdf_env.py index 8493a5f0..786f634f 100644 --- a/urdfenvs/urdf_common/urdf_env.py +++ b/urdfenvs/urdf_common/urdf_env.py @@ -320,7 +320,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: @@ -385,7 +386,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 @@ -400,7 +402,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) From 2576da07fba2ff7c0b3d90983fa4a9115b775b87 Mon Sep 17 00:00:00 2001 From: Max Date: Wed, 6 Dec 2023 19:47:00 +0100 Subject: [PATCH 2/3] ft[sensors]: Adds orientation observation to full sensor. --- examples/point_robot_full_sensor.py | 2 +- urdfenvs/sensors/full_sensor.py | 9 +++++++++ 2 files changed, 10 insertions(+), 1 deletion(-) diff --git a/examples/point_robot_full_sensor.py b/examples/point_robot_full_sensor.py index 2c54cfe0..088caab9 100644 --- a/examples/point_robot_full_sensor.py +++ b/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() diff --git a/urdfenvs/sensors/full_sensor.py b/urdfenvs/sensors/full_sensor.py index 090e9c8e..e8229ed0 100644 --- a/urdfenvs/sensors/full_sensor.py +++ b/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) From a9c69de3be95e56bcf658bfad6433a076bc57931 Mon Sep 17 00:00:00 2001 From: Max Date: Wed, 6 Dec 2023 19:48:32 +0100 Subject: [PATCH 3/3] Bumps version. --- pyproject.toml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/pyproject.toml b/pyproject.toml index c2585c1e..4f3f772e 100644 --- a/pyproject.toml +++ b/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 "] maintainers = [