# Mobile Robot - Multiple cost functions

Hi

I am new to ACADOS and MPC in general.

I am trying to implement Acados numerical solver for finding the solution of the MPC problem for trajectory optimization of a Mobile robot

I am using Python interface and I want to implement two different cost functions:

1. The cost function of minimizing the error between real and initial trajectory has the following general formula

where

• q and u are the output and input vectors of the trajectory of the robot
• p and r are the output and input vector of the trajectory,
• w_1 , w_2 , w_3 are weights.
• Control inputs are r = [a, w]^T=0
1. Cost function for avoiding obstacles:

I was told to use NONLINEAR_LS for defining the cost functions. but I am not sure how to combine two different costs.

My code:

Model

``````
def mobile_robot_model():

model_name = 'mobile_robot'

# Define symbolic variables (states)
x = ca.MX.sym('x')
y = ca.MX.sym('y')
v = ca.MX.sym('v')
theta = ca.MX.sym('theta')

# Control
a = ca.MX.sym('a')  # acceleration
w = ca.MX.sym('w')  # angular velocity

# Define state and control vectors
states = ca.vertcat(x, y, v, theta)
controls = ca.vertcat(a, w)
rhs = [v*ca.cos(theta), v*ca.sin(theta), a, w]
x_dot = ca.MX.sym('x_dot', len(rhs))

# Create a CasADi function for the continuous-time dynamics
continuous_dynamics = ca.Function(
'continuous_dynamics',
[states, controls],
[ca.vcat(rhs)],
["state", "control_input"],
["rhs"]
)

f_impl = x_dot - continuous_dynamics(states, controls)

model.f_expl_expr = continuous_dynamics(states, controls)
model.f_impl_expr = f_impl
model.x = states
model.xdot = x_dot
model.u = controls
model.p = []
model.name = model_name

return model
``````

Solver:

``````def create_ocp_solver():

# Set up the optimization problem
model = mobile_robot_model()
ocp.model = model

# --------------------PARAMETERS--------------
# constants
nx = model.x.size()[0]
nu = model.u.size()[0]
T = 30
N = 100
n_params = len(model.p)

# Setting initial conditions
ocp.dims.N = N
ocp.dims.nx = nx
ocp.dims.nu = nu
ocp.solver_options.tf = T

# initial state
x_ref = np.zeros(nx)

# Set initial condition for the robot
ocp.constraints.x0 = x_ref

# initialize parameters
ocp.dims.np = n_params
ocp.parameter_values = np.zeros(n_params)

# ---------------------CONSTRAINTS------------------
# Define constraints on states and control inputs
ocp.constraints.lbu = np.array([-0.1, -0.3])  # Lower bounds on control inputs
ocp.constraints.ubu = np.array([0.1, 0.3])    # Upper bounds on control inputs
ocp.constraints.lu = np.array([100, 100, 1, 10])  # Upper bounds on states
ocp.constraints.idxbu = np.array([0, 1])  # for indices 0 & 1

# ---------------------COSTS--------------------------

# Define J1 and J2
# Define cost_y_expr, cost_y_expr_e
# Define W and W_e
# Define yref and yref_e

ocp.cost.cost_type = 'NONLINEAR_LS'
ocp.cost.cost_type_e = 'NONLINEAR_LS'

X = ocp.model.x
U = ocp.model.u

# Cost weights
w1 = 1.0  # adjust as needed
w2 = 1.0  # adjust as needed
w3 = 1.0  # adjust as needed
w = 1.0

J2 = obstacle_cost(X, w)

# Parameters:
ocp.model.cost_y_expr = J2
ocp.model.cost_y_expr_e = J2

ocp.cost.W = np.eye(1)
ocp.cost.W_e = np.eye(1)

ocp.cost.yref = np.array([0])
ocp.cost.yref_e = np.array([0])

# ---------------------SOLVER-------------------------
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.nlp_solver_type = 'SQP_RTI'

you just need to to define every nonlinear least-squares cost term as an entry of `cost_y_expr`.