Claude/fix ur7e robot connection zwkk5

.gitignore

@@ -1,7 +1,5 @@
-crisp_py/__pycache__
-crisp_py/control/__pycache__
-crisp_py/gripper/__pycache__
-crisp_py/camera/__pycache__
+__pycache__/
+*.pyc
 .ros_env.sh
 crisp_py.egg-info
 .pypirc

CLAUDE.md

@@ -0,0 +1,10 @@
+For the first question I ask always:
+- Prepare a plan of potential changes
+- DO NOT CHANGE ANYTHING EXCEPT SPECIFICALLY SPECIFIED
+- Ask questions on how to proceed always
+
+Before changing anything in this repo: READ HANDOFF.md in the repo root.
+It documents the rot6d convention in utils/geometry.py, config-field parity
+rules, the agreed registry-based extensibility direction, known bugs, and the
+ROS-coupling map. Changes here ripple into crisp_gym — see also
+crisp_gym/HANDOFF.md for the data pipeline conventions.

Dockerfile

@@ -0,0 +1,43 @@
+# crisp_py development image (mirrors crisp_gym/Dockerfile).
+#
+# This is a DEV image: it ships only the pixi runtime and the system libraries
+# the ROS 2 / visualization stack needs. The repo itself is bind-mounted at
+# runtime (see docker-compose.yml) and the pixi environment is created on first
+# container start with `pixi install -e humble`. That keeps the image small and
+# lets you edit crisp_py on the host with changes visible immediately.
+
+FROM ghcr.io/prefix-dev/pixi:0.63.2-jammy
+
+USER root
+
+# X11 + OpenGL runtime libs for the visualization tools crisp_py pulls in
+# (viser, yourdfpy, matplotlib). No ffmpeg/libav here — crisp_py has no video
+# pipeline (that set lives in crisp_gym for lerobot).
+RUN apt-get update && apt-get install -y --no-install-recommends \
+    libx11-6 \
+    libxext6 \
+    libxrender1 \
+    libxtst6 \
+    libxi6 \
+    libxkbcommon-x11-0 \
+    libgl1-mesa-glx \
+    libgl1-mesa-dri \
+    && rm -rf /var/lib/apt/lists/*
+
+WORKDIR /workspace/crisp_py
+
+# Pixi metadata drives dependency resolution. These are shadowed by the
+# workspace bind-mount at runtime, but copying them lets `docker build` cache a
+# layer and (optionally) pre-resolve the env if you uncomment the install below.
+COPY pixi.toml pixi.lock* ./
+
+# Optional: bake the environment into the image instead of installing at
+# container start. Leave commented to match the crisp_gym dev-mount workflow.
+# COPY pyproject.toml ./
+# COPY crisp_py ./crisp_py
+# COPY scripts ./scripts
+# RUN pixi install -e humble
+
+# Safe interactive entrypoint (same as crisp_gym).
+ENTRYPOINT ["/bin/bash", "-c", "exec bash"]
+CMD []

HANDOFF.md

@@ -0,0 +1,99 @@
+# Development Handoff Notes — crisp_py
+
+Instructions for future AI-assisted development sessions on this repo.
+crisp_py is the ROS2 client library (robots, grippers, cameras, sensors,
+controller clients) consumed by `crisp_gym`. Changes here ripple into
+crisp_gym's recording and deployment pipelines — read `crisp_gym/HANDOFF.md`
+too before touching anything pose- or gripper-related.
+
+---
+
+## 1. Things that must not change silently
+
+### 1.1 OrientationRepresentation (`crisp_py/utils/geometry.py`)
+- Members: `QUATERNION`, `EULER`, `ANGLE_AXIS`, `ROTATION_6D` ("rotation_6d").
+- `ROTATION_6D` encodes the **first two ROWS of the rotation matrix, flattened
+  row-major** (UMI / pytorch3d convention). NOT columns. Decoding requires
+  Gram-Schmidt. crisp_gym's recorded datasets and trained models depend on
+  this exact convention — changing it invalidates existing data/checkpoints.
+- crisp_gym's test suite stubs this enum
+  (`crisp_gym/tests/test_lerobot_record.py::_OrientationRepresentation`);
+  if a member is added/renamed here, update that stub in the same change set.
+
+### 1.2 Config field parity
+- FIXED: `GripperConfig.from_yaml` now uses `cls(**data)` (and
+  `Gripper.from_yaml` constructs `GripperConfig(**config_data)`), so new
+  dataclass fields load automatically. The historical bug (fields silently
+  dropped by a manually-enumerated dict) can no longer recur through this
+  path. `GripperConfig` also rejects `min_value == max_value` (zero range ->
+  divide-by-zero in `_normalize`); an INVERTED range (Robotiq) stays valid.
+- `RobotConfig` has `has_effort_feedback: bool = False` — effort observation in
+  crisp_gym only activates when a robot config sets this true AND the JointState
+  messages actually carry effort values.
+
+### 1.3 TCP frame definition
+- The TCP frame is `RobotConfig.target_frame` (e.g. `fr3_hand_tcp`), realized by
+  the crisp_controllers CIC. crisp_gym's UMI handheld pipeline calibrates its
+  handheld TCP (`tx_body_tcp`) to match THIS frame's convention. Renaming or
+  re-orienting a robot's target_frame breaks handheld<->robot data compatibility.
+
+---
+
+## 2. Extensibility: the agreed direction (assessed, partially pending)
+
+The repo has FOUR different extension patterns; new code should follow the
+best one (the sensor registry) rather than adding to the worst:
+
+| Family | Current mechanism | State |
+|---|---|---|
+| Sensors | decorator registry (`sensors/sensor.py`: `@register_sensor`) | GOOD — the model to copy |
+| Robots | `make_robot_config()` if/elif chain (`robot/robot_config.py`) + subclass + manual `__init__.py` export (export drift FIXED: Panda/UR/DynaArm now exported) | to be replaced by a `@register_robot_config` registry |
+| Grippers | DONE: `GripperBase` (gripper/gripper_base.py) + `@register_gripper` registry; `Gripper`/`MultiDofGripper` subclass it (shared node/spin/monitor/reboot/torque incl. tri-state `torque_interface` — MultiDof's silent `true` divergence fixed). `make_gripper` dispatches on the YAML `type:` key (`gripper` default, `multi_dof`). | model for the robot-config registry |
+| Cameras | DONE (transport): `image_transport: compressed|raw`, `compressed_topic_suffix`, `image_encoding` in CameraConfig; raw logs a bandwidth/storage warning. Depth would still need a base class. | — |
+
+Known small bugs (agreed to fix):
+- FIXED: `Robot.home()` now uses `config.home_controller_name` (a NEW field —
+  `joint_trajectory_controller_name` could not be reused because, despite its
+  name, it is the parameter-client target of the streaming joint controller).
+  `JointTrajectoryControllerClient` takes `controller_name`.
+- Broadcaster detection by `name.endswith("broadcaster")` in
+  `control/controller_switcher.py`.
+- Duplicated crop/resize validation between `CameraConfig.__post_init__` and
+  `Camera._pre_crop` (still open — dedup carefully, `_pre_crop` also validates
+  runtime values).
+- Camera `resolution` is **(HEIGHT, WIDTH)** — the code unpacks `(h, w)` and
+  the camera-info fallback stores `(msg.height, msg.width)`. All shipped
+  configs are square, which masked the historically ambiguous comment.
+- Thread-safety: `robot.py` now guards `_current_pose`/`_target_pose` with
+  `_pose_lock`; keep new accesses under it. `Pose.__add__/__sub__` implement a
+  WORLD-FRAME decoupled delta (order-sensitive: `base + delta`, never
+  `delta + base`) — NOT the UMI body-frame relative pose; see crisp_gym
+  tests/test_pose_math.py for the convention tests.
+
+Shared boilerplate (`_spin_node`, `from_yaml`, `wait_until_ready`, `make_*`,
+`list_*_configs`) is re-implemented in Robot/Gripper/Camera/Sensor — a shared
+base/mixin is welcome, but do it as its own PR, not mixed into feature work.
+
+## 3. ROS coupling map (relevant when decoupling inference from ROS)
+
+- Pure Python (safe to import anywhere, keep them that way):
+  all `*_config.py` dataclasses, `config/path.py`, `utils/geometry.py`,
+  `utils/sliding_buffer.py`.
+- Hard ROS2-coupled (rclpy / ROS msgs at module top): `robot/robot.py`,
+  `gripper/*.py`, `camera/camera.py`, `sensors/*.py`, all of `control/`,
+  `utils/tf_pose.py`.
+- Do not add rclpy imports (even indirect) to the pure-Python modules —
+  crisp_gym's training/inference code paths import `geometry.py` on machines
+  without ROS.
+
+## 4. Process rules
+
+- Develop on the designated `claude/...` branch; the owner merges PRs via the
+  GitHub website. Never push to other branches without explicit permission.
+- Check ALL remote branches (`git ls-remote --heads origin`) before assessing
+  repo state — side branches (UR7e fixes, DG3F gripper) have carried unmerged
+  work before.
+- ROS2 Humble pins Python to 3.11 on robot machines; anything intended to run
+  on a training/GPU machine must not require crisp_py at all (crisp_gym keeps
+  those files dependency-free — don't break that by "helpfully" importing
+  crisp_py utilities there).

README.md

@@ -10,3 +10,48 @@
 *CRISP_PY /krɪspi/*, a python package to interface with robots using [CRISP controllers](https://github.com/utiasDSL/crisp_controllers). Check the [project website](https://utiasdsl.github.io/crisp_controllers/) for further information!
 
 ![crisp_py](https://github.com/user-attachments/assets/e4cbf5fd-6ba7-4d7c-917a-bbb78d79ab10)
+
+## Running in Docker
+
+A pixi-based dev container is provided (`Dockerfile` + `docker-compose.yml`),
+mirroring the crisp_gym setup. It ships only the pixi runtime and system
+libraries; the repo is **bind-mounted** at runtime and the pixi environment is
+created on first start with `pixi install -e humble`, so edits on the host take
+effect immediately.
+
+**Expected layout** — the compose file mounts `~/workspace`, so clone this repo
+(and any sibling repos you co-develop) under it:
+
+```
+~/workspace/
+├── crisp_py/     # this repo
+└── crisp_gym/    # optional
+```
+
+**Build and start:**
+
+```bash
+cd ~/workspace/crisp_py
+NETWORK_INTERFACE=enp0s31f6 docker compose up -d crisp-py   # set to your NIC (see `ip addr`)
+docker exec -it crisp-py-humble bash
+```
+
+The first start runs `pixi install -e humble` (a few minutes); later starts
+reuse the `.pixi/envs` cache on the host. Inside the container, activate and use
+the environment as usual:
+
+```bash
+pixi shell -e humble          # ROS 2 Humble + crisp_py
+python examples/<...>.py
+```
+
+**Notes:**
+
+- The container uses `network_mode: host` (ROS 2 DDS discovery) and mounts
+  `/dev` + the X11 socket so visualization tools (viser, yourdfpy) and USB
+  devices work.
+- `ROS_DOMAIN_ID` is set to `1` in the compose file to match crisp_gym. crisp_py's
+  pixi activation (`scripts/set_ros_env.sh`) otherwise defaults it to `100` when
+  `scripts/personal_ros_env.sh` is absent — create that file to override.
+- For the Jazzy stack, swap `humble` for `jazzy` in the compose `command` and
+  the `.pixi/envs/humble` check (or run `pixi install -e jazzy` manually).

crisp_py/camera/camera.py

@@ -64,9 +64,29 @@ def __init__(
 
         self.cv_bridge = CvBridge()
 
+        # Transport selection (config.image_transport):
+        #   compressed (default) -> CompressedImage on topic + suffix
+        #   raw                  -> sensor_msgs/Image on the topic as-is
+        if self.config.image_transport == "raw":
+            msg_type = Image
+            image_topic = self.config.camera_color_image_topic
+            self.node.get_logger().warning(
+                f"Camera '{self.config.camera_name}' uses RAW (uncompressed) "
+                "image transport — every frame is moved and, if recorded, "
+                "stored uncompressed, which costs far more bandwidth/storage "
+                "than 'compressed'. Prefer image_transport: compressed unless "
+                "the publisher offers no compressed topic."
+            )
+        else:
+            msg_type = CompressedImage
+            image_topic = (
+                f"{self.config.camera_color_image_topic}"
+                f"{self.config.compressed_topic_suffix}"
+            )
+
         self._camera_subscriber = self.node.create_subscription(
-            CompressedImage,
-            f"{self.config.camera_color_image_topic}/compressed",
+            msg_type,
+            image_topic,
             self._callback_monitor.monitor(
                 f"{self._namespace.capitalize()} Camera {self.config.camera_name} Image".strip(),
                 self._callback_current_color_image,
@@ -77,9 +97,10 @@ def __init__(
         assert (
             self.config.camera_color_info_topic is not None or self.config.resolution is not None
         ), "You have to set resolution or camera info topic"
-        if self.config.camera_color_info_topic is None or self.config.resolution is None:
-            print(
-                "[Camera warning] You have set resolution and camera info topic, camera info topic will be ignored"
+        if self.config.camera_color_info_topic is not None and self.config.resolution is not None:
+            self.node.get_logger().info(
+                "Camera: both resolution and camera info topic are set — the "
+                "info topic is ignored (resolution wins)."
             )
         if self.config.camera_color_info_topic is not None:
             self.node.create_subscription(
@@ -160,11 +181,19 @@ def _spin_node(self):
             executor.spin_once(timeout_sec=0.1)
 
     def _uncompress(self, compressed_image: CompressedImage) -> Image:
-        """Uncompress a CompressedImage message to an Image message."""
+        """Uncompress a CompressedImage message to a numpy array."""
         return np.asarray(
-            self.cv_bridge.compressed_imgmsg_to_cv2(compressed_image, desired_encoding="rgb8")
+            self.cv_bridge.compressed_imgmsg_to_cv2(
+                compressed_image, desired_encoding=self.config.image_encoding
+            )
         )
 
+    def _decode(self, msg) -> np.ndarray:  # noqa: ANN001
+        """Decode an incoming image message according to the configured transport."""
+        if self.config.image_transport == "raw":
+            return self._image_to_array(msg)
+        return self._uncompress(msg)
+
     def has_image_changed_since_last_retrieval(self) -> bool:
         """Return true if the image has changed since the last time that the current_image has been accessed.
 
@@ -209,6 +238,24 @@ def is_ready(self) -> bool:
         """Returns True if camera image and resolution are available."""
         return self._current_image is not None and self.config.resolution is not None
 
+    @property
+    def is_stale(self) -> bool:
+        """True if the image stream stopped updating (publisher died mid-run).
+
+        is_ready()/current_image keep serving the LAST image after a stream
+        outage; consumers that must not act on frozen frames should check this.
+        """
+        for callback_name in self._callback_monitor.callbacks.keys():
+            if (
+                "Camera" in callback_name
+                and self.config.camera_name in callback_name
+                and "Image" in callback_name
+            ):
+                data = self._callback_monitor.get_callback_data(callback_name)
+                if data is not None:
+                    return bool(data.is_stale)
+        return False  # no monitor data yet — cannot judge staleness
+
     def wait_until_ready(self, timeout: float = 10.0, check_frequency: float = 10.0):
         """Wait until camera image and resolution are available."""
         rate = self.node.create_rate(check_frequency)
@@ -226,8 +273,19 @@ def wait_until_ready(self, timeout: float = 10.0, check_frequency: float = 10.0)
 
     def _callback_current_color_image(self, msg: CompressedImage):
         """Receive and store the current image."""
+        try:
+            image = self.ros_msg_to_image(msg)
+        except Exception as e:
+            # A corrupt/truncated frame (e.g. USB bandwidth glitches) must not
+            # raise inside the executor thread — drop it and keep the last
+            # good image; the staleness monitor flags a persistent outage.
+            self.node.get_logger().warning(
+                f"Dropping undecodable image on {self.config.camera_name}: {e}",
+                throttle_duration_sec=5.0,
+            )
+            return
         self._image_has_changed = True
-        self._current_image = self.ros_msg_to_image(msg)
+        self._current_image = image
 
     def _callback_current_color_info(self, msg: CameraInfo):
         """Receive and store the current camera info."""
@@ -236,7 +294,9 @@ def _callback_current_color_info(self, msg: CameraInfo):
 
     def _image_to_array(self, msg: Image) -> np.ndarray:
         """Converts an Image message to a numpy array."""
-        return np.asarray(self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding="rgb8"))
+        return np.asarray(
+            self.cv_bridge.imgmsg_to_cv2(msg, desired_encoding=self.config.image_encoding)
+        )
 
     def _resize_with_aspect_ratio(
         self,
@@ -266,10 +326,10 @@ def _resize_with_aspect_ratio(
 
         return cropped_image
 
-    def ros_msg_to_image(self, msg: CompressedImage) -> np.ndarray:
-        """Convert a ROS message to numpy array for this camera configuration."""
+    def ros_msg_to_image(self, msg) -> np.ndarray:  # noqa: ANN001
+        """Convert a ROS image message (raw or compressed) to a numpy array."""
         return self._resize_with_aspect_ratio(
-            self._uncompress(msg),
+            self._decode(msg),
             target_res=self.config.resolution,
             crop_width=self.config.crop_width,
             crop_height=self.config.crop_height,