Toy hybrid system MPC formulation

[LemonPi]

Hi Tobia,

I'm trying to use your pympc to solve a toy 2D problem:

Where there are 2 modes; red is the initial state and green is the goal state:

init = np.array([-0.32715457, 1.53059952])
goal = np.array([1.09687902, -1.85073286])

The dynamics inside each mode are very similar:

A = np.eye(2)
B1 = np.eye(2)
B2 = np.eye(2) * 0.2
S1 = LinearSystem(A, B1)
S2 = LinearSystem(A, B2)
S_list = [S1, S2]

The A and B are the same even though we want x -> x_r because we can make the substitution z = x - x_r:

x = z + xr
x' = Ax + Bu
z' + xr = A(z + xr) + Bu
z' = Az + Bu + (Axr - xr)
since A = I
z' = Az + Bu

The domains need to be shifted to this error space where the origin is when our state is at the goal. So I'm defining this by:

# domains of the modes
x1_min = np.array([-3, 0]) - goal
x1_max = np.array([3, 3]) - goal
X1 = Polyhedron.from_bounds(x1_min, x1_max)
assert X1.bounded

# state domain n.2
x2_min = np.array([-3, -3]) - goal
x2_max = np.array([3, 0]) - goal
X2 = Polyhedron.from_bounds(x2_min, x2_max)
assert X2.bounded

# input domain
u_min = np.array([-4, -4])
u_max = np.array([4, 4])
U = Polyhedron.from_bounds(u_min, u_max)
assert U.bounded

# domains
D1 = X1.cartesian_product(U)
D2 = X2.cartesian_product(U)
D_list = [D1, D2]

I set up the controller as shown in the example:

S = PieceWiseAffineSystem(S_list, D_list)

# controller parameters
N = 20

# terminal set and cost
P, K = S2.solve_dare(Q, R)
X_N = S2.mcais(K, D2)

# hybrid MPC controller
controller = HybridModelPredictiveController(S, N, Q, R, P, X_N)

for i in range(50):
    # ww is my own simulation of the hybrid system; we get similar results when simulating on S
    u = controller.feedback(ww.x - ww.goal())
    ww.step(u)

But the end result is something really weird:

It just ping pongs around the boundary of the modes. Is any part of my formulation wrong?