Awesome

contact_stability

ROS package for checking multi-contact stability.

Criteria

• Pendular ZMP support areas: most of today's walking pattern generators regulate the angular momentum to a constant value (Linear Pendulum Mode). In this case, the motion is contact stable if and only if the whole-body ZMP lies in the pendular ZMP support area [1].

• Static-equilibrium COM polygon: when the robot is not moving, contact stability is enforced if and only if the (horizontal projection of the) center of mass lies in this polygon [2].

Algorithms

• Bretl and Lall: the algorithm introduced by Bretl and Lall [2], and used in a wealth of subsequent works [3, 4, 5]. It can also be applied to ZMP support areas [1].

• Double-description method: an algorithm used to convert between halfspace and vertex representation of polyhedral sets. Has been applied to compute the static-equilibrium polygon [6] as well as the pendular ZMP support area [1]. It is faster but less numerically stable than the other method (see e.g. the Appendix in this paper).

To choose between both, set the ALGORITHM global variable to 'bretl' or 'cdd' in the server script (pendular_server.py or static_server.py).

Installation

Clone the repository:

git clone --recursive https://github.com/stephane-caron/contact_stability.git

Link it into your Catkin workspace and launch the service corresponding to the criterion you want to compute:

roslaunch contact_stability pendular.launch  # pendular ZMP support area
roslaunch contact_stability static.launch    # static-equilibrium COM polygon
roslaunch contact_stability all.launch       # everything

Usage

Let us compute the pendular ZMP support area. First, initialize ROS and make a service proxy:

rospy.init_node('my_node')
rospy.wait_for_service(
    '/contact_stability/pendular/compute_support_area')
compute_support_area = rospy.ServiceProxy(
    '/contact_stability/pendular/compute_support_area',
    contact_stability.srv.SupportArea)

Next, you will need to prepare a list of Contact messages. Assuming you already have a list contacts describing your surface contacts (e.g. a list of pymanoid.Contact objects):

ros_contacts = [
    contact_stability.msg.Contact(
        position=geometry_msgs.msg.Point(
            contact.p[0],      # x
            contact.p[1],      # y
            contact.p[2]),     # z
        orientation=geometry_msgs.msg.Quaternion(
            contact.pose[1],   # x
            contact.pose[2],   # y
            contact.pose[3],   # z
            contact.pose[0]),  # w comes first in OpenRAVE convention
        halflen=contact.X,     # half-length of the contact surface
        halfwidth=contact.Y,   # half-width of the contact surface
        friction=contact.friction)
    for contact in contacts]

Finally, create a SupportAreaRequest and call the ROS service with:

p_com = geometry_msgs.msg.Point(com.x, com.y, com.z)
req = contact_stability.srv.SupportAreaRequest(
    contacts=ros_contacts,
    mass=robot.mass,
    p_in=p_com,  # area depends on COM position
    z_out=z_out)
res = compute_support_area(req)

The res object then contains the vertex representation res.vertices and res.rays of your support area.