#!/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(r) **
**# 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