use direct body frame positions
use direct body frame positions

By using qrot(q, pos_body) there is no boundary artifact in the
computation of torques and relative velocities

pdebuyl-lab/RMPCDMD,pdebuyl/RMPCDMD,pdebuyl-lab/RMPCDMD,pdebuyl/RMPCDMD