value of the first joint variable only, and getting closedform values for the remaining variables.
Let the column vectors of pose matrix Q of Eq. (6.3) be given by m, c, u, and q, in order, so that
mx cx ux my cy Uy mz cz uz 0 0 0
m c u q 0 0 0 1
u = R1-1 R z = RI-1 t
q = R1 p p-
R-1 11 = R1-l(p 11).
From the left hand side of Eq. (6.2), two vectors corresponding to vectors u and q, are given by
(6.6)
UL and qL'
uL = R2 R3 R4 z
qL = R2(R3 14 + 13) + 12.
A nonlinear function of el can be defined as a difference between corresponding quantities from the left and the right side of Eq. (6.2). For example, the difference between the inner-products (uL.qL) and (u.q) yields the function
f(e1) = uL q- u.q.
then
(6.4)
and
(6.5)
and
(6.7) (6.8)
(6.9)
..