
i
i
i
i
i
i
i
i
11.4. Inverse Kinematics 277
Star t with the linkage configuration defined by the set of angles:
θ
1
, θ
2
, θ
3
...θ
n
(which we will write as Θ), and endpoint located at P,
i.e., at coordinate (x, y).
Apply the steps below to move the endpoint towards its goal
at P
g
(i.e., at :(x
g
, y
g
))
Step 1:
Calculate the incremental step ΔX = P
g
− P
Step 2:
Calculate J(θ
1
, θ
2
, θ
3
, ...θ
n
)
(use the current values of θ
1
etc.)
Step 3:
Find the inverse of J, which we will call J
−
J
−
= J
T
(JJ
T
)
−1
(if J is a 2 × n matrix, J
−
is a n × 2matrix)
Step 4:
Test for a valid convergence of the iteration:
if (I − JJ
−
)ΔX > ε then the step towards the
goal (the ΔX) is too large, so set ΔX =
Δ