I'm building a control system for an underwater robot. I am given a target vector with components which range from 1 to -1. This vector is our target vector, meaning it is the direction I want my system to move. For the purpose of this problem we can imagine the system as a 2D plane, the motors are only controlling the x and y position, making this problem considerably easier.
Then we define the positions of our motors by doing the following: We define an angle from the origin (the center of the robot) and a magnitude. This defines the "coordinate" of the motor. Then we define an angle from the coordinate which represents a thrust vector. This thrust vector's magnitude can be anywhere from 1 to -1. A drawing is attached that describes this definition.
While the magnitude is variable and should be adjusted to achieve the target, the angle of this motor (blue and pink) from the offset vector (red) is constant and must not change.
The program I must build has to take the target vector and find the optimal magnitude of the thrust vector of each motor so that they add up to exactly the target vector with the following constraints:
The sum of all torque forces on the system must be 0
The sum of all magnitudes of the thrust vectors must be as small as possible as to achieve the above constraints without expending more energy than needed.
How do I build this program in python? I am open to using any library under the sun and any time complexity.
I've tried using a more fixed approach where motors are weighted or paired with each other so that torque would always be 0. However the code for this approach is less straight forward from a ui point of view and not very extendable from a longevity perspective. I'd like to be able to define motors entirely arbitrarily and not have to worry about pairing them within the program or worry about torque (meaning that the algorithm would manage torque, not the way we define our motors).