diff --git a/.DS_Store b/.DS_Store new file mode 100644 index 0000000..b0a8c52 Binary files /dev/null and b/.DS_Store differ diff --git a/bin/cartControl b/bin/cartControl new file mode 100644 index 0000000..945e357 Binary files /dev/null and b/bin/cartControl differ diff --git a/bin/drawImage b/bin/drawImage new file mode 100644 index 0000000..074da77 Binary files /dev/null and b/bin/drawImage differ diff --git a/bin/jointControl b/bin/jointControl new file mode 100644 index 0000000..a51d2f7 Binary files /dev/null and b/bin/jointControl differ diff --git a/bin/linearControl b/bin/linearControl new file mode 100644 index 0000000..1db5285 Binary files /dev/null and b/bin/linearControl differ diff --git a/bin/testKinematics b/bin/testKinematics new file mode 100644 index 0000000..c2ee9e3 Binary files /dev/null and b/bin/testKinematics differ diff --git a/include/Kinematics.h b/include/Kinematics.h index 246781a..04fc5e1 100644 --- a/include/Kinematics.h +++ b/include/Kinematics.h @@ -11,6 +11,10 @@ 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); diff --git a/lib/DynamixelHandler.o b/lib/DynamixelHandler.o new file mode 100644 index 0000000..eb89afc Binary files /dev/null and b/lib/DynamixelHandler.o differ diff --git a/lib/ImageProcessing.o b/lib/ImageProcessing.o new file mode 100644 index 0000000..78a0819 Binary files /dev/null and b/lib/ImageProcessing.o differ diff --git a/lib/Kinematics.o b/lib/Kinematics.o new file mode 100644 index 0000000..921b0f6 Binary files /dev/null and b/lib/Kinematics.o differ diff --git a/lib/cartControl.o b/lib/cartControl.o new file mode 100644 index 0000000..a5665e7 Binary files /dev/null and b/lib/cartControl.o differ diff --git a/lib/drawImage.o b/lib/drawImage.o new file mode 100644 index 0000000..aec7118 Binary files /dev/null and b/lib/drawImage.o differ diff --git a/lib/jointControl.o b/lib/jointControl.o new file mode 100644 index 0000000..0012f71 Binary files /dev/null and b/lib/jointControl.o differ diff --git a/lib/linearControl.o b/lib/linearControl.o new file mode 100644 index 0000000..18b0d97 Binary files /dev/null and b/lib/linearControl.o differ diff --git a/lib/testKinematics.o b/lib/testKinematics.o new file mode 100644 index 0000000..771c756 Binary files /dev/null and b/lib/testKinematics.o differ diff --git a/makefile b/makefile index 3238d1e..f39539c 100644 --- a/makefile +++ b/makefile @@ -1,22 +1,34 @@ -all: kinematics dynamics joint testKinematics +all: 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/testKinematics lib/testKinematics.o lib/Kinematics.o -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4` - -testKinematics: src/testKinematics.cpp - g++ -c src/testKinematics.cpp -o lib/testKinematics.o -I./include -I/usr/include/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` + kinematics: src/Kinematics.cpp g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4 +test: src/testKinematics.cpp + g++ -c src/testKinematics.cpp -o lib/testKinematics.o -I./include -I/usr/include/opencv4 + joint: src/jointControl.cpp g++ -c src/jointControl.cpp -o lib/jointControl.o -I./include -I/usr/include/opencv4 +cartesian: src/cartControl.cpp + g++ -c src/cartControl.cpp -o lib/cartControl.o -I./include -I/usr/include/opencv4 + +linear: src/linearControl.cpp + g++ -c src/linearControl.cpp -o lib/linearControl.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 dynamics: src/DynamixelHandler.cpp g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include - + +draw: src/drawImage.cpp + g++ -c src/drawImage.cpp -o lib/drawImage.o -I./include -I/usr/include/opencv4 + clean: rm lib/*.o - rm bin/* + rm bin/* \ No newline at end of file diff --git a/src/Kinematics.cpp b/src/Kinematics.cpp index 0ef3916..165ca69 100644 --- a/src/Kinematics.cpp +++ b/src/Kinematics.cpp @@ -3,26 +3,115 @@ float deg2rad(float angle) { + //Converts degrees to radians return angle/180.0*M_PI; } float rad2deg(float angle) { + //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 float x = L1 * cos(q1) + L2 * cos(q1+q2); float y = L1 * sin(q1) + L2 * sin(q1+q2); - std::cout << "[INFO] Forward Kinematics : (q1, q2)->(x, y) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ")->(" << x << ", " << y << ")" << std::endl; - std::vector X; + //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 + std::vector X; X.push_back(x); X.push_back(y); return X; } +std::vector computeInverseKinematics(float x, float y, float L1, float L2) +{ + //declares an empty vector qi + 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); + + std::cout << "[INFO] cos_q2= " << cos_q2 << std::endl; + //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 + qi.push_back(0.0); + std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl; + } + //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 + qi.push_back(1.0); + float q1 = atan2(y, x); + //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 + qi.push_back(q1); + qi.push_back(q2); + } + else if (cos_q2 == -1) + { + qi.push_back(1.0); + float q1 = atan2(y, x); + //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 + 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 + qi.push_back(2.0); + std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl; + //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 + qi.push_back(q1); + qi.push_back(q2); + + //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. + qi.push_back(q1); + qi.push_back(q2); + } + + return qi; +} + +std::vector computeDifferentialKinematics(float q1, float q2, float L1, float L2) +{ + //Creates a vector jacobian + std::vector jacobian; + //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 + jacobian.push_back(j11); + jacobian.push_back(j12); + jacobian.push_back(j21); + jacobian.push_back(j22); + + return jacobian; +} + int computeJacobianMatrixRank(std::vector vJacobianMatrix, float threshold) { int rank = -1; @@ -87,4 +176,4 @@ cv::Mat computeInverseJacobianMatrix(std::vector vJacobianMatrix) std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl; return oJacobianInverse; -} \ No newline at end of file +} diff --git a/src/cartControl.cpp b/src/cartControl.cpp new file mode 100644 index 0000000..02962e9 --- /dev/null +++ b/src/cartControl.cpp @@ -0,0 +1,78 @@ +#include +#include + +#include "Kinematics.h" +#include "DynamixelHandler.h" + +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 + initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); + + //Computes IK + std::vector qi = computeInverseKinematics(x, y, L1, L2); + + //Computes FK + computeForwardKinematics(qi[1], qi[2], atof(argv[1]), atof(argv[2])); + //Computes DK + std::vector vJacobianMatrix = computeDifferentialKinematics(qi[1], qi[2], L1, L2); + int rk = computeJacobianMatrixRank(vJacobianMatrix, 0.1); + + //Sends the target joint values received only if there is at least a solution + if (qi.size() >= 3) + { + std::vector vTargetJointPosition; + vTargetJointPosition.push_back(qi[1]); + vTargetJointPosition.push_back(qpen); + vTargetJointPosition.push_back(qi[2]); + _oDxlHandler.sendTargetJointPosition(vTargetJointPosition); + } + + //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 << ">> cartControl L1(cm) L2(cm) x(cm) y(cm)"<< std::endl; + } + + return 0; +} diff --git a/src/drawImage.cpp b/src/drawImage.cpp new file mode 100644 index 0000000..6e28c9d --- /dev/null +++ b/src/drawImage.cpp @@ -0,0 +1,76 @@ +#include +#include + +#include "Kinematics.h" +#include "ImageProcessing.h" + + +// 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 == 4) + { + // Retrieves the args and stores them into variables + float L1 = atof(argv[1]); // in cm + float L2 = atof(argv[2]); // in cm + std::string imgFilename = argv[3]; // image filename + + // Initializes the robot + initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); + + // Loads the image + cv::Mat l_oRawImg = cv::imread(imgFilename); + if(l_oRawImg.empty()) + { + std::cout << "[ERROR] Could not read the image: " << imgFilename << std::endl; + return 1; + } + + // Extracts its contours in the pixel reference frame + std::vector> vContoursInPixel = imageProcessing(l_oRawImg); + + // Converts the contours to the robot reference frame + float theta = -180.0f/180.0f*M_PI; float tx = 110; float ty = 20; int imageWidth = l_oRawImg.cols; int imageHeight = l_oRawImg.rows; + std::vector> vContoursInMm = convertContoursPixel2Mm(vContoursInPixel, theta, tx, ty, imageWidth, imageHeight); + + // Sends the contours to the robot + sendContours(vContoursInPixel, vContoursInMm, _oDxlHandler, L1, L2); + + // Waits 2s + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + + // Closes robot connection + _oDxlHandler.closePort(); + + } + else + { + std::cout << "[WARNING] Draw image" << std::endl; + std::cout << ">> drawImage L1(cm) L2(cm) img_filename"<< std::endl; + } + + return 0; +} \ No newline at end of file diff --git a/src/linearControl.cpp b/src/linearControl.cpp new file mode 100644 index 0000000..b5151b1 --- /dev/null +++ b/src/linearControl.cpp @@ -0,0 +1,141 @@ +#include +#include + +#include "Kinematics.h" +#include "DynamixelHandler.h" + +// TO BE ADJUSTED IF NECESSARY +float _fDistanceThreshold = 0.1f; // in cm +float _pController = 0.1f; +float _fps = 20.0f; // in Hz + +// 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 l_vCurrentJointPosition; + _oDxlHandler.readCurrentJointPosition(l_vCurrentJointPosition); + std::cout << "OK" << std::endl; + // Inits joint velocities + std::vector 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 vEndEffectorPosition = computeForwardKinematics(l_vCurrentJointPosition[0], l_vCurrentJointPosition[2], L1, L2); + // Computes deltaX + std::vector deltaX{x-vEndEffectorPosition[0], y - vEndEffectorPosition[1]}; + // Computes Jacobian matrix + std::vector vJacobianMatrix = computeDifferentialKinematics(l_vCurrentJointPosition[0], l_vCurrentJointPosition[2], L1, L2); + // Computes Inverse of Jacobian matrix + cv::Mat vInverseJacobianMatrix = computeInverseJacobianMatrix(vJacobianMatrix); + // Determines deltaX + cv::Mat1f oDeltaX(2, 1); + oDeltaX.at(0, 0) = _pController * deltaX[0]; + oDeltaX.at(1, 0) = _pController * deltaX[1]; + // Computes deltaQ + cv::Mat deltaQ = vInverseJacobianMatrix * oDeltaX; + // Updates the current joint position + l_vCurrentJointPosition[0] += deltaQ.at(0,0); + l_vCurrentJointPosition[2] += deltaQ.at(1,0); + // Updates the target joint velocity + l_vCurrentJointVelocity[0] = deltaQ.at(0,0)*_fps; + l_vCurrentJointVelocity[2] = deltaQ.at(1,0)*_fps; + // VELOCITY CONTROL qdot + // Creates the target joint position vector + std::vector 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 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 + + // 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; +} diff --git a/src/testKinematics.cpp b/src/testKinematics.cpp index ce3cfa3..1329482 100644 --- a/src/testKinematics.cpp +++ b/src/testKinematics.cpp @@ -5,8 +5,8 @@ #define ERROR_THRESHOLD 0.00001 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); +std::vector computeInverseKinematics(float x, float y, float L1, float L2); +std::vector computeDifferentialKinematics(float q1, float q2, float L1, float L2); bool testFK(float q1, float q2, float L1, float L2, std::vector expectedEndEffectorPosition, float errorThreshold) @@ -31,7 +31,7 @@ bool testFK(float q1, float q2, float L1, float L2, std::vector expectedE else return false; } -/* + bool testIK(float x, float y, float L1, float L2, std::vector expectedJointValues, float errorThreshold) { @@ -55,7 +55,7 @@ bool testIK(float x, float y, float L1, float L2, std::vector expectedJoi else return false; } -*/ + int main() { // Tests Forward Kinematics @@ -83,7 +83,7 @@ int main() std::cout << "PASSED!" << std::endl; else std::cout << "FAILED!" << std::endl; - /* + // Tests Inverse Kinematics std::cout << "====TESTING INVERSE KINEMATICS====" << std::endl; std::cout << "-> Test #1......"<< std::endl; @@ -119,5 +119,4 @@ int main() std::cout << "FAILED!" << std::endl; return 0; - */ -} +} \ No newline at end of file