Corrected the singularity detection within linearControlFurther to detect det(J) anomalies.

makefile

@@ -1,9 +1,10 @@
-all: kinematics dynamics joint cartesian image test linear draw
+all: linearFurther kinematics dynamics joint cartesian image test linear draw
 	g++ -o bin/jointControl lib/jointControl.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	g++ -o bin/cartControl lib/cartControl.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	g++ -o bin/testKinematics lib/testKinematics.o lib/Kinematics.o `pkg-config --libs opencv4`
 	g++ -o bin/linearControl lib/linearControl.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	g++ -o bin/drawImage lib/drawImage.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
+	g++ -o bin/linearControlFurther lib/linearControlFurther.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	
 kinematics: src/Kinematics.cpp
 	g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4
@@ -20,6 +21,9 @@ cartesian: src/cartControl.cpp
 linear: src/linearControl.cpp
 	g++ -c src/linearControl.cpp -o lib/linearControl.o -I./include -I/usr/include/opencv4
 
+linearFurther: src/linearControlFurther.cpp
+	g++ -c src/linearControlFurther.cpp -o lib/linearControlFurther.o -I./include -I/usr/include/opencv4
+	
 image: src/ImageProcessing.cpp
 	g++ -c src/ImageProcessing.cpp -o lib/ImageProcessing.o -I./include -I/usr/include/opencv4
 	

src/linearControlFurther.cpp

@@ -8,7 +8,7 @@
 float _fDistanceThreshold = 0.1f; // in cm
 float _pController = 0.1f;
 float _fps = 20.0f; // in Hz
-
+float det_threshold = 1e-6;
 // Dynamixel config infos
 DynamixelHandler _oDxlHandler;
 std::string _robotDxlPortName = "/dev/ttyUSB0";
@@ -48,6 +48,9 @@ int main(int argc, char** argv)
 		std::cout << "init" ;
 		initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
 		std::cout << "OK" << std::endl;
+		// Changes the control mode
+		//_oDxlHandler.setControlMode(1); //wheel mode
+		// VELOCITY CONTROL qdot
 		
 		// Gets the initial joint positions
 		std::cout << "readcurrent" ;
@@ -56,6 +59,7 @@ int main(int argc, char** argv)
 		std::cout << "OK" << std::endl;
 		// Inits joint velocities
 		std::vector<float> l_vCurrentJointVelocity; l_vCurrentJointVelocity.push_back(0.0f); l_vCurrentJointVelocity.push_back(0.0f); l_vCurrentJointVelocity.push_back(0.0f);
+		// VELOCITY CONTROL qdot
 		
 		// Inits the loop
 		bool bIsFarFromTarget = true;
@@ -68,8 +72,18 @@ int main(int argc, char** argv)
 			std::vector<float> deltaX{x-vEndEffectorPosition[0], y - vEndEffectorPosition[1]};
 			// Computes Jacobian matrix
 			std::vector<float> vJacobianMatrix = computeDifferentialKinematics(l_vCurrentJointPosition[0], l_vCurrentJointPosition[2], L1, L2);
-			// Computes Inverse of Jacobian matrix
-			cv::Mat vInverseJacobianMatrix = computeInverseJacobianMatrix(vJacobianMatrix);
+			
+			float det = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]);
+			cv::Mat vInverseJacobianMatrix;
+			if (det < det_threshold){
+				// Prints an error if close to a singularity.
+				std::cerr << "[WARNING] Jacobian determinant is close to 0 !(" << det << "(, potential singularity." << std::endl;
+				// If there is a singularity, return identity matrix (do not move)
+				cv::Mat vInverseJacobianMatrix = cv::Mat::eye(2,2,CV_32F);
+			} else {
+				// Computes Inverse of Jacobian matrix
+				cv::Mat vInverseJacobianMatrix = computeInverseJacobianMatrix(vJacobianMatrix);
+			}
 			// Determines deltaX
 			cv::Mat1f oDeltaX(2, 1);
 			oDeltaX.at<float>(0, 0) = _pController * deltaX[0];
@@ -79,27 +93,6 @@ int main(int argc, char** argv)
 			// Updates the current joint position
 			l_vCurrentJointPosition[0] += deltaQ.at<float>(0,0);
 			l_vCurrentJointPosition[2] += deltaQ.at<float>(1,0);
-            
-            float q1_rad = l_vCurrentJointPosition[0];
-                float q2_rad = l_vCurrentJointPosition[2];
-                float q_limit_rad = deg2rad(180.0f);
-
-                if (q1_rad > q_limit_rad) {
-                    l_vCurrentJointPosition[0] = q_limit_rad;
-                    std::cerr << "[WARNING] Joint 1 angle exceeded upper limit, clamping to " << rad2deg(q_limit_rad) << " degrees." << std::endl;
-                } else if (q1_rad < -q_limit_rad) {
-                    l_vCurrentJointPosition[0] = -q_limit_rad;
-                    std::cerr << "[WARNING] Joint 1 angle exceeded lower limit, clamping to " << rad2deg(-q_limit_rad) << " degrees." << std::endl;
-                }
-
-                if (q2_rad > q_limit_rad) {
-                    l_vCurrentJointPosition[2] = q_limit_rad;
-                    std::cerr << "[WARNING] Joint 2 angle exceeded upper limit, clamping to " << rad2deg(q_limit_rad) << " degrees." << std::endl;
-                } else if (q2_rad < -q_limit_rad) {
-                    l_vCurrentJointPosition[2] = -q_limit_rad;
-                    std::cerr << "[WARNING] Joint 2 angle exceeded lower limit, clamping to " << rad2deg(-q_limit_rad) << " degrees." << std::endl;
-                }
-            
 			// Updates the target joint velocity
 			l_vCurrentJointVelocity[0] = deltaQ.at<float>(0,0)*_fps;
 			l_vCurrentJointVelocity[2] = deltaQ.at<float>(1,0)*_fps;