Now CoM can change between each iteration. I have to use 4'th order polynomial to make it change. I do not like this high order polynomial