Skip to content

Commit

Permalink
Adding slack variables to soften input rate constraints.
Browse files Browse the repository at this point in the history
  • Loading branch information
@govvijaycal
govvijaycal committed Dec 26, 2018
1 parent 830a2ea commit 837afdc
Show file tree
Hide file tree
Showing 2 changed files with 40 additions and 16 deletions.
28 changes: 20 additions & 8 deletions scripts/mpc_utils/GPSKinMPCPathFollower.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -75,18 +75,29 @@ module GPSKinMPCPathFollower
@variable( mdl, a_min <= acc[1:N] <= a_max, start=0.0)
@variable( mdl, -steer_max <= d_f[1:N] <= steer_max, start=0.0)

# Input Steering Rate Constraints
# Input Steering Rate Constraints (with slack)
@variable( mdl, sl_df[1:N] >= 0.0, start=0.0) # slack on steering
@NLparameter(mdl, d_f_current == 0.0) # Current tire angle is a model parameter.
@NLconstraint(mdl, -steer_dmax*dt_control <= d_f[1] - d_f_current <= steer_dmax*dt_control)
#@NLconstraint(mdl, -steer_dmax*dt_control - sl_df[1] <= d_f[1] - d_f_current <= steer_dmax*dt_control + sl_df[1])
@NLconstraint(mdl, d_f[1] - d_f_current + sl_df[1] >= -steer_dmax*dt_control)
@NLconstraint(mdl, d_f[1] - d_f_current - sl_df[1] <= steer_dmax*dt_control)
for i in 1:(N-1)
@constraint(mdl, -steer_dmax*dt <= d_f[i+1] - d_f[i] <= steer_dmax*dt)
#@constraint(mdl, -steer_dmax*dt - sl_df[i+1] <= d_f[i+1] - d_f[i] <= steer_dmax*dt + sl_df[i+1])
@constraint(mdl, d_f[i+1] - d_f[i] + sl_df[i+1] >= -steer_dmax*dt)
@constraint(mdl, d_f[i+1] - d_f[i] - sl_df[i+1] <= steer_dmax*dt)
end

# Input Acceleration Rate Constraints
# Input Acceleration Rate Constraints (with slack)
@variable( mdl, sl_acc[1:N] >= 0.0, start=0.0) # slack on acceleration
@NLparameter(mdl, acc_current == 0.0) # Current acceleration is a model parameter.
@NLconstraint(mdl, -a_dmax*dt_control <= acc[1] - acc_current <= a_dmax*dt_control)
for i in 1:(N-1)
@constraint(mdl, -a_dmax*dt <= acc[i+1] - acc[i] <= a_dmax*dt)
#@NLconstraint(mdl, -a_dmax*dt_control - sl_acc[1] <= acc[1] - acc_current <= a_dmax*dt_control + sl_acc[1])
@NLconstraint(mdl, acc[1] - acc_current + sl_acc[1] >= -a_dmax*dt_control)
@NLconstraint(mdl, acc[1] - acc_current - sl_acc[1] <= a_dmax*dt_control)

for i in 1:(N-1)
#@constraint(mdl, -a_dmax*dt - sl_acc[i+1] <= acc[i+1] - acc[i] <= a_dmax*dt + sl_acc[i+1])
@constraint(mdl, acc[i+1] - acc[i] + sl_acc[i+1] >= -a_dmax*dt)
@constraint(mdl, acc[i+1] - acc[i] - sl_acc[i+1] <= a_dmax*dt)
end

#### (3) Define Objective ####
Expand All @@ -103,7 +114,8 @@ module GPSKinMPCPathFollower
C_acc*sum{(acc[i])^2, i=1:N} +
C_df*sum{(d_f[i])^2, i=1:N} +
C_dacc*sum{(acc[i+1] - acc[i])^2, i=1:(N-1)} +
C_ddf*sum{(d_f[i+1] - d_f[i])^2, i=1:(N-1)}
C_ddf*sum{(d_f[i+1] - d_f[i])^2, i=1:(N-1)} +
sum{ sl_df[i] + sl_acc[i], i=1:N}
)

#### (4) Define System Dynamics Constraints ####
Expand Down
28 changes: 20 additions & 8 deletions scripts/mpc_utils/GPSKinMPCPathFollowerDelay.jl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,18 +79,29 @@ module GPSKinMPCPathFollowerDelay
@variable( mdl, a_min <= acc[1:N] <= a_max, start=0.0)
@variable( mdl, -steer_max <= d_f[1:N] <= steer_max, start=0.0)

# Input Steering Rate Constraints
# Input Steering Rate Constraints (with slack)
@variable( mdl, sl_df[1:N] >= 0.0, start=0.0) # slack on steering
@NLparameter(mdl, d_f_current == 0.0) # Current tire angle is a model parameter.
@NLconstraint(mdl, -steer_dmax*dt_control <= d_f[1] - d_f_current <= steer_dmax*dt_control)
#@NLconstraint(mdl, -steer_dmax*dt_control - sl_df[1] <= d_f[1] - d_f_current <= steer_dmax*dt_control + sl_df[1])
@NLconstraint(mdl, d_f[1] - d_f_current + sl_df[1] >= -steer_dmax*dt_control)
@NLconstraint(mdl, d_f[1] - d_f_current - sl_df[1] <= steer_dmax*dt_control)
for i in 1:(N-1)
@constraint(mdl, -steer_dmax*dt <= d_f[i+1] - d_f[i] <= steer_dmax*dt)
#@constraint(mdl, -steer_dmax*dt - sl_df[i+1] <= d_f[i+1] - d_f[i] <= steer_dmax*dt + sl_df[i+1])
@constraint(mdl, d_f[i+1] - d_f[i] + sl_df[i+1] >= -steer_dmax*dt)
@constraint(mdl, d_f[i+1] - d_f[i] - sl_df[i+1] <= steer_dmax*dt)
end

# Input Acceleration Rate Constraints
# Input Acceleration Rate Constraints (with slack)
@variable( mdl, sl_acc[1:N] >= 0.0, start=0.0) # slack on acceleration
@NLparameter(mdl, acc_current == 0.0) # Current acceleration is a model parameter.
@NLconstraint(mdl, -a_dmax*dt_control <= acc[1] - acc_current <= a_dmax*dt_control)
for i in 1:(N-1)
@constraint(mdl, -a_dmax*dt <= acc[i+1] - acc[i] <= a_dmax*dt)
#@NLconstraint(mdl, -a_dmax*dt_control - sl_acc[1] <= acc[1] - acc_current <= a_dmax*dt_control + sl_acc[1])
@NLconstraint(mdl, acc[1] - acc_current + sl_acc[1] >= -a_dmax*dt_control)
@NLconstraint(mdl, acc[1] - acc_current - sl_acc[1] <= a_dmax*dt_control)

for i in 1:(N-1)
#@constraint(mdl, -a_dmax*dt - sl_acc[i+1] <= acc[i+1] - acc[i] <= a_dmax*dt + sl_acc[i+1])
@constraint(mdl, acc[i+1] - acc[i] + sl_acc[i+1] >= -a_dmax*dt)
@constraint(mdl, acc[i+1] - acc[i] - sl_acc[i+1] <= a_dmax*dt)
end

#### (3) Define Objective ####
Expand All @@ -107,7 +118,8 @@ module GPSKinMPCPathFollowerDelay
C_acc*sum{(acc[i])^2, i=1:N} +
C_df*sum{(d_f[i])^2, i=1:N} +
C_dacc*sum{(acc[i+1] - acc[i])^2, i=1:(N-1)} +
C_ddf*sum{(d_f[i+1] - d_f[i])^2, i=1:(N-1)}
C_ddf*sum{(d_f[i+1] - d_f[i])^2, i=1:(N-1)} +
sum{ sl_df[i] + sl_acc[i], i=1:N}
)

#### (4) Define System Dynamics Constraints ####
Expand Down

0 comments on commit 837afdc

Please sign in to comment.