Kinematics and Inverse Kinematics for the Humanoid Robot HUBO2+

Georgia Institute of Technology, Technical Report, GT-GOLEM-2013-001, 2013

Rowland O’Flaherty†, Peter Vieira†, Michael Grey†, Paul Oh‡, Aaron Bobick†, Magnus Egerstedt†, and Mike Stilman†

Abstract—This paper derives the forward and inverse kinematics of a humanoid robot. The speciﬁc 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. †Authors are affiliated with Georgia Institute of Technol- respectively. ogy, Atlanta, GA, 30332, USA. {rowland.oflaherty, plvieira, mxgrey, magnus}@gatech.edu and A. Forward Kinematic Solution for the Arm {afb,mstilman}@cc.gatech.edu. ‡Author 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 en h rnfrainfo h hudrt h adas hand the We to shoulder. shoulder the the and to from position relavtive transformation the the end-effector obtain define the to matrices pre-multiplied calculate of transformation are order orientation six joint In the each end-effector. (FK), from the kinematics to forward hand the the from mation transformation additional An is joint shoulder first the to tranformation frame to where as: form matrix given in next represented the is to parameters link DH general The one the parameters. coordinate from DH and transformation the geometry in homogeneous robot specified are the which using frames, solved easily is This frames coordinate Hubo 2. Fig. i − A 1 T 0 i T l = A4 i A − l l A2 A3    6 1 i cos( sin( T h aefaefrteami ttenc,adits and neck, the at is arm the for frame base The . = i 1 0 0 sin( 0 0 θ θ stetasomto rmcodnt frame coordinate from transformation the is A Link l l l l i egh famlinks arm of Lengths i l A A A A cos( ) ) AL .L I. TABLE E 0 l 4 3 2 1 L5 T l L4 l N − l L3 A L2 l A1 1 T sin(

egh(mm) Length A A θ l

L1 0 1 θ i l X cos( ) 178 121 182 179 215 T T i X N = Z cos( ) W α W A W i

2 cos( )    L4

Z X Y L6 α N A

W L0 N EGH OF LENGTHS INK 1 0 0 0 0 0 0 1 0 0 0 1 1 0 0 Y L5 α

i N 2

) i L3 L1 Z

sin( ) T F

X 6 L3

A L0 T Link l l l l l Y − X X Y 3 X l egh flglinks leg of Lengths L3 L L L L L Z L2 E L0 L4 F T A1 Z L1 Y L3 5 4 3 2 1 cos( A L4 Y Y

Z X F L2 L4 Z Y 3 Z A2 sue o h transfor- the for used is L5 L1 L2 θ X T Y L1 i θ A2 A2 X sin( ) A4 egh(mm) Length A i l E A6

sin( ) A Z A5

A3 α X 4 A2

1 i d ) A 300 300 182 187 Y    Z Y Z H 95 88 α E E L5 A1 4 α UBO i X . T a ) A0 i X a ) X X A A1 A5 A3 X Y 5 A0 A6 X A4 Y Y A Y A5 A1 i i A3 Y cos( Z sin( 5 A6 A0 Z T Z A5 i Z A4 A3 A θ θ Z Y A6 i A4 6 i ) ) .    . i − (2) (1) (3) 1 2 nti ees ehdb aigteivreo ohsdsof sides both of inverse the taking by method (6). solved reverse is problem this IK in The components. orientation and position coordinate hand vectors The the frame. three coordinate of These s shoulder shoulder. the orientation to the the relative to frame describe relative vectors hand the unit of p location and the frame hand the of axes x where ota h attrejit aeu h hudr hsthe thus vector, shoulder, of position new the function This described a up frame. be hand can make the frame to shoulder joints relative the three of orientation last and position the that so [3]. respectively hand, the of vector position and vector, write us Let Thus, transformation pre-multiplying the using by hand legs. for the for exists point and solution single arms kinematic a the for closed-form at point both a intersect single joints therefore a hip legs, at three the three the intersect The are and Hubo point. arms, manipulator the single the on a the axes at of intersect joint or shoulder axes another, joint one solution to consecutive parallel closed-form three a that if indicates of exists [2] kinematics inverse Pieper the manipulator, solving position, a When and solutions. multiple orientation are end-effector the as speciﬁed given angles joint Arm the for Solution Kinematic Inverse B. fjitage h Ki acltdas calculated is FK the end- angles the joint of of kinematics neck forward by the to the respect for with effector solving when Thus, , A a N 0 sn hskolde h r a evee nreverse in viewed be can arm the knowledge, this Using the to shoulder the from problem IK the solve will We the for solving of problem the is kinematics inverse The and , T T A A A 6 0 0 6 p 0 T y , or.Faei Frame Coord. n otmlile by post-multiplied and = A ersn h omlvco,siigvco,approach vector, sliding vector, normal the represent AL I DH II. TABLE N 6 6 A n z and , T = θ p a s n 1 0 0 0 0 6 5 4 3 2 1 4 E T N , A N hsi uhhre rbe eas there because problem harder much a is This . A x θ T 0 1 0 0 0 6 T 5 6 T E E 6 bandfrom obtained A and ih r Hparameters DH arm Right by 6 = r h ntvcosaogteprincipal the along vectors unit the are y θ θ θ θ = 6 2 6 3 1 A N θ − + + + A θ θ 0 θ 6 5 4 i 0 T T z AAEESO H ARMS THE OF PARAMETERS π/ π/ π/ π/ n hsdculsteaminto arm the decouples thus and , 0 6 6 A N T = 2 2 2 2 0 stepsto etrdescribing vector position the is N n otmliligby post-multiplying and A p A A N n 0 6 1 0 0 0 0 − − A T π/ π/ π/ 6 0 T T α π/ π/ N 0 T 0 A A i E = 2 2 2 T E T 6 2 2 s 6 A E 0 hrfr,gvnaset a given Therefore, . A E utb pre-multiplied be must 6 p a s n 6 1 0 0 0 T hsi bandby obtained is This . as l T a a A A 0 0 0 0 0 E i 0 4 6 . . p − − 0 d l l 0 0 0 0 A A i = 3 2 p 0 A sonly is , , 6 E T T A A 0 (6) (7) (5) (4) n 6

, .

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. To solve for the last three joint angles (i.e., the elbow, wrist yaw and wrist θ6 = wrapToPi(atan2(S4C5lA2, −C4lA2 − lA3) − ψ). (19) pitch angles) we can use the inverse transform method [1] by To solve for the three shoulder joints we calculate G3 by pre- A5 A4 A3 multiplying both sides of (7) by TA6. This results in an multiplying (8) and (9) by TA5 and TA4 to obtain, equation where the left side of the equation is1, 0 0 0 0 (left) A3 A4 A5 n s a p 0 0 0 0 G = TA4 TA5 TA6 = (left) n s a p 3−arm 0 0 0 1 G = A5T = 5−arm A6 0 0 0 1 g311 g312 g313 g314  0 0 0 0 0 0 0 0  −S6nx − C6ny, −S6sx − C6sy, −S6ax − C6ay, −S6(px + lA4) − C6py 0 0 0 0 0 0 0 0 g321 g322 g323 g324  C6nx − S6ny,C6sx − S6sy,C6ax − S6ay,C6(px + lA4) − S6py    (20) 0 0 0 0 , (8)    nz, sz, az, pz  g331 g332 g333 g334 0, 0, 0, 1 0 0 0 1 and the right side of G5 is on the left hand side and (right) A5 A5 A4 A3 A2 A1 G(right) = A3T = A3T A2T A1T = G5−arm = TA0 = TA4 TA3 TA2 TA1 TA0 = 3−arm A0 A2 A1 A0   g511 g512 g513 −lA2C5S4  C1C3 + S1S2S3 C3S1 − C1S2S3 C2S3 0 g521 g522 g523 −lA2C4 − lA3 −C2S1 C1C2 S2 −lA2 g g g l S S . (9)   531 532 533 A2 4 5 C3S1S2 − C1S3 −S1S3 − C1C3S2 C2C3 0  0 0 0 1 0 0 0 1 We can solve for the last three joint angles by equating the (21) position terms of (8) and (9) to get on the right hand side. By comparing element (2,3) of (20) 0 0 and (21) we obtain S2, and use that to get C2 and ﬁnally the S6(px + lA4) + C6py = S4C5lA2 (10) 0 0 shoulder roll joint solution θ2. Thus, C6(px + lA4) − S6py = −C4lA2 − lA3 (11) 0 0 0 S2 = g323 = ax(C4C6 − C5S4S6) − ay(C4S6 + C5C6S4) pz = lA2S4S5. (12) 0 − a S4S5, 0 z The joint angle θ4 is solved for by ﬁrst letting px +lA4 = rCψ q 0 2 and py = rSψ and then substituting these into (10) and (11) θ2 = atan2 S2, ±real 1 − S2 . (22) in addition to using the trigonometric sum identities to obtain, We compare elements (1,3) and (3,3) of (20) and (21) to obtain rS6ψ = S4C5lA2 (13) two equations. If we divide these we obtain S3 and C3 and rC6ψ = −C4lA2 − lA3 (14) then solve for the shoulder yaw joint solution θ3, 0 pz = S4S5lA2 (15) g C S 313 = 2 3 , q 0 2 0 2 0 0 g333 C2C3 where r = (px + lA4) + (py) and ψ = atan2(py, px + l ) 2 0 0 A4 . By then squaring (13), (14) and (15) and adding them, S3 = g313 = −ax(C6S4 + C4C5S6) + ay(S4S6 − C4C5C6) we can obtain an equation for C4: 0 − azC4S5, 0 2 0 2 0 2 2 2 0 0 0 (px + lA4) + py + pz − lA2 − lA3 C3 = g333 = −axS5S6 − ayC6S5 + azC5, C4 = , 2lA2lA3 θ3 = atan2(S3,C3). (23) from which we can obtain the elbow joint solution, We perform the same procedure to obtain the last joint solution, q θ1, which is the shoulder pitch angle. We compare elements 2 θ4 = atan2 ±real 1 − C4 ,C4 . (16) (2,1) and (2,2) of (20) and (21) and divide them to obtain, g321 −C2S1 From (15) we solve for S , and from that we solve for the = , 5 g322 C2C1 wrist yaw joint solution θ5, 0 S = −n0 (C C − C S S ) + n0 (C S + C C S )+ pz 1 x 4 6 5 4 6 y 4 6 5 6 4 S5 = , 0 S4lA2 nzS4S5, q C = s0 (C C − C S S ) − s0 (C S + C C S )− θ = atan2 S , ±real 1 − S2 . (17) 1 x 4 6 5 4 6 y 4 6 5 6 4 5 5 5 0 szS4S5,

To obtain the wrist pitch joint solution θ6 we ﬁrst divide (13) θ1 = atan2(S1,C1). (24) by (14) to get There are two solutions for θ2, θ4, and θ5, therefore there S6ψ S4C5lA2 are eight total solutions to the arm IK. When the goal position tan(θ6 + ψ) = = , (18) C6ψ −C4lA2 − lA3 is outside the feasible workspace of the limb, the joint solutions will have imaginary parts. To deal with this, we take only the 1 The sine and cosine of an angle α is abbreviated Sα and Cα, respectively. 2atan2(y, x) is the two argument arc tangent function. 3wrapToPi(α) wraps the angle α to the interval between −π and π.

3 real parts, which in turn gives the solution that is closest to the use a positive lL1 for the left leg and a negative lL1 for the desired position. Furthermore, there are five cases that result right leg. We define the transformation from the hip to the foot in singularities, and details of the solution methods can be as viewed in [1]. The following are our final equations for each L0T = L0T L1T L2T L3T L4T L5T . (26) case of the singularity conditions. L6 L1 L2 L3 L4 L5 L6 Thus, when solving for the forward kinematics W T must 1) Case 1 (elbow singularity): When θ = 0 joints θ and F 4 3 be pre-multiplied by W T and post-multiplied by L6T . θ are collinear, thus an infinite number of solutions exist to L0 F 5 Therefore, the FK is calculated as orient the end-effector in the desired orientation. We set θ3 to W W L0 L6 its previous value and define θS = θ5 + θ3. These joint angles TF = TL0 TL6 TF . (27) are solved by first solving for θ6 by using elements (2,4) and (1,4) of (5) and (6). Then solving for θS by using elements TABLE III. DH PARAMETERS OF THE LEGS (1,3) and (3,3) of (8) and (9). Finally, θ5 is found from θS. Right leg DH parameters Thus, Coord. Frame i θi αi ai di 0 0 1 θ1 + π/2 0 0 0 py −lA4 − p θ = atan2 , x , 2 θ2 − π/2 −π/2 0 0 6 3 θ 0 l 0 lA2 + lA3 lA2 + lA3 3 L3 0 0 0 0 4 θ4 0 lL4 0 θS = atan2(−C6ay − S6ax, az), 5 θ5 π/2 0 0 0 6 θ6 0 lL5 0 θS if C2 ≥ 0 θS = 0 , wrapToPi(θ + π) if C2 < 0 S D. Inverse Kinematic Solution for the Leg Similar to the IK for the arm the IK for the leg is solved θ5 = wrapToPi(θS − θ3). L0 from the hip to the foot by using the transformation TL6. This is obtained by pre-multiplying W T by L0T and post- 2) Case 2 (shoulder singularity): When θ = π/2 (for the F W 2 multiplying by F T . Let us write L0T for the legs in the left arm) or −π/2 (for the right arm) joints θ and θ are L6 L6 1 3 same way as for the arms in collinear. The same approach as above is performed. However, L0 L0 W F θS = θ3 ± θ5 and solving for it uses elements (2,1) and (2,2) TL6 = TW TF TL6. (28) of (8) and (9). This results in the following As with the arm, the three hip joint axes in the leg on Hubo 0 0 0 0 0 θS = atan2(S6sy − C6sx,S6ny − C6nx), intersect at a single point, therefore a closed-form kinematic θ0 if S ≥ 0 solution exists. The six tranformation matrices are obtained by θ = S 4 , S wrapToPi(θ0 + π) if S < 0 plugging the DH parameters into (1) and pre-multiplying them S 4 to obtain the position and orientation of the foot relavtive to the hip, wrapToPi(θS + θ3) for left arm x y z p n s a p θ1 = . L0T = 6 6 6 6 = (29) wrapToPi(θS − θ3) for right arm L6 0 0 0 1 0 0 0 1 where x , y and z are the unit vectors along the principal 3) Case 3 (elbow-shoulder singularity): When θ4 = 0 and 6 6 6 axes of the foot frame and describe the orientation of the foot θ2 = π/2 (for the left arm) or −π/2 (for the right arm) joints coordinate frame relative the the hip coordinate frame, and p θ1, θ3 and θ5 are collinear. The same approach as above is 6 is the position vector describing the location of the foot relative taken. However, θS = θ1 ± θ3 ± θ5, and solving for it uses elements (3,1) and (3,2) of (8) and (9). This results in the to the hip. The vectors in [n s a p] again represent the normal following, vector, sliding vector, approach vector and position vector of the foot, respectively [3]. LEF T 0 0 θS = atan2(nz, −sz), RIGHT 0 0 The leg can be viewed in reverse so that the last three θS = atan2(−nz, sz), joints make up the hip, and now the position and orientation LEF T wrapToPi(θ1 − θ3 − θS ) for left arm of the hip frame can be described relative to the foot frame. θ5 = RIGHT . This new position vector, p0, is only a function of θ , θ and wrapToPi(θS − θ1 − θ3) for right arm 4 5 θ6, and thus decouples the leg into position and orientation C. Forward Kinematic Solution for the Leg components. The IK problem is solved in this reverse method N by taking the inverse of both sides of (29), As with the arm, the forward kinematics of the leg, TF , 0 n s a p n0 s0 a0 p0 are straight forward once the DH parameters are derived. The L0T 0 = = = L6T . DH parameters for the right leg are shown in Table III and L6 0 0 0 1 0 0 0 1 L0 again (1) is used to find the transformation between adjacent (30) joint coordinate frames. The base frame for the leg is at the We can solve for the last three joint angles by equating the waist, and its tranformation to the first hip joint is, position terms of both sides of (30),   0 −1 1 ±lL1 −C (l C + l C ) = p0 + l , (31) 1 0 0 0 6 L3 45 L4 5 x L5 W   0 TL0 = , (25) S6(lL3C45 + lL4C5) = p , (32) 0 0 1 −lL2 y 0 0 0 0 1 −lL3S45 − lL4S5 = pz. (33)

4 We have three equations with the three unknowns θ4, θ5 and We can get the joint solution for θ2 by setting element (2,3) θ6. We can solve for C4 by squaring and adding all three of (41) and (42) equal to get, equations, and then obtain S4 from C4. This results in, 0 0 S2 = S6ax + C6ay (43) 0 2 0 2 0 2 2 3 (p + lL5) + p + p − l − l q x y z L3 L4 0 0 0 0 2 C4 = θ2 = atan2(S6ax + C6ay, ±real( 1 − (S6ax + C6ay) )). 2lL3lL4 q (44) 2 θ4 = atan2 ±real 1 − C4 ,C4 . (34) We get the joint solution for θ1 by comparing elements (2,1) and (2,2) of (41) and (42) and then dividing them to get, We can get θ5 by squaring equations (31) and (32) and adding 0 0 them to get, C2S1 = S6sx + C6sy (45) q 0 0 0 2 0 2 C2C1 = S6nx + C6ny (46) C5(C4lL3 + lL4) − S5(S4lL3) = ±real (px + lL5) + py . θ = atan2(S s0 + C s0 ,S n0 + C n0 ). (47) (35) 1 6 x 6 y 6 x 6 y Then, we can expand (33) to yield, If C2 < 0, then θ1 = wrapToPi(θ1 +π). Finally, to obtain the joint solution for θ3 we start by comparing elements (1,3) and 0 S5(C4lL3 + lL4) + C5(S4lL3) = −pz. (36) (3,3) of (41) and (42) to get two equations, and then divide them to get θ345, If we let C4lL3 + lL4 = rCψ and S4lL3 = rSψ and substitute 0 these into (35) and (36) we get, −C2S345 = az (48) q 0 0 0 2 0 2 −C2C345 = C6ax − S6ay (49) rC5ψ = ± (p + lL5) + p (37) x y θ = atan2(a0 ,C a0 − S a0 ) (50) 0 345 z 6 x 6 y rS5ψ = −pz (38) Then from (50) we can obtain θ3 by subtracting θ4 and θ5, where q θ3 = wrapToPi(θ345 − θ4 − θ5). (51) 0 2 0 2 0 2 r = ±real (px + lL5) + py + pz , As with the arm there are two solutions for θ2, θ4, and θ5, ψ = atan2(S4lL3,C4lL3 + lL4). which generate eight total solutions to the leg IK. Lik with the arm, if the goal position is outside the feasible workspace We can obtain tan(θ + ψ) by dividing (38) by (37), and then 5 of the limb the joint solutions will have imaginary parts and taking the inverse. By then subtracting ψ we can solve for the only the real parts are used. joint angle θ5, 0 θ5 = wrapToPi(atan2(−pz, E. Choosing Joint Solution q 0 2 0 2 ± real( (px + lL5) + py )) − ψ). (39) For the inverse kinematics of each of the arms and the legs there are eight joint solutions. The sum of squared joint values To get the joint solution for θ6 we can divide (32) by (31), is the primary metric that is used in picking one of the eight θ = atan2(p0 , −p0 − l ). (40) solutions. Choosing the solution that minimizes this metric is 6 y x L5 the solution that is “closest” to the zero position of the joints. If C45lL3 + C5lL4 < 0, then θ6 = wrapToPi(θ6 + π). Just as This works well if at least one of the solutions has all of its with the right arm, we will use the reverse method to solve for joints values within the joint limits (TableIV). the joint angles of the right leg. To solve for the last three joint If none of the solutions have all the joint values within the angles (i.e., the knee, ankle roll and ankle pitch joint angles) limits then there is no solution that satisﬁes the desired pose we can use the inverse transform method, just as we did for the (orientation and position). To get the end-effector to a position right arm, by multiplying both sides of (30) by L5T . This L6 as close as possible to the desired position the joint values in results in an equation we will call G , where the left hand side 5 all the solutions are capped at the closest joint limit value. Each of the equation is, of the solutions are then given to the FK to calculate the end- n0 s0 a0 p0 effector location with the capped joint values. The solution G(left) = L5T = 5−leg L6 0 0 0 1 that gets the end-effector position the closest to the desired  0 0 0 0 0 0 0 0  position is used. If none of the joint solutions get the end- C6nx − S6ny,C6sx − S6sy,C6ax − S6ay,C6(px + lL5) − S6py 0 0 0 0 0 0 0 0 effector within 5 cm of the desired position then the previous S6nx + C6ny,S6sx + C6sy,S6ax + C6ay,S6(px + lL5) + C6py  0 0 0 0   nz, sz, az, pz  joint values are used. 0, 0, 0, 1 (41) TABLE IV. JOINTLIMITSOFTHEARMSANDLEGS Arms Legs and the right hand side of G5 is, Joint Left Right Left Right (right) L5 L5 L4 L3 L2 L1 i min. max. min. max. min. max. min. max. G5−leg = TL0 = TL4 TL3 TL2 TL1 TL0 = 0 -2.0 2.0 -2.0 2.0 0 1.8 -1.8 0   1 -0.3 2.0 -2.0 0.3 0 0.6 -0.6 0 g511 g512 g513 −lL3C45 − lL4C5 2 -2.0 2.0 -2 2.0 -1.3 1.4 -1.3 1.4 g g g 0  521 522 523  . (42) 3 -2.5 0 -2.5 0 0 2.5 0 2.5 g531 g532 g533 −lL3S45 − lL4S5  4 -2.5 2.0 -2.5 2 -1.3 1.8 -1.3 1.8 5 -1.4 1.2 -1.4 1.2 -0.3 0.2 -0.2 0.3 g541 g542 g543 1

5 F. Forward and Inverse Kinematics for the Torso where we assume that the robot has already walked to a suitable position, then the IK is as follows: The kinematics for the torso are almost trivial because W the torso only has one joint at the waist, but is still worth 1) Set TF to identity with the exception of setting the presenting. The transformation from the waist to the neck is (2,4) element to py − lT and use the leg IK with this C −S 0 0  transformation to put the neck at the correct height. θT θT W S C 0 0 2) Calculate TE using the FK of the legs. W T =  θT θT  , (52) W W N  0 0 1 l  3) Set θT to −atan2( px, py) (left hand) or π − T W W W 0 0 0 1 atan2( px, py) (right hand), where p is the W position vector of TE. where θT is the angle of the waist joint. Thus, (52) is the FK 4) Calculate N T using the FK of the legs and waist W E for the torso. The IK is also just as simple. Given TN in the and use that with the IK of the arms to position the form end-effector. n s a p W T = (53) N 0 0 0 1 III.CONCLUSION then the angle of the waist joint is In this paper we have derived the forward and inverse kinematics for a humanoid robot with 27 degrees of freedom. θT = atan2(ny, nx). (54) The speciﬁc humanoid robot that this was designed for is the HUBO2+ platform, but this can easily be used for any robot G. Forward and Inverse Kinematics for the Head with the same joint conﬁguration. The kinematics solutions The kinematics for the head have two joints, so they are were divided up into four parts: arm, leg, torso, and head. slightly more complex than the torso, but still simple. The Analytical solutions were derived for each part. Then, using transformation from the neck to the head is these analytical solutions an algorithm is developed to obtain the forward and inverse kinematics for the entire robot. N N H0 H1 H2 TH = TH0 TH1 TH2 TH = C1C2 −S1 C1S2 lH2C1S2  REFERENCES C S C S S l S S  2 1 1 1 2 H2 1 2  . (55) [1] M. A. Ali, H. A. Park, and C. S. G. Lee, “Closed-form Inverse Kinematic  −S2 0 C1 lH1 + lH2C1 Joint Solution for Humanoid Robots,” pp. 704–709, 2010. 0 0 0 1 [2] D. L. Peiper, “The Kinematics of Manipulators Under Computer Con- Thus, (55) is the FK for the head given the two joint angles. trol,,” Oct. 1968. Given N T in the form [3] K. S. Fu, R. C. Gonzalez, and C. S. G. Lee, Robotics: Control, Sensing, H Vision, and Intelligence. McGraw-Hill, Inc., Jan. 1987. n s a p N T = H 0 0 0 1 (56) the IK for the head is solved by

θ1 = atan2(sy, −sx), θ2 = atan2(nx, ax). (57)

H. Forward and Inverse Kinematics for the Full Body With a humanoid robot most tasks are going to be pre- formed with the hands or with some tool that the hands are manipulating. Therefore, ultimately one would like to be able to specify some pose in the global frame for the end-effector of the hand and have all the joints on the robot move so that pose is reached. To accomplish this full body IK we break the problem up into parts. First, we assume the robot can walk to a location such that the desired hand location is reachable the hand. Next, the legs will be used to position the neck at the height of the desired hand position. This is because the workspace plane for the arms is maximized at shoulder height, which is the same level as the neck frame. Next, the waist will rotate so that the shoulder is as close as possible to the desired hand location. Finally, the arm will move the hand to the desired pose. Given a desired transformation from the global frame to the end-effector in the form n s a p GT = , E 0 0 0 1 (58)