Georgia Institute of Technology, Technical Report, GT-GOLEM-2013-001, 2013 Kinematics and Inverse Kinematics for the Humanoid Robot HUBO2+ Rowland O’Flahertyy, Peter Vieiray, Michael Greyy, Paul Ohz, Aaron Bobicky, Magnus Egerstedty, and Mike Stilmany Abstract—This paper derives the forward and inverse kine- matics of a humanoid robot. The specific humanoid that the derivation is for is a robot with 27 degrees of freedom but the procedure can be easily applied to other similar humanoid platforms. First, the forward and inverse kinematics are derived for the arms and legs. Then, the kinematics for the torso and the head are solved. Finally, the forward and inverse kinematic solutions for the whole body are derived using the kinematics of arms, legs, torso, and head. I. INTRODUCTION In this paper we present a derivation of the forward kinematics (FK) and inverse kinematics (IK) of a humanoid robot with 27 degrees of freedom, specifically the HUBO 2+ platform (Hubo). A picture of Hubo is shown in Fig.1. The FK and IK are not solved for the entire 27 joints but instead broken up into six parts. These parts consist of the two arms (six joints each), the two legs (six joints each), the torso (1 joint), and the head (2 joints). The majority of the paper is spent on the arms and the legs because the head and torso are almost trivial. However, even though the head and torso are simple to solve for as individual parts they can not be ignored because for a full body FK and IK they are essential components. Having the forward kinematics (FK) and inverse kinematics (IK) is crucial in any robot manipulator especially for a humanoid robot. This is because usually it is desirable to control the end-effector of the robot (e.g. the hand of one arm) in its workspace, not in joint space. In other words, tell the Fig. 1. The Humanoid Robot Hubo2+ for which we calculate kinematics. hand of the humanoid to go to point (x, y, z) not tell the joints to go to values θi. However, the joint values are what can be directly controlled, therefore, one needs to know how II. HUBO GEOMETRY AND KINEMATICS to convert from workspace to joint space and vice versa. In order to control a humanoid using workspace control Ali et. al. presented a closed-form solution for the inverse both forward and inverse kinematic solutions are required. The kinematics (IK) of the limbs of the HUBO2+ robot platform solution to this problem involves solving for the joint angles [1]. They used a reverse decoupling mechanism method by given a desired position and orientation while accounting for viewing the kinematic chain of a limb in reverse order and singularities, joint limits and feasible workspace issues. decoupling the position and orientation. The authors then used The kinematic structure of the right and left side of Hubo the inverse transform method to compute eight possible solu- are identical, therefore, the left and right arms have the tions for each limb. The correct solution is selected based on same joint coordinate frames and Denavit-Hartenberg (DH) joint limits and constraints. In working through their solution, parameters, as do the left and right legs. The only difference discrepancies were found in the calculations. We corrected the between the left and the right limbs is the offset direction errors and solved for the IK of all four limbs for our HUBO2+ from the base frame. We first go through the solution for the humanoid robot. Using the IK for the four limbs and the head IK of the arms, and then the legs. The joint coordinate frames and torso we have formulated an IK for the entire robot. are shown in Fig.2 and the length of each link is shown in This work was supported in part by DARPA #N65236-12-1-1005: DARPA TableI. The DH parameters (using the standard convention) Robotics Challenge and NSF CNS-0960061 MRI-R2: Unifying Humanoids for the arms and legs are shown in TableII and Table III, Research. yAuthors are affiliated with Georgia Institute of Technol- respectively. ogy, Atlanta, GA, 30332, USA. frowland.oflaherty, plvieira, mxgrey, [email protected] and A. Forward Kinematic Solution for the Arm fafb,[email protected]. zAuthor is affiliated with Drexel Univeristy, Philadelphia, PA, 19104, USA. The forward kinematics problem is that of solving for the [email protected]. end-effector orientation and position given the joint angles. 1 Thus, when solving for the forward kinematics of the end- A0 ZA2 effector with respect to the neck TA6 must be pre-multiplied N A6 by TA0 and post-multiplied by TE. Therefore, given a set XA2 Y of joint angles the FK is calculated as YA2 ZA1 A1 lA1 ZN XA1 N N A0 A6 TE = TA0 TA6 TE: (4) A1 YN A2 ZA0 XN XA0 YA0 l A2 A3 lT W ZL0 TABLE II. DH PARAMETERS OF THE ARMS Z W XL0 YL0 lL1 Right arm DH parameters A4 ZA3 θ α a d YW Coord. Frame i i i i i X YL1 XA3 YA3 W 1 θ1 + π=2 π=2 0 0 lA3 Z lL2 L1 A4 XL1 2 θ2 − π=2 π=2 0 0 A5 ZL1 3 θ3 + π=2 −π=2 0 −lA2 L2 XA4 YA4 A6 4 θ4 π=2 0 0 lA4 Z YL2 A5 θ −π=2 −l X 5 5 0 A3 L3 A5 ZL2 Y A5 6 θ6 + π=2 0 lA4 0 XL2 ZA6 lL3 XA6 Y YL3 YE A6 L4 ZL3 XE ZE X B. Inverse Kinematic Solution for the Arm l L3 L4 YL4 The inverse kinematics is the problem of solving for the L5 ZL4 X joint angles given the end-effector orientation and position, l L4 L5 Y N L6 L5 specified as TE. This is a much harder problem because there ZL5 XL5 YF are multiple solutions. When solving the inverse kinematics of Z F XF a manipulator, Pieper [2] indicates that a closed-form solution Fig. 2. Hubo coordinate frames exists if three consecutive joint axes of the manipulator are parallel to one another, or intersect at a single point. The three TABLE I. LINK LENGTHS OF HUBO shoulder joint axes on the Hubo intersect at a single point for the arms, and the three hip joints intersect at a single point for Lengths of leg links Lengths of arm links Link Length (mm) the legs, therefore a closed-form kinematic solution exists for Link Length (mm) lT 187 both the arms and the legs. lA1 215 lL1 88 lA2 179 lL2 182 We will solve the IK problem from the shoulder to the lA3 182 lL3 300 A0 lA4 121 hand by using the transformation TA6. This is obtained by lL4 300 lE 178 N A0 E lL5 95 pre-multiplying TE by TN and post-multiplying by TA6. Thus, A0T =A0 T N T ET : (5) This is easily solved using the robot geometry and coordinate A6 N E A6 A0 N frames, which are specified in the DH parameters. The general Let us write TA6 obtained from TE as homogeneous transformation from one link to the next given x y z p n s a p the DH parameters is represented in matrix form as: A0T = 6 6 6 6 = ; A6 0 0 0 1 0 0 0 1 (6) 2cos(θi) − sin(θi) cos(αi) sin(θi) sin(αi) ai cos(θi)3 i−1 sin(θi) cos(θi) cos(αi) − cos(θi) sin(αi) ai sin(θi) where x , y , and z are the unit vectors along the principal Ti = 6 7: (1) 6 6 6 4 0 sin(αi) cos(αi) di 5 axes of the hand frame and p is the position vector describing 0 0 0 1 6 the location of the hand relative to the shoulder. These three i−1 unit vectors describe the orientation of the hand coordinate where Ti is the transformation from coordinate frame i − 1 to frame i. The base frame for the arm is at the neck, and its frame relative to the shoulder coordinate frame. The vectors n, tranformation to the first shoulder joint is s, a, and p represent the normal vector, sliding vector, approach vector, and position vector of the hand, respectively [3]. 20 0 1 lA13 1 0 0 0 Using this knowledge, the arm can be viewed in reverse N T = 6 7 : A0 40 1 0 0 5 (2) so that the last three joints make up the shoulder, thus the 0 0 0 1 position and orientation of the shoulder frame can be described relative to the hand frame. This new position vector, p0, is only 6 a function of θ , θ and θ , and thus decouples the arm into An additional transformation TE is used for the transfor- 4 5 6 mation from the hand to the end-effector. In order calculate position and orientation components. The IK problem is solved the forward kinematics (FK), the six transformation matrices in this reverse method by taking the inverse of both sides of from each joint are pre-multiplied to obtain the position and (6). orientation of the end-effector relavtive to the shoulder. We 0 n s a p n0 s0 a0 p0 define the transformation from the shoulder to the hand as A0T 0 = = = A6T A6 0 0 0 1 0 0 0 1 A0 A0 A0 A1 A2 A3 A4 A5 TA6 = TA1 TA2 TA3 TA4 TA5 TA6: (3) (7) 2 3 As in [1], we will now show how to use this reverse method from which we solve for θ6 as to solve for the joint angles of the right arm.