4.4 Inverse Kinematics
4.4.1 Iterative Construction of Revolute k-Chains
We will use the geometric construction of revolute constraints as the basis for implement- ing a version of a conceptually simple iterative technique called Forward-And-Backward- Reaching-Inverse-Kinematics [FABRIK]. FABRIK was proposed by Aristidou and Lasenby in [1] as a method for motion capture – a way of representing a configuration of joint posi- tions of a chain given some set of 3D positions. In a subsequent work they modelled a human hand and a human skeleton. The approach has recently gathered attention in the robotics community for its intuitive construction and flexibility in solving a range of problems, and can be used to constrain the positions of an open or closed kinematic chain using coincidence relationships of round elements such as circles and spheres. Here we will detail an iterative solver for an open chain with an arbitrary number of revolute joints using this technique. The basic principle behind the technique is to satisfy constraints by reaching forward toward some target, and then re-solve them by reaching backwards to the root of the chain, and then
(TrM[⇧]⇤^ ) ⇤ dr+ da r a
⇧
Tr
M[⇧]
Figure 4.6: Given the ith frame in chain of revolute joints, we can use its ai and ri link
parameters to construct geometric elements which encode the possible positions of the (i + 1)th frame. Here the rotation plane isP which can be translated along the offset r and then intersected with the surrounding sphere s to create the circle of possible positions of the (i + 1)th frame. Note that in this figure the skew parameter,ai, is 0. See Algorithms 4.2,4.3,
repeating the process until the end effector is within some error threshold of the target. Note that there is no guarantee of convergence, though in practice its users report stable results. Our algorithm is similar in concept to Algorithms 2 and 3 of [1], though whereas those de- scriptions model bone-like chains, here we contribute specific CGA mathematical equations for k-chains in terms of Denavit-Hartenberg parameters visualized in Figure 4.6.
Basic Forward and Backward Approach with Spherical k-chains Before we tackle this problem, it will help to review the basic technique of the FABRIK method by assuming that all joints in our chain are universal ball joints, and therefore are constrained only by spheres, not planes. Algorithm 4.5 and Figure 4.7 detail this most basic method: we fix the last frame position, pn, to the desired target and “reach forwards” to constrain the pi 1 position to lie
on the sphere of radius ai 1 at pi. We iterate to the root of the chain. Then, we fix the first
frame position p0 to the desired base and “reach backwards” to constrain the pi+1 position
to lie on the sphere of radius ai at pi. We iterate towards the end effector. Note that the
terminology can seem counterintuitive: “reaching forwards” entails iterating backwards from the end effector, while “reaching backwards” entails iterating forward from the base of the chain.
We can define these spheres using the definition of a sphere at point piwith radius r from
Equation 2.52, and calculate the point on the sphere closest to pi+1by creating a line through
pi and pi+1 and intersecting it with the sphere. We then use Equation 2.57 to extract the
point closest to pi+1 from the resulting point pair intersection: for a lineL = pi^ pi+1^ n•
intersecting a spheres centered at piwe can find this point by extracting the second point in
the resulting point pair intersectionk from Equation 2.58, namely: k+p|k2| n•·k .
This particular distance constraint could easily be written with typical vector math, and the mechanics of geometric algebra may seem redundant here. The real power lies in adding additional constraints, such as only allowing the joints to rotate in a single plane.
Backward iteration of Revolute k-Chains We have seen (Section 2.6) that a rotation in a plane of fixed distance a can be encoded as a circle, and so we can use the circle as a geometric constraint. Given a point pi+1and a circle K at pi we can find the point on K closest to pi+1.
Aristidou and Lasenby provide a method that uses the midpoint of the reflection of pi+1 in
the carrier planeP of K, where P = K ^ n•. In Algorithm 4.1 we simply use the projection
of pi+1 onto this carrier plane of K. Whichever method is used, that projected point, call it
pi+1pro j, can then be used to construct a line that is intersected with a sphere at piwith radius ai
as in the spherical approach.
We note that since in the case of inverse kinematics chains the circle K is derived from its known center point q, orientation R, and radius r, an even more computationally efficient option to constrain pi+1is to project the Euclidean vectord between points pi+1and pionto
the Euclidean 2-bladex ^ yi=R[e12]of the frame at pi:
v = ((pi+1 pi)· (x ^ yi))/(x ^ yi).
We can then find a point pair by a construction using (normalized) Euclidean vectorv and the spheres surrounding the circle of possible positions of pi+1:
k = s ^(s ·(vn•))
which is based on Equation 2.59 and illustrated in Figure 2.13. Extracting of the point closest to p is then straightforward using Equation 2.57. In the case of links with an offset parameter, this spheres is not necessarily centered at pi.
Unlike the spherical case, where in-joint rotations can be determined after all positions have been determined, at each step of the revolute algorithm we must also set the rotor ori- entation Ri of each frame in addition to the position pi. When reaching backwards to satisfy
the base position constraint, the joint orientation is found by projecting the target direction onto the plane of rotation. When reaching forwards we use Algorithm 4.3, which compares
ai 1 pi pi 1 pi 2 p0i 1 ai 2 pi p0i 1 pi 2 pi 1 p0i 2
Figure 4.7: The forward reaching step of the FABRIK method for spherical joints constrains frames at pi 1 to lie within the ai 1 radius of pi. Algorithm 4.5 details the process used to
calculate this geometrically as the intersection of the line through pi 1and piand the sphere
at piwith radius ai 1. Using this method, a chain of ball joints can be represented as a series
of forward-constraining-spheres centered at i going through i 1, and a series of backward- constraining-spheres centered at i going through i + 1.
the projection of the link vector and target vector, each transformed to be in the frame of reference of the i 1th frame.
Because the calculations depend on the current orientation of each frame, it can be helpful to slowly rotate the target end effector about itsy axis in order to find a suitable solution, thereby changing thez axis direction and relaxing the link constraint. This method facilitates reaching a specified target position of the end effector (but not a target orientation).
a) pi pi+1 ⇧ xy i pproj v b) pproj
Figure 4.8: An implementation of the backward reaching step for chains composed of revolute joints from Algorithm 4.6. Each pi+1frame position is constrained to lie on the circle defined
by the Ri frame Pxy orientation. We project pi+1 onto the Pxy plane of the previous frame,
and then calculate the the line through that projected point and the piposition. We intersect
this line with the sphere at piwith radius ai.
Algorithm 4.5 Inverse kinematics of a chain of spherical joints procedure SPHERICALFABRIK(Base,Target)
while |pn· target| > error do .While end frame is too far from target
Point: pn Target .Place end frame at Target Position
for pi2 Frames (Starting at pn)do . Forward Reaching Iteration Starting at pn
s pi 12a2i 1n• .Possible positions of pi 1
l pi^ pi 1^ n• .Line through piand current pi 1
k (l⇤^ s)⇤ .Point Pair intersection ofl and s
pi 1 k+
p
|k2|
n•·k .Point ofk closest to current pi 1
p0 Base .Place first frame at Base Position for pi2 Frames (Starting at p0)do . Backward Reaching Iteration Starting at p0
s pi 12a2in• .Sphere at piwith radius ai
l pi^ pi+1^ n• .Line through piand current pi+1
k (l⇤^ s)⇤ .Point Pair intersection ofl and s
pi+1 k+
p
|k2|
n•·k .Point ofk closest to current pi+1
Calculate Joint Rotations Based on Positions (Algorithm 4.8) end procedure