Hello,
I am trying to implement MPC with obstacle avoidance for a simple mobile robot (kinematic model). I have implemented the code as below.
class mpc(object):
def __init__(self):
self.RUN_ONCE = True
self.tcomp_sum = 0
self.tcomp_max = 0
self.acados_solver = 0
self.N = 0
def kine_model(self):
model = types.SimpleNamespace()
model_name = 'kine_ode'
x = SX.sym('x')
y = SX.sym('y')
phi = SX.sym('phi')
states = vertcat(x, y, phi)
obs_x = SX.sym('obs_x')
obs_y = SX.sym('obs_y')
dist = sqrt((x- obs_x)*(x- obs_x)+(y - obs_y)*(y - obs_y))
# controls
v = SX.sym('v')
phidot = SX.sym('phidot')
u = vertcat(v,phidot)
# statesdot
xdot = SX.sym('xdot')
ydot = SX.sym('ydot')
phid = SX.sym('phid')
statesdot = vertcat(xdot, ydot, phid)
# algebraic variables
# z = None
# parameters
p = vertcat(obs_x, obs_y)
# dynamics
f_expl = vertcat(v*cos(phi),
v*sin(phi),
phidot
)
f_impl = statesdot - f_expl
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = states
model.xdot = statesdot
model.u = u
# model.z = z
model.p = p
model.name = model_name
model.con_h_expr = dist
model.con_h_expr_e = dist
return model
##############################################
def acado_set(self, x0):
# create ocp object to formulate the OCP
ocp = AcadosOcp()
Tf = 1
self.N = 20
# set model
model = self.kine_model()
nx = model.x.size()[0]
nu = model.u.size()[0]
ny = nx + nu
ny_e = nx
npa = model.p.size()[0]
ocp.model = model
# set dimensions
ocp.dims.N = self.N
ocp.dims.np = npa
# set cost module
ocp.cost.cost_type = 'LINEAR_LS'
ocp.cost.cost_type_e = 'LINEAR_LS'
ocp.parameter_values = np.array([0.6, 0.6]) # initial obstacle location
ocp.constraints.uh = np.array([10000])
ocp.constraints.uh_e = np.array([10000])
ocp.constraints.lh = np.array([0.15])
ocp.constraints.lh_e = np.array([0.15])
Q = np.diag([1, 1, 1])
R = np.diag([0.001, 0.01])
Qe = np.diag([0.1, 0.1, 0.1])
ocp.cost.W = scipy.linalg.block_diag(Q, R)
ocp.cost.W_e = Qe
ocp.cost.Vx = np.zeros((ny, nx))
ocp.cost.Vx[:nx,:nx] = np.eye(nx)
Vu = np.zeros((ny, nu))
Vu[3,0] = 1.0
Vu[4,1] = 1.0
ocp.cost.Vu = Vu
Vx_e = np.zeros((ny_e, nx))
Vx_e[:nx, :nx] = np.eye(nx)
ocp.cost.Vx_e = Vx_e
ocp.cost.yref = np.zeros((ny, ))
ocp.cost.yref_e = np.zeros((ny_e, ))
# set constraints
vmax = 0.5
phidotmax = 0.785
ocp.constraints.lbu = np.array([-vmax, -phidotmax])
ocp.constraints.ubu = np.array([vmax, phidotmax])
ocp.constraints.x0 = x0
ocp.constraints.idxbu = np.array([0, 1])
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
#ocp.solver_options.qp_solver = 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP'
ocp.solver_options.sim_method_num_stages = 4
ocp.solver_options.sim_method_num_steps = 3
ocp.solver_options.qp_solver_cond_N = 5
ocp.solver_options.qp_solver_iter_max = 100
ocp.solver_options.nlp_solver_max_iter = 500
# set prediction horizon
ocp.solver_options.tf = Tf
acados_ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp_' + model.name + '.json')
return acados_ocp_solver
############################################
def run_mpc(self, ydes, x0, obs_x, obs_y):
## Assuming ydes is single point
if self.RUN_ONCE:
self.acados_solver = self.acado_set(x0)
self.RUN_ONCE = False
for j in range(self.N):
# yref = ydes[j]
yref = ydes
self.acados_solver.set(j, "yref", yref)
self.acados_solver.set(j,"p", np.array([obs_x, obs_y]))
yref_N = ydes[0:3]
# yref_N=np.array([0,0,0,0,0,0])
self.acados_solver.set(self.N, "yref", yref_N)
self.acados_solver.set(0, "lbx", x0)
self.acados_solver.set(0, "ubx", x0)
# solve ocp
status = self.acados_solver.solve()
# get solution
x0 = self.acados_solver.get(0, "x")
u0 = self.acados_solver.get(0, "u")
#self.acados_solver.print_statistics()
return u0
The robot was able to reach the target each time when no obstacles were present. After adding the obstacles (taking into consideration only the nearest object) the solver either ignores the obstacle and crashes into it or if it passes really close the status of the solver is always 2 or 4. I tried changing the number of iterations but nothing changed.
- What am I doing wrong?
- How is the idxbu matrix set? (I used it as the race cars example and tried changing the values)
- Is it possible to reset the solver after it keeps returning status 4?
All suggestions are welcome.
