A warehouse robot steers with a potential field: the goal pulls it and obstacles push it away, and it moves in the direction of the total force. The robot is a point p = (x, y); positions are in metres.
- Attraction. The goal
goal = (gx, gy)pulls with the force k_{att}\,(\text{goal} - p): stronger the farther away it is. - Repulsion. Each obstacle is a round bin
(ox, oy, r): centre and radius. Let \rho be the distance frompto the bin's centre and d = \rho - r the distance to its surface. If d < d_0 (the influence distance), the bin pushespstraight away from its centre with magnitudek_{rep}\left(\frac{1}{d} - \frac{1}{d_0}\right)\frac{1}{d^2}so the push is zero at the edge of its influence and grows without bound near the surface. A bin with d \ge d_0 does not push at all.
The total force is the attraction plus every push. Write field_force(p, goal, obstacles, k_att, k_rep, d0) that returns it as a tuple (fx, fy).
Examples
Input: p = (1, 2), goal = (4, 6), obstacles = [], k_att = 0.5, k_rep = 1, d0 = 1
Output: (1.5, 2.0)
Explanation: no bins, only the pull: 0.5 * (3, 4).
Input: p = (0, 0), goal = (5, 0), obstacles = [(0, 2, 1)], k_att = 1, k_rep = 2, d0 = 2
Output: (5.0, -1.0)
Explanation: the bin's surface is d = 2 - 1 = 1 away, inside d0 = 2. The push has
magnitude 2 * (1/1 - 1/2) / 1 = 1, pointing from the centre (0, 2) to p: straight down.
Input: p = (2, 1), goal = (6, 4), obstacles = [(3, 3, 1), (8, 1, 1)], k_att = 0.4, k_rep = 1, d0 = 1.5
Output: (1.5583333333333333, 1.116666666666667)
Explanation: the pull is (1.6, 1.2). The first bin is d = sqrt(5) - 1 = 1.2361 away and
pushes with magnitude 0.09317 along (-1, -2) / sqrt(5), adding (-0.04167, -0.08333).
The second bin is d = 5 away, beyond its influence.
Constraints
pis outside every bin (d > 0); answers are compared with a tolerance of1e-6
Goals
- Add an attractive pull toward the goal and a repulsive push from each nearby obstacle
- Apply a repulsive term only inside its influence distance, measured from the obstacle's surface