From 3dfcbabf5bf394a622ba1e797c474d6e098a782a Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Thu, 7 Nov 2024 13:29:15 +0100 Subject: [PATCH] Add piper rerun example --- examples/piper/assets/piper_left.urdf | 568 ++++++++++++++++++++ examples/piper/assets/piper_left_pred.urdf | 568 ++++++++++++++++++++ examples/piper/assets/piper_right.urdf | 568 ++++++++++++++++++++ examples/piper/assets/piper_right_pred.urdf | 568 ++++++++++++++++++++ examples/piper/piper_inference.py | 229 ++++++++ examples/piper/play_inference.yml | 22 + 6 files changed, 2523 insertions(+) create mode 100644 examples/piper/assets/piper_left.urdf create mode 100644 examples/piper/assets/piper_left_pred.urdf create mode 100644 examples/piper/assets/piper_right.urdf create mode 100644 examples/piper/assets/piper_right_pred.urdf create mode 100644 examples/piper/piper_inference.py create mode 100644 examples/piper/play_inference.yml diff --git a/examples/piper/assets/piper_left.urdf b/examples/piper/assets/piper_left.urdf new file mode 100644 index 00000000..99fde57d --- /dev/null +++ b/examples/piper/assets/piper_left.urdf @@ -0,0 +1,568 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/piper/assets/piper_left_pred.urdf b/examples/piper/assets/piper_left_pred.urdf new file mode 100644 index 00000000..a74ed6c3 --- /dev/null +++ b/examples/piper/assets/piper_left_pred.urdf @@ -0,0 +1,568 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/piper/assets/piper_right.urdf b/examples/piper/assets/piper_right.urdf new file mode 100644 index 00000000..99fde57d --- /dev/null +++ b/examples/piper/assets/piper_right.urdf @@ -0,0 +1,568 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/piper/assets/piper_right_pred.urdf b/examples/piper/assets/piper_right_pred.urdf new file mode 100644 index 00000000..2541f63f --- /dev/null +++ b/examples/piper/assets/piper_right_pred.urdf @@ -0,0 +1,568 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/examples/piper/piper_inference.py b/examples/piper/piper_inference.py new file mode 100644 index 00000000..0a970c97 --- /dev/null +++ b/examples/piper/piper_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_inference.yml b/examples/piper/play_inference.yml new file mode 100644 index 00000000..48b1cee3 --- /dev/null +++ b/examples/piper/play_inference.yml @@ -0,0 +1,22 @@ +nodes: + - id: piper + path: piper_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