187 lines
7.6 KiB
C++
187 lines
7.6 KiB
C++
#include "Kinematics.h"
|
|
|
|
|
|
float deg2rad(float angle)
|
|
{
|
|
return angle/180.0*M_PI;
|
|
}
|
|
|
|
float rad2deg(float angle)
|
|
{
|
|
return angle*180.0/M_PI;
|
|
}
|
|
|
|
std::vector<float> computeForwardKinematics(float q1, float q2, float q3, float L1, float L2, float L3)
|
|
{
|
|
float z = L2 * cos(q2) + L3 * cos(q2+q3) + L1;
|
|
float xp = L2 * sin(q2) + L3 * sin(q2+q3);
|
|
float x = xp * cos(q1);
|
|
float y = xp * sin(q1);
|
|
//std::cout << "[INFO] Forward Kinematics : (q1, q2, q3)->(x, y, z) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")->(" << x << ", " << y << ", " << z << ")" << std::endl;
|
|
std::vector<float> X;
|
|
X.push_back(x);
|
|
X.push_back(y);
|
|
X.push_back(z);
|
|
|
|
return X;
|
|
}
|
|
|
|
std::vector<float> computeInverseKinematics(float x, float y, float z, float L1, float L2, float L3)
|
|
{
|
|
std::vector<float> qi;
|
|
|
|
// Determines q1
|
|
float q1 = atan2(y, x);
|
|
|
|
// Determines cos(q3) to find out the number of solutions
|
|
float xp = sqrt(x*x + y*y);
|
|
float zc = z - L1;
|
|
float cos_q3 = (zc*zc+xp*xp-(L2*L2+L3*L3)) / (2.0 * L2 * L3);
|
|
|
|
//std::cout << "[INFO] cos_q3= " << cos_q3 << std::endl;
|
|
|
|
if (cos_q3 >1 | cos_q3 <-1)
|
|
{
|
|
qi.push_back(0.0);
|
|
std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl;
|
|
}
|
|
else if (cos_q3 == 1)
|
|
{
|
|
qi.push_back(1.0);
|
|
float q2 = atan2(xp, zc);
|
|
float q3 = 0;
|
|
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y, z)->(q1, q2, q3) = (" << x << ", " << y << ", " << z << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")" << std::endl;
|
|
qi.push_back(q1);
|
|
qi.push_back(q2);
|
|
qi.push_back(q3);
|
|
}
|
|
else if (cos_q3 == -1)
|
|
{
|
|
qi.push_back(1.0);
|
|
float q2 = atan2(xp, zc);
|
|
float q3 = M_PI;
|
|
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y, z)->(q1, q2, q3) = (" << x << ", " << y << ", " << z << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")" << std::endl;
|
|
qi.push_back(q1);
|
|
qi.push_back(q2);
|
|
qi.push_back(q3);
|
|
}
|
|
else
|
|
{
|
|
qi.push_back(2.0);
|
|
std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl;
|
|
|
|
float q3 = acos(cos_q3);
|
|
float q2 = (float)(atan2(xp, zc) - atan2(L2*sin(q3), L1+L2*cos_q3));
|
|
std::cout << "\t(x, y)->(q1, q2, q3) = (" << x << ", " << y << ", " << z << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")" << std::endl;
|
|
qi.push_back(q1);
|
|
qi.push_back(q2);
|
|
qi.push_back(q3);
|
|
|
|
q3 = -acos(cos_q3);
|
|
q2 = (float)(atan2(xp, zc) - atan2(L2*sin(q3), L1+L2*cos_q3));
|
|
|
|
std::cout << "\t(x, y)->(q1, q2, q3) = (" << x << ", " << y << ", " << z << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ", " << rad2deg(q3) << ")" << std::endl;
|
|
qi.push_back(q1);
|
|
qi.push_back(q2);
|
|
qi.push_back(q3);
|
|
}
|
|
|
|
return qi;
|
|
}
|
|
|
|
/*std::vector<float> computeDifferentialKinematics(float q1, float q2, float q3, float L1, float L2, float L3)
|
|
{
|
|
std::vector<float> jacobian;
|
|
|
|
float j11 = -sin(q1)*(L2*sin(q2) + L3*sin(q2+q3));
|
|
float j12 = L2*cos(q1)*cos(q2)+L3*cos(q1)*cos(q2+q3);
|
|
float j13 = L3*cos(q1)*cos(q2+q3);
|
|
|
|
float j21 = cos(q1)*(L2*sin(q2)+L3*sin(q2+q3));
|
|
float j22 = L2*sin(q1)*cos(q2)+L3*sin(q1)*cos(q2+q3);
|
|
float j23 = L3*sin(q1)*cos(q2+q3);
|
|
|
|
float j31 = 0;
|
|
float j32 = -L2*sin(q2)-L3*sin(q2+q3);
|
|
float j33 = -L3*sin(q2+q3);
|
|
|
|
jacobian.push_back(j11); jacobian.push_back(j12); jacobian.push_back(j13);
|
|
jacobian.push_back(j21); jacobian.push_back(j22); jacobian.push_back(j23);
|
|
jacobian.push_back(j31); jacobian.push_back(j32); jacobian.push_back(j33);
|
|
|
|
return jacobian;
|
|
}
|
|
|
|
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
|
|
{
|
|
int rank = -1;
|
|
cv::Mat1f oJacobianMatrix(3, 3);
|
|
|
|
if (vJacobianMatrix.size() == 9)
|
|
{
|
|
// Converts the Jacobian matrix from std::vector to cv::Mat
|
|
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
|
|
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
|
|
oJacobianMatrix.at<float>(0, 2) = vJacobianMatrix[2];
|
|
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[3];
|
|
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[4];
|
|
oJacobianMatrix.at<float>(1, 2) = vJacobianMatrix[5];
|
|
oJacobianMatrix.at<float>(2, 0) = vJacobianMatrix[6];
|
|
oJacobianMatrix.at<float>(2, 1) = vJacobianMatrix[7];
|
|
oJacobianMatrix.at<float>(2, 2) = vJacobianMatrix[8];
|
|
std::cout << "=====Jacobian Matrix=====" << std::endl;
|
|
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << ", " << oJacobianMatrix.at<float>(0,2) << " ]" << std::endl;
|
|
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << ", " << oJacobianMatrix.at<float>(1,2)<< " ]" << std::endl;
|
|
std::cout << "[ " << oJacobianMatrix.at<float>(2,0) << ", " << oJacobianMatrix.at<float>(2,1) << ", " << oJacobianMatrix.at<float>(2,2)<< " ]" << std::endl;
|
|
// Computes the determinant of the Jacobian matrix
|
|
float determinant = abs(-vJacobianMatrix[7]*(vJacobianMatrix[0] * vJacobianMatrix[5] - vJacobianMatrix[3]*vJacobianMatrix[2])+vJacobianMatrix[8]*(vJacobianMatrix[0] * vJacobianMatrix[4] - vJacobianMatrix[3]*vJacobianMatrix[3]));
|
|
std::cout << "=====Determinant of the Jacobian matrix=====" << std::endl << determinant << std::endl;
|
|
// Computes SVD
|
|
cv::Mat1f w, u, vt;
|
|
cv::SVD::compute(oJacobianMatrix, w, u, vt);
|
|
// Finds non zero singular values
|
|
cv::Mat1f nonZeroSingularValues = w/w.at<float>(0,0) > threshold;
|
|
// Counts the number of non zero singular values
|
|
rank = cv::countNonZero(nonZeroSingularValues);
|
|
std::cout << "=====Rank of the Jacobian matrix=====" << std::endl << rank << " / " << oJacobianMatrix.rows << std::endl;
|
|
// Determines the inverse of the Jacobian matrix
|
|
cv::Mat oJacobianInverse = oJacobianMatrix.inv();
|
|
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
|
|
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1)<< ", " << oJacobianInverse.at<float>(0,2) << " ]" << std::endl;
|
|
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << ", " << oJacobianInverse.at<float>(1,2)<< " ]" << std::endl;
|
|
std::cout << "[ " << oJacobianInverse.at<float>(2,0) << ", " << oJacobianInverse.at<float>(2,1) << ", " << oJacobianInverse.at<float>(2,2)<< " ]" << std::endl;
|
|
}
|
|
else
|
|
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
|
|
|
|
return rank;
|
|
}
|
|
|
|
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
|
|
{
|
|
cv::Mat1f oJacobianMatrix(2, 2);
|
|
cv::Mat oJacobianInverse;
|
|
|
|
if (vJacobianMatrix.size() == 4)
|
|
{
|
|
// Converts the Jacobian matrix from std::vector to cv::Mat
|
|
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
|
|
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
|
|
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
|
|
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
|
|
std::cout << "=====Jacobian Matrix=====" << std::endl;
|
|
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << " ]" << std::endl;
|
|
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << " ]" << std::endl;
|
|
// Determines the inverse of the Jacobian matrix
|
|
cv::invert(oJacobianMatrix, oJacobianInverse, cv::DECOMP_SVD);
|
|
//oJacobianInverse = oJacobianMatrix.inv();
|
|
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
|
|
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1) << " ]" << std::endl;
|
|
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << " ]" << std::endl;
|
|
}
|
|
else
|
|
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
|
|
|
|
return oJacobianInverse;
|
|
}*/
|