Problem 360393 · medium · Robotics · Level 03 Reaching and Sensing

Reaching From a Parked Robot

inverse kinematics · coordinate frames · rotation · mobile manipulator

A hospital delivery robot parks next to a bedside table and uses its small arm to pick up a cup. The map gives the cup's position in the world frame (x right, y up); the arm needs joint angles.

The robot's pose is pose = (X, Y, theta): its centre is at (X, Y) and it faces the direction theta (radians, counter-clockwise from the world's +x axis). In the robot's frame, x points forward (the way the robot faces) and y to the robot's left. The arm's shoulder is bolted at mount = (mx, my) in the robot's frame.

The arm is a planar two-link arm with links l1 and l2. Its joint angles (t1, t2) are in radians, counter-clockwise and relative: t1 is measured from the robot's forward direction, t2 from the direction of the first link.

Seen from the shoulder, in the robot's frame, let the target be at distance r. The solutions are:

  • none if r is more than 10^{-9} outside the range from abs(l1 - l2) to l1 + l2;
  • one, t2 = 0.0, if r is within 10^{-9} of l1 + l2, or one, t2 = math.pi, if within 10^{-9} of abs(l1 - l2);
  • otherwise two: elbow-up (t2 < 0) first, then elbow-down (t2 > 0).

t1 is wrapped into (-\pi, \pi]. The target is never exactly at the shoulder.

Write ik_from_base(pose, mount, l1, l2, target) that returns the list of solutions (t1, t2) for the world-frame point target = (tx, ty).

Examples

Input:  pose = (0, 0, 0), mount = (0, 0), l1 = 1, l2 = 1, target = (1, 1)
Output: [(1.57079633, -1.57079633), (0.0, 1.57079633)]
Explanation: the robot's frame is the world frame here.

Input:  pose = (2, 1, math.pi / 2), mount = (0.3, 0.1), l1 = 0.4, l2 = 0.3, target = (2.2, 1.7)
Output: [(0.0, -1.57079633), (-1.28700222, 1.57079633)]
Explanation: the robot faces world +y. The cup is 0.7 ahead of the robot's centre
and 0.2 to its right, so (0.7, -0.2) in the robot's frame, and (0.4, -0.3) from the
shoulder. Elbow-up: first link straight ahead, then a right turn.

Input:  pose = (3, -2, -math.pi / 2), mount = (0.2, 0), l1 = 0.5, l2 = 0.5, target = (3, -4)
Output: []
Explanation: the robot faces world -y; the cup is 2 ahead, 1.8 from the shoulder.

Constraints

  • answers are compared with a tolerance of 1e-6; do not round

Goals

  • Bring a world-frame target into the robot's frame: shift, then rotate back
  • Measure the target from the arm's mount before solving
  • Reuse two-link inverse kinematics unchanged once the frames are right
Starting Python…