Nondim of trajectory gen fixed, write-up updated, joint optimization still not 100% correct