Updated each code. Added linearControl and imageDraw as provided resources.

src/Kinematics.cpp

@@ -3,20 +3,25 @@
 
 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;
+	//Printing each joint value to get the target x,y
-	std::vector<float> X;
+    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);
 	
@@ -25,23 +30,30 @@ std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float
 
 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);
 	}
@@ -49,27 +61,32 @@ std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2
 	{
 		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);
 	}
@@ -79,13 +96,14 @@ std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2
 
 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);
@@ -158,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

@@ -32,26 +32,26 @@ int main(int argc, char** argv)
 {
 	if (argc == 5)
 	{
-		// Retrieves the args and stores them into variables
+		//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
+		//Initializes the robot
 		initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
 		
-		// Computes IK
+		//Computes IK
 		std::vector<float> qi = computeInverseKinematics(x, y, L1, L2);
 		
-		// Computes FK
+		//Computes FK
 		computeForwardKinematics(qi[1], qi[2], atof(argv[1]), atof(argv[2]));
-		// Computes Differential Kinematics
+		//Computes DK
 		std::vector<float> vJacobianMatrix = computeDifferentialKinematics(qi[1], qi[2], L1, L2);
-		int rank =  computeJacobianMatrixRank(vJacobianMatrix, 0.1);
+		int rk =  computeJacobianMatrixRank(vJacobianMatrix, 0.1);
 		
-		// Sends the target joint values received only if there is at least a solution
+		//Sends the target joint values received only if there is at least a solution
 		if (qi.size() >= 3)
 		{
 			std::vector<float> vTargetJointPosition;
@@ -61,10 +61,10 @@ int main(int argc, char** argv)
 			_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
 		}
 		
-		// Waits 2s
+		//Waits 2s
 		std::this_thread::sleep_for(std::chrono::milliseconds(2000));
 
-		// Closes robot connection
+		//Closes robot connection
 		_oDxlHandler.closePort();
 		
 	}
@@ -75,4 +75,4 @@ int main(int argc, char** argv)
 	}
 		
 	return 0;
-}
+}

src/linearControl.cpp

@@ -83,7 +83,7 @@ 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);
-			// Uodates the target joint velocity
+			// 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