|
| 1 | +# # Planar Arm Example |
| 2 | + |
| 3 | +# Inverse Kinematics (IK) computes joint angles that place a robot’s end-effector at a desired target $(x_t,y_t)$. For a 2-link planar arm with joint angles $\theta_1,\theta_2$, the end-effector position is: |
| 4 | +# ```math |
| 5 | +# f(\theta_1,\theta_2) = \bigl(\ell_1\cos(\theta_1) + \ell_2\cos(\theta_1+\theta_2),\,\, |
| 6 | +# \ell_1\sin(\theta_1) + \ell_2\sin(\theta_1+\theta_2)\bigr). |
| 7 | +# ``` |
| 8 | +# We can solve an NLP: |
| 9 | +# ```math |
| 10 | +# \min_{\theta_1,\theta_2} \;\; (\theta_1^2 + \theta_2^2), |
| 11 | +# \quad\text{s.t.}\quad f(\theta_1,\theta_2) = (x_t,y_t). |
| 12 | +# ``` |
| 13 | +# Treat $(x_t,y_t)$ as parameters. Once solved, we differentiate w.r.t. $(x_t,y_t)$ to find how small changes in the target location alter the optimal angles - the *differential kinematics*. |
| 14 | + |
| 15 | +# ## Define and solve the 2-link planar arm problem and build sensitivity map of joint angles to target |
| 16 | + |
| 17 | +# First, import the libraries. |
| 18 | + |
| 19 | +using Test |
| 20 | +using JuMP |
| 21 | +import DiffOpt |
| 22 | +using LinearAlgebra |
| 23 | +using Statistics |
| 24 | +import Ipopt |
| 25 | +using Plots |
| 26 | +using Plots.Measures |
| 27 | + |
| 28 | +# Fixed data |
| 29 | + |
| 30 | +# Arm geometry |
| 31 | +l1 = 1.0; |
| 32 | +l2 = 1.0; |
| 33 | +reach = l1 + l2 # 2.0 |
| 34 | +tol = 1e-6 # numerical tolerance for feasibility |
| 35 | +# Sampling grid in workspace |
| 36 | +grid_res = 25 # grid resolution low for documentation compilation requirements |
| 37 | +xs = range(-reach, reach; length = grid_res) |
| 38 | +ys = range(-reach, reach; length = grid_res) |
| 39 | + |
| 40 | +heat = fill(NaN, grid_res, grid_res) # store ‖J_inv‖₂ |
| 41 | +feas = fill(0.0, grid_res, grid_res) # feasibility mask |
| 42 | +θ1mat = similar(heat) |
| 43 | + |
| 44 | +function ik_angles(x, y; l1 = 1.0, l2 = 1.0, elbow_up = true) |
| 45 | + c2 = (x^2 + y^2 - l1^2 - l2^2) / (2 * l1 * l2) |
| 46 | + θ2 = elbow_up ? acos(clamp(c2, -1, 1)) : -acos(clamp(c2, -1, 1)) |
| 47 | + k1 = l1 + l2 * cos(θ2) |
| 48 | + k2 = l2 * sin(θ2) |
| 49 | + θ1 = atan(y, x) - atan(k2, k1) |
| 50 | + return θ1, θ2 |
| 51 | +end |
| 52 | + |
| 53 | +for (i, x_t) in enumerate(xs), (j, y_t) in enumerate(ys) |
| 54 | + global θ1mat, heat |
| 55 | + ## skip points outside the circular reach |
| 56 | + norm([x_t, y_t]) > reach && continue |
| 57 | + |
| 58 | + ## ---------- build differentiable NLP ---------- |
| 59 | + model = DiffOpt.nonlinear_diff_model(Ipopt.Optimizer) |
| 60 | + set_silent(model) |
| 61 | + |
| 62 | + @variable(model, xt in Parameter(x_t)) |
| 63 | + @variable(model, yt in Parameter(y_t)) |
| 64 | + @variable(model, θ1) |
| 65 | + @variable(model, θ2) |
| 66 | + |
| 67 | + @objective(model, Min, θ1^2 + θ2^2) |
| 68 | + @constraint(model, l1 * cos(θ1) + l2 * cos(θ1 + θ2) == xt) |
| 69 | + @constraint(model, l1 * sin(θ1) + l2 * sin(θ1 + θ2) == yt) |
| 70 | + |
| 71 | + ## --- supply analytic start values --- |
| 72 | + θ1₀, θ2₀ = ik_angles(x_t, y_t; elbow_up = true) |
| 73 | + set_start_value(θ1, θ1₀) |
| 74 | + set_start_value(θ2, θ2₀) |
| 75 | + |
| 76 | + optimize!(model) |
| 77 | + println("Solving for target (", x_t, ", ", y_t, ")") |
| 78 | + ## check for optimality |
| 79 | + status = termination_status(model) |
| 80 | + println("Status: ", status) |
| 81 | + |
| 82 | + status == MOI.OPTIMAL || status == MOI.LOCALLY_SOLVED || continue |
| 83 | + |
| 84 | + θ1̂ = value(θ1) |
| 85 | + θ1mat[j, i] = θ1̂ # save pose |
| 86 | + |
| 87 | + ## ---- forward diff wrt xt (∂θ/∂x) ---- |
| 88 | + DiffOpt.empty_input_sensitivities!(model) |
| 89 | + DiffOpt.set_forward_parameter(model, xt, 0.01) |
| 90 | + DiffOpt.forward_differentiate!(model) |
| 91 | + dθ1_dx = DiffOpt.get_forward_variable(model, θ1) |
| 92 | + dθ2_dx = DiffOpt.get_forward_variable(model, θ2) |
| 93 | + |
| 94 | + ## check first order approximation keeps solution close to target withing tolerance |
| 95 | + θ_approx = [θ1̂ + dθ1_dx, θ1̂ + dθ2_dx] |
| 96 | + x_approx = l1 * cos(θ_approx[1]) + l2 * cos(θ_approx[1] + θ_approx[2]) |
| 97 | + y_approx = l1 * sin(θ_approx[1]) + l2 * sin(θ_approx[1] + θ_approx[2]) |
| 98 | + _error = [x_approx - (x_t + 0.01), y_approx - y_t] |
| 99 | + println("Error in first order approximation: ", _error) |
| 100 | + feas[j, i] = norm(_error) |
| 101 | + |
| 102 | + ## ---- forward diff wrt yt (∂θ/∂y) ---- |
| 103 | + DiffOpt.empty_input_sensitivities!(model) |
| 104 | + DiffOpt.set_forward_parameter(model, yt, 0.01) |
| 105 | + DiffOpt.forward_differentiate!(model) |
| 106 | + dθ1_dy = DiffOpt.get_forward_variable(model, θ1) |
| 107 | + dθ2_dy = DiffOpt.get_forward_variable(model, θ2) |
| 108 | + |
| 109 | + ## 2-norm of inverse Jacobian |
| 110 | + Jinv = [ |
| 111 | + dθ1_dx dθ1_dy |
| 112 | + dθ2_dx dθ2_dy |
| 113 | + ] |
| 114 | + heat[j, i] = opnorm(Jinv) # σ_max of Jinv |
| 115 | +end |
| 116 | +# Replace nans with 0.0 |
| 117 | +heat = replace(heat, NaN => 0.0) |
| 118 | + |
| 119 | +# ## Results with Plot graphs |
| 120 | + |
| 121 | +default(; |
| 122 | + size = (1150, 350), |
| 123 | + legendfontsize = 8, |
| 124 | + guidefontsize = 9, |
| 125 | + tickfontsize = 7, |
| 126 | +) |
| 127 | + |
| 128 | +plt = heatmap( |
| 129 | + xs, |
| 130 | + ys, |
| 131 | + heat; |
| 132 | + xlabel = "x target", |
| 133 | + ylabel = "y target", |
| 134 | + clims = (0, quantile(skipmissing(heat), 0.95)), # clip extremes |
| 135 | + colorbar_title = "‖∂θ/∂(x,y)‖₂", |
| 136 | + left_margin = 5Plots.Measures.mm, |
| 137 | + bottom_margin = 5Plots.Measures.mm, |
| 138 | +); |
| 139 | + |
| 140 | +# Overlay workspace boundary |
| 141 | +θ = range(0, 2π; length = 200) |
| 142 | +plot!(plt, reach * cos.(θ), reach * sin.(θ); c = :white, lw = 1, lab = "reach"); |
| 143 | + |
| 144 | +plt_feas = heatmap( |
| 145 | + xs, |
| 146 | + ys, |
| 147 | + feas; |
| 148 | + xlabel = "x target", |
| 149 | + ylabel = "y target", |
| 150 | + clims = (0, 1), |
| 151 | + colorbar_title = "Precision Error", |
| 152 | + left_margin = 5Plots.Measures.mm, |
| 153 | + bottom_margin = 5Plots.Measures.mm, |
| 154 | +); |
| 155 | + |
| 156 | +plot!( |
| 157 | + plt_feas, |
| 158 | + reach * cos.(θ), |
| 159 | + reach * sin.(θ); |
| 160 | + c = :white, |
| 161 | + lw = 1, |
| 162 | + lab = "reach", |
| 163 | +); |
| 164 | + |
| 165 | +plt_all = plot( |
| 166 | + plt, |
| 167 | + plt_feas; |
| 168 | + layout = (1, 2), |
| 169 | + left_margin = 5Plots.Measures.mm, |
| 170 | + bottom_margin = 5Plots.Measures.mm, |
| 171 | + legend = :bottomright, |
| 172 | +) |
| 173 | + |
| 174 | +# Left figure shows the spectral-norm heat-map |
| 175 | +# $\bigl\lVert\partial\boldsymbol{\theta}/\partial(x,y)\bigr\rVert_2$ |
| 176 | +# for a two-link arm - Bright rings mark near-singular poses. Right figure shows the normalized precision error of the first order approximation derived from calculated sensitivities. |
0 commit comments