float32[12] q
float32[12] dq 
float32[12] kp
float32[12] kd    
float32[12] tau