Consider an \( n \)-DOF robot manipulator with joint coordinates \( \mathbf{q} \in \mathbb{R}^n \). From previous robotics courses you already know how to derive its kinematics and dynamics. In this control-oriented course, we now emphasize that the robot does not move freely in \( \mathbb{R}^n \):

• Joint-level constraints: mechanical hard stops, allowed ranges for position, velocity, acceleration, and actuator torques or currents.
• Workspace constraints: the end-effector and links must stay inside a safe region and avoid obstacles, self-collisions, and forbidden zones.
• Contact constraints: surfaces, fixtures, or the ground impose geometric and force constraints when the robot is in contact (e.g., non-penetration, friction).

A control law must respect all of these simultaneously. A convenient high-level abstraction is to regard constraints as sets that the state and inputs must lie in:

• Configuration set \( \mathcal{C}_{\text{adm}} \subseteq \mathbb{R}^n \) (admissible joint positions).
• State set \( \mathcal{X}_{\text{adm}} \subseteq \mathbb{R}^{2n} \) (joint positions and velocities).
• Input set \( \mathcal{U}_{\text{adm}} \subseteq \mathbb{R}^n \) (admissible control inputs, e.g., motor torques).

The controller will later be designed so that \( (\mathbf{q}(t), \dot{\mathbf{q}}(t)) \in \mathcal{X}_{\text{adm}} \) and \( \mathbf{u}(t) \in \mathcal{U}_{\text{adm}} \) for all times where the system is operating.

flowchart TD
  R["Physical robot"] --> J["Joint limits & actuator bounds"]
  R --> W["Workspace & obstacle geometry"]
  R --> C["Contacts with environment"]

  J --> SJ["Joint constraint set \nin q, qdot, tau"]
  W --> SW["Workspace constraint set \nin q via x = f(q)"]
  C --> SC["Contact modes & \nforce constraints"]

  SJ --> U["Admissible state/input set X_adm, U_adm"]
  SW --> U
  SC --> U
        

This lesson focuses on how to mathematically characterize \( \mathcal{C}_{\text{adm}} \), \( \mathcal{X}_{\text{adm}} \), and \( \mathcal{U}_{\text{adm}} \) for these three sources of constraints. Later lessons will show how to design controllers that respect these sets.

Mathematically, constraints are often written as equality and inequality relations on configuration \( \mathbf{q} \), state \( \mathbf{x} := (\mathbf{q}, \dot{\mathbf{q}}) \), and control input \( \mathbf{u} \) (for a manipulator, usually \( \mathbf{u} = \boldsymbol{\tau} \), the vector of joint torques).

We distinguish:

• Equality constraints of the form \( h(\mathbf{q}, \dot{\mathbf{q}}, \mathbf{u}) = \mathbf{0} \), which must hold exactly (e.g., closed kinematic chains, rigid sticking contact).
• Inequality constraints of the form \( g(\mathbf{q}, \dot{\mathbf{q}}, \mathbf{u}) \leq \mathbf{0} \), which restrict variables to lie inside some admissible region (e.g., joint limits, torque bounds, distance to obstacles).

A general state-input constraint set can be written as

\[ \mathcal{Z}_{\text{adm}} = \left\{ (\mathbf{x}, \mathbf{u}) \in \mathbb{R}^{2n} \times \mathbb{R}^n \;\middle|\; h(\mathbf{x}, \mathbf{u}) = \mathbf{0}, \; g(\mathbf{x}, \mathbf{u}) \leq \mathbf{0} \right\}. \]

Later in the chapter we will see how equality constraints can be handled by projecting dynamics onto tangent spaces, and how inequality constraints appear as saturations and feasibility conditions in the controller. In this lesson we mainly construct \( h \) and \( g \) for three important physical sources: joints, workspace, and contacts.

For an \( n \)-DOF manipulator, denote the joint vector \( \mathbf{q} = (q_1, \dots, q_n)^\top \). Mechanical design typically provides lower and upper bounds \( q_i^{\min} \), \( q_i^{\max} \) such that joint \( i \) cannot rotate beyond these angles without collision or damage. The joint-position constraint set is

\[ \mathcal{C}_{\text{joint}} = \left\{ \mathbf{q} \in \mathbb{R}^n \;\middle|\; q_i^{\min} \leq q_i \leq q_i^{\max}, \; i = 1, \dots, n \right\}. \]

We can write these as inequality constraints:

\[ g_i^{-}(\mathbf{q}) := q_i^{\min} - q_i \leq 0, \qquad g_i^{+}(\mathbf{q}) := q_i - q_i^{\max} \leq 0. \]

Similarly, joint velocities and torques are limited:

\[ \dot{q}_i^{\min} \leq \dot{q}_i \leq \dot{q}_i^{\max}, \qquad \tau_i^{\min} \leq \tau_i \leq \tau_i^{\max}, \]

defining admissible sets \( \mathcal{V}_{\text{joint}} \subseteq \mathbb{R}^n \) and \( \mathcal{U}_{\text{joint}} \subseteq \mathbb{R}^n \), respectively.

3.1 Convexity and Compactness of Joint Constraint Sets

It is useful to know basic geometric properties of \( \mathcal{C}_{\text{joint}} \), since in later chapters we will intersect it with other sets (workspace and contact constraints).

Proposition 1. Suppose all joint bounds are finite: \( q_i^{\min} \in \mathbb{R} \), \( q_i^{\max} \in \mathbb{R} \) with \( q_i^{\min} \leq q_i^{\max} \). Then:

1. \( \mathcal{C}_{\text{joint}} \) is convex.
2. \( \mathcal{C}_{\text{joint}} \) is closed and bounded, hence compact.

Proof.

(1) Convexity: Take any \( \mathbf{q}, \mathbf{q}' \in \mathcal{C}_{\text{joint}} \) and scalar \( \alpha \in [0,1] \). Consider \( \mathbf{q}'' = \alpha \mathbf{q} + (1-\alpha)\mathbf{q}' \). Componentwise, \( q_i^{\min} \leq q_i, q_i' \leq q_i^{\max} \). Therefore

\[ q_i^{\min} \leq \alpha q_i + (1-\alpha)q_i' \leq q_i^{\max}, \quad i=1,\dots,n, \]

using the fact that the interval \( [q_i^{\min}, q_i^{\max}] \) is convex. Thus \( \mathbf{q}'' \in \mathcal{C}_{\text{joint}} \) and the set is convex.

(2) Closed and bounded: Each scalar constraint \( q_i^{\min} \leq q_i \leq q_i^{\max} \) defines a closed and bounded interval in \( \mathbb{R} \). The Cartesian product of finitely many closed and bounded intervals is closed and bounded in \( \mathbb{R}^n \). Thus \( \mathcal{C}_{\text{joint}} \) is closed and bounded; in finite dimensions, this implies compactness. □

Analogous arguments show that admissible velocity and torque sets \( \mathcal{V}_{\text{joint}} \) and \( \mathcal{U}_{\text{joint}} \) are convex polytopes. This is helpful because many control design methods under constraints (e.g., saturations, optimization-based controllers) exploit convexity.

Let \( \mathbf{x} \in \mathbb{R}^m \) denote a task-space quantity of interest, such as the Cartesian position of the end-effector. The forward kinematics map is \( \mathbf{x} = f(\mathbf{q}) \). The reachable workspace is

\[ \mathcal{W}_{\text{reach}} = \left\{ \mathbf{x} \in \mathbb{R}^m \;\middle|\; \exists \mathbf{q} \in \mathcal{C}_{\text{joint}} \text{ such that } \mathbf{x} = f(\mathbf{q}) \right\}. \]

In practice, only a subset of this workspace is allowed due to obstacles, safety margins, and self-collision constraints. Let the environment be described by implicit inequalities \( h_k(\mathbf{x}) \geq 0 \) that define free space (e.g., signed distance to obstacles). Then the admissible workspace is

\[ \mathcal{W}_{\text{adm}} = \left\{ \mathbf{x} \in \mathcal{W}_{\text{reach}} \;\middle|\; h_k(\mathbf{x}) \geq 0, \; k=1,\dots,N_{\text{obs}} \right\}. \]

To obtain constraints in joint space, we pull back these inequalities through the kinematics:

\[ g_k(\mathbf{q}) := -h_k(f(\mathbf{q})) \leq 0 \quad \Leftrightarrow \quad h_k(f(\mathbf{q})) \geq 0. \]

The configuration-space free set is then

\[ \mathcal{C}_{\text{free}} = \left\{ \mathbf{q} \in \mathcal{C}_{\text{joint}} \;\middle|\; g_k(\mathbf{q}) \leq 0, \; k=1,\dots,N_{\text{obs}} \right\}. \]

4.1 Compactness of the Reachable Workspace

We now state a simple result linking joint-space properties to workspace properties.

Proposition 2. If \( f : \mathbb{R}^n \to \mathbb{R}^m \) is continuous and \( \mathcal{C}_{\text{joint}} \) is compact, then \( \mathcal{W}_{\text{reach}} = f(\mathcal{C}_{\text{joint}}) \) is compact.

Proof. In topology, the continuous image of a compact set is compact. More concretely, any sequence \( \{\mathbf{x}_k\} \subset \mathcal{W}_{\text{reach}} \) has pre-images \( \mathbf{q}_k \in \mathcal{C}_{\text{joint}} \) with \( \mathbf{x}_k = f(\mathbf{q}_k) \). Since \( \mathcal{C}_{\text{joint}} \) is compact, there exists a convergent subsequence \( \mathbf{q}_{k_j} \to \mathbf{q}^\star \in \mathcal{C}_{\text{joint}} \). Continuity of \( f \) implies

\[ \mathbf{x}_{k_j} = f(\mathbf{q}_{k_j}) \to f(\mathbf{q}^\star) \in \mathcal{W}_{\text{reach}}. \]

Thus every sequence in \( \mathcal{W}_{\text{reach}} \) has a convergent subsequence with limit in \( \mathcal{W}_{\text{reach}} \), so the set is compact. □

From a control perspective, compactness of \( \mathcal{W}_{\text{reach}} \) and \( \mathcal{C}_{\text{free}} \) helps guarantee that safety margins can be defined and that certain optimization problems used for constrained control later in the course have solutions.

Contacts occur when a robot link or end-effector touches an environment surface. In contrast to pure workspace constraints (which simply avoid contact), contact-aware control exploits or at least respects the contact geometry and forces. Two levels are important:

• Kinematic contact constraints on positions and velocities.
• Force contact constraints on contact forces and torques.

5.1 Kinematic Contact Constraints

Let \( \mathbf{x}_c(\mathbf{q}) \in \mathbb{R}^3 \) denote the position of a contact point on the robot, and let the environment surface be described implicitly by a signed-distance function \( \phi(\mathbf{x}) \), such that \( \phi(\mathbf{x}) = 0 \) on the surface, \( \phi(\mathbf{x}) > 0 \) outside (no penetration), and \( \phi(\mathbf{x}) < 0 \) inside (penetration).

The non-penetration kinematic constraint is

\[ \phi(\mathbf{x}_c(\mathbf{q})) \geq 0. \]

In addition, when a contact is active and sticking, the contact point should have zero relative velocity normal to the surface. Let \( \mathbf{n}(\mathbf{x}_c) \) be the surface normal. With contact Jacobian \( \mathbf{J}_c(\mathbf{q}) \) mapping joint velocities to contact point velocity,

\[ \dot{\mathbf{x}}_c = \mathbf{J}_c(\mathbf{q}) \dot{\mathbf{q}}, \]

the normal-velocity constraint is

\[ \mathbf{n}(\mathbf{x}_c(\mathbf{q}))^\top \mathbf{J}_c(\mathbf{q}) \dot{\mathbf{q}} = 0 \]

for sticking contact, or \( \mathbf{n}(\mathbf{x}_c(\mathbf{q}))^\top \mathbf{J}_c(\mathbf{q}) \dot{\mathbf{q}} \geq 0 \) for separating contact (no interpenetration).

5.2 Force-Level Contact Constraints and Complementarity (Preview)

In dynamics, the manipulator equations with contact forces are often written as

\[ \mathbf{M}(\mathbf{q}) \ddot{\mathbf{q}} + \mathbf{h}(\mathbf{q}, \dot{\mathbf{q}}) = \boldsymbol{\tau} + \mathbf{J}_c(\mathbf{q})^\top \boldsymbol{\lambda}, \]

where \( \boldsymbol{\lambda} \) collects the contact forces in contact coordinates (normal and tangential components). For rigid, unilateral, frictionless contacts, a standard idealization is the complementarity set:

\[ \lambda_n \geq 0, \quad \phi(\mathbf{x}_c(\mathbf{q})) \geq 0, \quad \lambda_n \, \phi(\mathbf{x}_c(\mathbf{q})) = 0, \]

where \( \lambda_n \) is the normal component of the contact force. These relations encode:

• Contact force cannot pull: \( \lambda_n \geq 0 \).
• No penetration: \( \phi(\mathbf{x}_c(\mathbf{q})) \geq 0 \).
• Force is zero when separated and nonzero only at zero gap (contact): the product constraint.

Tangential components of \( \boldsymbol{\lambda} \) are constrained by friction laws (e.g., Coulomb friction cone), but we will revisit those in the context of force and impedance control. For now, it suffices to recognize contact as a source of state-dependent constraints on positions, velocities, and admissible forces.

flowchart TD
  Qn["Joint config q"] --> Xc["Contact point x_c(q)"]
  Xc --> GAP["Signed distance phi(x_c)"]
  GAP -->|phi > 0| SEP["No contact, lambda = 0"]
  GAP -->|phi = 0| CON["Active contact, constraints on velocity & lambda"]
  CON --> DYN["Dynamics: M(q) qddot + h(q, qdot) = tau + J_c^T lambda"]
        

We now implement basic constraint-checking routines for a simple 2-DOF planar manipulator in several languages. The arm has joint angles \( q_1, q_2 \), link lengths \( \ell_1, \ell_2 \), and end-effector position

\[ x(\mathbf{q}) = \ell_1 \cos q_1 + \ell_2 \cos(q_1 + q_2), \qquad y(\mathbf{q}) = \ell_1 \sin q_1 + \ell_2 \sin(q_1 + q_2). \]

We assume:

• Joint limits: \( q_1 \in [q_1^{\min}, q_1^{\max}] \), \( q_2 \in [q_2^{\min}, q_2^{\max}] \).
• A circular forbidden region (obstacle) of radius \( r_{\text{obs}} \) centered at \( (x_{\text{obs}}, y_{\text{obs}}) \), i.e. \( h(\mathbf{x}) := \Vert \mathbf{x} - \mathbf{x}_{\text{obs}} \Vert^2 - r_{\text{obs}}^2 \geq 0 \).
• Ground contact plane at \( y = 0 \) (non-penetration: \( y(\mathbf{q}) \geq 0 \)).

6.1 Python (with NumPy; robotics libraries mention)

In Python, one could use dedicated robotics libraries such as pinocchio or roboticstoolbox-python for kinematics and joint limits. Here we implement a minimal version from scratch using numpy.

import numpy as np

# Robot parameters
l1, l2 = 1.0, 0.7
q1_min, q1_max = -np.pi/2, np.pi/2
q2_min, q2_max = -np.pi, np.pi

# Obstacle parameters
x_obs, y_obs = 0.7, 0.3
r_obs = 0.2

def forward_kinematics(q):
    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])

def in_joint_limits(q):
    q1, q2 = q
    return (q1_min <= q1 <= q1_max) and (q2_min <= q2 <= q2_max)

def obstacle_constraint(q):
    x, y = forward_kinematics(q)
    dx = x - x_obs
    dy = y - y_obs
    # h(x) = ||x - x_obs||^2 - r_obs^2 >= 0 in free space
    return dx*dx + dy*dy - r_obs**2

def ground_constraint(q):
    # Non-penetration: y(q) >= 0
    _, y = forward_kinematics(q)
    return y  # should be >= 0

def admissible(q):
    """Check if q satisfies joint, workspace, and ground constraints."""
    if not in_joint_limits(q):
        return False
    if obstacle_constraint(q) < 0.0:
        return False
    if ground_constraint(q) < 0.0:
        return False
    return True

if __name__ == "__main__":
    q_test = np.array([0.0, 0.0])
    print("q_test admissible?", admissible(q_test))
      

6.2 C++ (Eigen-based; mention of robotics libraries)

In C++, libraries like Eigen provide linear algebra, while robotics libraries such as pinocchio or RBDL offer kinematics/dynamics. Below is a self-contained example using Eigen only.

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

using Eigen::Vector2d;

struct PlanarArm {
    double l1{1.0};
    double l2{0.7};
    double q1_min{-M_PI/2.0}, q1_max{M_PI/2.0};
    double q2_min{-M_PI},      q2_max{M_PI};
    double x_obs{0.7}, y_obs{0.3}, r_obs{0.2};

    Vector2d forwardKinematics(const Vector2d& q) const {
        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);
        return Vector2d(x, y);
    }

    bool inJointLimits(const Vector2d& q) const {
        double q1 = q(0), q2 = q(1);
        return (q1_min <= q1 && q1 <= q1_max) &&
               (q2_min <= q2 && q2 <= q2_max);
    }

    double obstacleConstraint(const Vector2d& q) const {
        Vector2d ee = forwardKinematics(q);
        double dx = ee(0) - x_obs;
        double dy = ee(1) - y_obs;
        return dx*dx + dy*dy - r_obs*r_obs; // should be >= 0
    }

    double groundConstraint(const Vector2d& q) const {
        Vector2d ee = forwardKinematics(q);
        return ee(1); // y >= 0
    }

    bool admissible(const Vector2d& q) const {
        if (!inJointLimits(q)) return false;
        if (obstacleConstraint(q) < 0.0) return false;
        if (groundConstraint(q) < 0.0) return false;
        return true;
    }
};

int main() {
    PlanarArm arm;
    Vector2d q(0.0, 0.0);
    std::cout << "Admissible? " << std::boolalpha
              << arm.admissible(q) << std::endl;
    return 0;
}
      

6.3 Java (basic implementation)

Java does not have a standard robotics library, but linear algebra packages such as EJML can be used. Here we implement simple scalar computations using arrays.

public class PlanarArm {
    double l1 = 1.0, l2 = 0.7;
    double q1Min = -Math.PI/2.0, q1Max = Math.PI/2.0;
    double q2Min = -Math.PI,      q2Max = Math.PI;
    double xObs = 0.7, yObs = 0.3, rObs = 0.2;

    public double[] forwardKinematics(double[] q) {
        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);
        return new double[]{x, y};
    }

    public boolean inJointLimits(double[] q) {
        double q1 = q[0], q2 = q[1];
        return (q1Min <= q1 && q1 <= q1Max) &&
               (q2Min <= q2 && q2 <= q2Max);
    }

    public double obstacleConstraint(double[] q) {
        double[] ee = forwardKinematics(q);
        double dx = ee[0] - xObs;
        double dy = ee[1] - yObs;
        return dx*dx + dy*dy - rObs*rObs; // >= 0 in free space
    }

    public double groundConstraint(double[] q) {
        double[] ee = forwardKinematics(q);
        return ee[1]; // y >= 0
    }

    public boolean admissible(double[] q) {
        if (!inJointLimits(q)) return false;
        if (obstacleConstraint(q) < 0.0) return false;
        if (groundConstraint(q) < 0.0) return false;
        return true;
    }

    public static void main(String[] args) {
        PlanarArm arm = new PlanarArm();
        double[] q = {0.0, 0.0};
        System.out.println("Admissible? " + arm.admissible(q));
    }
}
      

6.4 MATLAB / Simulink

In MATLAB, the Robotics System Toolbox offers functionalities such as rigidBodyTree with joint limits stored in the model. Here we implement a minimal script and indicate a simple Simulink setup.

function ok = planar_constraint_check(q)
    % q: [q1; q2]
    l1 = 1.0; l2 = 0.7;
    q1_min = -pi/2; q1_max = pi/2;
    q2_min = -pi;   q2_max = pi;
    x_obs = 0.7; y_obs = 0.3; r_obs = 0.2;

    q1 = q(1); q2 = q(2);
    % Joint limits
    if q1 < q1_min || q1 > q1_max || q2 < q2_min || q2 > q2_max
        ok = false; return;
    end

    % FK
    x = l1*cos(q1) + l2*cos(q1 + q2);
    y = l1*sin(q1) + l2*sin(q1 + q2);

    % Obstacle constraint
    dx = x - x_obs; dy = y - y_obs;
    h = dx*dx + dy*dy - r_obs^2;  % should be >= 0
    if h < 0
        ok = false; return;
    end

    % Ground constraint y >= 0
    if y < 0
        ok = false; return;
    end

    ok = true;
end
      

Simulink idea: you can implement these constraints as a subsystem that takes q as input and outputs logical flags (e.g., inJointLimits, collisionFree). This subsystem can feed into limiters or saturation blocks on reference trajectories or torques when you later implement controllers in Simulink.

6.5 Wolfram Mathematica (Symbolic Regions)

Mathematica is convenient for symbolic exploration of admissible sets using region primitives.

(* Robot parameters *)
l1 = 1.0; l2 = 0.7;
q1min = -Pi/2; q1max = Pi/2;
q2min = -Pi;   q2max = Pi;
xObs = 0.7; yObs = 0.3; rObs = 0.2;

(* Forward kinematics *)
fk[{q1_, q2_}] := {
  l1 Cos[q1] + l2 Cos[q1 + q2],
  l1 Sin[q1] + l2 Sin[q1 + q2]
};

(* Joint region *)
jointRegion = Rectangle[{q1min, q2min}, {q1max, q2max}];

(* Workspace obstacle as a disk in x-y plane *)
obstacleRegion = Disk[{xObs, yObs}, rObs];

(* Ground plane: y >= 0 *)
groundHalfSpace = ImplicitRegion[y >= 0, { {x, -2, 2}, {y, -2, 2} }];

(* Image of joint region under fk (approximate via sampling) *)
samplePoints = Flatten[
  Table[{q1, q2}, {q1, q1min, q1max, 0.05}, {q2, q2min, q2max, 0.05}],
  1
];
workspacePts = fk /@ samplePoints;

ListPlot[workspacePts,
  AspectRatio -> 1,
  PlotRange -> { {-2, 2}, {-2, 2} },
  Epilog -> {
    Red, obstacleRegion,
    Blue, Line[{ {-2, 0}, {2, 0} }]
  }
]
      

This symbolic-numeric exploration helps visualize how joint limits map to reachable workspace and how obstacles carve out forbidden regions, which is important when reasoning about feasible trajectories and contact locations.

In this lesson we elevated joint limits, workspace restrictions, and contact conditions to explicit mathematical objects: constraint sets in configuration, state, and input spaces. We saw that joint limits form convex polytopes, that workspace and environmental constraints are generally nonlinear when pulled back through forward kinematics, and that contact introduces state-dependent kinematic and force constraints, often described via complementarity. These structures are the foundation for the control strategies in the rest of this chapter: equality-constrained control via projections, inequality-constrained control via saturations and feasibility problems, and contact-aware controllers that respect non-penetration and friction laws.