Inverse Kinematics in Nao Using normalized vector

Inverse Kinematics in Nao Using normalized vector Since each link in Nao has two DOFs in maxima except the leg of which the HipYawPitch angle is assumed to be constant and the axis of the last DOF in each kinematical chain is not along the link, so we can solve the inverse kinematic problem analytically. In other words, the inverse problem is to find a set of values for the joints that drive the given link to the desired direction specified by the normalized vector related to the torso frame byˆB B i i i iX R X = (1) Where i i X represents the ith normalized vector of which the coordinate frame is attached to ith link and B i R is the corresponding rotation matrix from torso coordinate system to frame fixed in ith link andˆB iX is the normalized vector of ith link in torso coordinate system from kinect.Fig 1 Head of NaoTake the head as example (Fig.1), the normalized vector fixed in head frame is11(0,0,1)T X =and the corresponding rotation matrix is1(z,HeadYaw)R(x,HeadPitch)B R R =-. (2)Thus these two unknown joint angles can be solved with the combination of1ˆB X from Kinect which is also determined by two variables through equation 222111ˆˆˆ1...B BB X x X y X z =++. From equation (2), we have11212121ˆ.x sin sin ˆcos sin .ˆcos .B B B X X y X z θθθθθ⎛⎫-⎛⎫ ⎪⎪= ⎪ ⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭(3) where 1θ represents HeadYaw and 2θ represents HeadPitch. Numerically, there aretwodifferent solutions since θand πθ-have the same sine and θand θ-have the same cosine and equation (3) has to be satisfied. The unique solution can be determined by combination with reasonable constraints, such as which quadrant the vector belongs to.The rotation on head can also be represented by Fig.1 asFig.1 Rotation illustration in Head. The orientation of the head is z-axis in zero position. The head rotate about the z-axis by angle HeadYaw first, and then rotate about the rotating x-axis (x’) by angle HeadPitch, and then reach the orientation of {X’’,Y’’,Z’’}X and that Unit vector along Z axis is the direction of head in zero position which is equal to 11ˆB X.in Z’’ is the resulted direction of head which is equal to1Thus, angles of HeadPitch and HeadYaw can be solved through:X(Z) and (1)Calculation of the orientation of X’using the vector cross product between 11ˆB X(Z’’) in order;1(2)Calculation of HeadPitch (HP) using the angle between Z and Z’’ in order along X’ axis;(3)Calculation of HeadRoll (HR) using the angle between X and X’ along Z axis.In order to simplify the remainder analysis, we mark each link with number asThe kinematic chains and joints in zero position are depicted in Fig.2*LhipAxis refer to (1,0,1)T --which is the rotation axis of HipYawPitch on left leg, and RhipAxis refers to (1,0,1)T - which is the rotation axis of HipYawPitch on the right leg;*HipYawPitch is assumed to be constant;*Wrist angles in hands do not take into consideration since wrist yaw is rotated along Y axis which is along the link;*Ankle angles in feet also do not take into consideration, since the position in feet is highly unaccurate.Fig.2 kinematic chains and joints in zero positionIn the reminder of this technical report, each angle list in table 2 is solved by methods mentioned above.1. Left armFor the left upper arm,22(0,1,0)TX = (5)and2R = R(-X,LSP)R(Z,LSR). (6)By combination equation (1), (5) and (6), we have24342342ˆ.x sin ˆcos cos .ˆsin cos .B B B X Xy X z θθθθθ⎛⎫-⎛⎫ ⎪⎪= ⎪⎪ ⎪ ⎪ ⎪⎝⎭⎝⎭ (7)where 3θ represents LShoulderPitch and 4θ represents LShoulderRoll .For the lower arm with the hand offset 33(0,1,0)TX = (8),and the rotation matrix is3R =2R R(Y ,LEY)R(Z,LER) (9). Since rotation matrix is a 3X3 orthogonal matrix with determinant 1:T T R R RR I ==.So, the equation (1) can be written asR(Y ,LEY)R(Z,LER)= 23ˆT B R X . (10)Here we adopt the second method mentioned above to obtain the required angle by: (1) Calculation of Z ’ using the vector cross product between 33X (Y) and 23ˆT B R X (Y ’’) in order;(2) Calculation of LElbowRoll (LER) using the angle between Y and 23ˆT B R X in order along Z ’ axis;(3) Calculation of LElbowYaw (LEY) using the angle between Z and Z ’ along Y axis.Fig.3 Joints in Right arm2. Right armThe kinematic chain in the right arm is fully symmetric with the left arm chain relatively to the plane defined by the y-axis and z-axis. So, the calculation routine is similar with this in left arm which is omitted here.3. Left legFor the left upper leg,66(0,0,1)TX =- (11)and6R = R(LhipAxis,HYP)R(Y ,LHR)R(-X,LHP). (12)Since the LhipAxis for left leg is constant to (1,0,1)T -- and the HipYawPithch angle is not regarded as variable, so the combination equation (1), (11) and (12) is566656sin cos ˆsin (LHA)cos cos T B R X θθθθθ-⎛⎫ ⎪-= ⎪ ⎪-⎝⎭ (13) where 5θ represents LHipRoll and 6θ represents LHipPitch .Since there is only one DOF in the knee joint, the LKneePitch angle can be obtained by directly calculating the rotation angle in the plane defined by Y-axis and Z-axis.4. Right legSince the right leg is highly symmetric to the left leg except the RHipAxis is (1,0,1)T -. Thus, the calculation process is similar to the left leg.Conclusion。