How do I use LinearMPC/MPC with LinearConstraint?

TLDR

TLDR: When I use LinearMPC with LinearConstraint, it seems like self.sym_func = lambda x: self.A @ self.constraint_filter @ x - self.b wouldn’t work with A and b being ndarray whereas during initialization, x is still MX symbolic variable.

Context

Hello, I’m trying to test MPC or LinearMPC for my environment (states: [theta, theta_dot])

I am defining my constraints as such in my environment yaml file:

constraints:
  - constraint_form: bounded_constraint
    lower_bounds: [0] # should match state dim
    upper_bounds: [2.6] 
    constrained_variable: state
    active_dims: [0]  # only position

Then I define the control agent using the recommended make procedure as such (for linear_mpc or mpc):

from safe_control_gym.utils.registration import make
env_func = partial(make, config.task, output_dir=config.output_dir, **config.task_config)
control_agent = make(config.algo,
                    env_func,
                    training=True,
                    checkpoint_path=os.path.join(config.output_dir, "model_latest_manippulator.pt"),
                    output_dir=config.output_dir,
                    device=config.device,
                    seed=config.seed,
                    **config.algo_config)

Then when initializing with

control_agent.reset()

Error

I get the following error:

---------------------------------------------------------------------------
TypeError                                 Traceback (most recent call last)
Cell In [9], line 1
----> 1 control_agent.reset()

File ~/code/rm-sc/safe_control_gym/controllers/mpc/mpc.py:128, in MPC.reset(self)
    126 self.set_dynamics_func()
    127 # CasADi optimizer.
--> 128 self.setup_optimizer()
    129 # Previously solved states & inputs, useful for warm start.
    130 self.x_prev = None

File ~/code/rm-sc/safe_control_gym/controllers/mpc/linear_mpc.py:130, in LinearMPC.setup_optimizer(self)
    127     print(state_constraint)
    128     print(x_var[:,i])
--> 130     opti.subject_to(state_constraint(x_var[:,i] + self.X_LIN.T) < 0)
    131 for input_constraint in self.input_constraints_sym:
    132     opti.subject_to(input_constraint(u_var[:,i] + self.U_LIN.T) < 0)

File ~/code/rm-sc/safe_control_gym/envs/constraints.py:260, in LinearConstraint.__init__.<locals>.<lambda>(x)
    258 self.num_constraints = A.shape[0]
    259 print(self.constraint_filter)
--> 260 self.sym_func = lambda x: self.A @ self.constraint_filter @ x - self.b
    261 self.check_tolerance_shape()

TypeError: operand type(s) all returned NotImplemented from __array_ufunc__(<ufunc 'matmul'>, '__call__', array([[-1.],
       [ 1.]]), MX(([2.5, 0]+opti1_x_1[:2]))): 'ndarray', 'MX'

Hypothesis

It seems like this happens because under LinearConstraint, A and b are set to ndarray by default, but under MPC, at state_constraint(x_var[:,i] + self.X_LIN.T), x_var = opti.variable(nx, T + 1) which is a symbolic variable. My current understanding is if this eventually goes to the part where the controller runs and passes in the x array, it would work, but currently stuck during initialization seemingly due to mismatch between array and symbolic var.

I also tested the functionality with a dummy code as such and it would work with x as array:

lower_bounds = np.array( [0.2])
upper_bounds = np.array([5])

dim = lower_bounds.shape[0]

A = np.vstack((-np.eye(dim), np.eye(dim)))
b = np.hstack((-lower_bounds, upper_bounds))

active_dims = [0]
dim_state = 2
constraint_filter = np.eye(dim_state)[active_dims]
x = np.array([3., 5]) 

print(A@constraint_filter@x)  

which outputs array([-3., 3.]), whereas if i do

opti = cs.Opti()
nx = 1 
T = 5
x_var = opti.variable(nx, T + 1)

print(A@constraint_filter@x)  

A similar error is thrown.

TLDR

TLDR: When I use LinearMPC with LinearConstraint, it seems like self.sym_func = lambda x: self.A @ self.constraint_filter @ x - self.b wouldn’t work with A and b being ndarray whereas during initialization, x is still MX symbolic variable.

Question

May I know how MPC class has been used in the past? Perhaps I’m missing some context here (especially on the casadi side), would love some explanation/clarity on how I can work with the constraints.

Thanks a lot!

Tagging @adamhall @JacopoPan

[adamhall]

Thanks! This was very helpful. To me, it looks like you have a dimension issue. I know the error thrown isn’t very descriptive, but this is an error that can appear if you have a dimension mismatch.

If you look at the dimension of A its (2x1), b is 1x2, but the state variable x_var[:,i] is 2x1. Thus, you are trying to matrix multiply A @ x_var[:,i] which doesn’t work due to dimension mismatch. I think the problem lies in the fact that your env.state_dim = 1 yet x_var is 2x11. Shouldn’t this be a 1x11? But also, if this is a one-link manipulator then your state dim should be 2 (theta, theta_dot)?

You can verify that it works by setting nx=1 near line 151 of mpc.py. If you do this the error you present goes away. Maybe there is an issue with how you are defining the state dimension?

Also, a similar error used to appear due to a version mismatch with numpy and casadi, but I verified this is not the issue here.

[nicholasprayogo]

@JacopoPan Adam helped me find my initial issue, so I think it’s ok to close for now. Thanks @adamhall