I am new to ACADOS and MPC. I have a task where I need to define the dynamics of a mobile robot and solve the MPC problem using ACADOS. I have defined the model using CASADI as follows: def mobile_robot_model(): """ Define a simple mobile robot model. Returns: x_vec (MX): Symbolic vector representing the state [x, y, v, theta]. u (MX): Symbolic vector representing the control input [a, w]. continuous_dynamics (Function): CasADi function for the continuous-time dynamics. Takes state vector and control input vectors as input and returns the vector representing the rates of change of the state variables. """ 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 = AcadosModel() 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 Next I need to setup the Acados solver, including the cost function. ● The cost function of minimizing the error between real and initial trajectory has the following general formula (I can't post the image due to low rep): formula1 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, 1 , 2 , 3 are weights. The desired control inputs are = [ , ] = 0 ● For avoiding obstacles, use the following cost function: formula2 my question is how I can implement this kind of cost function in ACADOS. I was thinking I need to use EXTERNAL cost type but later I was told I should use NONLINEAR_LS. This is my code till now: def create_ocp_solver(): # Create AcadosOcp object ocp = AcadosOcp() # Set up the optimization problem model = mobile_robot_model() ocp.model = model # --------------------PARAMETERS-------------- # constants nx = model.x.size()[0] nu = model.u.size()[0] ny = nx + nu ny_e = nx 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.dims.ny = ny # ocp.dims.ny_e = ny_e 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 Continue reading...
