fix(converter): skip identity and ik failure steps

Author: juelg

python/rcs/lerobot_joint_converter.py

@@ -13,6 +13,7 @@
 from rcs._core.common import GripperType, RobotType
 from torchvision.io import decode_jpeg
 from torchvision.transforms import v2
+import warnings
 
 import rcs
 
@@ -309,8 +310,9 @@ def _convert_action_to_joint_space(self, row: pd.Series) -> np.ndarray:
                     target_pose, observation_joints_vec, tcp_offset=self.tcp_offset
                 )
                 if ik_joints is None:
-                    msg = f"IK failed for robot '{robot_key}' at step {row['step']}"
-                    raise ValueError(msg)
+                    msg = f"IK failed for robot '{robot_key}' at step {row['step']}, ignoring step"
+                    warnings.warn(msg)
+                    return None
                 arm_action_vec = np.asarray(ik_joints, dtype=np.float32)
 
             actions.append(np.concatenate([arm_action_vec, action_gripper_vec]).astype(np.float32))
@@ -329,15 +331,19 @@ def _prepare_transition_table(self, table: pd.DataFrame) -> pd.DataFrame:
         df["observation_state"] = df.apply(self._build_observation_state, axis=1)
         df["action_vector"] = df.apply(self._convert_action_to_joint_space, axis=1)
 
+        df = df[df["action_vector"].notna()]
+
         prev_action: np.ndarray | None = None
         keep_mask = []
         for action_vec in df["action_vector"]:
             assert isinstance(action_vec, np.ndarray)
-            keep_mask.append(prev_action is None or not np.array_equal(action_vec, prev_action))
+            keep_mask.append(prev_action is None or not np.allclose(action_vec, prev_action, atol=1e-4, rtol=0))
             prev_action = action_vec
 
         return df.loc[keep_mask].reset_index(drop=True)
 
+
+
     def parse_episode(self, episode_id: str, table: pd.DataFrame, success: bool):
         table = self._prepare_transition_table(table)
         if len(table) == 0: