Printing also q_dot
Added offsets for shoulders