Hierarchical QP

When multiple tasks has been assigned for a robot and can not be realized at the same time, a tradeoff needs to be made among them. In general, two approaches can be adopted to resolve the task redundancy: 

• Weighted Approach

  • Each task assigned with a weight

  • All tasks solved together

  • Tasks compromise each other

• Prioritised Approach 

  • Each task assigned with a priority

  • Solved in a strict hierarchical way

  • Low priority task does not compromise higher priority task

The weighted approach can be formulated as a single QP problem with a summed cost of multiple weighted tasks.

The prioritised approach can be formulated as a sequence of QP problems and they are one after another, you can call it Hierarchical QP or cascade QP. (Note: SQP is short for Sequential Quadratic Programming and it is a method for the numerical solution of constrained nonlinear optimization problems.)

Linear Least Squares problem and Quadratic Programming Problem

Weighted Approach

Multiple Tasks and Single Variable

Multiple Tasks and Multiple Variables

Prioritised Approach

Algorithm: HQP

A single task is defined as: 

task = {'A': A, 'b': b, 'C': C, 'd': d}

A set of tasks with different priorities is defined as:

tasks = [task1, task2, task3]

import numpy as np

import scipy as sp

from scipy.linalg import block_diag

def HQP(tasks, n, regularizer = 1e-10):

    # tasks: list of tasks

    # n: dimension of solution vector

    N_hat = np.eye(n)

    x_hat_star = np.zeros(n)

    w_hat_star = np.zeros(n) 

    C_hat = np.empty((0,n))

    d_hat = np.empty(0)

    for task in tasks:

        A, b, C, d = task['A'], task['b'], task['C'], task['d']

        if A is None or b is None:

            A, b = np.zeros((n,n)), np.zeros(n)

        if C is None or d is None:

            C, d = np.zeros((n,n)), np.zeros(n)

        nEq = len(b)

        nIneq = len(d)

        nNull = N_hat.shape[1]

        nX = nNull + nIneq

        A_hat = block_diag(A @ N_hat, np.eye(nIneq))

        b_hat = np.concatenate((-(A @ x_hat_star - b), np.zeros(nIneq)))

        Q = A_hat.T@A_hat + np.eye(nX)*regularizer

        p = -A_hat.T@b_hat

        C_hat = np.vstack((C_hat, C))

        d_hat = np.concatenate((d_hat, d))

        G = np.zeros((C_hat.shape[0], nX))

        G[:C_hat.shape[0], :nNull] = C_hat@N_hat

        G[-nIneq:, -nIneq:] = -np.eye(nIneq)

        h[:C_hat.shape[0]] = d_hat - C_hat@x_hat_star

        h[:len(w_hat_star)] += w_hat_star

        X = quadprog_solve_qp(P=Q, q=q, G=G, h=h)

        x_star, w_star = X[:nNull], X[-nIneq:]

        x_hat_star = x_hat_star + N_hat@x_star

        w_hat_star = np.concatenate((w_hat_star, w_star))

        N_hat = N_hat@null_space_projector(A@N_hat)

    return x_hat_star

references

1. De Lasa, Martin, Igor Mordatch, and Aaron Hertzmann. "Feature-based locomotion controllers." ACM Transactions on Graphics (TOG) 29.4 (2010): 1-10.

2. Kanoun, Oussama, Florent Lamiraux, and Pierre-Brice Wieber. "Kinematic control of redundant manipulators: Generalizing the task-priority framework to inequality task." IEEE Transactions on Robotics 27.4 (2011): 785-792.

3. Feng, Siyuan. "Online hierarchical optimization for humanoid control." (2016).

4. Herzog, Alexander, et al. "Momentum control with hierarchical inverse dynamics on a torque-controlled humanoid." Autonomous Robots 40.3 (2016): 473-491.

5. Bellicoso, C. Dario, et al. "Perception-less terrain adaptation through whole body control and hierarchical optimization." 2016 IEEE-RAS 16th International Conference on Humanoid Robots (Humanoids). IEEE, 2016.

6. Dietrich, Alexander, Christian Ott, and Alin Albu-Schäffer. "An overview of null space projections for redundant, torque-controlled robots." The International Journal of Robotics Research 34.11 (2015): 1385-1400.