More improvements

Main additions/improvements :

• Faster OpenRAVE trajectory extraction (thanks to Rosen)
• Recovery mechanism when integration fails
• PolygonConstraints class to handle redundant actuation (cf. http://www.ntu.edu.sg/home/cuong/docs/overactuated.pdf)