ENG-654 · Lecture 08

Path Planning for Redundant Robots

One workspace motion, many joint-space lifts

Running example: KUKA LBR iiwa 7 · null-space self motion · minimum-jerk rectangle · eight fixed-(q_3) path lifts.

The planning problem

A path in task space does not determine a unique robot motion

Geometric path\(T(s)\in SE(3)\)

Timing law\(s=s(t)\)

IK lift\(f(q(t))=T(s(t))\)

Selectionlimits · clearance · effort

For a redundant robot, planning includes choosing which point of the inverse image to use at every instant.

KUKA iiwa 7

Seven joints satisfy six independent pose constraints

\[f:\mathbb R^7\rightarrow SE(3),\qquad f(q)=T_d\]

Regular pose

\(\operatorname{rank}J=6\), so the local inverse image has dimension \(7-6=1\).

Physical meaning

The tool pose can remain fixed while the elbow and the other joints move.

The extra degree of freedom is internal motion, not extra tool motion.

Kinematic chain

Derive the iiwa Jacobian from its seven revolute axes

Compose the URDF joint transforms: \({}^0T_7(q)=\prod_{i=1}^{7}{}^{i-1}T_i(q_i)\).

Transform each local axis and origin: \(z_{i-1}={}^0R_{i-1}\hat z_i\), \(o_{i-1}=\operatorname{trans}({}^0T_{i-1})\).

Differentiate the tool point: \(\partial p_e/\partial q_i=z_{i-1}\times(p_e-o_{i-1})\).

\[J_G(q)=\begin{bmatrix}z_0\!\times(p_e-o_0)&\cdots&z_6\!\times(p_e-o_6)\\z_0&\cdots&z_6\end{bmatrix}\in\mathbb R^{6\times7}\]

This is the geometric Jacobian: \([\dot p_e^T\;\omega_e^T]^T=J_G(q)\dot q\). Each column is one joint screw written in the base frame.

Geometric versus analytical

An analytical Jacobian differentiates chosen pose coordinates

\[x=\begin{bmatrix}p\\\phi\\\theta\\\psi\end{bmatrix},\qquad \dot x=J_A(q)\dot q\]

For \(R=R_z(\psi)R_y(\theta)R_x(\phi)\), angular velocity and Euler rates obey \(\omega=E(\phi,\theta,\psi)\dot\eta\).

\[E=\begin{bmatrix}c_\psi c_\theta&-s_\psi&0\\s_\psi c_\theta&c_\psi&0\\-s_\theta&0&1\end{bmatrix},\quad J_A=\begin{bmatrix}I&0\\0&E^{-1}\end{bmatrix}J_G\]

Away from the Euler representation singularity \(\det E=\cos\theta=0\), the left factor is invertible, so \(\ker J_A=\ker J_G\). The self motion is physical; only its coordinate description changes.

Null-space derivation

Separate the primary task from all invisible joint motion

All solutions of \(J\dot q=\dot x_d\) equal one particular solution plus a homogeneous solution.

The Moore–Penrose solution \(J^+\dot x_d\) has minimum Euclidean norm.

\(N=I-J^+J\) is an orthogonal projector: \(N^2=N\), \(N^T=N\), and \(JN=0\).

\[\boxed{\dot q=J^+\dot x_d+Nz},\qquad \dot x_d=0\Rightarrow\dot q=Nz\in\ker J\]

For the regular 7R iiwa, \(\operatorname{rank}J=6\) and rank–nullity gives \(\dim\ker J=7-6=1\).

Fixed-pose experiment

Change \(q_3\); solve the other six joints to preserve the pose

The continuation begins at row 1 of assets/data/null/iiwa7_IK_q3_030deg.csv. The complete tool frame—not only its position—is constrained.

Geometry of redundancy

The slider traces one chart of the self-motion manifold

\[\mathcal M(T_d)=\{q\in\mathcal C\mid f(q)=T_d\}\]

Each point on \(\mathcal M\) is a different posture with the same tool pose.

The tangent space is \(T_q\mathcal M=\ker J(q)\).

A chosen coordinate such as \(q_3\) is only a local parameter; folds may require another chart.

Joint limits, singularities, and collisions cut the ideal curve into feasible intervals.

Secondary optimization

Choose \(z=-\alpha\nabla h\) to descend an objective without changing the task

\[\dot q=J^+\dot x_d-\alpha N(q)\nabla h(q),\qquad \dot h=\nabla h^T\dot q\]

Joint centering

\(h_L=\tfrac12\sum_i((q_i-q_{i,c})/r_i)^2\). Its gradient grows near joints with little remaining range.

Short joint motion

\(h_T=\tfrac12\|W(q-q_{ref})\|^2\), so \(\nabla h_T=W^TW(q-q_{ref})\).

Obstacle barrier

\(h_O=\sum_m\rho(d_m(q))\), with \(\rho(d)=\tfrac12(1/d-1/d_0)^2\) for \(d<d_0\).

Objective gradients

Project the gradient; then verify the finite motion

\[\nabla_q h_O=\sum_m\frac{\partial\rho}{\partial d_m}\underbrace{\frac{\partial d_m}{\partial p_m}}_{\text{surface normal}}\underbrace{\frac{\partial p_m}{\partial q}}_{J_{p_m}}\]

Compute a task-space repulsive direction at the closest robot point.

Pull it back to joint space with that point’s translational Jacobian.

Project with \(N\), integrate a small step, and recheck pose, limits, and collision.

Null-space projection is an instantaneous guarantee. Curvature of the manifold creates second-order task drift unless the pose is corrected after integration.

From geometry to motion

Parameterize every edge with a scalar minimum-jerk clock

\[p(t)=p_a+s(t)(p_b-p_a)\]

\[R(t)=R_a\exp\!\big(s(t)\log(R_a^TR_b)\big)\]

The same \(s(t)\) drives translation and quaternion slerp, so the complete pose begins and ends each edge at rest.

Calculus of variations

Perturb the trajectory and require zero first variation

\[\mathcal J[s]=\frac12\int_0^T(s^{(3)})^2dt,\qquad s_\varepsilon=s+\varepsilon\eta\]

The admissible perturbation preserves the six endpoint constraints, so \(\eta=\dot\eta=\ddot\eta=0\) at \(t=0,T\).

\[\delta\mathcal J=\left.\frac{d}{d\varepsilon}\mathcal J[s_\varepsilon]\right|_{0}=\int_0^T s^{(3)}\eta^{(3)}dt\]

We are minimizing jerk over an entire function, not differentiating a cost with respect to six coefficients yet.

Three integrations by parts

The Euler–Poisson equation is sixth order

\(\int s^{(3)}\eta^{(3)}dt=[s^{(3)}\eta^{(2)}]_0^T-\int s^{(4)}\eta^{(2)}dt\).

Repeat twice. Every boundary term vanishes because \(\eta,\dot\eta,\ddot\eta\) vanish at both ends.

\(\delta\mathcal J=-\int_0^T s^{(6)}(t)\eta(t)dt=0\) for every admissible \(\eta\).

\[\boxed{s^{(6)}(t)=0}\quad\Longrightarrow\quad s(t)=a_0+a_1t+\cdots+a_5t^5\]

The fundamental lemma of the calculus of variations forces the integrand coefficient \(s^{(6)}\) to vanish everywhere.

Boundary-value system

Write the six endpoint conditions as \(A a=b\)

\[\underbrace{\begin{bmatrix}1&0&0&0&0&0\\0&1&0&0&0&0\\0&0&2&0&0&0\\1&1&1&1&1&1\\0&1&2&3&4&5\\0&0&2&6&12&20\end{bmatrix}}_{A}\underbrace{\begin{bmatrix}a_0\\a_1\\a_2\\a_3\\a_4\\a_5\end{bmatrix}}_{a}=\underbrace{\begin{bmatrix}0\\0\\0\\1\\0\\0\end{bmatrix}}_{b}\]

Rows 1–3 impose \(s(0),s'(0),s''(0)\).

Rows 4–6 impose \(s(1),s'(1),s''(1)\).

Normalized time \(\tau=t/T\) removes powers of \(T\) from the matrix.

Solve the coefficients

The unique minimum-jerk clock is quintic

\[a=A^{-1}b=\begin{bmatrix}0&0&0&10&-15&6\end{bmatrix}^T\]

\[\boxed{s(\tau)=10\tau^3-15\tau^4+6\tau^5}\]

Velocity

\(s'(\tau)=30\tau^2(1-\tau)^2\), hence zero at both ends.

Acceleration

\(s''(\tau)=60\tau(1-\tau)(1-2\tau)\), also zero at both ends.

Physical time

\(\dot s=s'/T\), \(\ddot s=s''/T^2\), and \(\dddot s=s'''/T^3\).

Inspect the timing law

Velocity and acceleration vanish at every corner

The three bands show \(s\), \(ds/d\tau\), and the signed, unshifted \(d^2s/d\tau^2\) on their own axes.

Time discretization

The continuous curve becomes a finite sequence of pose requests

Choose \(T_e\)duration of edge \(e\)

Sample \(t_k\)\(t_k=k\Delta t\)

Evaluate \(T_k\)\(T(s(t_k))\)

Continue IKseed with \(q_{k-1}\)

\[T_k=T\!\left(10\tau_k^3-15\tau_k^4+6\tau_k^5\right),\qquad \tau_k=\frac{k\Delta t}{T_e}\]

Smaller \(\Delta t\) improves continuation reliability but increases solve and collision-check cost.

Inverse image of the path

Every path pose has infinitely many iiwa IK solutions

\[\mathcal M_k=f^{-1}(T_k)=\{q\in\mathbb R^7:f(q)=T_k\}\]

At a regular pose, \(\mathcal M_k\) is locally a one-dimensional self-motion curve.

As \(k\) advances, those curves sweep a two-dimensional solution surface over path progress and redundancy.

A robot trajectory is a continuous curve drawn on that surface.

Solving each pose independently loses connectivity. Path planning must follow the same branch from one sample to the next.

Analytical slice

Fixing \(q_3=\lambda\) turns each infinite IK family into finite branch points

\[\mathcal S_k(\lambda)=\mathcal M_k\cap\{q:q_3=\lambda\}\]

\[\bar q_{b,k}(\lambda)=\operatorname{IK}_b(T_k;q_3=\lambda),\quad b=1,\ldots,8\]

Select a redundancy value \(\lambda\).

Solve the remaining six coordinates \(\bar q=(q_1,q_2,q_4,q_5,q_6,q_7)\) analytically or by branch-preserving continuation.

Track each labeled root \(b\) across all poses; do not reseed from scratch.

Two complementary views

First inspect constant \(q_3\) paths; then inspect the complete feasibility surface

Part I · fixed slice

Set \(q_3=30^\circ\). Enumerate eight start-pose roots, separate their aspects, continue each path, and reject violations.

Part II · analytical map

Evaluate one branch over path index \(k=0\ldots1000\) and \(q_3=-90^\circ\ldots90^\circ\) in \(5^\circ\) increments.

Planning question

If no constant slice survives, can a continuous modulation \(q_3(k)\) weave through the green feasible set?

The first view produces candidate trajectories. The second exposes global feasibility before committing to one redundancy schedule.

Part I · choose the redundancy coordinate

Set \(q_3=30^\circ\), then enumerate the eight start-pose IK solutions

Each solution is continued over the same minimum-jerk pose samples with joint 3 held at exactly 30°.

Topological separation

Use aspects to keep the eight path lifts separate

With \(q_3\) fixed, remove column 3 from the iiwa Jacobian. A regular path cannot change the sign of \(\det\bar J\) without crossing a singularity.

Feasibility filter

Retain only violation-free path lifts

Each continuation is tested against physical iiwa limits, reduced-Jacobian singularity, sample continuity, and an elbow-clearance proxy obstacle.

Part II · global analytical view

Turn path progress and redundancy into a feasibility plane

\[\mathcal F_b(k,\lambda)=\begin{cases}1,&\operatorname{IK}_b(T_k;q_3=\lambda)\text{ is regular and violation free}\\0,&\text{otherwise}\end{cases}\]

Discretize the complete four-edge trajectory into indices \(k=0,\ldots,1000\).

Discretize \(q_3\in[-90^\circ,90^\circ]\) into 37 rows separated by 5°.

At every pair \((k,q_3)\), continue the selected analytical branch and test limits, regularity, and continuity.

Global feasibility map

Green cells reveal where the selected branch can exist safely

Choose one of the eight analytical branches. The 37,037 point pairs are evaluated as a branch-preserving continuation, not as unrelated IK solves.

Plan through the green set

Modulate \(q_3\) so the branch can reach the end of the path

\[q_3(k+1)\in\{q_3(k)-5^\circ,q_3(k),q_3(k)+5^\circ\}\]

\[\min_{q_3(0:1000)}\sum_k w_1|\Delta q_3|^2+w_2h(\bar q_{b,k}(q_3(k)))\quad\text{s.t. }\mathcal F_b=1\]

A horizontal green line means one constant \(q_3\) works globally. A winding green corridor requires redundancy modulation. A grey barrier certifies failure for that branch and sampled range.

Equations in motion

Animate the selected lift with the active timing and IK equations visible

The readout reports edge time \(\tau_k\), minimum-jerk progress \(s_k\), pose residual, reduced determinant, clearance, and the selected analytical aspect.

Selection after feasibility

Rank surviving paths by the objective that matters

Objective	Discrete cost	Effect
Joint travel	\(\sum_k\\|W(q_{k+1}-q_k)\\|^2\)	short, quiet joint motion
Clearance	\(\sum_k\phi(d_{min}(q_k))\)	distance from obstacles and self-collision
Conditioning	\(\sum_k1/\sigma_{min}(J(q_k))^2\)	distance from singularity
Energy proxy	\(\sum_k\\|\tau_k\\|^2\Delta t\)	reduced actuator effort

Feasibility is a hard filter; optimization chooses among the survivors.

Complete pipeline

Redundant planning couples timing, IK continuation, and validation

1 · Pathread poses and find edges

2 · Timingminimum-jerk samples

3 · Liftone continuation per IK/aspect

4 · Decidefilter, score, execute

Core idea: redundancy turns one commanded tool path into a family of joint paths. The planner’s job is to preserve the task while exploiting that family safely.