From 7fcc06a65dbbe87d1c07be7b53ca96c8db1ab0d1 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Anton=20Bj=C3=B8rndahl=20Mortensen?=
<56405924+abmoRobotics@users.noreply.github.com>
Date: Mon, 22 Jan 2024 10:46:39 +0100
Subject: [PATCH] Fixes the tensor shape for contact sensor's force matrix data
(#195)
# Description
This MR changed the shape of
`omni.isaac.orbit.sensors.contact_sensor.ContactSensorData.force_matrix_w`
in
[ContactSensor#L215-L217](https://github.com/NVIDIA-Omniverse/orbit/blob/963f3045367be3ccdac4f0902ac2103fcbb6070f/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/contact_sensor/contact_sensor.py#L215-L217)
from `(num_envs, num_bodies, num_shapes, num_filters, 3)` to `(num_envs,
num_bodies, num_filters, 3)`. This makes the returned tensor shape valid
with the data obtained from PhysX.
Fixes #193
## Type of change
- Bug fix (non-breaking change which fixes an issue)
## Videos
I have implemented this in an ORBIT environment and now get the expected
behaviour
Before | After (resets only the robot collides with obstacles)
--|--
|
## Checklist
- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./orbit.sh --format`
- [ ] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [ ] I have run all the tests with `./orbit.sh --test` and they pass
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there
---------
Signed-off-by: Mayank Mittal <12863862+Mayankm96@users.noreply.github.com>
Co-authored-by: Mayank Mittal <12863862+Mayankm96@users.noreply.github.com>
---
.../extensions/omni.isaac.orbit/config/extension.toml | 2 +-
source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst | 10 ++++++++++
.../orbit/sensors/contact_sensor/contact_sensor.py | 10 ++++------
.../sensors/contact_sensor/contact_sensor_data.py | 4 ++--
4 files changed, 17 insertions(+), 9 deletions(-)
diff --git a/source/extensions/omni.isaac.orbit/config/extension.toml b/source/extensions/omni.isaac.orbit/config/extension.toml
index c17ef75b44..c5fc4eeb73 100644
--- a/source/extensions/omni.isaac.orbit/config/extension.toml
+++ b/source/extensions/omni.isaac.orbit/config/extension.toml
@@ -1,7 +1,7 @@
[package]
# Note: Semantic Versioning is used: https://semver.org/
-version = "0.10.13"
+version = "0.10.14"
# Description
title = "ORBIT framework for Robot Learning"
diff --git a/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
index 7d3e42ca85..8d94ae9a09 100644
--- a/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
+++ b/source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
@@ -1,6 +1,16 @@
Changelog
---------
+0.10.14 (2024-01-22)
+~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed the tensor shape of :attr:`omni.isaac.orbit.sensors.ContactSensorData.force_matrix_w`. Earlier, the reshaping
+ led to a mismatch with the data obtained from PhysX.
+
+
0.10.13 (2024-01-15)
~~~~~~~~~~~~~~~~~~~~
diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/contact_sensor/contact_sensor.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/contact_sensor/contact_sensor.py
index 91b7ee1a15..44a04fb68d 100644
--- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/contact_sensor/contact_sensor.py
+++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/contact_sensor/contact_sensor.py
@@ -208,12 +208,11 @@ def _initialize_impl(self):
if self.cfg.track_air_time:
self._data.last_air_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device)
self._data.current_air_time = torch.zeros(self._num_envs, self._num_bodies, device=self._device)
- # force matrix: (num_sensors, num_bodies, num_shapes, num_filter_shapes, 3)
+ # force matrix: (num_envs, num_bodies, num_filter_shapes, 3)
if len(self.cfg.filter_prim_paths_expr) != 0:
- num_shapes = self.contact_physx_view.sensor_count // self._num_bodies
num_filters = self.contact_physx_view.filter_count
self._data.force_matrix_w = torch.zeros(
- self._num_envs, self._num_bodies, num_shapes, num_filters, 3, device=self._device
+ self._num_envs, self._num_bodies, num_filters, 3, device=self._device
)
def _update_buffers_impl(self, env_ids: Sequence[int]):
@@ -234,12 +233,11 @@ def _update_buffers_impl(self, env_ids: Sequence[int]):
# obtain the contact force matrix
if len(self.cfg.filter_prim_paths_expr) != 0:
- # shape of the filtering matrix: (num_sensors, num_bodies, num_shapes, num_filter_shapes, 3)
- num_shapes = self.contact_physx_view.sensor_count // self._num_bodies
+ # shape of the filtering matrix: (num_envs, num_bodies, num_filter_shapes, 3)
num_filters = self.contact_physx_view.filter_count
# acquire and shape the force matrix
force_matrix_w = self.contact_physx_view.get_contact_force_matrix(dt=self._sim_physics_dt)
- force_matrix_w = force_matrix_w.view(-1, self._num_bodies, num_shapes, num_filters, 3)
+ force_matrix_w = force_matrix_w.view(-1, self._num_bodies, num_filters, 3)
self._data.force_matrix_w[env_ids] = force_matrix_w[env_ids]
# obtain the pose of the sensor origin
if self.cfg.track_pose:
diff --git a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/contact_sensor/contact_sensor_data.py b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/contact_sensor/contact_sensor_data.py
index 39a7ed836a..d4fd7cbe93 100644
--- a/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/contact_sensor/contact_sensor_data.py
+++ b/source/extensions/omni.isaac.orbit/omni/isaac/orbit/sensors/contact_sensor/contact_sensor_data.py
@@ -49,8 +49,8 @@ class ContactSensorData:
force_matrix_w: torch.Tensor | None = None
"""The contact forces filtered between the sensor bodies and filtered bodies in world frame.
- Shape is (N, B, S, M, 3), where N is the number of sensors, B is number of bodies in each sensor,
- ``S`` is number of shapes per body and ``M`` is the number of filtered bodies.
+ Shape is (N, B, M, 3), where N is the number of sensors, B is number of bodies in each sensor
+ and ``M`` is the number of filtered bodies.
Note:
If the :attr:`ContactSensorCfg.filter_prim_paths_expr` is empty, then this quantity is None.