import time
import pinocchio as pin
import numpy as np
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(1000000):
            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