Add a modified osqp_solver that saves the osqp object

yangwill commented 3 years ago

This solver uses osqp but saves the osqp object and corresponding data objects to prevent unnecessary work constructing and destructing.

It does this by adding a function InitializeSolver which creates the OSQPWorkspace object. Provided the structure of the mathematical program does not change, calling Solve will now only update the costs and constraints. This along with the now-enabled native warm starting of osqp, speeds up the solve time significantly.

Testing on my lab desktop with the simulator running at 2000Hz, (so max controller rate is 2000Hz): Controller frequency of the walking controller: ~800Hz (drake osqp_solver), ~1200Hz (this PR) Controller frequency of the standing controller: ~1450Hz (drake osqp_solver), ~1700Hz (this PR)

This change is

yangwill commented 3 years ago

Added joint limits in the OSC, ready for final review.

yangwill commented 2 years ago

Summary of the changes in this branch

Modified OSQPSolver that stores the osqp workspace and warmstarts the solve
Added a gflag to the state estimator for the ontact force threshold
Added joint limits as a cost in the OSC

DAIRLab / dairlib

Add a modified osqp_solver that saves the osqp object #244