Searched refs:Kp_mat (Results 1 – 2 of 2) sorted by relevance
7440 Eigen::DiagonalMatrix<double, Eigen::Dynamic> Kp_mat = mKp.asDiagonal(); in processSendDesiredStateCommand() local7489 acc = Kp_mat * pose_err + Kd_mat * vel_err - C; in processSendDesiredStateCommand()7497 out_tau += Kp_mat * pose_err + Kd_mat * (vel_err - m_data->m_physicsDeltaTime * acc); in processSendDesiredStateCommand()