Somersault trajectory as an example of being far from current state

[avikde]

scott said maybe initial trajectory is dynamically infeasible: to solve this, take the first solution, get a traj, and then linearize around that and rinse repeat (before actually taking a control action in the sim). this is what a SQP (sequential QP) is.

• the model itself should use RefTraj.generate to assemble a reference into the specific coordinates it is using, then MPCUtils should be able to query for it
• check other MPC work; fill in intermediate traj points and not just jump to goal
• debug the cause of primal infeasible result https://osqp.readthedocs.io/en/stable/solver/index.html#primal-infeasibility
• first do an LQR controller for tracking a traj. would N=1 be a good debugging step? is it just LQR? N=1 means that y0, x1 are in the sol, and u0 (which is provided), but no u1 (the result)
• nonlinear update in model, SQP implementation in MPCUtils. Any variations? #23

Primal infeas debugging:

• looking at cert vector helps: it points out which constraint is violating
• removing that state/input consideration essentially moves the problem around
• now get max iter reached

From 3/19 meeting:

• write some debugging tools: plot state traj MPC planar debugging tools: plot the trajectory supplied to MPC #24
• reuse prev solution to propagate forward, or integrate twist forward #25
• running cost quadratic cost multiplied by dt. #26
• scaling time disable
• switch to analysis and work on averaged stroke map MPC new model stroke map #27

MPCUtils <- *Model interface:

• calls getLinearDynamics
• calls getReference - either returns a single goal point, or N points

Model in itself may expose hooks to modify the reference trajectory?

Related to #9

[avikde]

Debugging linearized dynamics. With openloop input [0,u2], get this output:

How is u2 leaking into x,z?

EDIT

• Seems like the potential cause is the (mb *g + u1) in the u1 control slot. The mb*g "nominal" term only works when upright.
• however, upright is the only equilibrium.
• need to work on theory for this. need an LTV system, but what to linearize about? the points are not equilibria, but that's OK?

[avikde]

More reasonable trajectory for an openloop command with constant thrust, strokedev

[avikde]

OSQP always ends in primal infeasible

[avikde]

Traj following finally working again, but seems to nee very small timestep

[avikde]

had to cheat and increase the body inertia delicate balance of timestep dt vs. body dynamics timescale

[avikde]

[avikde]

Effect of N 61aa359 and 04aafe4

[avikde]

Something sort of working for flip 2da5f10

However, clearly one problem is that u1 -> 0 here and so u2 can't really do much. Should check if this is going in the linearization and that is what is making u2 want to blow up when u1 gets small (would make sense). Could saturate it. EDIT: it is (m*g + u1)/ib in the element multiplying u2 in B, so mg is still there.

Or, need some way to create a situation where u1 is large.

[avikde]

with

	runMPCSim([0.01, 0.01, 1, 0, 0, 0, 0], [1, 10000], N=20, dt=0.004, epsi=1e-6, label='N=20')
	runMPCSim([0.01, 0.01, 1, 0, 0, 0, 0], [1, 10000], N=15, dt=0.005, epsi=1e-2, label='N=15')
	runMPCSim([0.01, 0.01, 1, 0, 0, 0, 0], [1, 10000], N=10, dt=0.005, epsi=1e-2, label='N=10')

results SINE

[avikde]

Isn't the reference trajectory identical to iterations of the dynamics with the same input?

[avikde]

Traj nonlin result (had to lower dt)

Traj lin is even worse.

[avikde]

will come back to this.