search

HaoruXue / spline-trajectory-optimization

Spline-based Trajectory Optimization Tool for Autonomous Racing (Indy Autonomous Challenge)
Apache License 2.0
10 stars 8 forks source link

unable to traj_opt_double_track with example #3

Closed mdlopezme closed 3 months ago

mdlopezme commented 6 months ago

I am running traj_opt_double_track in spline_traj_optm/min_time_optm/example to be able to generate a ttl. PS. The var I changed is interval to 10 (Just for testing). But it doesn't work for 1.0 either. I get this 

root@moises-legion:/WARREN# traj_opt_double_track 
/usr/local/lib/python3.10/dist-packages/matplotlib/projections/__init__.py:63: UserWarning: Unable to import Axes3D. This may be due to multiple versions of Matplotlib being installed (e.g. as a system package and as a pip package). As a result, the 3D projection is not available.
  warnings.warn("Unable to import Axes3D. This may be due to multiple versions of "
/spline-trajectory-optimization/spline_traj_optm/models/trajectory.py:229: IntegrationWarning: The occurrence of roundoff error is detected, which prevents 
  the requested tolerance from being achieved.  The error may be 
  underestimated.
  length, err = quad(self.__integrate_length, t_min, t_max, limit=1000)

******************************************************************************
This program contains Ipopt, a library for large-scale nonlinear optimization.
 Ipopt is released as open source code under the Eclipse Public License (EPL).
         For more information visit https://github.com/coin-or/Ipopt
******************************************************************************

This is Ipopt version 3.14.11, running with linear solver MUMPS 5.4.1.

Number of nonzeros in equality constraint Jacobian...:     6642
Number of nonzeros in inequality constraint Jacobian.:     2870
Number of nonzeros in Lagrangian Hessian.............:     8118

Total number of variables............................:      902
                     variables with only lower bounds:        0
                variables with lower and upper bounds:        0
                     variables with only upper bounds:        0
Total number of equality constraints.................:      656
Total number of inequality constraints...............:      984
        inequality constraints with only lower bounds:      164
   inequality constraints with lower and upper bounds:      410
        inequality constraints with only upper bounds:      410

iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
   0  5.1246256e+01 8.89e+00 8.48e-01  -1.0 0.00e+00    -  0.00e+00 0.00e+00   0
   1  7.5428540e+01 4.63e+01 2.89e+03  -1.0 2.14e+01    -  2.54e-01 5.31e-01H  1
   2  7.5643820e+01 4.55e+01 2.74e+03  -1.0 4.66e+00   2.0 5.74e-01 2.26e-02h  1
   3  7.7536591e+01 5.90e+01 2.10e+03  -1.0 4.55e+00   1.5 7.32e-01 1.89e-01h  1
   4  8.0618324e+01 5.86e+01 2.13e+03  -1.0 3.48e+00   1.0 6.47e-01 2.82e-01h  1
   5  8.1265742e+01 5.48e+01 2.05e+03  -1.0 2.41e+00   1.5 9.99e-01 6.69e-02h  1
   6  8.3164074e+01 4.33e+01 2.57e+03  -1.0 2.23e+00   1.9 1.00e+00 2.10e-01h  1
   7  8.4165556e+01 3.82e+01 2.83e+03  -1.0 1.78e+00   1.4 4.92e-01 1.19e-01h  1
   8  8.4991655e+01 3.38e+01 2.69e+03  -1.0 1.53e+00   1.8 1.00e+00 1.13e-01h  1
   9  9.0326386e+01 2.61e+01 5.53e+03  -1.0 1.40e+00   1.4 7.97e-02 7.48e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  10  9.0910169e+01 1.96e+01 4.22e+03  -1.0 3.26e-01   1.8 5.26e-01 2.57e-01h  1
  11  9.2201462e+01 6.88e+00 1.99e+03  -1.0 2.45e-01   1.3 1.68e-01 7.30e-01h  1
  12  9.2607087e+01 1.49e+00 2.71e+03  -1.0 1.04e-01   0.8 1.84e-01 1.00e+00f  1
  13  9.2280552e+01 5.36e-01 4.72e+02  -1.0 8.37e-02   0.4 6.16e-01 1.00e+00f  1
  14  9.1351136e+01 2.12e+00 1.73e+02  -1.0 1.37e-01  -0.1 6.98e-01 1.00e+00f  1
  15  8.8940057e+01 3.21e+00 3.46e+01  -1.0 2.56e-01  -0.6 9.72e-01 1.00e+00f  1
  16  8.3690817e+01 1.24e+01 4.68e+01  -1.0 1.45e+00  -1.1 3.82e-01 1.00e+00f  1
  17  7.7001403e+01 3.96e+01 1.16e+01  -1.0 1.20e+00  -1.5 1.00e+00 1.00e+00f  1
  18  7.1979406e+01 2.40e+01 3.72e+00  -1.0 2.35e+00  -2.0 1.00e+00 8.24e-01f  1
  19  6.6053110e+01 2.02e+01 4.14e+01  -1.0 8.59e+01    -  3.49e-01 4.22e-01f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  20  6.4066381e+01 2.24e+01 3.22e+01  -1.0 7.37e+01    -  1.26e-01 2.44e-01h  1
  21  6.4235885e+01 3.77e+01 4.78e+01  -1.0 1.84e+01  -2.5 1.49e-01 1.00e+00f  1
  22  6.0207397e+01 5.19e+03 6.00e+01  -1.0 9.18e+01    -  5.04e-01 7.83e-01f  1
  23  6.1033290e+01 3.20e+03 4.77e+01  -1.0 3.10e+01  -2.1 3.01e-01 7.12e-01h  1
  24  6.1096238e+01 3.13e+03 4.53e+01  -1.0 1.08e+01  -1.6 1.66e-01 4.03e-02h  1
  25  6.2635021e+01 2.61e+02 7.98e+01  -1.0 2.01e+01  -1.2 5.31e-01 1.00e+00h  1
  26  6.2581543e+01 2.00e+02 5.50e+01  -1.0 1.97e+00  -0.8 1.41e-01 3.09e-01h  1
  27  6.2697946e+01 2.88e+02 1.36e+02  -1.0 1.74e+01  -1.3 4.81e-02 9.13e-02f  2
  28  6.2803865e+01 2.59e+02 1.86e+02  -1.0 2.20e+00   0.1 9.98e-02 5.24e-01h  1
  29  6.2803592e+01 2.03e+02 1.95e+02  -1.0 4.09e+00  -0.4 1.12e-02 2.67e-01f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  30  6.2814296e+01 1.59e+02 2.00e+02  -1.0 1.65e+00   0.0 1.00e+00 2.41e-01h  1
  31  6.2986422e+01 2.24e+01 4.08e+02  -1.0 2.03e+00  -0.5 3.10e-01 9.76e-01h  1
  32  6.2881126e+01 6.03e+01 6.86e+01  -1.0 9.34e-01  -0.9 1.78e-01 1.00e+00f  1
  33  6.2782121e+01 2.10e+01 1.47e+01  -1.0 4.55e-01  -0.5 8.61e-01 1.00e+00h  1
  34  6.2605288e+01 7.44e+00 6.28e+00  -1.0 3.10e-01  -1.0 1.00e+00 1.00e+00h  1
  35  6.2054690e+01 9.84e+00 5.56e+00  -1.0 2.36e+02    -  2.06e-01 9.03e-02f  2
  36  6.1628330e+01 3.16e-01 8.47e-01  -1.0 5.78e+00  -1.5 1.00e+00 1.00e+00h  1
  37  5.9042965e+01 6.66e+00 1.07e+01  -1.7 1.92e+00  -2.0 7.23e-01 1.00e+00f  1
  38  5.6306634e+01 3.01e+01 8.14e+00  -1.7 2.71e+00  -2.4 9.26e-01 9.29e-01h  1
  39  5.5129935e+01 1.65e+01 3.26e+00  -1.7 4.81e+00  -2.9 1.00e+00 5.94e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  40  5.4815799e+01 2.61e+01 3.19e+00  -1.7 5.79e+01  -3.4 4.70e-01 9.60e-02h  1
  41  5.3869485e+01 3.30e+01 2.71e+00  -1.7 5.01e+01  -3.9 9.72e-01 2.90e-01h  1
  42  5.1600331e+01 4.07e+02 2.05e+00  -1.7 9.31e+01    -  4.37e-01 7.56e-01h  1
  43  5.1660917e+01 7.92e-01 5.07e-01  -1.7 4.96e+01    -  1.00e+00 1.00e+00h  1
  44  5.1664038e+01 1.14e-01 1.67e-01  -1.7 1.81e+01    -  1.00e+00 1.00e+00h  1
  45  5.1667252e+01 7.90e-02 6.70e-02  -1.7 8.86e+00    -  1.00e+00 1.00e+00h  1
  46  5.1664440e+01 1.02e-01 1.12e-01  -1.7 4.80e+00  -4.3 1.00e+00 1.00e+00h  1
  47  5.1667460e+01 1.68e-03 7.80e-02  -1.7 4.79e+00    -  1.00e+00 1.00e+00H  1
  48  5.0929513e+01 1.01e+00 1.95e+00  -2.5 5.88e+00  -3.9 7.81e-01 4.08e-01f  1
  49  4.9754409e+01 8.64e+00 1.48e+00  -2.5 1.69e+01    -  1.00e+00 7.64e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  50  4.9516694e+01 9.80e+00 1.65e+00  -2.5 9.83e+00    -  1.00e+00 1.00e+00h  1
  51  4.9517081e+01 6.84e-02 1.17e+00  -2.5 2.84e+00    -  1.00e+00 1.00e+00h  1
  52  4.9516217e+01 1.67e-02 1.17e-01  -2.5 2.90e-01    -  1.00e+00 1.00e+00h  1
  53  4.9514957e+01 6.02e-04 5.25e-02  -2.5 6.68e-02    -  1.00e+00 1.00e+00h  1
  54  4.9514947e+01 5.00e-08 8.30e-05  -2.5 1.31e-03    -  1.00e+00 1.00e+00h  1
  55  4.9345923e+01 1.74e+00 7.66e+00  -3.8 8.39e+00    -  8.74e-01 5.02e-01f  1
  56  4.9235576e+01 1.20e+00 7.82e+00  -3.8 5.51e+00    -  9.94e-01 6.88e-01h  1
  57  4.9190743e+01 5.09e-01 1.12e+00  -3.8 3.16e+00    -  1.00e+00 1.00e+00h  1
  58  4.9191495e+01 2.90e-01 1.03e+00  -3.8 5.47e-01    -  1.00e+00 1.00e+00h  1
  59  4.9191115e+01 1.12e+00 7.21e-01  -3.8 7.87e-01    -  1.00e+00 1.00e+00h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  60  4.9190427e+01 8.12e-01 3.69e-01  -3.8 3.83e-01  -3.5 1.00e+00 1.00e+00h  1
  61  4.9189958e+01 6.75e-02 4.84e-02  -3.8 1.76e-01  -3.1 1.00e+00 1.00e+00h  1
  62  4.9189396e+01 6.93e-01 6.92e+00  -3.8 1.48e+00  -3.5 1.00e+00 2.65e-01h  2
  63  4.9188853e+01 7.89e-01 6.62e+00  -3.8 3.26e+00  -4.0 1.00e+00 9.64e-02h  2
  64  4.9187379e+01 2.08e-01 5.37e-02  -3.8 2.93e+00    -  1.00e+00 1.00e+00h  1
  65  4.9187361e+01 7.51e-03 1.85e-03  -3.8 3.13e-01    -  1.00e+00 1.00e+00h  1
  66  4.9187401e+01 5.29e-04 1.35e-05  -3.8 6.28e-02    -  1.00e+00 1.00e+00h  1
  67  4.9179161e+01 1.99e-02 2.01e+02  -5.0 1.24e+00    -  9.90e-01 5.15e-01f  1
  68  4.9172880e+01 3.09e-02 8.61e+01  -5.0 9.17e-01    -  1.00e+00 8.43e-01h  1
  69  4.9171838e+01 3.55e-03 6.88e+01  -5.0 2.77e-01    -  1.00e+00 9.41e-01h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  70  4.9171734e+01 2.41e-04 6.96e+00  -5.0 5.23e-02    -  1.00e+00 1.00e+00f  1
  71  4.9171735e+01 1.02e-09 9.82e+03  -5.0 1.91e-06   3.8 4.28e-01 1.00e+00h  1
  72  4.9171734e+01 2.29e-08 2.05e+00  -5.0 5.84e-06   3.4 1.00e+00 1.00e+00h  1
  73  4.9171732e+01 6.58e-07 1.12e+02  -5.0 3.09e-05   2.9 7.35e-01 1.00e+00h  1
  74  4.9171711e+01 6.58e-07 4.30e+02  -5.0 7.68e+00    -  7.15e-02 4.12e-04f  2
  75  4.9171714e+01 2.03e-06 2.57e-01  -5.0 4.51e-05   3.3 1.00e+00 1.00e+00h  1
  76  4.9171525e+01 1.92e-05 7.16e+02  -5.0 7.05e-02    -  1.95e-01 4.45e-01h  1
  77  4.9171523e+01 3.18e-07 5.58e-01  -5.0 2.46e-05   2.8 1.00e+00 1.00e+00f  1
  78  4.9171206e+01 4.58e-05 9.01e+01  -5.0 2.02e-01    -  5.24e-02 1.20e-01f  1
  79  4.9171199e+01 5.95e-06 3.28e-01  -5.0 8.84e-05   2.3 1.00e+00 1.00e+00f  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  80  4.9168721e+01 2.63e-03 7.64e+01  -5.0 1.03e+00    -  2.91e-02 1.92e-01f  1
  81  4.9168698e+01 4.02e-06 1.97e-01  -5.0 1.80e-04   1.9 1.00e+00 1.00e+00f  1
  82  4.9163693e+01 9.66e-03 9.16e-01  -5.0 1.34e+01    -  1.51e-02 2.98e-02f  1
  83  4.9163667e+01 1.04e-04 1.58e-02  -5.0 5.84e-04   1.4 1.00e+00 1.00e+00f  1
  84  4.9148726e+01 8.26e-02 4.94e-02  -5.0 2.09e+01    -  5.19e-02 5.27e-02f  1
  85  4.9146419e+01 8.38e-02 8.69e+00  -5.0 2.50e+01    -  2.49e-01 8.55e-03h  1
  86  4.9137115e+01 1.25e-01 1.59e+01  -5.0 1.59e+01    -  3.69e-01 4.32e-02f  1
  87  4.9133660e+01 5.36e-02 1.47e+01  -5.0 1.00e+00    -  1.00e+00 6.48e-01h  1
  88  4.9130897e+01 2.29e-02 4.83e+00  -5.0 5.20e-01    -  1.00e+00 6.66e-01f  1
  89  4.9128926e+01 5.12e-03 4.02e-01  -5.0 2.73e-01    -  1.00e+00 1.00e+00h  1
iter    objective    inf_pr   inf_du lg(mu)  ||d||  lg(rg) alpha_du alpha_pr  ls
  90  4.9128640e+01 1.35e-04 2.46e-02  -5.0 3.71e-02    -  1.00e+00 1.00e+00h  1
  91  4.9128634e+01 4.86e-08 2.95e-05  -5.0 8.36e-04    -  1.00e+00 1.00e+00h  1

Number of Iterations....: 91

                                   (scaled)                 (unscaled)
Objective...............:   4.9128634396586015e+01    4.9128634396586015e+01
Dual infeasibility......:   2.9539488473950826e-05    2.9539488473950826e-05
Constraint violation....:   4.6531913888225172e-10    4.8625850013195304e-08
Variable bound violation:   0.0000000000000000e+00    0.0000000000000000e+00
Complementarity.........:   9.0952340324345571e-06    9.0952340324345571e-06
Overall NLP error.......:   2.9539488473950826e-05    2.9539488473950826e-05

Number of objective function evaluations             = 112
Number of objective gradient evaluations             = 92
Number of equality constraint evaluations            = 112
Number of inequality constraint evaluations          = 112
Number of equality constraint Jacobian evaluations   = 92
Number of inequality constraint Jacobian evaluations = 92
Number of Lagrangian Hessian evaluations             = 91
Total seconds in IPOPT                               = 0.915

EXIT: Optimal Solution Found.
      solver  :   t_proc      (avg)   t_wall      (avg)    n_eval
       nlp_f  | 970.00us (  8.66us) 965.28us (  8.62us)       112
       nlp_g  |  18.16ms (162.10us)  18.08ms (161.46us)       112
  nlp_grad_f  |   1.24ms ( 13.35us)   1.17ms ( 12.55us)        93
  nlp_hess_l  | 296.98ms (  3.26ms) 297.14ms (  3.27ms)        91
   nlp_jac_g  |  68.69ms (738.61us)  68.85ms (740.31us)        93
       total  | 915.18ms (915.18ms) 915.18ms (915.18ms)         1
shape of X: (82, 6)
shape of scale_x: (6, 1)
Traceback (most recent call last):
  File "/usr/local/bin/traj_opt_double_track", line 33, in <module>
    sys.exit(load_entry_point('spline-traj-optm', 'console_scripts', 'traj_opt_double_track')())
  File "/spline-trajectory-optimization/spline_traj_optm/entrypoints/traj_opt_double_track.py", line 68, in main
    x = opti.debug.value(X) * scale_x.T + np.hstack(
  File "/usr/local/lib/python3.10/dist-packages/casadi/casadi.py", line 11012, in __array_ufunc__
    raise e
  File "/usr/local/lib/python3.10/dist-packages/casadi/casadi.py", line 11005, in __array_ufunc__
    return fun(*inputs[1:])
  File "/usr/local/lib/python3.10/dist-packages/casadi/casadi.py", line 9040, in __rmul__
    def __rmul__(x, y): return _casadi.times(y, x)
RuntimeError: .../casadi/core/matrix_impl.hpp:1358: Dimension mismatch for (x*y), x is 82x6, while y is 6x1

Please halp pls.

mdlopezme commented 6 months ago

image This seems to work, can you verify is gives the correct results?