test inverse Kinematics

include/Kinematics.h

@@ -14,3 +14,5 @@ std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float
 int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
 
 cv::Mat  computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);
+
+std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2);

src/Kinematics.cpp

@@ -23,6 +23,37 @@ 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> q;
+    
+    // Calculate distance between end effector and origin
+    float r = sqrt(x*x + y*y);
+    
+    // Calculate q2 using cosine law
+    float cos_q2 = (r*r - L1*L1 - L2*L2) / (2 * L1 * L2);
+    float sin_q2 = sqrt(1 - cos_q2*cos_q2);
+    float q2 = atan2(sin_q2, cos_q2);
+    
+    // Calculate q1 using trigonometry
+    float q1 = atan2(y, x) - atan2(L2*sin(q2), L1 + L2*cos(q2));
+    
+    // Convert angles to degrees
+    q1 = rad2deg(q1);
+    q2 = rad2deg(q2);
+	
+	std::cout << "[INFO] Inverse Kinematics : (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << q1 << ", " << q2 << ")" << std::endl;
+    
+    q.push_back(q1);
+    q.push_back(q2);
+    
+    return q;
+}
+
+
 int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
 {
 	int rank = -1;

src/testKinematics.cpp

@@ -1,7 +1,7 @@
 #include "Kinematics.h"
 #include <iostream>
 int main (){
-	// 1st test
+	// 1st test FK
 	float threshold=0.00001;
 	float q1=deg2rad(0.0); //deg
 	float q2=deg2rad(0.0); //deg
@@ -18,7 +18,7 @@ int main (){
 		std::cout<<"test passed"<<std::endl;
 		}
 	
-	// 2nd test
+	// 2nd test FK
 	q1=deg2rad(90.0); //deg
 	q2=deg2rad(0.0); //deg
 	expectedOutput[0] = 0.0;
@@ -33,6 +33,25 @@ int main (){
 		std::cout<<"test passed"<<std::endl;
 		}	
 		
+	//1rs test IK
+	L1 = 5.0; // Length of link 1
+	L2 = 6.0; // Length of link 2
+	float x =11.0; // X coordinate
+	float y = 0.0;  // Y coordinate
+	expectedOutput[0] = 0.0;
+	expectedOutput[1] = 0.0;
+		
+	computedOutput=computeInverseKinematics(x,y,L1,L2);
+	error = 0.0;
+	for(int l=0; l< expectedOutput.size();l++){
+		error += abs(computedOutput[l]-expectedOutput[l]);
+		}
+	if(error < threshold){
+		std::cout<<"test passed"<<std::endl;
+		}	
+
+	
+		
 	return 0;
 	
 }