The goal of this task is to estimate joint axes of a robot from 3D point measurements.

As the task description explains, we will estimate the robot motion axes by moving joints one by one and measuring the coordinates of three points fixed to the end effector of the mechanism. Two sets of variables are required to solve the task: point coordinates before the motion that are stored in x_measurements, and point coordinates after the motion stored in y_measurements.

x_measurements and y_measurements are 4 x 3 x 3 numpy matrices saved to text files. The first dimension corresponds to the number of joints. The second dimension is 3 as we measure coordinates of vectors in $\mathbb{R}^3$. The third dimension corresponds to the number of points measured. So x_measurements[0, :, 0] represents the coordinates of $\vec{x_1}$ before moving the first joint.

To load the files, use

np.loadtxt(filename).reshape((4, 3, 3))

Create a function get_axis_from_point_measurement(x_measurement, y_measurement) which takes as inputs coordinates of three points before and after moving one revolute joint and outputs the axis of motion of that joint.

Input/Output specifications for get_axis_from_point_measurement:

1. x_measurement : coordinates of points before motion (3×3 matrix of type np.ndarray)
2. y_measurement : coordinates of points after motion (3×3 matrix of type np.ndarray)
3. Return value : dictionary with 2 keys “a_dir” and “a_point”, whose values are the direction vector (3×1 matrix of type np.ndarray) and the point (3×1 matrix of type np.ndarray) that describe the axis of motion.

Visualize found axis of motion as lines in 3D. Please use this viewpoint to make it easier for the assessment

ax.view_init(azim=50, elev=20) 

Upload a zip archive hw05.zip (via the course ware) containing:

1. hw05.py - python script containing the implemented function get_axis_from_point_measurement
2. hw05.pdf - report file containing visualization of the axes of motion