How do I use LinearMPC/MPC with LinearConstraint?

nicholasprayogo commented 1 year ago

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 commented 1 year ago

Hey! Thanks for the error reporting. Linear MPC does in general work with linear constraints, so I'll take a look at what is going on.

Is there a script I could run on your local branch to recreate the error? Also, would you be able to list all the packages with their versions that you are using, along with what OS you are using? Thanks :)

nicholasprayogo commented 1 year ago

Hi Adam thanks! Here are the details.

Test Script

A bit messy still, but here's my branch:

To run a test script I have prepared:

python3 examples/run_manipulator_classical.py

Versions

OS: M1 Mac OSX 12.2.1, running this on Rosetta (Intel X86 emulator).

Had issues installing pybullet through pip due to build issues (either related to clang or OSX 12), so I use the conda version which is apparently listed as 3.21 although on pip it shows 3.2.0.
casadi version: 3.5.5.post2
safe_control_gym version: 0.5.0, the one present in that branch. (apparently ever since I forked there has been crucial changes, so it's not up-to-date)

For full package list, it's listed here.

On a fresh Python 3.9.6 env, i did

conda install pybullet
pip install -e .

adamhall commented 1 year ago

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 commented 1 year ago

Thank you so much for pointing this out.

Yes indeed, my env.state_dim was not updated with my observation_space since I forgot to set observed_link_state_keys: [angular_pos_vel] instead of observed_link_state_keys: [angular_pos] in my manipulator.yaml file, when using [theta, theta_dot] (will also make sure this automatically changes if i change how many states I'm using).

Setting it as such fixes the issue now I can see the error is resolved. Thanks! 🙌 And thanks to verify regarding the possible version mismatch case.

Also since we are discussing state dimensions, I was actually testing with just 1 dim as you mentioned (nx=1) but it wouldn't work with mpc (the nonlinear version) since this error would be thrown:

Traceback (most recent call last):
  File "/Users/nicholasprayogo/code/rm-sc/examples/run_manipulator_classical.py", line 41, in <module>
    control_agent.run()
  File "/Users/nicholasprayogo/code/rm-sc/safe_control_gym/controllers/mpc/mpc.py", line 327, in run
    action = self.select_action(obs)
  File "/Users/nicholasprayogo/code/rm-sc/safe_control_gym/controllers/mpc/mpc.py", line 239, in select_action
    x_guess[:, :-1] = x_guess[:, 1:]
IndexError: too many indices for array: array is 1-dimensional, but 2 were indexed

Could you perhaps help me understand why only the 2nd dimension (theta_dot) is being indexed in this case, and therefore nx<2 wouldn't work? I see this isn't the case with linear_mpc, so curious to know what is different. (perhaps I will need to review how linear and nonlinear MPC works myself to understand this but thought an insight from you would help :D )

Much thanks

JacopoPan commented 1 year ago

@nicholasprayogo @adamhall is this ongoing or to be closed?

nicholasprayogo commented 1 year ago

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

utiasDSL / safe-control-gym