From 632ff694d2b26b7bdff55cbf1e4768f5c56a50d4 Mon Sep 17 00:00:00 2001
From: ros <ros@ubuntuVM.myguest.virtualbox.org>
Date: Mon, 31 Mar 2025 15:48:03 +0200
Subject: [PATCH] Added computeDifferentialKinematics.cpp and upated
 Kinematics.h.

---
 include/Kinematics.h   |  4 +++
 src/Kinematics.cpp     | 71 ++++++++++++++++++++++++++++++++++++++++++
 src/testKinematics.cpp | 12 +++----
 3 files changed, 81 insertions(+), 6 deletions(-)

diff --git a/include/Kinematics.h b/include/Kinematics.h
index 246781a..04fc5e1 100644
--- a/include/Kinematics.h
+++ b/include/Kinematics.h
@@ -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);
diff --git a/src/Kinematics.cpp b/src/Kinematics.cpp
index 0ef3916..db043c4 100644
--- a/src/Kinematics.cpp
+++ b/src/Kinematics.cpp
@@ -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;
diff --git a/src/testKinematics.cpp b/src/testKinematics.cpp
index ce3cfa3..04112e7 100644
--- a/src/testKinematics.cpp
+++ b/src/testKinematics.cpp
@@ -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;
-	*/
+	
 }