~~NOTOC~~

===== Homework 03 - Forward Kinematics =====

=== Task 3a ===

Implement the forward kinematic task for a general 6R manipulator.

Create a function ''fkt(mechanism, joints)'' which takes DH parameters of the manipulator ''mechanism'' and 6 joint angles of the manipulator ''joints'' and returns the pose of the end effector. 

Input/Output specifications for ''fkt'':
  - ''mechanism'' : dictionary with 24 keys ''"theta1 offset"'', $\dots$, ''"theta6 offset"'', ''"d1"'', $\dots$, ''"d6"'', ''"a1"'', $\dots$, ''"a6"'', ''"alpha1"'', $\dots$, ''"alpha6"''. The values are numbers.
  - ''joints'' : dictionary with 6 keys ''"theta1"'', $\dots$, ''"theta6"''. The values are numbers.
  - **Return value** : dictionary with 2 keys ''"r"'' and ''"t"'', whose values are the rotation (3x3 matrix of type ''np.array'') and the translation (3x1 matrix of type ''np.array'') of the end effector in the base coordinate system, respectively.

**For ''fkt'' implementation you can use NumPy only. It is forbidden to use other additional libraries! **

Implement the solution in a single file ''hw03.py''. The file must contain the ''fkt'' function, such that it can be imported (by an automatic evaluation) as

<code>
import hw03
pose = hw03.fkt(mechanism, joints)
</code>

=== Task 3b ===
Use the created ''fkt'' function to explore the range of motion of the end-effector when moving only the first joint of the Motoman MA1400 manipulator (that was used in the previous homework).

For this follow the next steps:
  - fix 2-6 joints to zero position
  - sample value for the first joint uniformly in robot limits for ''n'' times (you can fix ''n=10000'')
  - for each sampled value compute ''fkt'' and record the end-effector position
  - visualize the range of end-effector positions projected into the x-y plane. 

For the last step, you can divide the x-y plane into 0.1 x 0.1 cells, compute how many of the recorded positions fall into each cell, and plot the heatmap of the resulting values.

=== Upload ===

Upload a zip archive ''hw03.zip'' (via the [[https://cw.felk.cvut.cz/upload/|course ware]]) containing the following files:
  - ''hw03.py'' - python script containing the implemented function ''fkt''
  - ''hw03.pdf'' - report file describing your solution containing:
    - description of the implementation of ''fkt''
    - visualization of the range of end-effector positions projected into the x-y plane.