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