Drake NLP Inverse Kinematics Notes

NLP (Nonlinear Programming)

NLP := subfield of mathematical optimization where you try to find a set of variables (in our case, the robot's joint angles $q$) that minimize some objective cost function, subject to a set of constraints (where at least one of those equations is nonlinear).

• ❓ Why is IK a nonlinear optimization problem?
  • → because a robot's forward kinematics relies heavily on trigonometry (sines & cosines of joint angles multiplied through a kinematic chain) → so the relationship between a joint angle and the end-effector's position is inherently nonlinear.

IK written as an NLP

• Find: $q$ (joint angles)
• To minimize: effort, distance from rest pose, total movement, ...
• Subject to: [constraints]
  • $g_1(q) = 0$ (the gripper must be exactly at target XYZ)
  • $g_2(q) \leq 0$ (the camera must be inside the FOV cone)
  • $g_3(q) > 0$ (the distance between link A and B must be > 0 to avoid collision)

Multi-start

Nonlinear programming solvers are local optimizers: they act like a hiker dropped onto a mountain in the fog, using calculus (gradients, Jacobians) to feel the slope of the math and walk downhill until they hit a valley (a local minimum).

• ⚠️ If the problem is highly complex: there are many valleys! If you drop the solver in a bad starting position, it will get stuck in a "local minimum"
• 🔑 ^ Solution: try many starting positions!

Multi-start:

1. Generate $N$ random different initial joint configurations (the "seeds")
2. Start $N$ independent NLP solvers, one for each seed
3. If any of them converge on a valid pose that passes all constraints, return and declare success.

SNOPT (Sparse Nonlinear OPTimizer)

SNOPT is a commercial-grade Fortran package used to solve NLP problems - this is what Drake uses!

• Uses an algorithm called SQP: Sequential Quadratic Programming
• 💡 Core idea: SNOPT takes the current
• Advantages:
• Disadvantages:

Sequential Quadratic Programming

SQP is one of the main algorithms for solving constrained non-linear optimization problems.

• 💡 Core intuition: Newton's method!
  • If you want to find the bottom of a curved valley (a minimum), Newton's Method fits a parabola (quadratic bowl) to the ground right where you are standing. It calculates the exact bottom of that bowl, you jump to that spot, then you fit a new bowl.
  • ^ SQP does exactly this, but handles constraints:
• 🔖 At every step, SQP:
  1. Calculates the gradient (slope) and Hessian (curvature) of the objective
  2. Constructs a quadratic approximation of the objective (a bowl)
  3. Constructs a linear approx of the constraints (flat walls)
  4. Solves this QP to find the best step to take
  5. Takes the step, updates the approximations, and repeats until convergence

Concrete example

Let's work through a simple example.

• Objective: Move as far "up and right" as possible: minimize $f(x,y) = -x-y$
• Constraint: You are tethered to a pole by a 1m cable. You must stay exactly on the edge of the circle - $$g(x,y) = x^2 + y^2 - 1 = 0$$

1. SQP uses the Lagrangian to glue the objective with the constraint: $$\mathcal{L}(x, y, \lambda) = f(x,y) + \lambda g(x,y) = -x - y + \lambda(x^2 + y^2 - 1)$$(Read about the Lagrangian here: The Lagrangian (Calculus))
2. Initialize our guess: Let's drop the solver at a starting point & have an initial multiplier guess - $$(x_0, y_0) = (1,0), \quad \lambda_0 = 1$$
3. Build the approximations: To build the quadratic program for this step, the solver needs the slopes and curvatures at $(1,0)$:
   • Gradient of the objective: $\nabla f = \begin{bmatrix} -1 \\ -1 \end{bmatrix}$
   • Jacobian of the constraint (linearization):
   • Hessian of the Lagrangian (curvature):
4. Solve the quadratic subproblem: SQP now asks, "what step $p = [p_x, p_y]$ should I take?" It sets up the following simple QP to solve for $p$:
   • Minimize the quadratic objective: $$\frac{1}{2} p^\top H p + \nabla f^\top p = \frac{1}{2} (2 p_x^2 + 2p_y^2) - p_x - p_y = p_x^2 + p_y^2 - p_x - p_y$$ subject to the linearized constraint: [CONTINUE FROM HERE brain dead]

IPOPT (Interior Point OPTimizer)

IPOPT is an open-source rival to SNOPT and also widely used in robotics.

• Instead of SQP, IPOPT uses an interior point method.
• 💡 Imagine the constraints form a fenced-in yard. IPOPT solves the problem by adding a mathematical "force field" (barrier penalty) to the edges of the yard. As the solver gets closer to violating a constraint (e.g. self-collision), the penalty shoots toward infinity.