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

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 contact frame (if defined).

vertices
¶ Vertices of the contact area.

world_wrench
¶ Contact wrench in the world (inertial) frame.

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. Default is ‘quadprog’ (fastest). If you are planning on using extremal wrenches, ‘cvxopt’ is slower but works better on corner cases.
Returns: 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`\).
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.

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 their respective contact frames,
not in the world frame.

class
pymanoid.stance.
StanceWrenchDistributor
(stance)¶