Compare commits
No commits in common. "main" and "develop" have entirely different histories.
59
README.md
59
README.md
|
|
@ -15,33 +15,22 @@ 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
|
||||
* @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.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 :
|
||||
- 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
|
||||
* @author Guillaume Gibert, Charles STELANDRE
|
||||
* @file cartControl.cpp
|
||||
* @author Charles STELANDRE
|
||||
* @version V1
|
||||
* @date 2025/04/13
|
||||
* @brief Description : This file is a variant from jointControl, adapting cartesian Coordinates translation.
|
||||
|
|
@ -57,13 +46,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. : ```./cartControl 5 6 8 3```
|
||||
|
||||
|
||||
### testKinematics/
|
||||
|
||||
* @file testKinematics
|
||||
* @author Guillaume Gibert, Charles Stelandre
|
||||
* @file testKinematics.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.
|
||||
|
|
@ -71,12 +59,10 @@ 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
|
||||
* @file linearControl.cpp
|
||||
* @author Guillaume Gibert
|
||||
* @version -
|
||||
* @date 2025/04/13
|
||||
|
|
@ -93,12 +79,10 @@ 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
|
||||
* @file drawImage.cpp
|
||||
* @author Guillaume Gibert
|
||||
* @version -
|
||||
* @date 2025/04/13
|
||||
|
|
@ -111,18 +95,16 @@ 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
|
||||
* @author Guillaume Gibert, Charles Stelandre
|
||||
* @file linearControl.cpp
|
||||
* @author Guillaume Gibert
|
||||
* @version -
|
||||
* @date 2025/04/13
|
||||
* @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.
|
||||
* @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.
|
||||
* @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:
|
||||
|
|
@ -130,8 +112,6 @@ 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
|
||||
|
||||
|
|
@ -139,7 +119,7 @@ Documentation for the repository.
|
|||
|
||||
# Requirements
|
||||
|
||||
Hardware: Poppy Ergo Jr.
|
||||
Hardware: Ergo Poppy Jr.
|
||||
|
||||
Software: C++ Programming
|
||||
|
||||
|
|
@ -153,16 +133,15 @@ 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 library.
|
||||
Install any necessary libraries via Arduino Library Manager.
|
||||
|
||||
Upload the code to your robot using a USB connection.
|
||||
Upload the code to your mBot 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.
|
||||
|
||||
|
|
|
|||
|
|
@ -1,9 +1,9 @@
|
|||
/*
|
||||
@file Kinematics.cpp
|
||||
@author Guillaume Gibert, Charles STELANDRE
|
||||
@author 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 = -L1*sin(q1) - L2*sin(q1+q2);
|
||||
float j11 = -L2*sin(q1+q2) - L1*sin(q1);
|
||||
float j12 = -L2*sin(q1+q2);
|
||||
float j21 = L1*cos(q1) + L2*cos(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);
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/*
|
||||
@file cartControl.cpp
|
||||
@author Guillaume Gibert, Charles STELANDRE
|
||||
@author Charles STELANDRE
|
||||
@version V1
|
||||
@date 2025/04/13
|
||||
@brief Description : This file is a variant from jointControl, adapting cartesian Coordinates translation.
|
||||
|
|
|
|||
|
|
@ -156,7 +156,7 @@ int main(int argc, char** argv)
|
|||
else
|
||||
{
|
||||
std::cout << "[WARNING] Cartesian control" << std::endl;
|
||||
std::cout << ">> linearControlFurther L1(cm) L2(cm) x(cm) y(cm)"<< std::endl;
|
||||
std::cout << ">> linearControl L1(cm) L2(cm) x(cm) y(cm)"<< std::endl;
|
||||
}
|
||||
|
||||
return 0;
|
||||
|
|
|
|||
|
|
@ -1,6 +1,6 @@
|
|||
/*
|
||||
@file testKinematics.cpp
|
||||
@author Guillaume Gibert, Charles Stelandre
|
||||
@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.
|
||||
|
|
@ -10,145 +10,6 @@
|
|||
- 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
|
||||
|
|
@ -161,6 +22,7 @@ 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;
|
||||
|
|
|
|||
Loading…
Reference in New Issue