charles.stelandre
/
IntroRoboticsLab1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added computeDifferentialKinematics.cpp and upated Kinematics.h.

This commit is contained in:
ros 2025-03-31 15:48:03 +02:00
parent c04547d57b
commit 632ff694d2
3 changed files with 81 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
include/Kinematics.h
View File

@ -11,6 +11,10 @@ float rad2deg(float angle);
std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2);
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2);
std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2);
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);

71
src/Kinematics.cpp
View File

@ -23,6 +23,77 @@ std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float
return X;
}
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
{
std::vector<float> qi;
float cos_q2 = (x*x+y*y-(L1*L1+L2*L2)) / (2.0 * L1 * L2);
std::cout << "[INFO] cos_q2= " << cos_q2 << std::endl;
if (cos_q2 >1 | cos_q2 <-1)
{
qi.push_back(0.0);
std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl;
}
else if (cos_q2 == 1)
{
qi.push_back(1.0);
float q1 = atan2(y, x);
float q2 = 0;
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
}
else if (cos_q2 == -1)
{
qi.push_back(1.0);
float q1 = atan2(y, x);
float q2 = M_PI;
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
}
else
{
qi.push_back(2.0);
std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl;
float q2 = acos(cos_q2);
float q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2));
std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
q2 = -acos(cos_q2);
q1 = (float)(atan2(y, x) - atan2(L2*sin(q2), L1+L2*cos_q2));
std::cout << "\t(x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
}
return qi;
}
std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
{
std::vector<float> jacobian;
float j11 = -L2*sin(q1+q2) - L1*sin(q1);
float j12 = -L2*sin(q1+q2);
float j21 = L2*cos(q1+q2) + L1*cos(q1);
float j22 = L2*cos(q1+q2);
jacobian.push_back(j11);
jacobian.push_back(j12);
jacobian.push_back(j21);
jacobian.push_back(j22);
return jacobian;
}
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
{
int rank = -1;

12
src/testKinematics.cpp
View File

@ -5,8 +5,8 @@
#define ERROR_THRESHOLD 0.00001
std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2);
//std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2);
//std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2);
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2);
std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2);
bool testFK(float q1, float q2, float L1, float L2, std::vector<float> expectedEndEffectorPosition, float errorThreshold)
@ -31,7 +31,7 @@ bool testFK(float q1, float q2, float L1, float L2, std::vector<float> expectedE
else
return false;
}
/*
bool testIK(float x, float y, float L1, float L2, std::vector<float> expectedJointValues, float errorThreshold)
{
@ -55,7 +55,7 @@ bool testIK(float x, float y, float L1, float L2, std::vector<float> expectedJoi
else
return false;
}
*/
int main()
{
// Tests Forward Kinematics
@ -83,7 +83,7 @@ int main()
std::cout << "PASSED!" << std::endl;
else
std::cout << "FAILED!" << std::endl;
/*
// Tests Inverse Kinematics
std::cout << "====TESTING INVERSE KINEMATICS====" << std::endl;
std::cout << "-> Test #1......"<< std::endl;
@ -119,5 +119,5 @@ int main()
std::cout << "FAILED!" << std::endl;
return 0;
*/
}