diff --git a/.DS_Store b/.DS_Store index b0a8c52..e14549c 100644 Binary files a/.DS_Store and b/.DS_Store differ diff --git a/bin/cartControl b/bin/cartControl deleted file mode 100644 index 945e357..0000000 Binary files a/bin/cartControl and /dev/null differ diff --git a/bin/drawImage b/bin/drawImage deleted file mode 100644 index 074da77..0000000 Binary files a/bin/drawImage and /dev/null differ diff --git a/bin/jointControl b/bin/jointControl deleted file mode 100644 index a51d2f7..0000000 Binary files a/bin/jointControl and /dev/null differ diff --git a/bin/linearControl b/bin/linearControl deleted file mode 100644 index 1db5285..0000000 Binary files a/bin/linearControl and /dev/null differ diff --git a/bin/testKinematics b/bin/testKinematics deleted file mode 100644 index c2ee9e3..0000000 Binary files a/bin/testKinematics and /dev/null differ diff --git a/lib/DynamixelHandler.o b/lib/DynamixelHandler.o deleted file mode 100644 index eb89afc..0000000 Binary files a/lib/DynamixelHandler.o and /dev/null differ diff --git a/lib/ImageProcessing.o b/lib/ImageProcessing.o deleted file mode 100644 index 78a0819..0000000 Binary files a/lib/ImageProcessing.o and /dev/null differ diff --git a/lib/Kinematics.o b/lib/Kinematics.o deleted file mode 100644 index 921b0f6..0000000 Binary files a/lib/Kinematics.o and /dev/null differ diff --git a/lib/cartControl.o b/lib/cartControl.o deleted file mode 100644 index a5665e7..0000000 Binary files a/lib/cartControl.o and /dev/null differ diff --git a/lib/drawImage.o b/lib/drawImage.o deleted file mode 100644 index aec7118..0000000 Binary files a/lib/drawImage.o and /dev/null differ diff --git a/lib/jointControl.o b/lib/jointControl.o deleted file mode 100644 index 0012f71..0000000 Binary files a/lib/jointControl.o and /dev/null differ diff --git a/lib/linearControl.o b/lib/linearControl.o deleted file mode 100644 index 18b0d97..0000000 Binary files a/lib/linearControl.o and /dev/null differ diff --git a/lib/testKinematics.o b/lib/testKinematics.o deleted file mode 100644 index 771c756..0000000 Binary files a/lib/testKinematics.o and /dev/null differ diff --git a/makefile b/makefile index f39539c..f6aa804 100644 --- a/makefile +++ b/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 @@ -19,6 +20,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 @@ -31,4 +35,4 @@ draw: src/drawImage.cpp clean: rm lib/*.o - rm bin/* \ No newline at end of file + rm bin/* diff --git a/src/.DS_Store b/src/.DS_Store new file mode 100644 index 0000000..5008ddf Binary files /dev/null and b/src/.DS_Store differ diff --git a/src/linearControlFurther.cpp b/src/linearControlFurther.cpp index 82a8d4b..3e54ca4 100644 --- a/src/linearControlFurther.cpp +++ b/src/linearControlFurther.cpp @@ -8,7 +8,7 @@ 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"; @@ -48,6 +48,9 @@ int main(int argc, char** argv) 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" ; @@ -56,6 +59,7 @@ int main(int argc, char** argv) 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; @@ -68,8 +72,18 @@ int main(int argc, char** argv) 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); + + float det = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]); + cv::Mat vInverseJacobianMatrix; + 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(0, 0) = _pController * deltaX[0]; @@ -79,27 +93,6 @@ 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); - - float q1_rad = l_vCurrentJointPosition[0]; - float q2_rad = l_vCurrentJointPosition[2]; - float q_limit_rad = deg2rad(180.0f); - - if (q1_rad > q_limit_rad) { - l_vCurrentJointPosition[0] = q_limit_rad; - std::cerr << "[WARNING] Joint 1 angle exceeded upper limit, clamping to " << rad2deg(q_limit_rad) << " degrees." << std::endl; - } else if (q1_rad < -q_limit_rad) { - l_vCurrentJointPosition[0] = -q_limit_rad; - std::cerr << "[WARNING] Joint 1 angle exceeded lower limit, clamping to " << rad2deg(-q_limit_rad) << " degrees." << std::endl; - } - - if (q2_rad > q_limit_rad) { - l_vCurrentJointPosition[2] = q_limit_rad; - std::cerr << "[WARNING] Joint 2 angle exceeded upper limit, clamping to " << rad2deg(q_limit_rad) << " degrees." << std::endl; - } else if (q2_rad < -q_limit_rad) { - l_vCurrentJointPosition[2] = -q_limit_rad; - std::cerr << "[WARNING] Joint 2 angle exceeded lower limit, clamping to " << rad2deg(-q_limit_rad) << " degrees." << std::endl; - } - // Updates the target joint velocity l_vCurrentJointVelocity[0] = deltaQ.at(0,0)*_fps; l_vCurrentJointVelocity[2] = deltaQ.at(1,0)*_fps;