first lab
This commit is contained in:
parent
94da1729a2
commit
4df1688a3f
|
|
@ -31,10 +31,12 @@
|
|||
|
||||
// rotation direction
|
||||
#define ROT_DIRECTION_Q1 1
|
||||
#define ROT_DIRECTION_Q2 -1
|
||||
#define ROT_DIRECTION_QPEN 1
|
||||
#define ROT_DIRECTION_Q2 1
|
||||
#define ROT_DIRECTION_Q3 1
|
||||
#define ROT_DIRECTION_Q4 1
|
||||
|
||||
// nb of joints
|
||||
#define NB_JOINTS 3
|
||||
#define NB_JOINTS 4
|
||||
|
||||
|
||||
class DynamixelHandler
|
||||
|
|
|
|||
|
|
@ -5,16 +5,17 @@
|
|||
|
||||
#include "opencv2/opencv.hpp"
|
||||
|
||||
// Derece <-> Radyan dönüşüm fonksiyonları
|
||||
float deg2rad(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);
|
||||
|
||||
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
|
||||
|
||||
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);
|
||||
|
|
|
|||
|
|
@ -181,16 +181,19 @@ bool DynamixelHandler::readCurrentJointPosition(std::vector<float>& vCurrentJoin
|
|||
std::vector<uint16_t> l_vCurrentJointPosition;
|
||||
// Reads the current joint positions in motor command unit
|
||||
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
|
||||
vCurrentJointPosition.push_back(ROT_DIRECTION_Q1*convertJointCmdToAngle(l_vCurrentJointPosition[0]));
|
||||
// qpen
|
||||
vCurrentJointPosition.push_back(ROT_DIRECTION_QPEN*convertJointCmdToAngle(l_vCurrentJointPosition[1]));
|
||||
// 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;
|
||||
}
|
||||
|
|
@ -239,12 +242,15 @@ bool DynamixelHandler::sendTargetJointPosition(std::vector<float>& vTargetJointP
|
|||
std::vector<uint16_t> l_vTargetJointPosition;
|
||||
// q1
|
||||
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q1*vTargetJointPosition[0]));
|
||||
// qpen
|
||||
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_QPEN*vTargetJointPosition[1]));
|
||||
// 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
|
||||
bool bIsSendSuccessfull = this->sendTargetJointPosition(l_vTargetJointPosition);
|
||||
|
|
@ -301,13 +307,16 @@ bool DynamixelHandler::sendTargetJointVelocity(std::vector<float>& vTargetJointV
|
|||
std::vector<uint16_t> l_vTargetJointVelocity;
|
||||
// q1
|
||||
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q1*vTargetJointVelocity[0]));
|
||||
// qpen
|
||||
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_QPEN*vTargetJointVelocity[1]));
|
||||
// q2
|
||||
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q2*vTargetJointVelocity[1]));
|
||||
// q3
|
||||
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
|
||||
bool bIsSendSuccessfull = this->sendTargetJointVelocity(l_vTargetJointVelocity);
|
||||
|
||||
|
|
|
|||
|
|
@ -1,6 +1,5 @@
|
|||
#include "Kinematics.h"
|
||||
|
||||
|
||||
float deg2rad(float angle)
|
||||
{
|
||||
return angle / 180.0 * M_PI;
|
||||
|
|
@ -11,151 +10,39 @@ float rad2deg(float angle)
|
|||
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 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<float> X;
|
||||
X.push_back(x);
|
||||
X.push_back(y);
|
||||
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 = 6*sin(q1)*cos(q3)*cos(q4) - 6*sin(q1)*sin(q3)*sin(q4) + 5.5*sin(q1)*cos(q3) + 5*sin(q1);
|
||||
float z = 6*sin(q3)*cos(q4) + 6*cos(q3)*sin(q4) + 5.5*sin(q3);
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
|
||||
std::vector<float> computeInverseKinematics(float x, float y)
|
||||
{
|
||||
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 q2 = 0;
|
||||
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
|
||||
float q2 = deg2rad(-90.0); // sabit
|
||||
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(q2);
|
||||
}
|
||||
else if (cos_q2 == -1)
|
||||
{
|
||||
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);
|
||||
}
|
||||
qi.push_back(q3);
|
||||
qi.push_back(q4);
|
||||
|
||||
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;
|
||||
}
|
||||
|
|
@ -18,28 +18,27 @@
|
|||
#define FPS 30.0
|
||||
#define STRUCTURAL_ELEMENTS_SIZE 5
|
||||
#define AREA_THRESOLD 1000
|
||||
#define ROBOT_L 15.0f // 1 ve 4. aktüatörler arasındaki toplam uzunluk (cm)
|
||||
#define ROBOT_L4 5.0f // 4. aktüatörden sonraki uzunluk (cm)
|
||||
#define RATIO_CM_PER_PIXEL_X 0.1f
|
||||
#define RATIO_CM_PER_PIXEL_Y 0.1f
|
||||
#define ROBOT_L1 5
|
||||
#define ROBOT_L2 5.5
|
||||
#define ROBOT_L3 6
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
using namespace cv;
|
||||
|
||||
DynamixelHandler _oDxlHandler;
|
||||
std::string _robotDxlPortName = "/dev/ttyUSB0";
|
||||
string _robotDxlPortName = "/dev/ttyUSB0";
|
||||
float _robotDxlProtocol = 2.0;
|
||||
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.setProtocolVersion(protocol);
|
||||
dxlHandler.openPort();
|
||||
dxlHandler.setBaudRate(baudRate);
|
||||
dxlHandler.enableTorque(true);
|
||||
std::cout << std::endl;
|
||||
cout << endl;
|
||||
}
|
||||
|
||||
void closeRobot(DynamixelHandler& dxlHandler)
|
||||
|
|
@ -48,145 +47,155 @@ void closeRobot(DynamixelHandler& dxlHandler)
|
|||
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;
|
||||
float y = (u - img_width / 2) * RATIO_CM_PER_PIXEL_X;
|
||||
return cv::Point2f(x, y);
|
||||
FileStorage fs(filename, FileStorage::READ);
|
||||
if (!fs.isOpened()) {
|
||||
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)
|
||||
{
|
||||
float L = ROBOT_L;
|
||||
float L4 = ROBOT_L4;
|
||||
float qpen = deg2rad(-90); // rad
|
||||
float q2 = deg2rad(90);
|
||||
string sCameraParamFilename = CAM_PARAMS_FILENAME;
|
||||
string sColorParamFilename = COLOR_PARAMS_FILENAME;
|
||||
float fFPS = FPS;
|
||||
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
|
||||
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);
|
||||
|
||||
// === YAML dosyasından renk parametrelerini yükle ===
|
||||
cv::FileStorage fsColorParams(COLOR_PARAMS_FILENAME, cv::FileStorage::READ);
|
||||
if (!fsColorParams.isOpened())
|
||||
{
|
||||
std::cerr << "[ERROR] Renk parametreleri dosyasi acilamadi: " << COLOR_PARAMS_FILENAME << std::endl;
|
||||
int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV;
|
||||
bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
|
||||
if (!isColorParamsSet) {
|
||||
cout << "[ERROR] Color parameters could not be loaded!" << endl;
|
||||
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);
|
||||
cv::Scalar upperHSV(highH, highS, highV);
|
||||
|
||||
// === Kamera kalibrasyon parametrelerini yükle (XML) ===
|
||||
cv::Mat cameraMatrix, distCoeffs;
|
||||
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;
|
||||
bool bIsImageUndistorted = true;
|
||||
Mat cameraMatrix, distCoeffs;
|
||||
bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs);
|
||||
if (!isCamParamsSet) {
|
||||
cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << endl;
|
||||
}
|
||||
fsCamParams["camera_matrix"] >> cameraMatrix;
|
||||
fsCamParams["distortion_coefficients"] >> distCoeffs;
|
||||
fsCamParams.release();
|
||||
|
||||
// === Kamera açılıyor ===
|
||||
VideoCapture cap(0, cv::CAP_V4L2);
|
||||
if (!cap.isOpened())
|
||||
{
|
||||
if (!cap.isOpened()) {
|
||||
cout << "[ERROR] Could not open the camera!" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
int iLastX = -1, iLastY = -1;
|
||||
Mat imgTmp;
|
||||
cap.read(imgTmp);
|
||||
int img_width = imgTmp.size().width;
|
||||
int img_height = imgTmp.size().height;
|
||||
|
||||
int iLastX = -1, iLastY = -1;
|
||||
|
||||
while (true)
|
||||
{
|
||||
while (true) {
|
||||
Mat imgLines = Mat::zeros(imgTmp.size(), CV_8UC3);
|
||||
Mat imgOriginal;
|
||||
bool bSuccess = cap.read(imgOriginal);
|
||||
if (!bSuccess)
|
||||
{
|
||||
|
||||
if (!bSuccess) {
|
||||
cout << "[WARNING] Could not read a new frame from video stream" << endl;
|
||||
break;
|
||||
}
|
||||
|
||||
// === Görüntüyü kalibre et ===
|
||||
cv::Mat imgUndistorted;
|
||||
cv::undistort(imgOriginal, imgUndistorted, cameraMatrix, distCoeffs);
|
||||
if (bIsImageUndistorted && isCamParamsSet) {
|
||||
Mat temp = imgOriginal.clone();
|
||||
undistort(temp, imgOriginal, cameraMatrix, distCoeffs);
|
||||
}
|
||||
|
||||
// Kalibre edilmiş görüntüyü HSV formatına çevir
|
||||
cv::Mat imgHSV;
|
||||
cvtColor(imgUndistorted, imgHSV, cv::COLOR_BGR2HSV);
|
||||
Mat imgHSV;
|
||||
cvtColor(imgOriginal, imgHSV, COLOR_BGR2HSV);
|
||||
|
||||
// YAML'dan alınan renklerle eşikleme yap
|
||||
cv::Mat imgThresholded;
|
||||
inRange(imgHSV, lowerHSV, upperHSV, imgThresholded);
|
||||
Mat imgThresholded;
|
||||
inRange(imgHSV, Scalar(iLowH, iLowS, iLowV), Scalar(iHighH, iHighS, iHighV), imgThresholded);
|
||||
|
||||
// Morfolojik işlemler
|
||||
cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
|
||||
cv::dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(5, 5)));
|
||||
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)));
|
||||
dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)));
|
||||
dilate(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)));
|
||||
erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)));
|
||||
|
||||
// Moment hesapla
|
||||
Moments oMoments = moments(imgThresholded);
|
||||
double dM01 = oMoments.m01;
|
||||
double dM10 = oMoments.m10;
|
||||
double dArea = oMoments.m00;
|
||||
|
||||
int posX = -1, posY = -1;
|
||||
if (dArea > iAreaThresold)
|
||||
{
|
||||
int posX, posY;
|
||||
float x = 0, y = 0;
|
||||
|
||||
if (dArea > iAreaThresold) {
|
||||
posX = dM10 / dArea;
|
||||
posY = dM01 / dArea;
|
||||
|
||||
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
|
||||
{
|
||||
line(imgUndistorted, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
|
||||
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) {
|
||||
line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0, 0, 255), 2);
|
||||
}
|
||||
|
||||
iLastX = posX;
|
||||
iLastY = posY;
|
||||
|
||||
x = 10.0; // Örnek değer
|
||||
y = 10.0;
|
||||
}
|
||||
|
||||
cv::Point2f worldCoord = pixelToWorld(posX, posY, img_width, img_height);
|
||||
float x = worldCoord.x;
|
||||
float y = worldCoord.y;
|
||||
imshow("Thresholded Image", imgThresholded);
|
||||
drawMarker(imgOriginal, Point(imgTmp.size().width / 2, imgTmp.size().height / 2), 10, MARKER_CROSS, LINE_8);
|
||||
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() >= 3)
|
||||
{
|
||||
std::vector<float> vTargetJointPosition;
|
||||
vTargetJointPosition.push_back(qi[1]);
|
||||
vTargetJointPosition.push_back(qpen);
|
||||
vTargetJointPosition.push_back(qi[2]);
|
||||
if (qi.size() == 4) {
|
||||
vector<float> vTargetJointPosition = {qi[0], qi[1], qi[2], qi[3]};
|
||||
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
|
||||
}
|
||||
|
||||
|
||||
imshow("Original", imgOriginal);
|
||||
|
||||
|
||||
char key = (char)cv::waitKey(1000.0 / fFPS);
|
||||
if (key == 27)
|
||||
{
|
||||
cout << "[INFO] ESC key pressed -> Shutting down!" << endl;
|
||||
char key = (char)waitKey(1000.0 / fFPS);
|
||||
if (key == 27) {
|
||||
cout << "[INFO] esc key is pressed by user -> Shutting down!" << endl;
|
||||
break;
|
||||
}
|
||||
if (key == 'u') {
|
||||
bIsImageUndistorted = !bIsImageUndistorted;
|
||||
cout << "[INFO] Image undistorted: " << bIsImageUndistorted << endl;
|
||||
}
|
||||
}
|
||||
|
||||
closeRobot(_oDxlHandler);
|
||||
_oDxlHandler.closePort();
|
||||
return 0;
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
Loading…
Reference in New Issue