# Analytical Solutions of the Inverse Kinematics of a Humanoid Robot

This example shows how to derive analytical solutions for the inverse kinematics of the head chain of a humanoid robot.

### Step 1: Define Parameters

Describe the kinematics of the head-chain link (the link between the torso and the head) of the NAO humanoid robot [1] using the Denavit-Hartenberg (DH) parameters and notations based on a study by Kofinas et al. [2]. The following transformation defines the head-chain link.

`$T={A}_{Base,0}{T}_{0,1}{T}_{1,2}{R}_{x}{R}_{y}{A}_{2,Head}$`

where:

• ${A}_{Base,0}$ is the translation from the base (torso) to the joint or reference frame 0

• ${T}_{0,1}$ is the orientation of reference 1 relative to 0

• ${T}_{1,2}$ is the orientation of reference 2 relative to 1

• ${R}_{x}$ is the roll rotation

• ${R}_{y}$ is the pitch rotation

• ${A}_{2,Head}$ is the translation from reference 2 to the end-effector point (the head)

$T\left(1:3,4\right)$ defines the coordinates of the head, which are $xc$, $yc$, $zc$.

In this example, you analytically solve the inverse kinematics problem by returning all orientations of individual joints in the head-chain link given head coordinates of `xc`, `yc`, and `zc` within the reachable space. Then, you convert the analytical results to purely numeric functions for efficiency.

Solving analytically (when doing so is possible) lets you perform computations in real time and avoid singularities, which cause difficulty for numerical methods.

### Step 2: Define Forward Kinematics of Head Chain Using DH Parameters

The function `getKinematicChain` returns the specific kinematic chain for the NAO robot in terms of symbolic variables. For details on `getKinematicChain`, see the Helper Functions section.

```kinChain = 'head'; dhInfo = getKinematicChain(kinChain); T = dhInfo.T```
```T =  $\left(\begin{array}{cccc}{r}_{11}& {r}_{12}& {r}_{13}& \mathrm{xc}\\ {r}_{21}& {r}_{22}& {r}_{23}& \mathrm{yc}\\ {r}_{31}& {r}_{32}& {r}_{33}& \mathrm{zc}\\ 0& 0& 0& 1\end{array}\right)$```

Express the forward kinematics transformation matrix `T` as the following sequence of products: `T = ABase0*T01*T12*Rx*Ry*A2Head`. Define the individual matrices as follows.

Specify the translation matrix from the torso (base) to joint 0.

`ABase0 = dhInfo.ABase0`
```ABase0 =  $\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& \mathrm{NeckOffsetZ}\\ 0& 0& 0& 1\end{array}\right)$```

Specify the transformation matrix from joint 0 to joint 1.

`T01 = dhInfo.T01`
```T01 =  $\left(\begin{array}{cccc}\mathrm{cos}\left({\theta }_{1}\right)& -\mathrm{sin}\left({\theta }_{1}\right)& 0& 0\\ \mathrm{sin}\left({\theta }_{1}\right)& \mathrm{cos}\left({\theta }_{1}\right)& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right)$```

Specify the transformation matrix from joint 1 to joint 2.

`T12 = dhInfo.T12`
```T12 =  $\left(\begin{array}{cccc}\mathrm{cos}\left({\theta }_{2}-\frac{\pi }{2}\right)& -\mathrm{sin}\left({\theta }_{2}-\frac{\pi }{2}\right)& 0& 0\\ 0& 0& 1& 0\\ -\mathrm{sin}\left({\theta }_{2}-\frac{\pi }{2}\right)& -\mathrm{cos}\left({\theta }_{2}-\frac{\pi }{2}\right)& 0& 0\\ 0& 0& 0& 1\end{array}\right)$```

Specify the roll rotation matrix.

`Rx = dhInfo.Rx`
```Rx =  $\left(\begin{array}{cccc}1& 0& 0& 0\\ 0& 0& -1& 0\\ 0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right)$```

Specify the pitch rotation matrix.

`Ry = dhInfo.Ry`
```Ry =  $\left(\begin{array}{cccc}0& 0& 1& 0\\ 0& 1& 0& 0\\ -1& 0& 0& 0\\ 0& 0& 0& 1\end{array}\right)$```

Specify the translation matrix from joint 2 to the head.

`A2Head = dhInfo.A2Head`
```A2Head =  $\left(\begin{array}{cccc}1& 0& 0& \mathrm{CameraX}\\ 0& 1& 0& 0\\ 0& 0& 1& \mathrm{CameraZ}\\ 0& 0& 0& 1\end{array}\right)$```

The known parameters of this problem are `CameraX`, `CameraZ`, `NeckOffsetZ`, and the positions `xc`, `yc`, and `zc`. The unknown parameters are ${\theta }_{1}$ and ${\theta }_{2}$. After finding ${\theta }_{1}$ and ${\theta }_{2}$, you can compute the individual transformations of `T`. The robot can then achieve the desired position `xc`, `yc`, `zc`.

Although you can see these parameters in the transformation matrices, they do not exist as variables in the MATLAB base workspace. This is because these parameters originate from a function. Functions do not use the base workspace. Each function workspace is separate from the base workspace and all other workspaces to protect the data integrity. Thus, to use these variables outside of the function `getKinematicChain`, use `syms` to create them.

`syms CameraX CameraZ NeckOffsetZ xc yc zc theta_1 theta_2 real;`

### Step 3: Find Algebraic Solutions for ${\theta }_{1}$ and ${\theta }_{2}$

Rewrite the equation `T = ABase0*T01*T12*Rx*Ry*A2Head`, separating the terms that describe the torso and head movements of the robot: `inv(T01)*inv(ABase0)*T = T12*Rx*Ry*A2Head`. Simplify the left and right sides of the equation, and define equations of interest matching the expressions for coordinate positions.

```LHS = simplify(inv(T01)*inv(ABase0)*T); RHS = simplify(T12*Rx*Ry*A2Head); eqns = LHS(1:3,end) - RHS(1:3,end)```
```eqns =  $\left(\begin{array}{c}\mathrm{xc} \mathrm{cos}\left({\theta }_{1}\right)-\mathrm{CameraZ} \mathrm{sin}\left({\theta }_{2}\right)-\mathrm{CameraX} \mathrm{cos}\left({\theta }_{2}\right)+\mathrm{yc} \mathrm{sin}\left({\theta }_{1}\right)\\ \mathrm{yc} \mathrm{cos}\left({\theta }_{1}\right)-\mathrm{xc} \mathrm{sin}\left({\theta }_{1}\right)\\ \mathrm{zc}-\mathrm{NeckOffsetZ}-\mathrm{CameraZ} \mathrm{cos}\left({\theta }_{2}\right)+\mathrm{CameraX} \mathrm{sin}\left({\theta }_{2}\right)\end{array}\right)$```

This system of equations contains two variables for which you want to find solutions, ${\theta }_{1}$ and ${\theta }_{2}$. However, the equations also imply that you cannot arbitrarily choose `xc`, `yc`, and `zc`. Therefore, also consider `yc` as a variable. All other unknowns of the system are symbolic parameters.

This example follows a typical algebraic approach for solving inverse kinematics problems [3]. The idea is to get a compact representation of the solution, where the expression of each variable is in terms of parameters and variables for which you already solved the equations. As a first step, find either ${\theta }_{1}$ or ${\theta }_{2}$ in terms of the parameters. Then, express the other variable in terms of the known variable and parameters. To do this, start by identifying the equation that has only one variable.

`intersect(symvar(eqns(1)),[theta_1,theta_2,yc])`
`ans = $\left(\begin{array}{ccc}{\theta }_{1}& {\theta }_{2}& \mathrm{yc}\end{array}\right)$`
`intersect(symvar(eqns(2)),[theta_1,theta_2,yc])`
`ans = $\left(\begin{array}{cc}{\theta }_{1}& \mathrm{yc}\end{array}\right)$`
`intersect(symvar(eqns(3)),[theta_1,theta_2,yc])`
`ans = ${\theta }_{2}$`

The third equation contains only one variable, ${\theta }_{2}$. Solve this equation first.

```[theta_2_sol,theta_2_params,theta_2_conds] = ... solve(eqns(3),theta_2,'Real',true,'ReturnConditions',true)```
```theta_2_sol =  $\left(\begin{array}{c}2 \pi k-2 \mathrm{atan}\left(\frac{\mathrm{CameraX}-\sqrt{{\mathrm{CameraX}}^{2}+{\mathrm{CameraZ}}^{2}-{\mathrm{NeckOffsetZ}}^{2}+2 \mathrm{NeckOffsetZ} \mathrm{zc}-{\mathrm{zc}}^{2}}}{\mathrm{CameraZ}-\mathrm{NeckOffsetZ}+\mathrm{zc}}\right)\\ 2 \pi k-2 \mathrm{atan}\left(\frac{\mathrm{CameraX}+\sqrt{{\mathrm{CameraX}}^{2}+{\mathrm{CameraZ}}^{2}-{\mathrm{NeckOffsetZ}}^{2}+2 \mathrm{NeckOffsetZ} \mathrm{zc}-{\mathrm{zc}}^{2}}}{\mathrm{CameraZ}-\mathrm{NeckOffsetZ}+\mathrm{zc}}\right)\end{array}\right)$```
`theta_2_params = $k$`
```theta_2_conds =  $\left(\begin{array}{c}k\in \mathbb{Z}\wedge \mathrm{CameraZ}+\mathrm{zc}\ne \mathrm{NeckOffsetZ}\wedge {\left(\mathrm{NeckOffsetZ}-\mathrm{zc}\right)}^{2}\le {\mathrm{CameraX}}^{2}+{\mathrm{CameraZ}}^{2}\\ k\in \mathbb{Z}\wedge \mathrm{CameraZ}+\mathrm{zc}\ne \mathrm{NeckOffsetZ}\wedge {\left(\mathrm{NeckOffsetZ}-\mathrm{zc}\right)}^{2}\le {\mathrm{CameraX}}^{2}+{\mathrm{CameraZ}}^{2}\end{array}\right)$```

The solutions have an additive $2\pi k$ term with the parameter $k$. Without loss of generalization, you can set $k$ equal to 0 in the solutions and conditions.

`theta_2_sol = subs(theta_2_sol,theta_2_params,0)`
```theta_2_sol =  $\left(\begin{array}{c}-2 \mathrm{atan}\left(\frac{\mathrm{CameraX}-\sqrt{{\mathrm{CameraX}}^{2}+{\mathrm{CameraZ}}^{2}-{\mathrm{NeckOffsetZ}}^{2}+2 \mathrm{NeckOffsetZ} \mathrm{zc}-{\mathrm{zc}}^{2}}}{\mathrm{CameraZ}-\mathrm{NeckOffsetZ}+\mathrm{zc}}\right)\\ -2 \mathrm{atan}\left(\frac{\mathrm{CameraX}+\sqrt{{\mathrm{CameraX}}^{2}+{\mathrm{CameraZ}}^{2}-{\mathrm{NeckOffsetZ}}^{2}+2 \mathrm{NeckOffsetZ} \mathrm{zc}-{\mathrm{zc}}^{2}}}{\mathrm{CameraZ}-\mathrm{NeckOffsetZ}+\mathrm{zc}}\right)\end{array}\right)$```
```for p = 1:numel(theta_2_conds) theta_2_conds(p) = simplify(subs(theta_2_conds(p),theta_2_params,0)); end ```

Now solve for the variables ${\theta }_{1}$ and `yc` in terms of the variable ${\theta }_{2}$ and other symbolic parameters.

```[theta_1_sol,yc_sol,yc_theta_1_params,yc_theta_1_conds] = ... solve(eqns(1:2),theta_1,yc,'Real',true,'ReturnConditions',true);```

Display the solutions for ${\theta }_{1}$ and `yc`.

`theta_1_sol`
```theta_1_sol =  ```
`yc_sol`
```yc_sol =  ```

The solutions depend on two parameters, $k$ and $x$.

`yc_theta_1_params`
`yc_theta_1_params = $\left(\begin{array}{cc}k& x\end{array}\right)$`

The solutions have an additive $2\pi k$ term. Without loss of generalization, you can set $k$ equal to 0 in the solution for `theta_1_sol` and the conditions `yc_theta_1_conds`.

`theta_1_sol = simplify(subs(theta_1_sol,yc_theta_1_params,[0,0]))`
```theta_1_sol =  ```
```for p = 1:numel(yc_theta_1_conds) yc_theta_1_conds(p) = simplify(subs(yc_theta_1_conds(p),yc_theta_1_params,[0,0])); end```

A similar substitution is not required for `yc_sol` since there is no dependency on the parameters.

`intersect(symvar(yc_sol),yc_theta_1_params)`
``` ans = Empty sym: 1-by-0 ```

### Step 4: Verify the Solutions

Starting with an arbitrary set of known numeric values for ${\theta }_{1}$ and ${\theta }_{2}$, compute the numeric values of the end-effector positions `xc`, `yc`, and `zc` with forward kinematics. Then, work backwards from `xc`, `yc`, and `zc` to compute all possible numeric values for ${\theta }_{1}$ and ${\theta }_{2}$ using the inverse kinematics expressions from the previous computation. Verify that one set of the inverse solutions matches the starting set of numeric values for ${\theta }_{1}$ and ${\theta }_{2}$.

Set the fixed parameters for the robot. The values in this example are for illustration purposes only.

```CameraXValue = 53.9; CameraZValue = 67.9; NeckOffsetZValue = -5;```

Using forward computations, find the end-effector positions corresponding to the values ${\theta }_{1}$ and ${\theta }_{2}$.

```Tfk = ABase0*T01*T12*Rx*Ry*A2Head; xyz = Tfk(1:3,end);```

Create a symbolic function for these positions.

```xyzFunc = symfun(subs(xyz,[CameraX,CameraZ,NeckOffsetZ], ... [CameraXValue,CameraZValue,NeckOffsetZValue]),[theta_1,theta_2]);```

For forward kinematics, specify two variables `theta_1_known` and `theta_2_known` that contain an arbitrary starting set of values for ${\theta }_{1}$ and ${\theta }_{2}$.

```theta_1_known = [pi/4,pi/6,pi/8,pi/10]; theta_2_known = [pi/8,-pi/12,pi/16,-pi/10]; numPts = numel(theta_1_known); num_theta_2 = numel(theta_2_sol); num_theta_1 = numel(theta_1_sol);```

Note that there are potentially `num_theta_1` solutions for each `num_theta_2` solution.

Use the following sequence of operations to verify the solutions.

1. Loop over `(theta_1_known,theta_2_known)`. For each point, compute the end positions `x_known`, `y_known`, and `z_known`, which are then "known".

2. Loop over the solutions for ${\theta }_{2}$ corresponding to `x_known` and `z_known`. For each solution, check to see if the corresponding condition `cond_theta_2` is valid. If the condition is valid, compute the corresponding numeric solution `theta_2_derived`.

3. Loop over the solutions for ${\theta }_{1}$ corresponding to `x_known`, `z_known`, and `theta_2_derived`. For each solution, check to see if the corresponding condition `cond_theta_1` is valid. If the condition is valid, check to see if `y_derived` numerically matches `y_known` within relative and absolute tolerances through the condition `cond_yc`. If this condition is valid, then compute `theta_1_derived`.

4. Collect the results in a table `T` for display purposes.

```T = table([],[],[],[],'VariableNames',{'theta_1_known','theta_2_known',... 'theta_1_derived','theta_2_derived'}); for ix1 = 1:numPts xyz_known = double(xyzFunc(theta_1_known(ix1),theta_2_known(ix1))); x_known = xyz_known(1); y_known = xyz_known(2); z_known = xyz_known(3); for ix2 = 1:num_theta_2 % theta_2 loop cond_theta_2 = subs(theta_2_conds(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]); if isAlways(cond_theta_2) % if theta_2 is valid theta_2_derived = subs(theta_2_sol(ix2),[CameraX,CameraZ,NeckOffsetZ,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,x_known,z_known]); theta_2_derived = double(theta_2_derived); for ix3 = 1:num_theta_1 % theta_1 loop cond_theta_1 = subs(yc_theta_1_conds(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]); if isAlways(cond_theta_1) % if theta_1 is valid y_derived = subs(yc_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]); y_derived = double(y_derived); cond_yc = abs(y_known - y_derived) < max(100*eps,1e-6*abs(y_known)); % check rounding if isAlways(cond_yc) % if yc is valid theta_1_derived = subs(theta_1_sol(ix3),[CameraX,CameraZ,NeckOffsetZ,theta_2,xc,zc],... [CameraXValue,CameraZValue,NeckOffsetZValue,theta_2_derived,x_known,z_known]); theta_1_derived = double(theta_1_derived); T = vertcat(T,table(theta_1_known(ix1),theta_2_known(ix1),theta_1_derived,... theta_2_derived,'VariableNames',T.Properties.VariableNames)); end end end end end end T```
```T=8×4 table theta_1_known theta_2_known theta_1_derived theta_2_derived _____________ _____________ _______________ _______________ 0.7854 0.3927 0.7854 0.3927 0.7854 0.3927 -2.3562 -1.7346 0.5236 -0.2618 0.5236 -0.2618 0.5236 -0.2618 -2.618 -1.0801 0.3927 0.19635 0.3927 0.19635 0.3927 0.19635 -2.7489 -1.5383 0.31416 -0.31416 0.31416 -0.31416 0.31416 -0.31416 -2.8274 -1.0278 ```

Observe that there are two possible solution pairs `(theta_1_derived,theta_2_derived)` obtained using inverse kinematics for each pair of starting angles `(theta_1_known,theta_2_known)`. One set of the inverse solutions matches the starting angles.

### References

1. SoftBank Robotics. NAO. https://www.aldebaran.com/en/nao.

2. Kofinas, N., E. Orfanoudakis, and M. G. Lagoudakis. "Complete Analytical Inverse Kinematics for NAO." In 2013 13th International Conference on Autonomous Robot Systems. Lisbon, Portugal: Robotica, 2013.

3. Kendricks, K. "Solving the Inverse Kinematic Robotics Problem: A Comparison Study of the Denavit-Hartenberg Matrix and Groebner Basis Theory." Ph.D. Thesis. Auburn University, Auburn, AL, 2007.

### Helper Functions

```function kinChain = getKinematicChain(link) % This function returns the kinematic chain of the NAO humanoid robot % represented using Denavit-Hartenberg (DH) parameters. % The function uses as a reference the paper: Complete analytical inverse % kinematics for NAO by Kofinas, N., Orfanoudakis, E., Lagoudakis, M.G., % 2013 13th International Conference on Autonomous Robot Systems % (Robotica), Publication Year: 2013. if nargin < 1 link = 'head'; end % Notation: A, R, and T are the translation, rotation, and DH % transformation matrices, respectively. % Specify DH parameters for the desired end configuration. syms r11 r12 r13 r21 r22 r23 r31 r32 r33 xc yc zc real; R = [r11 r12 r13;r21 r22 r23;r31 r32 r33]; kinChain.T = [R,[xc;yc;zc];0,0,0,1]; switch link case 'head' % Kinematic chain from torso (base) to head. syms CameraX CameraZ NeckOffsetZ real; % Translation from torso (base) to joint 0. ABase0 = getA([0 0 NeckOffsetZ]); % Translation from joint 2 to head. A2Head = getA([CameraX 0 CameraZ]); % Transformation from joint 0 to joint 1. % theta_1 is the rotation angle corresponding to the 0-1 link. syms theta_1 real; alpha1 = 0; a1 = 0; d1 = 0; T01 = getT(a1,alpha1,d1,theta_1); % Transformation from joint 1 to joint 2. % theta_2 is the rotation angle corresponding to the 1-2 link. syms theta_2 real; piby2 = str2sym('pi/2'); d2 = 0; a2 = 0; alpha2 = -piby2; T12 = getT(a2,alpha2,d2,theta_2-piby2); % Rx is the roll rotation. Rx = getR('x',piby2); % Ry is the pitch rotation. Ry = getR('y',piby2); % Capture the kinematic chain as a string. % The transformation is from the base to the head. kinChain.DHChain = 'ABase0*T01*T12*Rx*Ry*A2Head'; kinChain.ABase0 = ABase0; kinChain.T01 = T01; kinChain.T12 = T12; kinChain.Rx = Rx; kinChain.Ry = Ry; kinChain.A2Head = A2Head; case 'lefthand' % Kinematic chain from torso to left hand. syms ShoulderOffsetY ElbowOffsetY ShoulderOffsetZ real; ABase0 = getA([0 (ShoulderOffsetY+ElbowOffsetY) ShoulderOffsetZ]); syms HandOffsetX LowerArmLength real; AEnd4 = getA([(HandOffsetX+LowerArmLength) 0 0]); piby2 = str2sym('pi/2'); syms theta_1 real; alpha1 = -piby2; a1 = 0; d1 = 0; T01 = getT(a1,alpha1,d1,theta_1); syms theta_2 real; d2 = 0; a2 = 0; alpha2 = piby2; T12 = getT(a2,alpha2,d2,theta_2-piby2); syms UpperArmLength theta_3 real; d3 = UpperArmLength; a3 = 0; alpha3 = -piby2; T32 = getT(a3,alpha3,d3,theta_3); syms theta_4 real; d4 = 0; a4 = 0; alpha4 = piby2; T43 = getT(a4,alpha4,d4,theta_4); Rz = getR('z',piby2); kinChain.DHChain = 'ABase0*T01*T12*T32*T43*Rz*AEnd4'; kinChain.ABase0 = ABase0; kinChain.T01 = T01; kinChain.T12 = T12; kinChain.T32 = T32; kinChain.T43 = T43; kinChain.Rz = Rz; kinChain.AEnd4 = AEnd4; end end function A = getA(vec) % This function returns the translation matrix. A = [1 0 0 vec(1); 0 1 0 vec(2); 0 0 1 vec(3); 0 0 0 1]; end function R = getR(orientation,theta) % This function returns the rotation matrix. switch orientation case 'x' R = [1 0 0 0; 0 cos(theta) -sin(theta) 0; 0 sin(theta) cos(theta) 0 0 0 0 1]; case 'y' R = [cos(theta) 0 sin(theta) 0; 0 1 0 0; -sin(theta) 0 cos(theta) 0; 0 0 0 1]; case 'z' R = [cos(theta) -sin(theta) 0 0; sin(theta) cos(theta) 0 0; 0 0 1 0; 0 0 0 1]; end end function T = getT(a,alpha,d,theta) % This function returns the Denavit-Hartenberg (DH) matrix. T = [cos(theta),-sin(theta),0,a; sin(theta)*cos(alpha),cos(theta)*cos(alpha),-sin(alpha),-sin(alpha)*d; sin(theta)*sin(alpha),cos(theta)*sin(alpha),cos(alpha),cos(alpha)*d; 0,0,0,1]; end```