In previous lessons, we studied singularities (loss of rank of the Jacobian), manipulability measures, condition numbers, and workspace computation. Here we focus on avoidance concepts: how to mathematically encode the idea that a manipulator should stay away from kinematic singularities, joint limits, and workspace boundaries during a motion. Importantly, we do this at the level of kinematics and trajectory design, without specifying any torque or control law.

Let \( \mathbf{q}(t) \in \mathbb{R}^n \) be the joint configuration, and let the end-effector twist be related via a Jacobian \( \mathbf{J}(\mathbf{q}) \in \mathbb{R}^{m \times n} \):

\[ \mathbf{v}(t) = \mathbf{J}(\mathbf{q}(t))\,\dot{\mathbf{q}}(t), \]

where \( \mathbf{v}(t) \) is a spatial or body twist, depending on the chosen Jacobian. A kinematic singularity at configuration \( \mathbf{q}_s \) is characterized by

\[ \operatorname{rank}(\mathbf{J}(\mathbf{q}_s)) < m \quad \Longleftrightarrow \quad \det\!\big(\mathbf{J}(\mathbf{q}_s)\mathbf{J}(\mathbf{q}_s)^{\mathsf{T}}\big) = 0. \]

The central idea of avoidance is to assign a cost or potential function that becomes large near configurations where undesirable phenomena occur (e.g., singularities, joint limits, workspace boundaries). Offline trajectory generation or path deformation can then seek joint paths that minimize this cost while satisfying kinematic constraints.

flowchart TD
  Q["Start: candidate path q(s)"] --> SING["Evaluate singularity measure w(q)"]
  SING --> WS["Check workspace / joint-limit margins"]
  WS --> COST["Compute avoidance cost U(q) along path"]
  COST --> MOD["Modify path in configuration space"]
  MOD --> CHECK["Convergence or constraints satisfied?"]
  CHECK -->|"no"| SING
  CHECK -->|"yes"| OUT["Output refined, avoidance-aware path"]
        

In this lesson we only develop metrics, potentials, and offline optimization formulations. Any mapping from these ideas to real-time control torques is intentionally deferred to later courses on robot control.

Consider an \(m \times n\) Jacobian \( \mathbf{J}(\mathbf{q}) \) with \(m \leq n\). A widely used scalar measure of proximity to singularity is the Yoshikawa manipulability index:

\[ w(\mathbf{q}) = \sqrt{\det\!\big(\mathbf{J}(\mathbf{q})\mathbf{J}(\mathbf{q})^{\mathsf{T}}\big)}. \]

We have \( w(\mathbf{q}) = 0 \) if and only if \( \mathbf{q} \) is singular. For non-singular configurations, \( w(\mathbf{q}) > 0 \) and large values correspond, roughly, to better isotropy in mapping joint velocities to task velocities.

Another useful characterization comes from the singular value decomposition (SVD) of the Jacobian:

\[ \mathbf{J}(\mathbf{q}) = \mathbf{U}(\mathbf{q})\, \boldsymbol{\Sigma}(\mathbf{q})\,\mathbf{V}(\mathbf{q})^{\mathsf{T}}, \quad \boldsymbol{\Sigma}(\mathbf{q}) = \operatorname{diag}(\sigma_1(\mathbf{q}),\dots,\sigma_m(\mathbf{q})), \]

with singular values ordered so that \( \sigma_1(\mathbf{q}) \geq \dots \geq \sigma_m(\mathbf{q}) \geq 0 \). A singularity occurs when \( \sigma_m(\mathbf{q}) = 0 \). We can define the minimum singular value distance measure

\[ d_{\text{sing}}(\mathbf{q}) = \sigma_m(\mathbf{q}), \]

and a corresponding penalty or barrier function, for example

\[ \Phi_{\text{sing}}(\mathbf{q}) = \frac{1}{\big(d_{\text{sing}}(\mathbf{q})\big)^2 + \varepsilon} = \frac{1}{\sigma_m(\mathbf{q})^2 + \varepsilon}, \quad \varepsilon > 0. \]

As \( \sigma_m(\mathbf{q}) \rightarrow 0 \), the cost diverges, disfavoring configurations that approach singularities. The parameter \( \varepsilon \) provides numerical regularization.

The condition number-based measure (for square or effectively square Jacobians) is

\[ \kappa(\mathbf{q}) = \frac{\sigma_1(\mathbf{q})}{\sigma_m(\mathbf{q})}, \]

which tends to infinity at singularities. An alternative penalty is

\[ \Phi_{\kappa}(\mathbf{q}) = \max\big(0,\,\kappa(\mathbf{q}) - \kappa_{\max}\big)^2, \]

where \( \kappa_{\max} \) is a prescribed upper bound. The robot is then encouraged to move in regions where the Jacobian is well-conditioned.

In practice, for computational efficiency, many algorithms use the manipulability index \( w(\mathbf{q}) \) or its square, both of which can be computed from \( \mathbf{J}\mathbf{J}^{\mathsf{T}} \) without a full SVD.

Besides singularities, a manipulator motion should also avoid joint limits and workspace boundaries. Let the joint space be bounded by intervals \( q_i^{-} \leq q_i \leq q_i^{+} \). Joint-limit avoidance can be expressed via a barrier-like cost

\[ \Phi_{\text{joint}}(\mathbf{q}) = \sum_{i=1}^{n} \left( \frac{1}{(q_i - q_i^{-})^2} + \frac{1}{(q_i^{+} - q_i)^2} \right), \]

which diverges as any \( q_i \) approaches its lower or upper bound.

Workspace boundaries appear in task space. For a planar 2R arm with link lengths \( \ell_1 \) and \( \ell_2 \), the reachable end-effector positions form an annulus with radial bounds

\[ r_{\min} = \big|\ell_1 - \ell_2\big|, \quad r_{\max} = \ell_1 + \ell_2, \]

where \( r = \|\mathbf{x}\|_2 \) is the distance of the end effector from the base. Given a configuration \( \mathbf{q} \), we can compute the end-effector position \( \mathbf{x}(\mathbf{q}) \) via forward kinematics and define the workspace margin

\[ d_{\text{ws}}(\mathbf{q}) = \min\big( r(\mathbf{q}) - r_{\min}, \; r_{\max} - r(\mathbf{q}) \big). \]

A simple workspace avoidance cost is then

\[ \Phi_{\text{ws}}(\mathbf{q}) = \frac{1}{d_{\text{ws}}(\mathbf{q})^2 + \varepsilon_{\text{ws}}}, \quad \varepsilon_{\text{ws}} > 0. \]

Together, these ingredients allow us to construct a total kinematic avoidance cost

\[ U(\mathbf{q}) = \alpha_{\text{sing}} \Phi_{\text{sing}}(\mathbf{q}) + \alpha_{\text{joint}} \Phi_{\text{joint}}(\mathbf{q}) + \alpha_{\text{ws}} \Phi_{\text{ws}}(\mathbf{q}), \quad \alpha_{\bullet} \geq 0, \]

where the weights \( \alpha_{\bullet} \) express the relative importance of each type of avoidance. Note that \( U(\mathbf{q}) \) is purely a kinematic function of configuration and known robot geometry.

For trajectory design or path deformation, it is useful to know the gradient of the cost with respect to configuration: \( \nabla_{\mathbf{q}} U(\mathbf{q}) \). We sketch the form of these gradients at a kinematic level.

For the manipulability index, recall \( \mathbf{M}(\mathbf{q}) = \mathbf{J}(\mathbf{q}) \mathbf{J}(\mathbf{q})^{\mathsf{T}} \) and

\[ w(\mathbf{q}) = \sqrt{\det\big(\mathbf{M}(\mathbf{q})\big)}. \]

Using matrix calculus, for each joint \( q_i \) we have

\[ \frac{\partial w}{\partial q_i} = \frac{1}{2} w(\mathbf{q})\, \operatorname{tr}\!\left( \mathbf{M}(\mathbf{q})^{-1} \frac{\partial \mathbf{M}(\mathbf{q})}{\partial q_i} \right), \]

where \( \partial \mathbf{M} / \partial q_i = (\partial \mathbf{J} / \partial q_i)\,\mathbf{J}^{\mathsf{T}} + \mathbf{J}\,(\partial \mathbf{J}^{\mathsf{T}} / \partial q_i) \). This requires Jacobian derivatives \( \partial \mathbf{J} / \partial q_i \), which can be obtained analytically from kinematic chains or approximated numerically.

For a singularity penalty \( \Phi_{\text{sing}}(\mathbf{q}) = 1 / (w(\mathbf{q})^2 + \varepsilon) \), the gradient is

\[ \frac{\partial \Phi_{\text{sing}}}{\partial q_i} = -\frac{2 w(\mathbf{q})}{\big(w(\mathbf{q})^2 + \varepsilon\big)^2} \frac{\partial w(\mathbf{q})}{\partial q_i}. \]

Joint-limit barrier gradients are simpler. For \( \Phi_{\text{joint}}(\mathbf{q}) = \sum_i \varphi_i(q_i) \) with

\[ \varphi_i(q_i) = \frac{1}{(q_i - q_i^{-})^2} + \frac{1}{(q_i^{+} - q_i)^2}, \]

we obtain

\[ \frac{\partial \varphi_i}{\partial q_i} = -\frac{2}{(q_i - q_i^{-})^3} + \frac{2}{(q_i^{+} - q_i)^3}. \]

In a trajectory design algorithm, one typically combines these gradients to obtain \( \nabla_{\mathbf{q}} U(\mathbf{q}) \), and then uses them within an optimization or path-deformation method. We stress again that at this stage we are not specifying control inputs, only shaping configuration paths.

Suppose we have a reference joint path \( \mathbf{q}^{\text{ref}}(s) \), \( s \in [0,1] \), obtained from simple inverse kinematics or by interpolating between key poses. We wish to modify this path to stay away from singularities and workspace boundaries while remaining close to the original.

Discretize the path at points \( s_k = k/N \), \( k = 0,\dots,N \), with configurations \( \mathbf{q}_k \in \mathbb{R}^n \). A typical offline shaping problem is

\[ \min_{\{\mathbf{q}_k\}} \left\{ \sum_{k=1}^{N-1} U(\mathbf{q}_k) + \lambda \sum_{k=1}^{N-1} \big\|\mathbf{q}_k - \mathbf{q}^{\text{ref}}_k\big\|_2^2 \right\} \]

subject to boundary conditions \( \mathbf{q}_0 = \mathbf{q}^{\text{ref}}_0 \), \( \mathbf{q}_N = \mathbf{q}^{\text{ref}}_N \), and possibly additional constraints (e.g., collision avoidance, velocity bounds). Here, \( \lambda > 0 \) trades off avoidance quality and path fidelity.

A simple gradient-based iteration for the interior points is

\[ \mathbf{q}_k^{(r+1)} = \mathbf{q}_k^{(r)} - \eta \left( \nabla_{\mathbf{q}} U(\mathbf{q}_k^{(r)}) + 2\lambda \big(\mathbf{q}_k^{(r)} - \mathbf{q}^{\text{ref}}_k\big) \right), \quad k = 1,\dots,N-1, \]

where \( r \) is the iteration index and \( \eta > 0 \) is a step size. This iteration is an offline numerical optimization over configuration samples, not a feedback controller in time.

More sophisticated planners (e.g., sampling-based planners, numerical continuation methods) can incorporate the same cost \( U(\mathbf{q}) \) as a bias or penalty when exploring configuration space, ensuring that feasible paths tend to remain in regions of good manipulability and safe workspace margin.

We now implement the main avoidance quantities for a planar 2R manipulator with link lengths \( \ell_1 \), \( \ell_2 \) and joint angles \( \mathbf{q} = [q_1, q_2]^{\mathsf{T}} \).

The end-effector position is

\[ \mathbf{x}(\mathbf{q}) = \begin{bmatrix} \ell_1 \cos q_1 + \ell_2 \cos(q_1 + q_2) \\ \ell_1 \sin q_1 + \ell_2 \sin(q_1 + q_2) \end{bmatrix}, \]

and the planar translational Jacobian is

\[ \mathbf{J}(\mathbf{q}) = \begin{bmatrix} -\ell_1 \sin q_1 - \ell_2 \sin(q_1 + q_2) & -\ell_2 \sin(q_1 + q_2) \\ \ell_1 \cos q_1 + \ell_2 \cos(q_1 + q_2) & \ell_2 \cos(q_1 + q_2) \end{bmatrix}. \]

We use the manipulability-based cost

\[ U_{\text{2R}}(\mathbf{q}) = \frac{1}{w(\mathbf{q})^2 + \varepsilon} + \Phi_{\text{joint}}(\mathbf{q}) + \Phi_{\text{ws}}(\mathbf{q}), \]

with \( \Phi_{\text{joint}} \) and \( \Phi_{\text{ws}} \) defined as in Sections 3 and 4.

6.1 Python (NumPy)

import numpy as np

# 2R Jacobian
def jacobian_2r(q, l1=1.0, l2=1.0):
    q1, q2 = q
    s1, c1 = np.sin(q1), np.cos(q1)
    s12, c12 = np.sin(q1 + q2), np.cos(q1 + q2)

    J = np.array([
        [-l1 * s1 - l2 * s12, -l2 * s12],
        [ l1 * c1 + l2 * c12,  l2 * c12]
    ], dtype=float)
    return J

def manipulability(J):
    JJt = J @ J.T
    return float(np.sqrt(np.linalg.det(JJt)))

def joint_limit_penalty(q, q_min, q_max):
    q = np.asarray(q, dtype=float)
    q_min = np.asarray(q_min, dtype=float)
    q_max = np.asarray(q_max, dtype=float)
    phi = 0.0
    for qi, ql, qu in zip(q, q_min, q_max):
        phi += 1.0 / (qi - ql)**2 + 1.0 / (qu - qi)**2
    return phi

def fk_2r(q, l1=1.0, l2=1.0):
    q1, q2 = q
    x = l1 * np.cos(q1) + l2 * np.cos(q1 + q2)
    y = l1 * np.sin(q1) + l2 * np.sin(q1 + q2)
    return np.array([x, y], dtype=float)

def workspace_penalty(q, l1=1.0, l2=1.0, eps_ws=1e-3):
    x = fk_2r(q, l1, l2)
    r = np.linalg.norm(x)
    r_min = abs(l1 - l2)
    r_max = l1 + l2
    d_ws = min(r - r_min, r_max - r)
    return 1.0 / (d_ws**2 + eps_ws)

def avoidance_cost(q,
                   l1=1.0, l2=1.0,
                   q_min=(-np.pi, -np.pi),
                   q_max=( np.pi,  np.pi),
                   eps=1e-3):
    J = jacobian_2r(q, l1, l2)
    w = manipulability(J)
    phi_sing = 1.0 / (w**2 + eps)
    phi_joint = joint_limit_penalty(q, q_min, q_max)
    phi_ws = workspace_penalty(q, l1, l2)
    return phi_sing + phi_joint + phi_ws

def finite_diff_grad(f, q, h=1e-4):
    q = np.asarray(q, dtype=float)
    g = np.zeros_like(q)
    for i in range(len(q)):
        dq = np.zeros_like(q)
        dq[i] = h
        g[i] = (f(q + dq) - f(q - dq)) / (2.0 * h)
    return g

def gradient_step(q, step=1e-2):
    # One offline gradient step to reduce avoidance cost
    cost_fun = lambda x: avoidance_cost(x)
    g = finite_diff_grad(cost_fun, np.array(q, dtype=float))
    return np.array(q, dtype=float) - step * g

if __name__ == "__main__":
    q = np.array([0.0, 0.5])
    for k in range(20):
        c = avoidance_cost(q)
        print(f"Iter {k}: q = {q}, cost = {c:.6f}")
        q = gradient_step(q, step=5e-2)
      

The loop illustrates purely kinematic gradient descent on \( U_{\text{2R}} \) for a single configuration, which can be extended to all discrete waypoints along a path as described in Section 5.

6.2 C++ (Eigen)

#include <iostream>
#include <Eigen/Dense>
#include <cmath>

using Eigen::Matrix2d;
using Eigen::Vector2d;

Matrix2d jacobian2R(const Vector2d& q, double l1 = 1.0, double l2 = 1.0) {
    double q1 = q(0), q2 = q(1);
    double s1  = std::sin(q1);
    double c1  = std::cos(q1);
    double s12 = std::sin(q1 + q2);
    double c12 = std::cos(q1 + q2);

    Matrix2d J;
    J(0, 0) = -l1 * s1 - l2 * s12;
    J(0, 1) = -l2 * s12;
    J(1, 0) =  l1 * c1 + l2 * c12;
    J(1, 1) =  l2 * c12;
    return J;
}

double manipulability(const Matrix2d& J) {
    Matrix2d JJt = J * J.transpose();
    return std::sqrt(JJt.determinant());
}

double jointLimitPenalty(const Vector2d& q,
                         const Vector2d& qmin,
                         const Vector2d& qmax) {
    double phi = 0.0;
    for (int i = 0; i < 2; ++i) {
        double qi = q(i);
        double ql = qmin(i);
        double qu = qmax(i);
        phi += 1.0 / std::pow(qi - ql, 2.0)
             + 1.0 / std::pow(qu - qi, 2.0);
    }
    return phi;
}

double workspacePenalty(const Vector2d& q,
                        double l1 = 1.0, double l2 = 1.0,
                        double eps_ws = 1e-3) {
    double q1 = q(0), q2 = q(1);
    double x = l1 * std::cos(q1) + l2 * std::cos(q1 + q2);
    double y = l1 * std::sin(q1) + l2 * std::sin(q1 + q2);
    double r = std::sqrt(x * x + y * y);

    double rmin = std::fabs(l1 - l2);
    double rmax = l1 + l2;
    double d_ws = std::min(r - rmin, rmax - r);
    return 1.0 / (d_ws * d_ws + eps_ws);
}

double avoidanceCost(const Vector2d& q,
                     double l1 = 1.0, double l2 = 1.0,
                     double eps = 1e-3) {
    Matrix2d J = jacobian2R(q, l1, l2);
    double w = manipulability(J);
    double phi_sing = 1.0 / (w * w + eps);

    Vector2d qmin(-M_PI, -M_PI);
    Vector2d qmax( M_PI,  M_PI);
    double phi_joint = jointLimitPenalty(q, qmin, qmax);
    double phi_ws = workspacePenalty(q, l1, l2);
    return phi_sing + phi_joint + phi_ws;
}

int main() {
    Vector2d q(0.0, 0.5);
    for (int k = 0; k < 10; ++k) {
        double c = avoidanceCost(q);
        std::cout << "q = [" << q.transpose()
                  << "], cost = " << c << std::endl;
        // Offline finite-difference gradient step could be added here
    }
    return 0;
}
      

6.3 Java

public class Avoidance2R {

    public static double[][] jacobian2R(double[] q,
                                        double l1, double l2) {
        double q1 = q[0], q2 = q[1];
        double s1  = Math.sin(q1);
        double c1  = Math.cos(q1);
        double s12 = Math.sin(q1 + q2);
        double c12 = Math.cos(q1 + q2);

        double[][] J = new double[2][2];
        J[0][0] = -l1 * s1 - l2 * s12;
        J[0][1] = -l2 * s12;
        J[1][0] =  l1 * c1 + l2 * c12;
        J[1][1] =  l2 * c12;
        return J;
    }

    public static double manipulability(double[][] J) {
        // JJ^T for 2x2 case
        double a = J[0][0] * J[0][0] + J[0][1] * J[0][1];
        double b = J[0][0] * J[1][0] + J[0][1] * J[1][1];
        double d = J[1][0] * J[1][0] + J[1][1] * J[1][1];

        double det = a * d - b * b;
        return Math.sqrt(det);
    }

    public static double jointLimitPenalty(double[] q,
                                           double[] qmin,
                                           double[] qmax) {
        double phi = 0.0;
        for (int i = 0; i < q.length; ++i) {
            double qi = q[i];
            double ql = qmin[i];
            double qu = qmax[i];
            phi += 1.0 / Math.pow(qi - ql, 2.0)
                 + 1.0 / Math.pow(qu - qi, 2.0);
        }
        return phi;
    }

    public static double workspacePenalty(double[] q,
                                          double l1, double l2,
                                          double epsWs) {
        double q1 = q[0], q2 = q[1];
        double x = l1 * Math.cos(q1) + l2 * Math.cos(q1 + q2);
        double y = l1 * Math.sin(q1) + l2 * Math.sin(q1 + q2);
        double r = Math.hypot(x, y);

        double rmin = Math.abs(l1 - l2);
        double rmax = l1 + l2;
        double dWs = Math.min(r - rmin, rmax - r);
        return 1.0 / (dWs * dWs + epsWs);
    }

    public static double avoidanceCost(double[] q,
                                       double l1, double l2,
                                       double eps) {
        double[][] J = jacobian2R(q, l1, l2);
        double w = manipulability(J);
        double phiSing = 1.0 / (w * w + eps);

        double[] qmin = new double[]{-Math.PI, -Math.PI};
        double[] qmax = new double[]{ Math.PI,  Math.PI};
        double phiJoint = jointLimitPenalty(q, qmin, qmax);
        double phiWs = workspacePenalty(q, l1, l2, 1e-3);

        return phiSing + phiJoint + phiWs;
    }

    public static void main(String[] args) {
        double[] q = new double[]{0.0, 0.5};
        for (int k = 0; k < 10; ++k) {
            double c = avoidanceCost(q, 1.0, 1.0, 1e-3);
            System.out.println("Iter " + k + ", cost = " + c);
            // Offline gradient-based update of q can be added as needed
        }
    }
}
      

6.4 MATLAB / Simulink

function cost = avoidanceCost2R(q, l1, l2, qmin, qmax, eps, eps_ws)
% q: [q1; q2]
if nargin < 2, l1 = 1.0; end
if nargin < 3, l2 = 1.0; end
if nargin < 4, qmin = [-pi; -pi]; end
if nargin < 5, qmax = [ pi;  pi]; end
if nargin < 6, eps  = 1e-3; end
if nargin < 7, eps_ws = 1e-3; end

q1 = q(1); q2 = q(2);

% Jacobian
s1  = sin(q1);  c1  = cos(q1);
s12 = sin(q1+q2); c12 = cos(q1+q2);
J = [ -l1*s1 - l2*s12, -l2*s12;
       l1*c1 + l2*c12,  l2*c12 ];

% Manipulability
JJt = J*J.';
w = sqrt(det(JJt));
phi_sing = 1/(w^2 + eps);

% Joint-limit penalty
phi_joint = 0;
for i = 1:2
    qi = q(i); ql = qmin(i); qu = qmax(i);
    phi_joint = phi_joint + 1/(qi - ql)^2 + 1/(qu - qi)^2;
end

% Workspace penalty
x = l1*cos(q1) + l2*cos(q1+q2);
y = l1*sin(q1) + l2*sin(q1+q2);
r = hypot(x, y);
rmin = abs(l1 - l2);
rmax = l1 + l2;
d_ws = min(r - rmin, rmax - r);
phi_ws = 1/(d_ws^2 + eps_ws);

cost = phi_sing + phi_joint + phi_ws;
end
      

In Simulink, this function can be wrapped in a MATLAB Function block to provide a kinematic avoidance cost output given joint angles \( \mathbf{q} \). The block can then be used in higher-level trajectory-generation diagrams (e.g., as part of an optimization loop or a path-evaluation subsystem).

6.5 Wolfram Mathematica

(* Link lengths *)
l1 = 1.0; l2 = 1.0;

(* 2R Jacobian *)
J2R[q1_, q2_] := {
  {-l1 Sin[q1] - l2 Sin[q1 + q2], -l2 Sin[q1 + q2]},
  { l1 Cos[q1] + l2 Cos[q1 + q2],  l2 Cos[q1 + q2]}
};

manipulability[q1_, q2_] := Module[{J, JJt},
  J = J2R[q1, q2];
  JJt = J . Transpose[J];
  Sqrt[Det[JJt]]
];

jointPenalty[q1_, q2_, qmin1_, qmax1_, qmin2_, qmax2_] :=
  1/(q1 - qmin1)^2 + 1/(qmax1 - q1)^2 +
  1/(q2 - qmin2)^2 + 1/(qmax2 - q2)^2;

workspacePenalty[q1_, q2_, epsws_] := Module[{x, y, r, rmin, rmax, dws},
  x = l1 Cos[q1] + l2 Cos[q1 + q2];
  y = l1 Sin[q1] + l2 Sin[q1 + q2];
  r = Sqrt[x^2 + y^2];
  rmin = Abs[l1 - l2];
  rmax = l1 + l2;
  dws = Min[r - rmin, rmax - r];
  1/(dws^2 + epsws)
];

avoidanceCost[q1_, q2_] := Module[
  {w, phiSing, phiJoint, phiWs},
  w = manipulability[q1, q2];
  phiSing = 1/(w^2 + 10^-3);
  phiJoint = jointPenalty[q1, q2, -Pi, Pi, -Pi, Pi];
  phiWs = workspacePenalty[q1, q2, 10^-3];
  phiSing + phiJoint + phiWs
];

(* Example: Plot avoidance cost over a joint-space grid *)
ContourPlot[
  avoidanceCost[q1, q2],
  {q1, -Pi, Pi}, {q2, -Pi, Pi},
  Contours -> 30,
  PlotRange -> All,
  FrameLabel -> {"q1", "q2"},
  PlotPoints -> 40
]
      

The contour plot reveals high-cost regions near singular postures and workspace boundaries, guiding selection of safer joint configurations and paths.

In this lesson, we formulated avoidance concepts for manipulator singularities, joint limits, and workspace boundaries in a purely kinematic framework. Using manipulability indices, singular values of the Jacobian, condition numbers, and geometric workspace bounds, we built scalar cost or potential functions that diverge near undesirable configurations. We derived key gradients for these potentials and embedded them in an offline trajectory-shaping formulation for joint-space paths. Finally, we implemented these avoidance metrics for a 2R arm in Python, C++, Java, MATLAB/Simulink, and Mathematica, preparing the ground for their integration into more advanced motion planning and, eventually, dynamic control schemes.