categories: robotics, automation & robotics, mechanics
Part three, and the question that actually matters when you're programming an arm: you know where you want the gripper to be, so which angle does each motor need? Forward kinematics (post #1) goes from angles to position; inverse kinematics goes the other way, and for a simple enough arm you can solve it with nothing but the law of cosines and some careful bookkeeping with atan2.
Task — analytic inverse kinematics of a planar 2R arm
A planar arm has two rotational links of length $$l_1$$ and $$l_2$$. Given a target tip position $$(x,y)$$, find the joint angles $$\theta_1$$ and $$\theta_2$$.
Step 1 — find $$\theta_2$$ from the law of cosines. The base, joint 2, and the target form a triangle with sides $$l_1$$, $$l_2$$, and $$\sqrt{x^2+y^2}$$. The law of cosines relates them: $$x^2+y^2=l_1^2+l_2^2-2l_1l_2\cos(180^\circ-\theta_2)=l_1^2+l_2^2+2l_1l_2\cos\theta_2$$. Solving for the cosine: $$\cos\theta_2=\frac{x^2+y^2-l_1^2-l_2^2}{2l_1l_2},\qquad \sin\theta_2=\pm\sqrt{1-\cos^2\theta_2},\qquad \theta_2=\text{atan2}(\sin\theta_2,\cos\theta_2)$$ The $$\pm$$ sign isn't a mistake to clean up — it's real information: it's the elbow-up versus elbow-down solution, both of which physically reach the same target.
Step 2 — find $$\theta_1$$ as a difference of two angles. Look at the same triangle again: $$\theta_1$$ is the bearing straight to the target, $$\psi$$, minus the interior angle $$\alpha$$ that the first link makes with that line. $$\psi$$ is trivial, $$\psi=\text{atan2}(y,x)$$. For $$\alpha$$, project link 2 onto the extension of link 1: its component along link 1 is $$l_1+l_2\cos\theta_2$$, and its component perpendicular to link 1 is $$l_2\sin\theta_2$$ — those two projections form a right triangle whose angle is exactly $$\alpha$$: $$\alpha=\text{atan2}(l_2\sin\theta_2,\ l_1+l_2\cos\theta_2)$$ $$\theta_1=\psi-\alpha=\text{atan2}(y,x)-\text{atan2}(l_2\sin\theta_2,\ l_1+l_2\cos\theta_2)$$ Using $$\text{atan2}$$ throughout instead of plain $$\arctan$$ isn't cosmetic — it's what keeps the angle in the correct quadrant automatically, rather than silently flipping sign whenever $$x$$ or the denominator goes negative.
Three posts, one shared habit: break a spatial problem into a chain of simple geometric steps — elementary transforms for D-H, half-angle sandwich products for quaternions, a triangle for inverse kinematics — and the "hard" formula falls out of the geometry instead of needing to be memorized. Thank you for reading, and thanks for following this whole series :)
The End
