A paint-spraying arm has cables running through its joints, so each joint can only turn between two mechanical stops. The arm is planar, with n links: 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 one from the direction of the link before it. The base is at the origin; x points right and y up.
limits[i] = (lo, hi) are joint i's stops. A configuration q is allowed when lo <= q[i] <= hi for every joint; an angle exactly on a stop is allowed. Compare the angles as they are given, with no wrapping: 3.5 is outside (-3, 3) even though it points the same way as 3.5 - 2π.
The planner proposes several configurations configs. Write allowed_hands(lengths, limits, configs) that returns a list with one tuple (index, x, y) for each allowed configuration, in the order of configs: its position in configs and where the hand (the end of the last link) would be.
Examples
Input: lengths = [1, 1], limits = [(-1.5, 1.5), (0, 2.5)],
configs = [[0, 0], [0.5, -0.3], [1.5, 2.5], [2.0, 1.0]]
Output: [(0, 2.0, 0.0), (2, -0.58290642, 0.24069249)]
Explanation: configuration 1 bends the elbow to -0.3, below its stop at 0;
configuration 3 turns the shoulder to 2.0, past its stop at 1.5.
Configurations 0 and 2 sit exactly on stops, which is allowed.
Input: lengths = [0.6, 0.4, 0.2], limits = [(-1, 1), (-1, 1), (-1, 1)],
configs = [[0.3, 1.2, -0.8], [-0.5, 0.9, 1.0]]
Output: [(1, 0.92896736, 0.06520196)]
Constraints
- answers are compared with a tolerance of
1e-6; do not round
Goals
- Check every joint angle against its own allowed interval
- Compute the hand position only for the configurations that pass
- Keep the configurations' original indices in the answer