Add urdf visualization in rerun (#704)

Add urdf visualization for rerun by reusing https://github.com/rerun-io/rerun-loader-python-example-urdf The urdf loader from rerun has couple of small issues such as not being on pip and having relative path complexity and so I might fork the project to work on it from here. Could be nice to have a bit of testing so I will live it open for now.
dora-rs · Nov 11, 2024 · 2f5945f · 2f5945f
2 parents e98d81d + aa63796
commit 2f5945f
Show file tree

Hide file tree

Showing 13 changed files with 3,992 additions and 687 deletions.
diff --git a/Cargo.lock b/Cargo.lock
diff --git a/examples/piper/assets/piper_left.urdf b/examples/piper/assets/piper_left.urdf
diff --git a/examples/piper/assets/piper_left_pred.urdf b/examples/piper/assets/piper_left_pred.urdf
diff --git a/examples/piper/assets/piper_right.urdf b/examples/piper/assets/piper_right.urdf
diff --git a/examples/piper/assets/piper_right_pred.urdf b/examples/piper/assets/piper_right_pred.urdf
diff --git a/examples/piper/dummy_inference.py b/examples/piper/dummy_inference.py
@@ -0,0 +1,229 @@
+from dora import Node
+
+node = Node()
+
+import numpy as np
+
+pred = np.array(
+    [
+        [
+            [
+                -0.0737,
+                -0.0447,
+                0.2695,
+                -0.0054,
+                -0.0461,
+                -0.0258,
+                0.1357,
+                0.1943,
+                0.0625,
+                0.1270,
+                -0.0259,
+                -0.0195,
+                0.0981,
+                0.1021,
+            ],
+            [
+                -0.0757,
+                -0.0332,
+                0.2793,
+                -0.0156,
+                -0.0374,
+                -0.0193,
+                0.1924,
+                0.2119,
+                0.0654,
+                0.1377,
+                0.0020,
+                -0.0076,
+                0.0884,
+                0.1143,
+            ],
+            [
+                -0.0771,
+                -0.0282,
+                0.2832,
+                -0.0225,
+                -0.0378,
+                -0.0156,
+                0.1992,
+                0.2324,
+                0.0674,
+                0.1416,
+                0.0088,
+                -0.0056,
+                0.0918,
+                0.1191,
+            ],
+            [
+                -0.0825,
+                -0.0325,
+                0.2832,
+                -0.0210,
+                -0.0420,
+                -0.0204,
+                0.1904,
+                0.2256,
+                0.0610,
+                0.1475,
+                0.0068,
+                -0.0071,
+                0.0879,
+                0.1172,
+            ],
+            [
+                -0.0825,
+                -0.0344,
+                0.2852,
+                -0.0208,
+                -0.0405,
+                -0.0167,
+                0.1777,
+                0.2109,
+                0.0518,
+                0.1621,
+                0.0059,
+                -0.0017,
+                0.0854,
+                0.1182,
+            ],
+            [
+                -0.0840,
+                -0.0332,
+                0.2871,
+                -0.0226,
+                -0.0417,
+                -0.0159,
+                0.1748,
+                0.2080,
+                0.0476,
+                0.1641,
+                0.0000,
+                0.0010,
+                0.0864,
+                0.1196,
+            ],
+            [
+                -0.0879,
+                -0.0234,
+                0.2969,
+                -0.0305,
+                -0.0396,
+                -0.0132,
+                0.1904,
+                0.2275,
+                0.0474,
+                0.1758,
+                0.0137,
+                0.0020,
+                0.0874,
+                0.1245,
+            ],
+            [
+                -0.0884,
+                -0.0225,
+                0.3008,
+                -0.0339,
+                -0.0386,
+                -0.0118,
+                0.1904,
+                0.2275,
+                0.0469,
+                0.1855,
+                0.0186,
+                -0.0017,
+                0.0879,
+                0.1270,
+            ],
+            [
+                -0.0918,
+                -0.0081,
+                0.3066,
+                -0.0449,
+                -0.0325,
+                -0.0074,
+                0.2021,
+                0.2363,
+                0.0454,
+                0.1953,
+                0.0332,
+                0.0015,
+                0.0942,
+                0.1348,
+            ],
+            [
+                -0.0938,
+                -0.0249,
+                0.3027,
+                -0.0366,
+                -0.0396,
+                -0.0125,
+                0.1777,
+                0.2207,
+                0.0376,
+                0.1953,
+                0.0146,
+                -0.0017,
+                0.0884,
+                0.1279,
+            ],
+            [
+                -0.0938,
+                -0.0229,
+                0.3047,
+                -0.0393,
+                -0.0386,
+                -0.0121,
+                0.1680,
+                0.2256,
+                0.0330,
+                0.2002,
+                0.0127,
+                0.0015,
+                0.0913,
+                0.1260,
+            ],
+            [
+                -0.0977,
+                -0.0206,
+                0.3105,
+                -0.0405,
+                -0.0359,
+                -0.0161,
+                0.1680,
+                0.2178,
+                0.0295,
+                0.2129,
+                0.0146,
+                -0.0085,
+                0.0923,
+                0.1309,
+            ],
+            [
+                -0.0938,
+                -0.0311,
+                0.3027,
+                -0.0334,
+                -0.0386,
+                -0.0188,
+                0.1377,
+                0.2139,
+                0.0227,
+                0.2031,
+                0.0039,
+                -0.0020,
+                0.0913,
+                0.1221,
+            ],
+        ]
+    ]
+)
+
+import time
+import pyarrow as pa
+
+data = pred[0]
+for joint in data:
+    node.send_output("jointstate_left", pa.array(joint[:7], type=pa.float32()))
+    node.send_output("jointstate_right", pa.array(joint[7:], type=pa.float32()))
+    time.sleep(0.1)
diff --git a/examples/piper/play_dummy_inference.yml b/examples/piper/play_dummy_inference.yml
@@ -0,0 +1,22 @@
+nodes:
+  - id: piper
+    path: dummy_inference.py
+    inputs:
+      tick: dora/timer/millis/20
+    outputs:
+      - jointstate_left
+      - jointstate_right
+    env:
+      CAN_BUS: can_left
+
+  - id: rerun
+    build: wget
+    path: dora-rerun
+    inputs:
+      jointstate_piper_left: piper/jointstate_left
+      jointstate_piper_right: piper/jointstate_right
+    env:
+      piper_left_urdf: assets/piper_left.urdf # Make sure to download assets/meshes from https://github.com/agilexrobotics/Piper_ros/tree/4f22c61f96b8fb3ef3f937b99b63edb697caadf0/src/piper_description/meshes and put them in the assets folder
+      piper_right_urdf: assets/piper_right.urdf # Make sure to download assets/meshes from https://github.com/agilexrobotics/Piper_ros/tree/4f22c61f96b8fb3ef3f937b99b63edb697caadf0/src/piper_description/meshes and put them in the assets folder
+      piper_left_transform: 0 0.2 0
+      piper_right_transform: 0 -0.2 0
diff --git a/node-hub/dora-rerun/Cargo.toml b/node-hub/dora-rerun/Cargo.toml
@@ -9,9 +9,24 @@ repository.workspace = true
 
 # See more keys and their definitions at https://doc.rust-lang.org/cargo/reference/manifest.html
 
+[features]
+default = []
+python = ["pyo3"]
+
 [dependencies]
 dora-node-api = { workspace = true, features = ["tracing"] }
 eyre = "0.6.8"
 tokio = { version = "1.24.2", features = ["rt"] }
-rerun = { version = "0.18.2", features = ["web_viewer", "image"] }
+rerun = { version = "0.19.0", features = ["web_viewer", "image"] }
 ndarray = "0.15.6"
+k = "0.32"
+pyo3 = { workspace = true, features = [
+    "extension-module",
+    "abi3",
+], optional = true }
+
+
+[lib]
+name = "dora_rerun"
+path = "src/lib.rs"
+crate-type = ["lib", "cdylib"]
diff --git a/node-hub/dora-rerun/README.md b/node-hub/dora-rerun/README.md
@@ -33,6 +33,36 @@ cargo install --git https://github.com/dora-rs/dora dora-rerun
 - image: UInt8Array + metadata { "width": int, "height": int, "encoding": str }
 - boxes2D: StructArray + metadata { "format": str }
 - text: StringArray
+- jointstate: Float32Array
+
+## (Experimental) For plotting 3D URDF
+
+```bash
+pip install git+https://github.com/rerun-io/rerun-loader-python-example-urdf.git
+```
+
+Make sure to name the dataflow as follows:
+
+```yaml
+- id: rerun
+  path: dora-rerun
+  inputs:
+    jointstate_<ENTITY_NAME>: <ENTITY_NAME>/jointstate
+  env:
+    <ENTITY_NAME>_urdf: /path/to/<ENTITY_NAME>.urdf
+    <ENTITY_NAME>_transform: 0 0.3 0
+```
+
+> [!IMPORTANT]  
+> Make sure that the urdf file name correspond to your dataflow object name otherwise, it will not be able to link to the corresponding entity.
+
+> [!WARNING]
+> Please make sure to review the following gotchas:
+>
+> - Filename included in URDF are going to be relative to your dataflow working directory instead of the URDF path: https://github.com/rerun-io/rerun-loader-python-example-urdf/issues/13
+> - URDF loader is not on pip and so you need to install it yourself https://github.com/rerun-io/rerun-loader-python-example-urdf/issues/12
+> - There is no warning if a file is not logged properly. https://github.com/rerun-io/rerun-loader-python-example-urdf/pull/14
+> - There is no transparent color in rerun. https://github.com/rerun-io/rerun/issues/1611
 
 ## Configurations
 

diff --git a/node-hub/dora-rerun/pyproject.toml b/node-hub/dora-rerun/pyproject.toml
@@ -4,3 +4,12 @@ build-backend = "maturin"
 
 [project]
 name = "dora-rerun"
+dependencies = [
+    'rerun_sdk==0.19.1',
+    # "rerun-loader-urdf @ git+https://github.com/rerun-io/rerun-loader-python-example-urdf.git",
+]
+
+scripts = { "dora-rerun" = "dora_rerun:py_main" }
+
+[tool.maturin]
+features = ["pyo3/extension-module"]