A warehouse team is testing a new inverse-kinematics solver for its picking arm. Rather than trust it, they check every answer it gives the cheap way: put the joint angles into the arm's forward kinematics and see whether the hand really lands on the target.
The arm is planar with n links, its base at the origin (x right, y up). Link i has length lengths[i] and joint angle q[i] (radians, counter-clockwise). The angles are relative: q[0] is measured from the +x axis, every later angle from the direction of the link before it. The hand is the end of the last link.
Write good_answers(lengths, target, answers, tol) that returns the list of indices i (in increasing order) for which the hand of configuration answers[i] lies within distance tol of target = (x, y). A hand exactly tol away passes.
Examples
Input: lengths = [1, 1], target = (1, 1), tol = 0.01,
answers = [[math.pi / 2, -math.pi / 2], [0, math.pi / 2], [math.pi / 2, 0], [0.02, 1.55]]
Output: [0, 1]
Explanation: the first two put the hand on (1, 1) (elbow at (0, 1), or at (1, 0)).
[pi/2, 0] would be right if the angles were absolute; as relative angles they
point both links straight up, to (0, 2). The last misses by 0.020.
Input: lengths = [0.5, 0.4, 0.2], target = (0.6, 0.5), tol = 0.001,
answers = [[1.5708, -1.5708, 0], [0.2213, 1.5708, -1.7921], [1.5708, -1.5708, 0.1]]
Output: [0, 1]
Explanation: the first two are exact answers rounded to 4 decimals; they miss by
less than 0.00001. The last one turns the wrist and misses by 0.020.
Constraints
- compute the distances in floating point; the test cases keep every miss well away from
tolunless it is exactlytol
Goals
- Check an inverse-kinematics answer by running forward kinematics on it
- Measure the miss as the distance between the hand and the target
- Accept answers within a tolerance, edge included