Problem 625606 · hard · Robotics · Level 06 Estimation and Autonomy

Two Fingers, One Pebble

grasping · friction cone · antipodal grasp · polygon orientation · normals · vectors

A sorting robot picks flat pebbles off a conveyor with a two-finger gripper. Seen from above, a pebble is a convex polygon polygon (a list of vertices (x, y) in centimetres, listed either counter-clockwise or clockwise). The vision system proposes contact points contacts: each is a pair (e, t) with 0 < t < 1, the point V_e + t\,(V_{e+1} - V_e) on edge e, which runs from vertex e to vertex e + 1 (the last edge runs from the last vertex back to vertex 0).

At a contact on edge e, the inward normal is the unit vector perpendicular to the edge that points into the pebble. With friction coefficient mu, a finger can push in any direction within the angle \arctan(\mu) of the inward normal: the contact's friction cone.

Two contacts i < j form an antipodal grasp when:

  • they are at different points, no farther apart than max_width (the gripper's opening), and
  • the direction from contact i to contact j lies in contact i's friction cone, and the direction from j to i lies in j's cone: each angle to the inward normal is at most \arctan(\mu) (allow 1e-6 radians of rounding).

Its margin is \arctan(\mu) minus the larger of those two angles: how much slack it has before slipping.

Write antipodal_grasps(polygon, contacts, mu, max_width) that returns (pairs, best): the list of all grasp pairs (i, j) in increasing order, and the pair with the largest margin (None if there are none). When margins are within 1e-6 of each other, the pair that comes first in pairs wins.

Examples

Input:  polygon = [(0, 0), (2, 0), (2, 2), (0, 2)], contacts = [(0, 0.5), (2, 0.5), (1, 0.25), (3, 0.5)],
        mu = 0.3, max_width = 3
Output: ([(0, 1), (2, 3)], (0, 1))
Explanation: the contacts are (1, 0), (1, 2), (2, 0.5) and (0, 1). Contacts 0 and 1 face
each other exactly: angles 0. The line from (2, 0.5) to (0, 1) is atan(0.25) = 0.245 rad
off both normals, inside arctan(0.3) = 0.291.

Input:  polygon = [(0, 0), (2, 0), (2, 2), (0, 2)], contacts = [(0, 0.5), (2, 0.5), (1, 0.25), (3, 0.5)],
        mu = 0.3, max_width = 2.05
Output: ([(0, 1)], (0, 1))
Explanation: contacts 2 and 3 are sqrt(4.25) = 2.06 cm apart, wider than the gripper opens.

Input:  polygon = [(0, 0), (1, 1.7320508075688772), (2, 0)], contacts = [(0, 0.5), (1, 0.5), (2, 0.5), (0, 0.2), (1, 0.7)],
        mu = 0.7, max_width = 5
Output: ([(0, 1), (0, 2), (1, 2)], (0, 1))
Explanation: a clockwise equilateral triangle. The midpoints of two edges are 30 degrees
(0.524 rad) off both normals, inside arctan(0.7) = 0.611; the three pairs tie on margin.
With mu = 0.5 (arctan = 0.464) there is no grasp at all.

Constraints

  • answers are compared with a tolerance of 1e-6

Goals

  • Find the inward normal of a polygon's edge whichever way round the vertices are listed
  • Test whether the line between two contacts lies inside both friction cones
  • Rank feasible grasps by their margin and break near-ties in a stated way
Starting Python…