From cf5ef2fcf4718c0719fad7229405403b73e9b604 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Sun, 25 Aug 2024 13:04:10 +0200 Subject: [PATCH 1/2] fix: sim gripper collision in state added gripper collision to sim franka hand state and to info dict in gripper env This is a hot fix to allow the collision tests to pass. In the future we should follow the implementation specific wrapper approach from FR3, where sim and hw specific functionalities are put into its own wrapper. --- python/rcsss/_core/sim.pyi | 6 ++++++ python/rcsss/envs/base.py | 8 ++++++++ src/pybind/rcsss.cpp | 8 +++++++- 3 files changed, 21 insertions(+), 1 deletion(-) diff --git a/python/rcsss/_core/sim.pyi b/python/rcsss/_core/sim.pyi index b4230dbe..7dfdf176 100644 --- a/python/rcsss/_core/sim.pyi +++ b/python/rcsss/_core/sim.pyi @@ -74,8 +74,14 @@ class FHConfig(rcsss._core.common.GConfig): class FHState(rcsss._core.common.GState): def __init__(self) -> None: ... @property + def collision(self) -> bool: ... + @property + def is_moving(self) -> bool: ... + @property def last_commanded_width(self) -> float: ... @property + def last_width(self) -> float: ... + @property def max_unnormalized_width(self) -> float: ... class FR3(rcsss._core.common.Robot): diff --git a/python/rcsss/envs/base.py b/python/rcsss/envs/base.py index 280836d9..86493d98 100644 --- a/python/rcsss/envs/base.py +++ b/python/rcsss/envs/base.py @@ -17,6 +17,8 @@ get_space_keys, ) +from python.rcsss import sim + class TRPYDictType(RCSpaceType): """Pose format is in transpose[3],r,p,y""" @@ -413,6 +415,12 @@ def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]: def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]: observation = copy.deepcopy(observation) observation[self.gripper_key] = self._gripper.get_normalized_width() + + # TODO: a cleaner solution would be to put this code into env/sim.py + # similar to sim fr3 has also a sim specific wrapper + if isinstance(self._gripper, sim.FrankaHand): + state = self._gripper.get_state() + info["collision"] = state.collision if self.binary: observation[self.gripper_key] = round(observation[self.gripper_key]) return observation, info diff --git a/src/pybind/rcsss.cpp b/src/pybind/rcsss.cpp index a23d8f48..e07f680e 100644 --- a/src/pybind/rcsss.cpp +++ b/src/pybind/rcsss.cpp @@ -424,7 +424,13 @@ PYBIND11_MODULE(_core, m) { .def_readonly("last_commanded_width", &rcs::sim::FHState::last_commanded_width) .def_readonly("max_unnormalized_width", - &rcs::sim::FHState::max_unnormalized_width); + &rcs::sim::FHState::max_unnormalized_width) + .def_readonly("is_moving", + &rcs::sim::FHState::is_moving) + .def_readonly("last_width", + &rcs::sim::FHState::last_width) + .def_readonly("collision", + &rcs::sim::FHState::collision); py::class_>(sim, "Sim") .def(py::init([](long m, long d) { From 0b06cb3497614d25020392480f6e29675b284187 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Tobias=20J=C3=BClg?= Date: Sun, 25 Aug 2024 13:08:05 +0200 Subject: [PATCH 2/2] style(cpp): formatting --- src/pybind/rcsss.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/src/pybind/rcsss.cpp b/src/pybind/rcsss.cpp index e07f680e..af9a308f 100644 --- a/src/pybind/rcsss.cpp +++ b/src/pybind/rcsss.cpp @@ -425,12 +425,9 @@ PYBIND11_MODULE(_core, m) { &rcs::sim::FHState::last_commanded_width) .def_readonly("max_unnormalized_width", &rcs::sim::FHState::max_unnormalized_width) - .def_readonly("is_moving", - &rcs::sim::FHState::is_moving) - .def_readonly("last_width", - &rcs::sim::FHState::last_width) - .def_readonly("collision", - &rcs::sim::FHState::collision); + .def_readonly("is_moving", &rcs::sim::FHState::is_moving) + .def_readonly("last_width", &rcs::sim::FHState::last_width) + .def_readonly("collision", &rcs::sim::FHState::collision); py::class_>(sim, "Sim") .def(py::init([](long m, long d) {