Kinematics via Optimization

MIT 6.4210/2: Robotic Manipulation

Fall 2026, Lecture 5

Kinematics

  • Forward kinematics:    joint positions \(\Rightarrow\) pose
  • Inverse kinematics*:      pose \(\Rightarrow\) joint positions

 

  • Differential kinematics:
          joint positions, velocities \(\Rightarrow\) spatial velocity
     
  • Differential inverse kinematics:
          spatial velocity, joint positions \(\Rightarrow\) joint velocities

 

q \Rightarrow X^B
X^B \Rightarrow q
q, v \Rightarrow V^B
V^B, q \Rightarrow v

Basic Pick and Place

Step 1: Kinematic Frames / Spatial Algebra

Step 2: Gripper Frame Plan "Sketch"

Step 3: Forward kinematics of the iiwa + WSG

Step 4: Differential Inverse Kinematics

Kinematic Jacobian:  \( {}^WV^G = J^{G}(q) v \)

\( v = [J^{G}(q)]^{+} {}^WV^G \)

spatial velocity

generalized velocity

Scalar case - equality

Scalar case - approximately

The matrix case

Level sets of cost

prog = MathematicalProgram()
x = prog.NewContinuousVariables(2)
prog.AddConstraint(x[0] + x[1] == 1)
prog.AddConstraint(x[0] <= x[1])
prog.AddCost(x[0] ** 2 + x[1] ** 2)
result = Solve(prog)

Clipping vs constraints

Joint centering in null space of J

X^B = f_{kin}^B(q)

Charles W. Wampler and Andrew J. Sommese. Numerical algebraic geometry and algebraic kinematics. Acta Numerica, 20:469-567, 2011.

Inverse kinematics (a rich history!)

Inverse kinematics as an optimization

\min_q | q-q_{desired}|

subject to:

  • rich end-effector constraints
  • joint limits
  • collision avoidance
  • "gaze constraints"
  • "feet stay put"
  • balance (center of mass)
  • ...
        ik = InverseKinematics(plant, plant_context)
        ik.AddPositionConstraint(
            gripper_frame,
            [0, 0, 0],
            plant.world_frame(),
            pose.translation(),
            pose.translation(),
        )
        ik.AddOrientationConstraint(
            gripper_frame,
            RotationMatrix(),
            plant.world_frame(),
            pose.rotation(),
            0.0,
        )
        prog = ik.get_mutable_prog()
        q = ik.q()
        prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)
        prog.SetInitialGuess(q, q0)
        result = Solve(ik.prog())

Configuration space

One grasp - three solutions

Motion planning as a (nonconvex) optimization

\begin{aligned} \min_{q_0, ..., q_N} \quad & \sum_{n=0}^{N-1} | q_{n+1} - q_n|_2^2 & \\ \text{subject to} \quad & q_0 = q_{start} \\ & q_N = q_{goal} \\ & |q_n|_1 \ge 1 & \forall n \end{aligned}

start

goal

fixed number of samples

collision-avoidance

(outside the \(L^1\) ball)

nonconvex

Initial trajectory guess

Optimized trajectory

Motivation: Moving fast! (at the dynamic limits)

"Using trajectory optimization made scooping up to twice as fast"

credit:

Cubic B-spline

Failure mode