Hello!
Being this my first acados post, pardon me for any mistakes. I am using python interface to create an mpc problem. I have a question mainly regarding the acados formulation. I want to achieve an optimal pushing force for an object on the ground to move to a certain distance defined by a reference positions given state and control constraints. I have defined cost functions separately for yref and yref_e as ocp.cost.yref = np.zeros((ny,)) and ocp.cost.yref_e = np.zeros((ny_e,)) respectively.
I am facing problem when I am setting the last node as yref_e as below:
for i in range(N):
if i<N-1:
# yref = p_ref[i]
# Update the OCP cost function's reference for the current time step
ocp_solver.set(i, "yref", p_ref[i,:])
# print(i)
else:
yref_e = p_ref[i,:][:4]
ocp_solver.set(i, "yref", yref_e)
print('finally made it!')
But I getting the error as: Exception: AcadosOcpSolver.set(): mismatching dimension for field "yref" with dimension 6 (you have 4). I saw several examples but still not able to figure out the problem. I see that other examples also have the dimension mismatch. I have attached the code below. Any help will be appreciated. Thanks!
code:
from acados_template import AcadosModel, AcadosOcp, AcadosOcpSolver
from casadi import SX, vertcat, sin, cos, Function
import casadi as ca
import numpy as np
import pandas as pd
import scipy.linalg
def peg_model() -> AcadosModel:
model_name = 'sliding_peg'
#Parameters
m_peg = SX.sym('m_peg')
p = vertcat(m_peg) #Parameter Vector
# constants
m_peg = 5 # mass of the cart [kg]
g = np.array([0,0,-9.81]) # gravity constant [m/s^2]
# Define state & control variables
x = SX.sym('x', 2,1) # Position of COM [x, y, z] (3,1)
v = SX.sym('v', 2,1) # Velocity of COM [vx, vy, vz] (3,1)
X = vertcat(x, v)
#Force
F = SX.sym('F',2,1)
u = vertcat(F)
# xdot
x_dot = SX.sym('xdot',2,1)
# x_dot = v
v_dot = SX.sym('vdot',2,1)
# v_dot = F/m_peg # Acceleration is just gravity
# print(xdot)
xdot = vertcat(x_dot, v_dot)
# Define the explicit ODE
f_expl = vertcat(v, F/m_peg) #qdot & wdot is excluded
f_impl = xdot - f_expl
#set paramters
#reference trajectories
# p_ref = SX.sym('p_ref', 4, 1) # Position reference (2D in this case)
# Create the Acados model
model = AcadosModel()
model.f_impl_expr = f_impl
model.f_expl_expr = f_expl
model.x = X
model.xdot = xdot
model.u = u
model.p = p
# print(model.x)
model.name = model_name
return model
def main():
# max_values, min_values, ref_p, ref_v, initial_state = ref_traj()
# print(min_values)
# create ocp object to formulate the OCP
ocp = AcadosOcp() #The ocp object contains the full problem setup: model equations, cost functions, constraints, time horizon, etc.
model = peg_model()
ocp.model = model
nx = model.x.rows() #4 states
print(f'np. of states: {nx}')
nu = model.u.rows() #2 controls
print(f'np. of controls: {nu}')
ny = nx + nu #6
ny_e = nx
m_peg = 5
ocp.parameter_values = np.array([m_peg]) #expects numpy array
Tf = 1 #horizon for the entire optimisation: here the plot is only for optimal sol for one horizon
N = 10
ocp.cost.cost_type = "LINEAR_LS"
ocp.cost.cost_type_e = "LINEAR_LS"
# N = 10#len(p_ref) #dt
# Fmax = np.array([0,50]) #N
#set cost
Q_pos = 10*np.diag(np.ones((2))) #weights on the psoition error
Q_vel = 0.1*np.diag(np.ones((2))) #weights on the velocity error
Q = scipy.linalg.block_diag(Q_pos, Q_vel)
R = np.zeros((2,2)) #weights on the control input
ocp.cost.W_e = Q
ocp.cost.W = scipy.linalg.block_diag(Q, R)
# Set the weighting matrix for state tracking
Vx = np.zeros((ny, nx)) #tracking 2D position-->(6,4)
Vx[0, 0] = 1 # Track the x-position
Vx[1, 1] = 1 # Track the y-position
Vx[2, 2] = 1 # Track the x-velocity
Vx[3, 3] = 1 # Track the y-velocity
ocp.cost.Vx = Vx
ocp.cost.Vx_e = np.eye(ny_e)
Vu = np.zeros((ny,nu)) # (6,2)
Vu[0, 0] = 1 # Track the x-position
Vu[1, 1] = 1 # Track the y-position
ocp.cost.Vu = Vu # Assign it to the cost function
ocp.cost.yref = np.zeros((ny,))
ocp.cost.yref_e = np.zeros((ny_e,))
# y_ref = model.p
p_ref = np.array([[0,0,0,0,0,0],[0,0.01,0,0,0,0],[0,0.02,0,0,0,0],[0,0.03,0,0,0,0],
[0,0.04,0,0,0,0],[0,0.05,0,0,0,0],[0,0.06,0,0,0,0],[0,0.07,0,0,0,0],
[0,0.08,0,0,0,0],[0,0.09,0,0,0,0]])
# set constraints
terminal_state = np.array([0.0, 0.09, 0,0])
ocp.constraints.x0 = np.array([0,0,0,0]) #bounds on the initial state vector
ocp.constraints.lbx_e = terminal_state #lower bounds on x (states) at terminal shooting node N
ocp.constraints.ubx_e = terminal_state #lower bounds on x (states) at terminal shooting node N
ocp.constraints.idxbx_e = np.arange(ny_e) #indices in x vector which needed to be bounded at terminal shooting node N
ocp.constraints.lbu = np.array([0,0]) #lower bound on control input
ocp.constraints.ubu = np.array([0,50]) #upper bound on control input
ocp.constraints.idxbu = np.arange(nu) #indices in x vector which needed to be bounded at terminal shooting node N
# set options
ocp.solver_options.qp_solver = 'PARTIAL_CONDENSING_HPIPM' # 'FULL_CONDENSING_QPOASES', 'PARTIAL_CONDENSING_HPIPM', 'FULL_CONDENSING_HPIPM'
ocp.solver_options.hessian_approx = 'GAUSS_NEWTON' # 'GAUSS_NEWTON', 'EXACT'
ocp.solver_options.integrator_type = 'ERK'
ocp.solver_options.print_level = 1
ocp.solver_options.nlp_solver_type = 'SQP' # SQP_RTI, SQP, DDP
ocp.solver_options.nlp_solver_max_iter = 1000
ocp.solver_options.globalization = 'MERIT_BACKTRACKING'
ocp.solver_options.with_adaptive_levenberg_marquardt = True
ocp.solver_options.tf = Tf
ocp.solver_options.N_horizon = N #timesteps for a predcited horizon
# NOTE: this is needed for PARTIAL_CONDENSING_HPIPM to get expected behavior
qp_tol = 5e-7
ocp.solver_options.qp_solver_tol_stat = qp_tol
ocp.solver_options.qp_solver_tol_eq = qp_tol
ocp.solver_options.qp_solver_tol_ineq = qp_tol
ocp.solver_options.qp_solver_tol_comp = qp_tol
# ocp.translate_to_feasibility_problem()
ocp_solver = AcadosOcpSolver(ocp, json_file = 'acados_ocp.json') #This file contains settings such as solver options, numerical tolerances, and possibly specific configuration data for the solver backend (Acados).
for i in range(N):
if i<N-1:
# yref = p_ref[i]
# Update the OCP cost function's reference for the current time step
ocp_solver.set(i, "yref", p_ref[i,:])
# print(i)
else:
yref_e = p_ref[i,:][:4]
ocp_solver.set(i, "yref", yref_e)
print('finally made it!')
status = ocp_solver.solve() #Solve the ocp with current input.
# ocp_solver.print_statistics()
ocp_solver.get_status()
if status != 0:
raise Exception(f'acados returned status {status}.')
if __name__ == '__main__':
main()