Example with Single trailer Truck

[hect1995]

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

[alexliniger]

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

[hect1995]

I would be much appreciated. In the meantime I have start developing your suggestion and the changes I have done are:

• New state vector is : Where: Being alpha the hitch angle of the truck-trailer system.

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:

• Add a new ergo and set all constraints for the new ergo. I use the coordinates of the truck like in the original problem and transform coordinates from truck to trailer using the formulas before to make collision avoidance for the new ergo vehicle (the trailer)

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

[alexliniger]

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

[hect1995]

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:

• Kinematics adding new state

    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.

[alexliniger]

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.

[hect1995]

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

[hect1995]

I have accomplished to make it work, thanks for everything, I just have a small theoretical question that I will formulate in another post

[huangatlas]

@hect1995 It's an interesting application of OBCA. Could you plz share your modifications to be studied ?