Added indication in README.md : Add a lib and bin folder to receive make results

README.md

@@ -15,22 +15,33 @@ Each of the following codes allows a specific directive to be sent to a 2D plana
 **[linearControlFurther](#linearControlFurther)**: Improvement of the linear control which checks for the determinant of the Jacobian to be valid.
 
 # Repository Structure
+## Find below the list of executables.
 
 ### jointControl/
 
-* @file      jointControl.cpp
-* @author    Guillaume Gibert
-* @version   -
-* @date      2025/04/13
-* @brief     Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
-* @functions :
+ * @file      jointControl
+ * @author    Guillaume Gibert
+ * @version   -
+ * @date      2025/04/13
+ * @brief     Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
+ * @functions :
         - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
         - void closeRobot(DynamixelHandler& dxlHandler)
         - int main()
+ * Input:
+ * - L1 : Length of the first link (cm)
+ * - L2 : Length of the second link (cm)
+ * - q1 : Input for first joint value (deg)
+ * - q2 : Input for second joint value (deg)
+ * - qpen : Input for the pen's joint value (deg) (-90 for drawing mode)
+ *
+ * Output:
+ * - Displacement of the robot using cartesian coordinates.
+ ## Ex. : ```./jointControl 5 6 0 0 -90``` 
 
 ### cartControl/
- * @file      cartControl.cpp
- * @author    Charles STELANDRE
+ * @file      cartControl
+ * @author    Guillaume Gibert, Charles STELANDRE
  * @version   V1
  * @date      2025/04/13
  * @brief     Description : This file is a variant from jointControl, adapting cartesian Coordinates translation.
@@ -46,12 +57,13 @@ Each of the following codes allows a specific directive to be sent to a 2D plana
  *
  * Output:
  * - Displacement of the robot using cartesian coordinates.
+  ## Ex. : ```./cartControl 5 6 8 3```
 
 
 ### testKinematics/
 
-* @file      testKinematics.cpp
-* @author    Guillaume Gibert
+* @file      testKinematics
+* @author    Guillaume Gibert, Charles Stelandre
 * @version   -
 * @date      2025/04/13
 * @brief     Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
@@ -59,10 +71,12 @@ Each of the following codes allows a specific directive to be sent to a 2D plana
         - bool testFK(float q1, float q2, float L1, float L2, std::vector<float> expectedEndEffectorPosition, float errorThreshold)
         - bool testIK(float x, float y, float L1, float L2, std::vector<float> expectedJointValues, float errorThreshold)
         - int main()
+  ## Ex. : ```./testKinematics```
+
 
 
  ### linearControl/
- * @file      linearControl.cpp
+ * @file      linearControl
  * @author    Guillaume Gibert
  * @version   -
  * @date      2025/04/13
@@ -79,10 +93,12 @@ Each of the following codes allows a specific directive to be sent to a 2D plana
  *
  * Output:
  * - Displacement of the robot using cartesian coordinates.
+   ## Ex. : ```./linearControl 5 6 8 3```
+   
 
 ### drawImage/
 
- * @file      drawImage.cpp
+ * @file      drawImage
  * @author    Guillaume Gibert
  * @version   -
  * @date      2025/04/13
@@ -95,16 +111,18 @@ Each of the following codes allows a specific directive to be sent to a 2D plana
  * - L1 : Length of the first link (cm)
  * - L2 : Length of the second link (cm)
  * - image : directory of an image to use for image drawing
+   ## Ex. : ```./drawImage 5 6 ../resource/twitter2.png ```
+   
 
 ### linearControlFurther/
 
- * @file      linearControl.cpp
- * @author    Guillaume Gibert
+ * @file      linearControl
+ * @author    Guillaume Gibert, Charles Stelandre
  * @version   -
  * @date      2025/04/13
- * @brief     Description : This file was provided by G. Gibert. It allows to use a proportionally controlled system to join two points in a linear trajectory.
+ * @brief     Description : This file was provided by G. Gibert, and completed by C. Stelandre. It allows to use a proportionally controlled system to join two points in a linear trajectory, while checking for the validity of a target point.
  * @functions :
- *       - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+         - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
          - void closeRobot(DynamixelHandler& dxlHandler)
          - int main(int argc, char** argv)
 * Input:
@@ -112,6 +130,8 @@ Each of the following codes allows a specific directive to be sent to a 2D plana
  * - L2 : Length of the second link (cm)
  * - x : x-component of target position (cm)
  * - y : y-component of target position (cm)
+    ## Ex. : ```./linearControlFurther 5 6 8 3 ```
+
 
 # README.md
 
@@ -119,7 +139,7 @@ Documentation for the repository.
 
 # Requirements
 
-Hardware: Ergo Poppy Jr.
+Hardware: Poppy Ergo Jr.
 
 Software: C++ Programming
 
@@ -133,15 +153,16 @@ git clone https://gitarero.ecam.fr/charles.stelandre/IntroRoboticsLab1.git
 
 Open a text editor and navigate to the folder containing the .cpp file you want to use.
 
-Install any necessary libraries via Arduino Library Manager.
+Install any necessary library.
 
-Upload the code to your mBot using a USB connection.
+Upload the code to your robot using a USB connection.
 
 # Usage
 
 Navigate to your installation directory.
 
 Make the file using ``` make ```
+Note : Do not forget to create a lib and bin folder !
 
 Navigate to the bin folder.
 

src/Kinematics.cpp

@@ -1,9 +1,9 @@
 /*
  @file      Kinematics.cpp
- @author    Charles STELANDRE
+ @author    Guillaume Gibert, Charles STELANDRE
  @version   V1
  @date      2025/04/13
- @brief     Description : This file contains all kinematics computations for FK, IK and DK.
+ @brief     Description : This file contains all kinematics computations for FK, IK and DK. 
  @functions :
  @      - float deg2rad(float angle)
         - float rad2deg(float angle)
@@ -30,8 +30,8 @@ float rad2deg(float angle)
 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);
+	float x = L1*cos(q1) + L2*cos(q1+q2);
+	float y = L1*sin(q1) + L2*sin(q1+q2);
 	// 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
@@ -48,7 +48,7 @@ std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2
 	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);
+	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)
@@ -113,9 +113,9 @@ std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, f
     // Creates a vector jacobian
 	std::vector<float> jacobian;
 	// Computes the jacobian's coefficients
-	float j11 = -L2*sin(q1+q2) - L1*sin(q1);
+	float j11 = -L1*sin(q1) - L2*sin(q1+q2);
 	float j12 = -L2*sin(q1+q2);
-	float j21 = L2*cos(q1+q2) + L1*cos(q1); 
+	float j21 = L1*cos(q1) + L2*cos(q1+q2);
 	float j22 = L2*cos(q1+q2);
 	// Adds each coefficient to the jacobian vector
 	jacobian.push_back(j11);

src/cartControl.cpp

@@ -1,6 +1,6 @@
 /*
  @file      cartControl.cpp
- @author    Charles STELANDRE
+ @author    Guillaume Gibert, Charles STELANDRE
  @version   V1
  @date      2025/04/13
  @brief     Description : This file is a variant from jointControl, adapting cartesian Coordinates translation.

src/linearControlFurther.cpp

@@ -156,7 +156,7 @@ int main(int argc, char** argv)
 	else
 	{
 		std::cout << "[WARNING] Cartesian control" << std::endl;
-		std::cout << ">> linearControl L1(cm) L2(cm) x(cm) y(cm)"<< std::endl;
+		std::cout << ">> linearControlFurther L1(cm) L2(cm) x(cm) y(cm)"<< std::endl;
 	}
 		
 	return 0;

src/testKinematics.cpp

@@ -1,6 +1,6 @@
 /*
  @file      testKinematics.cpp
- @author    Guillaume Gibert
+ @author    Guillaume Gibert, Charles Stelandre
  @version   -
  @date      2025/04/13
  @brief     Description : This file was provided by G. Gibert. It allows to test different kinds of kinematics.
@@ -10,6 +10,145 @@
         - int main()
  */
 
+/*
+#include "Kinematics.h" // Assuming this header file exists and contains necessary declarations
+
+#define ERROR_THRESHOLD 0.00001
+
+std::vector<float> computeForwardKinematics(float L1, float L2, float q1, float q2);
+std::vector<float> computeInverseKinematics(float L1, float L2, float q1, float q2);
+std::vector<float> computeDifferentialKinematics(float L1, float L2, float q1, float q2);
+
+// Helper function to convert degrees to radians
+float deg2rad(float degrees) {
+    return degrees * M_PI / 180.0f;
+}
+
+// Helper function to compare two vectors of floats with a given error threshold
+bool compareVectors(const std::vector<float>& expected, const std::vector<float>& calculated, float threshold) {
+    if (expected.size() != calculated.size()) {
+        std::cout << "[ERROR] Expected and calculated vectors have different sizes!" << std::endl;
+        return false;
+    }
+    float totalError = 0.0f;
+    for (size_t i = 0; i < expected.size(); ++i) {
+        totalError += std::abs(expected[i] - calculated[i]);
+    }
+    std::cout << "[INFO] Total error = " << totalError << std::endl;
+    return totalError < threshold;
+}
+
+bool testForwardKinematics(float L1, float L2, float q1, float q2, std::vector<float> expectedPosition, float errTol)
+{
+    std::cout << "[TEST] Forward Kinematics with q1=" << rad2deg(q1) << " deg, q2=" << rad2deg(q2)
+              << " deg, L1=" << L1 << " cm, L2=" << L2 << " cm" << std::endl;
+    std::vector<float> actualPosition = computeForwardKinematics(L1, L2, q1, q2);
+    bool testPassed = compareVectors(expectedPosition, actualPosition, errTol);
+    std::cout << (testPassed ? "[RESULT] PASSED!" : "[RESULT] FAILED!") << std::endl;
+    return testPassed;
+}
+
+bool testInverseKinematics(float L1, float L2, float x, float y, std::vector<float> expectedJointAngles, float errTol)
+{
+    std::cout << "[TEST] Inverse Kinematics for x=" << x << " cm, y=" << y
+              << " cm, L1=" << L1 << " cm, L2=" << L2 << " cm" << std::endl;
+    std::vector<float> currentJointAngles = computeInverseKinematics(L1, L2, x, y);
+    bool testPassed = compareVectors(expectedJointAngles, currentJointAngles, errTol);
+    std::cout << (testPassed ? "[RESULT] PASSED!" : "[RESULT] FAILED!") << std::endl;
+    return testPassed;
+}
+
+int main()
+{
+    float L1 = 5.0f;
+    float L2 = 6.0f;
+    float tolerance = ERROR_THRESHOLD;
+    bool overallTestResult = true;
+
+
+    std::cout << "==== TESTING FORWARD KINEMATICS ====" << std::endl;
+
+    // Test Case 1: Both joints at 0 degrees
+    bool fkTest1Passed = testForwardKinematics(L1, L2, deg2rad(0.0f), deg2rad(0.0f), {11.0f, 0.0f}, tolerance);
+    overallTestResult &= fkTest1Passed;
+
+    // Test Case 2: First joint at 90 degrees, second at 0 degrees
+    bool fkTest2Passed = testForwardKinematics(L1, L2, deg2rad(90.0f), deg2rad(0.0f), {0.0f, 11.0f}, tolerance);
+    overallTestResult &= fkTest2Passed;
+
+    // Test Case 3: First joint at 0 degrees, second at 90 degrees
+    bool fkTest3Passed = testForwardKinematics(L1, L2, deg2rad(0.0f), deg2rad(90.0f), {5.0f, 6.0f}, tolerance);
+    overallTestResult &= fkTest3Passed;
+
+    // Test Case 4: Both joints at 90 degrees
+    bool fkTest4Passed = testForwardKinematics(L1, L2, deg2rad(90.0f), deg2rad(90.0f), {-6.0f, 5.0f}, tolerance);
+    overallTestResult &= fkTest4Passed;
+
+    // Test Case 5: First joint at 45 degrees, second at 45 degrees
+    float q1_fk_5 = deg2rad(45.0f);
+    float q2_fk_5 = deg2rad(45.0f);
+    float expected_x_fk_5 = L1*cos(q1_fk_5) + L2*cos(q1_fk_5 + q2_fk_5);
+    float expected_y_fk_5 = L1*sin(q1_fk_5) + L2*sin(q1_fk_5 + q2_fk_5);
+    bool fkTest5Passed = testForwardKinematics(L1, L2, q1_fk_5, q2_fk_5, {expected_x_fk_5, expected_y_fk_5}, tolerance);
+    overallTestResult &= fkTest5Passed;
+
+    // Test Case 6: Negative angles
+    bool fkTest6Passed = testForwardKinematics(L1, L2, deg2rad(-30.0f), deg2rad(-60.0f), {8.196f, -6.928f}, tolerance);
+    overallTestResult &= fkTest6Passed;
+
+ 
+ 
+    std::cout << "\n==== TESTING INVERSE KINEMATICS ====" << std::endl;
+
+    // Test Case 1: Target at (11, 0) - Robot fully extended
+    bool ikTest1Passed = testInverseKinematics(L1, L2, 11.0f, 0.0f, {deg2rad(0.0f), deg2rad(0.0f)}, tolerance);
+    overallTestResult &= ikTest1Passed;
+
+    // Test Case 2: Target at (0, 11) - Robot arm straight up
+    bool ikTest2Passed = testInverseKinematics(L1, L2, 0.0f, 11.0f, {deg2rad(90.0f), deg2rad(0.0f)}, tolerance);
+    overallTestResult &= ikTest2Passed;
+
+    // Test Case 3: Target at (5, 6) - Specific configuration
+    bool ikTest3Passed = testInverseKinematics(L1, L2, 5.0f, 6.0f, {deg2rad(90.0f), deg2rad(0.0f)}, tolerance); // Expected might need adjustment based on IK implementation
+    overallTestResult &= ikTest3Passed;
+
+    // Test Case 4: Target at (12, 0) - Unreachable point
+    bool ikTest4Passed = testInverseKinematics(L1, L2, 12.0f, 0.0f, {0.0f}, tolerance); // Expecting empty or specific error indication based on IK implementation
+    overallTestResult &= ikTest4Passed;
+
+    // Test Case 5: Target at a point requiring negative angles
+    bool ikTest5Passed = testInverseKinematics(L1, L2, 8.196f, -6.928f, {deg2rad(-30.0f), deg2rad(-60.0f)}, tolerance);
+    overallTestResult &= ikTest5Passed;
+
+    // Test Case 6: Target at a point in the second quadrant
+    bool ikTest6Passed = testInverseKinematics(L1, L2, -3.0f, 10.44f, {deg2rad(120.0f), deg2rad(-30.0f)}, tolerance);
+    overallTestResult &= ikTest6Passed;
+
+    // Test Case 7: Target at a point in the third quadrant
+    bool ikTest7Passed = testInverseKinematics(L1, L2, -10.606f, -3.0f, {deg2rad(-135.0f), deg2rad(45.0f)}, tolerance);
+    overallTestResult &= ikTest7Passed;
+
+    // Test Case 8: Target at a point in the fourth quadrant
+    bool ikTest8Passed = testInverseKinematics(L1, L2, 3.0f, -10.44f, {deg2rad(-60.0f), deg2rad(30.0f)}, tolerance);
+    overallTestResult &= ikTest8Passed;
+
+    // Test Case 9: Target at the origin
+    bool ikTest9Passed = testInverseKinematics(L1, L2, 0.0f, 0.0f, {deg2rad(0.0f), deg2rad(180.0f)}, tolerance);
+
+    overallTestResult &= ikTest9Passed;
+
+    std::cout << "\n==== OVERALL TEST RESULTS ====" << std::endl;
+    if (overallTestResult) {
+        std::cout << "ALL TESTS PASSED!" << std::endl;
+    } else {
+        std::cout << "SOME TESTS FAILED!" << std::endl;
+    }
+
+    return 0;
+}
+*/
+
+
 #include <iostream>
 #include "Kinematics.h"
 #define ERROR_THRESHOLD 0.00001
@@ -22,7 +161,6 @@ std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, f
 bool testFK(float q1, float q2, float L1, float L2, std::vector<float> expectedEndEffectorPosition, float errorThreshold)
 {
 	std::vector<float> calculatedEndEffectorPosition = computeForwardKinematics(q1, q2, L1, L2);
-	
 	if (expectedEndEffectorPosition.size() != calculatedEndEffectorPosition.size())
 	{
 		std::cout << "[ERROR] (testFK) expected results and calculated ones have different sizes!" << std::endl;