[FEATURE]: Implement TPIK controller

[evan-palmer]

Feature Type

Adding new functionality to Angler

Problem Description

The UVMS requires a method of determining the desired joint velocities for the system given some desired end effector position. A well-tested technique is the task-priority inverse kinematic (TPIK) controller. It would be useful to integrate support for this type of control into the angler system

Feature Description

I propose adding a new angler_control package which implements the TPIK controller. It may be worth considering a method which enables configuring the tasks and their priority within a configuration file as well, if possible.

Alternative Solutions

In the future, this controller may be replaced with a motion planner. It would be worth considering whether this controller is better off being placed in the angler_planning package if this type of controller can be replaced with motion planner.

Additional Context

No response