alperen.isik
/
It_Robotics_Pong_Robot
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

first lab

This commit is contained in:
Alexandre LETOURNEUX 2025-04-02 11:53:16 +02:00
parent 94da1729a2
commit 4df1688a3f
6 changed files with 554 additions and 577 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

8
include/DynamixelHandler.h
View File

@ -31,10 +31,12 @@
// rotation direction // rotation direction
#define ROT_DIRECTION_Q1 1 #define ROT_DIRECTION_Q1 1
#define ROT_DIRECTION_Q2 -1 #define ROT_DIRECTION_Q2 1
#define ROT_DIRECTION_QPEN 1 #define ROT_DIRECTION_Q3 1
#define ROT_DIRECTION_Q4 1
// nb of joints // nb of joints
#define NB_JOINTS 3 #define NB_JOINTS 4
class DynamixelHandler class DynamixelHandler

11
include/Kinematics.h
View File

@ -5,16 +5,17 @@
#include "opencv2/opencv.hpp" #include "opencv2/opencv.hpp"
// Derece <-> Radyan dönüşüm fonksiyonları
float deg2rad(float angle); float deg2rad(float angle);
float rad2deg(float angle); float rad2deg(float angle);
std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2); // Yeni 4-eklemli robot için ileri kinematik (Forward Kinematics)
std::vector<float> computeForwardKinematics(float q1, float q2, float q3, float q4);
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2); // Yeni 4-eklemli robot için ters kinematik (Inverse Kinematics)
std::vector<float> computeInverseKinematics(float x, float y);
// Jacobian matris fonksiyonları (isteğe bağlı olarak genişletilebilir)
std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2); std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2);
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold); int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix); cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);

33
src/DynamixelHandler.cpp
View File

@ -181,16 +181,19 @@ bool DynamixelHandler::readCurrentJointPosition(std::vector<float>& vCurrentJoin
std::vector<uint16_t> l_vCurrentJointPosition; std::vector<uint16_t> l_vCurrentJointPosition;
// Reads the current joint positions in motor command unit // Reads the current joint positions in motor command unit
bool bIsReadSuccessfull = this->readCurrentJointPosition(l_vCurrentJointPosition); bool bIsReadSuccessfull = this->readCurrentJointPosition(l_vCurrentJointPosition);
//std::cout << "l_vCurrentJointPosition= " << l_vCurrentJointPosition[0] << ", " << l_vCurrentJointPosition[1] << ", " << l_vCurrentJointPosition[2]<< std::endl; //std::cout << "l_vCurrentJointPosition= " << l_vCurrentJointPosition[0] << ", " << l_vCurrentJointPosition[1] << ", " << l_vCurrentJointPosition[2] << ", " << l_vCurrentJointPosition[3]<< std::endl;
// q1 // q1
vCurrentJointPosition.push_back(ROT_DIRECTION_Q1*convertJointCmdToAngle(l_vCurrentJointPosition[0])); vCurrentJointPosition.push_back(ROT_DIRECTION_Q1*convertJointCmdToAngle(l_vCurrentJointPosition[0]));
// qpen
vCurrentJointPosition.push_back(ROT_DIRECTION_QPEN*convertJointCmdToAngle(l_vCurrentJointPosition[1]));
// q2 // q2
vCurrentJointPosition.push_back(ROT_DIRECTION_Q2*convertJointCmdToAngle(l_vCurrentJointPosition[2])); vCurrentJointPosition.push_back(ROT_DIRECTION_Q2*convertJointCmdToAngle(l_vCurrentJointPosition[1]));
// q3
vCurrentJointPosition.push_back(ROT_DIRECTION_Q3*convertJointCmdToAngle(l_vCurrentJointPosition[2]));
// q4
vCurrentJointPosition.push_back(ROT_DIRECTION_Q4*convertJointCmdToAngle(l_vCurrentJointPosition[3]));
//std::cout << "vCurrentJointPosition= " << vCurrentJointPosition[0] << ", " << vCurrentJointPosition[1] << ", " << vCurrentJointPosition[2]<< std::endl;
//std::cout << "vCurrentJointPosition= " << vCurrentJointPosition[0] << ", " << vCurrentJointPosition[1] << ", " << vCurrentJointPosition[2] << ", " << vCurrentJointPosition[3]<< std::endl;
return bIsReadSuccessfull; return bIsReadSuccessfull;
} }
@ -239,12 +242,15 @@ bool DynamixelHandler::sendTargetJointPosition(std::vector<float>& vTargetJointP
std::vector<uint16_t> l_vTargetJointPosition; std::vector<uint16_t> l_vTargetJointPosition;
// q1 // q1
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q1*vTargetJointPosition[0])); l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q1*vTargetJointPosition[0]));
// qpen
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_QPEN*vTargetJointPosition[1]));
// q2 // q2
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q2*vTargetJointPosition[2])); l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q2*vTargetJointPosition[1]));
// q3
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q3*vTargetJointPosition[2]));
// q4
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q4*vTargetJointPosition[3]));
//std::cout << "l_vTargetJointPosition= " << l_vTargetJointPosition[0] << ", " << l_vTargetJointPosition[1] << ", " << l_vTargetJointPosition[2]<< std::endl;
//std::cout << "l_vTargetJointPosition= " << l_vTargetJointPosition[0] << ", " << l_vTargetJointPosition[1] << ", " << l_vTargetJointPosition[2] << ", " << l_vTargetJointPosition[3]<< std::endl;
// call the dxl sendTargetJointPosition // call the dxl sendTargetJointPosition
bool bIsSendSuccessfull = this->sendTargetJointPosition(l_vTargetJointPosition); bool bIsSendSuccessfull = this->sendTargetJointPosition(l_vTargetJointPosition);
@ -301,13 +307,16 @@ bool DynamixelHandler::sendTargetJointVelocity(std::vector<float>& vTargetJointV
std::vector<uint16_t> l_vTargetJointVelocity; std::vector<uint16_t> l_vTargetJointVelocity;
// q1 // q1
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q1*vTargetJointVelocity[0])); l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q1*vTargetJointVelocity[0]));
// qpen
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_QPEN*vTargetJointVelocity[1]));
// q2 // q2
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q2*vTargetJointVelocity[1]));
// q3
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q2*vTargetJointVelocity[2])); l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q2*vTargetJointVelocity[2]));
// q4
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q2*vTargetJointVelocity[3]));
std::cout << "l_vTargetJointVelocity= " << l_vTargetJointVelocity[0] << ", " << l_vTargetJointVelocity[1] << ", " << l_vTargetJointVelocity[2]<< std::endl;
std::cout << "l_vTargetJointVelocity= " << l_vTargetJointVelocity[0] << ", " << l_vTargetJointVelocity[1] << ", " << l_vTargetJointVelocity[2] << ", " << l_vTargetJointVelocity[3]<< std::endl;
// call the dxl sendTargetJointPosition // call the dxl sendTargetJointPosition
bool bIsSendSuccessfull = this->sendTargetJointVelocity(l_vTargetJointVelocity); bool bIsSendSuccessfull = this->sendTargetJointVelocity(l_vTargetJointVelocity);

161
src/Kinematics.cpp
View File

@ -1,161 +1,48 @@
#include "Kinematics.h" #include "Kinematics.h"
float deg2rad(float angle) float deg2rad(float angle)
{ {
return angle/180.0*M_PI; return angle / 180.0 * M_PI;
} }
float rad2deg(float angle) float rad2deg(float angle)
{ {
return angle*180.0/M_PI; return angle * 180.0 / M_PI;
} }
std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2) std::vector<float> computeForwardKinematics(float q1, float q2, float q3, float q4)
{ {
float x = L1 * cos(q1) + L2 * cos(q1+q2); float x = 6*cos(q1)*cos(q3)*cos(q4) - 6*cos(q1)*sin(q3)*sin(q4) + 5.5*cos(q1)*cos(q3) + 5*cos(q1);
float y = L1 * sin(q1) + L2 * sin(q1+q2); float y = 6*sin(q1)*cos(q3)*cos(q4) - 6*sin(q1)*sin(q3)*sin(q4) + 5.5*sin(q1)*cos(q3) + 5*sin(q1);
std::cout << "[INFO] Forward Kinematics : (q1, q2)->(x, y) = (" << rad2deg(q1) << ", " << rad2deg(q2) << ")->(" << x << ", " << y << ")" << std::endl; float z = 6*sin(q3)*cos(q4) + 6*cos(q3)*sin(q4) + 5.5*sin(q3);
std::vector<float> X;
X.push_back(x);
X.push_back(y);
std::cout << "[INFO] Forward Kinematics : (q1, q2, q3, q4)->(x, y, z) = ("
<< rad2deg(q1) << ", " << rad2deg(q2) << ", "
<< rad2deg(q3) << ", " << rad2deg(q4) << ") -> ("
<< x << ", " << y << ", " << z << ")" << std::endl;
std::vector<float> X = {x, y, z};
return X; return X;
} }
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2) std::vector<float> computeInverseKinematics(float x, float y)
{ {
std::vector<float> qi; std::vector<float> qi;
float cos_q2 = (x*x+y*y-(L1*L1+L2*L2)) / (2.0 * L1 * L2);
std::cout << "[INFO] cos_q2= " << cos_q2 << std::endl;
if (cos_q2 >1 | cos_q2 <-1)
{
qi.push_back(0.0);
std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl;
}
else if (cos_q2 == 1)
{
qi.push_back(1.0);
float q1 = atan2(y, x); float q1 = atan2(y, x);
float q2 = 0; float q2 = deg2rad(-90.0); // sabit
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl; float q3 = acos((sqrt(x*x + y*y) - 5.0 - (6.0 / sqrt((y*y)/(x*x) + 1.0))) / 5.5);
float q4 = -q1 - q3;
std::cout << "[INFO] Inverse Kinematics (New): (x, y) -> (q1, q2, q3, q4) = ("
<< x << ", " << y << ") -> ("
<< rad2deg(q1) << ", " << rad2deg(q2) << ", "
<< rad2deg(q3) << ", " << rad2deg(q4) << ")" << std::endl;
qi.push_back(q1); qi.push_back(q1);
qi.push_back(q2); qi.push_back(q2);
} qi.push_back(q3);
else if (cos_q2 == -1) qi.push_back(q4);
{
qi.push_back(1.0);
float q1 = atan2(y, x);
float q2 = M_PI;
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
qi.push_back(q1);
qi.push_back(q2);
}
else
{
qi.push_back(2.0);
std::cout << "[INFO] Inverse Kinematics: Two solutions: "<< std::endl;
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;
qi.push_back(q1);
qi.push_back(q2);
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;
qi.push_back(q1);
qi.push_back(q2);
}
return qi; return qi;
} }
std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
{
std::vector<float> jacobian;
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);
jacobian.push_back(j11);
jacobian.push_back(j12);
jacobian.push_back(j21);
jacobian.push_back(j22);
return jacobian;
}
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
{
int rank = -1;
cv::Mat1f oJacobianMatrix(2, 2);
if (vJacobianMatrix.size() == 4)
{
// Converts the Jacobian matrix from std::vector to cv::Mat
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
std::cout << "=====Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << " ]" << std::endl;
// Computes the determinant of the Jacobian matrix
float determinant = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]);
std::cout << "=====Determinant of the Jacobian matrix=====" << std::endl << determinant << std::endl;
// Computes SVD
cv::Mat1f w, u, vt;
cv::SVD::compute(oJacobianMatrix, w, u, vt);
// Finds non zero singular values
cv::Mat1f nonZeroSingularValues = w/w.at<float>(0,0) > threshold;
// Counts the number of non zero singular values
rank = cv::countNonZero(nonZeroSingularValues);
std::cout << "=====Rank of the Jacobian matrix=====" << std::endl << rank << " / " << oJacobianMatrix.rows << std::endl;
// Determines the inverse of the Jacobian matrix
cv::Mat oJacobianInverse = oJacobianMatrix.inv();
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << " ]" << std::endl;
}
else
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
return rank;
}
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
{
cv::Mat1f oJacobianMatrix(2, 2);
cv::Mat oJacobianInverse;
if (vJacobianMatrix.size() == 4)
{
// Converts the Jacobian matrix from std::vector to cv::Mat
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
std::cout << "=====Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << " ]" << std::endl;
// Determines the inverse of the Jacobian matrix
cv::invert(oJacobianMatrix, oJacobianInverse, cv::DECOMP_SVD);
//oJacobianInverse = oJacobianMatrix.inv();
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1) << " ]" << std::endl;
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << " ]" << std::endl;
}
else
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
return oJacobianInverse;
}

195
src/RobotServoing.cpp
View File

@ -18,28 +18,27 @@
#define FPS 30.0 #define FPS 30.0
#define STRUCTURAL_ELEMENTS_SIZE 5 #define STRUCTURAL_ELEMENTS_SIZE 5
#define AREA_THRESOLD 1000 #define AREA_THRESOLD 1000
#define ROBOT_L 15.0f // 1 ve 4. aktüatörler arasındaki toplam uzunluk (cm) #define ROBOT_L1 5
#define ROBOT_L4 5.0f // 4. aktüatörden sonraki uzunluk (cm) #define ROBOT_L2 5.5
#define RATIO_CM_PER_PIXEL_X 0.1f #define ROBOT_L3 6
#define RATIO_CM_PER_PIXEL_Y 0.1f
using namespace cv;
using namespace std; using namespace std;
using namespace cv;
DynamixelHandler _oDxlHandler; DynamixelHandler _oDxlHandler;
std::string _robotDxlPortName = "/dev/ttyUSB0"; string _robotDxlPortName = "/dev/ttyUSB0";
float _robotDxlProtocol = 2.0; float _robotDxlProtocol = 2.0;
int _robotDxlBaudRate = 1000000; int _robotDxlBaudRate = 1000000;
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate) void initRobot(DynamixelHandler& dxlHandler, string portName, float protocol, int baudRate)
{ {
std::cout << "=== Initialization of the Dynamixel Motor communication ===" << std::endl; cout << "===Initialization of the Dynamixel Motor communication====" << endl;
dxlHandler.setDeviceName(portName); dxlHandler.setDeviceName(portName);
dxlHandler.setProtocolVersion(protocol); dxlHandler.setProtocolVersion(protocol);
dxlHandler.openPort(); dxlHandler.openPort();
dxlHandler.setBaudRate(baudRate); dxlHandler.setBaudRate(baudRate);
dxlHandler.enableTorque(true); dxlHandler.enableTorque(true);
std::cout << std::endl; cout << endl;
} }
void closeRobot(DynamixelHandler& dxlHandler) void closeRobot(DynamixelHandler& dxlHandler)
@ -48,145 +47,155 @@ void closeRobot(DynamixelHandler& dxlHandler)
dxlHandler.closePort(); dxlHandler.closePort();
} }
cv::Point2f pixelToWorld(int u, int v, int img_width, int img_height) bool readCameraParameters(string filename, Mat &camMatrix, Mat & distCoeffs)
{ {
float x = (v - img_height / 2) * RATIO_CM_PER_PIXEL_Y; FileStorage fs(filename, FileStorage::READ);
float y = (u - img_width / 2) * RATIO_CM_PER_PIXEL_X; if (!fs.isOpened()) {
return cv::Point2f(x, y); cout << "[ERROR] Could not open the camera parameter file storage: " << filename << " !" << endl;
return false;
}
fs["camera_matrix"] >> camMatrix;
fs["distortion_coefficients"] >> distCoeffs;
return true;
}
bool readColorParameters(string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV)
{
FileStorage fs(filename, FileStorage::READ);
if (!fs.isOpened()) {
cout << "[ERROR] Could not open the color paramter file storage: " << filename << " !" << endl;
return false;
}
fs["lowH"] >> iLowH; fs["highH"] >> iHighH;
fs["lowS"] >> iLowS; fs["highS"] >> iHighS;
fs["lowV"] >> iLowV; fs["highV"] >> iHighV;
return true;
} }
int main(int argc, char** argv) int main(int argc, char** argv)
{ {
float L = ROBOT_L; float q2 = deg2rad(90);
float L4 = ROBOT_L4; string sCameraParamFilename = CAM_PARAMS_FILENAME;
float qpen = deg2rad(-90); // rad string sColorParamFilename = COLOR_PARAMS_FILENAME;
float fFPS = FPS; float fFPS = FPS;
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
int iAreaThresold = AREA_THRESOLD; int iAreaThresold = AREA_THRESOLD;
int opt;
while ((opt = getopt(argc, argv, ":c:f:s:a:i:")) != -1) {
switch (opt) {
case 'c': sColorParamFilename = optarg; break;
case 'f': fFPS = atof(optarg); break;
case 's': iStructuralElementSize = atoi(optarg); break;
case 'a': iAreaThresold = atoi(optarg); break;
case 'i': sCameraParamFilename = optarg; break;
case '?':
fprintf(stderr, "Unknown option -%c.\n", optopt);
return 1;
default:
abort();
}
}
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
// === YAML dosyasından renk parametrelerini yükle === int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV;
cv::FileStorage fsColorParams(COLOR_PARAMS_FILENAME, cv::FileStorage::READ); bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
if (!fsColorParams.isOpened()) if (!isColorParamsSet) {
{ cout << "[ERROR] Color parameters could not be loaded!" << endl;
std::cerr << "[ERROR] Renk parametreleri dosyasi acilamadi: " << COLOR_PARAMS_FILENAME << std::endl;
return -1; return -1;
} }
int lowH, highH, lowS, highS, lowV, highV;
fsColorParams["lowH"] >> lowH;
fsColorParams["highH"] >> highH;
fsColorParams["lowS"] >> lowS;
fsColorParams["highS"] >> highS;
fsColorParams["lowV"] >> lowV;
fsColorParams["highV"] >> highV;
fsColorParams.release();
cv::Scalar lowerHSV(lowH, lowS, lowV); bool bIsImageUndistorted = true;
cv::Scalar upperHSV(highH, highS, highV); Mat cameraMatrix, distCoeffs;
bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs);
// === Kamera kalibrasyon parametrelerini yükle (XML) === if (!isCamParamsSet) {
cv::Mat cameraMatrix, distCoeffs; cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << endl;
cv::FileStorage fsCamParams(CAM_PARAMS_FILENAME, cv::FileStorage::READ);
if (!fsCamParams.isOpened())
{
std::cerr << "[ERROR] Kamera parametreleri dosyasi acilamadi: " << CAM_PARAMS_FILENAME << std::endl;
return -1;
} }
fsCamParams["camera_matrix"] >> cameraMatrix;
fsCamParams["distortion_coefficients"] >> distCoeffs;
fsCamParams.release();
// === Kamera açılıyor ===
VideoCapture cap(0, cv::CAP_V4L2); VideoCapture cap(0, cv::CAP_V4L2);
if (!cap.isOpened()) if (!cap.isOpened()) {
{
cout << "[ERROR] Could not open the camera!" << endl; cout << "[ERROR] Could not open the camera!" << endl;
return -1; return -1;
} }
int iLastX = -1, iLastY = -1;
Mat imgTmp; Mat imgTmp;
cap.read(imgTmp); cap.read(imgTmp);
int img_width = imgTmp.size().width;
int img_height = imgTmp.size().height;
int iLastX = -1, iLastY = -1; while (true) {
Mat imgLines = Mat::zeros(imgTmp.size(), CV_8UC3);
while (true)
{
Mat imgOriginal; Mat imgOriginal;
bool bSuccess = cap.read(imgOriginal); bool bSuccess = cap.read(imgOriginal);
if (!bSuccess)
{ if (!bSuccess) {
cout << "[WARNING] Could not read a new frame from video stream" << endl; cout << "[WARNING] Could not read a new frame from video stream" << endl;
break; break;
} }
// === Görüntüyü kalibre et === if (bIsImageUndistorted && isCamParamsSet) {
cv::Mat imgUndistorted; Mat temp = imgOriginal.clone();
cv::undistort(imgOriginal, imgUndistorted, cameraMatrix, distCoeffs); undistort(temp, imgOriginal, cameraMatrix, distCoeffs);
}
// Kalibre edilmiş görüntüyü HSV formatına çevir Mat imgHSV;
cv::Mat imgHSV; cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
cvtColor(imgUndistorted, imgHSV, cv::COLOR_BGR2HSV);
// YAML'dan alınan renklerle eşikleme yap Mat imgThresholded;
cv::Mat imgThresholded; inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);
inRange(imgHSV, lowerHSV, upperHSV, imgThresholded);
// Morfolojik işlemler erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)));
cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)));
cv::dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5))); dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)));
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)));
// Moment hesapla
Moments oMoments = moments(imgThresholded); Moments oMoments = moments(imgThresholded);
double dM01 = oMoments.m01; double dM01 = oMoments.m01;
double dM10 = oMoments.m10; double dM10 = oMoments.m10;
double dArea = oMoments.m00; double dArea = oMoments.m00;
int posX = -1, posY = -1; int posX, posY;
if (dArea > iAreaThresold) float x = 0, y = 0;
{
if (dArea > iAreaThresold) {
posX = dM10 / dArea; posX = dM10 / dArea;
posY = dM01 / dArea; posY = dM01 / dArea;
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) {
{ line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
line(imgUndistorted, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
} }
iLastX = posX; iLastX = posX;
iLastY = posY; iLastY = posY;
x = 10.0; // Örnek değer
y = 10.0;
} }
cv::Point2f worldCoord = pixelToWorld(posX, posY, img_width, img_height); imshow("Thresholded Image", imgThresholded);
float x = worldCoord.x; drawMarker(imgOriginal, Point(imgTmp.size().width / 2, imgTmp.size().height / 2), 10, MARKER_CROSS, LINE_8);
float y = worldCoord.y; imgOriginal = imgOriginal + imgLines;
imshow("Original", imgOriginal);
cout << "(pixel -> cm) = (" << posX << ", " << posY << ") -> (" << x << ", " << y << ")" << endl; vector<float> qi = computeInverseKinematics(x, y);
computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]);
std::vector<float> qi = computeInverseKinematics(x, y, L, L4); if (qi.size() == 4) {
vector<float> vTargetJointPosition = {qi[0], qi[1], qi[2], qi[3]};
if (qi.size() >= 3)
{
std::vector<float> vTargetJointPosition;
vTargetJointPosition.push_back(qi[1]);
vTargetJointPosition.push_back(qpen);
vTargetJointPosition.push_back(qi[2]);
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition); _oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
} }
char key = (char)waitKey(1000.0 / fFPS);
imshow("Original", imgOriginal); if (key == 27) {
cout << "[INFO] esc key is pressed by user -> Shutting down!" << endl;
char key = (char)cv::waitKey(1000.0 / fFPS);
if (key == 27)
{
cout << "[INFO] ESC key pressed -> Shutting down!" << endl;
break; break;
} }
if (key == 'u') {
bIsImageUndistorted = !bIsImageUndistorted;
cout << "[INFO] Image undistorted: " << bIsImageUndistorted << endl;
}
} }
closeRobot(_oDxlHandler); _oDxlHandler.closePort();
return 0; return 0;
} }

69
src/robotest.cpp Normal file
View File

@ -0,0 +1,69 @@
#include <iostream>
#include <vector>
#include "Kinematics.h"
#include "DynamixelHandler.h"
#define L1 5.0f
#define L2 5.5f
#define L3 6.0f
using namespace std;
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)
{
cout << "=== Initialization of the Dynamixel Motor communication ===" << endl;
dxlHandler.setDeviceName(portName);
dxlHandler.setProtocolVersion(protocol);
dxlHandler.openPort();
dxlHandler.setBaudRate(baudRate);
dxlHandler.enableTorque(true);
cout << endl;
}
void closeRobot(DynamixelHandler& dxlHandler)
{
dxlHandler.enableTorque(false);
dxlHandler.closePort();
}
void moveToPosition(float x, float y)
{
vector<float> qi = computeInverseKinematics(x, y, L1, L2, L3);
if (qi.size() >= 4)
{
vector<float> vTargetJointPosition;
vTargetJointPosition.push_back(qi[1]);
vTargetJointPosition.push_back(qi[2]);
vTargetJointPosition.push_back(qi[3]);
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
}
else
{
cout << "[ERROR] No valid inverse kinematics solution found!" << endl;
}
}
int main()
{
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
float x, y;
while (true)
{
cout << "Enter target X coordinate: ";
cin >> x;
cout << "Enter target Y coordinate: ";
cin >> y;
moveToPosition(x, y);
}
closeRobot(_oDxlHandler);
return 0;
}