493 std::vector<int> idx;
513 std::vector<int> idx;
544 Eigen::MatrixXd mc(3, 1);
547 mc = Eigen::MatrixXd::Zero(3, 1);
562 Eigen::MatrixXd COM(3, 1);
602 int idx_size = idx.size();
603 int end = idx_size - 1;
606 Eigen::MatrixXd jacobian = Eigen::MatrixXd::Zero(6, idx_size);
608 for (
int id = 0;
id < idx_size;
id++)
610 int curr_id = idx[id];
616 jacobian.block(3,
id, 3, 1) = tar_orientation;
624 int idx_size = idx.size();
625 int end = idx_size - 1;
628 Eigen::MatrixXd jacobian_com = Eigen::MatrixXd::Zero(6, idx_size);
630 for (
int id = 0;
id < idx_size;
id++)
632 int curr_id = idx[id];
639 jacobian_com.block(3,
id, 3, 1) = tar_orientation;
646 Eigen::MatrixXd tar_orientation, Eigen::MatrixXd curr_orientation)
648 Eigen::MatrixXd pos_err = tar_position - curr_position;
649 Eigen::MatrixXd ori_err = curr_orientation.transpose() * tar_orientation;
652 Eigen::MatrixXd err = Eigen::MatrixXd::Zero(6, 1);
653 err.block<3, 1>(0, 0) = pos_err;
654 err.block<3, 1>(3, 0) = ori_err_dash;
660 int max_iter,
double ik_err)
662 bool ik_success =
false;
663 bool limit_success =
false;
669 for (
int iter = 0; iter < max_iter; iter++)
676 Eigen::MatrixXd err =
calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
678 if (err.norm() < ik_err)
686 Eigen::MatrixXd jacobian_trans = jacobian * jacobian.transpose();
687 Eigen::MatrixXd jacobian_inverse = jacobian.transpose() * jacobian_trans.inverse();
689 Eigen::MatrixXd delta_angle = jacobian_inverse * err;
691 for (
int id = 0;
id < idx.size();
id++)
693 int joint_num = idx[id];
700 for (
int id = 0;
id < idx.size();
id++)
702 int joint_num = idx[id];
706 limit_success =
false;
711 limit_success =
false;
715 limit_success =
true;
718 if (ik_success ==
true && limit_success ==
true)
725 Eigen::MatrixXd tar_orientation,
int max_iter,
double ik_err)
727 bool ik_success =
false;
728 bool limit_success =
false;
732 std::vector<int> idx =
findRoute(from, to);
734 for (
int iter = 0; iter < max_iter; iter++)
741 Eigen::MatrixXd err =
calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
743 if (err.norm() < ik_err)
751 Eigen::MatrixXd jacobian_trans = jacobian * jacobian.transpose();
752 Eigen::MatrixXd jacobian_inv = jacobian.transpose() * jacobian_trans.inverse();
754 Eigen::MatrixXd delta_angle = jacobian_inv * err;
756 for (
int id = 0;
id < idx.size();
id++)
758 int joint_num = idx[id];
765 for (
int id = 0;
id < idx.size();
id++)
767 int joint_num = idx[id];
771 limit_success =
false;
776 limit_success =
false;
780 limit_success =
true;
783 if (ik_success ==
true && limit_success ==
true)
790 int max_iter,
double ik_err, Eigen::MatrixXd weight)
792 bool ik_success =
false;
793 bool limit_success =
false;
800 Eigen::MatrixXd weight_matrix = Eigen::MatrixXd::Identity(idx.size(), idx.size());
802 for (
int ix = 0; ix < idx.size(); ix++)
803 weight_matrix.coeffRef(ix, ix) = weight.coeff(idx[ix], 0);
806 Eigen::MatrixXd eval = Eigen::MatrixXd::Zero(6, 6);
808 double p_damping = 1e-5;
809 double R_damping = 1e-5;
811 for (
int ix = 0; ix < 3; ix++)
813 eval.coeffRef(ix, ix) = p_damping;
814 eval.coeffRef(ix + 3, ix + 3) = R_damping;
818 for (
int iter = 0; iter < max_iter; iter++)
825 Eigen::MatrixXd err =
calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
827 if (err.norm() < ik_err)
835 Eigen::MatrixXd jacobian_trans = (jacobian * weight_matrix * jacobian.transpose() + eval);
836 Eigen::MatrixXd jacobian_inv = weight_matrix * jacobian.transpose() * jacobian_trans.inverse();
838 Eigen::MatrixXd delta_angle = jacobian_inv * err;
840 for (
int id = 0;
id < idx.size();
id++)
842 int joint_id = idx[id];
850 for (
int id = 0;
id < idx.size();
id++)
852 int joint_num = idx[id];
856 limit_success =
false;
861 limit_success =
false;
865 limit_success =
true;
868 if (ik_success ==
true && limit_success ==
true)
875 Eigen::MatrixXd tar_orientation,
int max_iter,
double ik_err,
876 Eigen::MatrixXd weight)
878 bool ik_success =
false;
879 bool limit_success =
false;
883 std::vector<int> idx =
findRoute(from, to);
886 Eigen::MatrixXd weight_matrix = Eigen::MatrixXd::Identity(idx.size(), idx.size());
888 for (
int ix = 0; ix < idx.size(); ix++)
889 weight_matrix.coeffRef(ix, ix) = weight.coeff(idx[ix], 0);
892 Eigen::MatrixXd eval = Eigen::MatrixXd::Zero(6, 6);
894 double p_damping = 1e-5;
895 double R_damping = 1e-5;
897 for (
int ix = 0; ix < 3; ix++)
899 eval.coeffRef(ix, ix) = p_damping;
900 eval.coeffRef(ix + 3, ix + 3) = R_damping;
904 for (
int iter = 0; iter < max_iter; iter++)
909 Eigen::MatrixXd err =
calcVWerr(tar_position, curr_position, tar_orientation, curr_orientation);
911 if (err.norm() < ik_err)
919 Eigen::MatrixXd jacobian_trans = (jacobian * weight_matrix * jacobian.transpose() + eval);
920 Eigen::MatrixXd jacobian_inv = weight_matrix * jacobian.transpose() * jacobian_trans.inverse();
921 Eigen::MatrixXd delta_angle = jacobian_inv * err;
923 for (
int id = 0;
id < idx.size();
id++)
925 int joint_id = idx[id];
932 for (
int id = 0;
id < idx.size();
id++)
934 int _joint_num = idx[id];
937 limit_success =
false;
942 limit_success =
false;
946 limit_success =
true;
949 if (ik_success ==
true && limit_success ==
true)
956 double pitch,
double yaw)
958 Eigen::Matrix4d trans_ad, trans_da, trans_cd, trans_dc, trans_ac;
962 double rac, arc_cos, arc_tan, k, l, m, n,
s, c, theta;
969 vec.coeffRef(0) = trans_ad.coeff(0, 3) + trans_ad.coeff(0, 2) * ankle_length;
970 vec.coeffRef(1) = trans_ad.coeff(1, 3) + trans_ad.coeff(1, 2) * ankle_length;
971 vec.coeffRef(2) = trans_ad.coeff(2, 3) + trans_ad.coeff(2, 2) * ankle_length;
976 (rac * rac - thigh_length * thigh_length - calf_length * calf_length) / (2.0 * thigh_length * calf_length));
977 if (std::isnan(arc_cos) == 1)
979 *(out + 3) = arc_cos;
982 trans_ad.computeInverseWithCheck(trans_da, invertible);
983 if (invertible ==
false)
986 k = sqrt(trans_da.coeff(1, 3) * trans_da.coeff(1, 3) + trans_da.coeff(2, 3) * trans_da.coeff(2, 3));
988 trans_da.coeff(1, 3) * trans_da.coeff(1, 3)
989 + (trans_da.coeff(2, 3) - ankle_length) * (trans_da.coeff(2, 3) - ankle_length));
990 m = (k * k - l * l - ankle_length * ankle_length) / (2.0 * l * ankle_length);
998 if (std::isnan(arc_cos) == 1)
1001 if (trans_da.coeff(1, 3) < 0.0)
1002 *(out + 5) = -arc_cos;
1004 *(out + 5) = arc_cos;
1008 trans_cd.computeInverseWithCheck(trans_dc, invertible);
1009 if (invertible ==
false)
1012 trans_ac = trans_ad * trans_dc;
1013 arc_tan = atan2(-trans_ac.coeff(0, 1), trans_ac.coeff(1, 1));
1014 if (std::isinf(arc_tan) != 0)
1019 arc_tan = atan2(trans_ac.coeff(2, 1), -trans_ac.coeff(0, 1) * sin(*(out)) + trans_ac.coeff(1, 1) * cos(*(out)));
1020 if (std::isinf(arc_tan) != 0)
1022 *(out + 1) = arc_tan;
1025 arc_tan = atan2(trans_ac.coeff(0, 2) * cos(*(out)) + trans_ac.coeff(1, 2) * sin(*(out)),
1026 trans_ac.coeff(0, 0) * cos(*(out)) + trans_ac.coeff(1, 0) * sin(*(out)));
1027 if (std::isinf(arc_tan) == 1)
1030 k = sin(*(out + 3)) * calf_length;
1031 l = -thigh_length - cos(*(out + 3)) * calf_length;
1032 m = cos(*(out)) * vec.coeff(0) + sin(*(out)) * vec.coeff(1);
1033 n = cos(*(out + 1)) * vec.coeff(2) + sin(*(out)) * sin(*(out + 1)) * vec.coeff(0)
1034 - cos(*(out)) * sin(*(out + 1)) * vec.coeff(1);
1035 s = (k * n + l * m) / (k * k + l * l);
1036 c = (n - k * s) / l;
1037 arc_tan = atan2(s, c);
1038 if (std::isinf(arc_tan) == 1)
1040 *(out + 2) = arc_tan;
1041 *(out + 4) = theta - *(out + 3) - *(out + 2);
1047 double pitch,
double yaw)
1051 for(
int ix = 0 ; ix < 6; ix++)
1061 double pitch,
double yaw)
1065 for(
int ix = 0 ; ix < 6; ix++)
1099 Eigen::MatrixXd joint_axis;
1103 if (link_data != NULL)
1113 double joint_direction = 0.0;
1116 if (link_data != NULL)
1122 return joint_direction;
1127 double joint_direction = 0.0;
1130 if (link_data != NULL)
1136 return joint_direction;
1141 Eigen::MatrixXd K, Eigen::MatrixXd P)
1143 double t = control_cycle;
1144 double preview_size_ = round(preview_time/control_cycle) + 1;
1148 A_ << 1, t, t*t/2.0,
1160 c_ << 1, 0, -lipm_height/9.81;
1162 Eigen::MatrixXd tempA = Eigen::MatrixXd::Zero(4,4);
1163 Eigen::MatrixXd tempb = Eigen::MatrixXd::Zero(4,1);
1164 Eigen::MatrixXd tempc = Eigen::MatrixXd::Zero(1,4);
1166 tempA.coeffRef(0,0) = 1;
1167 tempA.block<1,3>(0,1) = c_*A_;
1168 tempA.block<3,3>(1,1) = A_;
1170 tempb.coeffRef(0,0) = (c_*b_).coeff(0,0);
1171 tempb.block<3,1>(1,0) = b_;
1173 tempc.coeffRef(0,0) = 1;
1179 Eigen::MatrixXd Q = Eigen::MatrixXd::Zero(4,4);
1180 Q.coeffRef(0,0) = Q_e;
1181 Q.coeffRef(1,1) = Q_e;
1182 Q.coeffRef(2,2) = Q_e;
1183 Q.coeffRef(3,3) = Q_x;
1186 f_.resize(1, preview_size_);
1188 Eigen::MatrixXd mat_R = Eigen::MatrixXd::Zero(1,1);
1189 mat_R.coeffRef(0,0) = R;
1191 Eigen::MatrixXd tempCoeff1 = mat_R + ((tempb.transpose() * P) * tempb);
1192 Eigen::MatrixXd tempCoeff1_inv = tempCoeff1.inverse();
1193 Eigen::MatrixXd tempCoeff2 = tempb.transpose();
1194 Eigen::MatrixXd tempCoeff3 = Eigen::MatrixXd::Identity(4,4);
1195 Eigen::MatrixXd tempCoeff4 = P*tempc.transpose();
1197 f_.block<1,1>(0,0) = ((tempCoeff1_inv*tempCoeff2)* tempCoeff3) * tempCoeff4;
1199 for(
int i = 1; i < preview_size_; i++)
1201 tempCoeff3 = tempCoeff3*((tempA - tempb*K).transpose());
1202 f_.block<1,1>(0,i) = ((tempCoeff1_inv*tempCoeff2)* tempCoeff3) * tempCoeff4;
Eigen::MatrixXd joint_axis_
Eigen::MatrixXd calcJacobianCOM(std::vector< int > idx)
Eigen::Matrix3d calcHatto(const Eigen::Vector3d &matrix3d)
Eigen::Matrix3d calcRodrigues(const Eigen::Matrix3d &hat_matrix, double angle)
Eigen::Vector3d convertRotToOmega(const Eigen::Matrix3d &rotation)
LinkData * op3_link_data_[ALL_JOINT_ID+1]
void calcForwardKinematics(int joint_ID)
Eigen::MatrixXd calcJacobian(std::vector< int > idx)
Eigen::Matrix4d getTransformationXYZRPY(double position_x, double position_y, double position_z, double roll, double pitch, double yaw)
Eigen::MatrixXd relative_position_
Eigen::Vector3d getTransitionXYZ(double position_x, double position_y, double position_z)
Eigen::Vector3d calcCross(const Eigen::Vector3d &vector3d_a, const Eigen::Vector3d &vector3d_b)
std::vector< int > findRoute(int to)
bool calcInverseKinematicsForLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
double leg_side_offset_m_
Eigen::MatrixXd center_of_mass_
double getJointDirection(const std::string link_name)
Eigen::MatrixXd getJointAxis(const std::string link_name)
Eigen::MatrixXd calcPreviewParam(double preview_time, double control_cycle, double lipm_height, Eigen::MatrixXd K, Eigen::MatrixXd P)
Eigen::MatrixXd calcMC(int joint_id)
bool calcInverseKinematics(int to, Eigen::MatrixXd tar_position, Eigen::MatrixXd tar_orientation, int max_iter, double ik_err)
Eigen::MatrixXd position_
Eigen::Matrix3d getInertiaXYZ(double ixx, double ixy, double ixz, double iyy, double iyz, double izz)
Eigen::MatrixXd calcCOM(Eigen::MatrixXd mc)
Eigen::MatrixXd transformation_
LinkData * getLinkData(const std::string link_name)
double calcTotalMass(int joint_id)
bool calcInverseKinematicsForRightLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
bool calcInverseKinematicsForLeftLeg(double *out, double x, double y, double z, double roll, double pitch, double yaw)
Eigen::MatrixXd calcVWerr(Eigen::MatrixXd tar_position, Eigen::MatrixXd curr_position, Eigen::MatrixXd tar_orientation, Eigen::MatrixXd curr_orientation)
Eigen::MatrixXd orientation_