# Time-parametrization of a geometric path

Robotic motion planning is the problem of finding a smooth, high-quality and collision-free trajectory for a robots moving in an cluttered environment. See here for an introduction to robotic motion planning.

In this note, we only consider an component of the robotic motion
planning pipeline–path parametrization. This is a **critical step** in
the pipeline for achieving high-quality motions, especially when the
task subject to stringent dynamic constraints (suction cup, friction) or
kinematic constraints (e.g. robot actuation limits). See our recent work
Pham and Pham (2019) for experimental results.

The time-parametrization problem can be concisely given as follows. Given a geometric path \(\mathbf{p}(s) \in \mathbb{R}^n\) the time parametrization \(s(t) \in \mathbb{R}\), which is found in a prior stage, find the time-parametrized trajectory \(\mathbf{q}(t)\in \mathbb{R}^n\) of the robot.

Note that how to find the time-parametrization is a different problem from the one discussed here. See Pham and Pham (2018) or the accompanying library toppra instead.

## Kinematic derivations

By application of the chain rule, one can derive the trajectory and its higher-oder derivatives as functions of the geometric path and the time-parameterizion. Indeed, the trajectory is simply

\begin{equation} \mathbf q(t) := \mathbf p(s(t)) \end{equation}

Applying the chain rule to obtain

\begin{align}
\dot {\mathbf{q}} (t) & = \mathbf{p}’(s(t)) \dot s(t)\\

\ddot {\mathbf{q}} (t) & = \mathbf{p}’‘(s(t)) \dot s(t)^2 + \mathbf{p}’(s(t)) \ddot s(t)
\end{align}

We can carry on this evaluation further to obtain higher-order derivatives of the trajectory using the geometric path and the parametrization.

```
import sympy as sy
t = sy.symbols('t')
p, s = sy.symbols('p s', cls=sy.Function)
q = p(s(t))
qd = sy.Derivative(q, (t, 1), evaluate=True)
qdd = sy.Derivative(q, (t, 2), evaluate=True)
qddd = sy.Derivative(q, (t, 3), evaluate=True)
outputs = """
{:} &= {:} \\\\
{:} &= {:} \\\\
{:} &= {:}
""".format(
"\\dot{q}(t)", sy.latex(qd),
"\\ddot{q}(t)", sy.latex(qdd),
"\\dddot{q}(t)", sy.latex(qddd))
outputs = "\\begin{align}\n" + outputs + "\n\\end{align}"
outputs
```

Evaluating the above code block leads to:

\begin{align}
\dot{q}(t) &= \frac{d}{d s{\left(t \right)}} p{\left(s{\left(t \right)} \right)} \frac{d}{d t} s{\left(t \right)} \\

\ddot{q}(t) &= \frac{d}{d s{\left(t \right)}} p{\left(s{\left(t \right)} \right)} \frac{d^{2}}{d t^{2}} s{\left(t \right)} + \frac{d^{2}}{d s{\left(t \right)}^{2}} p{\left(s{\left(t \right)} \right)} \left(\frac{d}{d t} s{\left(t \right)}\right)^{2} \\

\dddot{q}(t) &= \frac{d}{d s{\left(t \right)}} p{\left(s{\left(t \right)} \right)} \frac{d^{3}}{d t^{3}} s{\left(t \right)} + 3 \frac{d^{2}}{d s{\left(t \right)}^{2}} p{\left(s{\left(t \right)} \right)} \frac{d}{d t} s{\left(t \right)} \frac{d^{2}}{d t^{2}} s{\left(t \right)} + \frac{d^{3}}{d s{\left(t \right)}^{3}} p{\left(s{\left(t \right)} \right)} \left(\frac{d}{d t} s{\left(t \right)}\right)^{3}
\end{align}

Assuming that the geometric paths and the time-parametrization are available and compatible (e.g., having the same domain), the time-parametrization is solved. Indeed, the output trajectory’s derivatives are computed using the input geometric path and the time-parametrization as in the following pseudocode:

```
def trajectory(t):
return p(s(t))
def first_derivative(t):
return p'(s(t)) * s'(t)
def second_derivative(t):
return p'(s(t)) * s''(t) + p''(s(t)) s'(t) ** 2
```

The geometric path \(\mathbf{p}(s)\) is usually given as a smooth piecewise-polynomial trajectory, and thus can be used directly. In contrast, \(s(t)\) is usually not available and must be derived.

## Derivatives of the time-parameterization function

In practice, the path parametrization is usually given by the path positions \((s_0, \dots, s_{N})\) and path velocities \((s^d_0, \dots, s^d_{N})\). Further, the path velocities are all non-negative. Note that \(N\) is the number of segments along the path. In order to solve the path parametrization problem using the expressions derived in the last section, we need to find a representation of the time-parametrization function \(s(t)\) that allows differentiation. This representation \(s_m(t)\) must satisfies the following conditions:

\begin{equation} \forall i, \exists t_i, s_m(t_i) = s_i, \; \dot{s}_m(t_i) = s^d_i. \end{equation}

In words, the representation \(s_m(t)\) needs to be consistent with the input data.

In OOP language, we want to implement a class that realizes the following interface:

```
class SmoothFunction:
domain: double[2]
def evaluate(time: double, order: int) -> ndarray[double, dof]:
```

Here, `dof`

equals 1 because the time-parametrization is a scalar
function. The actual parametrization, is constructed from the path
positions and velocities:

```
class TimeParametrization(SmoothFunction):
def __init__(self,
positions: ndarray[double, N + 1],
velocities: ndarray[double, N + 1]) -> None:
```

### Constant-acceleration representation

We consider now a common condition for deriving \(s_m(t)\): The path acceleration \(\ddot{s}(t)\) is constant in each segment.

See Hauser (2014) for an expository of this approach.

### Spline-based velocity profile

A problem with the constant acceleration assumption is that for time-optimal parametrizations, there are acceleration jumps in the velocity profile. Physically, this is harmful to the robot motors and cause bad tracking error. In this section, we explore the idea of fitting a smooth polynomial that satisfies acceleration continuity or satisfying jerk constraints

Notice that it is not actually necessary to satisfy this consistency condition:

\begin{equation} \forall i, \exists t_i, s_m(t_i) = s_i, \; \dot{s}_m(t_i) = s^d_i. \end{equation}

Rather, it’s sufficient to satisfies

\begin{equation} \forall i \in [0, N], \exists t_i, s_m(t_i) = s_i, \; \dot{s}_m(t_i) = s^d_i. \end{equation}

This is because there is simply no merit to “hitting” all the velocities.

Suppose we fit a smoothing spline that returns path velocity \(\dot s\) given path position \(s\). Let this function be \(v(s | \theta)\) where \(\theta\) is the spline parameters. We can obtain function \(t_m(s)\) by doing the integral \[ t(s) = \int_{0}^s \frac{ds}{v(s)} \] This can be done numerically using a numerical integration algorithm scipy.integrate. The result of this step is the time array \(t_0,\dots,t_{N}\) corresponding to \(s_0,\dots,s_{N}\). We can then fit another polynomial on the arrays \(t_i, s_i\) to obtain respectively the position, velocity and acceleration functions:

\begin{equation} s_m(t), \dot{s}_m(t), \ddot{s}_m(t). \end{equation}

Obtain sequences \(t_0,\dots,t_M\) and \(s_0,\dots,s_M\), we can then fit a spline \(s_{spl}(t)\) and then uses this spline as \(s_m(t)\). This method actually has very good property, i.e. with densed-points, spline approximate approaches the actual function.

### Obtaining a good representation of the path velocity

This section is pretty much a work in progress.

```
import sympy as sy
t = sy.symbols('t')
v, s = sy.symbols('v s', cls=sy.Function)
sfunc = v(s(t))
sd = sy.Derivative(sfunc, (t, 1), evaluate=True)
sdd = sy.Derivative(sfunc, (t, 2), evaluate=True)
sddd = sy.Derivative(sfunc, (t, 3), evaluate=True)
outputs = """
{:} &= {:} \\\\
{:} &= {:} \\\\
{:} &= {:}
""".format(
"\\dot{s}(t)", sfunc,
"\\ddot{s}(t)", sy.latex(sd),
"\\dddot{s}(t)", sy.latex(sdd),
"\\ddddot{s}(t)", sy.latex(sddd))
outputs = "\\begin{align}\n" + outputs + "\n\\end{align}"
outputs
```

\begin{align}

```
\dot{s}(t) &= v(s(t)) \\\\\\
\ddot{s}(t) &= \frac{d}{d t} s{\left(t \right)} \frac{d}{d s{\left(t \right)}} v{\left(s{\left(t \right)} \right)} \\\\\\
\dddot{s}(t) &= \left(\frac{d}{d t} s{\left(t \right)}\right)^{2} \frac{d^{2}}{d s{\left(t \right)}^{2}} v{\left(s{\left(t \right)} \right)} + \frac{d^{2}}{d t^{2}} s{\left(t \right)} \frac{d}{d s{\left(t \right)}} v{\left(s{\left(t \right)} \right)}
```

\end{align}

In short form we can write \(\dot{v} = v v’, ; \ddot{v} = v^2 v’’ + v v’^2\)

Ok, we are here but I am still stucked. Unable to move forward. Perharps there is nothing more to this?

## Related note

MaintainToppra OnTrajectoryInterpolationInToppra