Parent topics
HULKs Bootcamp

## Introduction

In this task you are supposed to help the Nao robot kick the ball as close to the target position as possible. To simplify this task, we will work in 2-dimensions and you can only control the robots right leg. Everything else is fixed in position. For defining a kick motion, you should set the angles for the three parts of the Nao leg (thigh, tibia and foot).

Note that every joint has a minimum and maximum angle.

## Keyframes

A motion is defined by a list of keyframes. Each keyframe contains the target angles (in radians) of the thigh, tibia and foot joints as well as the duration in which the keyframe should be executed. The position of the three joints and the neutral angles can be seen in the picture below.

In order to familiarise you with the concept of keyframes, your first task is to find keyframes by trying out different values. The text editor below already contains a list of 5 keyframes. When you press the "Generate animation button", you will see how these keyframes make the Nao robot move.

Now, it is your turn to tell the Nao how to move. Change the angle and duration values in the keyframes, delete keyframes, or add new ones. And don't forget to regularly try out your code by pressing "Save" and then "Generate animation".

Remember that the ultimate goal is to kick the ball as close to the target as possible, so try to generate motions that resemble a kick.

## Kinematic Chain

In the following sections, we explore how end effector positions and joint angles can be computed with kinematic chains. A kinematic chain is a mathematical model of how joints are connected together. It allows to calculate the position of the end effector from joint angles (forward kinematics) or joint angles from the position of the end effector (inverse kinematics).

### Forward Kinematics

With the so called forward kinematics one can calculate the position of the end effector from the set of involved joint angles. Just from common sense it is obvious that such a calculation is possible because the position in space is exactly defined when given all joint angles. Since we want to control the robot's leg, we need a model of the leg first. Recall the leg structure that we introduced earlier:

The kinematic chain is constructed starting at the hip of the robot where the leg is mounted. In our case, this is the position of the hip joint or `position_0`. For calculating `position_1` at the end of the thigh with the length `length_1` and angle `theta_1`:

``````theta_1_with_offset = theta_1 - math.radians(45)

position_1 = Vec2d(
position_0.x + math.cos(theta_1_with_offset) * length_1,
position_0.y + math.sin(theta_1_with_offset) * length_1,
)
``````

The end position of the tibia (`position_2`) is rotated by `theta_2` around `position_1` and translated by `length_2`. Since the zero angle depends on `theta_1`, it is added to `theta_2` (rotating the hip joint also rotates the end effector of our kinematic chain):

``````theta_2_with_offset = theta_1_with_offset + theta_2 - math.radians(90)

position_2 = Vec2d(
position_1.x + math.cos(theta_2_with_offset) * length_2,
position_1.y + math.sin(theta_2_with_offset) * length_2,
)
``````

Lastly, the end position of the foot span (`position_3`) is rotated by `theta_3` around `position_2` and translated by `length_3`. Again, the previous rotation `theta_2` is added to `theta_3`:

``````theta_3_with_offset = theta_2_with_offset + theta_3 + math.radians(111.61)

position_3 = Vec2d(
position_2.x + math.cos(theta_3_with_offset) * length_3,
position_2.y + math.sin(theta_3_with_offset) * length_3,
)
``````

When combining all calculations together, we can now calculate all positions and rotations of the joints given the joint angles. In the following code, the forward kinematics calculations are defined in the function `forward_kinematics()`. This function takes as arguments the initial position `position_0`, all joint angles `theta_*`, and all segment lengths `length_*`. It returns all joint positions and the rotation of the end effector, i.e. `rotation_3` (at `position_3`):

You can execute the above code by saving it and then clicking the "Apply Forward Kinematics" button. Feel free to change the code (e.g. the angles `theta_*`).

The output shows a visualization of the calculated joint positions with the help of the forward kinematics:

We can see that the forward kinematics are an easy way to calculate the end effector position and rotation given all joint angles and segment lengths. In the next section, we will use inverse kinematics to calculate the joint angles from a given end effector position and rotation.

### Inverse Kinematics

Inverse kinematics describe the inverse transform of the above described forward kinematics. In other words, when given a desired position and rotation of an end effector, one can obtain the joint angles necessary to reach this position from the inverse kinematics. One could use the forward kinematics to form an equation system for finding the joint angles (forward kinematics: joint angles -> position, inverse kinematics: position -> joint angles).

In contrast to the forward kinematics such a solution does not always exist and even if it exists, it can be ambiguous. In our example, we may have one of three possible cases: 1. no solution (impossible to reach that position), 2. one solution (only one set of joint angles reach the position), 3. two solutions (position is reachable with two sets of joint angles). As a more understandable example, consider holding your hand in fixed position in front of your head. You will find that there exist an infinite amount of different arm angles to achieve this, i.e. there exist infinite solutions for possible joint angles.

Calculating the inverse kinematics can be performed in many possible ways. In special cases, the inverse kinematics can be calculated analytically. But solving more complex forward kinematics may require numerical methods. In our example, we will calculate the inverse kinematics analytically and step-by-step to understand the concepts. Again, this is the kinematic chain used in the following sections:

Given an end effector position `position_3` and rotation `rotation_3` we will now calculate the required joint angles to reach the position. The calculation is divided into two parts: First, calculating the position `position_2` and then calculating the remaining angles `theta_3`, `theta_2`, and `theta_1`. Since we start with the position of the end effector and rotation, the `position_2` is simply the vector from `position_3` rotated by `rotation_3` with length `length_3` (the vector is subtracted to point in the other direction, i.e. to `position_2`):

``````position_2 = Vec2d(
position_3.x - math.cos(rotation_3) * length_3,
position_3.y - math.sin(rotation_3) * length_3,
)
``````

This achieves the first step of our calculation. The second step is to calculate the joint angles. As mentioned earlier, we might either not reach the end effector position (because the leg is simply too short), or the joint angles may be ambiguous. In the inverse kinematics calculation, we know the hip joint position `position_0` which means we can calculate the euclidian distance between `position_0` and `position_2`. We consider three cases:

1. Distance between `position_0` and `position_2` is larger than `length_1 + length_2`.
2. Distance between `position_0` and `position_2` is equal to `length_1 + length_2`.
3. Distance between `position_0` and `position_2` is smaller than `length_1 + length_2`.

In case 1 there exists no solution (leg is too short), case 2 yields one exact solution of joint angles (non-ambiguous), and case 3 creates two possible sets of joint angles (ambiguous).

Let's begin with case 1 (leg too short): In this case the inverse kinematics does not have any solution. There does not exist any set of joint angles reaching the given end effector position and rotation.

In case 2 (non-ambiguous) the leg is fully stretched (no rotation change in knee joint, i.e. `90°` when including angle offsets):

``````theta_2 = 0
theta_2_with_offset = theta_2 + math.radians(90)
``````

`theta_1` is calculated as the angle between `-45°` (angle offset) and the distance vector of `position_0` to `position_2`:

``````theta_1 = math.atan((position_2.y - position_0.y) /
(position_2.x - position_0.x))
theta_1_with_offset = theta_1 + math.radians(45)
``````

The ankle joint must rotate to the final rotation `rotation_3` which means it is the remaining angle `theta_3` (since `theta_2` is zero, it can be omitted):

``````theta_3 = rotation_3 - theta_1 - theta_2
theta_3_with_offset = theta_3 - math.radians(111.61)
``````

The inverse kinematics for case 2 yields the joint angles `theta_*_with_offset`.

For case 3 (ambiguous) the inverse kinematics has two possible positions for the knee joint because the leg is longer than the distance between `position_0` and `position_2`. We are interested in the angles of the joints and already know the lengths between the joints. With the law of cosines we can interpret the two solutions for the inverse kinematics as two triangles:

Assume that the point A is `position_0`, C and C' are the two solutions for `position_1`, and B is `position_2`. Then α corresponds to `theta_1`, γ corresponds to `theta_2`, and β corresponds to `theta_3` (they are not the same because of angle and rotation offsets). The lengths `a` and `b` are thigh and tibia length (`0.2m`), `c` is the distance between `position_0` and `position_2`.

First, the rotation of the distance vector of `position_0` to `position_2` is calculated (as before but we don't need the angle offset):

``````theta_1_1 = math.atan((position_2.y - position_0.y) /
(position_2.x - position_0.x))
``````

This angle represents the rotation of the `c` line in the triangle above. The angle α is calculated with:

``````theta_1_2 = math.acos(
(distance_0_to_2 ** 2 + length_1 ** 2 - length_2 ** 2) /
(2 * distance_0_to_2 * length_1),
)
``````

The resulting angles for both joint angle solutions are calculated by substracting or adding α to the rotation of the distance vector:

`````` first_theta_1 = theta_1_1 - theta_1_2
second_theta_1 = theta_1_1 + theta_1_2
first_theta_1_with_offset =  first_theta_1 + math.radians(45)
second_theta_1_with_offset = second_theta_1 + math.radians(45)
``````

The next step is to calculate the angle γ with the same approach:

``````theta_2 = math.acos(
(length_1 ** 2 + length_2 ** 2 - distance_0_to_2 ** 2) /
(2 * length_1 * length_2),
)
``````

The angle γ measures between the thigh and tibia but is rotated by `180°` and again substracted or added:

`````` first_theta_2 = math.pi - theta_2_1
second_theta_2 = theta_2_1 - math.pi
first_theta_2_with_offset =  first_theta_2 + math.radians(90)
second_theta_2_with_offset = second_theta_2 + math.radians(90)
``````

The third angle `theta_3` can be calculated by substracting `theta_1` and `theta_2` from `rotation_3` because the sum of all three angles must result in the rotation of the end effector:

`````` first_theta_3 = rotation_3 -  first_theta_1 -  first_theta_2
second_theta_3 = rotation_3 - second_theta_1 - second_theta_2
first_theta_3_with_offset =  first_theta_3 - math.radians(111.61)
second_theta_3_with_offset = second_theta_3 - math.radians(111.61)
``````

At this point, all required calculations for the inverse kinematics are finished which allow calculating the required joint angles for reaching the end effector position and rotation. The following code contains the inverse kinematics calculations in the function `inverse_kinematics()`. This function takes as arguments the initial position `position_0`, the end effector position `position_3` and rotation `rotation_3`, and all segment lengths `length_*`. It returns the joint angles, i.e. `theta_*`:

You can execute the above code by saving it and then clicking the "Apply Inverse Kinematics" button. Feel free to change for example the end effector position `position_3` and rotation `rotation_3`.

The output shows a visualization of the joints when moved to their calculated angles with the help of the inverse kinematics:

Inverse kinematics allow to calculate the joint angles from the end effector position and rotation. Not all kinematic chains in the real world are as easily computable as our example. Often they cannot be solved analytically but require numerical methods. In the next section we will use the inverse kinematics to calculate the joint angles of our example NAO robot and try to kick the ball at the correct position.

## Keyframes with Inverse Kinematics

As you can see, not all solutions of the inverse kinematics are feasible, as they can violate joint angle limits. Such infeasible solutions are beeing discarded in the following code.