Made the implementation more abstract using strategy pattern for kinematics and an array for each joint.