# Lab 2 Report

## 2D Transformation Matrix Calculations
Here are our diagrams and calculations to find the transformation matrix using PoE and DH for the planar robot arm.

### 2D Forward Kinematics in PoE
We found the transformation matrix with the PoE method by finding the screw vectors for each joint as well as the M matrix (transformation matrix when all theta angles equal to zero).

#### 2D PoE Diagrams

The figure below show the placement of the torso {t} and hand {b} frames. The PoE screw axes are also labelled for each joint: the shoulder revolute joint in the shoulder {s}-frame, and the elbow revolute joint in the {e}-frame.

#### 2D Screws

##### Elbow
$$
\hat{w}_{elbow} =
\begin{bmatrix}
0 \\
-1 \\
0
\end{bmatrix}
$$
This will always be true since the initial position of the arm is held straight out (meaning that the elbow joint is 0°) but the elbow joint must always be between [2°, 88.5°]. Moving the elbow joint from an angle of 0° to any positive angle corresponds toa negative rotation along the y-axis. Since the angular velocity vector must be a unit vector, the y component must be -1.

When there is a pure rotation, the angular velocity can be calculated using:

$$
v = -\hat{w} \times q
$$

Where $q$ is the point vector of the joint with reference to the space frame.

The elbow joint is located at

$$
q_{elbow} =
\begin{bmatrix}
105 \\
-113 \\
100
\end{bmatrix}
$$

Thus

$$
v_{elbow} = -
\begin{bmatrix}
0 \\
-1 \\
0
\end{bmatrix}
\times
\begin{bmatrix}
105 \\
-113 \\
100
\end{bmatrix} =
\begin{bmatrix}
100 \\
0 \\
-105
\end{bmatrix}
$$

Putting these together, we get the following screw axis.

$$
S_{elbow} =
\begin{bmatrix}
\hat{w}_{elbow} \\
v_{elbow}
\end{bmatrix} =
\begin{bmatrix}
0 \\
-1 \\
0 \\
100 \\
0 \\
-105
\end{bmatrix}
$$

##### Shoulder
For positive shoulder joint angles:
$$
\hat{w}_{shoulder} =
\begin{bmatrix}
0 \\
1 \\
0
\end{bmatrix}
$$

For negative shoulder joint angles:
$$
\hat{w}_{shoulder} =
\begin{bmatrix}
0 \\
-1 \\
0
\end{bmatrix}
$$

The shoulder rotation is along the y-axis. For positive angles, it rotates in the positive direction, and for negative angles, it rotates in the negative direction.

$$
q_{shoulder} =
\begin{bmatrix}
0 \\
-98 \\
100
\end{bmatrix}
$$

Thus,

$$
v_{shoulder} = -
\begin{bmatrix}
0 \\
1 \\
0
\end{bmatrix}
\times
\begin{bmatrix}
0 \\
-98 \\
100
\end{bmatrix} =
\begin{bmatrix}
-100 \\
0 \\
0
\end{bmatrix}
$$

for positive shoulder angles and $v_{shoulder}$ is the negative of the above result for negative angles.

This gives the following screw axis.
$$
S_{shoulder} =
\begin{bmatrix}
\hat{w}_{shoulder} \\
v_{shoulder}
\end{bmatrix} =
\begin{bmatrix}
0 \\
1 \\
0 \\
-100 \\
0 \\
0
\end{bmatrix}
$$

For positive shoulder angles. $S_{shoulder}$ is just the negative of the above result for negative joint angles.

#### 2D PoE Matrices
##### End Effector Matrix M
Let M be the transformation matrix of the end-effector {b} when $\theta = 0$.

$$
R =
\begin{bmatrix}
1 & 0 & 0\\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix}
$$

$$
p =
\begin{bmatrix}
218.7 \\
-113 \\
87.69
\end{bmatrix}
$$

Where $R$ is the rotation matrix from the {s} frame to the hand {b} frame, and $p$ is the location of the hand end effector in the {s} frame.

$$
M =
\begin{bmatrix}
R & p\\
0 & 1
\end{bmatrix} =
\begin{bmatrix}
1 & 0 & 0 & 218.7 \\
0 & 1 & 0 & -113\\
0 & 0 & 1 & 87.69 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

##### PoE formula
Given $\theta$, the product of exponentials formula in the space frame is:

$$
T(\theta) = e^{[S_1] \theta_1} e^{[S_2] \theta_2} M
$$

where:
$S_1$ = $S_{shoulder}$
$S_2$ = $S_{elbow}$

### 2D Forward Kinematics in DH
Here are our diagrams and calculations to find the transformation matrix using the DH method. We find the DH parameters for each joint and build the intermediate transformation matrices. We then multiply the intermediate transformation matrices to find the final transformation matrix.

#### 2D DH Diagrams
The diagrams below indicate the position of the torso {T}-frame, the right hand end-effector {B}-frame, and the DH frames. We also make use of an intermediate {A}-frame to help simplify some calculations.

#### 2D DH Parameters
Let:
{T}-frame = torso frame
{S}-frame = shoulder joint frame
{E}-frame = elbow joint frame
{A}-frame = intermediate hand frame
{B}-frame = right hand end-effector frame

For initial position
| | S -> E| E -> A | A -> B |
| --- | -------- | -------- | -------- |
| d | -15 | 0 | -12.31 |
| $\theta$ | 0 | 0 | 0 |
| r | 105 | 113.7 | 0 |
| $\alpha$ | 180° | -90° | 0 |

In general
| | S -> E| E -> A | A -> B |
| --- | -------- | -------- | -------- |
| d | -15 | 0 | -12.31 |
| $\theta$ | shoulder pitch angle| elbow roll angle | 0 |
| r | 105 | 113.7 | 0 |
| $\alpha$ | 180° | -90° | 0 |

#### 2D DH Matrices
In general, a transformation matrix from frame n-1 to n using DH parameters can be constructed as follows

$$
T_{n-1, n} =
\begin{bmatrix}
\cos(\theta) & -\sin(\theta)\cos(\alpha) & \sin(\theta)\sin(\alpha) & r\cos(\theta) \\
\sin(\theta) & \cos(\theta)\cos(\alpha) & -cos(\theta)\sin(\alpha) & r\sin(\theta)\\
0 & \sin(\alpha) & \cos(\alpha) & d \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

Thus,

$$
T_{TS} =
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 0 & 1 & -98\\
0 & -1 & 0 & 100 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

$$
T_{SE} =
\begin{bmatrix}
\cos(\theta_s) & \sin(\theta_s) & 0 & 105\cos(\theta_s) \\
\sin(\theta_s) & -\cos(\theta_s) & 0 & 105\sin(\theta_s)\\
0 & 0 & -1 & -15 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

Where $\theta_s$ is the angle of the shoulder pitch.

$$
T_{EA} =
\begin{bmatrix}
\cos(\theta_e) & 0 & -\sin(\theta_e) & 113.7\cos(\theta_e) \\
\sin(\theta_e) & 0 & cos(\theta_e) & 113.7\sin(\theta_e)\\
0 & -1 & 0 & d \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

Where $\theta_e$ is the angle of the elbow roll.

$$
T_{AB} =
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0\\
0 & 0 & 1 & -12.31 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

The final transformation matrix from the torso {T}-frame to the right hand end-effector {B}-frame can be generated with the following formula:

$$
T_{TB} (\theta_s,\theta_e) = T_{TS} T_{SE}(\theta_s) T_{EA}(\theta_e) T_{AB}
$$

### 2D Workspace
This workspace was calculated by plotting each point of the end effector while sweeping over the possible joint angles. The y coordinate is constant at y = -113.7 mm (-0.1137 m) from the torso reference frame.

## 3D Transformation Matrix Calculations
Here are our diagrams and calculations to find the transformation matrix using PoE and DH for the spatial robot arm.

### 3D Forward Kinematics in PoE
We found the transformation matrix with the PoE method by finding the screw vectors for each joint as well as the M matrix (transformation matrix when all theta angles equal to zero).

#### 3D PoE Diagrams

The figure below show the placement of the torso {t} and hand {b} frames. The PoE screw axes are also labelled for each joint: the shoulder joint in the shoulder {s}-frame, and the elbow revolute joint in the {e}-frame.

#### 3D Screws
The shoulder joint can be divided into two joints at the same origin: the shoulder pitch revolute joint and the shoulder roll revolute joint. Since the shoulder roll depends on the shoulder pitch, the order of joints from {t}-frame (torso) to {b}-frame (right hand end effector) will be: shoulder pitch, shoulder roll, and elbow.

##### Shoulder Pitch Joint
The shoulder pitch's screw axis vector will be denoted as $S_{p}$. The PoE shoulder roll axes can be found in the diagram where axes are labelled $x_S$, $y_S$, and $z_S$. The shoulder pitch revolute joint rotates the $y_S$ axis.

$$
\hat{w}_{p} =
\begin{bmatrix}
0 \\
1 \\
0
\end{bmatrix}
$$

$$
v = - \hat{w} \times q
$$

Where $q$ is the point vector of the joint with reference to the space frame.

The shoulder pitch joint is located at

$$
q_{p} =
\begin{bmatrix}
0 \\
-98 \\
100
\end{bmatrix}
$$

Thus

$$
v_{p} =
-\begin{bmatrix}
0 \\
1 \\
0
\end{bmatrix}
\times
\begin{bmatrix}
0 \\
-98 \\
100
\end{bmatrix} =
\begin{bmatrix}
-100 \\
0 \\
0
\end{bmatrix}
$$

Putting these together, we get the following screw axis.

$$
S_{p} =
\begin{bmatrix}
\hat{w}_{p} \\
v_{p}
\end{bmatrix} =
\begin{bmatrix}
0 \\
1 \\
0 \\
-100 \\
0 \\
0
\end{bmatrix}
$$

##### Shoulder Roll Joint
The shoulder roll's screw axis vector will be denoted as $S_{r}$. The PoE shoulder roll axes can be found in the diagram where axes are labelled $x_S$, $y_S$, and $z_S$. The shoulder roll revolute joint rotates the $z_S$ axis.

$$
\hat{w}_{r} =
\begin{bmatrix}
0 \\
0 \\
1
\end{bmatrix}
$$

$$
v = - \hat{w} \times q
$$

Where $q$ is the point vector of the joint with reference to the space frame.

The shoulder roll joint is located at

$$
q_{r} =
\begin{bmatrix}
0 \\
-98 \\
100
\end{bmatrix}
$$

Thus

$$
v_{r} =
-\begin{bmatrix}
0 \\
0 \\
1
\end{bmatrix}
\times
\begin{bmatrix}
0 \\
-98 \\
100
\end{bmatrix} =
\begin{bmatrix}
-98 \\
0 \\
0
\end{bmatrix}
$$

Putting these together, we get the following screw axis.

$$
S_{r} =
\begin{bmatrix}
\hat{w}_{r} \\
v_{r}
\end{bmatrix} =
\begin{bmatrix}
0 \\
0 \\
1 \\
-98 \\
0 \\
0
\end{bmatrix}
$$

##### Elbow Joint
The elbow's screw axis vector will be denoted as $S_{E}$. The PoE elbow axes can be found in the diagram where axes are labelled $x_E$, $y_E$, and $z_E$.

$$
\hat{w}_{e} =
\begin{bmatrix}
0 \\
-1 \\
0
\end{bmatrix}
$$

$$
v = - \hat{w} \times q
$$

Where $q$ is the point vector of the joint with reference to the space frame.

The elbow joint is located at

$$
q_{e} =
\begin{bmatrix}
105 \\
-113 \\
100
\end{bmatrix}
$$

Thus

$$
v_{e} =
-\begin{bmatrix}
0 \\
-1 \\
0
\end{bmatrix}
\times
\begin{bmatrix}
105 \\
-113 \\
100
\end{bmatrix} =
\begin{bmatrix}
100 \\
0 \\
-105
\end{bmatrix}
$$

Putting these together, we get the following screw axis.

$$
S_{e} =
\begin{bmatrix}
\hat{w}_{e} \\
v_{e}
\end{bmatrix} =
\begin{bmatrix}
0 \\
-1 \\
0 \\
100 \\
0 \\
-105
\end{bmatrix}
$$

#### 3D PoE Matrices
##### End Effector Matrix M
Let M be the transformation matrix of the end-effector {b} when $\theta = 0$.

$$
R =
\begin{bmatrix}
1 & 0 & 0\\
0 & 1 & 0 \\
0 & 0 & 1
\end{bmatrix}
$$

$$
p =
\begin{bmatrix}
218.7 \\
-113 \\
87.69
\end{bmatrix}
$$

Where $R$ is the rotation matrix from the torso {t} frame to the hand {b} frame, and $p$ is the location of the hand end effector in the torso {t} frame.

$$
M =
\begin{bmatrix}
R & p\\
0 & 1
\end{bmatrix} =
\begin{bmatrix}
1 & 0 & 0 & 218.7 \\
0 & 1 & 0 & -113\\
0 & 0 & 1 & 87.69 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

##### PoE formula
Given $\theta$, the product of exponentials formula in the space frame is:

$$
T(\theta) = e^{[S_p] \theta_p} e^{[S_r] \theta_r} e^{[S_{e}] \theta_{e}} M
$$

where:
$S_p$ = screw vector for shoulder pitch
$S_r$ = screw vector for shoulder roll
$S_e$ = screw vector for elbow

### 3D Forward Kinematics in DH
Here are our diagrams and calculations to find the transformation matrix using the DH method. We find the DH parameters for each joint and build the intermediate transformation matrices. We then multiply the intermediate transformation matrices to find the final transformation matrix.

#### 3D DH Diagrams
The diagrams below indicate the position of the torso {T}-frame, the right hand end-effector {B}-frame, and the DH frames. We also make use of an intermediate {A}-frame to help simplify some calculations.

#### 3D DH Parameters
Let:
{T}-frame = torso frame
{P}-frame = shoulder pitch joint frame
{R}-frame = shoulder roll joint frame
{E}-frame = elbow joint frame
{A}-frame = intermediate hand frame
{B}-frame = right hand end-effector frame

For inital position
| | P -> R | R -> E | E -> A | A -> B |
| -------- | -------- | -------- | -------- | -------- |
| d | 0 | 0 | 15 | -12.31 |
|$\theta$ | 0 | 0 | 0 | 0 |
| r | 0 | 105 | 113.7 | 0 |
|$\alpha$ | 90° | 90° | -90° | 0 |

In general
| | P -> R | R -> E | E -> A | A -> B |
| -------- | -------- | -------- | -------- | -------- |
| d | 0 | 0 | 15 | -12.31 |
|$\theta$ |shoulder pitch angle|shoulder roll angle|elbow roll angle|0|
| r | 0 | 105 | 113.7 | 0 |
|$\alpha$ | 90° | 90° | -90° |0 |

#### 3D DH Matrices
In general, a transformation matrix from DH parameters can be constructed as follows

$$
T_{n-1, n} =
\begin{bmatrix}
\cos(\theta) & -\sin(\theta)\cos(\alpha) & \sin(\theta)\sin(\alpha) & r\cos(\theta) \\
\sin(\theta) & \cos(\theta)\cos(\alpha) & -cos(\theta)\sin(\alpha) & r\sin(\theta)\\
0 & \sin(\alpha) & \cos(\alpha) & d \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

Here are the DH offset matrices generated by the DH parameters:

$$
T_{TP} =
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 0 & 1 & -98\\
0 & -1 & 0 & 100 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

$$
T_{PR} =
\begin{bmatrix}
\cos(\theta_p) & 0 & \sin(\theta_p) & 0 \\
\sin(\theta_p) & 0 & -cos(\theta_p) & 0\\
0 & 1 & 0 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

Where $\theta_p$ is the shoulder pitch angle.

$$
T_{RE} =
\begin{bmatrix}
\cos(\theta_r) & 0 & \sin(\theta_r) & 105\cos(\theta_r) \\
\sin(\theta_r) & 0 & -cos(\theta_r) & 105\sin(\theta_r)\\
0 & 1 & 0 & 0 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

Where $\theta_r$ is the shoulder roll angle.

$$
T_{EA} =
\begin{bmatrix}
\cos(\theta_e) & 0 & -\sin(\theta_e) & 113.7\cos(\theta_e) \\
\sin(\theta_e) & 0 & cos(\theta_e) & 113.7\sin(\theta_e)\\
0 & -1 & 0 & 15 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

Where $\theta_e$ is the elbow roll angle.

$$
T_{AB} =
\begin{bmatrix}
1 & 0 & 0 & 0 \\
0 & 1 & 0 & 0\\
0 & 0 & 1 & -12.31 \\
0 & 0 & 0 & 1
\end{bmatrix}
$$

The final transformation matrix from the torso {T}-frame to the right hand end-effector {B}-frame can be generated with the following formula:

$$
T_{TB} (\theta_p,\theta_r,\theta_e) = T_{TP} T_{PR}(\theta_p) T_{RE}(\theta_r) T_{EA}(\theta_e) T_{AB}
$$

### 3D Workspace
These workspace diagrams were found by plotting the hand position over a sweep of possible joint angles. The reason there may be gaps is because a step size of 5 degrees was used for the angle sweep, so these diagrams are not continuous.


## Results

Here are the transformation matrices generated by the Matlab script, Choregraphe simulation, and real NAO.


We perform the following comparisons: script vs Choregraphe, script vs real NAO, and Choregraphe vs real NAO. For each comparison, we show the absolute error and percent error, as well as represent the errors in a heat map.


## Results Analysis

In order to analyse our results, we generate the absolute error and percent error between the script, Choregraphe simulation, and real NAO results for each set of angles. Note that we do no generate comparisons for the last row of the transformation matrices, since they are always the same. Hence, we will only be comparing the generated rotation matrices and position vectors.

We use a heat map in order to graphically observe the closeness between each element in two transformation matrices: red shows a larger discrepancy between the data points, and green shows little or no differences.

For absolute error, we chose 0.1 as the threshold for red. For percent error, we chose red to be 100%, and yellow to start at 25%.

The following analysis will reference the set of angles as (shoulder roll, shoulder pitch, elbow) in degrees.

### Choregraphe vs real NAO
This is a very interesting comparison because both methods to generate the transformation matrix were not produced by our team, so its a great opportunity for us to examine what is considered "acceptable error".

We expect the simulation to be the most accurate representation of the arm positions, and yet we see some notable error when comparing it to the real NAO.

Error between the simulation and the real NAO could be attributed to a variety of electrical and mechanical deficiencies in the real NAO. For the mechanical side, tolerances in regards to link actual lengths and slop in the joints can combine over the many joints and links that make up the robot to produce error that appears only in certain configurations of the arm. The calibration and mounting of the encoders to measure the angles of the arms is another possible source of error.

The results between the real NAO and Choregraphe results show fairly similar transformation matrices.
- Absolute error:
- We see a maximum absolute error of 0.078353 in one of the 2D configurations of the robot. This represents a 13% difference when compared to the actual data.
- Percent error:
- We see some high 100% errors, but this is due to incredibly small magnitudes in both the choregraphe and real NAO values. As such, these should be ignored.
- We see one insanely high datapoint of 475% error for one particular data point in the 3D values. Again, this is due to small magnitudes in the actual measured values, and can be treated as an outlier.
- Overall, we have good percentage differences between the Choreographe simulation and the Real NAO. As stated, the larger error percentages happened when the magnitudes being measured were small, and could be a byproduct of encoder resolution.

### Script vs Choregraphe
Overall, the results between the script and the Choregraphe simulation are very similar. Some of the causes for the errors observed include that while we specified the joint angles to be a certain value in Choregraphe, the joints often were not exactly what we specified. This difference in joint angles would cause small differences in the absolute error.
- Absolute error:
- The absolute errors are mostly green in the heat maps, meaning the transformation matrices are very similar.
- One exception is between the script and Choregrahe simulation transformation matrices for the planar arm with angles (0,90,45). Since we chose an absolute error of 0.1 to be red, the absolute error heat map for this set of angles also has some yellow and orange, denoting absolute differences in the range of 0.0049 to 0.0694.
- Percent error:
- The percent errors are mostly green in the heat maps, meaning the transformation matrices are very similar.
- The heat maps show some % errors shown as 100% in red. However, if you take a close look at those data points, we notice that they are comparing extremely small values to each other. Hence, since the values are inherently minuscule, a slight difference will cause a large percent difference. Since the percent error does not accurately reflect the magnitude of the difference for small values, the percent errors marked as 100% are negligeable and can be ignored.

### Script vs real NAO
The results between the real NAO and script results show fairly similar transformation matrices. This comparison resembles the comparison between Choregraphe and the real NAO, which makes sense since the results between the script and Choregraphe are very similar. Again, any discrepancies may be due to differences in the real link lengths of the actual NAO robot and small differences in the desired angles vs the actual observed angles.
- Absolute error:
- We start to see some more variance in the absolute error with this comparison, with the matrices being a green and yellow spread. On average, it seems there is more error here than Script vs Choregraphe comparison.
- High of ~0.078 difference, which is still not very much, and it only is a problem in one of the 2D angle sets.
- Percent error:
- The majority of the percent error heatmap is green, with some yellow. This indicates that the transformation matrices generated by the script and the real NAO are fairly similar, but have more differences than the script vs Choregraphe simulation.
- We assume that the red % errors in the heat map are negligeable. As previously stated, the percent error may not accurately reflect the magnitude of the difference for small values. Therefore, the red in the heatmap are comparisons of near zero values and can be ignored. If you check against the absolute errors for the comparisons that generated 100% error, the error magnitude is extremely small and is actually green in the absolute error heatmap.
- For example, an absolute error of 0.007462 is seen as 100% error since it is comparing the values 0 and 0.007462.
- Another example of a negligeable % error is the 457% error from comparing the values 0 and 0.070792 which also have small magnitudes.