fix: avoid numpy truth ambiguity in pick task

Author: juelg

python/rcs/envs/tasks.py

@@ -37,11 +37,9 @@ def __init__(self, env, robot_name: str, shared2world: rcs.common.Pose, obj_join
     def step(self, action: dict[str, Any]):  # type: ignore
         obs, reward, _, truncated, info = super().step(action)
 
-        if (
-            self._gripper.get_normalized_width() > 0.01
-            and self._gripper.get_normalized_width() < 0.99
-            and obs[self.robot_name]["gripper"] == GripperWrapper.BINARY_GRIPPER_CLOSED
-        ):
+        gripper_closed = obs[self.robot_name]["gripper"][0] == GripperWrapper.BINARY_GRIPPER_CLOSED[0]
+
+        if self._gripper.get_normalized_width() > 0.01 and self._gripper.get_normalized_width() < 0.99 and gripper_closed:
             self._gripper_closing += 1
         else:
             self._gripper_closing = 0
@@ -61,7 +59,7 @@ def step(self, action: dict[str, Any]):  # type: ignore
         # NOTE: 4 depends on the time passing between each step.
         is_grasped = (
             self._gripper_closing >= 4  # gripper is closing since more than 4 steps
-            and obs[self.robot_name]["gripper"] == GripperWrapper.BINARY_GRIPPER_CLOSED  # command is still close
+            and gripper_closed  # command is still close
             and tcp_to_obj_dist <= 0.01  # tcp to cube center is max 1cm
         )
         success = obj_to_goal_dist <= 0.022 and info[self.robot_name]["is_grasped"]

python/tests/test_pick_task_wrapper.py

@@ -0,0 +1,60 @@
+import warnings
+
+import gymnasium as gym
+import numpy as np
+
+import rcs
+from rcs.envs.tasks import PickObjSuccessWrapper
+
+
+class _DummyJoint:
+    def __init__(self):
+        self.qpos = np.array([0.0, 0.0, 0.0])
+
+
+class _DummyData:
+    def __init__(self):
+        self._joint = _DummyJoint()
+        self.qvel = np.zeros(1)
+
+    def joint(self, _name: str):
+        return self._joint
+
+
+class _DummySim:
+    def __init__(self):
+        self.data = _DummyData()
+
+
+class _DummyGripper:
+    def get_normalized_width(self):
+        return 0.5
+
+
+class _DummyEnv(gym.Env):
+    def __init__(self):
+        super().__init__()
+        self.sim = _DummySim()
+        self.gripper = {"robot": _DummyGripper()}
+
+    def get_wrapper_attr(self, name: str):
+        return getattr(self, name)
+
+    def step(self, _action):
+        obs = {"robot": {"gripper": np.array([0.0]), "tquat": np.zeros(7)}}
+        info = {"robot": {"is_grasped": False}}
+        return obs, 0.0, False, False, info
+
+    def reset(self, *, seed=None, options=None):
+        super().reset(seed=seed)
+        return {}, {}
+
+
+def test_pick_obj_success_wrapper_step_avoids_numpy_truth_ambiguity():
+    wrapper = PickObjSuccessWrapper(_DummyEnv(), "robot", rcs.common.Pose())
+
+    with warnings.catch_warnings(record=True) as caught:
+        warnings.simplefilter("error", DeprecationWarning)
+        wrapper.step({})
+
+    assert not caught