Determining the position of an extremity of a humanoid is a formidable
problem(at least to me) in forward kinematics. For example, finding out
how high a humanoid's heel is from the floor is the original problem I
had in mind. A more direct solution would be to use proximity sensors
with large sizes to detect the position. One proximity sensor can placed
at a fixed position, say (0, 0, 0). The other is placed at the extremity
in question, in this case the heel. The proximity sensors will give the
position of the viewer in relation to themselves. The problem is how to
use this information i.e. the position of the viewer from one known and
one unknown position to determine the unknown position.
The position of the viewer is given in relation to the coordinate system
of the proximity sensor. While two sensors may have the same scale, they
have different positions and, more importantly, different alignments. The
relative position of a coordinate system in relation to another coordinate
system can be expressed as a translation to make their origins coincide
followed by a single rotation to make their axes coincide. This can be
expressed as x, y, z, Q where Q is a
quaternion representing the required rotation.
Let the fixed sensor be A, the moving sensor in the heel be
B and the viewer V. Each have their own
coordinate systems. The position of V in relation to
A and B - let us call it POSA,V and
POSB,V - are known. I need to determine the position of
B in relation to A i.e. POSA,B.
A java class called CoordSys will be used to implement the
position of a coordinate system in relation to another. The position and
orientation of the other point can be used to construct this class. It will
take the coordinates and the quaternion representing the rotation as
parameters. The constructor will look like this.
CoordSys ( float x, float y, float z, Quaternion q ) ;
Let us start with a smaller problem - determining POSM,L given
POSL,M. The coordinate system for M in relation
to L is a translation of (x,y,z) followed by a rotation
of Q. Therefore, the coordinate system for L
in relation to M will be the reverse - a rotation of
Q-1 followed by a translation of (-x,-y,-z). Let
us call this reversing the coordinate system(anybody know what it is really
called?). We add another method to the CoordSys class.
CoordSys reverse() ;
Note that applying a rotation of Q on a point P is
Q*P*Q-1. The code to get the reverse of the position of a
coordinate system will be as follows.
pos_M_L = pos_L_M . reverse() ;
The second problem is determining POSL,N given POSL,M
and POSM,N. This can be done by applying the transformation for the
reverse of POSL,M on POSM,N. This would give the position
of coordinate system N in relation to L. This adds another method to the class.
CoordSys transform ( CoordSys cs ) ;
This function applies the reverse of the current coordinate system on the
passed coordinate system. The code will be as follows.
pos_P_R = pos_P_Q . transform ( pos_Q_R ) ;
Returning to our problem of finding POSA,B given POSA,V
and POSB,V, the code is as follows.
pos_V_B = pos_B_V . reverse() ;
pos_A_B = pos_A_V . transform ( pos_V_B ) ;
or
pos_A_B = pos_A_V . transform ( pos_B_V . reverse() ) ;
