Multi goals steering for robots with independently steerable wheels

I am currently on a controller for robots with independently steerable wheels.

The robots have to perform complex maneuver in team, and in this context, we want to be able to control not only the direction of movement, but also being able to track and keep aligned with a target. We define the tracking target T (in blue) and the movement goal G (in red) and compute a desired Instantaneous Center of Rotation (ICR, in white). We use the ICR to compute the optimal wheel orientation that minimizes slip while fulfilling the 2 objectives.