Stability of Surface Contacts for Humanoid Robots

Closed-Form Formulae of the Contact Wrench Cone for Rectangular Support Areas

Stéphane Caron, Quang-Cuong Pham, Yoshihiko Nakamura. The 2015 IEEE International Conference on Robotics and Automation (ICRA), Seattle, USA, May 2015.

Abstract

Humanoids locomote by making and breaking contacts with their environment. Thus, a crucial question for them is to anticipate whether a contact will hold or break under effort. For rigid surface contacts, existing methods usually consider several point-contact forces, which has some drawbacks due to the underlying redundancy. We derive a criterion, the Contact Wrench Cone (CWC), which is equivalent to any number of applied forces on the contact surface, and for which we provide a closed-form formula.

Illustration of the paper's contribution

It turns out that the CWC can be decomposed into three conditions: (i) Coulomb friction on the resultant force, (ii) CoP inside the support area, and (iii) upper and lower bounds on the yaw torque. While the first two are well-known, the third one is novel. It can, for instance, be used to prevent the undesired foot yaws observed in biped locomotion. We show that our formula yields simpler and faster computations than existing approaches for humanoid motions in single support, and assess its validity in the OpenHRP simulator.

BibTeX

@inproceedings{caron2015icra,
  title = {Stability of Surface Contacts for Humanoid Robots: Closed-Form Formulae of the Contact Wrench for Rectangular Support Areas},
  author = {Caron, St{\'e}phane and Pham, Quang-Cuong and Nakamura, Yoshihiko},
  booktitle = {Robotics and Automation (ICRA), 2015 IEEE International Conference on},
  pages = {5107--5112},
  year = {2015},
  month = {May},
  organization = {IEEE},
  doi = {10.1109/ICRA.2015.7139910}
}

Code

Here is a Python script that checks the formula of the Contact Wrench Cone against an LP solver on random inputs:

#!/usr/bin/env python
# -*- coding: utf-8 -*-

import cvxopt
import cvxopt.solvers
import pylab

cvxopt.solvers.options['show_progress'] = False

X = .5 * 0.224
Y = .5 * 0.130
NB_TRIALS = 10000

def check_on_random_instance():
    """
    Returns a random problem instance, the CWC formula's outcome on this
    point (true or false) and whether an LP solution was found (positive or
    negative). If the formula is correct, true (resp. false) should always
    match positive (resp. negative).
    """
    K1, K2, K3, C1, C2 = 2 * pylab.random(5) - 1
    px, py = X / (X + Y), Y / (X + Y)
    D4_max = .5 * min(2, 1 + C1, 1 + C2, 2 + C1 + C2)
    D4_min = .5 * max(-1 + C1, C1 + C2, -1 + C2, 0)
    D4 = D4_min + (D4_max - D4_min) * pylab.random()
    D1, D2, D3 = .5 * (1 + C1) - D4, -.5 * (C1 + C2) + D4, .5 * (1 + C2) - D4
    c = cvxopt.matrix(pylab.array([[1.]] * 8))   # arbitrary score vector
    G = cvxopt.matrix(pylab.array([  # h - G x >= 0
        [+1, 0., 0., 0., 0., 0., 0., 0.],
        [-1, 0., 0., 0., 0., 0., 0., 0.],
        [0., +1, 0., 0., 0., 0., 0., 0.],
        [0., -1, 0., 0., 0., 0., 0., 0.],
        [0., 0., +1, 0., 0., 0., 0., 0.],
        [0., 0., -1, 0., 0., 0., 0., 0.],
        [0., 0., 0., +1, 0., 0., 0., 0.],
        [0., 0., 0., -1, 0., 0., 0., 0.],
        [0., 0., 0., 0., +1, 0., 0., 0.],
        [0., 0., 0., 0., -1, 0., 0., 0.],
        [0., 0., 0., 0., 0., +1, 0., 0.],
        [0., 0., 0., 0., 0., -1, 0., 0.],
        [0., 0., 0., 0., 0., 0., +1, 0.],
        [0., 0., 0., 0., 0., 0., -1, 0.],
        [0., 0., 0., 0., 0., 0., 0., +1],
        [0., 0., 0., 0., 0., 0., 0., -1]]))
    h = cvxopt.matrix(pylab.array([[1.]] * 16))
    A = cvxopt.matrix(pylab.array([  # A x = b
        [D1, D2, D3, D4, 0, 0, 0, 0],
        [0, 0, 0, 0, D1, D2, D3, D4],
        [-py * D1, +py * D2, +py * D3, -py * D4,
        +px * D1, +px * D2, -px * D3, -px * D4]]))
    b = cvxopt.matrix(pylab.array([K1, K2, K3]))
    sol = cvxopt.solvers.lp(c, G, h, A, b)
    K3_min = -1 + py * abs(K1 - C1) + px * abs(K2 - C2)
    K3_max = 1 - py * abs(K1 + C1) - px * abs(K2 + C2)
    is_true = K3_min <= K3 <= K3_max
    is_positive = sol['x'] is not None
    return is_true, is_positive, (K1, K2, K3, C1, C2)

if __name__ == "__main__":
    true_positives = []
    true_negatives = []
    false_positives = []
    false_negatives = []
    print "Checking against %d random samples..." % NB_TRIALS
    for _ in xrange(NB_TRIALS):
        is_true, is_positive, inst = check_on_random_instance()
        l = true_positives if is_true and is_positive \
            else true_negatives if is_true and not is_positive \
            else false_positives if is_positive and not is_true \
            else false_negatives
        l.append(inst)
    print "Success rate: %.2f%%" % (
        (len(true_positives) + len(false_negatives)) * 100. / NB_TRIALS)
    print "%d false-positives" % len(false_positives)
    print "%d true-negatives" % len(true_negatives)
Content on this website is under the CC-BY 4.0 license.