RobotLocomotion/drake — pull request #619 by rdeits (2014-12-15 17:20:17+00:00)
Title: Clean up and clarify ZMP closed-form solution

This makes a few internal changes to the closed-form ZMP solution to more closely match the corrected math in the paper. The code is now correct for arbitrary cost scaling (even though such scaling actually has no effect). We also now verify the computed comtraj by simulating with the cost-to-go from both the closed form and from TVLQR.

In addition, I fixed a minor issue with MultiCoordinateFrames which failed when given a frame of size 0 because `sizecheck(zeros(0,1), 0)` fails. This was preventing me from simulating using the closed-form controller because it produced a function handle trajectory of size 0 for the (empty) discrete states.