Use getPose and getPoseVelocity rather than get_q and get_dq.