TinyMPC Bound Constraints Not Working? Let's Troubleshoot!

Hey there, fellow robotics enthusiasts! If you're here, chances are you've run into a bit of a snag while trying to implement bound constraints in your TinyMPC setup. Specifically, you're seeing those constraints on your row and pitch angles get violated, and it's got you scratching your head. Don't worry, you're definitely not alone! Let's dive deep and see if we can get your TinyMPC model behaving the way you want it to. We'll start by breaking down the code you've provided, understanding where the potential issues might lie, and then offer some suggestions to get those constraints locked in.

Understanding the Problem: Why Aren't Your Bounds Working?

So, you've set up your get_bound_constraints function, you've fed in your x_min, x_max, u_min, and u_max to solver.set_bound_constraints, and yet, the row and pitch angles are straying outside of the limits you've defined. It's frustrating, I know. The core of the issue lies in how these constraints are being interpreted and enforced within the TinyMPC solver itself. Let's dissect the code snippet you shared to pinpoint the weak spots.

Your code looks pretty standard for setting up the bounds. You're correctly using np.full to initialize your state bounds and then selectively modifying the elements. The use of np.deg2rad(delta) is also spot on, ensuring your angle limits are in the correct units. However, we have to consider how TinyMPC actually uses these bounds during the optimization process. Things to look for could be numerical instability, incorrect scaling, or an issue with the solver's internal settings. The first thing we should look at is whether the bounds are being correctly applied at each time step during the simulation. Another area to look at is the structure of your state and input vectors. Double-check that the indices you're using to set the bounds (e.g., x_min[3] = -s, x_max[4] = s) correspond to the correct state variables (row and pitch in your case). Lastly, review your reference trajectory. If the reference trajectory itself violates the bounds, the solver will struggle to enforce them, and you might see the actual states deviate. Also, make sure the units used in your get_bound_constraints are consistent with the units used in your A_d and B_d matrices which are the discrete-time state-space representation of your system.

Potential Causes of Constraint Violations

• Incorrect Indexing: A common mistake is misinterpreting the order of states in your state vector x. Double-check that indices 3 and 4 (row and pitch) actually correspond to those angles in your state vector. It's an easy thing to overlook!
• Units Mismatch: Make sure the units used in your get_bound_constraints are consistent with the units used in your state-space representation (A_d and B_d). For example, if A_d and B_d are expecting radians, but you're providing degrees in your bounds, that's a problem.
• Numerical Issues: Optimization algorithms, even those in TinyMPC, can have trouble with extremely tight bounds or values that are very close to zero. The solver might struggle to find a feasible solution in such cases.
• Reference Trajectory Conflicts: If the desired trajectory (x_ref) or control inputs (u_ref) you're feeding the solver inherently violate the bounds, the solver will have a tough time. It will try its best to stay within bounds, but it may not always be possible. Ensure that your reference trajectory respects the limits you've set.

Deep Dive into Your Code: Pinpointing the Issues

Let's break down your code snippet and look for areas that might be causing problems. We'll examine each function to see if we can find some clues.

Analyzing get_bound_constraints

def get_bound_constraints(delta):
    s = np.deg2rad(delta)
    print(s)
    x_min = np.full(NSTATES, -np.inf, dtype=np.float64)
    x_max = np.full(NSTATES,  np.inf, dtype=np.float64)
    x_min[2] = 0.05   # z
    x_min[3] = -s  # row
    x_max[3] = s
    x_min[4] = -s  # pitch
    x_max[4] =  s

    u_min = np.array([0, 0, 0, 0], dtype=np.float64)
    u_max = np.array([0.35,  1e-5,  1e-5,  1e-5], dtype=np.float64)
    return x_min, x_max, u_min, u_max

This function seems correctly structured. The delta parameter, which determines the maximum roll and pitch angles, is converted to radians. The use of np.inf for the non-constrained states is a good practice. But, let's look at the NSTATES variable which dictates the total number of states in your model. Ensure that the indices you are using to constrain row and pitch (indices 3 and 4) correctly correspond to the correct states in your system. This also applies to NINPUTS that you have set.

Examining tinympc_control

def tinympc_control(x0, x_ref, u_ref, Q, R, N, p_o):
    #  x_ref: nx * N
    #  u_ref: nu * (N-1)
    solver = tinympc.TinyMPC()
    solver.setup(A_d, B_d, Q, R, N, rho=1.0, verbose=False)
    solver.set_x0(x0)
    solver.set_x_ref(x_ref)
    solver.set_u_ref(u_ref)
    x_min, x_max, u_min, u_max = get_bound_constraints(delta=10)
    solver.set_bound_constraints(x_min, x_max, u_min, u_max)

    p_q = x0[0:3]
    # a, b = make_obstacle_plane(p_q, p_o, 0.15)
    # solver.set_linear_constraints(a, b, np.zeros(NINPUTS), 0)

    solution = solver.solve()  # dict
    u0 = solution[