Use jacobian returned by fri
Add operation for tasks
Add torque limit