Answers

U2.6 Go to a point

Kinematics and frames · University · about 25 min

What this lesson is about

The first useful controller: a vector to the target, straight through the inverse kinematics.

• What is going on
• Where the position came from
• Stopping

Questions 6 marks in all

1. [1 mark]The go-to-point controller sets its speed from the gap to the target. What does this print?

   for gap in (60.0, 28.0, 10.0, 3.0):
       speed = min(14.0, max(4.0, 0.5 * gap))
       print(gap, speed, "stop" if gap < 4 else "drive")

   Answer:

   60.0 14.0 drive
   28.0 14.0 drive
   10.0 5.0 drive
   3.0 4.0 stop

   Far away 0.5 × gap exceeds the ceiling, so speed is 14. Closer in it eases off in proportion, 5.0 at 10 cm, but never below the floor of 4 cm/s, which keeps the command out of the drive's dead band. Inside 4 cm the loop stops.

2. [1 mark]The target is 30 cm east and 40 cm north of the robot. What does this print?

   import math
   dx, dy = 30.0, 40.0
   gap = math.hypot(dx, dy)
   speed = min(14.0, max(4.0, 0.5 * gap))
   wx, wy = speed * dx / gap, speed * dy / gap
   print(gap, speed, round(wx, 1), round(wy, 1))

   Answer:

   50.0 14.0 8.4 11.2

   The gap is 50 cm, so speed is capped at 14. The unit vector is (0.6, 0.8), giving a world velocity of (8.4, 11.2), which is 14 cm/s towards the target.

3. [1 mark]A student writes wx, wy = speed * dx, speed * dy, leaving out the division by gap. What goes wrong?

   1. AThe effective gain depends on how far away the target is, so the robot behaves differently on long and short runs
   2. BThe robot heads in the wrong direction
   3. CNothing, because speed is already capped at 14
   4. DThe robot can never get within 4 cm, because the velocity is always zero
   Answer: A. Dividing by gap makes a unit vector, direction without magnitude. Without it the magnitude is speed × gap, so the direction is right but the size grows with distance.

4. [1 mark]How should the stopping threshold, gap < 4, be chosen?

   1. ALarger than the distance the robot coasts from the speed it arrives at, or it sails past and hunts
   2. BAs small as possible, since a smaller threshold is always more accurate
   3. CEqual to the robot's radius
   4. DSmaller than the noise on position(), so noise cannot trigger a stop
   Answer: A. If the robot is still moving fast enough to coast further than the threshold, it overshoots, turns round and comes back. Measure the coast, then pick the number, or ramp the speed down.

5. [1 mark]The robot starts at world (60, 60), and position() counts from where it started. How far is a target at world (100, 45) from the start, in cm to one decimal place?

   Answer: 42.7 (accept within 0.05). Relative to the start the target is at (40, -15), and the square root of 40² + 15² is 42.7 cm.

6. [1 mark]This controller reads position(), which U1.3 called the lab's overhead camera. Why is that acceptable here?

   1. AThe subject is kinematics for now, and from U6 an estimate the robot works out for itself replaces it under the same loop
   2. Bposition() is what a real robot would use for this controller
   3. CThe inverse kinematics only works with a perfect position
   4. Dodometry() is in the body frame, so it cannot be used with world targets
   Answer: A. The loop is written cleanly now so that later estimators plug straight into it.

The task: go to a point

from bugbot import *
import math
connect()

V_MAX, V_LAT = 20.0, 15.0
TX, TY = 80.0, 90.0        # the target, in cm from the start

from bugbot import *
import math
connect()

V_MAX, V_LAT = 20.0, 15.0
TX, TY = 80.0, 90.0        # the target, in cm from where the robot started

for tick in range(400):
    x, y = position()
    dx, dy = TX - x, TY - y
    gap = math.hypot(dx, dy)
    if gap < 4:
        break
    speed = min(14.0, max(4.0, 0.5 * gap))
    wx, wy = speed * dx / gap, speed * dy / gap
    a = math.radians(heading())
    bx = wx * math.cos(a) - wy * math.sin(a)
    by = wx * math.sin(a) + wy * math.cos(a)
    drive(100 * by / V_MAX, 100 * bx / V_LAT, 0)
    wait(0.1)
stop()