Added CoM position cost model
This PR tackles the issue #209 (closed). Additionally, it merges the latest changes in the legacy Python code from devel branch.
This PR tackles the issue #209 (closed). Additionally, it merges the latest changes in the legacy Python code from devel branch.