Got a solution for Single Stance One walking

I also moved costFunction and packConstraints to the shared folder, and
changed a bit of the IO for them. There were also a variety of changes
made so that the code is more generalizable.

I added a lot of code to make a sample trajectory for single stance on
the first foot.