We consider an \( n \)-DOF robot with configuration \( \mathbf{q}(t) \in \mathbb{R}^n \). A geometric planner (from earlier chapters) provides a collision-free path \( \mathbf{q}(s) \) with scalar path parameter \( s \in [0, s_f] \) and \( \mathbf{q} \) at least twice differentiable in \( s \). The lab objective is to find a time-parameterization \( s(t) \) such that:

• Boundary conditions: \( s(0) = 0 \), \( s(T) = s_f \).
• Monotonicity: \( \dot{s}(t) \ge 0 \) (no backward motion along the path).
• Joint velocity limits: for each joint \( i \), \( |\dot{q}_i(t)| \le \dot{q}_{i,\max} \).
• Joint acceleration limits: for each joint \( i \), \( |\ddot{q}_i(t)| \le \ddot{q}_{i,\max} \).

The time-parameterization problem can be formulated as:

\[ \begin{aligned} &\min_{s(\cdot)} \; T \quad \text{subject to} \\ &\mathbf{q}(t) = \mathbf{q}(s(t)), \\ &\dot{\mathbf{q}}(t), \ddot{\mathbf{q}}(t) \; \text{satisfy joint bounds}, \\ &s(0) = 0,\; s(T)=s_f,\; \dot{s}(t) \ge 0. \end{aligned} \]

Using the chain rule, we relate time derivatives to derivatives with respect to the path parameter \( s \):

\[ \dot{\mathbf{q}}(t) = \frac{d\mathbf{q}}{ds}(s(t)) \, \dot{s}(t), \quad \ddot{\mathbf{q}}(t) = \frac{d\mathbf{q}}{ds}(s(t)) \, \ddot{s}(t) + \frac{d^2\mathbf{q}}{ds^2}(s(t)) \, \dot{s}^2(t). \]

The lab will focus on discretizing \( s \), computing feasible bounds on \( \dot{s} \) and \( \ddot{s} \), and implementing a forward/backward pass algorithm to obtain a feasible and near time-optimal velocity profile along the path.

flowchart TD
  A["Geometric path q(s)"] --> B["Sample s-grid and compute q'(s), q''(s)"]
  B --> C["Project joint limits to path velocity/acc constraints"]
  C --> D["Forward pass with max accel"]
  D --> E["Backward pass with max decel"]
  E --> F["Time-parameterized trajectory q(t), qdot(t), qddot(t)"]
        

Let \( \mathbf{q}(s) = (q_1(s),\dots,q_n(s))^\top \). Denote derivatives with respect to \( s \) by primes: \( q_i'(s) = \frac{dq_i}{ds} \), \( q_i''(s) = \frac{d^2 q_i}{ds^2} \). The joint-level limits are

\[ |\dot{q}_i(t)| \le \dot{q}_{i,\max}, \quad |\ddot{q}_i(t)| \le \ddot{q}_{i,\max}. \]

Substituting the chain-rule expressions yields, for each joint \( i \):

\[ \dot{q}_i(t) = q_i'(s(t))\,\dot{s}(t), \quad \ddot{q}_i(t) = q_i'(s(t))\,\ddot{s}(t) + q_i''(s(t))\,\dot{s}^2(t). \]

The velocity bound becomes

\[ |q_i'(s)\,\dot{s}| \le \dot{q}_{i,\max} \quad \Longrightarrow \quad |\dot{s}| \le \begin{cases} \dfrac{\dot{q}_{i,\max}}{|q_i'(s)|} & \text{if } q_i'(s) \ne 0,\\[0.4em] +\infty & \text{if } q_i'(s) = 0. \end{cases} \]

The acceleration bound becomes two inequalities:

\[ -\ddot{q}_{i,\max} \le q_i'(s)\,\ddot{s} + q_i''(s)\,\dot{s}^2 \le \ddot{q}_{i,\max}. \]

For a fixed \( s \) and \( \dot{s} \), this defines an interval of admissible \( \ddot{s} \) values. We can write the acceleration constraints compactly as

\[ \mathbf{A}(s)\,\ddot{s} \le \mathbf{b}(s) + \mathbf{c}(s)\,\dot{s}^2, \]

where each row corresponds to one of the scalar inequalities above (with appropriate signs). The intersection over all joints gives the feasible interval \( [\ddot{s}_{\min}(s,\dot{s}),\; \ddot{s}_{\max}(s,\dot{s})] \). The lab algorithms will repeatedly query these bounds at discrete samples of \( s \) and \( \dot{s} \).

The classical time-optimal path parameterization (TOPP) problem states: given a fixed path \( \mathbf{q}(s) \) and velocity/acceleration bounds, find the monotone \( s(t) \) with minimal traversal time \( T \). Under mild assumptions (convex, symmetric bounds, no torque limits), the optimal \( \ddot{s}(t) \) is bang-bang, switching between the extreme values \( \ddot{s}_{\max} \) and \( \ddot{s}_{\min} \).

A standard algorithmic approach (used in many manipulators) is:

1. Compute a velocity limit curve \( \dot{s}_{\max}(s) \) from the velocity constraints.
2. Perform a forward pass from \( s = 0 \) enforcing the maximum feasible acceleration \( \ddot{s}_{\max}(s,\dot{s}) \), clipped by the velocity limit curve.
3. Perform a backward pass from \( s = s_f \) enforcing the minimum feasible acceleration \( \ddot{s}_{\min}(s,\dot{s}) \) (typically negative), again clipped by the velocity limit curve.
4. The intersection of the forward and backward velocity profiles yields a feasible and near-optimal \( \dot{s}(s) \).

Once \( \dot{s}(s_k) \) is known on a discrete grid \( \{s_k\} \), the corresponding time stamps are obtained via

\[ t_{k+1} = t_k + \frac{2\,\Delta s_k}{\dot{s}_{k+1} + \dot{s}_k}, \quad \Delta s_k = s_{k+1} - s_k, \]

using the trapezoidal integration rule. The configuration trajectory \( \mathbf{q}(t_k) = \mathbf{q}(s_k) \) then automatically satisfies the joint bounds by construction.

For the lab, we discretize the path parameter domain: \( s_0 = 0 < s_1 < \dots < s_N = s_f \). At each grid point \( s_k \) we precompute \( \mathbf{q}'(s_k) \) and \( \mathbf{q}''(s_k) \), then derive:

• A maximum allowable path speed \( \dot{s}_{\max}(s_k) \) from velocity limits.
• For any given \( \dot{s}_k \), bounds \( [\ddot{s}_{\min,k}(\dot{s}_k), \ddot{s}_{\max,k}(\dot{s}_k)] \) from acceleration limits.

Forward pass (enforcing acceleration):

1. Initialize \( \dot{s}_0 = 0 \).
2. For \( k = 0,\dots,N-1 \), compute the maximal feasible \( \dot{s}_{k+1} \) via

\[ \dot{s}_{k+1}^{\text{cand}} = \sqrt{\dot{s}_k^2 + 2\,\ddot{s}_{\max,k}\,\Delta s_k}, \quad \dot{s}_{k+1} = \min\bigl(\dot{s}_{k+1}^{\text{cand}},\; \dot{s}_{\max}(s_{k+1})\bigr). \]

Backward pass (enforcing deceleration):

1. Initialize \( \dot{s}_N = 0 \).
2. For \( k = N-1,\dots,0 \), compute the maximal speed at \( s_k \) compatible with stopping at \( s_N \):

\[ \dot{s}_k^{\text{cand}} = \sqrt{\dot{s}_{k+1}^2 + 2\,\ddot{s}_{\min,k}\,\Delta s_k}, \quad \dot{s}_k = \min\bigl(\dot{s}_k,\; \dot{s}_k^{\text{cand}}\bigr). \]

The combined forward/backward constrained profile is guaranteed to satisfy the discrete approximation of acceleration and velocity limits (assuming the square roots are real; failure indicates path infeasibility under the given bounds).

flowchart TD
  S0["Init: s-grid, vmax(s), acc bounds"] --> F0["Forward pass: k = 0..N-1"]
  F0 --> F1["For each k: compute sdot[k+1]_cand using max accel"]
  F1 --> F2["Clip with vmax(s[k+1])"]
  F2 --> B0["Backward pass: k = N-1..0"]
  B0 --> B1["For each k: compute sdot[k]_cand using min accel (decel)"]
  B1 --> B2["Set sdot[k] = min(current, cand)"]
  B2 --> OUT["Feasible sdot profile and time stamps"]
        

We first implement the algorithm for a single degree of freedom (e.g., a prismatic joint) where \( q(s) = s \), so \( q'(s) = 1 \), \( q''(s) = 0 \). In this case, path limits reduce to \( |\dot{s}| \le \dot{q}_{\max} \) and \( |\ddot{s}| \le \ddot{q}_{\max} \), i.e., a standard double-integrator with saturation.

import numpy as np

def topp_1d_path(s_f, N, v_max, a_max):
    """
    Time-parameterization for 1D path q(s) = s on [0, s_f] with
    |sdot| <= v_max, |sddot| <= a_max.
    Returns arrays s, sdot, t.
    """
    # Discretize path
    s = np.linspace(0.0, s_f, N + 1)
    ds = np.diff(s)

    # Precompute velocity limit curve (here constant)
    sdot_max = np.full_like(s, v_max, dtype=float)

    # Forward pass
    sdot_fwd = np.zeros_like(s)
    for k in range(N):
        # Max accel
        sdot_cand_sq = sdot_fwd[k]**2 + 2.0 * a_max * ds[k]
        sdot_cand = np.sqrt(max(0.0, sdot_cand_sq))
        sdot_fwd[k + 1] = min(sdot_cand, sdot_max[k + 1])

    # Backward pass
    sdot = sdot_fwd.copy()
    for k in reversed(range(N)):
        # Max decel (note: a_min = -a_max)
        sdot_cand_sq = sdot[k + 1]**2 + 2.0 * a_max * ds[k]
        sdot_cand = np.sqrt(max(0.0, sdot_cand_sq))
        sdot[k] = min(sdot[k], sdot_cand, sdot_max[k])

    # Time integration using trapezoidal rule
    t = np.zeros_like(s)
    for k in range(N):
        if sdot[k] + sdot[k + 1] <= 1e-9:
            raise RuntimeError("Profile infeasible: zero velocity segment")
        t[k + 1] = t[k] + 2.0 * ds[k] / (sdot[k] + sdot[k + 1])

    return s, sdot, t

if __name__ == "__main__":
    s_f = 1.0      # path length
    N = 100        # number of segments
    v_max = 1.0    # max speed
    a_max = 2.0    # max accel

    s, sdot, t = topp_1d_path(s_f, N, v_max, a_max)
    print("Final time T =", t[-1])
      

In a full manipulator implementation, you would use a geometric planner to produce joint-space waypoints, then reparameterize each segment into \( s \)-grid samples and apply a multi-DOF generalization of this algorithm using the projected joint constraints described in Sections 2 and 3.

The following C++17 code implements a similar 1D time-parameterization. For multi-DOF robots, you would replace the scalar bounds with path-projected joint bounds and use Eigen vectors for joint quantities.

#include <cmath>
#include <vector>
#include <stdexcept>

struct TOPPResult1D {
    std::vector<double> s;
    std::vector<double> sdot;
    std::vector<double> t;
};

TOPPResult1D topp1D(double s_f, int N, double v_max, double a_max) {
    if (N <= 0) {
        throw std::invalid_argument("N must be positive");
    }
    TOPPResult1D res;
    res.s.resize(N + 1);
    res.sdot.assign(N + 1, 0.0);
    res.t.assign(N + 1, 0.0);

    // Discretize s
    double ds = s_f / static_cast<double>(N);
    for (int k = 0; k <= N; ++k) {
        res.s[k] = ds * static_cast<double>(k);
    }

    // Velocity limit curve (constant here)
    std::vector<double> sdot_max(N + 1, v_max);

    // Forward pass
    for (int k = 0; k < N; ++k) {
        double sdot_sq = res.sdot[k] * res.sdot[k] + 2.0 * a_max * ds;
        double sdot_cand = std::sqrt(std::max(0.0, sdot_sq));
        res.sdot[k + 1] = std::min(sdot_cand, sdot_max[k + 1]);
    }

    // Backward pass
    for (int k = N - 1; k >= 0; --k) {
        double sdot_sq = res.sdot[k + 1] * res.sdot[k + 1] + 2.0 * a_max * ds;
        double sdot_cand = std::sqrt(std::max(0.0, sdot_sq));
        res.sdot[k] = std::min({res.sdot[k], sdot_cand, sdot_max[k]});
    }

    // Time stamps (trapezoidal rule)
    for (int k = 0; k < N; ++k) {
        double denom = res.sdot[k] + res.sdot[k + 1];
        if (denom <= 1e-9) {
            throw std::runtime_error("Infeasible profile: zero velocity segment");
        }
        res.t[k + 1] = res.t[k] + 2.0 * ds / denom;
    }

    return res;
}

// Example usage:
// int main() {
//     auto res = topp1D(1.0, 100, 1.0, 2.0);
//     std::cout << "Final time T = " << res.t.back() << std::endl;
// }
      

In robotics codebases, this routine is often integrated after a PRM/RRT planner and before trajectory execution, sometimes wrapped in a controller interface that can provide timestamped joint targets.

The Java version mirrors the C++ implementation and is suitable for robotics middleware written in Java (e.g., some industrial robot SDKs).

public class TOPP1D {

    public static class Result {
        public double[] s;
        public double[] sdot;
        public double[] t;
    }

    public static Result topp1D(double sF, int N, double vMax, double aMax) {
        if (N <= 0) {
            throw new IllegalArgumentException("N must be positive");
        }

        Result res = new Result();
        res.s = new double[N + 1];
        res.sdot = new double[N + 1];
        res.t = new double[N + 1];

        double ds = sF / (double) N;
        for (int k = 0; k <= N; ++k) {
            res.s[k] = ds * k;
            res.sdot[k] = 0.0;
            res.t[k] = 0.0;
        }

        double[] sdotMax = new double[N + 1];
        for (int k = 0; k <= N; ++k) {
            sdotMax[k] = vMax;
        }

        // Forward pass
        for (int k = 0; k < N; ++k) {
            double sdotSq = res.sdot[k] * res.sdot[k] + 2.0 * aMax * ds;
            double sdotCand = Math.sqrt(Math.max(0.0, sdotSq));
            res.sdot[k + 1] = Math.min(sdotCand, sdotMax[k + 1]);
        }

        // Backward pass
        for (int k = N - 1; k >= 0; --k) {
            double sdotSq = res.sdot[k + 1] * res.sdot[k + 1] + 2.0 * aMax * ds;
            double sdotCand = Math.sqrt(Math.max(0.0, sdotSq));
            double min = Math.min(res.sdot[k], sdotCand);
            res.sdot[k] = Math.min(min, sdotMax[k]);
        }

        // Time stamps
        for (int k = 0; k < N; ++k) {
            double denom = res.sdot[k] + res.sdot[k + 1];
            if (denom <= 1e-9) {
                throw new RuntimeException("Infeasible profile: zero velocity segment");
            }
            res.t[k + 1] = res.t[k] + 2.0 * ds / denom;
        }

        return res;
    }

    // Example usage:
    // public static void main(String[] args) {
    //     Result res = topp1D(1.0, 100, 1.0, 2.0);
    //     System.out.println("Final time T = " + res.t[res.t.length - 1]);
    // }
}
      

Note that the discretization resolution \( N \) trades off computational load and approximation accuracy. In practice, \( N \) is chosen according to curvature of the path and the required timing precision for the robot controller.

MATLAB is widely used in robotics for prototyping and integration with Simulink-based control schemes. The script below computes the 1D time-parameterization and then shows how to feed the resulting trajectory into Simulink.

function [s, sdot, t] = topp1D(s_f, N, v_max, a_max)
    % Time-parameterization for 1D path q(s) = s
    s = linspace(0, s_f, N + 1);
    ds = diff(s);

    sdot_max = v_max * ones(1, N + 1);

    % Forward pass
    sdot = zeros(1, N + 1);
    for k = 1:N
        sdot_cand_sq = sdot(k)^2 + 2 * a_max * ds(k);
        sdot_cand = sqrt(max(0, sdot_cand_sq));
        sdot(k + 1) = min(sdot_cand, sdot_max(k + 1));
    end

    % Backward pass
    for k = N:-1:1
        sdot_cand_sq = sdot(k + 1)^2 + 2 * a_max * ds(k);
        sdot_cand = sqrt(max(0, sdot_cand_sq));
        sdot(k) = min([sdot(k), sdot_cand, sdot_max(k)]);
    end

    % Time stamps
    t = zeros(1, N + 1);
    for k = 1:N
        denom = sdot(k) + sdot(k + 1);
        if denom <= 1e-9
            error("Infeasible profile: zero velocity segment");
        end
        t(k + 1) = t(k) + 2 * ds(k) / denom;
    end
end

% Example usage:
% [s, sdot, t] = topp1D(1.0, 100, 1.0, 2.0);
% q = s; % since q(s) = s in 1D
% % Create a timeseries for Simulink:
% q_ts = timeseries(q, t);
% sdot_ts = timeseries(sdot, t);
% % These time series can be fed into a "From Workspace" block in Simulink.
      

In Simulink, you typically use a From Workspace block to import the q_ts and sdot_ts signals, then connect them to the desired plant or joint-space controller model.

Mathematica is well-suited for both symbolic derivations and numerical experiments. The following code implements the discrete forward/backward TOPP algorithm for the 1D case.

topp1D[sf_, n_Integer?Positive, vmax_, amax_] :=
 Module[{s, ds, sdotMax, sdotFwd, sdot, t},
  s = Range[0, n] * (sf/n);
  ds = Differences[s];
  sdotMax = ConstantArray[vmax, n + 1];

  (* Forward pass *)
  sdotFwd = ConstantArray[0., n + 1];
  Do[
   With[{sdotSq = sdotFwd[[k]]^2 + 2.0*amax*ds[[k]]},
    sdotFwd[[k + 1]] =
      Min[Sqrt[Max[0., sdotSq]], sdotMax[[k + 1]]];
   ],
   {k, 1, n}
  ];

  (* Backward pass *)
  sdot = sdotFwd;
  Do[
   With[{sdotSq = sdot[[k + 1]]^2 + 2.0*amax*ds[[k]]},
    sdot[[k]] =
      Min[sdot[[k]], Sqrt[Max[0., sdotSq]], sdotMax[[k]]];
   ],
   {k, n, 1, -1}
  ];

  (* Time stamps *)
  t = ConstantArray[0., n + 1];
  Do[
   Module[{den = sdot[[k]] + sdot[[k + 1]]},
    If[den <= 10^-9,
      Throw["Infeasible profile: zero velocity segment"];
    ];
    t[[k + 1]] = t[[k]] + 2.0*ds[[k]]/den;
   ],
   {k, 1, n}
  ];

  <|"s" -> s, "sdot" -> sdot, "t" -> t|>
 ]

(* Example usage:
   res = topp1D[1.0, 100, 1.0, 2.0];
   ListLinePlot[{Transpose[{res["s"], res["sdot"]}],
                 Transpose[{res["t"], res["sdot"]}]},
                PlotLegends -> {"sdot(s)", "sdot(t)"}]
*)
      

Mathematica can also be used to symbolically verify properties of the motion (e.g., checking that the resulting acceleration profile never exceeds \( \ddot{q}_{\max} \)) or to explore different cost functionals beyond minimum time.

Problem 1 (Projection of Velocity Limits): Let \( \mathbf{q}(s) \in \mathbb{R}^n \) be a differentiable path and suppose joint velocity limits satisfy \( |\dot{q}_i(t)| \le \dot{q}_{i,\max} \) for all \( i \). Derive the path-speed bound \( \dot{s}_{\max}(s) \) such that any \( \dot{s}(t) \) with \( \dot{s}(t) \le \dot{s}_{\max}(s(t)) \) respects all joint velocity limits.

Solution: Using \( \dot{q}_i(t) = q_i'(s(t))\,\dot{s}(t) \), the constraint reads

\[ |q_i'(s)\,\dot{s}| \le \dot{q}_{i,\max}. \]

For a fixed \( s \) with \( q_i'(s) \ne 0 \), we obtain

\[ |\dot{s}| \le \frac{\dot{q}_{i,\max}}{|q_i'(s)|}. \]

If \( q_i'(s) = 0 \), the constraint imposes no restriction on \( \dot{s} \) for joint \( i \). Taking the minimum over all joints yields

\[ \dot{s}_{\max}(s) = \min_{i \in \{1,\dots,n\} \,:\, q_i'(s) \ne 0} \frac{\dot{q}_{i,\max}}{|q_i'(s)|}, \]

and any \( \dot{s}(t) \in [0, \dot{s}_{\max}(s(t))] \) guarantees all joint velocities remain within bounds.

Problem 2 (Time-Optimal 1D Double Integrator): Consider a point mass on a line with dynamics \( \ddot{x}(t) = u(t) \), where \( |u(t)| \le a_{\max} \) and \( |\dot{x}(t)| \le v_{\max} \). Given \( x(0)=0, \dot{x}(0)=0, x(T)=L, \dot{x}(T)=0 \), derive the minimum-time \( T^\star \) and describe the corresponding control profile.

Solution: The optimal control is bang-bang: accelerate at \( u(t) = a_{\max} \) up to some switching time, then decelerate at \( u(t) = -a_{\max} \). There are two regimes:

• Triangular profile (no cruise): If the distance is small enough that the velocity bound is never active, we solve \( L = \tfrac{1}{2} a_{\max} t_a^2 + \tfrac{1}{2} a_{\max} t_a^2 = a_{\max} t_a^2 \), so \( t_a = \sqrt{L / a_{\max}} \) and \( T^\star = 2 t_a = 2 \sqrt{L / a_{\max}} \). The peak velocity is \( \dot{x}_{\text{peak}} = a_{\max} t_a \).
• Trapezoidal profile (with cruise): If the triangular profile would exceed \( v_{\max} \), then the motion saturates the velocity. The minimal distance to reach \( v_{\max} \) and then brake symmetrically is \( L_{\triangle} = v_{\max}^2 / a_{\max} \). If \( L \ge L_{\triangle} \), the motion consists of: accelerate from 0 to \( v_{\max} \) in time \( t_a = v_{\max} / a_{\max} \), cruise at \( v_{\max} \) for time \( t_c \), then decelerate symmetrically. Total distance is

\[ L = \underbrace{\tfrac{1}{2} a_{\max} t_a^2}_{\text{acc}} + \underbrace{v_{\max} t_c}_{\text{cruise}} + \underbrace{\tfrac{1}{2} a_{\max} t_a^2}_{\text{dec}} = v_{\max}^2 / a_{\max} + v_{\max} t_c. \]

Hence

\[ t_c = \frac{L - v_{\max}^2 / a_{\max}}{v_{\max}}, \quad T^\star = 2 t_a + t_c = 2 \frac{v_{\max}}{a_{\max}} + \frac{L - v_{\max}^2 / a_{\max}}{v_{\max}}. \]

The optimal control switches: accelerate at \( a_{\max} \), optionally cruise at 0 acceleration, then decelerate at \( -a_{\max} \).

Problem 3 (Feasibility Check in Discrete TOPP): In the discrete forward pass, consider step \( k \) with known \( s_k, \dot{s}_k \) and step length \( \Delta s_k \). Show that if \( \dot{s}_k^2 + 2\,\ddot{s}_{\max,k}\,\Delta s_k < 0 \) for some segment, the continuous-time problem is infeasible under the given acceleration bounds.

Solution: The discrete forward update assumes constant acceleration \( \ddot{s} = \ddot{s}_{\max,k} \) over the segment of length \( \Delta s_k \). The kinematic relation

\[ \dot{s}_{k+1}^2 = \dot{s}_k^2 + 2\,\ddot{s}_{\max,k}\,\Delta s_k \]

must hold for some \( \dot{s}_{k+1} \ge 0 \). If the right-hand side is negative, there exists no real \( \dot{s}_{k+1} \) satisfying constant acceleration within the allowed bound. Since the discrete model is a necessary condition for feasibility of the continuous model (it is a direct integration of the ODE), violation implies that the continuous-time trajectory cannot be constructed with the given per-segment acceleration bound.

Problem 4 (Multi-DOF Path Acceleration Bounds): For an \( n \)-DOF manipulator, the acceleration constraint for joint \( i \) is \( |\ddot{q}_i(t)| \le \ddot{q}_{i,\max} \). Using \( \ddot{q}_i = q_i'(s)\,\ddot{s} + q_i''(s)\,\dot{s}^2 \), derive an interval for admissible \( \ddot{s} \) at a given \( (s,\dot{s}) \), assuming \( q_i'(s) \ne 0 \).

Solution: The inequality

\[ -\ddot{q}_{i,\max} \le q_i'(s)\,\ddot{s} + q_i''(s)\,\dot{s}^2 \le \ddot{q}_{i,\max} \]

yields two linear bounds on \( \ddot{s} \). Rearranging:

\[ q_i'(s)\,\ddot{s} \ge -\ddot{q}_{i,\max} - q_i''(s)\,\dot{s}^2, \quad q_i'(s)\,\ddot{s} \le \ddot{q}_{i,\max} - q_i''(s)\,\dot{s}^2. \]

If \( q_i'(s) > 0 \),

\[ \ddot{s} \in \left[ \frac{-\ddot{q}_{i,\max} - q_i''(s)\,\dot{s}^2}{q_i'(s)}, \frac{\ddot{q}_{i,\max} - q_i''(s)\,\dot{s}^2}{q_i'(s)} \right]. \]

If \( q_i'(s) < 0 \), the interval bounds swap, but can be written in the same ordered form by taking the minimum and maximum of the two expressions. The global path-acceleration interval is the intersection of such intervals over all joints.

Problem 5 (Monotonicity and No Backtracking): Explain why enforcing \( \dot{s}(t) \ge 0 \) is important in the path-parameterization context, and how allowing \( \dot{s} < 0 \) could affect collision avoidance guarantees provided by the original geometric path.

Solution: The geometric path \( \mathbf{q}(s) \) is assumed collision-free for \( s \in [0, s_f] \). If \( s(t) \) is monotone nondecreasing, the trajectory \( \mathbf{q}(t) = \mathbf{q}(s(t)) \) stays on the same collision-free curve in the same order; obstacles are never intersected. If negative \( \dot{s} \) were allowed, the robot would traverse the path backward in \( s \) during some intervals, possibly re-entering regions that are geometrically safe but temporally inconsistent with the intended motion plan (e.g., when other moving agents are present). Moreover, many proofs of correctness for sampling-based planners assume monotone parameterizations; violating monotonicity can break these guarantees and complicate reasoning about collision-free execution.

In this lab, you implemented time-parameterization of a given geometric path under joint velocity and acceleration limits. By expressing \( \dot{\mathbf{q}} \) and \( \ddot{\mathbf{q}} \) in terms of path derivatives and the scalar variables \( \dot{s}, \ddot{s} \), we obtained convex inequality constraints defining feasible intervals for the path acceleration at each point. The resulting forward/backward pass algorithm yields a feasible and near time-optimal path-speed profile.

Practical implementations in Python, C++, Java, MATLAB/Simulink, and Mathematica show how the abstract kinodynamic formulation translates into concrete code. In a full kinodynamic planning pipeline, this parameterization step is combined with geometric planning (e.g., PRM, RRT, lattice planners) to produce trajectories that are both collision-free and dynamically realizable by the robot actuators.