Issue with Julia MPC Out of Bounds Error

question

#1

#!/usr/bin/env julia

#=
** Licensing Information: You are free to use or extend these projects for **
** education or research purposes provided that (1) you retain this notice**
** and (2) you provide clear attribution to UC Berkeley, including a link **
** to http://barc-project.com**

** Attibution Information: The barc project ROS code-base was developed**
** at UC Berkeley in the Model Predictive Control (MPC) lab by Jon Gonzales**
** (jon.gonzales@berkeley.edu). The cloud services integation with ROS was developed**
** by Kiet Lam (kiet.lam@berkeley.edu). The web-server app Dator was **
** based on an open source project by Bruce Wootton**

** The original code is being modified by Angshuman Goswami from Clemson University**
** working in Efficient Mobility via Connectivity and Control (EMC2) lab**
**=# **

using RobotOS
@rosimport barc.msg: ECU_raw, Encoder, Ultrasound_xy, Z_KinBkMdl
@rosimport data_service.msg: TimeData
@rosimport geometry_msgs.msg: Vector3
rostypegen()
using barc.msg
using data_service.msg
using geometry_msgs.msg
using JuMP
using Ipopt
using AmplNLWriter
#using NLopt
using geometry_msgs.msg

# define model parameters
L_a = 0.125 # distance from CoG to front axel
L_b = 0.125 # distance from CoG to rear axel
dt = 0.1 # time step of system
Ff = 0.1711 # friction coefficient
a0 = 0.1308 # air drag coeff
a = 0.125 # distance from CoG to rear axel
b = 0.125 # distance from CoG to front axel
I_z = 0.24 # moment of Inertia
m = 1.98 # mass of the vehicle

# Tire Properties
TM_F = [25, 1.5, -0.981]
TM_R = [25, 1.5, -1.4715]
(B1,C1,D1) = TM_F
(B2,C2,D2) = TM_R

# preview horizon
N = 5

# define targets [generic values]
x_ref = 5
y_ref = 5

**# define decision variables **
**# states: position (x,y), velocity (v_x, v_y), yaw angle (phi) , yaw rate® **
**# inputs: acceleration, steering angle **
println(“Creating kinematic bicycle model …”)
mdl = Model(solver = IpoptSolver(print_level=3))
#mdl = Model(solver = BonminNLSolver())
#mdl = Model(solver = NLoptSolver(algorithm=:LD_MMA))
@defVar( mdl, x[1:(N+1)] )
@defVar( mdl, y[1:(N+1)] )
@defVar( mdl, phi[1:(N+1)] )
@defVar( mdl, v_x[1:(N+1)] )
@defVar( mdl, v_y[1:(N+1)] )
@defVar( mdl, r[1:N] )
@defVar( mdl, FxR[1:N] )

# define objective function
@setNLObjective(mdl, Min, (x[N+1] - x_ref)^2 + (y[N+1] - y_ref)^2 )

# define system dynamics

# define constraints

#initialise the system

@defNLParam(mdl, x0 == 0); @addNLConstraint(mdl, x[1] == x0);
@defNLParam(mdl, y0 == 0); @addNLConstraint(mdl, y[1] == y0);
@defNLParam(mdl, phi0 == 0); @addNLConstraint(mdl, phi[1] == phi0 );
@defNLParam(mdl, v_x0 == 0); @addNLConstraint(mdl, v_x[1] == v_x0);
@defNLParam(mdl, v_y0 == 0); @addNLConstraint(mdl, v_y[1] == v_y0 );


# Define constraints

#@defNLExpr(mdl, front[i=1:N] , (frontx[i]-x[i])^2 + (fronty[i]-y[i])^2)

for i in 1:N
** @addNLConstraint(mdl, x[i+1] == x[i] + dt*(v_x[i]cos(phi[i]) - v_y[i]sin(phi[i])) )
** @addNLConstraint(mdl, y[i+1] == y[i] + dt*(v_x[i]sin(phi[i]) + v_y[i]cos(phi[i])) )
** @addNLConstraint(mdl, phi[i+1] == phi[i] + dtr[i] )**
** @addNLConstraint(mdl, v_x[i+1] == v_x[i] + dt
(r[i] v_y[i] + FxR[i]/m) )*
** @addNLConstraint(mdl, v_y[i+1] == v_y[i] + dt*(-r[i] v_x[i] ) )*
** #@addNLConstraint(mdl, frontx[i+1] == frontx[i])**

** @addNLConstraint(mdl, -1 <= a[i] <=1)**
** @addNLConstraint(mdl, -1 <= r[i] <=1)**
** #@addNLConstraint(mdl, front[N] >=0.03)**




end

# status update
println(“initial solve …”)
solve(mdl)
println(“finished initial solve!”)

function postion_callback(msg::Vector3)
** # update mpc initial condition **
** setValue(x0, msg.x)**
** setValue(y0, msg.y)**
** setValue(phi0, msg.z) **
end

function velocity_callback(msg::Vector3)
** # update mpc initial condition **
** setValue(v_x0, msg.x)**
** setValue(v_y0, msg.y)**

end

#=
function ultrasound_xy_callback(msg::Ultrasound_xy)
** # update obstacles initial condition **
** setValue(frontx0 , msg.frontx)**
** setValue(fronty0 , msg.fronty)**
** #setValue(frontleftx0 , msg.frontleftx)**
** #setValue(frontlefty0 , msg.frontlefty)**
** #setValue(frontrightx0 , msg.frontrightx)**
** #setValue(frontrighty0 , msg.frontrighty)**



end
=#

function main()
** # initiate node, set up publisher / subscriber topics**
** init_node(“mpc”)**
** pub = Publisher(“ecu”, ECU_raw, queue_size=10)**
** s1 = Subscriber(“position_info”, Vector3, postion_callback, queue_size=10)**
** s2 = Subscriber(“state_estimate”, Vector3, velocity_callback, queue_size=10)**
** #s2 = Subscriber(“ultrasound_xy”, Ultrasound_xy, ultrasound_xy_callback, queue_size=10)**
** loop_rate = Rate(10)**
** counter = 0**
** while ! is_shutdown()**
** # run mpc, publish command**
** tic()**
** solve(mdl)**
** toc()**


** # get optimal solutions**
** FxR_opt = getValue(FxR[1])**
** #=**
** if -0.01<a_opt<0.01**
** a_opt = 0**
** end**
** =#**
** r_opt = getValue(r[1])**
** #if -0.001<r_opt<0.001**
** # d_f_opt = 0**
** #end**
** phi_opt = getValue(phi[1])**



** counter += 1**
** if counter > 75**
** # publish commands**


** cmd = ECU_raw(a_opt, r_opt,phi_opt)**
** publish(pub, cmd)**
** end**
** rossleep(loop_rate)**
** end**
end

if ! isinteractive()
** main()**
end

When I run this code it is giving me out of bounds error


#2

Welcome to the Julia Discourse board! Instead of wrapping each line with **, please post short snippets of code that is simply “fenced” at the top and bottom by three backticks ```.

When seeking help, it behooves you to make it as easy as possible for someone to provide that help. This includes minimizing your example to the smallest piece of code that exhibits the issue you’re running into and including the exact error message — which typically will include a line number that should help you narrow down your example.