I use scipy.optimize.minimize to obtain the optimal lane-change duration when planning the trajactory.
the following is my code:
def laneChangePlanning(self, x_0, v_x0, a_x0, adjLeader, adjFollower, curLeader, y_0, v_y0, a_y0, y_t, laneIndex):
v_yt, a_yt = 0, 0
args = (x_0, v_x0, a_x0, y_0, v_y0, a_y0, y_t, v_yt, a_yt, adjLeader, adjFollower, curLeader, laneIndex)
cons = ({'type': 'ineq', 'fun': self.constraints(args), 'jac': self.jacs(args)})
bounds = Bounds([3.0], [7.0])
x0 = np.array([4.0])
res = minimize(fun=self.calObject(args), x0=x0, method='SLSQP', constraints=cons, bounds=bounds)
t_plan = res.x[0]
the self.constraints is a little complicated, so i just put the output:
res = np.concatenate((np.array(distCurLeader), np.array(distAdjLeader),
np.array(distAdjFollower), np.array(vx_ego), np.array(ax_ego),
np.array(ay_ego)))
the shape is {tuple:1} 360:
enter image description here
Also, i just put the output of self.jacs(args):
res = np.concatenate((np.array(jac_distCurLeader), np.array(jac_distAdjLeader),
np.array(jac_distAdjFollower), np.array(jac_vx_ego), np.array(jac_ax_ego), np.array(jac_ay_ego)), axis=0)
the shape is {tuple:2} (360, 1), as follows:
however, the exceptions are:
ValueError: 0-th dimension must be fixed to 378 but got 360
The above exception was the direct cause of the following exception:
Traceback (most recent call last):
File "C:\Users\chong\PycharmProjects\sumo_test\test.py", line 35, in <module>
obs, reward, done, _, info = env.step(action)
File "C:\ProgramData\Anaconda3\envs\stableBaseline\lib\site-packages\gymnasium\wrappers\order_enforcing.py", line 56, in step
return self.env.step(action)
File "C:\ProgramData\Anaconda3\envs\stableBaseline\lib\site-packages\gymnasium\wrappers\env_checker.py", line 51, in step
return self.env.step(action)
File "C:\Users\chong\PycharmProjects\sumo_test\sumo_env\test_env.py", line 700, in step
vx_ego, xy_ego, delta_ts = self.laneChangePlanning(0, egoState[1], egoState[2], adjRightLeader,
File "C:\Users\chong\PycharmProjects\sumo_test\sumo_env\test_env.py", line 406, in laneChangePlanning
res = minimize(fun=self.calObject(args), x0=x0, method='SLSQP', constraints=cons, bounds=bounds)
File "C:\ProgramData\Anaconda3\envs\stableBaseline\lib\site-packages\scipy\optimize\_minimize.py", line 719, in minimize
res = _minimize_slsqp(fun, x0, args, jac, bounds,
File "C:\ProgramData\Anaconda3\envs\stableBaseline\lib\site-packages\scipy\optimize\_slsqp_py.py", line 422, in _minimize_slsqp
slsqp(m, meq, x, xl, xu, fx, c, g, a, acc, majiter, mode, w, jw,
ValueError: failed in converting 9th argument `a' of _slsqp.slsqp to C/Fortran array
Through the source code, it seems that the 0-th dimension of 'a' should be 378, rather than 360. The 0-th dimension of 'a' is obtained through the dimension of 'fun' and 'jac'. But I can't figure out how the '378' is calculated.
I really appreciate it if someone could debug this error.