Linear centroidal model predictive control for quadruped locomotion

paLeziart commented 7 months ago

Problem

I propose to add the following problem to the GitHub free-for-all test set. The problem can be built as follows by using the CentroidalQuadruped class:

def build_problem():
    # Create the optimal control problem
    gait = np.array(
        [[8, 1, 0, 0, 1], [8, 0, 1, 1, 0], [8, 1, 0, 0, 1], [8, 0, 1, 1, 0]]
    )
    vref = np.array([0.1, 0.0, 0.0]).reshape((-1, 1))
    SQ = CentroidalQuadruped(gait, vref)

    # Cost: x^T P x + q^T x
    P = SQ.get_P()
    q = SQ.get_q()

    # Inequality constraints: G x <= h
    G = SQ.get_G()
    h = SQ.get_h()

    # Equality constraints: A x == b
    A = SQ.get_A()
    b = SQ.get_b()

    # Box constraints: lb <= x <= ub
    lb = SQ.get_lb()
    ub = SQ.get_ub()

    return P, q, G, h, A, b, lb, ub, SQ

Parameter range

gait (Nx5 np array): description of the contact statuses of the feet over the prediction horizon. Each row describes a contact phase, with the first column setting the duration of the phase (in time steps) and the four remaining columns indicating if a foot is in contact (1) or not (0) for the FL, FR, HL, HR feet. Can have an arbitrary number of lines to add various gait phases. For instance, for a trotting gait:
```
gait = np.array(
    [[8, 1, 0, 0, 1], [8, 0, 1, 1, 0]]
)
```
means the prediction horizon spans a single gait period, and that gait period is 16 * dt = 0.32 s. The gait period can be made longer by making the phase longer, like for T = 0.48s:
```
gait = np.array(
    [[12, 1, 0, 0, 1], [12, 0, 1, 1, 0]]
)
```
The prediction horizon can also include more than a single gait period:
```
gait = np.array(
    [[8, 1, 0, 0, 1], [8, 0, 1, 1, 0], [8, 1, 0, 0, 1], [8, 0, 1, 1, 0]]
)
```
vref (3x1 np array): the reference base velocity (Vx, Vy, Wyaw). Can have arbitrary values. For simplicity I did not include a footstep planner in the problem so contact points do not move with the base. So if you put a long prediction horizon and a high velocity target forwards, the MPC will not be able to keep the base horizontal because all the contact points will be far "behind" the robot, next to the origin.

Internal parameter range

There are a bunch of parameters in the init of the class whose values are those used for Solo-12. I disabled the regularization cost for the contact forces to know the optimal solution in simple standing cases (see below), but it can be set up to 5e-5 if you want.

Motivation

This problem is interesting as centroidal model predictive problems are a well-known approach in the community of legged robots when the mass of the legs is not too important with respect to the one of the base.

Solution and optimal cost

Four feet in contact while staying in place

Parameters:

gait = np.array([[16, 1, 1, 1, 1]])
vref = np.zeros((3, 1))

As the base starts horizontal and centered in the middle of the 4 contact points, if the contact force is vertical and perfectly distributed on the four contacts to compensate the gravity, the base will stay perfectly still without tilting.

Solution: $x^* = [0, 0, h, 0, 0, 0, 0, 0, 0, 0, 0, 0]$ for all time steps (base remains at initial position)
Solution: $f^* = [0, 0, mass * g / 4] = [0, 0, 6.13125]$ for all feet (perfectly distributed support)
Optimal cost: $\frac12 x^{*T} P x^* + q^T x^* = 0$ (perfect tracking)

Note that in practice the QP solves for x = [X-Xref F] for all time steps stacked together, so in term of result returned by the solver, for a prediction horizon with 3 steps it is x = [X1-X1ref X2-X2ref X3-X3ref F0 F1 F2]

Two feet in contact while staying in place

Parameters:

gait = np.array([[8, 1, 0, 0, 1], [8, 0, 1, 1, 0]])
vref = np.zeros((3, 1))

As the base starts horizontal and centered in the middle of the 4 contact points, if the contact force is vertical and perfectly distributed on the two contacts to compensate the gravity, the base will stay perfectly still without tilting.

Solution: $x^* = [0, 0, h, 0, 0, 0, 0, 0, 0, 0, 0, 0]$ for all time steps (base remains at initial position)
Solution: $f^* = [0, 0, mass * g / 2] = [0, 0, 12.2625]$ for feet in contact (perfectly distributed support)
Optimal cost: $\frac12 x^{*T} P x^* + q^T x^* = 0$ (perfect tracking)

These optimal solutions are only valid when the contact force regularization coefficients are 0, otherwise there is a small trade-off between state tracking and applied forces.

Source code

https://gitlab.laas.fr/paleziart/quadruped-qp-problems

Testing

You can launch centroidalMPC.py to solve an example problem with OSQP and display a bunch of plots about state tracking and contact forces.

References

1. Léziart, P. A. (2022). Locomotion control of a lightweight quadruped robot (Doctoral dissertation, UPS Toulouse).

See pages 91-95 for a description of the linear centroidal model predictive controller.

stephane-caron commented 6 months ago

Thanks a bunch @paLeziart for this contribution! I've kick-started the mpc_qpbenchmark test set with your contribution, to which I'll add the ones I have lying around for HRP-4 and Upkie. Feel free to share your thoughts on this test set and how we could improve it.

stephane-caron commented 6 months ago

I'll add the ones I have lying around for HRP-4 and Upkie.

Closing this issue now that it is done.

qpsolvers / mpc_qpbenchmark