Added function for resetting the state and prepared calculation of cartesian twist

Also, typos and convenience fixes