WLQP C++ implementation to mex

avikde / robobee3d

Robobee research including controls, modeling, and simulation

2 stars 1 forks source link

WLQP C++ implementation to mex #174

Closed avikde closed 4 years ago

avikde commented 4 years ago

Closes #171

Building on from #173

References

https://stackoverflow.com/questions/54763204/how-to-configure-cmake-to-compile-mex-files

avikde commented 4 years ago

Output from matlab 145.0000 0.0081 -0.0000 -0.0014

Output from C++ 145 0.00807963 -9.36425e-07 -0.00139418

avikde commented 4 years ago

Next step: connect this mex in the target_driver file, and run on the robot.

avikde commented 4 years ago

Using empirical popts; nonzero drv_roll for vertical momentum.

avikde commented 4 years ago

Simulink file (in dropbox) and mex file (built from this repo):
simulink.zip

avikde commented 4 years ago

Lots of trouble getting this into simulink. https://www.mathworks.com/help/simulink/sfg/integrating-existing-c-functions-into-simulink-models-with-the-legacy-code-tool.html

avikde commented 4 years ago

Works with QP solve

10 iterations: avg TET 9us
100 iterations: avg TET 25us

avikde commented 4 years ago

drv_pch appears to be hitting 0.5. need to check. pddes = [0,0,10,0,0,0]

avikde commented 4 years ago

Hypothesis: wlControllerInit() was not being called

Precise test to do:

copy the code over again -> flat structure
rebuild everything and run
try ignoring inputs and overwriting them from main.c

Result at home 144.019363,0.032662,-0.034836,0.012172

avikde commented 4 years ago

There are some spikes (will attach matlab). Not sure if they are due to some aspect of simulink. Should try to create a mex of this one on my computer and run that to check.

Otherwise, seems like it produces ~0 drv_pch, ~20 drv_roll.

in target_driver_setup, drv_roll = (vleft - vright)/4
my previous openloop tests show I had to run 180L 220R etc., i.e. negative drv_roll. Will add a -1 gain

avikde commented 4 years ago

Can replicate the spikes with mex on my computer

confirmed wlControllerInit() is only called once

they are because of maximum iterations reached

du = -0.000 0.010 0.000, solved inaccurate
du = -0.000 0.010 0.003, maximum iterations reached
du = -0.002 -0.036 -0.004, maximum iterations reached
du = 0.006 0.204 -0.006, maximum iterations reached
du = 0.008 -0.070 0.060, maximum iterations reached
du = -0.001 -0.016 -0.006, maximum iterations reached
du = -0.000 -0.014 -0.008, maximum iterations reached
du = -0.000 -0.010 -0.006, solved
du = -0.000 -0.010 -0.005, solved

With osqp default settings

avikde commented 4 years ago

This is now basically working but it looks like this robot is more even left and right now. Need to get kinematic features again?

avikde commented 4 years ago

Should merge this in. Try to combine the momentum reference kpmom(pdes - p) into the QP?