quadruped planning seems ok, need to rotate constraints by 90 degrees, need to sort initial com repartition