A greenhouse robot follows a row of plants by pure pursuit: it picks a point on its path some way ahead, the "carrot", and drives along the circular arc that starts at its own position, leaves in the direction it faces, and passes through the carrot. Then it picks a new carrot and does it again.
The robot's pose is pose = (x, y, theta) in metres and radians (world frame: x to the right, y up, theta counter-clockwise from +x). The carrot is the world point point = (px, py). In the robot's frame (x forward, y to the robot's left) the carrot sits at (x_r, y_r), at distance d from the robot. The arc through it has curvature
(one over the turning radius; positive for a left turn, negative for a right turn, 0 when the carrot is straight ahead or straight behind).
The robot has a differential drive: two wheels b metres apart. To drive the arc at forward speed v (m/s) it turns at \omega = v\kappa rad/s, and the wheels run at
Write pursuit_wheels(pose, point, v, b) that returns the tuple (kappa, v_left, v_right).
Examples
Input: pose = (0, 0, 0), point = (2, 1), v = 0.5, b = 0.4
Output: (0.4, 0.46, 0.54)
Explanation: facing +x, the carrot is 2 m ahead and 1 m to the left: kappa = 2*1/5.
omega = 0.5*0.4 = 0.2 rad/s, and each wheel differs from v by 0.2*0.4/2 = 0.04.
Input: pose = (1, 2, math.pi / 2), point = (0, 3), v = 1.0, b = 0.5
Output: (1.0, 0.75, 1.25)
Explanation: facing +y, the carrot is 1 m ahead and 1 m to the left (x_r = 1, y_r = 1).
Input: pose = (4, 1, 0.5), point = (6, 0), v = 0.8, b = 0.3
Output: (-0.7345734556395115, 0.8881488146767414, 0.7118511853232586)
Explanation: the carrot lies to the robot's right (y_r = -1.8364, d^2 = 5), so the
curvature is negative and the left wheel runs faster.
Constraints
- answers are compared with a tolerance of
1e-6
Goals
- Express a target point in the robot's own frame (x forward, y to the left)
- Find the curvature $\kappa = 2y/d^2$ of the arc that passes through the point
- Turn the arc into left and right wheel speeds for a differential drive