Azi_Drive: Thrust constraints allow Azi to command a force too small for thrusters

[jpanikulam]

We can improve vehicle response in control regions where we desire little force (<10 N) by making Azi aware of the lower constraints on thrust.

Current schema:

u < u_max = 100
u > u_min  = -70

New schema:

u < u_max = 100
u > u_min = -70
abs(u) > 10

if norm(wrench.xyz == 0): u = 0

With this, if you command a force less than 10 N, what the thrusters typically bottom out at, Azi will angle the thrusters further out to allow smaller forces, instead of commanding un-achievable small thrusts.

[jpanikulam]

• Fixed in branch jacob_planner
• Added to Master