From 8f0fa67d39e1082d7af36fbfdc27820436595228 Mon Sep 17 00:00:00 2001 From: haixuanTao Date: Sun, 27 Apr 2025 17:56:28 +0200 Subject: [PATCH] draft: add arctan to anfgle computation --- examples/so100-remote/parse_keyboard.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/examples/so100-remote/parse_keyboard.py b/examples/so100-remote/parse_keyboard.py index 8483916e..95ba3435 100644 --- a/examples/so100-remote/parse_keyboard.py +++ b/examples/so100-remote/parse_keyboard.py @@ -2,6 +2,7 @@ import time +import numpy as np import pyarrow as pa from dora import Node @@ -10,13 +11,19 @@ node = Node() now = time.time() -for j in range(100): +for j in range(0, 100): for i in range(110): time.sleep(0.033) + x = 0.0 + (j * 0.005) + y = -0.1 - (i * 0.001) + if x == 0: + theta = -1.8 + else: + theta = np.arctan(y / x) - print("Sending action") + print("Sending action: ", x, y, theta) node.send_output( "action", - pa.array([0.0 + (j * 0.005), -0.1 - (i * 0.001), 0.18, -3.1, -1.8, 0.45]), + pa.array([0.0 + (j * 0.005), -0.1 - (i * 0.001), 0.18, -3.1, theta, 0.45]), metadata={"encoding": "xyzrpy"}, )