Robotics II

L11. Task-space position control of robotic manipulators

Open virtual laboratory notebook

PhD Eng. Paweł Kwiatoń

Lab Objective

During the laboratory students:

• implement task-space position control of a robotic manipulator,
• compute end-effector position error in Cartesian space,
• use the manipulator Jacobian to map task-space commands into joint-space motion,
• compare joint-space and task-space control behavior,
• analyze convergence, singularity effects, and sensitivity to controller gains.

Joint-space vs task-space control

Joint-space control	Task-space control
controls joint variables directly	controls end-effector position directly
reference is defined as (q_d)	reference is defined as (x_d)
simple implementation	more intuitive for many tasks
indirect end-effector behavior	requires Jacobian-based mapping

End-effector position control problem

Let the end-effector position be:

$$ x = f(q) $$

The control goal is:

$$ x \rightarrow x_d $$

Define the task-space error:

$$ e = x_d - x $$

The objective is to design a control law that reduces this error to zero.

Differential kinematics

The end-effector velocity is related to joint velocity by:

$$ \dot{x} = J(q)\dot{q} $$

where (J(q)) is the manipulator Jacobian.

If a desired Cartesian velocity (\dot{x}_d) is given, joint velocity can be computed from:

$$ \dot{q} = J^{+}(q)\dot{x}_d $$

where (J^{+}) is the Jacobian pseudoinverse.

Resolved-rate task-space control

One of the most important task-space control strategies is resolved-rate control.

A basic control law is:

$$ \dot{x}_c = K e $$

$$ \dot{q} = J^{+}(q)\dot{x}_c $$

where:

• (K) – diagonal gain matrix,
• (e) – Cartesian position error.

This controller iteratively drives the end-effector toward the target point.

Damped least-squares inverse

Near singularities the pseudoinverse may become numerically unstable.

A common alternative is damped least-squares:

$$ J^{\#} = J^T \left(JJ^T + \lambda^2 I\right)^{-1} $$

Advantages:

• improved numerical robustness,
• reduced sensitivity near singular configurations,
• smoother motion close to poorly conditioned Jacobians.

import numpy as np
import matplotlib.pyplot as plt
from matplotlib.animation import FuncAnimation
from IPython.display import HTML

plt.rcParams["figure.figsize"] = (7, 5)
plt.rcParams["axes.grid"] = True
plt.rcParams["font.size"] = 11

l = np.array([1.0, 0.8, 0.6])

def fk_points(q, l):
    q1, q2, q3 = q
    l1, l2, l3 = l
    p0 = np.array([0.0, 0.0])
    p1 = np.array([l1 * np.cos(q1), l1 * np.sin(q1)])
    p2 = p1 + np.array([l2 * np.cos(q1 + q2), l2 * np.sin(q1 + q2)])
    p3 = p2 + np.array([l3 * np.cos(q1 + q2 + q3), l3 * np.sin(q1 + q2 + q3)])
    return np.array([p0, p1, p2, p3])

def fk(q, l):
    return fk_points(q, l)[-1]

def jacobian(q, l):
    q1, q2, q3 = q
    l1, l2, l3 = l
    s1 = np.sin(q1)
    s12 = np.sin(q1 + q2)
    s123 = np.sin(q1 + q2 + q3)
    c1 = np.cos(q1)
    c12 = np.cos(q1 + q2)
    c123 = np.cos(q1 + q2 + q3)

    j11 = -l1 * s1 - l2 * s12 - l3 * s123
    j12 = -l2 * s12 - l3 * s123
    j13 = -l3 * s123
    j21 =  l1 * c1 + l2 * c12 + l3 * c123
    j22 =  l2 * c12 + l3 * c123
    j23 =  l3 * c123

    return np.array([[j11, j12, j13],
                     [j21, j22, j23]])

def damped_pinv(J, lam):
    return J.T @ np.linalg.inv(J @ J.T + (lam ** 2) * np.eye(J.shape[0]))

Manipulator model used in the laboratory

In the main numerical experiments a planar 3DOF manipulator is used.

Task: 2D end-effector position control

This gives:

$$ J \in \mathbb{{R}}^{{2 \times 3}} $$

The manipulator is kinematically redundant for planar position control, which allows:

• multiple joint solutions,
• smoother convergence,
• additional experiments with secondary criteria.

q0 = np.array([0.5, -0.8, 0.4])
xd = np.array([1.4, 0.9])

pts = fk_points(q0, l)

plt.figure(figsize=(6, 6))
plt.plot(pts[:, 0], pts[:, 1], "o-", linewidth=3, label="Manipulator")
plt.scatter([0], [0], c="black", s=60, label="Base")
plt.scatter(xd[0], xd[1], c="red", s=80, label="Target")
plt.gca().set_aspect("equal")
plt.xlim(-2.6, 2.6)
plt.ylim(-2.6, 2.6)
plt.title("Planar 3DOF manipulator model")
plt.xlabel("x")
plt.ylabel("y")
plt.legend()
plt.show()

Basic task-space control loop

Control loop:

• compute current end-effector position,
• compute task-space error,
• compute desired Cartesian velocity,
• map task command to joint velocity using the Jacobian,
• integrate joint motion,
• repeat.

This structure is simple, effective, and fundamental in modern robotics.

def simulate_task_space(q_init, xd, l, dt=0.02, steps=500, K_gain=2.0):
    q = q_init.copy()
    K = np.diag([K_gain, K_gain])

    q_hist = []
    x_hist = []
    e_hist = []

    for _ in range(steps):
        x = fk(q, l)
        J = jacobian(q, l)
        e = xd - x
        xdot_cmd = K @ e
        qdot = np.linalg.pinv(J) @ xdot_cmd
        q = q + qdot * dt

        q_hist.append(q.copy())
        x_hist.append(x.copy())
        e_hist.append(np.linalg.norm(e))

    return np.array(q_hist), np.array(x_hist), np.array(e_hist)

q_hist, x_hist, e_hist = simulate_task_space(q0, xd, l)

plt.figure(figsize=(7, 6))
plt.plot(x_hist[:, 0], x_hist[:, 1], label="End-effector trajectory", linewidth=2)
plt.scatter(x_hist[0, 0], x_hist[0, 1], c="green", s=70, label="Start")
plt.plot(xd[0], xd[1], "ro", label="Target")
plt.axis("equal")
plt.title("Basic task-space position control")
plt.xlabel("x")
plt.ylabel("y")
plt.legend()
plt.show()

plt.figure(figsize=(8, 4))
plt.plot(e_hist, linewidth=2)
plt.xlabel("Step")
plt.ylabel(r"Task-space error norm $\|e\|$")
plt.title("Error norm over time")
plt.show()

plt.figure(figsize=(8, 4))
plt.plot(q_hist[:, 0], label="q1")
plt.plot(q_hist[:, 1], label="q2")
plt.plot(q_hist[:, 2], label="q3")
plt.xlabel("Step")
plt.ylabel("Joint position")
plt.title("Joint motion")
plt.legend()
plt.show()

Singularity and numerical robustness

When the manipulator approaches singularity:

• small Cartesian commands may require large joint velocities,
• control may become unstable,
• numerical noise may grow.

Students should compare:

• pseudoinverse control,
• damped least-squares control,

and analyze the effect of damping.

Task-space error analysis

The controller should be evaluated using:

• final Cartesian error,
• error norm over time,
• convergence rate,
• joint motion amplitude,
• behavior near singularity.

These measures provide an objective basis for controller comparison.

def simulate_comparison(q0, xd, l, use_damping=False, lam=0.15, dt=0.02, steps=500):
    q = q0.copy()
    K = np.diag([2.0, 2.0])

    x_hist = []
    e_hist = []
    qdot_hist = []

    for _ in range(steps):
        x = fk(q, l)
        J = jacobian(q, l)
        e = xd - x
        xdot_cmd = K @ e

        if use_damping:
            qdot = damped_pinv(J, lam) @ xdot_cmd
        else:
            qdot = np.linalg.pinv(J) @ xdot_cmd

        q = q + qdot * dt

        x_hist.append(x.copy())
        e_hist.append(np.linalg.norm(e))
        qdot_hist.append(np.linalg.norm(qdot))

    return np.array(x_hist), np.array(e_hist), np.array(qdot_hist)

q0_cmp = np.array([0.0, 0.04, -0.03])
xd_cmp = np.array([1.9, 0.15])

traj1, err1, vel1 = simulate_comparison(q0_cmp, xd_cmp, l, use_damping=False)
traj2, err2, vel2 = simulate_comparison(q0_cmp, xd_cmp, l, use_damping=True, lam=0.15)

plt.figure(figsize=(7, 6))
plt.plot(traj1[:, 0], traj1[:, 1], label="Pseudoinverse", linewidth=2)
plt.plot(traj2[:, 0], traj2[:, 1], label="Damped least-squares", linewidth=2)
plt.plot(xd_cmp[0], xd_cmp[1], "ro", label="Target")
plt.axis("equal")
plt.title("Controller comparison in workspace")
plt.xlabel("x")
plt.ylabel("y")
plt.legend()
plt.show()

plt.figure(figsize=(8, 4))
plt.plot(err1, label="Pseudoinverse", linewidth=2)
plt.plot(err2, label="Damped least-squares", linewidth=2)
plt.xlabel("Step")
plt.ylabel(r"Task-space error norm $\|e\|$")
plt.title("Error comparison")
plt.legend()
plt.show()

plt.figure(figsize=(8, 4))
plt.plot(vel1, label="Pseudoinverse", linewidth=2)
plt.plot(vel2, label="Damped least-squares", linewidth=2)
plt.xlabel("Step")
plt.ylabel(r"Joint velocity norm $\|\dot{q}\|$")
plt.title("Joint velocity demand comparison")
plt.legend()
plt.show()

print("Final error pseudoinverse:", err1[-1])
print("Final error damped least-squares:", err2[-1])

Final error pseudoinverse: 3.217346800098821e-10
Final error damped least-squares: 2.580373096579332e-09

Animation

The animation below shows the manipulator moving toward the target point.

The original desktop script was adapted so that the animation is embedded directly in the notebook and works in Colab/Jupyter.

q_anim = np.array([0.4, -0.7, 0.5])
xd_anim = np.array([1.5, 0.7])

dt = 0.03
steps = 250
K = np.diag([2.0, 2.0])

q_hist_anim = []

for i in range(steps):
    x = fk(q_anim, l)
    J = jacobian(q_anim, l)
    e = xd_anim - x

    if np.linalg.norm(e) < 1e-3 and i > 20:
        q_hist_anim.append(q_anim.copy())
        break

    qdot = np.linalg.pinv(J) @ (K @ e)
    q_anim = q_anim + qdot * dt
    q_hist_anim.append(q_anim.copy())

q_hist_anim = np.array(q_hist_anim)

trace_points = []

fig, ax = plt.subplots(figsize=(6, 6))
ax.set_xlim(-2.6, 2.6)
ax.set_ylim(-2.6, 2.6)
ax.set_aspect("equal")
ax.grid(True)
ax.set_title("Task-space position control animation")

line, = ax.plot([], [], "o-", linewidth=3, label="Manipulator")
trace, = ax.plot([], [], linewidth=1.5, label="End-effector path")
ax.plot(xd_anim[0], xd_anim[1], "ro", label="Target")
ax.legend()

def init():
    line.set_data([], [])
    trace.set_data([], [])
    return line, trace

def update(frame):
    pts = fk_points(q_hist_anim[frame], l)
    trace_points.append(pts[-1])
    tr = np.array(trace_points)
    line.set_data(pts[:, 0], pts[:, 1])
    trace.set_data(tr[:, 0], tr[:, 1])
    return line, trace

ani = FuncAnimation(fig, update, frames=len(q_hist_anim), init_func=init, interval=40, blit=False)
plt.close(fig)
HTML(ani.to_jshtml())

Once Loop Reflect

Laboratory task

Implement task-space position control for a planar manipulator.

Required elements:

• forward kinematics,
• Jacobian computation,
• Cartesian position error,
• Jacobian pseudoinverse control,
• trajectory visualization in workspace,
• error analysis.

Students should verify whether the end-effector reaches the target point from different initial configurations.

Project task

Design and evaluate a task-space controller for a redundant planar manipulator.

The project should include:

• at least two control variants,
• standard pseudoinverse control,
• damped least-squares control or an additional secondary criterion,
• comparative analysis of convergence,
• experiments near singular configurations,
• interpretation of numerical stability and motion quality.

Students should justify which controller is more robust and under what conditions.

Extended experiment

Investigate the influence of:

• proportional gain values,
• damping parameter,
• initial configuration,
• target point location,
• proximity to singularity.

Analyze the effect on:

• convergence speed,
• final error,
• joint velocity magnitude,
• smoothness of motion.

Report

The report should include:

• description of the task-space control objective,
• manipulator model and Jacobian,
• implemented control law,
• comparison of inverse Jacobian variants,
• workspace plots and error plots,
• interpretation of controller performance.

Summary

• task-space position control of a manipulator was implemented,
• Cartesian error was used as the control signal,
• the Jacobian pseudoinverse mapped task commands into joint motion,
• damped least-squares control improved robustness near singularities,
• controller performance was evaluated experimentally.

Thank you!