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 (halflength, halfwidth) in [m].
pos (ndarray) – Contact position in world frame.
rpy (ndarray) – Contact orientation in world frame.
pose (ndarray) – Initial pose. Supersedes
pos
andrpy
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
¶ Hrepresentation of friction inequalities (and optional pressure limits) in world frame.
This matrixvector 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 (Vrep) 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 4sided 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 staticequilibrium polygon of the center of mass.
 Parameters
method (string, optional) – Choice between ‘bretl’, ‘cdd’ or ‘hull’.
 Returns
vertices – 2D vertices of the staticequilibrium polygon.
 Return type
list of arrays
Notes
The method ‘bretl’ is adapted from in [Bretl08] where the staticequilibrium polygon was introduced. The method ‘cdd’ corresponds to the doubledescription 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 resultantwrench 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 contactwrench 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
orrobot.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.