fix(sim): fix collision detection and improve gripper/robot reset

examples/so101/so101_env_cartesian_control.py

@@ -34,7 +34,7 @@ def main():
     gripper_cfg.min_joint_width = -0.17453292519943295
     gripper_cfg.max_joint_width = 1.7453292519943295
     gripper_cfg.actuator = "6"
-    gripper_cfg.joint = "6"
+    gripper_cfg.joints = ["6"]
     gripper_cfg.collision_geoms = []
     gripper_cfg.collision_geoms_fingers = []
 

examples/so101/so101_env_joint_control.py

@@ -35,7 +35,7 @@ def main():
     gripper_cfg.min_joint_width = -0.17453292519943295
     gripper_cfg.max_joint_width = 1.7453292519943295
     gripper_cfg.actuator = "6"
-    gripper_cfg.joint = "6"
+    gripper_cfg.joints = ["6"]
     gripper_cfg.collision_geoms = []
     gripper_cfg.collision_geoms_fingers = []
 

extensions/rcs_robotics_library/src/rcs_robotics_library/__init__.pyi

@@ -2,6 +2,4 @@ from __future__ import annotations
 
 from rcs_robotics_library._core import rl
 
-from . import _core
-
 __all__: list = ["rl"]

extensions/rcs_so101/src/rcs_so101/creators.py

@@ -14,8 +14,6 @@
 from rcs_so101 import SO101IK
 from rcs_so101.hw import SO101, SO101Config, SO101Gripper
 
-import rcs
-
 logger = logging.getLogger(__name__)
 logger.setLevel(logging.INFO)
 

python/rcs/_core/sim.pyi

@@ -118,6 +118,7 @@ class SimConfig:
 
 class SimGripper(rcs._core.common.Gripper):
     def __init__(self, sim: Sim, cfg: SimGripperConfig) -> None: ...
+    def clear_collision_flag(self) -> None: ...
     def get_config(self) -> SimGripperConfig: ...
     def get_state(self) -> SimGripperState: ...
     def set_config(self, cfg: SimGripperConfig) -> bool: ...
@@ -129,7 +130,7 @@ class SimGripperConfig(rcs._core.common.GripperConfig):
     epsilon_inner: float
     epsilon_outer: float
     ignored_collision_geoms: list[str]
-    joint: str
+    joints: list[str]
     max_actuator_width: float
     max_joint_width: float
     min_actuator_width: float
@@ -153,6 +154,7 @@ class SimRobot(rcs._core.common.Robot):
     def __init__(
         self, sim: Sim, ik: rcs._core.common.Kinematics, cfg: SimRobotConfig, register_convergence_callback: bool = True
     ) -> None: ...
+    def clear_collision_flag(self) -> None: ...
     def get_config(self) -> SimRobotConfig: ...
     def get_state(self) -> SimRobotState: ...
     def set_config(self, cfg: SimRobotConfig) -> bool: ...

python/rcs/envs/base.py

@@ -401,7 +401,7 @@ def __init__(
                 max_mov = self.DEFAULT_MAX_JOINT_MOV
             assert isinstance(
                 max_mov, float
-            ), "in cartesian control max_mov must be a float representing the maximum allowed rotation (in rad)."
+            ), "in joint control max_mov must be a float representing the maximum allowed rotation (in rad)."
             if max_mov > np.deg2rad(180):
                 _logger.warning(
                     "maximal movement is set higher to a value higher than 180 degree, which is really high, consider setting it lower"
@@ -687,7 +687,7 @@ class GripperWrapper(ActObsInfoWrapper):
     BINARY_GRIPPER_CLOSED = 0
     BINARY_GRIPPER_OPEN = 1
 
-    def __init__(self, env, gripper: common.Gripper, binary: bool = True, open_on_reset: bool = True):
+    def __init__(self, env, gripper: common.Gripper, binary: bool = True):
         super().__init__(env)
         self.unwrapped: RobotEnv
         self.observation_space: gym.spaces.Dict
@@ -698,16 +698,13 @@ def __init__(self, env, gripper: common.Gripper, binary: bool = True, open_on_re
         self.gripper = gripper
         self.binary = binary
         self._last_gripper_cmd = None
-        self.open_on_reset = open_on_reset
 
     def close(self):
         self.gripper.close()
         super().close()
 
     def reset(self, **kwargs) -> tuple[dict[str, Any], dict[str, Any]]:
-        if self.open_on_reset:
-            # resetting opens the gripper
-            self.gripper.reset()
+        self.gripper.reset()
         self._last_gripper_cmd = None
         return super().reset(**kwargs)
 

python/rcs/envs/creators.py

@@ -87,14 +87,6 @@ def __call__(  # type: ignore
 
         robot = rcs.sim.SimRobot(simulation, ik, robot_cfg)
         env: gym.Env = RobotEnv(robot, control_mode)
-        env = RobotSimWrapper(env, simulation, sim_wrapper)
-
-        if cameras is not None:
-            camera_set = typing.cast(
-                BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True)
-            )
-            env = CameraSetWrapper(env, camera_set, include_depth=True)
-
         assert not (
             hand_cfg is not None and gripper_cfg is not None
         ), "Hand and gripper configurations cannot be used together."
@@ -107,8 +99,20 @@ def __call__(  # type: ignore
         if gripper_cfg is not None and isinstance(gripper_cfg, rcs.sim.SimGripperConfig):
             gripper = sim.SimGripper(simulation, gripper_cfg)
             env = GripperWrapper(env, gripper, binary=True)
+        else:
+            gripper = None
+
+        env = RobotSimWrapper(env, simulation, sim_wrapper)
+
+        if gripper is not None:
             env = GripperWrapperSim(env, gripper)
 
+        if cameras is not None:
+            camera_set = typing.cast(
+                BaseCameraSet, SimCameraSet(simulation, cameras, physical_units=True, render_on_demand=True)
+            )
+            env = CameraSetWrapper(env, camera_set, include_depth=True)
+
         # TODO: collision guard not working atm
         # if collision_guard:
         #     env = CollisionGuard.env_from_xml_paths(

python/rcs/envs/sim.py

@@ -47,7 +47,7 @@ def __init__(self, env, simulation: sim.Sim, sim_wrapper: Type[SimWrapper] | Non
         self.frame_rate = SimpleFrameRate(1 / cfg.frequency, "RobotSimWrapper")
 
     def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, bool, dict]:
-        _, _, _, _, info = super().step(action)
+        obs, _, _, _, info = super().step(action)
         cfg = self.sim.get_config()
         if cfg.async_control:
             self.sim.step(round(1 / cfg.frequency / self.sim.model.opt.timestep))
@@ -56,23 +56,27 @@ def step(self, action: dict[str, Any]) -> tuple[dict[str, Any], float, bool, boo
                 self.frame_rate()
 
         else:
+            self.sim_robot.clear_collision_flag()
             self.sim.step_until_convergence()
         state = self.sim_robot.get_state()
-        info["collision"] = state.collision
+        if "collision" not in info:
+            info["collision"] = state.collision
+        else:
+            info["collision"] = info["collision"] or state.collision
         info["ik_success"] = state.ik_success
         info["is_sim_converged"] = self.sim.is_converged()
         # truncate episode if collision
-
-        return dict(self.unwrapped.get_obs()), 0, False, state.collision or not state.ik_success, info
+        obs.update(self.unwrapped.get_obs())
+        return obs, 0, False, info["collision"] or not state.ik_success, info
 
     def reset(
         self, seed: int | None = None, options: dict[str, Any] | None = None
     ) -> tuple[dict[str, Any], dict[str, Any]]:
         self.sim.reset()
-        _, info = super().reset(seed=seed, options=options)
-        # self.unwrapped.robot.move_home()
+        obs, info = super().reset(seed=seed, options=options)
         self.sim.step(1)
-        obs = cast(dict, self.unwrapped.get_obs())
+        # todo: an obs method that is recursive over wrappers would be needed
+        obs.update(self.unwrapped.get_obs())
         return obs, info
 
 
@@ -122,6 +126,10 @@ def __init__(self, env, gripper: sim.SimGripper):
         super().__init__(env)
         self._gripper = gripper
 
+    def action(self, action: dict[str, Any]) -> dict[str, Any]:
+        self._gripper.clear_collision_flag()
+        return action
+
     def observation(self, observation: dict[str, Any], info: dict[str, Any]) -> tuple[dict[str, Any], dict[str, Any]]:
         state = self._gripper.get_state()
         if "collision" not in info or not info["collision"]:

src/pybind/rcs.cpp

@@ -463,7 +463,7 @@ PYBIND11_MODULE(_core, m) {
                      &rcs::sim::SimGripperConfig::collision_geoms)
       .def_readwrite("collision_geoms_fingers",
                      &rcs::sim::SimGripperConfig::collision_geoms_fingers)
-      .def_readwrite("joint", &rcs::sim::SimGripperConfig::joint)
+      .def_readwrite("joints", &rcs::sim::SimGripperConfig::joints)
       .def_readwrite("max_joint_width",
                      &rcs::sim::SimGripperConfig::max_joint_width)
       .def_readwrite("min_joint_width",
@@ -513,6 +513,7 @@ PYBIND11_MODULE(_core, m) {
            py::arg("sim"), py::arg("cfg"))
       .def("get_config", &rcs::sim::SimGripper::get_config)
       .def("get_state", &rcs::sim::SimGripper::get_state)
+      .def("clear_collision_flag", &rcs::sim::SimGripper::clear_collision_flag)
       .def("set_config", &rcs::sim::SimGripper::set_config, py::arg("cfg"));
   py::class_<rcs::sim::SimRobot, rcs::common::Robot,
              std::shared_ptr<rcs::sim::SimRobot>>(sim, "SimRobot")
@@ -525,6 +526,7 @@ PYBIND11_MODULE(_core, m) {
       .def("set_config", &rcs::sim::SimRobot::set_config, py::arg("cfg"))
       .def("set_joints_hard", &rcs::sim::SimRobot::set_joints_hard,
            py::arg("q"))
+      .def("clear_collision_flag", &rcs::sim::SimRobot::clear_collision_flag)
       .def("get_state", &rcs::sim::SimRobot::get_state);
 
   // SimTilburgHandState

src/sim/SimGripper.cpp

@@ -11,20 +11,22 @@ namespace rcs {
 namespace sim {
 
 SimGripper::SimGripper(std::shared_ptr<Sim> sim, const SimGripperConfig &cfg)
-    : sim{sim}, cfg{cfg} {
+    : sim{sim}, cfg{cfg}, joint_ids{} {
   this->state = SimGripperState();
   this->actuator_id =
       mj_name2id(this->sim->m, mjOBJ_ACTUATOR, this->cfg.actuator.c_str());
   if (this->actuator_id == -1) {
     throw std::runtime_error(
         std::string("No actuator named " + this->cfg.actuator));
   }
-  // this->tendon_id =
-  //     mj_name2id(this->sim->m, mjOBJ_TENDON, ("split_" + id).c_str());
-  this->joint_id = this->sim->m->jnt_qposadr[mj_name2id(
-      this->sim->m, mjOBJ_JOINT, this->cfg.joint.c_str())];
-  if (this->joint_id == -1) {
-    throw std::runtime_error(std::string("No joint named " + this->cfg.joint));
+  std::string name;
+  for (size_t i = 0; i < std::size(this->cfg.joints); ++i) {
+    name = this->cfg.joints[i];
+    this->joint_ids.push_back(
+        mj_name2id(this->sim->m, mjOBJ_JOINT, name.c_str()));
+    if (this->joint_ids[i] == -1) {
+      throw std::runtime_error(std::string("No joint named " + name));
+    }
   }
   // Collision geoms
   this->add_collision_geoms(this->cfg.collision_geoms, this->cgeom, false);
@@ -93,8 +95,11 @@ void SimGripper::set_normalized_width(double width, double force) {
 double SimGripper::get_normalized_width() {
   // TODO: maybe we should use the mujoco sensors? Not sure what the difference
   // is between reading out from qpos and reading from the sensors.
+  if (this->joint_ids.size() == 0) {
+    throw std::invalid_argument("No joint ids for gripper available.");
+  }
   double width =
-      (this->sim->d->qpos[this->joint_id] - this->cfg.min_joint_width) /
+      (this->sim->d->qpos[this->joint_ids[0]] - this->cfg.min_joint_width) /
       (this->cfg.max_joint_width - this->cfg.min_joint_width);
   // sometimes the joint is slightly outside of the bounds
   if (width < 0) {
@@ -106,7 +111,6 @@ double SimGripper::get_normalized_width() {
 }
 
 bool SimGripper::collision_callback() {
-  this->state.collision = false;
   for (size_t i = 0; i < this->sim->d->ncon; ++i) {
     if (this->cfgeom.contains(this->sim->d->contact[i].geom[0]) &&
         this->cfgeom.contains(this->sim->d->contact[i].geom[1])) {
@@ -119,7 +123,7 @@ bool SimGripper::collision_callback() {
         // ignore all collision with ignored objects with frankahand
         // not just fingers
         not(this->ignored_collision_geoms.contains(
-                this->sim->d->contact[i].geom[1]) or
+                this->sim->d->contact[i].geom[0]) or
             this->ignored_collision_geoms.contains(
                 this->sim->d->contact[i].geom[1]))) {
       this->state.collision = true;
@@ -129,6 +133,8 @@ bool SimGripper::collision_callback() {
   return this->state.collision;
 }
 
+void SimGripper::clear_collision_flag() { this->state.collision = false; }
+
 bool SimGripper::is_grasped() {
   double width = this->get_normalized_width();
 
@@ -158,7 +164,10 @@ void SimGripper::shut() { this->set_normalized_width(0); }
 void SimGripper::m_reset() {
   this->state = SimGripperState();
   // reset state hard
-  this->sim->d->qpos[this->joint_id] = this->cfg.max_joint_width;
+  this->state.last_width = 1.0;
+  for (size_t i = 0; i < this->joint_ids.size(); ++i) {
+    this->sim->d->qpos[this->joint_ids[i]] = this->cfg.max_joint_width;
+  }
   this->sim->d->ctrl[this->actuator_id] = this->cfg.max_actuator_width;
 }
 

src/sim/SimGripper.h

@@ -26,7 +26,7 @@ struct SimGripperConfig : common::GripperConfig {
 
   std::vector<std::string> collision_geoms_fingers{"finger_0_left",
                                                    "finger_0_right"};
-  std::string joint = "finger_joint1";
+  std::vector<std::string> joints = {"finger_joint1", "finger_joint2"};
   std::string actuator = "actuator8";
 
   void add_id(const std::string &id) {
@@ -39,7 +39,9 @@ struct SimGripperConfig : common::GripperConfig {
     for (auto &s : this->ignored_collision_geoms) {
       s = s + "_" + id;
     }
-    this->joint = this->joint + "_" + id;
+    for (auto &s : this->joints) {
+      s = s + "_" + id;
+    }
     this->actuator = this->actuator + "_" + id;
   }
 };
@@ -56,7 +58,7 @@ class SimGripper : public common::Gripper {
   SimGripperConfig cfg;
   std::shared_ptr<Sim> sim;
   int actuator_id;
-  int joint_id;
+  std::vector<int> joint_ids;
   SimGripperState state;
   bool convergence_callback();
   bool collision_callback();
@@ -88,6 +90,7 @@ class SimGripper : public common::Gripper {
   void grasp() override;
   void open() override;
   void shut() override;
+  void clear_collision_flag();
   void close() override {};
 };
 }  // namespace sim

src/sim/SimRobot.cpp

@@ -131,6 +131,10 @@ void SimRobot::set_joint_position(const common::VectorXd& q) {
 }
 
 common::VectorXd SimRobot::get_joint_position() {
+  return m_get_joint_position();
+}
+
+common::VectorXd SimRobot::m_get_joint_position() {
   common::VectorXd q(std::size(this->cfg.joints));
   for (size_t i = 0; i < std::size(this->cfg.joints); ++i) {
     q[i] = this->sim->d->qpos[this->sim->m->jnt_qposadr[this->ids.joints[i]]];
@@ -170,7 +174,6 @@ void SimRobot::is_arrived_callback() {
 }
 
 bool SimRobot::collision_callback() {
-  this->state.collision = false;
   for (size_t i = 0; i < this->sim->d->ncon; ++i) {
     if (this->ids.cgeom.contains(this->sim->d->contact[i].geom[0]) ||
         this->ids.cgeom.contains(this->sim->d->contact[i].geom[1])) {
@@ -181,6 +184,8 @@ bool SimRobot::collision_callback() {
   return this->state.collision;
 }
 
+void SimRobot::clear_collision_flag() { this->state.collision = false; }
+
 bool SimRobot::convergence_callback() {
   /* When ik failed, the robot is not doing anything */
   if (not this->state.ik_success) {
@@ -191,8 +196,13 @@ bool SimRobot::convergence_callback() {
 }
 
 void SimRobot::m_reset() {
+  this->state = SimRobotState();
   this->set_joints_hard(
       common::robots_meta_config.at(this->cfg.robot_type).q_home);
+  this->state.previous_angles = this->m_get_joint_position();
+  this->state.target_angles = this->m_get_joint_position();
+  this->state.is_arrived = true;
+  this->state.is_moving = false;
 }
 
 void SimRobot::set_joints_hard(const common::VectorXd& q) {

src/sim/SimRobot.h

@@ -74,6 +74,7 @@ class SimRobot : public common::Robot {
   std::optional<std::shared_ptr<common::Kinematics>> get_ik() override;
   void reset() override;
   void set_joints_hard(const common::VectorXd &q);
+  void clear_collision_flag();
   void close() override {};
 
  private:
@@ -95,6 +96,7 @@ class SimRobot : public common::Robot {
   void init_ids();
   void construct();
   void m_reset();
+  common::VectorXd m_get_joint_position();
 };
 }  // namespace sim
 }  // namespace rcs