A hospital delivery robot gets a command (v, omega) from its planner: move forward at v m/s while turning at omega rad/s (positive counter-clockwise). Its two wheels, of radius r metres and L metres apart, roll along the floor at
and neither motor can spin its wheel faster than phi_max radians per second, in either direction. A wheel spinning at \varphi rad/s rolls r\varphi m/s along the floor.
If a wheel would exceed its limit, the robot must not simply cut that wheel down: that changes the ratio of the wheel speeds, and the robot would swing off its planned curve. Instead it multiplies both v and omega by the same factor, the largest factor (at most 1) that brings the faster wheel down to exactly the limit. The path keeps its shape; the robot just drives it more slowly. A command within the limits is passed on unchanged.
Write speed_cap(v, omega, L, r, phi_max) that returns the command to send, (v, omega).
Examples
Input: v = 1.0, omega = 2.0, L = 0.4, r = 0.1, phi_max = 8
Output: (0.57142857, 1.14285714)
Explanation: the wheels would roll at 0.6 and 1.4 m/s, but the limit is 8 * 0.1 = 0.8 m/s.
Scaling by 0.8 / 1.4 keeps the turning radius v / omega = 0.5 m.
Input: v = 0.3, omega = 0.5, L = 0.4, r = 0.1, phi_max = 8
Output: (0.3, 0.5)
Input: v = 0.0, omega = -6.0, L = 0.5, r = 0.05, phi_max = 20
Output: (0.0, -4.0)
Explanation: a spin on the spot; each wheel would roll at 1.5 m/s, in opposite directions.
Constraints
- floats are compared with a tolerance of
1e-6
Goals
- Find the wheel speeds a command asks for and compare them with the motors' limit
- Scale a command so the robot keeps the same path, only slower
- See why clipping each wheel on its own bends the path