# Contact stability¶

## Contact¶

class pymanoid.contact.Contact(shape, pos=None, rpy=None, pose=None, friction=None, color='r', link=None, slab_thickness=0.01)

Rectangular contact surface.

Parameters: shape ((scalar, scalar)) – Surface dimensions (half-length, half-width) in [m]. pos (ndarray) – Contact position in world frame. rpy (ndarray) – Contact orientation in world frame. pose (ndarray) – Initial pose. Supersedes pos and rpy if they are provided at the same time. friction (scalar) – Static friction coefficient. color (char, optional) – Color code in matplotlib convention (‘b’ for blue, ‘g’ for green, ...). link (body.Manipulator, optional) – Robot link frame in contact. slab_thickness (scalar, optional) – Thickness of the contact slab displayed in the GUI, in [m]. max_pressure (scalar, optional) – Maximum pressure on contact.
compute_grasp_matrix(p)

Compute the grasp matrix for a given destination point.

The grasp matrix $$G_P$$ converts the local contact wrench w to the contact wrench $$w_P$$ at another point P:

$w_P = G_P w$

All wrench coordinates being taken in the world frame.

Parameters: p (array, shape=(3,)) – Point, in world frame coordinates, where the wrench is taken. G – Grasp matrix $$G_P$$. ndarray
copy(color=None, link=None, hide=False)

Return a copy of the contact.

Parameters: color (char, optional) – Color code in matplotlib convention (e.g. ‘b’ for blue). link (body.Manipulator, optional) – Robot link frame in contact in the copy. hide (bool, optional) – Hide copy?
force

Resultant of contact forces in the contact frame (if defined).

force_inequalities

Matrix of force friction cone inequalities in the world frame.

Notes

All linearized friction cones in pymanoid use the inner (conservative) approximation. See <https://scaron.info/teaching/friction-cones.html>.

force_rays

Rays of the force friction cone in the world frame.

Notes

All linearized friction cones in pymanoid use the inner (conservative) approximation. See <https://scaron.info/teaching/friction-cones.html>.

force_span

Span matrix of the force friction cone in world frame.

This matrix S is such that all valid contact forces can be written:

$f = S \lambda, \quad \lambda \geq 0$

Notes

All linearized friction cones in pymanoid use the inner (conservative) approximation. See <https://scaron.info/teaching/friction-cones.html>.

get_scaled_contact_area(scale)

Get the vertices of the scaled contact area.

Parameters: scale (scalar) – Contact area is scaled by this ratio. vertices – List of vertex coordinates in the world frame. list of arrays
moment

Moment of contact forces in the contact frame (if defined).

vertices

Vertices of the contact area.

world_wrench

Contact wrench in the world (inertial) frame.

wrench_hrep

H-representation of friction inequalities (and optional pressure limits) in world frame.

This matrix-vector pair describes the linearized Coulomb friction model (in the fixed contact mode) and pressure limits by:

$F w \leq g$

where w is the contact wrench at the contact point (self.p) in the world frame. See [Caron15] for the derivation of the formula for F.

wrench_inequalities

Matrix F of friction inequalities in world frame.

This matrix describes the linearized Coulomb friction model (in the fixed contact mode) by:

$F w \leq 0$

where w is the contact wrench at the contact point (self.p) in the world frame. See [Caron15] for the derivation of the formula for F.

wrench_rays

Rays (V-rep) of the contact wrench cone in world frame.

wrench_span

Span matrix of the contact wrench cone in world frame.

This matrix is such that all valid contact wrenches can be written as:

$w_P = S \lambda, \quad \lambda \geq 0$

where S is the friction span and $$\lambda$$ is a vector with positive coordinates.

Returns: S – Span matrix of the contact wrench cone. array, shape=(6, 16)

Notes

Note that the contact wrench coordinates $$w_P$$ (“output” of S) are taken at the contact point P (self.p) and in the world frame. Meanwhile, the number of columns of S results from our choice of 4 contact points (one for each vertex of the rectangular area) with 4-sided friction pyramids at each.

## Multiple contacts¶

class pymanoid.contact.ContactSet(contacts=None)
compute_grasp_matrix(p)

Compute the grasp matrix of all contact wrenches at point p.

Parameters: p (array, shape=(3,)) – Point where the resultant wrench is taken at. G – Grasp matrix giving the resultant contact wrench $$w_P$$ of all contact wrenches as $$w_P = G w_{all}$$, with $$w_{all}$$ the stacked vector of contact wrenches (each wrench being taken at its respective contact point and in the world frame). array, shape=(6, m)
compute_static_equilibrium_polygon(method='hull')

Compute the static-equilibrium polygon of the center of mass.

Parameters: method (string, optional) – Choice between ‘bretl’, ‘cdd’ or ‘hull’. vertices – 2D vertices of the static-equilibrium polygon. list of arrays

Notes

The method ‘bretl’ is adapted from in [Bretl08] where the static-equilibrium polygon was introduced. The method ‘cdd’ corresponds to the double-description approach described in [Caron17z]. See the Appendix from [Caron16] for a performance comparison.

compute_wrench_inequalities(p)

Compute the matrix of wrench cone inequalities in the world frame.

Parameters: p (array, shape=(3,)) – Point where the resultant wrench is taken at. F – Friction matrix such that all valid contact wrenches satisfy $$F w \leq 0$$, where w is the resultant contact wrench at p. array, shape=(m, 6)
compute_wrench_span(p)

Compute the span matrix of the contact wrench cone in world frame.

Parameters: p (array, shape=(3,)) – Point where the resultant-wrench coordinates are taken. S – Span matrix of the net contact wrench cone. array, shape=(6, m)

Notes

The span matrix $$S_P$$ such that all valid contact wrenches can be written as:

$w_P = S_P \lambda, \quad \lambda \geq 0$

where $$w_P$$ denotes the contact-wrench coordinates at point P.

find_supporting_wrenches(wrench, point, friction_weight=0.01, cop_weight=1.0, yaw_weight=0.0001, solver='quadprog')

Find supporting contact wrenches for a given net contact wrench.

Parameters: wrench (array, shape=(6,)) – Resultant contact wrench $$w_P$$ to be realized. point (array, shape=(3,)) – Point P where the wrench is expressed. friction_weight (scalar, optional) – Weight on friction minimization. cop_weight (scalar, optional) – Weight on COP deviations from the center of the contact patch. solver (string, optional) – Name of the QP solver to use. Default is ‘quadprog’ (fastest). If you are planning on using extremal wrenches, ‘cvxopt’ is slower but works better on corner cases. support – Mapping between each contact i and a supporting contact wrench $$w^i_{C_i}$$. All contact wrenches satisfy friction constraints and sum up to the net wrench: $$\sum_c w^i_P = w_P$$. list of (Contact, array) pairs

Notes

Wrench coordinates are returned in their respective contact frames ($$w^i_{C_i}$$), not at the point P where the net wrench $$w_P$$ is given.

## Computing contact forces¶

Contact wrenches exerted on the robot while it moves can be computed by quadratic programming in the wrench distributor of a stance. This process is automatically created when binding a Stance to the robot model. It will only be executed if you schedule it to your simulation. Here is a small example:

from pymanoid import robots, Simulation, Stance

sim = Simulation(dt=0.03)
stance = Stance(
com=robot.get_com_point_mass(),
left_foot=robot.left_foot.get_contact(pos=[0, 0.3, 0]),
right_foot=robot.right_foot.get_contact(pos=[0, -0.3, 0]))
stance.com.set_z(0.8)
stance.bind(robot)
sim.schedule(robot.ik)
sim.schedule(robot.wrench_distributor)
sim.start()


You can see the computed wrenches in the GUI by scheduling the corresponding drawer process:

from pymanoid.gui import RobotWrenchDrawer

sim.set_viewer()
sim.schedule_extra(RobotWrenchDrawer(robot))


Once the wrench distributor is scheduled, it will store its outputs in the contacts of the stance as well as in the robot’s manipulators. Therefore, you can access robot.left_foot.wrench or robot.stance.left_foot.wrench equivalently. Note that wrenches are given in their respective contact frames, not in the world frame.

class pymanoid.stance.StanceWrenchDistributor`(stance)