From 1c820e6665e879429ce59b874bc7426c97fbf8c0 Mon Sep 17 00:00:00 2001 From: CHARLES Date: Sun, 13 Apr 2025 16:54:29 +0200 Subject: [PATCH] Added starting comments for all codes. Updated the README.md. --- .DS_Store | Bin 8196 -> 8196 bytes README.md | 151 ++++++++++++++++++++++++++++++++++- src/Kinematics.cpp | 60 ++++++++------ src/cartControl.cpp | 12 +++ src/drawImage.cpp | 14 +++- src/jointControl.cpp | 14 +++- src/linearControl.cpp | 12 +++ src/linearControlFurther.cpp | 24 ++++-- src/testKinematics.cpp | 16 +++- 9 files changed, 268 insertions(+), 35 deletions(-) diff --git a/.DS_Store b/.DS_Store index e14549c47ec1cae8d94617b3070b25b00ee29dd0..36f88d1a5f9bf4123c64ee00946bf9ca6eac664d 100644 GIT binary patch delta 25 gcmZp1XmQvuMVQ0Pz(_~I(8zG|S^?+H_k~q?0BBhU$p8QV delta 16 XcmZp1XmQvuMR@XjA@9xig;jX~InM?~ diff --git a/README.md b/README.md index 388388a..bda4f1d 100644 --- a/README.md +++ b/README.md @@ -1 +1,150 @@ -Repository of Introduction to Robotics Lab Robotic arm. +# Overview + +Each of the following codes allows a specific directive to be sent to a 2D planar robot: + +**[jointControl](#jointControl)**: Drives the mBot robot to a target location (x, y, theta) using gyroscopic data for precise navigation and orientation tracking. + +**[cartControl](#cartControl)**: Drives the mBot robot to a target location (x, y, theta) using gyroscopic data for precise navigation and orientation tracking. + +**[testKinematics](#testKinematics)**: Retrieves and prints real-time X, Y, and Z angles from the gyro sensor, enabling orientation monitoring. + +**[linearControl](#linearControl)**: Implements basic movement patterns such as moving forward, backward, and turning left or right for foundational motion control. + +**[drawImage](#drawImage)**: Simulates Roomba-like behavior by integrating ultrasonic sensors for obstacle detection and avoidance during autonomous navigation. + +**[linearControlFurther](#linearControlFurther)**: Implements line-following behavior using a line-finder module to detect and respond to black lines on the ground for path tracking. + +# Repository Structure + +### 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 : + - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) + - void closeRobot(DynamixelHandler& dxlHandler) + - int main() + +### cartControl/ + * @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. + * @functions : + * @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) + - void closeRobot(DynamixelHandler& dxlHandler) + - int main(int argc, char** argv) + * Input: + * - L1 : Length of the first link (cm) + * - L2 : Length of the second link (cm) + * - x : x-component of target position (cm) + * - y : y-component of target position (cm) + * + * Output: + * - Displacement of the robot using cartesian coordinates. + + +### testKinematics/ + +* @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. +* @functions : + - 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() + + + ### linearControl/ + * @file linearControl.cpp + * @author Guillaume Gibert + * @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. + * @functions : + * @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) + - void closeRobot(DynamixelHandler& dxlHandler) + - int main(int argc, char** argv) +* Input: + * - L1 : Length of the first link (cm) + * - L2 : Length of the second link (cm) + * - x : x-component of target position (cm) + * - y : y-component of target position (cm) + * + * Output: + * - Displacement of the robot using cartesian coordinates. + +### drawImage/ + + * @file drawImage.cpp + * @author Guillaume Gibert + * @version - + * @date 2025/04/13 + * @brief Description : This file was provided by G. Gibert. It allows to use image processing to draw an image after converting it to correct dimensions. + * @functions : + * @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) + - void closeRobot(DynamixelHandler& dxlHandler) + - int main(int argc, char** argv) + * Input: + * - L1 : Length of the first link (cm) + * - L2 : Length of the second link (cm) + * - image : directory of an image to use for image drawing + +### linearControlFurther/ + + * @file linearControl.cpp + * @author Guillaume Gibert + * @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. + * @functions : + * - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) + - void closeRobot(DynamixelHandler& dxlHandler) + - int main(int argc, char** argv) +* Input: + * - L1 : Length of the first link (cm) + * - L2 : Length of the second link (cm) + * - x : x-component of target position (cm) + * - y : y-component of target position (cm) + +# README.md + +Documentation for the repository. + +# .DS_Store + +System file (can be ignored or removed from the repository). + +# Requirements + +Hardware: mBot robot kit + +Software: Arduino IDE + +Libraries: Ensure required libraries (e.g., MeMCore.h) are installed in Arduino IDE. + +# Installation + +Clone this repository: + +git clone https://gitarero.ecam.fr/charles.stelandre/IntroRoboticsLab2.git + +Open Arduino IDE and navigate to the folder containing the .ino file you want to use. + +Install any necessary libraries via Arduino Library Manager. + +Upload the code to your mBot using a USB connection. + +# Usage + +Navigate to the folder corresponding to the desired functionality (e.g., gyro/). + +Open the .ino file in Arduino IDE. + +Follow any instructions provided in the comments within the code to execute experiments. diff --git a/src/Kinematics.cpp b/src/Kinematics.cpp index 165ca69..cbe43fe 100644 --- a/src/Kinematics.cpp +++ b/src/Kinematics.cpp @@ -1,6 +1,20 @@ +/* + @file Kinematics.cpp + @author Charles STELANDRE + @version V1 + @date 2025/04/13 + @brief Description : This file contains all kinematics computations for FK, IK and DK. + @functions : + @ - float deg2rad(float angle) + - float rad2deg(float angle) + - std::vector computeForwardKinematics(float q1, float q2, float L1, float L2) + - std::vector computeInverseKinematics(float x, float y, float L1, float L2) + - std::vector computeDifferentialKinematics(float q1, float q2, float L1, float L2) + - int computeJacobianMatrixRank(std::vector vJacobianMatrix, float threshold) + - cv::Mat computeInverseJacobianMatrix(std::vector vJacobianMatrix) + */ #include "Kinematics.h" - float deg2rad(float angle) { //Converts degrees to radians @@ -9,18 +23,18 @@ float deg2rad(float angle) float rad2deg(float angle) { - //Converts radians to degrees + // Converts radians to degrees return angle*180.0/M_PI; } std::vector computeForwardKinematics(float q1, float q2, float L1, float L2) { - //Computes the x,y coordinates + // Computes the x,y coordinates 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 + // 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 + // Declare a vector X containing target x,y std::vector X; X.push_back(x); X.push_back(y); @@ -30,30 +44,30 @@ std::vector computeForwardKinematics(float q1, float q2, float L1, float std::vector computeInverseKinematics(float x, float y, float L1, float L2) { - //declares an empty vector qi + // Declares an empty vector qi std::vector qi; - //From the equations, computes the cosine(q2) to be used to find the number of solutions + // 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) + // 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 + // 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 + // 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 + // 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 + // 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 + // Adds q1 and q2 to qi qi.push_back(q1); qi.push_back(q2); } @@ -61,32 +75,32 @@ std::vector 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 + // 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 + // 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 + // 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 + // 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 + // Adds q1 and q2 to qi qi.push_back(q1); qi.push_back(q2); - //Compute the second solution + // 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. + // Adds the second solution at the end of vector qi. qi.push_back(q1); qi.push_back(q2); } @@ -96,14 +110,14 @@ std::vector computeInverseKinematics(float x, float y, float L1, float L2 std::vector computeDifferentialKinematics(float q1, float q2, float L1, float L2) { - //Creates a vector jacobian + // Creates a vector jacobian std::vector jacobian; - //Computes the jacobian's coefficients + // 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 + // Adds each coefficient to the jacobian vector jacobian.push_back(j11); jacobian.push_back(j12); jacobian.push_back(j21); diff --git a/src/cartControl.cpp b/src/cartControl.cpp index 02962e9..ea7e321 100644 --- a/src/cartControl.cpp +++ b/src/cartControl.cpp @@ -1,3 +1,15 @@ +/* + @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. + @functions : + @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) + - void closeRobot(DynamixelHandler& dxlHandler) + - int main(int argc, char** argv) + */ + #include #include diff --git a/src/drawImage.cpp b/src/drawImage.cpp index 6e28c9d..4db2fcb 100644 --- a/src/drawImage.cpp +++ b/src/drawImage.cpp @@ -1,3 +1,15 @@ +/* + @file drawImage.cpp + @author Guillaume Gibert + @version - + @date 2025/04/13 + @brief Description : This file was provided by G. Gibert. It allows to use image processing to draw an image after converting it to correct dimensions. + @functions : + @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) + - void closeRobot(DynamixelHandler& dxlHandler) + - int main(int argc, char** argv) + */ + #include #include @@ -73,4 +85,4 @@ int main(int argc, char** argv) } return 0; -} \ No newline at end of file +} diff --git a/src/jointControl.cpp b/src/jointControl.cpp index 1a00795..3fdb861 100644 --- a/src/jointControl.cpp +++ b/src/jointControl.cpp @@ -1,3 +1,15 @@ +/* + @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() + */ + #include #include @@ -71,4 +83,4 @@ int main(int argc, char** argv) } return 0; -} \ No newline at end of file +} diff --git a/src/linearControl.cpp b/src/linearControl.cpp index b5151b1..4c2c27a 100644 --- a/src/linearControl.cpp +++ b/src/linearControl.cpp @@ -1,3 +1,15 @@ +/* + @file linearControl.cpp + @author Guillaume Gibert + @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. + @functions : + @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) + - void closeRobot(DynamixelHandler& dxlHandler) + - int main(int argc, char** argv) + */ + #include #include diff --git a/src/linearControlFurther.cpp b/src/linearControlFurther.cpp index 3e54ca4..8e69094 100644 --- a/src/linearControlFurther.cpp +++ b/src/linearControlFurther.cpp @@ -1,14 +1,27 @@ +/* + @file linearControl.cpp + @author Guillaume Gibert, Charles Stelandre + @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 the target point. + @functions : + @ - void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) + - void closeRobot(DynamixelHandler& dxlHandler) + - int main(int argc, char** argv) + */ + #include #include #include "Kinematics.h" #include "DynamixelHandler.h" -// TO BE ADJUSTED IF NECESSARY +// TO BE ADJUSTED FOR TESTINGS float _fDistanceThreshold = 0.1f; // in cm float _pController = 0.1f; float _fps = 20.0f; // in Hz float det_threshold = 1e-6; + // Dynamixel config infos DynamixelHandler _oDxlHandler; std::string _robotDxlPortName = "/dev/ttyUSB0"; @@ -73,9 +86,11 @@ int main(int argc, char** argv) // Computes Jacobian matrix std::vector vJacobianMatrix = computeDifferentialKinematics(l_vCurrentJointPosition[0], l_vCurrentJointPosition[2], L1, L2); + // Computes the determinant of the Jacobian float det = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]); cv::Mat vInverseJacobianMatrix; - if (det < det_threshold){ + // Checking if the determinant is within valid range. + if (det < det_threshold){ // Prints an error if close to a singularity. std::cerr << "[WARNING] Jacobian determinant is close to 0 !(" << det << "(, potential singularity." << std::endl; // If there is a singularity, return identity matrix (do not move) @@ -110,10 +125,7 @@ int main(int argc, char** argv) std::vector vTargetJointVelocity; vTargetJointVelocity.push_back(l_vCurrentJointVelocity[0]); vTargetJointVelocity.push_back(0.0f); // pen lift up/down - vTargetJointVelocity.push_back(l_vCurrentJointVelocity[2]); - // Sends the target joint position to actuate the robot - //_oDxlHandler.sendTargetJointVelocity(vTargetJointVelocity); - // VELOCITY CONTROL qdot + vTargetJointVelocity.push_back(l_vCurrentJointVelocity[2]); // Waits 1000/fps ms std::this_thread::sleep_for(std::chrono::milliseconds((long int)(1000.0f/_fps))); diff --git a/src/testKinematics.cpp b/src/testKinematics.cpp index 1329482..18226dc 100644 --- a/src/testKinematics.cpp +++ b/src/testKinematics.cpp @@ -1,7 +1,17 @@ +/* + @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. + @functions : + - 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() + */ + #include - #include "Kinematics.h" - #define ERROR_THRESHOLD 0.00001 std::vector computeForwardKinematics(float q1, float q2, float L1, float L2); @@ -119,4 +129,4 @@ int main() std::cout << "FAILED!" << std::endl; return 0; -} \ No newline at end of file +}