Not having an accurate result / error not match the cost

[alwaysbyx]

❓ Questions and Help

I am following the simple example to solve a simple Model predictive control (1dimension) problem:
but got the incorrect result.

Using LQR controller law u[0] should around -23, and the solver gives -11.4.

And also I was wondering why the error output is not the same as the cost I defined. Thanks!

        A = th.Variable(torch.ones(1,1), name='A')
        B = th.Variable(torch.ones(1,1), name='B')
        Q = th.Variable(torch.ones(1,1), name='Q')
        R = th.Variable(torch.ones(1,1), name='R')
        x_init = th.Variable(torch.ones(1,1),name='x0')
        us = [th.Vector(1, name=f'action{t}') for t in range(self.T)]
        def cost_fn(optim_vars, aux_vars):
            us_ = optim_vars
            A_, B_, Q_, R_, x_init_ = aux_vars
            x_lists_ = [x_init_]
            cost = []
            for t in range(self.T):
                if t == 0: 
                    x_lists_.append(x_lists_[-1].tensor @ A_.tensor +  us_[t].tensor @ B_.tensor)
                    cost.append(Q_.tensor * x_lists_[0].tensor.square()) + R_.tensor * us_[t].tensor.square())
                else: 
                    x_lists_.append(x_lists_[-1] * A_.tensor + us_[t].tensor * B_.tensor)
                    cost.append(Q_.tensor * x_lists_[t].square()  + R_.tensor * us_[t].tensor.square())
            return torch.cat(cost_list, axis=-1)
        optim_vars = us
        aux_vars = [A, B, Q, R, x_init]
        cost_function = th.AutoDiffCostFunction(
            optim_vars, cost_fn, self.T, aux_vars=aux_vars, name="cost_fn"
        )
        objective = th.Objective()
        objective.add(cost_function)
        optimizer = th.LevenbergMarquardt(
            objective,
            max_iterations=10,
            step_size=1.0,
        )
        layer = th.TheseusLayer(optimizer, vectorize=False)

        theseus_inputs = {
                    "A": 0.5 * torch.ones(1,1), 
                    "B": 1.0 * torch.ones(1,1),
                    "Q": 10.0 * torch.ones(1,1),
                    "R": 1.0 * torch.ones(1,1),
                    "x0": 50.0 * torch.ones(1,1),
                    "action": torch.ones(1,self.T),
                }
          with torch.no_grad():
              updated_inputs, info = layer.forward(
                  theseus_inputs, optimizer_kwargs={"track_best_solution": True, "verbose":True})

[luisenp]

Hi @alwaysbyx, sorry for the long delay, I've been incredibly busy. The code you shared above doesn't run, I had to make some modifications: set self.T to some value (I used 10), and replace torch.cat(cost_list) with torch.cat(cost), and also fix some parenthesis issues. As such, I cannot reproduce the numbers you are reporting.

[luisenp]

Regarding the error output, I'm assuming you are referring to the optimizer logs. If so, this corresponds to the sum of weighted squared costs, divided by two; here is the code.

[alwaysbyx]

Hi, sorry for some mistakes in the above code. I am sending the code file link to your email and compare a simple problem (MPC with disturbance) of theseus and gurobi solver. Do you receive that email? I believe you can directly run that code and compare the performance.

[luisenp]

Hi @alwaysbyx ,

I saw your email, thanks for your interest in Theseus and your kind words!

I was looking at your colab, and one thing I noticed is that you are squaring the terms in your cost function. However, note that Theseus automatically squares the weighted costs passed to the optimizer. So, for example, if you create this

import theseus as th
 
x = th.Vector(1, name="x")
t = th.Vector(1, name="t")
w = th.ScaleCostWeight(2.0, name="w")
obj = th.Objective()
obj.add(th.Difference(x, t, w)

what you are minimizing is obj = 4(x - t)**2. So, in your example, your optimization terms should be of the form sqrt(R) * u_t. Also, notice that the sum cost_u + cost_r, means you are optimizing (cost_u ** 2 + cost_u * cost_r + cost_r **2), so this should be avoided as well.

I also didn't see the terms xu[t] == x[t] * u[t] being added to Theseus, did I miss this?

The other thing I noticed is that you are creating a single cost function for all terms, which are a mix of equality constraints and optimization terms. Might be clearer to create separate cost functions for each, although this is not a hard requirement.

Finally, note that we don't yet have explicit support for equality constraints, so it might not be possible to exactly reproduce the results with gurobi, even after adding these fixes (see #519, for example).

Let me know if this helps and feel free to share updated versions; this small example could be useful for us in debugging problems with equality constraints.

[alwaysbyx]

Hi Thank you for your comments. I do not implement xu[t] == x[t] * u[t] in Theseus but this holds when evaluating the dynamics in Theseus (B_.tensor * us_[t].tensor * xs_true[-1]).