제어 3 - 역기구학 수식 직접 풀기 + PID

Introduction

Part 1 covered what inverse kinematics (IK) is and why it is hard, and introduced the two branches — closed-form and Jacobian — at a conceptual level.
This post follows, by hand, where those result formulas actually come from. Starting from a single line of the law of cosines, we trace every step: how the Jacobian matrix is re-filled with concrete numbers at each iteration and how the error shrinks. At the end we also derive the PID controller that was only in Part 1's title but missing from its body.

Setup: a 2-link planar manipulator (two links, 2 planar DOF). Link lengths $L_1, L_2$, joint angles $\theta_1, \theta_2$. All angles are in radians.

0. Starting point — Forward Kinematics (FK)

To solve inverse kinematics we first need forward kinematics. Once we write down "where the end-effector goes given the angles," both the closed-form and the Jacobian solutions follow by manipulating or differentiating this one expression.

Joint 1 is at the origin; $\theta_1$ is link 1's angle from the $x$-axis, and $\theta_2$ is link 2's angle relative to link 1. Stacking the elbow and end-effector (EE) coordinates:

$$\text{elbow} = \big(L_1\cos\theta_1,\; L_1\sin\theta_1\big)$$ $$\boxed{\;x = L_1\cos\theta_1 + L_2\cos(\theta_1+\theta_2)\;}$$ $$\boxed{\;y = L_1\sin\theta_1 + L_2\sin(\theta_1+\theta_2)\;}$$

To shorten notation, write $c_1=\cos\theta_1,\; s_1=\sin\theta_1,\; c_{12}=\cos(\theta_1+\theta_2),\; s_{12}=\sin(\theta_1+\theta_2)$, so that $x=L_1c_1+L_2c_{12},\; y=L_1s_1+L_2s_{12}$.

Now we invert this expression two ways in turn: first the closed form that gives the answer in one shot, then the iterative Jacobian method that approaches it gradually.

1. Closed-form — solving directly with the law of cosines

The goal is reversed: given a target EE position $(x,y)$, find $\theta_1,\theta_2$.

1-1. $\theta_2$ first — law of cosines

Consider the triangle with vertices at the origin, the elbow, and the EE. Its three sides have lengths $L_1$, $L_2$, and the straight-line distance to the EE, $r=\sqrt{x^2+y^2}$.

$$r^2 = x^2 + y^2$$

Let $\beta$ be the interior angle between $L_1$ and $L_2$. The law of cosines gives

$$r^2 = L_1^2 + L_2^2 - 2L_1L_2\cos\beta$$

But the joint angle $\theta_2$ is defined to be 0 when the arm is straight. So the relation between the interior angle $\beta$ and the joint angle $\theta_2$ is $\beta = \pi-\theta_2$. Check: when the arm is fully extended the links are colinear, so $\beta=\pi$ and $\theta_2=0$; when fully folded, $\beta=0$ and $\theta_2=\pi$. Therefore

$$\cos\beta = \cos(\pi-\theta_2) = -\cos\theta_2$$

Substituting flips the sign and tidies up:

$$r^2 = L_1^2 + L_2^2 + 2L_1L_2\cos\theta_2 \;\;\Longrightarrow\;\; \boxed{\;\cos\theta_2 = \dfrac{r^2 - L_1^2 - L_2^2}{2L_1L_2}\;}$$

This is exactly the formula Part 1 stated without derivation. The angle is

$$\theta_2 = \pm\arccos\!\left(\dfrac{r^2 - L_1^2 - L_2^2}{2L_1L_2}\right)$$

The $\pm$ is the elbow-up / elbow-down pair — the two poses reaching the same point, as discussed in Part 1.

1-2. $\theta_1$ — decompose as a difference of angles

Grouping the FK expression by $\theta_1$ reveals a clean structure. First expand $c_{12},\,s_{12}$ with the angle-sum identities:

$$c_{12}=\cos(\theta_1+\theta_2)=c_1\cos\theta_2 - s_1\sin\theta_2,\qquad s_{12}=\sin(\theta_1+\theta_2)=s_1\cos\theta_2 + c_1\sin\theta_2$$

Substitute into $x=L_1c_1+L_2c_{12}$, $y=L_1s_1+L_2s_{12}$ and collect $c_1,\,s_1$ terms:

$$x = c_1\,(L_1 + L_2\cos\theta_2) - s_1\,(L_2\sin\theta_2)$$ $$y = s_1\,(L_1 + L_2\cos\theta_2) + c_1\,(L_2\sin\theta_2)$$

Letting $k_1 = L_1 + L_2\cos\theta_2,\; k_2 = L_2\sin\theta_2$, this is exactly the vector $(k_1,k_2)$ rotated by $\theta_1$:

$$\begin{bmatrix} x \\ y \end{bmatrix} = \begin{bmatrix} \cos\theta_1 & -\sin\theta_1 \\ \sin\theta_1 & \cos\theta_1 \end{bmatrix}\begin{bmatrix} k_1 \\ k_2 \end{bmatrix}$$

A rotation adds angles, so the direction angle of the target vector equals "$\theta_1$ + the direction angle of $(k_1,k_2)$." (Here $\operatorname{atan2}(y,x)$ returns the direction angle of a vector with correct quadrant handling — unlike plain $\arctan$, it gets the sign and quadrant right.)

$$\operatorname{atan2}(y,x) = \theta_1 + \operatorname{atan2}(k_2, k_1)$$

Therefore

$$\boxed{\;\theta_1 = \operatorname{atan2}(y,x) - \operatorname{atan2}\big(L_2\sin\theta_2,\; L_1 + L_2\cos\theta_2\big)\;}$$

Once you pick the sign of $\theta_2$ (up/down), the matching $\theta_1$ is uniquely determined.

1-3. Checking with numbers — a clean example

Let $L_1=L_2=1$ and target $(x,y)=(1,1)$.

r²      = 1² + 1² = 2
cosθ2   = (2 − 1 − 1) / (2·1·1) = 0
θ2      = ±90°        (up / down, two solutions)

[elbow-up]  θ2 = +90°
θ1 = atan2(1,1) − atan2(1·sin90°, 1 + 1·cos90°)
   = 45° − atan2(1, 1) = 45° − 45° = 0°
check (FK):  x = cos0° + cos(0°+90°) = 1 + 0 = 1  ✓
            y = sin0° + sin(0°+90°) = 0 + 1 = 1  ✓

[elbow-down]  θ2 = −90°
θ1 = 45° − atan2(−1, 1) = 45° − (−45°) = 90°
check (FK):  x = cos90° + cos(90°−90°) = 0 + 1 = 1  ✓
            y = sin90° + sin(0°)       = 1 + 0 = 1  ✓

Both solutions land exactly on $(1,1)$ — two poses, one point, confirmed by hand.

Pros: one exact answer in one shot, and it gives all solutions. Cons: change the structure and you must re-derive from scratch, and for complex arms (e.g. 6-DOF) such a clean formula is not guaranteed to exist. Hence we need a general method.

2. Jacobian — a general method via differentiation

If closed-form "solves for the angles directly," the Jacobian method "nudges toward the target a little at a time." The key is to differentiate FK with respect to the angles to build a matrix describing "how the EE moves when each joint turns a bit."

2-1. Filling in $J$ by partial derivatives

The Jacobian is the $2\times2$ matrix of partials of the EE coordinates $(x,y)$ with respect to the joint angles $(\theta_1,\theta_2)$:

$$J = \begin{bmatrix} \dfrac{\partial x}{\partial \theta_1} & \dfrac{\partial x}{\partial \theta_2} \\[2mm] \dfrac{\partial y}{\partial \theta_1} & \dfrac{\partial y}{\partial \theta_2} \end{bmatrix}$$

Differentiate $x=L_1c_1+L_2c_{12}$, $y=L_1s_1+L_2s_{12}$ entry by entry. Note that $c_{12}=\cos(\theta_1+\theta_2)$ depends on both $\theta_1$ and $\theta_2$ (chain rule).

∂x/∂θ1 = −L1 s1 − L2 s12      (d/dθ1 of c12 → −s12)
∂x/∂θ2 =        − L2 s12      (d/dθ2 of c12 → −s12; L1 term has no θ2)
∂y/∂θ1 =  L1 c1 + L2 c12
∂y/∂θ2 =          L2 c12

Collecting:

$$\boxed{\;J = \begin{bmatrix} -L_1 s_1 - L_2 s_{12} & -L_2 s_{12} \\ \;\;L_1 c_1 + L_2 c_{12} & \;\;L_2 c_{12} \end{bmatrix}\;}$$

This matrix links joint velocity to EE velocity. In differential form it is Part 1's relation:

$$\begin{bmatrix} \Delta x \\ \Delta y \end{bmatrix} = J\,\begin{bmatrix} \Delta\theta_1 \\ \Delta\theta_2 \end{bmatrix} \quad\Longleftrightarrow\quad \Delta \mathbf{x} = J(\mathbf{q})\,\Delta\mathbf{q}$$

2-2. Inverting — the inverse matrix and the determinant

We want the reverse: "to move the EE by $\Delta\mathbf{x}$, how much $\Delta\mathbf{q}$ must the joints turn?" Being $2\times2$, if the inverse exists it solves immediately:

$$\Delta\mathbf{q} = J^{-1}\,\Delta\mathbf{x}, \qquad J^{-1} = \frac{1}{\det J}\begin{bmatrix} J_{22} & -J_{12} \\ -J_{21} & J_{11} \end{bmatrix}$$

Let us expand the determinant fully (this result is exactly what flags singularities):

$$\det J = (-L_1 s_1 - L_2 s_{12})(L_2 c_{12}) - (-L_2 s_{12})(L_1 c_1 + L_2 c_{12})$$ $$= -L_1 L_2 s_1 c_{12} - L_2^2 s_{12} c_{12} + L_1 L_2 c_1 s_{12} + L_2^2 s_{12} c_{12}$$

The $L_2^2 s_{12}c_{12}$ terms cancel; factoring $L_1L_2$ from the rest gives $\;L_1 L_2 (c_1 s_{12} - s_1 c_{12})$. Using the sine-difference identity $c_1 s_{12}-s_1 c_{12}=\sin\big((\theta_1+\theta_2)-\theta_1\big)=\sin\theta_2$:

$$\boxed{\;\det J = L_1 L_2 \sin\theta_2\;}$$

This one line answers Part 1's quiz ("when can the Jacobian not be inverted?"). $\det J = 0$ when $\sin\theta_2=0$, i.e. $\theta_2=0$ (fully extended) or $\theta_2=\pi$ (fully folded) — a singularity. There the $1/\det J$ in $J^{-1}$ diverges to infinity.

2-3. When non-square or singular — pseudoinverse and DLS

At a singularity, or when the matrix is non-square (mismatched DOF), an ordinary inverse does not exist. Then we use the pseudoinverse $J^{+}$:

$$\Delta\mathbf{q} = J^{+}\,\Delta\mathbf{x}, \qquad J^{+} = J^{\mathsf T}\big(JJ^{\mathsf T}\big)^{-1}\ \text{(right inverse, when there are fewer rows)}$$

For a square, nonsingular matrix the pseudoinverse gives exactly the same value as the ordinary inverse. To make that concrete: for the first pose used in Section 3, $\mathbf{q}_0=(30^\circ,60^\circ)$, with $J=\begin{bmatrix}-1.5 & -1\\ 0.866 & 0\end{bmatrix}$, computing $J^{+}\Delta\mathbf{x}$ yields $(-0.1155,\;0.0392)$ — digit-for-digit identical to $J^{-1}\Delta\mathbf{x}$. So when nonsingular, the two coincide.

They differ when not full rank. There $J^{+}$ picks (1) the closest reachable solution when the target is unreachable, or (2) the smallest-motion solution when infinitely many exist. But near a singularity $JJ^{\mathsf T}$ is nearly singular too, so $\Delta\mathbf{q}$ blows up. The fix is DLS (Damped Least Squares):

$$\Delta\mathbf{q} = J^{\mathsf T}\big(JJ^{\mathsf T} + \lambda^2 I\big)^{-1}\,\Delta\mathbf{x}$$

Adding $\lambda^2 I$ keeps the inverse from ever diverging. It trades a little accuracy for stability near singularities; larger $\lambda$ is smoother but slower to converge. (Section 4 compares the numbers directly.)

3. How the matrix rolls each step — with numbers

Now the part we most wanted to see. As Jacobian IK iterates step by step, we trace exactly what numbers fill the matrix and how the error shrinks.

Setup: $L_1=L_2=1$, start $\mathbf{q}_0=(30^\circ, 60^\circ)$, target $\mathbf{x}^\ast=(1.0,\;1.4)$, step size $\alpha=0.5$. Each step is $\mathbf{q} \leftarrow \mathbf{q} + \alpha\,J^{+}\Delta\mathbf{x}$.

Step 0 — fill $J$, compute $\Delta\mathbf{q}$

q0 = (30°, 60°)
current EE = (cos30°+cos90°, sin30°+sin90°) = (0.866, 1.500)
error Δx = target − EE = (1.0−0.866, 1.4−1.500) = (0.1340, −0.1000)
|Δx| = √(0.1340² + 0.1000²) = 0.1672

fill the Jacobian "at this pose":
s1=sin30°=0.5,  c1=cos30°=0.866,  s12=sin90°=1,  c12=cos90°=0
J = [ −0.5−1   −1 ] = [ −1.5   −1 ]
    [  0.866   0 ]     [ 0.866   0 ]
det J = L1·L2·sin60° = 0.866      (≠0 → inverse exists)

J⁻¹ = (1/0.866)·[ 0      1   ] = [  0      1.155 ]
                [ −0.866 −1.5 ]   [ −1.0   −1.732 ]

full correction  J⁻¹·Δx = (−0.1155, +0.0392) rad
apply step       α·J⁻¹·Δx = 0.5·(−0.1155, 0.0392) = (−0.0578, +0.0196) rad
                                                  = (−3.31°, +1.12°)     ← rad × 180/π
q1 = q0 + α·Δq = (30−3.31, 60+1.12) = (26.69°, 61.12°)

Step 1 — re-fill the same matrix (the numbers change)

q1 = (26.69°, 61.12°)
EE = (0.9315, 1.4485),   error |Δx| = 0.0839   (about half of step 0's 0.1672)

recompute J at the new pose → every entry changes:
J = [ −1.4485  −0.9993 ]    det J = L1·L2·sin(61.12°) = 0.8757
    [  0.9315   0.0381 ]
Δx = (0.0685, −0.0485)
α·J⁻¹·Δx = (−1.50°, +0.21°)
q2 = (25.19°, 61.33°)

The key point: $J$ is re-filled entirely at every step. The $J$ at step 0 and the $J$ at step 1 are completely different matrices. This is exactly Part 1's "moves along a curve, but by repeating straight-line calculations" — each step builds a fresh linear approximation ($J$) at the current pose to follow the curved path.

Steps 0–6 — watching the error shrink

step |  θ1(°)    θ2(°)  |   EE (x, y)        |   error
  0  |  30.000  60.000  |  (0.8660, 1.5000)  |  0.16718
  1  |  26.692  61.124  |  (0.9315, 1.4485)  |  0.08388
  2  |  25.193  61.334  |  (0.9655, 1.4238)  |  0.04197
  3  |  24.485  61.353  |  (0.9827, 1.4118)  |  0.02099
  4  |  24.141  61.341  |  (0.9913, 1.4059)  |  0.01049
  5  |  23.972  61.330  |  (0.9956, 1.4029)  |  0.00525
  6  |  23.888  61.323  |  (0.9978, 1.4015)  |  0.00262

With $\alpha=0.5$ the error drops by almost exactly half each step (0.167 → 0.084 → 0.042 → …), reaching the target in a handful of steps. (With $\alpha=1$ this example nearly converges in a single step, but for far targets or strong nonlinearity it can overshoot and become unstable, so one usually keeps $0<\alpha\le1$.)

As an algorithm:

repeat:
    Δx = x_target − FK(q)          # remaining error
    if |Δx| < tolerance: break
    J  = jacobian(q)               # re-filled at the current pose
    Δq = pinv(J) · Δx              # (use DLS near a singularity)
    q  = q + α · Δq                # α: step size (0<α≤1)

The simulator assignment opens in exactly this state — start $(30^\circ,60^\circ)$, target $(1.0,1.4)$, $\alpha=0.5$ — so the table above plays out on screen. One call of the ik_step(q, target) function you implement is one step above, and the on-screen det(J)=L1·L2·sinθ2 readout is the very value derived in Section 2-2.

4. What happens at a singularity — pinv vs DLS in numbers

Now the DLS we deferred in Section 2-3, in numbers. Push $\theta_2$ toward $0^\circ$: pose $\mathbf{q}=(20^\circ,\,5^\circ)$, the arm nearly straight.

q = (20°, 5°)   →   EE = (1.846, 0.765)
J = [ −0.765  −0.423 ]    det J = sin5° = 0.0872   →   1/detJ = 11.47 (amplified!)
    [  1.846   0.906 ]

want to push the EE outward (radially) by just 0.05 (5% of a link length):
Δx = (0.046, 0.019),  |Δx| = 0.05

[pseudoinverse]  Δq = pinv(J)·Δx = (32.8°, −65.7°)   |Δq| = 1.28 rad
                 → tens of degrees of joint motion for a tiny 0.05 move (blow-up)

[DLS, λ=0.1]     Δq = J ᵀ(J J ᵀ + λ²I)⁻¹·Δx = (4.3°, −8.7°)   |Δq| = 0.17 rad
                 → same target, ~7× smaller joint change, stable

For the same $\Delta\mathbf{x}$, the pseudoinverse wants to swing the joints by $32.8^\circ/-65.7^\circ$ (the radial direction is nearly unreachable, so $1/\det J$ amplifies it), while DLS limits it smoothly to $4.3^\circ/-8.7^\circ$ (here $\lambda=0.1$). So in the assignment, "why is this pose a singularity" is explained by $\det J = L_1L_2\sin\theta_2 = 0$; the pseudoinverse alone stays unstable nearby, and adding DLS stabilizes it. (The printed $\Delta\mathbf{x}$ and $\Delta\mathbf{q}$ values are rounded for display, so recomputing from the printed digits may differ in the last place.)

5. PID — actually tracking the target joint angle

Once IK has given a "target joint angle $\theta^\ast$," the real motor must reach it. The standard controller for driving the error between the current angle $\theta$ and the target to zero is PID.

$$e(t) = \theta^\ast - \theta(t)$$ $$u(t) = K_p\,e(t) + K_i\!\int_0^t e(\tau)\,d\tau + K_d\,\frac{de(t)}{dt}$$

$u(t)$ is the motor input (torque or velocity command). The three terms:

• P (proportional) — pushes in proportion to the error. Larger is faster but too large oscillates. P alone usually leaves a small steady-state error.

• I (integral) — integrates accumulated error to remove that steady-state error. Too much causes overshoot or integral windup.

• D (derivative) — reacts to the rate of change of error, acting as a brake. It reduces overshoot and oscillation but is sensitive to measurement noise.

To run it in code we need the discrete form. At each time step $\Delta t$:

e_k        = θ_target − θ_k
integral  += e_k · Δt                      # accumulate integral
derivative = (e_k − e_{k−1}) / Δt          # finite-difference derivative
u_k        = Kp·e_k + Ki·integral + Kd·derivative
# apply u_k to the motor → θ updates → repeat next step

Tuning intuition: raise $K_p$ first for response speed (up to the brink of oscillation), use $K_d$ to suppress overshoot, then $K_i$ to slowly remove the remaining steady-state error. In practice add anti-windup (clamping) and derivative-term filtering.

Big picture: IK converts target coordinates → target joint angles, and PID makes the motor actually track those angles. Part 1's two problems (coordinates→angles / tracking the angle) map respectively here.

Wrap-up

In summary — closed-form gives $\theta_2$ from the law of cosines and $\theta_1$ from the rotation decomposition; the Jacobian comes from the partials of FK, and its determinant $L_1L_2\sin\theta_2$ flags singularities. Iteration means re-filling $J$ each step to refresh the linear approximation, with DLS preventing blow-up near singularities. PID then makes the motor follow the target angle so obtained.

Now it is your turn to run it. The accompanying assignment ports the formulas above into code and watches a 3D robot arm follow the target in a browser simulator. Implement one ik_step of Section 3 yourself, and see the difference between the pseudoinverse and DLS at a singularity with your own eyes. (The simulator and assignment details are attached at the end of this post or provided separately.)

Assignment — Browser Simulator

Port the formulas derived above into code and make the 2-link arm follow a target — no install, it runs right in the browser (Python + numpy execute in-browser).

• Goal — fill in the skeleton’s ik_step(q, target) to implement Jacobian IK so the arm converges to the target (one step of Section 3 = one call of this function).

• Compare — see the difference between the pseudoinverse and DLS near a singularity (Section 4); the “singularity example” button reproduces it in one click.

• Apply — switch to Pick & Place mode and check that the same ik_step performs pick → carry → place.

• Submit — your finished code + a run recording/screenshots + a 1–2 page report (pinv vs DLS at a singularity).

▶ Open the simulator — only the first load needs internet and takes a few seconds.