Add Compute InverseKinematics

include/Kinematics.h

@@ -11,6 +11,8 @@ 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);
+
 int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
 
 cv::Mat  computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);

src/Kinematics.cpp

@@ -1,5 +1,5 @@
 #include "Kinematics.h"
-
+#include <cmath>
 
 float deg2rad(float angle)
 {
@@ -19,10 +19,27 @@ std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float
 	std::vector<float> X;
 	X.push_back(x);
 	X.push_back(y);
-	
+
 	return X;
 }
 
+std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
+{	std::vector<float> Q;
+	float cosQ2 = (x*x+y*y-L1*L1-L2*L2)/(2*L1*L2);
+	if(cosQ2 <= 1 && cosQ2 >= -1)
+	{
+		float q2 = acos(cosQ2);
+                float q1 = atan2(y, x) -atan2(L2*sin(q2), L1 + L2*cosQ2);
+
+		Q.push_back(q1);
+		Q.push_back(q2);
+	}
+
+        //std::cout << "[INFO] Inverse Kinematics : (x, y)->(q1, q2) = (" << x << ", " << y << ")->("<< deg2rad(q1) << ", " << deg2rad(q2) << ")"<< std::endl;
+
+	return Q;
+}
+
 int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
 {
 	int rank = -1;
@@ -87,4 +104,4 @@ cv::Mat  computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
 		std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
 
 	return oJacobianInverse;
-}
+}

src/testKinematics.cpp

@@ -7,6 +7,10 @@ int main()
         float q2 = deg2rad(0.0);
         float l1 = 5.0;
         float l2 = 6.0;
+        float x = 0;
+        float y = 0;
+
+        //1st test
         std::vector<float> expectedOutput {0.0, 11.0};
 
         std::vector<float> computedOutput = computeForwardKinematics(q1, q2, l1, l2);
@@ -17,5 +21,17 @@ int main()
                 }
                 if (error < threshold)
                         std::cout<<"test passed"<<std::endl;
+        //2nd test
+        std::vector<float> expectedOutput2 {0.0, 0.0};
+
+        std::vector<float> computedOutput2 = computeInverseKinematics(x, y, l1, l2);
+            float error2 = 0.0;
+            for (int l=0; l<expectedOutput2.size(); l++)
+            {
+                error2 += abs(computedOutput2[l]-expectedOutput2[l]);
+            }
+            if (error2 < threshold)
+                    std::cout<<"test passed"<<std::endl;
+
         return 0;
 }