search

vincekurtz / drake_ddp

Differential Dynamic Programming and Iterative LQR in Drake
MIT License
44 stars 11 forks source link

Added early version of first order dynamcis derivatives - basic ideas from this paper (https://eprints.whiterose.ac.uk/197059/1/ICRA23_0389_FI.pdf) #3

Closed DMackRus closed 10 months ago

DMackRus commented 11 months ago

Hi Vince,

I have been doing some testing with my dynamics derivatives interpolation methods with drake using your base code and models. Got some quite good early results for some of your models.

In this pull request, I amended you iLQR algorithm to use a new get derivatives function which computes key-points over the trajectory and compute the derivatives via autodiff there, then in between these key-points it interpolates the matrices using linear interpolation. You can pass in a key-point interpolation method into your ilqr algorithm, or if nothing is passed in, it simply defaults to the baseline case where all derivatives are computed via auto diff.

I also made some slight changes to some of the tasks to show the power of inteprolating the derivatives a bit more (time horizons and time steps for some tasks like the acrobot) to showcase the time save that can be achieved.

There shouldn't be any detrimental effect from adding my code in as the baseline case still works as you had it before, please let me know if you like it or have any questions or concerns :).

DMackRus commented 11 months ago

Hi Vince, Think ive made all the changes you suggested. The code should be a lot neater now. All the tasks have some ok values for the interpolation methods, might try and improve on these in the future!

Also added some documentation for the derivatives interpolation stuff and linked to my paper

DMackRus commented 11 months ago

Hi Vince, just modified the kinova and mini cheetah interpolation settings to try and squeeze some more performance out of them, I think you will particularly like the mini cheetahs improved optimization time (iterations reduced from around 11s to about 2 or seconds). The kinova manipulation time is improved also, one interesting thing is with interpolation it actually takes longer overall as it manages to keep reducing the cost for more iterations than the baseline (it reaches the final cost of the baseline much faster which is the main thing). So i guess it could be worth capping the max iterations maybe.

For the cheetah, I think potentially more speed up could be achieved but I kept running into an error where the linesearch failed to find an improvement occasionally and stopped the optimization (I think maybe this could be changed as it those instances it had already reduced the cost substantially so maybe the optimisation should converge in these instances instead of crashing.

I cant seem to get it to work with the cart pole wall example yet, but for every other model it works very well :).

vincekurtz commented 10 months ago

Merged! Thanks for the contribution @DMackRus!