ReturnMatrix torque_cmd(Robot_basic & robot, const ColumnVector & pdpp,
const ColumnVector & pdp, const ColumnVector & pd,
const ColumnVector & wdp, const ColumnVector & wd,
const Quaternion & qd, const short link_pc,
const Real dt);
Matrix