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);
}
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)));
}