[TASK] Add calculation of thrust allocation matrix to the thrust allocator

[alekskl01]

Description of task

Instead of having to recalculate the thrust allocation matrix ourselves every time the thruster position is changed or center of mass is shifted, the thruster allocator should be given the pose of each thruster relative to the origin and a vector from the origin to the center of mass

Suggested Workflow

No response

Specifications

No response

Contacts

No response

Code Quality

• [ ] Every function in header files are documented (inputs/returns/exceptions)
• [ ] The project has automated tests that cover MOST of the functions and branches in functions (pytest/gtest)
• [ ] The code is documented on the wiki (provide link)