Basic Robotics with c++ (eigen)