Start of kinematics code. It verifies against my MATLAB sandbox version; adding that next.
Adding matlab kinematics experiments for future reference. It includes a simple visualization as a sanity check.