Eigen::Matrix4d compute_A_of_DH(double q, double a, double d, double alpha) { Eigen::Matrix4d A; Eigen::Matrix3d R; Eigen::Vector3d p; A = Eigen::Matrix4d::Identity(); R = Eigen::Matrix3d::Identity(); //ROS_INFO("compute_A_of_DH: a,d,alpha,q = %f, %f %f %f",a,d,alpha,q); //could correct q w/ q_offset now... double cq = cos(q); double sq = sin(q); double sa = sin(alpha); double ca = cos(alpha); R(0, 0) = cq; R(0, 1) = -sq*ca; //% - sin(q(i))*cos(alpha); R(0, 2) = sq*sa; //%sin(q(i))*sin(alpha); R(1, 0) = sq; R(1, 1) = cq*ca; //%cos(q(i))*cos(alpha); R(1, 2) = -cq*sa; //% //%R(3,1)= 0; %already done by default R(2, 1) = sa; R(2, 2) = ca; p(0) = a * cq; p(1) = a * sq; p(2) = d; A.block<3, 3>(0, 0) = R; A.col(3).head(3) = p; return A; }
Eigen::Matrix4d compute_A_of_DH(int i, double q_abb) { Eigen::Matrix4d A; Eigen::Matrix3d R; Eigen::Vector3d p; double a = DH_a_params[i]; double d = DH_d_params[i]; double alpha = DH_alpha_params[i]; double q = q_abb + DH_q_offsets[i]; A = Eigen::Matrix4d::Identity(); R = Eigen::Matrix3d::Identity(); //ROS_INFO("compute_A_of_DH: a,d,alpha,q = %f, %f %f %f",a,d,alpha,q); double cq = cos(q); double sq = sin(q); double sa = sin(alpha); double ca = cos(alpha); R(0, 0) = cq; R(0, 1) = -sq*ca; //% - sin(q(i))*cos(alpha); R(0, 2) = sq*sa; //%sin(q(i))*sin(alpha); R(1, 0) = sq; R(1, 1) = cq*ca; //%cos(q(i))*cos(alpha); R(1, 2) = -cq*sa; //% //%R(3,1)= 0; %already done by default R(2, 1) = sa; R(2, 2) = ca; p(0) = a * cq; p(1) = a * sq; p(2) = d; A.block<3, 3>(0, 0) = R; A.col(3).head(3) = p; return A; }