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

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