Configuration and Velocity Constraints

---

This section introduces configuration constraints and velocity constraints in mechanical systems, focusing on holonomic and non-holonomic constraints. It also explains the role of Jacobian matrices, which relate configuration variables to velocities.

Jacobian Principle

Given configuration variables:

\[\theta = [\theta_1, \theta_2, \ldots, \theta_n]^T\]

and a vector-valued function:

\[f(\theta) = [f_1(\theta), f_2(\theta), \ldots, f_m(\theta)]^T\]

where

\[f: \mathbb{R}^n \rightarrow \mathbb{R}^m\]

For a scalar function, the Jacobian is the derivative:

\[y = f(x) \implies J = \frac{dy}{dx}\]

For vector functions, the Jacobian is a matrix of partial derivatives:

\[J = \begin{bmatrix} \frac{\partial f_1}{\partial \theta_1} & \cdots & \frac{\partial f_1}{\partial \theta_n} \\ \vdots & \ddots & \vdots \\ \frac{\partial f_m}{\partial \theta_1} & \cdots & \frac{\partial f_m}{\partial \theta_n} \end{bmatrix}\]

---

Loop Closure in Closed-Loop Mechanisms

For a closed-loop mechanism (e.g., a 4-bar linkage), the loop-closure constraint ensures the links form a closed chain:

\[L_1 e^{i\theta_1} + L_2 e^{i\theta_2} + L_3 e^{i\theta_3} + L_4 e^{i\theta_4} = 0\]

where:

• \(L_i\) is the length of the \(i\)-th link
• \(\theta_i\) is the angle of the \(i\)-th link
• \[e^{i\theta_i} = \cos(\theta_i) + i\sin(\theta_i)\]

Decomposing into real and imaginary parts (x and y components):

\[\sum_{i=1}^{n} L_i \cos(\theta_i) = d_x, \quad \sum_{i=1}^{n} L_i \sin(\theta_i) = d_y\]

If the loop closes at the origin:

\[\begin{bmatrix} \sum L_i \cos(\theta_i) - d_x \\ \sum L_i \sin(\theta_i) - d_y \end{bmatrix} = \mathbf{0}\]

These are position-level holonomic constraints:

\[g(\theta) = \begin{bmatrix} g_1(\theta_1, \ldots, \theta_n) \\ g_2(\theta_1, \ldots, \theta_n) \end{bmatrix} = \mathbf{0}\]

---

Holonomic Constraints

Holonomic constraints reduce the dimensionality of the configuration space (C-space). For \(n\) configuration variables and \(k\) independent holonomic constraints:

\[\text{DOF} = n - k\]

General form:

\[g(\theta) = \begin{bmatrix} g_1(\theta_1, \ldots, \theta_n) \\ \vdots \\ g_k(\theta_1, \ldots, \theta_n) \end{bmatrix} = 0\]

---

Velocity-Level Constraints

To analyze how constraints evolve over time, differentiate the constraint:

\[\frac{d}{dt} g(\theta(t)) = 0\]

By the chain rule:

\[\frac{d}{dt} g(\theta(t)) = \frac{\partial g}{\partial \theta} \cdot \dot{\theta} = 0\]

where:

• \(\dot{\theta}\) is the joint instantaneous velocity vector
• \(\frac{\partial g}{\partial \theta}\) is the Jacobian

This is the Pfaffian velocity constraint:

\[J(\theta) \cdot \dot{\theta} = 0\]

• Linear in \(\dot{\theta}\)
• Generally nonlinear in \(\theta\)

---

Non-Holonomic Constraints

Non-holonomic constraints restrict allowable velocities but cannot be integrated to position constraints. For example, a robot with configuration \(q = [x, y, \theta]^T\) that cannot move sideways (only forward and rotate):

\[A(q) \cdot \dot{q} = 0\]

The direction vectors:

• Forward: \(\begin{bmatrix} \cos(\theta) \\ \sin(\theta) \end{bmatrix}\)
• Sideways: \(\begin{bmatrix} -\sin(\theta) \\ \cos(\theta) \end{bmatrix}\)

No sideways motion constraint:

\[\begin{bmatrix} -\sin(\theta) & \cos(\theta) \end{bmatrix} \begin{bmatrix} \dot{x} \\ \dot{y} \end{bmatrix} = 0\]

or

\[-\sin(\theta) \dot{x} + \cos(\theta) \dot{y} = 0\]

This is a non-holonomic constraint because it cannot be integrated to a constraint on \(x, y, \theta\) alone.

---

Great! We have modelled the position constraints and velocity constraints in robotic systems. Let’s apply these concepts to a practical example involving a 4-bar linkage mechanism.

Application: 4-Bar Linkage

Suppose:

• 4 revolute joints: \(\theta_1, \theta_2, \theta_3, \theta_4\)
• Ground joint: \(\theta_4 = 0\) (fixed)
• Control input: \(\theta_1\); unknowns: \(\theta_2, \theta_3\)

Loop-closure constraint:

\[L_1 e^{i\theta_1} + L_2 e^{i\theta_2} + L_3 e^{i\theta_3} + L_4 e^{i\theta_4} = 0\]

Split into x and y components:

\[L_1 \cos(\theta_1) + L_2 \cos(\theta_2) + L_3 \cos(\theta_3) + L_4 \cos(\theta_4) = 0\] \[L_1 \sin(\theta_1) + L_2 \sin(\theta_2) + L_3 \sin(\theta_3) + L_4 \sin(\theta_4) = 0\]

Given \(\theta_1\) and \(\theta_4\), solve for \(\theta_2, \theta_3\) numerically (e.g., Newton-Raphson, Broyden’s method).

from scipy.optimize import root
import numpy as np

# Link lengths
L1, L2, L3, L4 = 1.0, 1.0, 1.0, 1.0

# Known joint angles
theta1 = np.deg2rad(30)
theta4 = 0.0

# Loop-closure constraint function
def loop_closure(vars):
    theta2, theta3 = vars
    eq1 = L1 * np.cos(theta1) + L2 * np.cos(theta2) + L3 * np.cos(theta3) + L4 * np.cos(theta4)
    eq2 = L1 * np.sin(theta1) + L2 * np.sin(theta2) + L3 * np.sin(theta3) + L4 * np.sin(theta4)
    return [eq1, eq2]

# Initial guess
initial_guess = [np.deg2rad(60), np.deg2rad(90)]
sol = root(loop_closure, initial_guess)

if not sol.success:
    raise RuntimeError(f"Solver failed: {sol.message}")

theta2_sol, theta3_sol = sol.x
print("theta2 (deg):", np.rad2deg(theta2_sol))
print("theta3 (deg):", np.rad2deg(theta3_sol))

This code numerically solves the nonlinear equations for \(\theta_2\) and \(\theta_3\) given \(\theta_1\) and \(\theta_4\).

---

Velocity Constraint Step

Given \(\theta = [\theta_1, \theta_2, \theta_3, \theta_4]^T\) and the loop-closure constraints, derive the velocity constraints:

Known velocities:

\[\dot{\theta}_1 = 1 \, \text{rad/s}, \quad \dot{\theta}_4 = 0 \, \text{rad/s}\]

Unknown velocities:

\[\dot{\theta}_2, \dot{\theta}_3\]

The velocity constraint:

\[J(\theta) \cdot \dot{\theta} = 0, \quad \dot{\theta} = \begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_2 \\ \dot{\theta}_3 \\ \dot{\theta}_4 \end{bmatrix}\]

---

Partitioning Jacobian and Velocity Vector

Partition the Jacobian and velocity vector into known (\(\dot{\theta}_1, \dot{\theta}_4\)) and unknown (\(\dot{\theta}_2, \dot{\theta}_3\)) components:

\[J(\theta) = \begin{bmatrix} J_{\theta_1} & J_{\theta_2} & J_{\theta_3} & J_{\theta_4} \end{bmatrix}\]

where

\[J_1 = \begin{bmatrix} - L_1 \sin(\theta_1) & - L_4 \sin(\theta_4) \\ L_1 \cos(\theta_1) & L_4 \cos(\theta_4) \end{bmatrix}, \quad J_2 = \begin{bmatrix} - L_2 \sin(\theta_2) & - L_3 \sin(\theta_3) \\ L_2 \cos(\theta_2) & L_3 \cos(\theta_3) \end{bmatrix}\]

• \(J_1\): columns for \(\theta_1\), \(\theta_4\) (known velocities)
• \(J_2\): columns for \(\theta_2\), \(\theta_3\) (unknown velocities)

Velocity vectors:

\[\dot{\theta}_{known} = \begin{bmatrix} \dot{\theta}_1 \\ \dot{\theta}_4 \end{bmatrix} = \begin{bmatrix} 1 \\ 0 \end{bmatrix}, \quad \dot{\theta}_{unknown} = \begin{bmatrix} \dot{\theta}_2 \\ \dot{\theta}_3 \end{bmatrix}\]

---

Solving for Unknown Velocities

The velocity constraint becomes:

\[J_1 \cdot \dot{\theta}_{known} + J_2 \cdot \dot{\theta}_{unknown} = 0\]

Substitute known velocities:

\[J_1 \begin{bmatrix} 1 \\ 0 \end{bmatrix} + J_2 \begin{bmatrix} \dot{\theta}_2 \\ \dot{\theta}_3 \end{bmatrix} = 0\]

Rearrange:

\[J_2 \cdot \dot{\theta}_{unknown} = - J_1 \begin{bmatrix} 1 \\ 0 \end{bmatrix} = \begin{bmatrix} L_1 \sin(\theta_1) \\ -L_1 \cos(\theta_1) \end{bmatrix}\]

If \(J_2\) is invertible (\(\det(J_2) \neq 0\)):

\[\dot{\theta}_{unknown} = - J_2^{-1} J_1 \dot{\theta}_{known}\]

where

\[\det(J_2) = L_2 L_3 \sin(\theta_3 - \theta_2)\]

\(J_2\) is invertible if \(\sin(\theta_3 - \theta_2) \neq 0\) (i.e., links 2 and 3 are not aligned).

---

Given the previously computed values for \(\theta_1, \theta_2, \theta_3, \theta_4\), we can solve for \(\dot{\theta}_2, \dot{\theta}_3\) as follows:

import numpy as np

# Link lengths
L1, L2, L3, L4 = 1.0, 1.0, 1.0, 1.0

# Joint angles (in radians)
theta1 = np.deg2rad(30)
theta2 = ...  # Using the solution from the previous nonlinear solve
theta3 = ...  # Using the solution from the previous nonlinear solve
theta4 = 0.0

# Known velocities
dtheta1 = 1.0  # rad/s
dtheta4 = 0.0  # rad/s

# Construct J1 and J2
J1 = np.array([
    [-L1 * np.sin(theta1), -L4 * np.sin(theta4)],
    [ L1 * np.cos(theta1),  L4 * np.cos(theta4)]
])
J2 = np.array([
    [-L2 * np.sin(theta2), -L3 * np.sin(theta3)],
    [ L2 * np.cos(theta2),  L3 * np.cos(theta3)]
])

# Known velocity vector
dtheta_known = np.array([dtheta1, dtheta4])

# Right-hand side
rhs = -J1 @ dtheta_known

# Check if J2 is invertible
if np.abs(np.linalg.det(J2)) < 1e-6:
    raise ValueError("Jacobian is nearly singular.")

# Solve for unknown velocities
dtheta_unknown = np.linalg.solve(J2, rhs)

print("dot_theta_2 (rad/s):", dtheta_unknown[0])
print("dot_theta_3 (rad/s):", dtheta_unknown[1])

---