Add support for cubic polynomial interpolation for the CoM trajectory
Fix a bug on scaling the comddot in P-step. The current results have two suspicious features
1. In NLP result, the pitch angular momentum always drift away from 0
2. In NLP result, the com z height goes up first and then goes down