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.

inverse_kinematics.setup_ik_from_tasks()

Setup the inverse kinematics task by task.

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.

on_tick(sim)

Main function called by the simulation at each control cycle.

Parameters

sim (Simulation) – Current simulation instance.

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.

on_tick(sim)

Main function called by the simulation at each control cycle.

Parameters

sim (Simulation) – Current simulation instance.

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.COMSync(stance, com_above)
on_tick(sim)

Main function called by the simulation at each control cycle.

Parameters

sim (Simulation) – Current simulation instance.

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.

class zmp_support_area.SupportAreaDrawer(stance, height=0.0, color=None)

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.

on_tick(sim)

Main function called by the simulation at each control cycle.

Parameters

sim (Simulation) – Current simulation instance.

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.

on_tick(sim)

Main function called by the simulation at each control cycle.

Parameters

sim (Simulation) – Current simulation instance.

class com_accel_cone.COMSync(stance, com_above)
on_tick(sim)

Main function called by the simulation at each control cycle.

Parameters

sim (Simulation) – Current simulation instance.

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.

CoM robust static-equilibrium polytope

This example computes the robust static-equilibrium CoM polyhedron. See [Audren18] for details. Running this example requires the StabiliPy library.

class com_robust_static_polytope.StaticWrenchDrawer(stance)

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

Parameters

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

class com_robust_static_polytope.SupportPolyhedronDrawer(stance, z=0.0, color=None, method='qhull')

Draw the robust static-equilibrium polyhedron of a contact set.

Parameters

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

  • color (tuple or string, optional) – Area color.

  • method (string, optional) – Method to compute the static equilibrium polygon. Choices are cdd, qhull (default) and parma

on_tick(sim)

Main function called by the simulation at each control cycle.

Parameters

sim (Simulation) – Current simulation instance.

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.

horizontal_walking.generate_footsteps(distance, step_length, foot_spread, friction)

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.PointMassWrenchDrawer(point_mass, contact_set)
on_tick(sim)

Find supporting contact forces at each COM acceleration update.

Parameters

sim (pymanoid.Simulation) – Simulation instance.

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.

Return type

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.

class multi_contact_walking.UpdateCOMTargetAccel(com_target, preview_buffer)
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(size=None)

Return random floats in the half-open interval [0.0, 1.0). Alias for random_sample to ease forward-porting to the new random API.

multi_contact_walking.seed(self, seed=None)

Reseed a legacy MT19937 BitGenerator

Notes

This is a convenience, legacy function.

The best practice is to not reseed a BitGenerator, rather to recreate a new one. This method is here for legacy reasons. This example demonstrates best practice.

>>> from numpy.random import MT19937
>>> from numpy.random import RandomState, SeedSequence
>>> rs = RandomState(MT19937(SeedSequence(123456789)))
# Later, you want to restart the stream
>>> rs = RandomState(MT19937(SeedSequence(987654321)))

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.DCMPlotter(stabilizers)
on_tick(sim)

Main function called by the simulation at each control cycle.

Parameters

sim (Simulation) – Current simulation instance.

class vhip_stabilization.Plotter(stabilizers)
on_tick(sim)

Main function called by the simulation at each control cycle.

Parameters

sim (Simulation) – Current simulation instance.

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

Contact frame and area dimensions.

Type

pymanoid.Contact

dcm

Position of the DCM in the world frame.

Type

(3,) array

omega

Instantaneous natural frequency of the pendulum.

Type

scalar

pendulum

Measured state of the reduced model.

Type

pymanoid.InvertedPendulum

ref_com

Desired center of mass (CoM) position.

Type

(3,) array

ref_comd

Desired CoM velocity.

Type

(3,) array

ref_cop

Desired center of pressure (CoP).

Type

(3,) array

ref_lambda

Desired normalized leg stiffness.

Type

scalar

ref_omega

Desired natural frequency.

Type

scalar

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

Desired (3D) divergent component of motion.

Type

(3,) array

ref_vrp

Desired virtual repellent point (VRP).

Type

(3,) array

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.