Derived and wrote functions for double stance

Still need to write code for double stance trajectory optimization