Added more comments in Kinematics.

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);

makefile

@@ -1,22 +1,34 @@
-all: kinematics dynamics joint testKinematics
+all: 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/testKinematics lib/testKinematics.o lib/Kinematics.o -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
-
-testKinematics: src/testKinematics.cpp
-	g++ -c src/testKinematics.cpp -o lib/testKinematics.o -I./include -I/usr/include/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`
+	
 kinematics: src/Kinematics.cpp
 	g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4
 
+test: src/testKinematics.cpp
+	g++ -c src/testKinematics.cpp -o lib/testKinematics.o -I./include -I/usr/include/opencv4
+	
 joint: src/jointControl.cpp
 	g++ -c src/jointControl.cpp -o lib/jointControl.o -I./include -I/usr/include/opencv4
 	
+cartesian: src/cartControl.cpp
+	g++ -c src/cartControl.cpp -o lib/cartControl.o -I./include -I/usr/include/opencv4
+	
+linear: src/linearControl.cpp
+	g++ -c src/linearControl.cpp -o lib/linearControl.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
 	
 dynamics: src/DynamixelHandler.cpp
 	g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include
-
+	
+draw: src/drawImage.cpp
+	g++ -c src/drawImage.cpp -o lib/drawImage.o -I./include -I/usr/include/opencv4
+	
 clean:
 	rm lib/*.o
-	rm bin/*
+	rm bin/*

src/Kinematics.cpp

@@ -3,26 +3,115 @@
 
 float deg2rad(float angle)
 {
+    //Converts degrees to radians
 	return angle/180.0*M_PI;
 }
 
 float rad2deg(float angle)
 {
+    //Converts radians to degrees
 	return angle*180.0/M_PI;
 }
 
 std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2)
 {
+    //Computes the x,y coordinates
 	float x = L1 * cos(q1) + L2 * cos(q1+q2);
 	float y = L1 * sin(q1) + L2 * sin(q1+q2);
-	std::cout << "[INFO] Forward Kinematics : (q1, q2)->(x, y) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ")->(" << x << ", " << y << ")" << std::endl;
-	std::vector<float> X;
+	//Printing each joint value to get the target x,y
+    std::cout << "[INFO] Forward Kinematics : (q1, q2)->(x, y) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ")->(" << x << ", " << y << ")" << std::endl;
+	//Declare a vector X containing target x,y
+    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)
+{
+    //declares an empty vector qi
+	std::vector<float> qi;
+	
+    //From the equations, computes the cosine(q2) to be used to find the number of solutions
+	float cos_q2 = (x*x+y*y-(L1*L1+L2*L2)) / (2.0 * L1 * L2);
+	
+	std::cout << "[INFO] cos_q2= " << cos_q2 << std::endl;
+	//Checks if there is no solution (out of reach)
+	if (cos_q2 >1 | cos_q2 <-1)
+	{
+        //Fills the first value of qi with a value qi[0]=0.0, indicating no solution
+		qi.push_back(0.0);
+		std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl;
+	}
+    //Checks if there is one solution exactly
+	else if (cos_q2 == 1)
+	{
+        //Fills the first value of qi with a value qi[0]=1.0, indicating a single solution
+		qi.push_back(1.0);
+		float q1 = atan2(y, x);
+        //q2=0 means arm is extended, consistantly with cos_q2==1
+		float q2 = 0;
+		std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
+        //Adds q1 and q2 to qi
+		qi.push_back(q1);
+		qi.push_back(q2);
+	}
+	else if (cos_q2 == -1)
+	{
+		qi.push_back(1.0);
+		float q1 = atan2(y, x);
+        //q2=0 means arm is folded on itself, consistantly with cos_q2==-1
+		float q2 = M_PI;
+		std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
+        //Adds q1 and q2 to qi
+		qi.push_back(q1);
+		qi.push_back(q2);
+	}
+	else
+	{
+        //Fills the first value of qi with a value qi[0]=2.0, indicating two solutions
+		qi.push_back(2.0);
+		std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl;
+		//Computes the joint values
+		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;
+        //Adds q1 and q2 to qi
+		qi.push_back(q1);
+		qi.push_back(q2);
+		
+		//Compute the second solution
+		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;
+        //Adds the second solution at the end of vector qi.
+		qi.push_back(q1);
+		qi.push_back(q2);
+	}
+	
+	return qi;
+}
+
+std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
+{
+    //Creates a vector jacobian
+	std::vector<float> jacobian;
+	//Computes the jacobian's coefficients
+	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);
+	//Adds each coefficient to the jacobian vector
+	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;
@@ -87,4 +176,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/cartControl.cpp

@@ -0,0 +1,78 @@
+#include <chrono>
+#include <thread>
+
+#include "Kinematics.h"
+#include "DynamixelHandler.h"
+
+DynamixelHandler _oDxlHandler;
+std::string _robotDxlPortName = "/dev/ttyUSB0";
+float _robotDxlProtocol = 2.0;
+int _robotDxlBaudRate = 1000000;
+
+
+void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+{
+	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
+	dxlHandler.setDeviceName(portName);
+	dxlHandler.setProtocolVersion(protocol);
+	dxlHandler.openPort();
+	dxlHandler.setBaudRate(baudRate);
+	dxlHandler.enableTorque(true);
+	std::cout << std::endl;
+}
+
+void closeRobot(DynamixelHandler& dxlHandler)
+{
+	dxlHandler.enableTorque(false);
+	dxlHandler.closePort();
+}
+
+
+int main(int argc, char** argv)
+{
+	if (argc == 5)
+	{
+		//Retrieves the args and stores them into variables
+		float L1 = atof(argv[1]); // in cm
+		float L2 = atof(argv[2]); // in cm
+		float x = atof(argv[3]); // in cm
+		float y = atof(argv[4]); // in cm
+		float qpen = deg2rad(-90); // in rad
+			
+		//Initializes the robot
+		initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
+		
+		//Computes IK
+		std::vector<float> qi = computeInverseKinematics(x, y, L1, L2);
+		
+		//Computes FK
+		computeForwardKinematics(qi[1], qi[2], atof(argv[1]), atof(argv[2]));
+		//Computes DK
+		std::vector<float> vJacobianMatrix = computeDifferentialKinematics(qi[1], qi[2], L1, L2);
+		int rk =  computeJacobianMatrixRank(vJacobianMatrix, 0.1);
+		
+		//Sends the target joint values received only if there is at least a solution
+		if (qi.size() >= 3)
+		{
+			std::vector<float> vTargetJointPosition;
+				vTargetJointPosition.push_back(qi[1]); 
+				vTargetJointPosition.push_back(qpen); 
+				vTargetJointPosition.push_back(qi[2]); 
+			_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
+		}
+		
+		//Waits 2s
+		std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+
+		//Closes robot connection
+		_oDxlHandler.closePort();
+		
+	}
+	else
+	{
+		std::cout << "[WARNING] Cartesian control" << std::endl;
+		std::cout << ">> cartControl L1(cm) L2(cm) x(cm) y(cm)"<< std::endl;
+	}
+		
+	return 0;
+}

src/drawImage.cpp

@@ -0,0 +1,76 @@
+#include <chrono>
+#include <thread>
+
+#include "Kinematics.h"
+#include "ImageProcessing.h"
+
+
+// Dynamixel config infos
+DynamixelHandler _oDxlHandler;
+std::string _robotDxlPortName = "/dev/ttyUSB0";
+float _robotDxlProtocol = 2.0;
+int _robotDxlBaudRate = 1000000;
+
+
+void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+{
+	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
+	dxlHandler.setDeviceName(portName);
+	dxlHandler.setProtocolVersion(protocol);
+	dxlHandler.openPort();
+	dxlHandler.setBaudRate(baudRate);
+	dxlHandler.enableTorque(true);
+	std::cout << std::endl;
+}
+
+void closeRobot(DynamixelHandler& dxlHandler)
+{
+	dxlHandler.enableTorque(false);
+	dxlHandler.closePort();
+}
+
+int main(int argc, char** argv)
+{
+	if (argc == 4)
+	{
+		// Retrieves the args and stores them into variables
+		float L1 = atof(argv[1]); // in cm
+		float L2 = atof(argv[2]); // in cm
+		std::string imgFilename = argv[3]; // image filename
+			
+		// Initializes the robot
+		initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
+		
+		// Loads the image
+		cv::Mat l_oRawImg = cv::imread(imgFilename);
+		if(l_oRawImg.empty())
+		{
+			std::cout << "[ERROR] Could not read the image: " << imgFilename << std::endl;
+			return 1;
+		}
+		
+		// Extracts its contours in the pixel reference frame
+		std::vector<std::vector<cv::Point>> vContoursInPixel = imageProcessing(l_oRawImg);
+		
+		// Converts the contours to the robot reference frame
+		float theta = -180.0f/180.0f*M_PI; float tx = 110; float ty = 20; int imageWidth =  l_oRawImg.cols; int imageHeight = l_oRawImg.rows;
+		std::vector<std::vector<float>> vContoursInMm = convertContoursPixel2Mm(vContoursInPixel, theta, tx, ty, imageWidth, imageHeight);
+		
+		// Sends the contours to the robot 
+		sendContours(vContoursInPixel, vContoursInMm, _oDxlHandler, L1, L2);
+		
+		// Waits 2s
+		std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+
+		// Closes robot connection
+		_oDxlHandler.closePort();
+		
+	}
+	else
+	{
+		std::cout << "[WARNING] Draw image" << std::endl;
+		std::cout << ">> drawImage L1(cm) L2(cm) img_filename"<< std::endl;
+	}
+		
+	return 0;
+}

src/linearControl.cpp

@@ -0,0 +1,141 @@
+#include <chrono>
+#include <thread>
+
+#include "Kinematics.h"
+#include "DynamixelHandler.h"
+
+// TO BE ADJUSTED IF NECESSARY
+float _fDistanceThreshold = 0.1f; // in cm
+float _pController = 0.1f;
+float _fps = 20.0f; // in Hz
+
+// Dynamixel config infos
+DynamixelHandler _oDxlHandler;
+std::string _robotDxlPortName = "/dev/ttyUSB0";
+float _robotDxlProtocol = 2.0;
+int _robotDxlBaudRate = 1000000;
+
+
+void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+{
+	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
+	dxlHandler.setDeviceName(portName);
+	dxlHandler.setProtocolVersion(protocol);
+	dxlHandler.openPort();
+	dxlHandler.setBaudRate(baudRate);
+	dxlHandler.enableTorque(true);
+	std::cout << std::endl;
+}
+
+void closeRobot(DynamixelHandler& dxlHandler)
+{
+	dxlHandler.enableTorque(false);
+	dxlHandler.closePort();
+}
+
+int main(int argc, char** argv)
+{
+	if (argc == 5)
+	{
+		// Retrieves the args and stores them into variables
+		float L1 = atof(argv[1]); // in cm
+		float L2 = atof(argv[2]); // in cm
+		float x = atof(argv[3]); // in cm
+		float y = atof(argv[4]); // in cm
+		float qpen = deg2rad(-90); // in rad
+			
+		// Initializes the robot
+		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" ;
+		std::vector<float> l_vCurrentJointPosition;
+		_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+		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;
+				
+		while (bIsFarFromTarget)
+		{
+			// Estimates the position of the end-effector using FK
+			std::vector<float> vEndEffectorPosition = computeForwardKinematics(l_vCurrentJointPosition[0], l_vCurrentJointPosition[2], L1, L2);
+			// Computes deltaX
+			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);
+			// Determines deltaX
+			cv::Mat1f oDeltaX(2, 1);
+			oDeltaX.at<float>(0, 0) = _pController * deltaX[0];
+			oDeltaX.at<float>(1, 0) = _pController * deltaX[1];
+			// Computes deltaQ
+			cv::Mat deltaQ = vInverseJacobianMatrix * oDeltaX;
+			// Updates the current joint position
+			l_vCurrentJointPosition[0] += deltaQ.at<float>(0,0);
+			l_vCurrentJointPosition[2] += deltaQ.at<float>(1,0);
+			// Updates the target joint velocity
+			l_vCurrentJointVelocity[0] = deltaQ.at<float>(0,0)*_fps;
+			l_vCurrentJointVelocity[2] = deltaQ.at<float>(1,0)*_fps;
+			// VELOCITY CONTROL qdot
+			// Creates the target joint position vector
+			std::vector<float> vTargetJointPosition;
+			if(l_vCurrentJointPosition.size()>=3){
+				vTargetJointPosition.push_back(l_vCurrentJointPosition[0]); 
+				vTargetJointPosition.push_back(qpen); 
+				vTargetJointPosition.push_back(l_vCurrentJointPosition[2]); 
+			// Sends the target joint position to actuate the robot
+			_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
+			}
+			// Creates the target joint velocity vector
+			std::vector<float> vTargetJointVelocity;
+				vTargetJointVelocity.push_back(l_vCurrentJointVelocity[0]); 
+				vTargetJointVelocity.push_back(0.0f); // pen lift up/down
+				vTargetJointVelocity.push_back(l_vCurrentJointVelocity[2]); 
+			// Sends the target joint position to actuate the robot
+			//_oDxlHandler.sendTargetJointVelocity(vTargetJointVelocity);
+			// VELOCITY CONTROL qdot
+			
+			// Waits 1000/fps ms
+			std::this_thread::sleep_for(std::chrono::milliseconds((long int)(1000.0f/_fps)));
+			
+			// Estimates the distance to the target
+			float currentDistanceToTarget = sqrt(pow(y - vEndEffectorPosition[1],2) + pow(x - vEndEffectorPosition[0], 2));
+			std::cout << "====LINEAR CONTROL====" << std::endl;
+			std::cout << "Current Joint Position in deg (q1, q2)= (" << rad2deg(l_vCurrentJointPosition[0]) << ", " << rad2deg(l_vCurrentJointPosition[2]) << ")" << std::endl;
+			std::cout << "Current Joint Velocity in deg/s (q1, q2)= (" << rad2deg(l_vCurrentJointVelocity[0]) << ", " << rad2deg(l_vCurrentJointVelocity[2]) << ")" << std::endl;
+			std::cout << "Current Cartesian Position (x, y)= (" << vEndEffectorPosition[0] << ", " << vEndEffectorPosition[0] << ")" << std::endl;
+			std::cout << "Current Distance To Target= " << currentDistanceToTarget << std::endl;
+			std::cout << "==================" << std::endl;
+
+			// Determines if the loop should continue
+			if (currentDistanceToTarget < _fDistanceThreshold)
+			{
+				bIsFarFromTarget = false;
+			}
+		}
+		
+		// Waits 2s
+		std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+
+		// Closes robot connection
+		_oDxlHandler.closePort();
+		
+	}
+	else
+	{
+		std::cout << "[WARNING] Cartesian control" << std::endl;
+		std::cout << ">> linearControl L1(cm) L2(cm) x(cm) y(cm)"<< std::endl;
+	}
+		
+	return 0;
+}

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,4 @@ int main()
 		std::cout << "FAILED!" << std::endl;
 	
 	return 0;
-	*/
-}
+}