Trajectory consisting of multiple segments

[Peyje]

Currently, a trajectory only consists of a single segment. Ideally, an arbitrary number of segments should be possible.

Proposed course of action:

• Clean up QP matrices
• Construct QP matrices in a loop
• Try manually set waypoints
• Extend GUI to make it able to set multiple waypoints

[Peyje]

As of #6 multiple segments are now possible in two ways: Joint and separated.
However, the solver does not allow to solve for all constraints, following are the current active constraints for each mode.

Joint

• X,Y,Z,Psi position of starting waypoint
• X,Y,Z,Psi position of ending waypoint
• X,Y,Z,Psi velocity at rendezvous of segments
• X,Y,Z,Psi velocity of absolute starting waypoint
• X,Y,Z velocity of absolute ending waypoint

Separate

• X,Y,Z,Psi position of starting waypoint
• X,Y,Z,Psi velocity of starting waypoint
• X,Y,Z acceleration of starting waypoint
• X,Y,Z,Psi position of ending waypoint
• X,Y,Z velocity of absolute ending waypoint

Any more constraints will resolve in the solver finishing without a solution and setting the status to primary infeasible.

[Peyje]

The CVX solver could be used instead to figure out why more constraints are not possible to set.