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
¶ array, shape=(6,)
Contact wrench coordinates at the contact point in the inertial frame.

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?

force
¶ Resultant of contact forces in the world 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 for instance this introduction to friction cones for details.

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.

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

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.

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,)

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.

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 (Vrep) 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. 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.

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.
