Add rdt 1b (#718)

This PR add all code required to finetune and do inference of RDT 1B on
Agilex Piper.

The current WIP is on making the code cleaner.

.github/workflows/node-hub-ci-cd.yml

@@ -39,7 +39,16 @@ jobs:
     steps:
       - name: Checkout repository
         if: runner.os == 'Linux' || github.event_name == 'workflow_dispatch' || (github.event_name == 'release' && startsWith(github.ref, 'refs/tags/'))
-        uses: actions/checkout@v2
+        uses: actions/checkout@v4
+        with:
+          submodules: true # Make sure to check out the sub-module
+
+      - name: Update submodule
+        if: runner.os == 'Linux'
+        run: |
+          git submodule update --init --recursive
+          git submodule update --remote --recursive
+
       - name: Free Disk Space (Ubuntu)
         uses: jlumbroso/free-disk-space@main
         if: runner.os == 'Linux'

.gitmodules

@@ -0,0 +1,3 @@
+[submodule "node-hub/dora-rdt-1b/dora_rdt_1b/RoboticsDiffusionTransformer"]
+	path = node-hub/dora-rdt-1b/dora_rdt_1b/RoboticsDiffusionTransformer
+	url = https://github.com/thu-ml/RoboticsDiffusionTransformer

CONTRIBUTING.md

@@ -12,6 +12,8 @@ You can use the standard `cargo check`, `cargo build`, `cargo run`, and `cargo t
 To run a command for a specific package only, pass e.g. `--package dora-daemon`.
 Running a command for the whole workspace is possible by passing `--workspace`.
 
+
+
 ## Continuous Integration (CI)
 
 We're using [GitHub Actions](https://github.com/features/actions) to run automated checks on all commits and pull requests.

examples/piper/README.md

@@ -0,0 +1,59 @@
+# Getting Started with Tracer + Piper
+
+# DATA COLLECTION
+
+## Installation (To do once)
+
+Make sure to:
+
+```bash
+dora build record.yaml
+
+# Make sure to install from source pyorbbecksdk
+
+git clone https://github.com/orbbec/pyorbbecsdk
+cd pyorbbecsdk
+pip3 install -r requirements.txt
+mkdir build
+cd build
+cmake -Dpybind11_DIR=`pybind11-config --cmakedir` ..
+make -j4
+make install
+cd ..
+pip3 install wheel
+python3 setup.py bdist_wheel
+pip3 install dist/*.whl
+
+export PYTHONPATH=$PYTHONPATH:$(pwd)/install/lib/ # Make sure to save this in your .bashrc
+cd ..
+
+
+# Install ugv_sdk_py from source
+git clone https://github.com/westonrobot/ugv_sdk
+cd ugv_sdk
+python setup.py build_ext --inplace
+
+export PYTHONPATH=$PYTHONPATH:$(pwd) # Make sure to save this in your .bashrc
+cd ..
+```
+
+### Your bashrc should contain something like this
+
+```bash
+export PYTHONPATH=$PYTHONPATH:/home/agilex/pyorbbecsdk/install/lib/:/home/agilex/ugv_sdk
+```
+
+## Setup ( Every boot of the computer )
+
+```bash
+# Run on Agilex provided computer
+source /home/agilex/cobot_magic/Piper_ros_private-ros-noetic/can_config.sh
+```
+
+## Run
+
+### For recording episode
+
+```bash
+dora run record.yml
+```

examples/piper/arms_camera.yml

@@ -0,0 +1,74 @@
+nodes:
+  - id: piper_left
+    path: /home/agilex/1ms.ai/piper_sdk/dora_piper.py
+    _unstable_deploy:
+      machine: piper
+    inputs:
+      tick: dora/timer/millis/20
+    outputs:
+      - jointstate
+    env:
+      CAN_BUS: can_left
+
+  - id: camera_left
+    path: /home/agilex/1ms.ai/pyorbbecsdk/examples/color_viewer.py
+    _unstable_deploy:
+      machine: piper
+    inputs:
+      tick: dora/timer/millis/50
+    outputs:
+      - image
+    env:
+      DEVICE_INDEX: 0
+      ENCODING: jpeg
+
+  - id: camera_center
+    path: /home/agilex/1ms.ai/pyorbbecsdk/examples/color_viewer.py
+    _unstable_deploy:
+      machine: piper
+    inputs:
+      tick: dora/timer/millis/50
+    outputs:
+      - image
+    env:
+      DEVICE_INDEX: 1
+      ENCODING: jpeg
+
+  - id: camera_right
+    path: /home/agilex/1ms.ai/pyorbbecsdk/examples/color_viewer.py
+    _unstable_deploy:
+      machine: piper
+    inputs:
+      tick: dora/timer/millis/50
+    outputs:
+      - image
+    env:
+      DEVICE_INDEX: 2
+      ENCODING: jpeg
+      # import opencv as cv
+      # [cv2.VideoCapture(i) for i in range(12)]
+
+  - id: piper_right
+    path: /home/agilex/1ms.ai/piper_sdk/dora_piper.py
+    _unstable_deploy:
+      machine: piper
+    inputs:
+      tick: dora/timer/millis/20
+    outputs:
+      - jointstate
+    env:
+      CAN_BUS: can_right
+
+  - id: rerun
+    path: dora-rerun1
+    inputs:
+      jointstate_piper_left: piper_left/jointstate
+      jointstate_piper_right: piper_right/jointstate
+      image_camera_left: camera_left/image
+      image_camera_center: camera_center/image
+      image_camera_right: camera_right/image
+    env:
+      piper_left_urdf: assets/piper_left.urdf
+      piper_right_urdf: assets/piper_right.urdf
+      piper_left_transform: 0 0.2 0
+      piper_right_transform: 0 -0.2 0

examples/piper/arms_only.yml

@@ -0,0 +1,33 @@
+nodes:
+  - id: piper_left
+    path: /home/agilex/1ms.ai/piper_sdk/dora_piper.py
+    _unstable_deploy:
+      machine: piper
+    inputs:
+      tick: dora/timer/millis/20
+    outputs:
+      - jointstate
+    env:
+      CAN_BUS: can_left
+
+  - id: piper_right
+    path: /home/agilex/1ms.ai/piper_sdk/dora_piper.py
+    _unstable_deploy:
+      machine: piper
+    inputs:
+      tick: dora/timer/millis/20
+    outputs:
+      - jointstate
+    env:
+      CAN_BUS: can_right
+
+  - id: rerun
+    path: dora-rerun
+    inputs:
+      jointstate_piper_left: piper_left/jointstate
+      jointstate_piper_right: piper_right/jointstate
+    env:
+      piper_left_urdf: /home/peter/Documents/work/dora/examples/piper/assets/piper_left.urdf
+      piper_right_urdf: /home/peter/Documents/work/dora/examples/piper/assets/piper_right.urdf
+      piper_left_transform: 0 0.3 0
+      piper_right_transform: 0 -0.3 0

examples/piper/convert.py

@@ -0,0 +1,81 @@
+import numpy as np
+from scipy.spatial.transform import Rotation as R
+
+
+def convert_quaternion_to_euler(quat):
+    """
+    Convert Quaternion (xyzw) to Euler angles (rpy)
+    """
+    # Normalize
+    quat = quat / np.linalg.norm(quat)
+    euler = R.from_quat(quat).as_euler("xyz")
+
+    return euler
+
+
+def convert_euler_to_quaternion(euler):
+    """
+    Convert Euler angles (rpy) to Quaternion (xyzw)
+    """
+    quat = R.from_euler("xyz", euler).as_quat()
+
+    return quat
+
+
+def convert_euler_to_rotation_matrix(euler):
+    """
+    Convert Euler angles (rpy) to rotation matrix (3x3).
+    """
+    quat = R.from_euler("xyz", euler).as_matrix()
+
+    return quat
+
+
+def convert_rotation_matrix_to_euler(rotmat):
+    """
+    Convert rotation matrix (3x3) to Euler angles (rpy).
+    """
+    r = R.from_matrix(rotmat)
+    euler = r.as_euler("xyz", degrees=False)
+
+    return euler
+
+
+def normalize_vector(v):
+    v_mag = np.linalg.norm(v, axis=-1, keepdims=True)
+    v_mag = np.maximum(v_mag, 1e-8)
+    return v / v_mag
+
+
+def cross_product(u, v):
+    i = u[:, 1] * v[:, 2] - u[:, 2] * v[:, 1]
+    j = u[:, 2] * v[:, 0] - u[:, 0] * v[:, 2]
+    k = u[:, 0] * v[:, 1] - u[:, 1] * v[:, 0]
+
+    out = np.stack((i, j, k), axis=1)
+    return out
+
+
+def compute_rotation_matrix_from_ortho6d(ortho6d):
+    x_raw = ortho6d[:, 0:3]
+    y_raw = ortho6d[:, 3:6]
+
+    x = normalize_vector(x_raw)
+    z = cross_product(x, y_raw)
+    z = normalize_vector(z)
+    y = cross_product(z, x)
+
+    x = x.reshape(-1, 3, 1)
+    y = y.reshape(-1, 3, 1)
+    z = z.reshape(-1, 3, 1)
+    matrix = np.concatenate((x, y, z), axis=2)
+    return matrix
+
+
+def compute_ortho6d_from_rotation_matrix(matrix):
+    # The ortho6d represents the first two column vectors a1 and a2 of the
+    # rotation matrix: [ | , |,  | ]
+    #                  [ a1, a2, a3]
+    #                  [ | , |,  | ]
+    ortho6d = matrix[:, :, :2].transpose(0, 2, 1).reshape(matrix.shape[0], -1)
+    return ortho6d