bool InverseKinematics::GetDirectKinematics(std::vector<float>& articular, std::vector<float>& cartesian)
{
if(articular.size() != 7)
{
std::cout << "DirectKinematics.->Articular values must be seven values! " << std::endl;
return false;
}
std::cout << "DirectKinematics.->Calculating for angles: ";
for (int i=0; i < 7; i++) std::cout << articular[i] << " ";
std::cout << std::endl;
//In the urdf, in each joint, originZ corresponds to the 'D' params of Denavit-Hartenberg
//and originX corresponds to 'A' params of DenavitHartenberg. originR are the alpha parameters ant originYaw are the Theta parameters
//in URDF:
//<joint name="la_2_joint" type="revolute"><origin xyz="0.0603 0 0" rpy="1.5708 0 0"/>
//<joint name="la_3_joint" type="revolute"><origin xyz="0.0 0 0" rpy="1.5708 0 1.5708"/><!--Transformation from link2 to link3 when theta2 = 0 -->
//<joint name="la_4_joint" type="revolute"><origin xyz="0 0 0.27" rpy="-1.5708 0 -1.5708"/>
//<joint name="la_5_joint" type="revolute"><origin xyz="0.0 0 0" rpy="1.5708 0 0"/><!--Transformation from link4 to link5 when theta4 = 0 -->
//<joint name="la_6_joint" type="revolute"><origin xyz="0 0 0.2126" rpy="-1.5708 0 0"/>
//<joint name="la_7_joint" type="revolute"><origin xyz="0.0 0 0" rpy="1.5708 0 0"/><!--Transformation from link6 to link7 when theta6 = 0 -->
//<joint name="la_grip_center_joint" type="fixed"><origin xyz="0 0 0.13" rpy="0 -1.5708 3.141592"/>
float dhD[7] = {0, 0, 0.27, 0, 0.2126, 0, 0.13};
float dhA[7] = {0.0603, 0, 0, 0, 0, 0, 0};
float dhAlpha[7] = {1.5708, 1.5708, -1.5708, 1.5708, -1.5708, 1.5708, 0};
float dhTheta[7] = {0, 1.5708, -1.5708, 0, 0, 0, 0};
tf::Quaternion q;
tf::Transform R07;
R07.setIdentity();
for(size_t i=0; i < 7; i++)
{
tf::Transform temp;
temp.setOrigin(tf::Vector3(dhA[i]*cos(articular[i]), dhA[i]*sin(articular[i]), dhD[i]));
q.setRPY(dhAlpha[i],0,articular[i] + dhTheta[i]);
temp.setRotation(q);
R07 = R07 * temp;
}
tf::Vector3 endEffector(0,0,0);
endEffector = R07 * endEffector; //XYZ position of the end effector
q = R07.getRotation();
double roll, pitch, yaw;
tf::Matrix3x3(q).getRPY(roll, pitch, yaw);
cartesian.clear();
cartesian.push_back(endEffector.x());
cartesian.push_back(endEffector.y());
cartesian.push_back(endEffector.z());
cartesian.push_back(roll - 1.5708); //This minus pi/2 corrects the fact that the final-effector frame is not aligned with left_arm_link0
cartesian.push_back(pitch);
cartesian.push_back(yaw - 1.5708);
cartesian.push_back(0);
std::cout << "DirectKinematics.->Calculated cartesian: ";
for (int i=0; i < 7; i++) std::cout << cartesian[i] << " ";
std::cout << std::endl;
return true;
}
vector<float> controller::getTargetPosition(int leg_id) {
//Last link
//translation
Eigen::MatrixXf mt = Eigen::MatrixXf::Identity(4, 4);
mt(2, 3) = body_bag->getFootLinkLength(leg_id, 3);
//rotation
Eigen::MatrixXf mr = Eigen::MatrixXf::Identity(4, 4);
const dReal *rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 3));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr(i, j) = rotation_matrix_ode[i*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf point = Eigen::MatrixXf::Random(4, 1);
point(0, 0) = current_foot_location[leg_id][0];
point(1, 0) = current_foot_location[leg_id][1];
point(2, 0) = current_foot_location[leg_id][2];
point(3, 0) = 1;
Eigen::MatrixXf transformedPoint = mr*mt*mr.inverse()*point;
//Second last link
//translation
mt = Eigen::MatrixXf::Identity(4, 4);
mt(2, 3) = body_bag->getFootLinkLength(leg_id, 2);
//rotation
mr = Eigen::MatrixXf::Identity(4, 4);
rotation_matrix_ode = dBodyGetRotation(body_bag->getFootLinkBody(leg_id, 2));
for(int i = 0; i < 3; i++) {
for(int j = 0; j < 4; j++) {
mr(i, j) = rotation_matrix_ode[i*4+j];
}
}
mr(3, 0) = 0;
mr(3, 1) = 0;
mr(3, 2) = 0;
mr(3, 3) = 1;
Eigen::MatrixXf endEffectorM = mr*mt*mr.inverse()*transformedPoint;
vector<float> endEffector(3);
endEffector[0] = endEffectorM(0,0);
endEffector[1] = endEffectorM(1,0);
endEffector[2] = endEffectorM(2,0);
return endEffector;
}
