diff --git a/include/DynamixelHandler.h b/include/DynamixelHandler.h index c1d4ee6..4a43900 100644 --- a/include/DynamixelHandler.h +++ b/include/DynamixelHandler.h @@ -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 @@ -85,4 +87,4 @@ private: float m_fMinJointAngle = -150.0f/180.0f*M_PI; float m_fMaxJointAngle = 150.0f/180.0f*M_PI; -}; \ No newline at end of file +}; diff --git a/include/Kinematics.h b/include/Kinematics.h index d465457..b94ae53 100644 --- a/include/Kinematics.h +++ b/include/Kinematics.h @@ -5,16 +5,17 @@ #include "opencv2/opencv.hpp" +// Derece <-> Radyan dönüşüm fonksiyonları float deg2rad(float angle); - float rad2deg(float angle); -std::vector computeForwardKinematics(float q1, float q2, float L1, float L2); +// Yeni 4-eklemli robot için ileri kinematik (Forward Kinematics) +std::vector computeForwardKinematics(float q1, float q2, float q3, float q4); -std::vector computeInverseKinematics(float x, float y, float L1, float L2); +// Yeni 4-eklemli robot için ters kinematik (Inverse Kinematics) +std::vector computeInverseKinematics(float x, float y); +// Jacobian matris fonksiyonları (isteğe bağlı olarak genişletilebilir) std::vector computeDifferentialKinematics(float q1, float q2, float L1, float L2); - int computeJacobianMatrixRank(std::vector vJacobianMatrix, float threshold); - -cv::Mat computeInverseJacobianMatrix(std::vector vJacobianMatrix); +cv::Mat computeInverseJacobianMatrix(std::vector vJacobianMatrix); diff --git a/src/DynamixelHandler.cpp b/src/DynamixelHandler.cpp index 372d381..1de93d1 100644 --- a/src/DynamixelHandler.cpp +++ b/src/DynamixelHandler.cpp @@ -1,407 +1,416 @@ #include "DynamixelHandler.h" DynamixelHandler::DynamixelHandler(): - m_sDeviceName(""), m_fProtocolVersion(0.0), m_i32BaudRate(0), - m_pPacketHandler(nullptr), m_pPortHandler(nullptr), - m_bIsDeviceNameSet(false), m_bIsProtocolVersionSet(false), m_bIsPortOpened(false), m_bIsBaudRateSet(false), - m_ui8DxlError(0), m_i32DxlCommunicationResult(COMM_TX_FAIL) + m_sDeviceName(""), m_fProtocolVersion(0.0), m_i32BaudRate(0), + m_pPacketHandler(nullptr), m_pPortHandler(nullptr), + m_bIsDeviceNameSet(false), m_bIsProtocolVersionSet(false), m_bIsPortOpened(false), m_bIsBaudRateSet(false), + m_ui8DxlError(0), m_i32DxlCommunicationResult(COMM_TX_FAIL) { - + } DynamixelHandler::~DynamixelHandler() { - + } int DynamixelHandler::convertJointVelocityToJointCmd(float fJointVelocity) -{ - if (fJointVelocity == 0.0f) - return 0; - - float a = 0.0f; - float b = 0.0f; - if (fJointVelocity>0) - { - float l_fMaxJointCmd = 1023; - float l_fMinJointCmd = 0; - float l_fMaxJointVelocity = 114 * 60.0f * 2 * M_PI; - float l_fMinJointAngle = 0.0f; - // y = ax + b - a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle); - b = l_fMinJointCmd - a * l_fMinJointAngle; - } - - if (fJointVelocity<0) - { - float l_fMaxJointCmd = 2047; - float l_fMinJointCmd = 1024; - float l_fMaxJointVelocity = 0.0f; - float l_fMinJointAngle = -114 * 60.0f * 2 * M_PI; - // y = ax + b - a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle); - b = l_fMinJointCmd - a * l_fMinJointAngle; - } - - float jointCmd = a * fJointVelocity + b; - return (int)jointCmd; +{ + if (fJointVelocity == 0.0f) + return 0; + + float a = 0.0f; + float b = 0.0f; + if (fJointVelocity>0) + { + float l_fMaxJointCmd = 1023; + float l_fMinJointCmd = 0; + float l_fMaxJointVelocity = 114 * 60.0f * 2 * M_PI; + float l_fMinJointAngle = 0.0f; + // y = ax + b + a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle); + b = l_fMinJointCmd - a * l_fMinJointAngle; + } + + if (fJointVelocity<0) + { + float l_fMaxJointCmd = 2047; + float l_fMinJointCmd = 1024; + float l_fMaxJointVelocity = 0.0f; + float l_fMinJointAngle = -114 * 60.0f * 2 * M_PI; + // y = ax + b + a = (l_fMaxJointCmd-l_fMinJointCmd) / (l_fMaxJointVelocity - l_fMinJointAngle); + b = l_fMinJointCmd - a * l_fMinJointAngle; + } + + float jointCmd = a * fJointVelocity + b; + return (int)jointCmd; } int DynamixelHandler::convertAngleToJointCmd(float fJointAngle) -{ - // y = ax + b - float a = (m_fMaxJointCmd-m_fMinJointCmd) / (m_fMaxJointAngle - m_fMinJointAngle); - float b = m_fMinJointCmd - a * m_fMinJointAngle; - float jointCmd = a * fJointAngle + b; - return (int)jointCmd; +{ + // y = ax + b + float a = (m_fMaxJointCmd-m_fMinJointCmd) / (m_fMaxJointAngle - m_fMinJointAngle); + float b = m_fMinJointCmd - a * m_fMinJointAngle; + float jointCmd = a * fJointAngle + b; + return (int)jointCmd; } float DynamixelHandler::convertJointCmdToAngle(int iJointCmd) -{ - // y = ax + b - float a = (m_fMaxJointAngle - m_fMinJointAngle) / (m_fMaxJointCmd-m_fMinJointCmd); - float b = m_fMinJointAngle - a * m_fMinJointCmd; - float jointAngle = a * iJointCmd + b; - return jointAngle; +{ + // y = ax + b + float a = (m_fMaxJointAngle - m_fMinJointAngle) / (m_fMaxJointCmd-m_fMinJointCmd); + float b = m_fMinJointAngle - a * m_fMinJointCmd; + float jointAngle = a * iJointCmd + b; + return jointAngle; } bool DynamixelHandler::openPort() { - if (m_pPortHandler == nullptr) - { - std::cout << "[ERROR](DynamixelHandler::openPort) m_pPortHandler is null!" << std::endl; - m_bIsPortOpened = false; - return m_bIsPortOpened; - } - - if (!m_bIsDeviceNameSet) - { - std::cout << "[ERROR](DynamixelHandler::openPort) m_sDeviceName is not set!" << std::endl; - m_bIsPortOpened = false; - return m_bIsPortOpened; - } - - if (m_bIsPortOpened) - { - std::cout << "[WARNING](DynamixelHandler::openPort) port is already opened!" << std::endl; - return m_bIsPortOpened; - } - - if (m_pPortHandler->openPort()) - { - std::cout << "[INFO](DynamixelHandler::openPort) Succeeded to open the port!" << std::endl; - m_bIsPortOpened = true; - } - else - { - std::cout << "[ERROR](DynamixelHandler::openPort) Failed to open the port!" << std::endl; - m_bIsPortOpened = false; - } - return m_bIsPortOpened; + if (m_pPortHandler == nullptr) + { + std::cout << "[ERROR](DynamixelHandler::openPort) m_pPortHandler is null!" << std::endl; + m_bIsPortOpened = false; + return m_bIsPortOpened; + } + + if (!m_bIsDeviceNameSet) + { + std::cout << "[ERROR](DynamixelHandler::openPort) m_sDeviceName is not set!" << std::endl; + m_bIsPortOpened = false; + return m_bIsPortOpened; + } + + if (m_bIsPortOpened) + { + std::cout << "[WARNING](DynamixelHandler::openPort) port is already opened!" << std::endl; + return m_bIsPortOpened; + } + + if (m_pPortHandler->openPort()) + { + std::cout << "[INFO](DynamixelHandler::openPort) Succeeded to open the port!" << std::endl; + m_bIsPortOpened = true; + } + else + { + std::cout << "[ERROR](DynamixelHandler::openPort) Failed to open the port!" << std::endl; + m_bIsPortOpened = false; + } + return m_bIsPortOpened; } void DynamixelHandler::closePort() { - if (m_pPortHandler == nullptr) - { - std::cout << "[ERROR](DynamixelHandler::closePort) m_pPortHandler is null!" << std::endl; - m_bIsPortOpened = false; - return; - } - - if (!m_bIsPortOpened) - { - std::cout << "[WARNING](DynamixelHandler::openPort) port is already closed!" << std::endl; - return; - } - - m_pPortHandler->closePort(); - - std::cout << "[INFO](DynamixelHandler::closePort) Succeeded to close the port!" << std::endl; - m_bIsPortOpened = false; + if (m_pPortHandler == nullptr) + { + std::cout << "[ERROR](DynamixelHandler::closePort) m_pPortHandler is null!" << std::endl; + m_bIsPortOpened = false; + return; + } + + if (!m_bIsPortOpened) + { + std::cout << "[WARNING](DynamixelHandler::openPort) port is already closed!" << std::endl; + return; + } + + m_pPortHandler->closePort(); + + std::cout << "[INFO](DynamixelHandler::closePort) Succeeded to close the port!" << std::endl; + m_bIsPortOpened = false; } bool DynamixelHandler::setBaudRate(int i32BaudRate) { - m_i32BaudRate = i32BaudRate; - - if (nullptr != m_pPortHandler) - { - if (m_pPortHandler->setBaudRate(m_i32BaudRate)) - { - std::cout << "[INFO](DynamixelHandler::setBaudRate) Succeeded to change the baudrate!" << std::endl; - m_bIsBaudRateSet = true; - } - else - { - std::cout << "[ERROR](DynamixelHandler::setBaudRate) Failed to change the baudrate!" << std::endl; - m_bIsBaudRateSet = false; - } - } - else - { - std::cout << "[ERROR](DynamixelHandler::setBaudRate) m_pPortHandler is null!" << std::endl; - m_bIsBaudRateSet = false; - } - return m_bIsBaudRateSet; + m_i32BaudRate = i32BaudRate; + + if (nullptr != m_pPortHandler) + { + if (m_pPortHandler->setBaudRate(m_i32BaudRate)) + { + std::cout << "[INFO](DynamixelHandler::setBaudRate) Succeeded to change the baudrate!" << std::endl; + m_bIsBaudRateSet = true; + } + else + { + std::cout << "[ERROR](DynamixelHandler::setBaudRate) Failed to change the baudrate!" << std::endl; + m_bIsBaudRateSet = false; + } + } + else + { + std::cout << "[ERROR](DynamixelHandler::setBaudRate) m_pPortHandler is null!" << std::endl; + m_bIsBaudRateSet = false; + } + return m_bIsBaudRateSet; } void DynamixelHandler::setDeviceName(std::string sDeviceName) { - m_sDeviceName = sDeviceName; - m_bIsDeviceNameSet = true; - - if (nullptr != m_pPortHandler) - { - delete m_pPortHandler; - m_pPortHandler = nullptr; - } - - // Initialize PortHandler instance - m_pPortHandler = dynamixel::PortHandler::getPortHandler(m_sDeviceName.c_str()); + m_sDeviceName = sDeviceName; + m_bIsDeviceNameSet = true; + + if (nullptr != m_pPortHandler) + { + delete m_pPortHandler; + m_pPortHandler = nullptr; + } + + // Initialize PortHandler instance + m_pPortHandler = dynamixel::PortHandler::getPortHandler(m_sDeviceName.c_str()); } void DynamixelHandler::setProtocolVersion(float fProtocolVersion) { - m_fProtocolVersion = fProtocolVersion; - m_bIsProtocolVersionSet = true; - - if (nullptr != m_pPacketHandler) - { - delete m_pPacketHandler; - m_pPacketHandler = nullptr; - } - - m_pPacketHandler = dynamixel::PacketHandler::getPacketHandler(m_fProtocolVersion); + m_fProtocolVersion = fProtocolVersion; + m_bIsProtocolVersionSet = true; + + if (nullptr != m_pPacketHandler) + { + delete m_pPacketHandler; + m_pPacketHandler = nullptr; + } + + m_pPacketHandler = dynamixel::PacketHandler::getPacketHandler(m_fProtocolVersion); } bool DynamixelHandler::readCurrentJointPosition(std::vector& vCurrentJointPosition) { - // Creates a vector of joint position - std::vector 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; - - // 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])); - - //std::cout << "vCurrentJointPosition= " << vCurrentJointPosition[0] << ", " << vCurrentJointPosition[1] << ", " << vCurrentJointPosition[2]<< std::endl; - - return bIsReadSuccessfull; + // Creates a vector of joint position + std::vector 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] << ", " << l_vCurrentJointPosition[3]<< std::endl; + + // q1 + vCurrentJointPosition.push_back(ROT_DIRECTION_Q1*convertJointCmdToAngle(l_vCurrentJointPosition[0])); + // q2 + 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] << ", " << vCurrentJointPosition[3]<< std::endl; + + return bIsReadSuccessfull; } bool DynamixelHandler::readCurrentJointPosition(std::vector& vCurrentJointPosition) { - bool bIsReadSuccessfull = false; + bool bIsReadSuccessfull = false; - for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) - { - int dxl_comm_result = COMM_TX_FAIL; // Communication result - uint8_t dxl_error = 0; - uint16_t dxl_present_position = 0; + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + uint16_t dxl_present_position = 0; - dxl_comm_result = m_pPacketHandler->read2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_PRESENT_POSITION, &dxl_present_position, &dxl_error); - if (dxl_comm_result != COMM_SUCCESS) - { - //std::cout << "[ERROR] " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; - bIsReadSuccessfull = false; - } - else if (dxl_error != 0) - { - //std::cout << "[ERROR] " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; - bIsReadSuccessfull = false; - } - else - { - vCurrentJointPosition.push_back(dxl_present_position); - bIsReadSuccessfull = true; - } - } + dxl_comm_result = m_pPacketHandler->read2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_PRESENT_POSITION, &dxl_present_position, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsReadSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsReadSuccessfull = false; + } + else + { + vCurrentJointPosition.push_back(dxl_present_position); + bIsReadSuccessfull = true; + } + } - return bIsReadSuccessfull; + return bIsReadSuccessfull; } bool DynamixelHandler::sendTargetJointPosition(std::vector& vTargetJointPosition) { - // Checks if the input vector has the right size - if (vTargetJointPosition.size() != NB_JOINTS) - { - std::cout << "[ERROR] (sendTargetJointPosition) Input vector has not the right size!" << std::endl; - return false; - } - - // Creates a vector of motor commands - std::vector 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])); - - //std::cout << "l_vTargetJointPosition= " << l_vTargetJointPosition[0] << ", " << l_vTargetJointPosition[1] << ", " << l_vTargetJointPosition[2]<< std::endl; - - // call the dxl sendTargetJointPosition - bool bIsSendSuccessfull = this->sendTargetJointPosition(l_vTargetJointPosition); - - return bIsSendSuccessfull; + // Checks if the input vector has the right size + if (vTargetJointPosition.size() != NB_JOINTS) + { + std::cout << "[ERROR] (sendTargetJointPosition) Input vector has not the right size!" << std::endl; + return false; + } + + // Creates a vector of motor commands + std::vector l_vTargetJointPosition; + // q1 + l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q1*vTargetJointPosition[0])); + // q2 + 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] << ", " << l_vTargetJointPosition[3]<< std::endl; + + // call the dxl sendTargetJointPosition + bool bIsSendSuccessfull = this->sendTargetJointPosition(l_vTargetJointPosition); + + return bIsSendSuccessfull; } bool DynamixelHandler::sendTargetJointPosition(std::vector& vTargetJointPosition) { - bool bIsSendSuccessfull = false; + bool bIsSendSuccessfull = false; - // checks if the vector size is correct - if (vTargetJointPosition.size() != NB_JOINTS) - { - std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): Size of command vector is not correct: " << vTargetJointPosition.size() << " instead of " << NB_JOINTS << "!" << std::endl; - bIsSendSuccessfull = false; - } + // checks if the vector size is correct + if (vTargetJointPosition.size() != NB_JOINTS) + { + std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): Size of command vector is not correct: " << vTargetJointPosition.size() << " instead of " << NB_JOINTS << "!" << std::endl; + bIsSendSuccessfull = false; + } - for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) - { - int dxl_comm_result = COMM_TX_FAIL; // Communication result - uint8_t dxl_error = 0; - uint16_t dxl_present_position = 0; - dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_POSITION, vTargetJointPosition[l_joint], &dxl_error); - if (dxl_comm_result != COMM_SUCCESS) - { - //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; - bIsSendSuccessfull = false; - } - else if (dxl_error != 0) - { - //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; - bIsSendSuccessfull = false; - } - else - { - bIsSendSuccessfull = true; - } - } - return bIsSendSuccessfull; + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + uint16_t dxl_present_position = 0; + dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_POSITION, vTargetJointPosition[l_joint], &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsSendSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsSendSuccessfull = false; + } + else + { + bIsSendSuccessfull = true; + } + } + return bIsSendSuccessfull; } bool DynamixelHandler::sendTargetJointVelocity(std::vector& vTargetJointVelocity) { - // Checks if the input vector has the right size - if (vTargetJointVelocity.size() != NB_JOINTS) - { - std::cout << "[ERROR] (sendTargetJointVelocity) Input vector has not the right size!" << std::endl; - return false; - } - - // Creates a vector of motor commands - std::vector 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[2])); - - std::cout << "l_vTargetJointVelocity= " << l_vTargetJointVelocity[0] << ", " << l_vTargetJointVelocity[1] << ", " << l_vTargetJointVelocity[2]<< std::endl; - - // call the dxl sendTargetJointPosition - bool bIsSendSuccessfull = this->sendTargetJointVelocity(l_vTargetJointVelocity); - - return bIsSendSuccessfull; + // Checks if the input vector has the right size + if (vTargetJointVelocity.size() != NB_JOINTS) + { + std::cout << "[ERROR] (sendTargetJointVelocity) Input vector has not the right size!" << std::endl; + return false; + } + + // Creates a vector of motor commands + std::vector l_vTargetJointVelocity; + // q1 + l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q1*vTargetJointVelocity[0])); + // 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] << ", " << l_vTargetJointVelocity[3]<< std::endl; + // call the dxl sendTargetJointPosition + bool bIsSendSuccessfull = this->sendTargetJointVelocity(l_vTargetJointVelocity); + + return bIsSendSuccessfull; } bool DynamixelHandler::sendTargetJointVelocity(std::vector& vTargetJointVelocity) { - bool bIsSendSuccessfull = false; + bool bIsSendSuccessfull = false; - // checks if the vector size is correct - if (vTargetJointVelocity.size() != NB_JOINTS) - { - std::cout << "[ERROR] (DynamixelHandler::sendTargetJointVelocity): Size of command vector is not correct: " << vTargetJointVelocity.size() << " instead of " << NB_JOINTS << "!" << std::endl; - bIsSendSuccessfull = false; - } + // checks if the vector size is correct + if (vTargetJointVelocity.size() != NB_JOINTS) + { + std::cout << "[ERROR] (DynamixelHandler::sendTargetJointVelocity): Size of command vector is not correct: " << vTargetJointVelocity.size() << " instead of " << NB_JOINTS << "!" << std::endl; + bIsSendSuccessfull = false; + } - for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) - { - int dxl_comm_result = COMM_TX_FAIL; // Communication result - uint8_t dxl_error = 0; - uint16_t dxl_present_position = 0; - dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_VELOCITY, vTargetJointVelocity[l_joint], &dxl_error); - if (dxl_comm_result != COMM_SUCCESS) - { - //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; - bIsSendSuccessfull = false; - } - else if (dxl_error != 0) - { - //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; - bIsSendSuccessfull = false; - } - else - { - bIsSendSuccessfull = true; - } - } - return bIsSendSuccessfull; + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + uint16_t dxl_present_position = 0; + dxl_comm_result = m_pPacketHandler->write2ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_GOAL_VELOCITY, vTargetJointVelocity[l_joint], &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsSendSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] (DynamixelHandler::sendTargetJointPosition): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsSendSuccessfull = false; + } + else + { + bIsSendSuccessfull = true; + } + } + return bIsSendSuccessfull; } bool DynamixelHandler::enableTorque(bool bEnableTorque) { - bool bIsSendSuccessfull = false; - - for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) - { - int dxl_comm_result = COMM_TX_FAIL; // Communication result - uint8_t dxl_error = 0; - - dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_TORQUE_ENABLE, bEnableTorque, &dxl_error); - if (dxl_comm_result != COMM_SUCCESS) - { - //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; - bIsSendSuccessfull = false; - } - else if (dxl_error != 0) - { - //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; - bIsSendSuccessfull = false; - } - else - { - bIsSendSuccessfull = true; - } - } - return bIsSendSuccessfull; + bool bIsSendSuccessfull = false; + + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + + dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_TORQUE_ENABLE, bEnableTorque, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsSendSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsSendSuccessfull = false; + } + else + { + bIsSendSuccessfull = true; + } + } + return bIsSendSuccessfull; } bool DynamixelHandler::setControlMode(int iControlMode) { - bool bIsSendSuccessfull = false; - - for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) - { - int dxl_comm_result = COMM_TX_FAIL; // Communication result - uint8_t dxl_error = 0; - - dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_CONTROL_MODE, iControlMode, &dxl_error); - if (dxl_comm_result != COMM_SUCCESS) - { - //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; - bIsSendSuccessfull = false; - } - else if (dxl_error != 0) - { - //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; - bIsSendSuccessfull = false; - } - else - { - bIsSendSuccessfull = true; - } - } - return bIsSendSuccessfull; + bool bIsSendSuccessfull = false; + + for (unsigned int l_joint = 0; l_joint < NB_JOINTS; l_joint++) + { + int dxl_comm_result = COMM_TX_FAIL; // Communication result + uint8_t dxl_error = 0; + + dxl_comm_result = m_pPacketHandler->write1ByteTxRx(m_pPortHandler, l_joint + 1, ADDR_XL320_CONTROL_MODE, iControlMode, &dxl_error); + if (dxl_comm_result != COMM_SUCCESS) + { + //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getTxRxResult(dxl_comm_result) << std::endl; + bIsSendSuccessfull = false; + } + else if (dxl_error != 0) + { + //std::cout << "[ERROR] (DynamixelHandler::enableTorque): " << m_pPacketHandler->getRxPacketError(dxl_error) << std::endl; + bIsSendSuccessfull = false; + } + else + { + bIsSendSuccessfull = true; + } + } + return bIsSendSuccessfull; } diff --git a/src/Kinematics.cpp b/src/Kinematics.cpp index 9c154fc..9a56904 100644 --- a/src/Kinematics.cpp +++ b/src/Kinematics.cpp @@ -1,161 +1,48 @@ #include "Kinematics.h" - float deg2rad(float angle) { - return angle/180.0*M_PI; + return angle / 180.0 * M_PI; } float rad2deg(float angle) { - return angle*180.0/M_PI; + return angle * 180.0 / M_PI; } -std::vector computeForwardKinematics(float q1, float q2, float L1, float L2) +std::vector 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 X; - X.push_back(x); - X.push_back(y); - - return X; + 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 X = {x, y, z}; + return X; } -std::vector computeInverseKinematics(float x, float y, float L1, float L2) +std::vector computeInverseKinematics(float x, float y) { - std::vector 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; - 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); - } - - return qi; + std::vector qi; + + float q1 = atan2(y, x); + 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); + qi.push_back(q3); + qi.push_back(q4); + + return qi; } - -std::vector computeDifferentialKinematics(float q1, float q2, float L1, float L2) -{ - std::vector 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 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(0, 0) = vJacobianMatrix[0]; - oJacobianMatrix.at(0, 1) = vJacobianMatrix[1]; - oJacobianMatrix.at(1, 0) = vJacobianMatrix[2]; - oJacobianMatrix.at(1, 1) = vJacobianMatrix[3]; - std::cout << "=====Jacobian Matrix=====" << std::endl; - std::cout << "[ " << oJacobianMatrix.at(0,0) << ", " << oJacobianMatrix.at(0,1) << " ]" << std::endl; - std::cout << "[ " << oJacobianMatrix.at(1,0) << ", " << oJacobianMatrix.at(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(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(0,0) << ", " << oJacobianInverse.at(0,1) << " ]" << std::endl; - std::cout << "[ " << oJacobianInverse.at(1,0) << ", " << oJacobianInverse.at(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 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(0, 0) = vJacobianMatrix[0]; - oJacobianMatrix.at(0, 1) = vJacobianMatrix[1]; - oJacobianMatrix.at(1, 0) = vJacobianMatrix[2]; - oJacobianMatrix.at(1, 1) = vJacobianMatrix[3]; - std::cout << "=====Jacobian Matrix=====" << std::endl; - std::cout << "[ " << oJacobianMatrix.at(0,0) << ", " << oJacobianMatrix.at(0,1) << " ]" << std::endl; - std::cout << "[ " << oJacobianMatrix.at(1,0) << ", " << oJacobianMatrix.at(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(0,0) << ", " << oJacobianInverse.at(0,1) << " ]" << std::endl; - std::cout << "[ " << oJacobianInverse.at(1,0) << ", " << oJacobianInverse.at(1,1) << " ]" << std::endl; - } - else - 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/RobotServoing.cpp b/src/RobotServoing.cpp index cb715d3..aad7f1f 100644 --- a/src/RobotServoing.cpp +++ b/src/RobotServoing.cpp @@ -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 qi = computeInverseKinematics(x, y); + computeForwardKinematics(qi[0], qi[1], qi[2], qi[3]); - std::vector qi = computeInverseKinematics(x, y, L, L4); - - if (qi.size() >= 3) - { - std::vector vTargetJointPosition; - vTargetJointPosition.push_back(qi[1]); - vTargetJointPosition.push_back(qpen); - vTargetJointPosition.push_back(qi[2]); + if (qi.size() == 4) { + vector 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; } diff --git a/src/robotest.cpp b/src/robotest.cpp new file mode 100644 index 0000000..b9437b1 --- /dev/null +++ b/src/robotest.cpp @@ -0,0 +1,69 @@ +#include +#include +#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 qi = computeInverseKinematics(x, y, L1, L2, L3); + + if (qi.size() >= 4) + { + vector 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; +}