Warning

Homework 03 - Forward Kinematics

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:

1. mechanism : dictionary with 24 keys “theta1 offset”, $\dots$, “theta6 offset”, “d1”, $\dots$, “d6”, “a1”, $\dots$, “a6”, “alpha1”, $\dots$, “alpha6”. The values are numbers.
2. joints : dictionary with 6 keys “theta1”, $\dots$, “theta6”. The values are numbers.
3. Return value : dictionary with 2 keys “r” and “t”, whose values are the rotation (3×3 matrix of type np.array) and the translation (3×1 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

import hw03
pose = hw03.fkt(mechanism, joints)

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:

1. fix 2-6 joints to zero position
2. sample value for the first joint uniformly in robot limits for n times (you can fix n=10000)
3. for each sampled value compute fkt and record the end-effector position
4. 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 a zip archive hw03.zip (via the course ware) containing the following files:
1. hw03.py - python script containing the implemented function fkt
2. hw03.pdf - report file describing your solution containing:
1. description of the implementation of fkt