solid trajectory optimization