import time

import numpy as np

import pinocchio as pin
import example_robot_data as robex


class TaskParameters:
    '''Common information to all tasks, to be allocated by each minion and the boss. '''
    robotname = 'ur5'
    robot = robex.load(robotname)


class Task:
    def __init__(self, identifier, q, v, a):
        self.identifier = identifier

        self.q = q.copy()
        self.v = v.copy()
        self.a = a.copy()

        self.tau = np.zeros(self.a.shape)
        self.time = 0

    def work(self):
        start = time.perf_counter()

        for i in range(1_000_000):
            pin.rnea(TaskParameters.robot.model, TaskParameters.robot.data, self.q, self.v, self.a)
        self.tau[:] = TaskParameters.robot.data.tau

        self.time = time.perf_counter() - start