Peter Muehlfellner

Probably this is intentional, I just ran into it while doing some checks. The line is: ``` void set( const Eigen::Vector4d & q){ _q = q; _p_q = q; }...