fn for chomp to optimize constructed trajectory