Straight-line paths and trajectories

A trajectory is a path for a robot hand together with when it is at each point along it. A robot that jumps straight to a target would throw the load it carries, so it follows a planned path in many small steps.

In this lesson you plan a straight line by interpolation and use a speed limit to decide how many steps it takes.

To move from a start to an end, split the move into equal steps. At step k of n, you are a fraction k divided by n of the way there.

Each coordinate is start plus fraction times (end minus start). Do that for x and for y. The points fall on a straight line, equally spaced.

docker exec jupyter python3 -c "import numpy as np; print(np.linspace([0, 0], [4, 2], 5))"

The Run above prints five rows, the points of the table: (0, 0), (1, 0.5), (2, 1), (3, 1.5) and (4, 2). np.linspace does the interpolation, and the 5 is the number of points, not steps.

Plain Python needs a loop, as in the next block, and gives the same points.

start = [0, 0]
end = [4, 2]
steps = 4

for step in range(steps + 1):
    fraction = step / steps
    x = start[0] + (end[0] - start[0]) * fraction
    y = start[1] + (end[1] - start[1]) * fraction
    print("step", step, "point", [x, y])
# Change steps to 8 and press Run.
# There are more points, and each one is closer to the last.

The length of a path is the sum of the distances between neighbouring points. Each gap is a Pythagoras sum on the change in x and the change in y.

For a straight path all gaps are equal, so the length is just the distance from start to end. For (0, 0) to (4, 2) that is the square root of 20, about 4.472.

The hand may move at most a maximum speed. If each step lasts dt seconds, one step may cover at most max speed times dt.

Divide the distance by that and round up. For 10 units at 3 units a second with 0.5 second steps, 10 divided by 1.5 is 6.67, so 7 steps.

import math

distance = 10
max_speed = 3
dt = 0.5

steps = math.ceil(distance / (max_speed * dt))
per_step = distance / steps
print("steps:", steps)
print("distance each step:", round(per_step, 3))
print("speed:", round(per_step / dt, 3))
# Change max_speed to 5. How many steps now, and is the speed below the limit?

In start.ipynb, plan a path from (3, 0) to (0, 3) in 6 steps. Feed each point through the inverse kinematics from lesson 3 for links 3 and 2.

Print the angles of each point. Do the joint angles change evenly, even though the hand moves evenly?