Skip to content

Commit

Permalink
Fixes the tensor shape for contact sensor's force matrix data (#195)
Browse files Browse the repository at this point in the history
# 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) 
--|--
<video
src="https://github.com/NVIDIA-Omniverse/orbit/assets/56405924/96dee42f-d84b-4a52-9480-77e800ab7f23"
width="600" />|<video
src="https://github.com/NVIDIA-Omniverse/orbit/assets/56405924/4112a941-87e2-43fd-a63b-19154cf6040f"
width="600" />

## 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>
  • Loading branch information
abmoRobotics and Mayankm96 authored Jan 22, 2024
1 parent 052c277 commit 7fcc06a
Show file tree
Hide file tree
Showing 4 changed files with 17 additions and 9 deletions.
2 changes: 1 addition & 1 deletion source/extensions/omni.isaac.orbit/config/extension.toml
Original file line number Diff line number Diff line change
@@ -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"
Expand Down
10 changes: 10 additions & 0 deletions source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst
Original file line number Diff line number Diff line change
@@ -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)
~~~~~~~~~~~~~~~~~~~~

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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]):
Expand All @@ -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:
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down

0 comments on commit 7fcc06a

Please sign in to comment.