# R1 Vision / Visual Servo Prototype

Standalone project for the Unitree R1. The Unitree SDK remains an external dependency.

## Layout

- `src/vision_node.cpp` — Unitree camera frames -> atomic `/dev/shm/robot_frame.jpg`
- `scripts/yolo_detector.py` — YOLO bottle detection -> `/dev/shm/robot_detection.json`
- `src/r1_hand_calibrate.cpp` — Pinocchio wrist + clicked wrist pixels -> camera extrinsics
- `src/r1_visual_servo.cpp` — YOLO bottle target + Pinocchio wrist projection -> image-space visual servo
- `include/r1_vision/robot_common.hpp` — 500 Hz `rt/lowcmd` controller
- `include/r1_vision/kinematics.hpp` — R1 joint state -> Pinocchio configuration / frame Jacobians
- `include/r1_vision/vision_io.hpp` — detection JSON reader
- `config/` — calibration files

## Dependencies

Assumes:

- Unitree SDK2 is built at `/home/jvizner/unitree_sdk2`
- R1 URDF is at `/home/jvizner/unitree_ros/robots/r1_description/R1.urdf`
- OpenCV, Eigen3, Pinocchio, C++17
- Python environment with Ultralytics + OpenCV + NumPy

Override the SDK path when configuring:

```bash
cmake -S . -B build -DUNITREE_SDK2_DIR=/home/jvizner/unitree_sdk2
```

Build:

```bash
cmake --build build -j$(nproc)
```

## Camera / YOLO

Start the camera:

```bash
./build/vision_node enp3s0
```

Start YOLO in your `ai_robot` environment:

```bash
python scripts/yolo_detector.py
```

The detector defaults to `yolo26m.pt` and class `bottle`. Override with:

```bash
R1_YOLO_MODEL=yolo26m.pt R1_TARGET_CLASS=bottle python scripts/yolo_detector.py
```

## Existing intrinsic calibration

Copy your known-good intrinsic calibration into `config/camera_intrinsics.yaml` or pass its path to the hand calibrator:

```bash
./build/r1_hand_calibrate enp3s0 /home/jvizner/r1_camera_calibration/camera_intrinsics.yaml
```

The calibrator uses the left wrist as the 3D reference point. Collect at least 8, preferably 12-15, distinct wrist poses. Click the same physical wrist point every time and press `c`.

It writes:

```text
/home/jvizner/r1_camera_calibration/camera_extrinsics.yaml
```

## Visual servo

The servo is **dry-run by default**. It calculates `dq` but does not move the robot.

Run:

```bash
./build/r1_visual_servo enp3s0 /home/jvizner/r1_camera_calibration/camera_extrinsics.yaml
```

Only enable motion explicitly after validating the predicted wrist pixel:

```bash
ENABLE_MOTION=1 ./build/r1_visual_servo enp3s0 /home/jvizner/r1_camera_calibration/camera_extrinsics.yaml
```

The first runtime target is left wrist -> detected bottle center. It controls left shoulder pitch/roll/yaw/elbow only.