Trajectory Basics I: Foundations

Trajectories are inherent to physical movements. This is one of the most foundational topics regarding how human interacts with the world. Microscopically, the mouse sliding on your desktop, the fingertips wiggling on the keyboard, all form trajectories. Macroscopically, from the earliest days of manmade vehicles, trajectory tracking has been an on-going topic for centuries. Carriages driven by livestock are the original non-holonomic vehicles of the agricultural age, and as humanity strides into the industrial era, equestrian appeared as a finicky art of control that requires dynamic coordination and adaptive adjustment for both 3d-path generation and tracking. Later with the emergence of modern transportation vehicles, together with higher requirements of automation, trajectory generation and tracking, from its original form of an experience-based, empirical hand-eye coordination training, turns into a full-fledged billion-dollar industry. The question we are trying to address simply is:

How to traverse a path as fast as possible?

Up to now, we have used the words “path” and “trajectory” interchangeably. As the first step, we have to reach agreement on the differences between definitions of path and trajectory. There might be different conventions or semantic understandings, so let us define it in our context:

Path. An ordered sequence of n-dimensional points \(P = \{p_0, p_1, ..., p_{N-1} \vert \ p_i \in \mathbb{R}^n, i \in\mathbb{N} \}\) . The points could lie in 2d plane, 3d space, or a higher-dimensional space, representing a static state in that space.

Path is a topological structure that indicates how to go from one point to another through space. The most common type of space is the Cartesian space, where (x, y, z) coordinates define locations of the points. In practice this is the output from some path-finding algorithm, such as RRT, A*, etc. Another one is a 4d vector space that holds a set of quaternions, representing the rotations of the points. Beyond positional and orientational domains, the space also extends to other domains, such as velocity and force. In this series, we will be focusing on paths defined in the positional domain. Given the definition, the question above could be transcribed into:

How to go through a sequence of points as fast as possible?

Before we start the discussion, there is one clarification: the current position is always the starting point of the trajectory, so if there is an error between current tracker position and trajectory beginning point, it will reach there in the same controlled manner as it will traverse the rest. This sets the baseline for the trajectory tracking in the rest of this series.

Now, imagine there is a friendly match between two person, A and B. Suppose there is a set of cones, and one has to reach each cone and turn to the next one until the end. Two players, A and B are playing this game, and whoever reaches the end first wins. Person A, who is very bad at sports, is trying this for the first time. Person B, on the other hand, is a pro football player, and has played this a few times. It may not be a fair game, but as a case study, this scenario will be heavily referred to side-by-side when going through our tracking approaches.

A trivial tracker would move point to point in a straightforward fashion. Assume we have a perfect articulated robot, tracking a path \(P\) with its end effector. It has two non-blocking API’s for us: HasReached that checks if a state condition is met, and MoveTo that commands the end effector to given state. We also have a handy adaptive timer that adjust the length of sleep based on elapsed time, so our program is not entirely at the mercy of the OS scheduler. The implementation of the adaptive timer is an interesting topic by itself, which had also caused a Heisenbug, and will be covered in another article.

// helper function declarations
template <typename State>
virtual bool RobotInterface::HasReached(const State& s) = 0;

template <typename State>
virtual void RobotInterface::MoveTo(const State& s) = 0;

// path follower
template <typename State>
bool SimplePathFollow(const std::vector<State>& path, 
  const RobotInterface& robot) 
{
  AdaptiveTimer clk{control_frequency};
  clk.start();
  for (const auto& p : path) {
    while (!robot.HasReached(p)) {
      if (clk.elapsed_time() > kMaxFollowDurationS) {
        return false;
      }
      robot.MoveTo(p);
      clk.sleep();
    }
  }
  return true;
}

This naive approach turns the robot end effector purely to the disposal of its controller, without any kinematics or dynamics concerns. If the path points have sharp turns, cusps or zigzags, it may overshoot and/or impose huge pressure to the robot joints and easily damage the arm. This is analogous to the players running full speed towards the next cone on the path, making abrupt stops and turns, then run full speed again, which is particularly bad for their knees. Just like the players, if the tracker overshoot a point, it has to stumble back, causing additional traveled distance. The worst part is that the next point is sent only after reaching the current one; this is the nightmare of path following—we want a smooth and natural “pursuit” along the path, not a bumpy “stop-and-go” style bus trip. The original question now becomes:

How to go through a sequence of points as fast and as smooth as possible?

Let’s revisit the cone game. Both players want to improve their speed, but they take different approaches. Player B is very confident as a football player, and since he have practiced many times, he memorizes the approximate locations of those cones, and starts warming up. Player A has another strategy. As a beginner, he uses chalk to mark down the running route on the ground, and draws it with varying width, where thicker line implies faster speed. The route is not all straight lines between the cones, but with small curvature so that he doesn’t need to make abrupt turns. Player A plans to strictly stick to his plan during the game.

There are a lot to learn from them. B is at another level, and we don’t know what he is thinking yet. Player A, however, enlightens us with his plan: interpolating the path. The line with varying width he drew that connects the cones is called a trajectory.

Trajectory. A sequence of ordered states of dimension N-by-D, \(\sigma = \{s_0, ..., s_{N-1} \vert \ s_i \in \mathbb{R}^{N\times D}, i \in\mathbb{N} \}\) where \(N\) is the number of the parameters describing the state, \(D\) is the dimension of each parameter. Path is a special case of trajectory with \(N = 1\), positional parameter only

In Player A’s example, the varying width encoded with speed is an additional dimension. Simply speaking, a trajectory is a path with schedule, which is encoded in additional dimensions. Typically this information is about how to reach each state on the path, represented by derivatives, such as velocity \(v_i\), acceleration \(\alpha_i\), and/or higher order derivatives.

We can now formulate the answer to the question above in math language. First, let’s set up the notations: given a sequence of points, if there exists no kinematics and dynamics constraint, the ideal path \(P\) is the linear connections between each point. We use \(p_{i,\ i+1}\) to denote these oracle segments. We are trying to find a trajectory \(\sigma = \{\sigma_p, \sigma_v\}\) parameterized by the trajectory parameter \(u\), that goes through every point in \(P\); to mark the points \(p_i\) in parameter space \(\Theta\), we have to define \(\kappa: p_i \mapsto u\) that identifies the position of each path point in \(\Theta\), where \(\kappa(p_0) = 0, \kappa(p_{N-1}) = 1\). Every \(\kappa(p_i)\) represents the point’s position in the parameter space. We want to achieve

under the constraint \(\sigma_p(\kappa(p_i)) = p_i\), where \(\alpha\) is a coefficient to penalize the deviation from the original linear path. This equation captures the “fast” part of the tracking, and we define “smooth” as

which suggests the minimal continuity requirement of the trajectory, that velocities must be continuous. We can sketch the trajectory using the formulation above: go straight to somewhere near the next point, round in the corner to switch direction and move straight again. This is the same strategy player A took. We can define certain rules and parameters to generate a “hand-crafted” trajectory, but it is quite inefficient in tuning and susceptible to edge cases.

It turns out a tool in computer graphics, called spline, is especially useful for solving this problem. A spline is a function defined piecewise by polynomials, and in our case, each piece is a polynomial curve from \(p_i\) to \(p_{i+1}\). The pieces are connected by knots, which are path points. \(\kappa\) generates the knot vector that maps them into the parameter space. A cubic spline uses cubic polynomials, and ensures continuous second-order derivatives at the knots. It is a common choice for trajectory generation purpose. In the next article, we will discuss more on spline calculation, and help player A improve his route design. Now assume we have the trajectory generated, where we intuitively choose time as our trajectory parameter by setting \(u:=t\), so we can get the desired states \(\sigma(t_i) = p_i, v_i\) at any time stamp \(t_i\). We can write a time-based robot gripper trajectory tracker:

template <typename State>
bool TimeBasedPathFollow(const Trajectory<State>& trajectory, 
  const RobotInterface& robot, const double control_frequency) 
{ 
  AdaptiveTimer clk{control_frequency};
  clk.start();
  while (true) {
    const double elapsed = clk.elapsed_time();
    if (elapsed > kMaxFollowDurationS) {
      break;
    }
    // sending the next state based on current time
    robot.MoveTo(trajectory.GetState(elapsed));
    clk.sleep();
  }
  if (!robot.HasReached(trajectory.back()) {
    return false; 
  }
  return true;
}

The robot is mostly getting a smooth trajectory to follow, but note how we are not checking if the robot has arrived at each commanded state, because it is not important; at the next tick, it needs to move to the next point. If the duration is given properly and loop is running fast enough, the robot is always moving towards the next goal, displaying a “pursuit” behavior, ideal for the path following task. However, if the robot is missing a tick and fail to catch up the states, the next command will tell the robot to skip other points, and the errors may get accumulated and cause the robot to fail reaching the target. We have to carefully assign the velocity part for the trajectory, making it fast and reasonably achievable for the robot. A big challenge here is the dynamics of an articulated robot. A different path in Cartesian space implies different dynamics in joint (configuration) space. To get the bounds, we have to convert them from the joint jerk/acceleration limits of a specific robot’s control space. Furthermore, the control_frequency also plays a very nuanced role here, as it affects how long the program waits for the robot to achieve the commanded goal, which in turn affects the path following behavior. We will dig into these details later in this series.

Before introducing a better version of time-based tracker, let us revisit the thought path in this article: starting from a simple task to traverse points in order, we realized why the trivial method would not work, and used a pre-defined trajectory for help. Like Player A, once we define a trajectory that is smooth and fast for traversal, we command the robot to strictly follow it. However, we are not dealing with potential lagging and misclicks. In the next article, we will talk about the spline-based trajectory generation, and see how a better understanding on these fundamentals could help resolving current tracking problems.