Merge branch 'develop'

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 robot using joint values.
+
+**[cartControl](#cartControl)**: Drives the robot using cartesian values.
+
+**[testKinematics](#testKinematics)**: Tests each kinematic algorithm and makes sure they are at the expected value.
+
+**[linearControl](#linearControl)**: Drives the robot using cartesian values in a linear trajectory.
+
+**[drawImage](#drawImage)**: Draws an input image.
+
+**[linearControlFurther](#linearControlFurther)**: Improvement of the linear control which checks for the determinant of the Jacobian to be valid.
+
+# 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<float> expectedEndEffectorPosition, float errorThreshold)
+        - bool testIK(float x, float y, float L1, float L2, std::vector<float> 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.
+
+# Requirements
+
+Hardware: Ergo Poppy Jr.
+
+Software: C++ Programming
+
+Libraries: Ensure required libraries (Dynamixel SDK - DynamixelHandler.h)
+
+# Installation
+
+Clone this repository:
+
+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.
+
+Upload the code to your mBot using a USB connection.
+
+# Usage
+
+Navigate to your installation directory.
+
+Make the file using ``` make ```
+
+Navigate to the bin folder.
+
+Execute the desired file in the command line/
+
+Follow any instructions provided in the comments within the code to execute experiments.

makefile

@@ -1,9 +1,10 @@
-all: kinematics dynamics joint cartesian image test linear draw
+all: linearFurther kinematics dynamics joint cartesian image test linear draw
 	g++ -o bin/jointControl lib/jointControl.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	g++ -o bin/cartControl lib/cartControl.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	g++ -o bin/testKinematics lib/testKinematics.o lib/Kinematics.o `pkg-config --libs opencv4`
 	g++ -o bin/linearControl lib/linearControl.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	g++ -o bin/drawImage lib/drawImage.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
+	g++ -o bin/linearControlFurther lib/linearControlFurther.o lib/Kinematics.o lib/DynamixelHandler.o lib/ImageProcessing.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
 	
 kinematics: src/Kinematics.cpp
 	g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4
@@ -20,6 +21,9 @@ cartesian: src/cartControl.cpp
 linear: src/linearControl.cpp
 	g++ -c src/linearControl.cpp -o lib/linearControl.o -I./include -I/usr/include/opencv4
 
+linearFurther: src/linearControlFurther.cpp
+	g++ -c src/linearControlFurther.cpp -o lib/linearControlFurther.o -I./include -I/usr/include/opencv4
+	
 image: src/ImageProcessing.cpp
 	g++ -c src/ImageProcessing.cpp -o lib/ImageProcessing.o -I./include -I/usr/include/opencv4
 	

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<float> computeForwardKinematics(float q1, float q2, float L1, float L2)
+        - std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
+        - std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
+        - int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
+        - cv::Mat  computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
+ */
 #include "Kinematics.h"
 
-
 float deg2rad(float angle)
 {
     //Converts degrees to radians
@@ -30,7 +44,7 @@ std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float
 
 std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
 {
-    //declares an empty vector qi
+    // Declares an empty vector qi
 	std::vector<float> qi;
 	
     // From the equations, computes the cosine(q2) to be used to find the number of solutions

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 <chrono>
 #include <thread>
 

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 <chrono>
 #include <thread>
 

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 <chrono>
 #include <thread>
 

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 <chrono>
 #include <thread>
 

src/linearControlFurther.cpp

@@ -0,0 +1,163 @@
+/*
+ @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 <chrono>
+#include <thread>
+
+#include "Kinematics.h"
+#include "DynamixelHandler.h"
+
+// 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";
+float _robotDxlProtocol = 2.0;
+int _robotDxlBaudRate = 1000000;
+
+
+void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
+{
+	std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
+	dxlHandler.setDeviceName(portName);
+	dxlHandler.setProtocolVersion(protocol);
+	dxlHandler.openPort();
+	dxlHandler.setBaudRate(baudRate);
+	dxlHandler.enableTorque(true);
+	std::cout << std::endl;
+}
+
+void closeRobot(DynamixelHandler& dxlHandler)
+{
+	dxlHandler.enableTorque(false);
+	dxlHandler.closePort();
+}
+
+int main(int argc, char** argv)
+{
+	if (argc == 5)
+	{
+		// Retrieves the args and stores them into variables
+		float L1 = atof(argv[1]); // in cm
+		float L2 = atof(argv[2]); // in cm
+		float x = atof(argv[3]); // in cm
+		float y = atof(argv[4]); // in cm
+		float qpen = deg2rad(-90); // in rad
+			
+		// Initializes the robot
+		std::cout << "init" ;
+		initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
+		std::cout << "OK" << std::endl;
+		// Changes the control mode
+		//_oDxlHandler.setControlMode(1); //wheel mode
+		// VELOCITY CONTROL qdot
+		
+		// Gets the initial joint positions
+		std::cout << "readcurrent" ;
+		std::vector<float> l_vCurrentJointPosition;
+		_oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition);
+		std::cout << "OK" << std::endl;
+		// Inits joint velocities
+		std::vector<float> l_vCurrentJointVelocity; l_vCurrentJointVelocity.push_back(0.0f); l_vCurrentJointVelocity.push_back(0.0f); l_vCurrentJointVelocity.push_back(0.0f);
+		// VELOCITY CONTROL qdot
+		
+		// Inits the loop
+		bool bIsFarFromTarget = true;
+				
+		while (bIsFarFromTarget)
+		{
+			// Estimates the position of the end-effector using FK
+			std::vector<float> vEndEffectorPosition = computeForwardKinematics(l_vCurrentJointPosition[0], l_vCurrentJointPosition[2], L1, L2);
+			// Computes deltaX
+			std::vector<float> deltaX{x-vEndEffectorPosition[0], y - vEndEffectorPosition[1]};
+			// Computes Jacobian matrix
+			std::vector<float> 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;
+			// 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)
+				cv::Mat vInverseJacobianMatrix = cv::Mat::eye(2,2,CV_32F);
+			} else {
+				// Computes Inverse of Jacobian matrix
+				cv::Mat vInverseJacobianMatrix = computeInverseJacobianMatrix(vJacobianMatrix);
+			}
+			// Determines deltaX
+			cv::Mat1f oDeltaX(2, 1);
+			oDeltaX.at<float>(0, 0) = _pController * deltaX[0];
+			oDeltaX.at<float>(1, 0) = _pController * deltaX[1];
+			// Computes deltaQ
+			cv::Mat deltaQ = vInverseJacobianMatrix * oDeltaX;
+			// Updates the current joint position
+			l_vCurrentJointPosition[0] += deltaQ.at<float>(0,0);
+			l_vCurrentJointPosition[2] += deltaQ.at<float>(1,0);
+			// Updates the target joint velocity
+			l_vCurrentJointVelocity[0] = deltaQ.at<float>(0,0)*_fps;
+			l_vCurrentJointVelocity[2] = deltaQ.at<float>(1,0)*_fps;
+			// VELOCITY CONTROL qdot
+			// Creates the target joint position vector
+			std::vector<float> vTargetJointPosition;
+			if(l_vCurrentJointPosition.size()>=3){
+				vTargetJointPosition.push_back(l_vCurrentJointPosition[0]); 
+				vTargetJointPosition.push_back(qpen); 
+				vTargetJointPosition.push_back(l_vCurrentJointPosition[2]); 
+			// Sends the target joint position to actuate the robot
+			_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
+			}
+			// Creates the target joint velocity vector
+			std::vector<float> vTargetJointVelocity;
+				vTargetJointVelocity.push_back(l_vCurrentJointVelocity[0]); 
+				vTargetJointVelocity.push_back(0.0f); // pen lift up/down
+				vTargetJointVelocity.push_back(l_vCurrentJointVelocity[2]);
+			
+			// Waits 1000/fps ms
+			std::this_thread::sleep_for(std::chrono::milliseconds((long int)(1000.0f/_fps)));
+			
+			// Estimates the distance to the target
+			float currentDistanceToTarget = sqrt(pow(y - vEndEffectorPosition[1],2) + pow(x - vEndEffectorPosition[0], 2));
+			std::cout << "====LINEAR CONTROL====" << std::endl;
+			std::cout << "Current Joint Position in deg (q1, q2)= (" << rad2deg(l_vCurrentJointPosition[0]) << ", " << rad2deg(l_vCurrentJointPosition[2]) << ")" << std::endl;
+			std::cout << "Current Joint Velocity in deg/s (q1, q2)= (" << rad2deg(l_vCurrentJointVelocity[0]) << ", " << rad2deg(l_vCurrentJointVelocity[2]) << ")" << std::endl;
+			std::cout << "Current Cartesian Position (x, y)= (" << vEndEffectorPosition[0] << ", " << vEndEffectorPosition[0] << ")" << std::endl;
+			std::cout << "Current Distance To Target= " << currentDistanceToTarget << std::endl;
+			std::cout << "==================" << std::endl;
+
+			// Determines if the loop should continue
+			if (currentDistanceToTarget < _fDistanceThreshold)
+			{
+				bIsFarFromTarget = false;
+			}
+		}
+		
+		// Waits 2s
+		std::this_thread::sleep_for(std::chrono::milliseconds(2000));
+
+		// Closes robot connection
+		_oDxlHandler.closePort();
+		
+	}
+	else
+	{
+		std::cout << "[WARNING] Cartesian control" << std::endl;
+		std::cout << ">> linearControl L1(cm) L2(cm) x(cm) y(cm)"<< std::endl;
+	}
+		
+	return 0;
+}

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<float> expectedEndEffectorPosition, float errorThreshold)
+        - bool testIK(float x, float y, float L1, float L2, std::vector<float> expectedJointValues, float errorThreshold)
+        - int main()
+ */
+
 #include <iostream>
-
 #include "Kinematics.h"
-
 #define ERROR_THRESHOLD 0.00001
 
 std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2);