In optimal control, the controller is obtained by minimizing a scalar performance index or cost functional over trajectories of states and control inputs. For a manipulator with state \( x(t) \) (e.g., joint positions and velocities) and control input \( u(t) \) (typically joint torques), a general finite-horizon cost functional has the form

\[ J[x(\cdot),u(\cdot)] = \Phi\big(x(T)\big) + \int_0^T \ell\big(x(t),u(t),t\big)\,\mathrm{d}t, \]

where \( \Phi \) is a terminal cost (e.g., final tracking error) and \( \ell \) is a running cost density (stage cost). For manipulators we typically encode:

• Tracking: penalize deviation from a desired trajectory in joint or task space.
• Energy/Effort: penalize large torque magnitudes or power usage.
• Smoothness: penalize large accelerations, jerks, or fast control variations.

These terms are not arbitrary: their precise mathematical structure (often quadratic) is chosen so that the cost is nonnegative, convex (for linear systems and quadratic costs), and computationally tractable for optimization algorithms.

flowchart TD
  A["Specify objectives: tracking, energy, smoothness"] --> B["Choose mathematical terms J_track, J_effort, J_smooth"]
  B --> C["Select weights Q, R, S, lambdas"]
  C --> D["Assemble scalar cost J"]
  D --> E["Feed J into optimal control solver"]
  E --> F["Obtain feedback policy u_k(x_k)"]
  F --> G["Execute on manipulator"]
  G --> A
        

We recall the standard rigid manipulator dynamics (covered in robotics kinematics and dynamics):

\[ \mathbf{M}(q)\ddot q + \mathbf{C}(q,\dot q)\dot q + g(q) = \boldsymbol{\tau}, \]

where:

• \( q \in \mathbb{R}^n \): joint coordinates,
• \( \dot q, \ddot q \): joint velocities and accelerations,
• \( \mathbf{M}(q) \): positive definite inertia matrix,
• \( \mathbf{C}(q,\dot q)\dot q \): Coriolis/centrifugal term,
• \( g(q) \): gravity vector,
• \( \boldsymbol{\tau} \in \mathbb{R}^n \): vector of joint torques.

For control design we often choose a state vector \( x = \begin{bmatrix} q^\top & \dot q^\top \end{bmatrix}^\top \). In this lesson we treat the dynamics abstractly and focus on the cost functional \( J \), assuming that the state and input sequences \( \{x(t),u(t)\} \) satisfy the dynamics.

Let \( q_d(t) \) be a desired joint trajectory. A natural tracking error is \( e_q(t) = q(t) - q_d(t) \). A standard quadratic joint-space tracking cost is

\[ J_{\text{track},q} = \tfrac{1}{2}\int_0^T e_q(t)^\top \mathbf{Q}_q\, e_q(t)\,\mathrm{d}t, \]

where \( \mathbf{Q}_q \in \mathbb{R}^{n\times n} \) is symmetric positive semidefinite. Weighting different joints differently is done by choosing diagonal entries of \( \mathbf{Q}_q \) accordingly.

If we include velocities in the state, we can define a stacked error \( e_x(t) = x(t) - x_d(t) \) and use

\[ J_{\text{track},x} = \tfrac{1}{2}\int_0^T e_x(t)^\top \mathbf{Q}_x\, e_x(t)\,\mathrm{d}t, \]

where \( \mathbf{Q}_x \) can weigh joint position and velocity errors differently. A terminal tracking term is often added:

\[ J_{\text{terminal}} = \tfrac{1}{2}\big(x(T)-x_d(T)\big)^\top \mathbf{P}_f\big(x(T)-x_d(T)\big), \]

with \( \mathbf{P}_f \) symmetric positive semidefinite. This emphasizes accuracy at the end of the motion (e.g., exact placement of end-effector).

Let \( x = f(q) \in \mathbb{R}^m \) be the task-space (operational space) pose of the end-effector (position and possibly orientation parameters), with desired trajectory \( x_d(t) \). The task-space error is \( e_x(t) = x(t) - x_d(t) \), and we can define

\[ J_{\text{track},\text{task}} = \tfrac{1}{2}\int_0^T e_x(t)^\top \mathbf{Q}_x\, e_x(t)\,\mathrm{d}t. \]

Here, \( \mathbf{Q}_x \) reflects importance of directions in task space (e.g., higher weight for vertical position than lateral position). Using task-space errors is particularly natural for tasks like end-effector trajectory tracking.

In many designs, joint and task-space tracking terms are blended:

\[ J_{\text{track}} = \tfrac{1}{2}\int_0^T \Big( e_q(t)^\top \mathbf{Q}_q\, e_q(t) + e_x(t)^\top \mathbf{Q}_x\, e_x(t) \Big)\,\mathrm{d}t. \]

The choice of \( \mathbf{Q}_q \) and \( \mathbf{Q}_x \) is part of performance tuning: larger entries penalize deviations more strongly.

Effort penalties discourage large torques, which improves energy efficiency and reduces actuator stress. A common quadratic effort cost is

\[ J_{\text{effort}} = \tfrac{1}{2}\int_0^T \boldsymbol{\tau}(t)^\top \mathbf{R}\, \boldsymbol{\tau}(t)\,\mathrm{d}t,\quad \mathbf{R} = \mathbf{R}^\top \succeq 0. \]

For electric actuators, a closer proxy to energy is mechanical power \( p(t) = \boldsymbol{\tau}(t)^\top \dot q(t) \). One can define

\[ J_{\text{power}} = \tfrac{1}{2}\int_0^T \big(\boldsymbol{\tau}(t)^\top \dot q(t)\big)^2\,\mathrm{d}t, \]

which penalizes high instantaneous power. However, this cost is nonlinear in both torque and velocity, and may complicate optimization. For many control designs, the simpler quadratic torque penalty is preferred because it preserves the quadratic structure.

Lemma (Nonnegativity). If \( \mathbf{R} \) is positive semidefinite, then \( J_{\text{effort}} \ge 0 \), with equality if and only if \( \boldsymbol{\tau}(t) = 0 \) almost everywhere on \( [0,T] \) for which \( \mathbf{R} \) is positive definite.

Proof. For any \( t \), we have \( \boldsymbol{\tau}(t)^\top \mathbf{R}\,\boldsymbol{\tau}(t) \ge 0 \) because \( \mathbf{R} \) is positive semidefinite. Integrating over \( [0,T] \) preserves nonnegativity. If \( \mathbf{R} \) is positive definite, the integrand is zero if and only if \( \boldsymbol{\tau}(t) = 0 \), so \( J_{\text{effort}} = 0 \) implies zero torque almost everywhere.

Smooth motions are critical for safety, wear reduction, and human–robot interaction. Smoothness can be enforced by penalizing derivatives of motion or control:

• Acceleration penalty: penalize \( \ddot q(t) \).
• Jerk penalty: penalize \( \dddot q(t) \).
• Control rate penalty: penalize changes in torque \( \Delta \boldsymbol{\tau} \).

Typical acceleration penalty:

\[ J_{\text{accel}} = \tfrac{1}{2}\int_0^T \ddot q(t)^\top \mathbf{S}_a\,\ddot q(t)\,\mathrm{d}t, \]

with \( \mathbf{S}_a \succeq 0 \). Similarly, a jerk penalty is

\[ J_{\text{jerk}} = \tfrac{1}{2}\int_0^T \dddot q(t)^\top \mathbf{S}_j\,\dddot q(t)\,\mathrm{d}t. \]

In discrete time (see next section), smoothness is often enforced via control increments:

\[ J_{\Delta u} = \tfrac{1}{2}\sum_{k=1}^{N} \big(u_k - u_{k-1}\big)^\top \mathbf{S}_u\big(u_k - u_{k-1}\big). \]

This discourages rapid changes in commanded torques and is easy to integrate into discrete-time optimal control solvers.

A practical cost functional combines multiple objectives with scalar weights:

\[ J = J_{\text{terminal}} + \int_0^T \Big( \tfrac{1}{2} e_x(t)^\top \mathbf{Q}_x e_x(t) + \tfrac{1}{2} \boldsymbol{\tau}(t)^\top \mathbf{R}\,\boldsymbol{\tau}(t) + \tfrac{1}{2} \ddot q(t)^\top \mathbf{S}_a \ddot q(t) \Big)\,\mathrm{d}t. \]

Alternatively, one can factor weights explicitly as scalars:

\[ J = J_{\text{terminal}} + \lambda_{\text{track}} J_{\text{track}} + \lambda_{\text{effort}} J_{\text{effort}} + \lambda_{\text{smooth}} J_{\text{accel}}, \]

with \( \lambda_{\text{track}},\lambda_{\text{effort}},\lambda_{\text{smooth}} > 0 \). Tuning these scalars trades tracking accuracy against energy and smoothness.

If all matrices \( \mathbf{Q}_x,\mathbf{R},\mathbf{S}_a,\mathbf{P}_f \) are positive semidefinite and at least one is positive definite in the components of interest, the resulting quadratic cost is strictly convex in those components for fixed dynamics, which is fundamental for the uniqueness of optimal solutions in linear-quadratic problems.

flowchart TD
  W1["Weights: lambdas and matrices"] --> J1["Total cost J"]
  J1 --> OC["Optimal control solver"]
  OC --> PATH["Optimal state and control trajectory"]
  PATH --> TRADE["Observed trade-off: accuracy vs effort vs smoothness"]
        

For digital control and numerical optimization, we discretize time with step size \( \Delta t \) and grid \( t_k = k\Delta t \), \( k=0,\dots,N \), with \( T = N\Delta t \). Integrals are approximated by sums:

\[ J_{\text{track}} \approx \tfrac{1}{2}\sum_{k=0}^{N} e_x(k)^\top \mathbf{Q}_x\, e_x(k)\,\Delta t, \]

\[ J_{\text{effort}} \approx \tfrac{1}{2}\sum_{k=0}^{N-1} \boldsymbol{\tau}_k^\top \mathbf{R}\,\boldsymbol{\tau}_k\,\Delta t, \]

\[ J_{\text{accel}} \approx \tfrac{1}{2}\sum_{k=0}^{N} \ddot q_k^\top \mathbf{S}_a\,\ddot q_k\,\Delta t. \]

A general discrete-time quadratic stage cost at step \( k \) is

\[ \ell_k(x_k,u_k) = \tfrac{1}{2}\big(x_k - x_k^d\big)^\top \mathbf{Q}\big(x_k - x_k^d\big) + \tfrac{1}{2}u_k^\top \mathbf{R}u_k + \tfrac{1}{2}\big(u_k - u_{k-1}\big)^\top \mathbf{S}_u\big(u_k - u_{k-1}\big), \]

leading to total discrete-time cost

\[ J = \tfrac{1}{2}\big(x_N - x_N^d\big)^\top \mathbf{P}_f\big(x_N - x_N^d\big) + \sum_{k=0}^{N-1}\ell_k(x_k,u_k). \]

This is the canonical form used in finite-horizon LQR and many nonlinear optimal control methods (e.g., iLQR/DDP, covered in later lessons).

For optimal control algorithms (direct methods, iLQR, etc.) we need derivatives of the stage cost \( \ell_k(x_k,u_k) \). Consider the simplified quadratic form

\[ \ell_k(x_k,u_k) = \tfrac{1}{2}\big(x_k - x_k^d\big)^\top \mathbf{Q}\big(x_k - x_k^d\big) + \tfrac{1}{2}u_k^\top \mathbf{R}u_k, \]

with symmetric \( \mathbf{Q},\mathbf{R} \). The gradients are

\[ \frac{\partial \ell_k}{\partial x_k} = \mathbf{Q}\big(x_k - x_k^d\big),\qquad \frac{\partial \ell_k}{\partial u_k} = \mathbf{R}u_k. \]

The Hessians (second derivatives) are constant:

\[ \frac{\partial^2 \ell_k}{\partial x_k^2} = \mathbf{Q},\qquad \frac{\partial^2 \ell_k}{\partial u_k^2} = \mathbf{R},\qquad \frac{\partial^2 \ell_k}{\partial x_k \partial u_k} = \mathbf{0}. \]

Proposition (Strict Convexity). If \( \mathbf{Q} \succ 0 \) and \( \mathbf{R} \succ 0 \), then for fixed \( k \) the function \( \ell_k(x_k,u_k) \) is strictly convex in \( (x_k,u_k) \).

Sketch of Proof. The Hessian matrix of \( \ell_k \) with respect to the stacked variable \( z_k = \begin{bmatrix} x_k \\ u_k \end{bmatrix} \) is block-diagonal: \( \mathrm{H} = \mathrm{diag}(\mathbf{Q},\mathbf{R}) \). Since block diagonal matrices are positive definite if and only if each block is positive definite, \( \mathrm{H} \succ 0 \) and strict convexity follows from standard results in convex analysis.

We implement discrete-time tracking, effort, and smoothness costs in Python. We assume a manipulator model provided by a robotics library (e.g., roboticstoolbox or pinocchio) to compute joint torques from a given state and control sequence. For the cost computation itself, only numpy is strictly necessary.

import numpy as np

def quadratic_tracking_cost(q, q_d, Q, dt):
    """
    Joint-space tracking cost:
    J_track = 0.5 * sum_k (q_k - q_d_k)^T Q (q_k - q_d_k) * dt
    q, q_d: arrays of shape (N+1, n)
    Q: (n, n) symmetric positive semidefinite
    """
    e = q - q_d                      # shape (N+1, n)
    # compute v_k = e_k^T Q e_k for all k
    v = np.einsum("ki,ij,kj->k", e, Q, e)
    return 0.5 * dt * np.sum(v)

def quadratic_effort_cost(tau, R, dt):
    """
    Effort cost:
    J_effort = 0.5 * sum_k tau_k^T R tau_k * dt
    tau: shape (N, n)
    R: (n, n) symmetric positive semidefinite
    """
    v = np.einsum("ki,ij,kj->k", tau, R, tau)
    return 0.5 * dt * np.sum(v)

def control_smoothness_cost(tau, S_u):
    """
    Smoothness cost on torque increments:
    J_du = 0.5 * sum_k (tau_k - tau_{k-1})^T S_u (tau_k - tau_{k-1})
    tau: shape (N, n)
    """
    dtau = tau[1:] - tau[:-1]        # shape (N-1, n)
    v = np.einsum("ki,ij,kj->k", dtau, S_u, dtau)
    return 0.5 * np.sum(v)

def total_cost(q, q_d, tau, Q, R, S_u, dt, lambdas):
    """
    Combine all terms with scalar weights:
      J = lambda_track * J_track + lambda_effort * J_effort
          + lambda_smooth * J_du
    lambdas: dict with keys "track", "effort", "smooth"
    """
    J_tr = quadratic_tracking_cost(q, q_d, Q, dt)
    J_eff = quadratic_effort_cost(tau, R, dt)
    J_du = control_smoothness_cost(tau, S_u)

    return (lambdas["track"] * J_tr
            + lambdas["effort"] * J_eff
            + lambdas["smooth"] * J_du)

# Example usage:
N = 100
n = 6
dt = 0.01
q = np.zeros((N+1, n))
q_d = np.zeros_like(q)
tau = np.zeros((N, n))
Q = np.eye(n)
R = 0.01 * np.eye(n)
S_u = 0.1 * np.eye(n)
lambdas = {"track": 1.0, "effort": 1.0, "smooth": 0.5}

J = total_cost(q, q_d, tau, Q, R, S_u, dt, lambdas)
print("Total cost:", J)
      

In a full optimal control implementation, the sequences q, q_d, and tau are generated from dynamic simulation or a numerical optimizer that enforces the manipulator dynamics.

11.1 C++ (Eigen + robotics library, e.g., Pinocchio)

In C++, we typically combine Eigen for linear algebra with a robotics library such as pinocchio or ROS-based dynamics. Below is a minimal cost implementation that assumes Eigen::MatrixXd for trajectories.

#include <Eigen/Dense>

double trackingCost(const Eigen::MatrixXd& q,
                    const Eigen::MatrixXd& qd,
                    const Eigen::MatrixXd& Q,
                    double dt)
{
    // q, qd: (N+1) x n
    const int Np1 = static_cast<int>(q.rows());
    double J = 0.0;
    for (int k = 0; k < Np1; ++k) {
        Eigen::VectorXd e = q.row(k).transpose() - qd.row(k).transpose();
        J += e.transpose() * Q * e;
    }
    return 0.5 * dt * J;
}

double effortCost(const Eigen::MatrixXd& tau,
                  const Eigen::MatrixXd& R,
                  double dt)
{
    // tau: N x n
    const int N = static_cast<int>(tau.rows());
    double J = 0.0;
    for (int k = 0; k < N; ++k) {
        Eigen::VectorXd t = tau.row(k).transpose();
        J += t.transpose() * R * t;
    }
    return 0.5 * dt * J;
}

double smoothnessCost(const Eigen::MatrixXd& tau,
                      const Eigen::MatrixXd& Su)
{
    const int N = static_cast<int>(tau.rows());
    if (N < 2) return 0.0;
    double J = 0.0;
    for (int k = 1; k < N; ++k) {
        Eigen::VectorXd dtau =
            tau.row(k).transpose() - tau.row(k - 1).transpose();
        J += dtau.transpose() * Su * dtau;
    }
    return 0.5 * J;
}

double totalCost(const Eigen::MatrixXd& q,
                 const Eigen::MatrixXd& qd,
                 const Eigen::MatrixXd& tau,
                 const Eigen::MatrixXd& Q,
                 const Eigen::MatrixXd& R,
                 const Eigen::MatrixXd& Su,
                 double dt,
                 double lambdaTrack,
                 double lambdaEffort,
                 double lambdaSmooth)
{
    double Jtr = trackingCost(q, qd, Q, dt);
    double Jeff = effortCost(tau, R, dt);
    double Js = smoothnessCost(tau, Su);
    return lambdaTrack * Jtr
         + lambdaEffort * Jeff
         + lambdaSmooth * Js;
}
      

11.2 Java (plain arrays, suitable for integration with ROS or Java-based middleware)

Java does not have a default matrix library, but we can implement cost computations using arrays or use a library like EJML. Here, we use plain arrays and assume diagonal weighting matrices.

public class RobotCost {

    public static double trackingCost(double[][] q,
                                      double[][] qd,
                                      double[] Qdiag,
                                      double dt) {
        int Np1 = q.length;
        int n = q[0].length;
        double J = 0.0;
        for (int k = 0; k < Np1; ++k) {
            for (int i = 0; i < n; ++i) {
                double e = q[k][i] - qd[k][i];
                J += Qdiag[i] * e * e;
            }
        }
        return 0.5 * dt * J;
    }

    public static double effortCost(double[][] tau,
                                    double[] Rdiag,
                                    double dt) {
        int N = tau.length;
        int n = tau[0].length;
        double J = 0.0;
        for (int k = 0; k < N; ++k) {
            for (int i = 0; i < n; ++i) {
                double t = tau[k][i];
                J += Rdiag[i] * t * t;
            }
        }
        return 0.5 * dt * J;
    }

    public static double smoothnessCost(double[][] tau,
                                        double[] SuDiag) {
        int N = tau.length;
        if (N < 2) return 0.0;
        int n = tau[0].length;
        double J = 0.0;
        for (int k = 1; k < N; ++k) {
            for (int i = 0; i < n; ++i) {
                double dtau = tau[k][i] - tau[k - 1][i];
                J += SuDiag[i] * dtau * dtau;
            }
        }
        return 0.5 * J;
    }
}
      

11.3 MATLAB/Simulink

In MATLAB, vectorization with matrices is natural. The cost functions can be implemented as:

function J = total_cost(q, qd, tau, Q, R, Su, dt, lambdaTrack, lambdaEffort, lambdaSmooth)
% q, qd: (N+1)-by-n
% tau:   N-by-n

e = q - qd;                           % (N+1)-by-n
J_track = 0.5 * dt * sum(diag(e * Q * e'));

J_effort = 0.5 * dt * sum(diag(tau * R * tau'));

dtau = diff(tau, 1, 1);               % (N-1)-by-n
J_smooth = 0.5 * sum(diag(dtau * Su * dtau'));

J = lambdaTrack * J_track + ...
    lambdaEffort * J_effort + ...
    lambdaSmooth * J_smooth;
end
      

In Simulink, these terms can be realized using blocks:

• Tracking: subtract desired joint trajectory from actual joint trajectory, feed into a Gain block with matrix Q, then into a dot-product and integrator.
• Effort: square torques (using Product or Math Function blocks), apply diagonal gains from R, then integrate.
• Smoothness: apply a discrete derivative or Unit Delay block to compute tau(k) - tau(k-1), square, weight with Su, and integrate.

11.4 Wolfram Mathematica (symbolic and numeric)

Mathematica is convenient for symbolic derivation of gradients and for verifying analytical expressions. For a single joint with state \( q(t) \), torque \( \tau(t) \), and desired trajectory \( q_d(t) \), we can define:

(* Define symbolic functions and parameters *)
Clear[q, qd, tau, t, Q, R, Sa];
q  = q[t];
qd = qd[t];
tau = tau[t];

Q  = Symbol["Q"];   (* scalar weights for 1-DOF example *)
R  = Symbol["R"];
Sa = Symbol["Sa"];

Jtrack  = 1/2 Integrate[ Q  (q - qd)^2, {t, 0, T} ];
Jeffort = 1/2 Integrate[ R  tau^2,       {t, 0, T} ];
Jaccel  = 1/2 Integrate[ Sa D[q, {t, 2}]^2, {t, 0, T} ];

Jtotal  = Jtrack + Jeffort + Jaccel;

(* Compute functional derivative of Jtotal w.r.t. q(t) *)
dJdq = FunctionalD[Jtotal, q];

(* For discrete-time implementation, work numerically: *)
costDiscrete[qList_, qdList_, tauList_, Qmat_, Rmat_, SuMat_, dt_] :=
 Module[{e, JtrackN, JeffortN, dtau, JsmoothN},
  e = qList - qdList;
  JtrackN = 1/2 dt Total[ (e . Qmat) e ];
  JeffortN = 1/2 dt Total[ (tauList . Rmat) tauList ];
  dtau = Differences[tauList];
  JsmoothN = 1/2 Total[ (dtau . SuMat) dtau ];
  JtrackN + JeffortN + JsmoothN
 ]
      

The symbolic derivative dJdq can be used to study necessary conditions for optimality (Euler–Lagrange equations) in simple settings, illustrating how smoothness penalties lead to higher-order differential equations for the optimal trajectory.

Problem 1 (Nonnegativity of Quadratic Cost).

Consider the discrete-time cost

\[ J = \tfrac{1}{2}\sum_{k=0}^{N-1} \Big( e_k^\top \mathbf{Q} e_k + u_k^\top \mathbf{R} u_k \Big), \]

where \( e_k = x_k - x_k^d \), and \( \mathbf{Q},\mathbf{R} \) are symmetric positive semidefinite. Show that \( J \ge 0 \) and find a condition under which \( J = 0 \) implies \( e_k = 0 \) and \( u_k = 0 \) for all \( k \).

Solution. For each \( k \), we have \( e_k^\top \mathbf{Q} e_k \ge 0 \) and \( u_k^\top \mathbf{R} u_k \ge 0 \) because \( \mathbf{Q},\mathbf{R} \) are positive semidefinite. Therefore each summand is nonnegative, and their sum is nonnegative, so \( J \ge 0 \). If in addition \( \mathbf{Q} \succ 0 \) and \( \mathbf{R} \succ 0 \), then \( e_k^\top \mathbf{Q} e_k = 0 \) implies \( e_k = 0 \), and similarly \( u_k^\top \mathbf{R} u_k = 0 \) implies \( u_k = 0 \). Thus \( J = 0 \) implies \( e_k = 0 \) and \( u_k = 0 \) for all \( k \).

Problem 2 (Discrete Approximation of Acceleration Cost).

Let \( q_k \) be the joint position samples at times \( t_k = k\Delta t \). A central finite-difference approximation of acceleration is

\[ \ddot q_k \approx \frac{q_{k+1} - 2q_k + q_{k-1}}{\Delta t^2}. \]

Derive a discrete expression for \( J_{\text{accel}} \approx \tfrac{1}{2}\sum_{k=1}^{N-1}\ddot q_k^\top \mathbf{S}_a \ddot q_k \Delta t \) in terms of \( q_{k-1},q_k,q_{k+1} \).

Solution. Substituting the finite-difference approximation:

\[ J_{\text{accel}} \approx \tfrac{1}{2}\sum_{k=1}^{N-1} \Big( \big(\tfrac{q_{k+1} - 2q_k + q_{k-1}}{\Delta t^2}\big)^\top \mathbf{S}_a \big(\tfrac{q_{k+1} - 2q_k + q_{k-1}}{\Delta t^2}\big) \Big)\Delta t. \]

Pulling out the factor \( 1/\Delta t^4 \) from the quadratic term and multiplying by \( \Delta t \) yields:

\[ J_{\text{accel}} \approx \tfrac{1}{2}\sum_{k=1}^{N-1} \big(q_{k+1} - 2q_k + q_{k-1}\big)^\top \Big(\tfrac{\mathbf{S}_a}{\Delta t^3}\Big) \big(q_{k+1} - 2q_k + q_{k-1}\big). \]

Hence the discrete acceleration cost is a quadratic form in the second-order finite differences of \( q_k \).

Problem 3 (Weight Tuning Trade-off).

Suppose we use the discrete cost

\[ J(\lambda_{\text{effort}}) = \tfrac{1}{2}\sum_{k=0}^{N-1} \Big( e_k^\top \mathbf{Q} e_k + \lambda_{\text{effort}}\,u_k^\top \mathbf{R}u_k \Big), \]

for a fixed closed-loop control law \( u_k = Kx_k \) stabilizing the system. Qualitatively explain how increasing \( \lambda_{\text{effort}} \) affects the resulting trajectories and tracking error.

Solution. Increasing \( \lambda_{\text{effort}} \) increases the penalty on control energy relative to tracking error. An optimal design using this cost would respond by reducing the magnitude of the inputs \( u_k \). This typically leads to:

• smaller torques and smoother actuator behavior,
• slower convergence to the desired trajectory, and
• larger tracking errors, especially during transients.

Thus, \( \lambda_{\text{effort}} \) encodes a trade-off between accuracy and energy usage.

Problem 4 (Single-DOF Example).

A single joint is commanded from \( q(0) = 0 \) to \( q(T) = q_f \) in time \( T \). Consider a candidate trajectory \( q(t) = \alpha t^3 + \beta t^2 + \gamma t \) with scalar control effort approximated by \( u(t) = k\ddot q(t) \) for some constant \( k > 0 \). Using the continuous-time cost

\[ J = \tfrac{1}{2}\int_0^T \Big( q(t) - q_f \Big)^2 Q\,\mathrm{d}t + \tfrac{1}{2}\int_0^T u(t)^2 R\,\mathrm{d}t, \]

outline how you would compute \( J \) as a function of \( \alpha,\beta,\gamma \). (You do not need to carry out all integrals explicitly.)

Solution. First, compute the tracking error: \( e(t) = q(t) - q_f = \alpha t^3 + \beta t^2 + \gamma t - q_f \). Then \( e(t)^2 \) is a polynomial of degree six in \( t \). The first integral becomes

\[ \tfrac{1}{2}Q\int_0^T e(t)^2\,\mathrm{d}t, \]

which is a polynomial expression in \( \alpha,\beta,\gamma,q_f,T \). Next, compute \( \ddot q(t) = 6\alpha t + 2\beta \), so \( u(t) = k(6\alpha t + 2\beta) \), and \( u(t)^2 \) is quadratic in \( t \). The second integral becomes

\[ \tfrac{1}{2}Rk^2\int_0^T (6\alpha t + 2\beta)^2\,\mathrm{d}t, \]

which can be evaluated analytically. Adding the two terms yields \( J(\alpha,\beta,\gamma) \) as a polynomial in the parameters. Minimizing this function with respect to \( \alpha,\beta,\gamma \) gives the best cubic trajectory within this family.

In this lesson we formalized cost functionals for robot manipulators, focusing on quadratic tracking, effort, and smoothness terms. We showed how weighting matrices and scalar weights encode trade-offs between accuracy, energy usage, and motion quality, and how continuous-time cost functionals are discretized for digital control and numerical optimization. We also derived gradients and Hessians for quadratic stage costs, establishing nonnegativity and convexity properties essential for finite-horizon LQR and nonlinear optimal control algorithms, which will be developed in the following lessons.