Contact stability¶

Contact¶

class pymanoid.contact.Contact(shape, pos=None, rpy=None, pose=None, friction=None, 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.
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.

wrench¶

Contact wrench coordinates at the contact point in the inertial frame.

Type: array, shape=(6,)

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.
Returns: G – Grasp matrix \(G_P\).
Return type: ndarray

copy(link=None, hide=False)¶

Return a copy of the contact.

Parameters

link (body.Manipulator, optional) – Robot link frame in contact in the copy.
hide (bool, optional) – Hide copy?

property force¶: Resultant of contact forces in the world frame (if defined).

property 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 for instance this introduction to friction cones for details.

property 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 for instance this introduction to friction cones for details.

property 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 for instance this introduction to friction cones for details.

get_scaled_contact_area(scale)¶

Get the vertices of the scaled contact area.

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

property moment¶: Moment of contact forces in the world frame (if defined).

set_wrench(wrench)¶

Set contact wrench directly.

Parameters: wrench (array, shape=(6,)) – Wrench coordinates given in the contact frame.

Notes

This function switches the contact to “managed” mode, as opposed to the default “supporting” mode where the wrench distributor finds contact wrenches by numerical optimization.

unset_wrench()¶: Return contact to supporting mode.

property vertices¶: Vertices of the contact area.

wrench_at(point)¶

Get contact wrench at a given point in the world frame.

Parameters: point (array, shape=(3,)) – Point P where the wrench is expressed.
Returns: wrench – Contact wrench \(w_P\) at P in the world frame.
Return type: array, shape=(6,)

property 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.

property 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.

property wrench_rays¶: Rays (V-rep) of the contact wrench cone in world frame.

property 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.
Return type: 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.
Returns: 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).
Return type: 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’.
Returns: vertices – 2D vertices of the static-equilibrium polygon.
Return type: 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.
Returns: F – Friction matrix such that all valid contact wrenches satisfy \(F w \leq 0\), where w is the resultant contact wrench at p.
Return type: 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.
Returns: S – Span matrix of the net contact wrench cone.
Return type: 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. Options are ‘quadprog’ (default) or ‘cvxopt’. The latter is slower but more numerically stable if your resulting wrenches are extremal.

Returns

support – Mapping between each contact i and a supporting contact wrench \(w^i_{C_i}\). Contact wrenches satisfy friction constraints and sum up to the net wrench: \(\sum_c w^i_P = w_P\).

Return type

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.

property supporting_contacts¶: Set of supporting contacts, i.e. excluding managed contacts where the user provides the external wrench.

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)
robot = robots.JVRC1(download_if_needed=True)
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 the world frame rooted at their respective contact points.

class pymanoid.stance.StanceWrenchDistributor(stance)¶

Wrench distribution process.

Parameters: stance (Stance) – Stance to distribute wrenches from.

Notes

This process computes wrenches for supporting contacts and stores them in each contact, as well as in the robot’s manipulators. For instance, you will be able to access robot.left_foot.wrench or robot.stance.left_foot.wrench equivalently. Note that supporting wrenches are given in the world frame rooted at their respective contacts.

on_tick(sim)¶

Main function called by the simulation at each control cycle.

Parameters: sim (Simulation) – Current simulation instance.