Inertial Wheel Pendulum Swing up, from MIT6.832 Underactuated Robotics

formulation as follows:

作业推荐了几段impressive的视频，
https://www.youtube.com/watch?v=eQiu3BKMPVk,
https://www.youtube.com/watch?v=3YYT-1tHdFk,
https://www.youtube.com/watch?v=woCdjbsjbPg,
https://www.youtube.com/watch?v=n_6p-1J551Y\
说起来好久上不去了。。。

Swing up是一个经典问题了，可惜我现在几乎还只会energy shaping。 其他的控制方案，我知道有几种可以，包括SMC, 但是我自己还没拿出时间试过。 这里的文献其实值得花时间去读。作业里面有文献列表。 此外冯纯伯先生的弟子XinXin在这方面做了出色的工作，似乎也是energy based的。

本题结果没啥好说的：

另，这道题与其他的一个不同是，代码执行速度成了问题。test里面必须30s内能够通过8个数据才能拿最后4分。 我还提交issue确认了这件事情。但是好不容易通过了，然而后来在 20190423版本下速度又慢下来了，这是drake版本问题，不关我的事。