Joint trajectory optimization complete