Ejemplo n.º 1
0
bool JointPathEx::calcInverseKinematics2Loop(const Vector3& end_effector_p, const Matrix33& end_effector_R,
                                             const double LAMBDA, const double avoid_gain, const double reference_gain, const hrp::dvector* reference_q,
                                             const double vel_gain,
                                             const hrp::Vector3& localPos, const hrp::Matrix33& localR)
{
    hrp::Matrix33 target_link_R(end_effector_R * localR.transpose());
    hrp::Vector3 target_link_p(end_effector_p - target_link_R * localPos);
    hrp::Vector3 vel_p(target_link_p - endLink()->p);
    // TODO : matrix_logEx should be omegaFromRotEx after checking on real robot testing.
    //hrp::Vector3 vel_r(endLink()->R * omegaFromRotEx(endLink()->R.transpose() * target_link_R));
    hrp::Vector3 vel_r(endLink()->R * matrix_logEx(endLink()->R.transpose() * target_link_R));
    vel_p *= vel_gain;
    vel_r *= vel_gain;
    return calcInverseKinematics2Loop(vel_p, vel_r, LAMBDA, avoid_gain, reference_gain, reference_q);
}
Ejemplo n.º 2
0
 void difference_rotation(hrp::Vector3& ret_dif_rot, const hrp::Matrix33& self_rot, const hrp::Matrix33& target_rot)
 {
   //ret_dif_rot = self_rot * hrp::omegaFromRot(self_rot.transpose() * target_rot);
   ret_dif_rot = self_rot * hrp::Vector3(rats::matrix_log(hrp::Matrix33(self_rot.transpose() * target_rot)));
 }