diff --git a/README.md b/README.md index c786bf1..e4a31d7 100644 --- a/README.md +++ b/README.md @@ -15,6 +15,7 @@ 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/ @@ -36,10 +37,11 @@ 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. : ```./jointControl 5 6 0 0 -90 ### cartControl/ * @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. @@ -55,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 +* @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. @@ -68,6 +71,8 @@ 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 expectedEndEffectorPosition, float errorThreshold) - bool testIK(float x, float y, float L1, float L2, std::vector expectedJointValues, float errorThreshold) - int main() + ## Ex. : ```./testKinematics + ### linearControl/ @@ -88,6 +93,8 @@ 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/ @@ -104,6 +111,8 @@ 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/ @@ -121,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. : ```./linearControl 5 6 8 3 + # README.md diff --git a/src/Kinematics.cpp b/src/Kinematics.cpp index cbe43fe..d55bd5e 100644 --- a/src/Kinematics.cpp +++ b/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 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 computeInverseKinematics(float x, float y, float L1, float L2 std::vector 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 computeDifferentialKinematics(float q1, float q2, float L1, f // Creates a vector jacobian std::vector 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); diff --git a/src/cartControl.cpp b/src/cartControl.cpp index ea7e321..626f1c0 100644 --- a/src/cartControl.cpp +++ b/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. diff --git a/src/testKinematics.cpp b/src/testKinematics.cpp index 18226dc..b9cedba 100644 --- a/src/testKinematics.cpp +++ b/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 computeForwardKinematics(float L1, float L2, float q1, float q2); +std::vector computeInverseKinematics(float L1, float L2, float q1, float q2); +std::vector 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& expected, const std::vector& 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 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 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 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 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 #include "Kinematics.h" #define ERROR_THRESHOLD 0.00001 @@ -22,7 +161,6 @@ std::vector computeDifferentialKinematics(float q1, float q2, float L1, f bool testFK(float q1, float q2, float L1, float L2, std::vector expectedEndEffectorPosition, float errorThreshold) { std::vector 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;