Inverse-Kinematics Controllers

Forward kinematics maps the robot’s joint configuration $q$ to its task-space coordinates $x$ according to $x=f(q)$. Differentiating that map gives the Jacobian,

which tells us how joint velocities produce end-effector velocity. The forward direction is straightforward: given $q$ and $\dot q$, compute $x$ and $\dot x$. Control asks the inverse question: given the Cartesian motion we want, what should the joints do?

The following section focuses on purely kinematic controllers, which rely on the robot’s geometry without modelling its dynamics. They assume that a low-level joint servo tracks the commanded position or velocity. This interface is common in industrial robots, making inverse kinematics a natural starting point for Cartesian control.

The setup

Consider a two-link planar arm. Its forward-kinematics function $f$ maps the joint angles $q$ to the end-effector position $x$, so $x=f(q)$, with

An IK controller maps a task-space command to a joint-space command. Two complementary forms are widely used:

• Position form. Given a target $x_d$, solve the full nonlinear equation $f(q_d) = x_d$ for a joint target $q_d$. This is batch nonlinear IK (Section 2).
• Velocity form. Given a desired Cartesian velocity $\dot x_d$ (and optionally a position error), solve the linearised relation $\dot x = J(q)\,\dot q$ at every control tick. This is differential IK (Section 3).

The two forms serve different roles. Batch IK finds complete joint configurations for motion planning, while differential IK continuously converts Cartesian motion commands into joint velocities for tasks such as teleoperation, visual servoing, and closed-loop tracking.

Batch nonlinear IK

Batch IK solves $f(q_d) = x_d$ for a complete joint configuration, typically before execution. For the 2-link planar arm, there is a closed-form solution, whereas general 6- or 7-DOF robots are often handled with numerical solvers. A numerical solver searches for a configuration $q$ that reaches the target pose while respecting joint limits and, when multiple solutions exist, favoring a preferred posture.

This is the form many motion-planning systems need: an IK solver that turns a desired end-effector pose into one or more feasible joint configurations. Before execution, a planner can test reachability, choose among those configurations, and reject solutions that collide or violate other planning requirements.

Once a joint target $q_d$ has been obtained, the simplest controller is joint-space PD with gravity compensation:

After $q_d$ is computed for the target pose, the joint servo drives the arm toward that configuration. This combination of a single batch IK solution and joint-space feedback has two important limitations:

• The end-effector path is determined indirectly by the joint-space controller. A path that is straight in joint space is generally curved in task space.
• The stiffness felt at the end-effector is not set directly by the joint-space gain. It varies with configuration and direction, so a simple constant joint-space gain matrix cannot produce the same Cartesian stiffness in every direction throughout the workspace.

Differential IK addresses the path limitation by repeatedly computing joint velocities from the desired Cartesian velocity and the current position error. It does not, by itself, address the Cartesian-stiffness limitation; that requires a controller that explicitly regulates task-space forces or dynamics.

Differential IK

Differential IK approaches the problem locally rather than solving directly for a complete target joint configuration. At every control tick, it solves the local relation

for a joint velocity that produces the desired Cartesian velocity. Because this inversion is performed during motion, differential IK must handle singularities in real time: near a singularity, producing motion in some Cartesian directions can require extremely large joint velocities. Batch IK can also become ill-conditioned near singularities, but a batch solver can apply damping, restart from a different initial guess, choose another solution branch, or reject the target before motion begins. Differential IK has to return a joint command immediately.

Differential IK with an undamped inverse

The simplest differential IK method uses an undamped inverse of the current Jacobian. This is a form of resolved-rate control, following Whitney's formulation [Whitney, 1969]D. E. Whitney, “Resolved Motion Rate Control of Manipulators and Human Prostheses,” IEEE Transactions on Man-Machine Systems, 10(2), 47–53, 1969.: the controller “resolves” a desired task-space velocity into joint velocities at each control tick. For Euclidean task coordinates, add proportional feedback:

Here $J^\#$ is $J^{-1}$ when $J$ is square and nonsingular, or an appropriate generalized inverse such as the Moore–Penrose pseudoinverse $J^+$ otherwise.^{1For a redundant manipulator, the dynamically consistent inverse is another option. It selects the feasible joint velocity that minimizes instantaneous kinetic energy. This inverse is discussed in Part 4.} If the task is feasible, $J$ has full row rank, the inner velocity servo tracks perfectly, and no limits are active, then $JJ^\#=I$ and the closed-loop task dynamics are $\dot x = \dot x_d + K_p(x_d-x)$. For a fixed target and $K_p=kI$, this ideal model follows the straight line from the initial position to the target.

For a full pose $T_d$, replace the Euclidean difference $x_d-f(q)$ with a six-dimensional pose error $e(T,T_d)$ and use $V^\star=V_d+K e$.

An undamped inverse becomes problematic as $J$ becomes ill-conditioned. Near a singularity — for example, when the arm is nearly fully stretched out — the controller asks for joint velocities that grow without bound.

Behavior of an undamped inverse near a singularity

The singular value decomposition factors the Jacobian as $J=U\Sigma V^\top$. Its singular values $\sigma_i$ measure how strongly joint motion produces motion along the corresponding task-space directions. The Moore–Penrose pseudoinverse is

Thus, a desired velocity along the task-space direction $u_i$ produces a joint-velocity component scaled by $1/\sigma_i$ along $v_i$. As the arm approaches a singularity, $\sigma_{\min}\to 0$ and $1/\sigma_{\min}\to\infty$. If the requested motion points along that weak direction — the short axis of the velocity ellipsoid — even a tiny Cartesian velocity requires enormous joint velocity. A usable controller should reduce motion in poorly conditioned directions rather than generate unbounded joint commands.

Damped least squares (DLS): limiting amplification near singularities

Damped least-squares replaces exact inversion with a regularised least-squares problem. First define the desired task-space velocity

Then choose the joint velocity that trades off Cartesian tracking error against joint-speed magnitude:

For every $\lambda>0$, this damped inverse is defined even when $J$ loses rank.

This produces two useful limiting behaviours:

• When $\sigma_i \gg \lambda$, the gain is approximately $1/\sigma_i$, so well-conditioned directions behave like ordinary resolved-rate control.
• When $\sigma_i \ll \lambda$, the gain is approximately $\sigma_i/\lambda^2$, which goes to zero as $\sigma_i\to 0$. The controller gives up on the singular direction instead of demanding infinite joint speed.

In practice, damping may be set to a small constant $\lambda$ or increased only near singularities. One possible trigger is Yoshikawa's manipulability measure $w = \sqrt{\det(JJ^\top)}$ [Yoshikawa, 1985]T. Yoshikawa, “Manipulability of Robotic Mechanisms,” The International Journal of Robotics Research, 4(2), 3–9, 1985..

Damping addresses singular-value amplification, but it does not enforce joint-position, velocity, or acceleration limits, avoid collisions, or choose a preferred redundant posture. To handle these requirements, a differential-IK controller can combine damping with dedicated constraint handling, or formulate each update as a constrained least-squares or quadratic program.

Comparison of the undamped pseudoinverse and DLS

The contrast is easiest to see at the edge of the workspace. This comparison considers end-effector position only. Both controllers use the UR5e's translational Jacobian $J_p$ and a target just outside the reachable workspace. Both trajectories use the same Cartesian gain and joint constraints. The only algorithmic difference is the inverse: the undamped Moore–Penrose pseudoinverse $J_p^+$ versus DLS with $\lambda=0.05$.

Summary

• Use batch IK + joint PD for discrete point-to-point moves when a planner can choose and validate a feasible configuration before execution.
• For teleoperation on a position- or velocity-controlled arm, constrained differential IK is often a good choice. DLS provides a useful foundation, but a deployable controller also needs joint and Cartesian rate limits, joint-limit handling, collision handling where required, and a defined policy for infeasible commands.
• On a torque-controlled arm, operational-space control, Cartesian impedance, and joint impedance are all possible; the right choice depends on the tracking and interaction requirements. Part 2 develops the task-space torque mapping.
• For contact tasks, kinematic IK alone does not specify the contact force. A position servo can sustain excessive force against an obstruction until torque or safety limits intervene.

A note on gravity compensation

The controllers in this part do not compute torques from dynamics. They produce $q_d$ or $\dot q_d$ and rely on an inner joint servo to execute those commands. If gravity compensation is present, it usually lives inside that servo or the robot's SDK, not inside the IK algorithm.

Gravity compensation is important because gravity continuously loads the joints. Without feedforward, the position loop must counteract gravity through feedback alone, which often requires higher gains and leaves configuration-dependent steady-state error. A model-based $g(q)$ term cancels most of that predictable load, so the servo can use lower gains and spend its effort tracking changes rather than fighting the arm's own weight.

On geared industrial arms, this compensation is approximate: gear friction, transmission losses, and payload uncertainty mean the commanded torque is not the same as the delivered joint torque. Nevertheless, approximate gravity compensation combined with a moderate-gain joint servo is a common control architecture. Part 2 makes the dynamics explicit by deriving the torque-control law. At that stage, $M(q)$, $C(q,\dot q)$, $g(q)$, and task-space inertia enter the controller explicitly instead of being handled by the inner joint servo.

References

• D. E. Whitney, “Resolved Motion Rate Control of Manipulators and Human Prostheses”, IEEE Transactions on Man-Machine Systems, vol. 10, no. 2, pp. 47–53, 1969.
• T. Yoshikawa, “Manipulability of Robotic Mechanisms”, The International Journal of Robotics Research, vol. 4, no. 2, pp. 3–9, 1985.