Closed hect1995 closed 4 years ago
First you need to change the dynamics and add one state, second, the trailer is a second 'ego' obstacle, thus you need to add the same obstacle avoidance constraint as for the truck also for the trailer, with changed center, shape and rotation. This makes the problem quite a bit slower to solve.
The more complex part is how to get a good warm start, because hybrid A* does not really work for the truck with trailer. I solved this by first solving the "parking" problem without obstacle constraint (but some heuristic bounds approximating the obstacles) and then used this as a warm start for the problem with obstacles.
I ask George if we can also add the truck example on github.
Best, Alex
I would be much appreciated. In the meantime I have start developing your suggestion and the changes I have done are:
As you state in the paper I have discretized the dynamics of the alpha using second-order Runge-Kutta method obtaining:
Which I introduce it in Julia as:
@NLconstraint(m, x[4,i+1] == x[4,i] - timeScale[i]*Ts*(x[5,i] + timeScale[i]*Ts/2*u[2,i])*(sin(x[4,i]+0.5*timeScale[i]*Ts*(-x[5,i]*(sin(x[4,i])/L_trailer + tan(u[1,i])/L)))/L_trailer + tan(u[1,i])/L))
As my state vector is for the pose in the rear axle of the truck, to obtain the pose for the rear axle of the trailer I do:
The resulting trajectory does not make sense (I attach a Plot) and I think the math is correct.
Another issue I have is that now runs far slower. It plots this:
Number of nonzeros in equality constraint Jacobian...: 88176
Number of nonzeros in inequality constraint Jacobian.: 70344
Number of nonzeros in Lagrangian Hessian.............: 342741
Total number of variables............................: 20998
variables with only lower bounds: 0
variables with lower and upper bounds: 0
variables with only upper bounds: 0
Total number of equality constraints.................: 13652
Total number of inequality constraints...............: 25197
inequality constraints with only lower bounds: 21000
inequality constraints with lower and upper bounds: 4197
inequality constraints with only upper bounds: 0
iter objective inf_pr inf_du lg(mu) ||d|| lg(rg) alpha_du alpha_pr ls
0 4.4513169e+04 1.47e+00 1.00e+02 -1.0 0.00e+00 - 0.00e+00 0.00e+00 0
And after some time (around 30s) starts doing more iterations faster but the 0th iteration appears alone for those 30s without any other iteration appearing.
Do you see any issue in my approach. Thanks a lot back again
This may help you to get started, this code computes a trajectory from x0 to xF, without obstacle constraints.
The parameters are:
x0 = [-5 11 0 0 0]
xF = [0 0 pi/2 pi/2 0]
N = 100
Ts = 1
L = 3.7
D = 7
m = Model(solver=IpoptSolver(hessian_approximation="exact",mumps_pivtol=1e-6,alpha_for_y="min",recalc_y="yes",
mumps_mem_percent=6000,max_iter=800,tol=1e-4, print_level=0,
min_hessian_perturbation=1e-12,jacobian_regularization_value=1e-7))
#state
@variable(m, x[1:5,1:(N+1)])
@variable(m, timeScale[1:N+1])
#control
@variable(m, u[1:2,1:(N)])
# cost function
@NLobjective(m, Min,sum(0.1*u[1,i]^2 + 1*u[2,i]^2 for i = 1:N) +
sum(100*((u[1,i+1]-u[1,i])/(timeScale[i]*Ts))^2 + 100*((u[2,i+1]-u[2,i])/(timeScale[i]*Ts))^2 for i = 1:N-1) +
sum(0.5*timeScale[i] + 1*timeScale[i]^2 for i =1:N+1)) # with potential function
#input constraints
@constraint(m, [i=1:N], -0.6 <= u[1,i] <= 0.6)
@constraint(m, [i=1:N], -0.4 <= u[2,i] <= 0.4)
#state constraints
@constraint(m, [i=1:N+1], -22 <= x[1,i] <= 22)
@constraint(m, [i=1:N+1], -0.1 <= x[2,i] <= 13)
# @constraint(m, [i=61:N+1], -0.1 <= x[2,i] <= 13)
# @constraint(m, [i=1:60], 6 <= x[2,i] <= 13)
@constraint(m, [i=1:N+1], -15 <= x[3,i] <= 15)
@constraint(m, [i=1:N+1], -15 <= x[4,i] <= 15)
@constraint(m, [i=1:N+1], -0.5 <= x[5,i] <= 1)
@constraint(m, 0.4 .<= timeScale .<= 1.25)
#starting point
@constraint(m, x[1,1] == x0[1])
@constraint(m, x[2,1] == x0[2])
@constraint(m, x[3,1] == x0[3])
@constraint(m, x[4,1] == x0[4])
@constraint(m, x[5,1] == x0[5])
#end point
@constraint(m, x[1,N+1] == xF[1])
@constraint(m, x[2,N+1] == xF[2])
@constraint(m, x[3,N+1] == xF[3])
@constraint(m, x[4,N+1] == xF[4])
@constraint(m, x[5,N+1] == xF[5])
#unicycle dynamic with euler forward and x[5] is a pseudo time
for i in 1:N
@NLconstraint(m, x[1,i+1] == x[1,i] + timeScale[i]*Ts*x[5,i]*cos(x[3,i]))
@NLconstraint(m, x[2,i+1] == x[2,i] + timeScale[i]*Ts*x[5,i]*sin(x[3,i]))
@NLconstraint(m, x[3,i+1] == x[3,i] + timeScale[i]*Ts*x[5,i]*u[1,i]/L)
@NLconstraint(m, x[4,i+1] == x[4,i] + timeScale[i]*Ts*x[5,i]/D*sin(x[3,i]-x[4,i]))
@NLconstraint(m, x[5,i+1] == x[5,i] + timeScale[i]*Ts*u[2,i])
@constraint(m,timeScale[i] == timeScale[i+1])
end
@constraint(m, [i=1:N+1], (x[3,i]-x[4,i])^2 <= 0.8^2)
u0 = [0,0]
for i in 1:N
if i==1
@NLconstraint(m,-0.6<=(u0[1]-u[1,i])/(timeScale[i]*Ts) <= 0.6)
# @NLconstraint(m,-1<=(u0[2]-u[2,i])/(timeScale[i]*Ts) <= 1)
else
@NLconstraint(m,-0.6<=(u[1,i-1]-u[1,i])/(timeScale[i]*Ts) <= 0.6)
# @NLconstraint(m,-1<=(u[2,i-1]-u[2,i])/(timeScale[i]*Ts) <= 1)
end
end
# xWS = [repmat([11;11;x0[3];x0[4];0.1],1,60) repmat([0;7;xF[3];xF[4];0.1],1,41)]#ones(5,N+1)#
for b = 1:5
xWS[b,:] = linspace(x0[b],xF[b],N+1)
end
uWS = 0.2*ones(2,N)
# xWS, uWS = WarmStart01(N,Ts,L,x0,xF)
setvalue(timeScale,0.8*ones(N+1,1))
setvalue(x,xWS)
setvalue(u,uWS)
tic()
status = solve(m)
time = toq();
# plot results
xp = getvalue(x)'
up = getvalue(u)'
timeScalep = getvalue(timeScale)
close("all")
# figure(1)
# plot(xp);
for i = 1:1:N+1
figure(2)
clf()
title("warm start")
plot([ob1[1],ob1[1],-ob1[3],-ob1[3],ob1[1]],[ob1[2],-ob1[4],-ob1[4],ob1[2],ob1[2]],"g")
plot([ob2[1],ob2[1],-ob2[3],-ob2[3],ob2[1]],[ob2[2],-ob2[4],-ob2[4],ob2[2],ob2[2]],"g")
plot([ob3[1],ob3[1],-ob3[3],-ob3[3],ob3[1]],[ob3[2],-ob3[4],-ob3[4],ob3[2],ob3[2]],"g")
R = [cos(xp[i,3]) -sin(xp[i,3]);sin(xp[i,3]) cos(xp[i,3])];
obR = [R*[ ego1[1]; ego1[2]];
R*[ ego1[1];-ego1[4]];
R*[-ego1[3];-ego1[4]];
R*[-ego1[3]; ego1[2]];
R*[ ego1[1]; ego1[2]]]
plot(xp[i,1]+[obR[1],obR[3],obR[5],obR[7],obR[9]],xp[i,2]+[obR[2],obR[4],obR[6],obR[8],obR[10]],"k")
xt = [xp[i,1] - D*cos(xp[i,4]);
xp[i,2] - D*sin(xp[i,4])]
Rt = [cos(xp[i,4]) -sin(xp[i,4]);sin(xp[i,4]) cos(xp[i,4])];
obRt = [Rt*[ ego2[1]; ego2[2]];
Rt*[ ego2[1];-ego2[4]];
Rt*[-ego2[3];-ego2[4]];
Rt*[-ego2[3]; ego2[2]];
Rt*[ ego2[1]; ego2[2]]]
plot(xt[1]+[obRt[1],obRt[3],obRt[5],obRt[7],obRt[9]],xt[2]+[obRt[2],obRt[4],obRt[6],obRt[8],obRt[10]],"k")
plot(xp[1:i,1],xp[1:i,2],"b")
axis("equal")
axis([-25,25,-12,25])
sleep(0.05)
end
Without considering obstacles your results provides me a nice result for your environment but in the next step, when the obstacles are added results in this:
The result from the warm-start you send is the one I use to initialize my xWS
, uWS
and timeScale
In the other files the only changes that I make are:
for i in 1:N
# sampling time is variable
@NLconstraint(m, x[2,i+1] == x[2,i] + timeScale[i]*Ts*x[5,i]*sin(x[3,i]))
@NLconstraint(m, x[3,i+1] == x[3,i] + timeScale[i]*Ts*x[5,i]*tan(u[1,i])/L)
@NLconstraint(m, x[4,i+1] == x[4,i] + timeScale[i]*Ts*x[5,i]/D*sin(x[3,i]-x[4,i]))
@NLconstraint(m, x[5,i+1] == x[5,i] + timeScale[i]*Ts*u[2,i])
@constraint(m, timeScale[i] == timeScale[i+1])
end
Adding ergo vehicle, which is similar to what I have but repeated twice. For the constraints of the second vehicle (trailer) where there was x[1,i] for the truck for the trailer it will be x[1,i] - L*cos(x[4,i])
, x[2,i] now is x[2,i] - L*sin(x[4,i])
and x[3] is x[4]
ego_truck = ego1
ego_trailer = ego2
W_ev_truck = ego_truck[2]+ego_truck[4]
L_ev_truck = ego_truck[1]+ego_truck[3]
g_truck = [L_ev_truck/2,W_ev_truck/2,L_ev_truck/2,W_ev_truck/2]
# ofset from X-Y to the center of the ego set
offset_truck = (ego_truck[1]+ego_truck[3])/2 - ego_truck[3]
### TRAILER ###
# width and length of ego set
W_ev_trailer = ego_trailer[2]+ego_trailer[4]
L_ev_trailer = ego_trailer[1]+ego_trailer[3]
g_trailer = [L_ev_trailer/2,W_ev_trailer/2,L_ev_trailer/2,W_ev_trailer/2]
# ofset from X-Y to the center of the ego set
offset_trailer = (ego_trailer[1]+ego_trailer[3])/2 - ego_trailer[3]
for i in 1:N+1 # iterate over time steps
for j = 1 : nOb # iterate over obstacles
Aj = A[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract obstacle matrix associated with j-th obstacle
lj = l[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract lambda dual variables associate j-th obstacle
nj = n[(j-1)*4+1:j*4 ,:] # extract mu dual variables associated with j-th obstacle
bj = b[sum(vOb[1:j-1])+1 : sum(vOb[1:j])] # extract obstacle matrix associated with j-th obstacle
# norm(A'*lambda) = 1
@NLconstraint(m, (sum(Aj[k,1]*lj[k,i] for k = 1 : vOb[j]))^2 + (sum(Aj[k,2]*lj[k,i] for k = 1 : vOb[j]))^2 == 1 )
### TRUCK ###
@NLconstraint(m, (nj[1,i] - nj[3,i]) + cos(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + sin(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
@NLconstraint(m, (nj[2,i] - nj[4,i]) - sin(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + cos(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
@NLconstraint(m, (-sum(g_truck[k]*nj[k,i] for k = 1:4) + (x[1,i]+cos(x[3,i])*offset_truck)*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j])
+ (x[2,i]+sin(x[3,i])*offset_truck)*sum(Aj[k,2]*lj[k,i] for k=1:vOb[j]) - sum(bj[k]*lj[k,i] for k=1:vOb[j])) >= dmin )
### TRAILER ###
@NLconstraint(m, (nj[1,i] - nj[3,i]) + cos(x[4,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + sin(x[4,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
@NLconstraint(m, (nj[2,i] - nj[4,i]) - sin(x[4,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + cos(x[4,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
@NLconstraint(m, (-sum(g_trailer[k]*nj[k,i] for k = 1:4) + (x[1,i] - L*cos(x[4,i])+cos(x[4,i])*offset_trailer)*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j])
+ (x[2,i] - L*sin(x[4,i])+sin(x[4,i])*offset_trailer)*sum(Aj[k,2]*lj[k,i] for k=1:vOb[j]) - sum(bj[k]*lj[k,i] for k=1:vOb[j])) >= dmin )
end
end
Am I missing something? Both with your initilization and with my initialization (I have a path obtained from RRT* for a truck and trailer) the results I obtain make no sense, mainly the values of the new state created, which is the yaw of the trailer.
I think the previous code plus this should allow you to get it working
#control
@variable(m, u[1:2,1:(N)])
# lagrange multipliers for dual dist function
@variable(m, l1[1:sum(vOb),1:(N+1)]) # dual multiplier associated with obstacleShape
@variable(m, n1[1:nOb*4,1:(N+1)]) # dual multiplier associated with carShape
@variable(m, l2[1:sum(vOb),1:(N+1)]) # dual multiplier associated with obstacleShape
@variable(m, n2[1:nOb*4,1:(N+1)]) # dual multiplier associated with carShape
#input constraints
@constraint(m, [i=1:N], -0.6 <= u[1,i] <= 0.6)
@constraint(m, [i=1:N], -0.4 <= u[2,i] <= 0.4)
#state constraints
@constraint(m, [i=1:N+1], XYbounds[1] <= x[1,i] <= XYbounds[2])
@constraint(m, [i=1:N+1], XYbounds[3] <= x[2,i] <= XYbounds[4])
@constraint(m, [i=1:N+1], -15 <= x[3,i] <= 15)
@constraint(m, [i=1:N+1], -15 <= x[4,i] <= 15)
@constraint(m, [i=1:N+1], -0.5 <= x[5,i] <= 1)
# bounds on time scaling
if fixTime == 0
@constraint(m, 0.8 .<= timeScale .<= 1.2)
end
# timeScalefixed = 1
# positivity constraints on dual multipliers
@constraint(m, l1 .>= 0 )
@constraint(m, n1 .>= 0)
@constraint(m, l2 .>= 0 )
@constraint(m, n2 .>= 0)
##############################
# obstacle avoidance constraints
##############################
# width and length of ego set
W_ev1 = ego1[2]+ego1[4]
L_ev1 = ego1[1]+ego1[3]
g1 = [L_ev1/2,W_ev1/2,L_ev1/2,W_ev1/2]
# ofset from X-Y to the center of the ego set
offset1 = (ego1[1]+ego1[3])/2 - ego1[3]
for i in 1:N+1 # iterate over time steps
for j = 1 : nOb # iterate over obstacles
Aj = A[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract obstacle matrix associated with j-th obstacle
lj = l1[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract lambda dual variables associate j-th obstacle
nj = n1[(j-1)*4+1:j*4 ,:] # extract mu dual variables associated with j-th obstacle
bj = b[sum(vOb[1:j-1])+1 : sum(vOb[1:j])] # extract obstacle matrix associated with j-th obstacle
# norm(A'*lambda) <= 1
@NLconstraint(m, (sum(Aj[k,1]*lj[k,i] for k = 1 : vOb[j]))^2 + (sum(Aj[k,2]*lj[k,i] for k = 1 : vOb[j]))^2 <= 1 )
# G'*mu + R'*A*lambda = 0
@NLconstraint(m, (nj[1,i] - nj[3,i]) + cos(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + sin(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
@NLconstraint(m, (nj[2,i] - nj[4,i]) - sin(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + cos(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
# -g'*mu + (A*t - b)*lambda > 0
@NLconstraint(m, -sum(g1[k]*nj[k,i] for k = 1:4) + (x[1,i]+cos(x[3,i])*offset1)*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j])
+ (x[2,i]+sin(x[3,i])*offset1)*sum(Aj[k,2]*lj[k,i] for k=1:vOb[j]) - sum(bj[k]*lj[k,i] for k=1:vOb[j]) >= dmin )
end
end
W_ev2 = ego2[2]+ego2[4]
L_ev2 = ego2[1]+ego2[3]
g2 = [L_ev2/2,W_ev2/2,L_ev2/2,W_ev2/2]
# ofset from X-Y to the center of the ego set
offset2 = (ego2[1]+ego2[3])/2 - ego2[3]
for i in 1:N+1 # iterate over time steps
for j = 1 : nOb # iterate over obstacles
Aj = A[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract obstacle matrix associated with j-th obstacle
lj = l2[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract lambda dual variables associate j-th obstacle
nj = n2[(j-1)*4+1:j*4 ,:] # extract mu dual variables associated with j-th obstacle
bj = b[sum(vOb[1:j-1])+1 : sum(vOb[1:j])] # extract obstacle matrix associated with j-th obstacle
# norm(A'*lambda) <= 1
@NLconstraint(m, (sum(Aj[k,1]*lj[k,i] for k = 1 : vOb[j]))^2 + (sum(Aj[k,2]*lj[k,i] for k = 1 : vOb[j]))^2 <= 1 )
# G'*mu + R'*A*lambda = 0
@NLconstraint(m, (nj[1,i] - nj[3,i]) + cos(x[4,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + sin(x[4,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
@NLconstraint(m, (nj[2,i] - nj[4,i]) - sin(x[4,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + cos(x[4,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
# -g'*mu + (A*t - b)*lambda > 0
@NLconstraint(m, -sum(g2[k]*nj[k,i] for k = 1:4) + (x[1,i]-cos(x[4,i])*offset2)*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j])
+ (x[2,i]-sin(x[4,i])*offset2)*sum(Aj[k,2]*lj[k,i] for k=1:vOb[j]) - sum(bj[k]*lj[k,i] for k=1:vOb[j]) >= dmin )
end
end
l1WS,n1WS = DualMultWS1(N,nOb,vOb, A, b,xWS[1,:],xWS[2,:],xWS[3,:])
l2WS,n2WS = DualMultWS2(N,nOb,vOb, A, b,xWS[1,:],xWS[2,:],xWS[4,:])
setvalue(l1,l1WS')
setvalue(n1,n1WS')
setvalue(l2,l2WS')
setvalue(n2,n2WS')
setvalue(x,xWS)
setvalue(u,0.5*uWS)
Normally warm starting the l
and n
with 0.1 works also fine.
Ok, I understand that you create a pair of dual multipliers for each ergo, but it still fails. The initialization without obstacles is correct so the obtained x
, u
and timeScale
I pass them to the original ParkingSignedDist().
x0 = [-5 11 0 0 0]
xF = [0 0 pi/2 pi/2 0]
N = 300#500
Ts = 1
L_truck = 3.7#3.6
L_trailer = 7#12.036
m = Model(solver=IpoptSolver(hessian_approximation="exact",mumps_pivtol=1e-6,alpha_for_y="min",recalc_y="yes",
mumps_mem_percent=6000,max_iter=800,tol=1e-4, print_level=0,
min_hessian_perturbation=1e-12,jacobian_regularization_value=1e-7))
#state
@variable(m, x[1:5,1:(N+1)])
@variable(m, timeScale[1:N+1])
#control
@variable(m, u[1:2,1:N])
# cost function
@NLobjective(m, Min,sum(0.1*u[1,i]^2 + 1*u[2,i]^2 for i = 1:N) +
sum(100*((u[1,i+1]-u[1,i])/(timeScale[i]*Ts))^2 + 100*((u[2,i+1]-u[2,i])/(timeScale[i]*Ts))^2 for i = 1:N-1) +
sum(0.5*timeScale[i] + 1*timeScale[i]^2 for i =1:N+1)) # with potential function
#input constraints
@constraint(m, [i=1:N], -0.6 <= u[1,i] <= 0.6)
@constraint(m, [i=1:N], -0.4 <= u[2,i] <= 0.4)
#state constraints
@constraint(m, [i=1:N+1], -22 <= x[1,i] <= 22)
@constraint(m, [i=1:N+1], -0.1 <= x[2,i] <= 13)
#@constraint(m, [i=1:N+1], -30 <= x[1,i] <= 100)
#@constraint(m, [i=1:N+1], -10 <= x[2,i] <= 100)
@constraint(m, [i=1:N+1], -15 <= x[3,i] <= 15)
@constraint(m, [i=1:N+1], -15 <= x[4,i] <= 15)
@constraint(m, [i=1:N+1], -0.5 <= x[5,i] <= 1)
@constraint(m, 0.2 .<= timeScale .<= 1.25)
#starting point
@constraint(m, x[1,1] == x0[1])
@constraint(m, x[2,1] == x0[2])
@constraint(m, x[3,1] == x0[3])
@constraint(m, x[4,1] == x0[4])
@constraint(m, x[5,1] == x0[5])
#end point
@constraint(m, x[1,N+1] == xF[1])
@constraint(m, x[2,N+1] == xF[2])
@constraint(m, x[3,N+1] == xF[3])
@constraint(m, x[4,N+1] == xF[4])
@constraint(m, x[5,N+1] == xF[5])
#unicycle dynamic with euler forward and x[5] is a pseudo time
for i in 1:N
@NLconstraint(m, x[1,i+1] == x[1,i] + timeScale[i]*Ts*x[5,i]*cos(x[3,i]))
@NLconstraint(m, x[2,i+1] == x[2,i] + timeScale[i]*Ts*x[5,i]*sin(x[3,i]))
@NLconstraint(m, x[3,i+1] == x[3,i] + timeScale[i]*Ts*x[5,i]*u[1,i]/L_truck)
@NLconstraint(m, x[4,i+1] == x[4,i] + timeScale[i]*Ts*x[5,i]/L_trailer*sin(x[3,i]-x[4,i]))
@NLconstraint(m, x[5,i+1] == x[5,i] + timeScale[i]*Ts*u[2,i])
@constraint(m,timeScale[i] == timeScale[i+1])
end
@constraint(m, [i=1:N+1], (x[3,i]-x[4,i])^2 <= 0.8^2)
u0 = [0,0]
for i in 1:N
if i==1
@NLconstraint(m,-0.6<=(u0[1]-u[1,i])/(timeScale[i]*Ts) <= 0.6)
else
@NLconstraint(m,-0.6<=(u[1,i-1]-u[1,i])/(timeScale[i]*Ts) <= 0.6)
end
end
xWS = zeros(5,N+1)
for b = 1:5
xWS[b,:] = linspace(x0[b],xF[b],N+1)
end
uWS = 0.2*ones(2,N)
setvalue(timeScale,0.8*ones(N+1,1))
setvalue(x,xWS)
setvalue(u,uWS)
tic()
status = solve(m)
time = toq();
# plot results
xp = getvalue(x)
up = getvalue(u)
timeScalep = getvalue(timeScale)
ego_trailer = [L_trailer , 1 , 0 , 1 ]
ego_truck = [L_truck-1, 1, 1, 1]
plotTraj(xp,up,N,ego_truck,ego_trailer,L_truck,L_trailer,0,[],[],true,0)
## INITIALIZATION OF VARIABLES DONE ##
XYbounds = [ -22 , 23 , -5 , 15 ]
close("all")
fixTime = 0 # default: 0 (variable time steps)
sampleN = 1 # With works
Ts = Ts*sampleN
# for plotting
nObPlot = 3 # number of obstacles
vObPlot = [4 4 4] # number of vertices of each obstacle, vector of dimenion nOb
# obstacle representation for optimization problem
nOb = 3 # number of obstacles
vOb = [3 3 2] # number of vertices of each obstacle, vector of dimenion nOb
vObMPC = vOb-1
lObPlot = [ [ [-22;4.5], [-1.60;4.5], [-1.60;-5], [-22;-5], [-22;4.5] ] ,
[ [1.60;4.5], [22;4.5], [22;-5], [1.60;-5], [1.60;4.5] ] ,
[ [-22;15], [22;15], [22;15], [-22,15], [-22;15] ] ] #vetices given in CLOCK-WISE direction
# for optimization problem
lOb = [ [ [-22;4.5], [-1.60;4.5], [-1.60;-5]] ,
[ [1.60;-5] , [1.60;4.5] , [22;4.5] ] ,
[ [22;15], [-22;15]] ]
### solve OBCA step ###
N = length(xWS[1,:])-1
AOb, bOb = obstHrep(nOb, vOb, lOb) # obtain H-representation of obstacles
xWS = xp
uWS = up
xp10, up10, scaleTime10, exitflag10, time10, lp10, np10, lp20, np20 = ParkingSignedDist(x0,xF,N,Ts,L_truck,L_trailer,ego_truck, ego_trailer,XYbounds,nOb,vObMPC,AOb,bOb,fixTime,xWS,uWS, timeScalep)
In ParkingSignedDist.jl
I initialize a new model with the parameters obtained before, the original ParkingSignedDist.jl and the extra code you send:
function ParkingSignedDist(x0,xF,N,Ts,L, L_trailer, ego_truck, ego_trailer,XYbounds,nOb,vOb, A, b,fixTime,xWS,uWS, timeScalep = [])
# desired safety distance
dmin = 0.05 # anything bigger than 0, e.g. 0.05
m = Model(solver=IpoptSolver(hessian_approximation="exact",mumps_pivtol=1e-6,alpha_for_y="min",recalc_y="yes",
mumps_mem_percent=6000,max_iter=200,tol=1e-5, print_level=5,
min_hessian_perturbation=1e-12,jacobian_regularization_value=1e-7))#,nlp_scaling_method="none"
##############################
# defining optimization variables
##############################
#state
@variable(m, x[1:5,1:(N+1)])
#scaling on sampling time
@variable(m, timeScale[1:N+1])
#control
@variable(m, u[1:2,1:N])
# lagrange multipliers for dual dist function
@variable(m, l1[1:sum(vOb),1:(N+1)]) # dual multiplier associated with obstacleShape
@variable(m, n1[1:nOb*4,1:(N+1)]) # dual multiplier associated with carShape
@variable(m, l2[1:sum(vOb),1:(N+1)]) # dual multiplier associated with obstacleShape
@variable(m, n2[1:nOb*4,1:(N+1)]) # dual multiplier associated with carShape
reg = 1e-7;
u0 = [0,0]
@NLobjective(m, Min,sum(0.1*u[1,i]^2 + 1*u[2,i]^2 for i = 1:N) +
sum(100*((u[1,i+1]-u[1,i])/(timeScale[i]*Ts))^2 + 100*((u[2,i+1]-u[2,i])/(timeScale[i]*Ts))^2 for i = 1:N-1) +
sum(0.5*timeScale[i] + 1*timeScale[i]^2 for i =1:N+1))
@constraint(m, [i=1:N], -0.6 <= u[1,i] <= 0.6)
@constraint(m, [i=1:N], -0.4 <= u[2,i] <= 0.4)
#state constraints
@constraint(m, [i=1:N+1], XYbounds[1] <= x[1,i] <= XYbounds[2])
@constraint(m, [i=1:N+1], XYbounds[3] <= x[2,i] <= XYbounds[4])
@constraint(m, [i=1:N+1], -15 <= x[3,i] <= 15)
@constraint(m, [i=1:N+1], -15 <= x[4,i] <= 15)
@constraint(m, [i=1:N+1], -1 <= x[5,i] <= 4) # constraint in velocity
# bounds on time scaling
@constraint(m, 0.2 .<= timeScale .<= 4)
# positivity constraints on dual multipliers
@constraint(m, l1 .>= 0 )
@constraint(m, n1 .>= 0)
@constraint(m, l2 .>= 0 )
@constraint(m, n2 .>= 0)
##############################
# start and finish point
##############################
#starting point
@constraint(m, x[1,1] == x0[1])
@constraint(m, x[2,1] == x0[2])
@constraint(m, x[3,1] == x0[3])
@constraint(m, x[4,1] == x0[4])
@constraint(m, x[5,1] == x0[5])
#end point
@constraint(m, x[1,N+1] == xF[1])
@constraint(m, x[2,N+1] == xF[2])
@constraint(m, x[3,N+1] == xF[3])
@constraint(m, x[4,N+1] == xF[4])
@constraint(m, x[5,N+1] == xF[5])
#############################
for i in 1:N
# sampling time is variable
@NLconstraint(m, x[2,i+1] == x[2,i] + timeScale[i]*Ts*x[5,i]*sin(x[3,i]))
@NLconstraint(m, x[3,i+1] == x[3,i] + timeScale[i]*Ts*x[5,i]*tan(u[1,i])/L)
@NLconstraint(m, x[4,i+1] == x[4,i] + timeScale[i]*Ts*x[5,i]/L_trailer*sin(x[3,i]-x[4,i])) #x4 is theta_trailer = theta_truck + alpha
@NLconstraint(m, x[5,i+1] == x[5,i] + timeScale[i]*Ts*u[2,i])
@constraint(m, timeScale[i] == timeScale[i+1])
end
@constraint(m, [i=1:N+1], (x[3,i]-x[4,i])^2 <= 1)
u0 = [0,0]
if fixTime == 1
for i in 1:N
if i==1
@constraint(m,-0.6<=(u0[1]-u[1,i])/Ts <= 0.6)
else
@constraint(m,-0.6<=(u[1,i-1]-u[1,i])/Ts <= 0.6)
end
end
else
for i in 1:N
if i==1
@NLconstraint(m,-0.6<=(u0[1]-u[1,i])/(timeScale[i]*Ts) <= 0.6)
else
@NLconstraint(m,-0.6<=(u[1,i-1]-u[1,i])/(timeScale[i]*Ts) <= 0.6)
end
end
end
ego1 = ego_truck
ego2 = ego_trailer
# width and length of ego set
W_ev1 = ego1[2]+ego1[4]
L_ev1 = ego1[1]+ego1[3]
g1 = [L_ev1/2,W_ev1/2,L_ev1/2,W_ev1/2]
# ofset from X-Y to the center of the ego set
offset1 = (ego1[1]+ego1[3])/2 - ego1[3]
for i in 1:N+1 # iterate over time steps
for j = 1 : nOb # iterate over obstacles
Aj = A[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract obstacle matrix associated with j-th obstacle
lj = l1[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract lambda dual variables associate j-th obstacle
nj = n1[(j-1)*4+1:j*4 ,:] # extract mu dual variables associated with j-th obstacle
bj = b[sum(vOb[1:j-1])+1 : sum(vOb[1:j])] # extract obstacle matrix associated with j-th obstacle
# norm(A'*lambda) <= 1
@NLconstraint(m, (sum(Aj[k,1]*lj[k,i] for k = 1 : vOb[j]))^2 + (sum(Aj[k,2]*lj[k,i] for k = 1 : vOb[j]))^2 <= 1 )
# G'*mu + R'*A*lambda = 0
@NLconstraint(m, (nj[1,i] - nj[3,i]) + cos(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + sin(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
@NLconstraint(m, (nj[2,i] - nj[4,i]) - sin(x[3,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + cos(x[3,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
# -g'*mu + (A*t - b)*lambda > 0
@NLconstraint(m, -sum(g1[k]*nj[k,i] for k = 1:4) + (x[1,i]+cos(x[3,i])*offset1)*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j])
+ (x[2,i]+sin(x[3,i])*offset1)*sum(Aj[k,2]*lj[k,i] for k=1:vOb[j]) - sum(bj[k]*lj[k,i] for k=1:vOb[j]) >= dmin )
end
end
W_ev2 = ego2[2]+ego2[4]
L_ev2 = ego2[1]+ego2[3]
g2 = [L_ev2/2,W_ev2/2,L_ev2/2,W_ev2/2]
# ofset from X-Y to the center of the ego set
offset2 = (ego2[1]+ego2[3])/2 - ego2[3]
for i in 1:N+1 # iterate over time steps
for j = 1 : nOb # iterate over obstacles
Aj = A[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract obstacle matrix associated with j-th obstacle
lj = l2[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract lambda dual variables associate j-th obstacle
nj = n2[(j-1)*4+1:j*4 ,:] # extract mu dual variables associated with j-th obstacle
bj = b[sum(vOb[1:j-1])+1 : sum(vOb[1:j])] # extract obstacle matrix associated with j-th obstacle
# norm(A'*lambda) <= 1
@NLconstraint(m, (sum(Aj[k,1]*lj[k,i] for k = 1 : vOb[j]))^2 + (sum(Aj[k,2]*lj[k,i] for k = 1 : vOb[j]))^2 <= 1 )
# G'*mu + R'*A*lambda = 0
@NLconstraint(m, (nj[1,i] - nj[3,i]) + cos(x[4,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + sin(x[4,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
@NLconstraint(m, (nj[2,i] - nj[4,i]) - sin(x[4,i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + cos(x[4,i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
# -g'*mu + (A*t - b)*lambda > 0
@NLconstraint(m, -sum(g2[k]*nj[k,i] for k = 1:4) + (x[1,i]-cos(x[4,i])*offset2)*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j])
+ (x[2,i]-sin(x[4,i])*offset2)*sum(Aj[k,2]*lj[k,i] for k=1:vOb[j]) - sum(bj[k]*lj[k,i] for k=1:vOb[j]) >= dmin )
end
end
##############################
# set initial guesses obtained before
##############################
setvalue(timeScale,timeScalep)
setvalue(x,xWS)
setvalue(u,0.5*uWS[:,1:N])
l1WS,n1WS = DualMultWS1(N,nOb,vOb, A, b,xWS[1,:],xWS[2,:],xWS[3,:],ego1)
setvalue(l1,l1WS')
setvalue(n1,n1WS')
l2WS,n2WS = DualMultWS2(N,nOb,vOb, A, b,xWS[1,:],xWS[2,:],xWS[4,:],ego2)
setvalue(l2,l2WS')
setvalue(n2,n2WS')
##############################
# solve problem
##############################
# at most three attempts considered
time1 = 0
time2 = 0
exitflag = 0
tic()
status = solve(m; suppress_warnings=true)
time1 = toq();
In DualMultWS1
and DualMultWS1
I pass as a parameter the size of ergo1 and ergo2 respectively, which you don't do, and basically they result in something very similar to the original DualMultWS.jl
function DualMultWS2(N,nOb,vOb, A, b,x,y,yaw,ego)
m = Model(solver=IpoptSolver(hessian_approximation="exact",mumps_pivtol=1e-5,
max_iter=100,tol=1e-5, print_level=3, suppress_all_output="yes"))
# width and length of ego set
W_ev = ego[2]+ego[4]
L_ev = ego[1]+ego[3]
g = [L_ev/2,W_ev/2,L_ev/2,W_ev/2]
# ofset from X-Y to the center of the ego set
offset = (ego[1]+ego[3])/2 - ego[3]
@variable(m, l[1:sum(vOb),1:(N+1)]) # dual multiplier associated with obstacleShape
@variable(m, n[1:nOb*4,1:(N+1)]) # dual multiplier associated with carShape
@variable(m, d[1:nOb,1:(N+1)])
@NLobjective(m, Max,sum(sum(d[i,k] for k=1:N+1) for i=1:nOb ))
@constraint(m, l .>= 0 )
@constraint(m, n .>= 0)
for i in 1:N+1 # iterate over time steps
for j = 1 : nOb # iterate over obstacles
Aj = A[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract obstacle matrix associated with j-th obstacle
lj = l[sum(vOb[1:j-1])+1 : sum(vOb[1:j]) ,:] # extract lambda dual variables associate j-th obstacle
nj = n[(j-1)*4+1:j*4 ,:] # extract mu dual variables associated with j-th obstacle
bj = b[sum(vOb[1:j-1])+1 : sum(vOb[1:j])] # extract obstacle matrix associated with j-th obstacle
### TRUCK ###
# norm(A'*lambda) <= 1
@constraint(m, (sum(Aj[k,1]*lj[k,i] for k = 1 : vOb[j]))^2 + (sum(Aj[k,2]*lj[k,i] for k = 1 : vOb[j]))^2 <= 1 )
# G'*mu + R'*A*lambda = 0
@constraint(m, (nj[1,i] - nj[3,i]) + cos(yaw[i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + sin(yaw[i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
@constraint(m, (nj[2,i] - nj[4,i]) - sin(yaw[i])*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j]) + cos(yaw[i])*sum(Aj[k,2]lj[k,i] for k = 1:vOb[j]) == 0 )
# -g'*mu + (A*t - b)*lambda > 0
@constraint(m, d[j,i] == -sum(g[k]*nj[k,i] for k = 1:4) + (x[i]-cos(yaw[i])*offset)*sum(Aj[k,1]*lj[k,i] for k = 1:vOb[j])
+ (y[i]-sin(yaw[i])*offset)*sum(Aj[k,2]*lj[k,i] for k=1:vOb[j]) - sum(bj[k]*lj[k,i] for k=1:vOb[j]))
end
end
tic()
solve(m; suppress_warnings=true)
time = toq();
# print("Auxillery problem time (to warm start dual variables) = ",time," s \n")
lp = getvalue(l)'
np = getvalue(n)'
return lp,np
end
The problem stated as I do results in strange results that do not make sense, and I do not understand why for the constraints of the second vehicle where for the first one there is a x[1,i]+cos(x[3,i])
you substitu it for x[1,i]-cos(x[4,i])
,x[2,i]+sin(x[3,i])
you substitu it for x[2,i]-sin(x[4,i])
without including the length of the second vehicle, I think it should be changed from x[1,i]+cos(x[3,i])
to x[1,i] - wheelbase_2_ergo*cos(x[3,i])+cos(x[3,i])
, does it make sense?
Thanks for everything @alexliniger , I hope not to bother you anymore soon
I have accomplished to make it work, thanks for everything, I just have a small theoretical question that I will formulate in another post
@hect1995 It's an interesting application of OBCA. Could you plz share your modifications to be studied ?
Sorry to bother again but could you tell the required changes to simulate the Truck with Trailer example that you plot in the README? As it does not appear in the code currently provided and I am not sure how to include the trailer into the ParkingDist.jl and as my current project deals with Optimization for trucks with a single trailer I would e super interested