charles.stelandre
/
IntroRoboticsLab1
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Corrected README. Added test kinematics personal trial in comments. Corrected Kinematics syntax.

This commit is contained in:
Charles STELANDRE 2025-04-13 21:42:35 +02:00
parent e4cb5307d4
commit 95e18735bc
4 changed files with 161 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
README.md
View File

@ -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<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/
@ -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

14
src/Kinematics.cpp
View File

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

2
src/cartControl.cpp
View File

@ -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.

142
src/testKinematics.cpp
View File

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