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)