# Examples¶

## Linear inverted pendulum stabilization¶

This example implements a basic stabilizer for the linear inverted pendulum model based on proportional feedback of the divergent component of motion.

class lip_stabilization.Pusher(pendulum, gain=0.1)

Send impulses to the inverted pendulum every once in a while.

Parameters: pendulum (pymanoid.models.InvertedPendulum) – Inverted pendulum to de-stabilize. gain (scalar) – Magnitude of velocity jumps.

Notes

You know, I’ve seen a lot of people walkin’ ‘round // With tombstones in their eyes // But the pusher don’t care // Ah, if you live or if you die

on_tick(sim)

Apply regular impulses to the inverted pendulum.

Parameters: sim (pymanoid.Simulation) – Simulation instance.
class lip_stabilization.Stabilizer(pendulum, gain=2.0)

Inverted pendulum stabilizer based on proportional DCM feedback.

Parameters: pendulum (pymanoid.models.InvertedPendulum) – Inverted pendulum to stabilizer. gain (scalar) – DCM feedback gain.
draw(dcm, cop)

Draw extra points to illustrate stabilizer behavior.

Parameters: dcm ((3,) array) – Divergent component of motion. cop ((3,) array) – Center of pressure.
on_tick(sim)

Set inverted pendulum CoP and stiffness inputs.

Parameters: sim (pymanoid.Simulation) – Simulation instance.

Notes

See [Englsberger15] for details on the definition of the virtual repellent point (VRP). Here we differentiate between the constants lambda and omega: lambda corresponds to the “CoP-based inverted pendulum” and omega to the “floating-base inverted pendulum” models described in Section II.B of [Caron17w].

Overall, we can interpret omega as a normalized stiffness between the CoM and VRP, while lambda corresponds to the virtual leg stiffness between the CoM and ZMP. The reason why the mapping is nonlinear is that the ZMP is constrained to lie on the contact surface, while the CoM can move in 3D.

If we study further the relationship between lambda and omega, we find that they are actually related by a Riccati equation [Caron19].

## Inverse kinematics¶

This example uses inverse kinematics (IK) to achieve a set of whole-body tasks. It contains two equivalent implementations of the IK solver setup. The former is best for beginners as it uses the simpler Stance interface. The latter is for more advanced users and shows how to add individual tasks one by one.

The example loads the JVRC-1 humanoid model, then generates a posture where the robot has both feet on pre-defined contact locations. The robot tracks a reference COM position given by the red box, which you can move around directly by using the interaction mode of the OpenRAVE GUI.

inverse_kinematics.setup_ik_from_stance()

Setup inverse kinematics from the simpler stance interface.

Notes

This function is equivalent to setup_ik_from_tasks() below.

Note

This function is equivalent to setup_ik_from_stance() above. Beginners should take a look at that one first.

Notes

See this tutorial on inverse kinematics for details.

## Contact stability¶

### Wrench friction cone¶

This example shows how to compute the contact wrench cone (CWC), a generalized multi-contact friction cone. See [Caron15] for details.

### CoM static-equilibrium polygon¶

This example computes the static-equilibrium CoM polygon, i.e. the set of CoM positions that can be sustained usingf a given set of contacts. See [Caron16] for details.

class com_static_polygon.COMSync(stance, com_above)

Update stance CoM from the GUI handle in polygon above the robot.

Parameters: stance (pymanoid.Stance) – Contacts and COM position of the robot. com_above (pymanoid.Cube) – CoM handle in static-equilibrium polygon.
class com_static_polygon.SupportPolygonDrawer(stance, method, z_polygon, color='g')

Draw the static-equilibrium polygon of a contact set.

Parameters: stance (pymanoid.Stance) – Contacts and COM position of the robot. method (string) – Method to compute the static equilibrium polygon. Choices are: ‘bretl’, ‘cdd’ and ‘hull’. z_polygon (scalar) – Height where to draw the CoM static-equilibrium polygon. color (tuple or string, optional) – Area color.

### Multi-contact ZMP support areas¶

This example computes the multi-contact ZMP support area for a given robot stance (contacts and CoM position). See [Caron16] for details.

class zmp_support_area.StaticWrenchDrawer(stance)

Draw contact wrenches applied to a robot in static-equilibrium.

Parameters: stance (Stance) – Contacts and COM position of the robot.

Draw the pendular ZMP area of a contact set.

Parameters: stance (Stance) – Contacts and COM position of the robot. height (scalar, optional) – Height of the ZMP support area in the world frame. color (tuple or string, optional) – Area color.

### CoM acceleration cone¶

This example shows the set of 3D CoM accelerations that a given set of contacts can realize in the pendulum mode of motion (i.e. no angular momentum). See [Caron16] for details.

class com_accel_cone.AccelConeDrawer(stance, scale=0.1, color=None)

Draw the COM acceleration cone of a contact set.

Parameters: stance (Stance) – Contacts and COM position of the robot. scale (scalar, optional) – Acceleration to distance conversion ratio, in [s]^2. color (tuple or string, optional) – Area color.
class com_accel_cone.StaticWrenchDrawer(stance)

Draw contact wrenches applied to a robot in static-equilibrium.

Parameters: stance (Stance) – Contacts and COM position of the robot.

## Horizontal walking¶

This example implements a walking pattern generator for horizontal walking based on linear model predictive control <https://hal.inria.fr/file/index/docid/390462/filename/Preview.pdf>.

class horizontal_walking.HorizontalWalkingFSM(ssp_duration, dsp_duration)

Finite State Machine for biped walking.

Parameters: ssp_duration (scalar) – Duration of single-support phases, in [s]. dsp_duration (scalar) – Duration of double-support phases, in [s].
on_tick(sim)

Update function run at every simulation tick.

Parameters: sim (Simulation) – Instance of the current simulation.
run_com_mpc()

Run CoM predictive control for single-support state.

run_double_support()

Run double-support state.

run_single_support()

Run single-support state.

run_standing()

Run standing state.

run_swing_foot()

Run swing foot interpolator for single-support state.

start_double_support()

Switch to double-support state.

start_single_support()

Switch to single-support state.

start_standing()

Switch to standing state.

start_swing_foot()

Initialize swing foot interpolator for single-support state.

Generate a set of footsteps for walking forward.

Parameters: distance (scalar) – Total distance to walk forward in [m]. step_length (scalar) – Distance between right and left heel in double support. foot_spread (scalar) – Lateral distance between left and right foot centers. friction (scalar) – Friction coefficient between a robot foot and a step.

## Multi-contact walking¶

This example implements a walking pattern generator for multi-contact locomotion based on predictive control of the 3D acceleration of the center of mass [Caron16].

class multi_contact_walking.COMTube(start_com, target_com, start_stance, next_stance, radius, margin=0.01)

Primal tube of COM locations, computed with its dual acceleration cone.

Parameters: start_com (array) – Start position of the COM. target_com (array) – End position of the COM. start_stance (Stance) – Stance used to compute the contact wrench cone. radius (scalar) – Side of the cross-section square (for shape > 2). margin (scalar) – Safety margin (in [m]) around boundary COM positions.

Notes

When there is an SS-to-DS contact switch, this strategy computes one primal tube and two dual intersection cones. The primal tube is a parallelepiped containing both the COM current and target locations. Its dual cone is used during the DS phase. The dual cone for the SS phase is calculated by intersecting the latter with the dual cone of the current COM position in single-contact.

compute_dual_hrep()

Compute halfspaces of the dual cones.

compute_dual_vrep()

Compute vertices of the dual cones.

compute_primal_hrep()

Compute halfspaces of the primal tube.

compute_primal_vrep()

Compute vertices of the primal tube.

class multi_contact_walking.COMTubePredictiveControl(com, fsm, preview_buffer, nb_mpc_steps, tube_radius)

Feedback controller that continuously runs the preview controller and sends outputs to a COMAccelBuffer.

Parameters: com (PointMass) – Current state (position and velocity) of the COM. fsm (MultiContactWalkingFSM) – Instance of finite state machine. preview_buffer (PreviewBuffer) – MPC outputs are sent to this buffer. nb_mpc_steps (int) – Discretization step of the preview window. tube_radius (scalar) – Tube radius in [m] for the L1 norm.
compute_preview_control(switch_time, horizon, state_constraints=False)

Compute controller and store it in self.preview_control.

compute_preview_tube()

Compute preview tube and store it in self.tube.

on_tick(sim)

Entry point called at each simulation tick.

Parameters: sim (Simulation) – Instance of the current simulation.
class multi_contact_walking.MultiContactWalkingFSM(stances, robot, swing_height, cycle=False)

Finite State Machine for biped walking.

Parameters: stances (list of Stances) – Consecutives stances traversed by the FSM. robot (Robot) – Controller robot. swing_height (scalar) – Relative height in [m] for the apex of swing foot trajectories. cycle (bool, optional) – If True, the first stance will succeed the last one.
on_tick(sim)

Update the FSM after a tick of the control loop.

Parameters: sim (Simulation) – Instance of the current simulation.
class multi_contact_walking.PreviewBuffer(u_dim, callback=None)

Buffer used to store controls on a preview window.

Parameters: u_dim (int) – Dimension of preview control vectors. callback (function, optional) – Function to call with each new control (u, dT).
get_next_control()

Get the next pair (u, dT) in the preview window.

Returns: (u, dT) – Next control in the preview window. array, scalar
on_tick(sim)

Entry point called at each simulation tick.

Parameters: sim (Simulation) – Current simulation instance.
reset()

Reset preview buffer to its empty state.

update_preview(U, dT, switch_step=None)

Update preview with a filled PreviewControl object.

Parameters: U (array, shape=(N * d,)) – Vector of stacked preview controls, each of dimension d. dT (array, shape=(N,)) – Sequence of durations, one for each preview control. switch_step (int, optional) – Optional index of a contact-switch step in the sequence.
class multi_contact_walking.PreviewDrawer

Draw preview trajectory, in blue and yellow for the SS and DS parts respectively.

on_tick(sim)

Entry point called at each simulation tick.

Parameters: sim (Simulation) – Instance of the current simulation.
class multi_contact_walking.SwingFoot(swing_height, color='c')

Invisible body used for swing foot interpolation.

Parameters: swing_height (double) – Height in [m] for the apex of the foot trajectory. color (char, optional) – Color applied to all links of the KinBody.
reset(start_pose, end_pose)

Reset both end poses of the interpolation.

Parameters: start_pose (array) – New start pose. end_pose (array) – New end pose.
update_pose(s)

Update pose to a given index s in the swing-foot motion.

Parameters: s (scalar) – Index between 0 and 1 in the swing-foot motion.
class multi_contact_walking.TubeDrawer

Draw preview COM tube along with its dual acceleration cone.

on_tick(sim)

Entry point called at each simulation tick.

Parameters: sim (Simulation) – Instance of the current simulation.
multi_contact_walking.generate_staircase(radius, angular_step, height, roughness, friction, ds_duration, ss_duration, init_com_offset=None)

Generate a new slanted staircase with tilted steps.

Parameters: radius (scalar) – Staircase radius in [m]. angular_step (scalar) – Angular step between contacts in [rad]. height (scalar) – Altitude variation in [m]. roughness (scalar) – Amplitude of contact roll, pitch and yaw in [rad]. friction (scalar) – Friction coefficient between a robot foot and a step. ds_duration (scalar) – Duration of double-support phases in [s]. ss_duration (scalar) – Duration of single-support phases in [s]. init_com_offset (array, optional) – Initial offset applied to first stance.
multi_contact_walking.random()

random_sample(size=None)

Return random floats in the half-open interval [0.0, 1.0).

Results are from the “continuous uniform” distribution over the stated interval. To sample $$Unif[a, b), b > a$$ multiply the output of random_sample by (b-a) and add a:

(b - a) * random_sample() + a
Parameters: size (int or tuple of ints, optional) – Output shape. If the given shape is, e.g., (m, n, k), then m * n * k samples are drawn. Default is None, in which case a single value is returned. out – Array of random floats of shape size (unless size=None, in which case a single float is returned). float or ndarray of floats

Examples

>>> np.random.random_sample()
0.47108547995356098
>>> type(np.random.random_sample())
<type 'float'>
>>> np.random.random_sample((5,))
array([ 0.30220482,  0.86820401,  0.1654503 ,  0.11659149,  0.54323428])

Three-by-two array of random numbers from [-5, 0):

>>> 5 * np.random.random_sample((3, 2)) - 5
array([[-3.99149989, -0.52338984],
[-2.99091858, -0.79479508],
[-1.23204345, -1.75224494]])
multi_contact_walking.seed(seed=None)

Seed the generator.

This method is called when RandomState is initialized. It can be called again to re-seed the generator. For details, see RandomState.

Parameters: seed (int or array_like, optional) – Seed for RandomState. Must be convertible to 32 bit unsigned integers.

RandomState()

## VHIP stabilization¶

This example compares two stabilizers for the inverted pendulum model. The first one (baseline) is based on proportional feedback of the 3D DCM [Englsberger15]. The second one (proposed) performs proportional feedback of a 4D DCM of the same model [Caron20].

class vhip_stabilization.BonusPolePlacementStabilizer(pendulum, k_z)

This is a “bonus” stabilizer, not reported in the paper, that was an intermediate step in our derivation of the VHIPQPStabilizer.

Parameters: pendulum (pymanoid.models.InvertedPendulum) – Inverted pendulum to stabilize. k_z (scalar) – Feedback gain between DCM altitude and normalized leg stiffness input.

Notes

This stabilizer also performs pole placement on a 4D DCM (using a velocity rather than position DCM though), but contrary to VHIPQPStabilizer it doesn’t force the closed-loop matrix to be diagonal. We started out exploring this stabilizer first.

The first thing to observe by direct pole placement is that the gain matrix has essentially four non-zero gains in general. You can try out the set_poles() function to verify this.

The closed-loop system with four gains has structure: in the horizontal plane it is equivalent to the VRPStabilizer, and the normalized leg stiffness lambda depends on both the vertical DCM and the natural frequency omega. We observed that this system performs identically to the previous one in the horizontal plane, and always worse than the previous one vertically.

However, raising the k_z (vertical DCM to lambda) gain to large values, we noticed that the vertical tracking of this stabilizer converged to that of the VRPStabilizer. In the limit where k_z goes to infinity, the system slides on the constraint given by Equation (21) in the paper. This is how we came to the derivation of the VHIPQPStabilizer.

compute_compensation()

Compute CoP and normalized leg stiffness compensation.

set_critical_gains(k_z)

Set critical gain k_omega for a desired vertical DCM gain k_z.

Parameters: k_z (scalar) – Desired vertical DCM to normalized leg stiffness gain.
set_gains(gains)

Set gains from 4D DCM error to 3D input [zmp_x, zmp_y, lambda].

Parameters: gains ((4,) array) – List of gains [k_x, k_y, k_z, k_omega].
set_poles(poles)

Place poles using SciPy’s implementation of Kautsky et al.’s algorithm.

Parameters: poles ((4,) array) – Desired poles of the closed-loop system.
class vhip_stabilization.Pusher(pendulums, gain=0.1)

Send impulses to the inverted pendulum every once in a while.

Parameters: pendulums (list of pymanoid.models.InvertedPendulum) – Inverted pendulums to de-stabilize. gain (scalar) – Magnitude of velocity jumps.

Notes

You know, I’ve seen a lot of people walkin’ ‘round // With tombstones in their eyes // But the pusher don’t care // Ah, if you live or if you die

on_tick(sim)

Apply regular impulses to the inverted pendulum.

Parameters: sim (pymanoid.Simulation) – Simulation instance.
class vhip_stabilization.Stabilizer(pendulum)

Base class for stabilizer processes.

Parameters: pendulum (pymanoid.models.InvertedPendulum) – Inverted pendulum to stabilize.
contact

pymanoid.Contact

Contact frame and area dimensions.

dcm

(3,) array

Position of the DCM in the world frame.

omega

scalar

Instantaneous natural frequency of the pendulum.

pendulum

pymanoid.InvertedPendulum

Measured state of the reduced model.

ref_com

(3,) array

Desired center of mass (CoM) position.

ref_comd

(3,) array

Desired CoM velocity.

ref_cop

(3,) array

Desired center of pressure (CoP).

ref_lambda

scalar

Desired normalized leg stiffness.

ref_omega

scalar

Desired natural frequency.

on_tick(sim)

Set inverted pendulum CoP and stiffness inputs.

Parameters: sim (pymanoid.Simulation) – Simulation instance.
reset_pendulum()

Reset inverted pendulum to its reference state.

class vhip_stabilization.VHIPQPStabilizer(pendulum)

Stabilizer based on proportional feedback of the 4D divergent component of motion of the variable-height inverted pendulum (VHIP).

Parameters: pendulum (pymanoid.models.InvertedPendulum) – Inverted pendulum to stabilize.

Notes

This implementation transcripts QP matrices from VHIPStabilizer. We checked that the two produce the same outputs before switching to C++ in <https://github.com/stephane-caron/vhip_walking_controller/>. (This step would not have been necessary if we had a modeling language for convex optimization directly in C++.)

compute_compensation()

Compute CoP and normalized leg stiffness compensation.

class vhip_stabilization.VHIPStabilizer(pendulum)

Stabilizer based on proportional feedback of the 4D divergent component of motion of the variable-height inverted pendulum (VHIP).

Parameters: pendulum (pymanoid.models.InvertedPendulum) – Inverted pendulum to stabilize.

Notes

This implementation uses CVXPY <https://www.cvxpy.org/>. Using this modeling language here allowed us to try various formulations of the controller before converging on this one. We can only praise the agility of this approach, as opposed to e.g. writing QP matrices directly.

See “Biped Stabilization by Linear Feedback of the Variable-Height Inverted Pendulum Model” (Caron, 2019) for detail on the controller itself.

compute_compensation()

Compute CoP and normalized leg stiffness compensation.

class vhip_stabilization.VRPStabilizer(pendulum)

Inverted pendulum stabilizer based on proportional feedback of the 3D divergent component of motion (DCM) applied to the virtual repellent point (VRP).

Parameters: pendulum (pymanoid.models.InvertedPendulum) – Inverted pendulum to stabilize.
ref_dcm

(3,) array

Desired (3D) divergent component of motion.

ref_vrp

(3,) array

Desired virtual repellent point (VRP).

Notes

See “Three-Dimensional Bipedal Walking Control Based on Divergent Component of Motion” (Englsberger et al., IEEE Transactions on Robotics) for details.

compute_compensation()

Compute CoP and normalized leg stiffness compensation.

vhip_stabilization.push_three_times()

Apply three pushes of increasing magnitude to the CoM.

Note

This is the function used to generate Fig. 1 in the manuscript <https://hal.archives-ouvertes.fr/hal-02289919v1/document>.

vhip_stabilization.record_video()

Record accompanying video of the paper.