Kinematic Bicycle Model

using NLOptControl, Parameters, VehicleModels, PrettyPlots

pa = Vpara(x0_=0.)
@unpack_Vpara pa # vehicle parameters
X0 = [x0_,y0_,psi0_,u0_]
XF = [NaN,NaN,NaN,NaN]
XL = [x_min,y_min,psi_min,u_min]
XU = [x_max,y_max,psi_max,u_max]
CL = [sa_min,ax_min]
CU = [sa_max,ax_max]

n = define(numStates=4,numControls=2,X0=X0,XF=XF,XL=XL,XU=XU,CL=CL,CU=CU)
n.ocp.params = [pa] # add vehicle parameters

c = Dict("goal"=>Dict("x"=>0.,"yVal"=>100.,"tol"=>5.), "obstacle"=>Dict("x0"=>[0.],"y0"=>[50.],"vx"=>[0.],"vy"=>[0.],"radius"=>[5.]),"tolerances"=>Dict("fx"=>NaN,"fy"=>NaN))

names = [:x,:y,:psi,:ux]
descriptions = ["X (m)","Y (m)","Yaw Angle (rad)","Longitudinal Velocity (m/s)"]
states!(n,names,descriptions=descriptions)
names = [:sa,:ax]
descriptions = ["Steering Angle (rad)","Longitudinal Acceleration (m/s^2)"]
controls!(n,names,descriptions=descriptions)

dx = KinematicBicycle_expr2(n)
dynamics!(n,dx)

configure!(n;(:Nck=>[12,10,8]),(:finalTimeDV=>true));

sm = 2
x = n.r.ocp.x[:,1];y = n.r.ocp.x[:,2]; # pointers to JuMP variables
obs_con = @NLconstraint(n.ocp.mdl, [i=1:n.ocp.state.pts-1], 1 <= ((x[(i+1)]-c["obstacle"]["x0"][1])^2)/((c["obstacle"]["radius"][1]+sm)^2) + ((y[(i+1)]-c["obstacle"]["y0"][1])^2)/((c["obstacle"]["radius"][1]+sm)^2))
newConstraint!(n,obs_con,:obs_con)

mXL = Any[false,false,false,false]
mXU = Any[false,false,false,-1];  # set to false if you don't want to taper that side
linearStateTolerances!(n;mXL=mXL,mXU=mXU)

@NLobjective(n.ocp.mdl, Min, n.ocp.tf + (n.r.ocp.x[end,1]-c["goal"]["x"])^2 + (n.r.ocp.x[end,2]-c["goal"]["yVal"])^2);

optimize!(n);

plotSettings(;(:size=>(700,700)));
allPlots(n)

plotSettings(;(:size=>(400,400)))
obs = obstaclePlot(n,c)
sp=statePlot(n,1,1,2,obs;(:append=>true),(:lims=>false))
xlims!(-45,55)
ylims!(0,110)