From ae7a839e9fe085cb9fd88da55049f068e92725be Mon Sep 17 00:00:00 2001 From: CHARLES Date: Sun, 13 Apr 2025 13:22:45 +0200 Subject: [PATCH] Updated each code. Added linearControl and imageDraw as provided resources. --- src/Kinematics.cpp | 36 +++++++++++++++++++++++++++--------- src/cartControl.cpp | 20 ++++++++++---------- src/linearControl.cpp | 2 +- 3 files changed, 38 insertions(+), 20 deletions(-) diff --git a/src/Kinematics.cpp b/src/Kinematics.cpp index db043c4..165ca69 100644 --- a/src/Kinematics.cpp +++ b/src/Kinematics.cpp @@ -3,20 +3,25 @@ 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); @@ -25,23 +30,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 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); } @@ -49,27 +61,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 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); } @@ -79,13 +96,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 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); @@ -158,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 index 26b9c29..02962e9 100644 --- a/src/cartControl.cpp +++ b/src/cartControl.cpp @@ -32,26 +32,26 @@ int main(int argc, char** argv) { if (argc == 5) { - // Retrieves the args and stores them into variables + //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 + //Initializes the robot initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); - // Computes IK + //Computes IK std::vector qi = computeInverseKinematics(x, y, L1, L2); - // Computes FK + //Computes FK computeForwardKinematics(qi[1], qi[2], atof(argv[1]), atof(argv[2])); - // Computes Differential Kinematics + //Computes DK std::vector vJacobianMatrix = computeDifferentialKinematics(qi[1], qi[2], L1, L2); - int rank = computeJacobianMatrixRank(vJacobianMatrix, 0.1); + int rk = computeJacobianMatrixRank(vJacobianMatrix, 0.1); - // Sends the target joint values received only if there is at least a solution + //Sends the target joint values received only if there is at least a solution if (qi.size() >= 3) { std::vector vTargetJointPosition; @@ -61,10 +61,10 @@ int main(int argc, char** argv) _oDxlHandler.sendTargetJointPosition(vTargetJointPosition); } - // Waits 2s + //Waits 2s std::this_thread::sleep_for(std::chrono::milliseconds(2000)); - // Closes robot connection + //Closes robot connection _oDxlHandler.closePort(); } @@ -75,4 +75,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 1acd15b..b5151b1 100644 --- a/src/linearControl.cpp +++ b/src/linearControl.cpp @@ -83,7 +83,7 @@ int main(int argc, char** argv) // Updates the current joint position l_vCurrentJointPosition[0] += deltaQ.at(0,0); l_vCurrentJointPosition[2] += deltaQ.at(1,0); - // Uodates the target joint velocity + // Updates the target joint velocity l_vCurrentJointVelocity[0] = deltaQ.at(0,0)*_fps; l_vCurrentJointVelocity[2] = deltaQ.at(1,0)*_fps; // VELOCITY CONTROL qdot