diff --git a/bin/RobotServoing b/bin/RobotServoing new file mode 100755 index 0000000..f20fda0 Binary files /dev/null and b/bin/RobotServoing differ diff --git a/include/DynamixelHandler.h b/include/DynamixelHandler.h new file mode 100644 index 0000000..c1d4ee6 --- /dev/null +++ b/include/DynamixelHandler.h @@ -0,0 +1,88 @@ +#if defined(__linux__) || defined(__APPLE__) +#include +#include +#include +#define STDIN_FILENO 0 +#elif defined(_WIN32) || defined(_WIN64) +#include +#endif + +#define _USE_MATH_DEFINES +#include + +// standard includes +#include +#include +#include +#include +#include + +// dynamixel sdk include +#include "dynamixel_sdk/dynamixel_sdk.h" + +// addresses of variables in the register +#define ADDR_XL320_CONTROL_MODE 11 +#define ADDR_XL320_TORQUE_ENABLE 24 +#define ADDR_XL320_GOAL_POSITION 30 +#define ADDR_XL320_GOAL_VELOCITY 32 +#define ADDR_XL320_PRESENT_POSITION 37 +#define ADDR_XL320_PRESENT_VELOCITY 39 +#define ADDR_XL320_HARDWARE_ERROR_STATUS 50 + +// rotation direction +#define ROT_DIRECTION_Q1 1 +#define ROT_DIRECTION_Q2 -1 +#define ROT_DIRECTION_QPEN 1 +// nb of joints +#define NB_JOINTS 3 + + +class DynamixelHandler +{ + +public: + DynamixelHandler(); + ~DynamixelHandler(); + +public: + bool openPort(); + void closePort(); + bool setBaudRate(int); + void setDeviceName(std::string); + void setProtocolVersion(float); + bool enableTorque(bool); + bool setControlMode(int iControlMode); + + bool readCurrentJointPosition(std::vector& vCurrentJointPosition); + bool readCurrentJointPosition(std::vector& vCurrentJointPosition); + bool sendTargetJointPosition(std::vector& vTargetJointPosition); + bool sendTargetJointPosition(std::vector& vTargetJointPosition); + bool sendTargetJointVelocity(std::vector& vTargetJointVelocity); + bool sendTargetJointVelocity(std::vector& vTargetJointVelocity); + int convertAngleToJointCmd(float fJointAngle); + float convertJointCmdToAngle(int iJointCmd); + int convertJointVelocityToJointCmd(float fJointVelocity); + +private: + std::string m_sDeviceName; + float m_fProtocolVersion; + int m_i32BaudRate; + + dynamixel::PortHandler* m_pPortHandler; + dynamixel::PacketHandler* m_pPacketHandler; + + bool m_bIsDeviceNameSet; + bool m_bIsProtocolVersionSet; + bool m_bIsPortOpened; + bool m_bIsBaudRateSet; + + int m_i32DxlCommunicationResult; // Communication result + uint8_t m_ui8DxlError; // Dynamixel error + std::vector m_vDxlCurrentPosition; // Present position + + float m_fMinJointCmd = 0; + float m_fMaxJointCmd = 1023; + 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 new file mode 100644 index 0000000..d465457 --- /dev/null +++ b/include/Kinematics.h @@ -0,0 +1,20 @@ +#define _USE_MATH_DEFINES +#include +#include +#include + +#include "opencv2/opencv.hpp" + +float deg2rad(float angle); + +float rad2deg(float angle); + +std::vector computeForwardKinematics(float q1, float q2, float L1, float L2); + +std::vector computeInverseKinematics(float x, float y, float L1, float L2); + +std::vector computeDifferentialKinematics(float q1, float q2, float L1, float L2); + +int computeJacobianMatrixRank(std::vector vJacobianMatrix, float threshold); + +cv::Mat computeInverseJacobianMatrix(std::vector vJacobianMatrix); diff --git a/lib/DynamixelHandler.o b/lib/DynamixelHandler.o new file mode 100644 index 0000000..e6414e1 Binary files /dev/null and b/lib/DynamixelHandler.o differ diff --git a/lib/Kinematics.o b/lib/Kinematics.o new file mode 100644 index 0000000..921b0f6 Binary files /dev/null and b/lib/Kinematics.o differ diff --git a/lib/RobotServoing.o b/lib/RobotServoing.o new file mode 100644 index 0000000..2e073ff Binary files /dev/null and b/lib/RobotServoing.o differ diff --git a/makefile b/makefile index 92845d4..7fc1be0 100644 --- a/makefile +++ b/makefile @@ -1,15 +1,27 @@ -all: CameraCalibration RedBallDetection RedBallTracking +all: CameraCalibration RedBallDetection RedBallTracking kinematics dynamics Robotservoing g++ lib/CameraCalibration.o -o bin/CameraCalibration -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4` g++ lib/RedBallDetection.o -o bin/RedBallDetection -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4` g++ lib/RedBallTracking.o -o bin/RedBallTracking -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4` - + g++ -o bin/RobotServoing lib/RobotServoing.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4` + CameraCalibration: src/CameraCalibration.cpp g++ -c src/CameraCalibration.cpp -o lib/CameraCalibration.o -I./include -I/usr/include/opencv4 + RedBallDetection: src/RedBallDetection.cpp g++ -c src/RedBallDetection.cpp -o lib/RedBallDetection.o -I./include -I/usr/include/opencv4 + RedBallTracking: src/RedBallTracking.cpp g++ -c src/RedBallTracking.cpp -o lib/RedBallTracking.o -I./include -I/usr/include/opencv4 +kinematics: src/Kinematics.cpp + g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4 + +dynamics: src/DynamixelHandler.cpp + g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include + +Robotservoing: src/RobotServoing.cpp + g++ -c src/RobotServoing.cpp -o lib/RobotServoing.o -I./include -I/usr/include/opencv4 + clean: rm lib/*.o rm bin/* diff --git a/src/DynamixelHandler.cpp b/src/DynamixelHandler.cpp new file mode 100644 index 0000000..372d381 --- /dev/null +++ b/src/DynamixelHandler.cpp @@ -0,0 +1,407 @@ +#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) +{ + +} + +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; +} + +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; +} + +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; +} + +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; +} + +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; +} + +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; +} + +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()); +} + +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); +} + +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; +} + +bool DynamixelHandler::readCurrentJointPosition(std::vector& vCurrentJointPosition) +{ + 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; + + 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; +} + +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; +} + +bool DynamixelHandler::sendTargetJointPosition(std::vector& vTargetJointPosition) +{ + 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; + } + + 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; +} + +bool DynamixelHandler::sendTargetJointVelocity(std::vector& vTargetJointVelocity) +{ + 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; + } + + 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 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; +} diff --git a/src/Kinematics.cpp b/src/Kinematics.cpp new file mode 100644 index 0000000..9c154fc --- /dev/null +++ b/src/Kinematics.cpp @@ -0,0 +1,161 @@ +#include "Kinematics.h" + + +float deg2rad(float angle) +{ + return angle/180.0*M_PI; +} + +float rad2deg(float angle) +{ + return angle*180.0/M_PI; +} + +std::vector computeForwardKinematics(float q1, float q2, float L1, float L2) +{ + 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; +} + +std::vector computeInverseKinematics(float x, float y, float L1, float L2) +{ + 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 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 new file mode 100644 index 0000000..fb351b4 --- /dev/null +++ b/src/RobotServoing.cpp @@ -0,0 +1,317 @@ +#include +#include +#include +#include +#include +#include +#include + +#include "opencv2/highgui/highgui.hpp" +#include "opencv2/imgproc/imgproc.hpp" +#include + +#include "Kinematics.h" +#include "DynamixelHandler.h" + +#define CAM_PARAMS_FILENAME "./data/microsoft_livecam_hd3000.xml" +#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml" +#define FPS 30.0 +#define STRUCTURAL_ELEMENTS_SIZE 5 +#define AREA_THRESOLD 1000 +#define ROBOT_L1 5 +#define ROBOT_L2 6 + + +using namespace cv; +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) +{ + std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl; + dxlHandler.setDeviceName(portName); + dxlHandler.setProtocolVersion(protocol); + dxlHandler.openPort(); + dxlHandler.setBaudRate(baudRate); + dxlHandler.enableTorque(true); + std::cout << std::endl; +} + +void closeRobot(DynamixelHandler& dxlHandler) +{ + dxlHandler.enableTorque(false); + dxlHandler.closePort(); +} + +bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open the camera parameter file storage: " << filename << " !"<< std::endl; + return false; + } + + fs["camera_matrix"] >> camMatrix; + fs["distortion_coefficients"] >> distCoeffs; + + return true; +} + +bool readColorParameters(std::string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV) +{ + cv::FileStorage fs(filename, cv::FileStorage::READ); + if (!fs.isOpened()) + { + std::cout << "[ERROR] Could not open the color paramter file storage: " << filename << " !"<< std::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) +{ + // initializes main parameters + float L1 = ROBOT_L1; + float L2 = ROBOT_L2; + float qpen = deg2rad(-90); // in rad + std::string sCameraParamFilename = CAM_PARAMS_FILENAME; + std::string sColorParamFilename = COLOR_PARAMS_FILENAME; + float fFPS = FPS; + int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE; + int iAreaThresold = AREA_THRESOLD; + + // updates main parameters from arguments + int opt; + while ((opt = getopt (argc, argv, ":c:f:s:a:i:p:l:m:")) != -1) + { + switch (opt) + { + case 'c': + sColorParamFilename = optarg; + break; + case 'f': + fFPS = atof(optarg); + break; + case 'p': + qpen = atof(optarg); + break; + case 'l': + L1 = atof(optarg); + break; + case 'm': + L2 = atof(optarg); + break; + case 's': + iStructuralElementSize = atoi(optarg); + break; + case 'a': + iAreaThresold = atoi(optarg); + break; + case 'i': + sCameraParamFilename = optarg; + break; + case '?': + if (optopt == 'c' || optopt == 'f' || optopt == 's' || optopt == 'a' || optopt == 'p' || optopt == 'l' || optopt == 'm' || optopt == 'i') + fprintf (stderr, "Option -%c requires an argument.\n", optopt); + else if (isprint (optopt)) + fprintf (stderr, "Unknown option `-%c'.\n", optopt); + else + fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt); + return 1; + default: + abort (); + } + } + + // Initializes the robot + initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); + + + // reads color parameters from the file storage + int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV; + bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV); + + // checks if the color parameters were successfully read + if (!isColorParamsSet) + { + std::cout << "[ERROR] Color parameters could not be loaded!" << std::endl; + return -1; + } + + // distorted/undistorted image + bool bIsImageUndistorted = true; + + // reads camera intrinsic parameters + cv::Mat cameraMatrix, distCoeffs; + bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs); + + // checks if the camera parameters were successfully read + if (!isCamParamsSet) + { + std::cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << std::endl; + } + + // creates a camera grabber + VideoCapture cap(0, cv::CAP_V4L2); //capture the video from webcam + + // checks if the camera was successfully opened + if ( !cap.isOpened() ) // if not success, exit program + { + cout << "[ERROR] Could not open the camera!" << endl; + return -1; + } + + // inits previous x,y location of the ball + int iLastX = -1; + int iLastY = -1; + + // captures a temporary image from the camera + Mat imgTmp; + cap.read(imgTmp); + + // main loop launched every FPS + while (true) + { + // creates a black image with the size as the camera output + Mat imgLines = Mat::zeros( imgTmp.size(), CV_8UC3 ); + + // reads a new frame from video + cv::Mat imgOriginal; + bool bSuccess = cap.read(imgOriginal); + + // checks if a new frame was grabbed + if (!bSuccess) //if not success, break loop + { + std::cout << "[WARNING] Could not read a new frame from video stream" << std::endl; + break; + } + + if (bIsImageUndistorted && isCamParamsSet) + { + cv::Mat temp = imgOriginal.clone(); + cv::undistort(temp, imgOriginal, cameraMatrix, distCoeffs); + } + + // converts the captured frame from BGR to HSV + cv::Mat imgHSV; + cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV); + + // thresholds the image based on the trackbar values + cv::Mat imgThresholded; + inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded); + + // applies morphological opening (removes small objects from the foreground) + cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); + cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); + + // applies morphological closing (removes small holes from the foreground) + cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); + cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) ); + + // calculates the moments of the thresholded image + Moments oMoments = moments(imgThresholded); + double dM01 = oMoments.m01; + double dM10 = oMoments.m10; + double dArea = oMoments.m00; + + // if the area <= iAreaThresold, considers that the there are no object in the image and it's because of the noise, the area is not zero + int posX, posY; + + if (dArea > iAreaThresold) + { + // calculates the position of the ball + posX = dM10 / dArea; + posY = dM01 / dArea; + + if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0) + { + // draww a red line from the previous point to the current point + line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0,0,255), 2); + } + + // stores the current position for enxt frame + iLastX = posX; + iLastY = posY; + } + + // displays the thresholded image + imshow("Thresholded Image", imgThresholded); + + // adds a cross at the centre of the image + cv::drawMarker(imgOriginal, cv::Point(imgTmp.size().width/2, imgTmp.size().height/2), 10, cv::MARKER_CROSS, cv::LINE_8); + + // shows the original image with the tracking (red) lines + imgOriginal = imgOriginal + imgLines; + imshow("Original", imgOriginal); + float h = 167; + float w = 239; + // converts posX, posY in mm in the world reference frame + float img_width = imgTmp.size().width; + float img_height = imgTmp.size().height; + ///////////////////////////////// + // TODO + float x = (posY - (480/2))*(0.05) + 7; + float y = (posX - (640/2))*(0.05); + + //std::cout << "(pixel -> cm) = (" << posX << ", " << posY << ") - > (" << x << ", " << y << ")" << std::endl; + ///////////////////////////////// + + + // Computes IK + std::vector qi = computeInverseKinematics(x, y, L1, L2); + + // Computes FK + //computeForwardKinematics(qi[1], qi[2], L1, L2); + + // Sends the target joint values received only if there is at least a solution + if (qi.size() >= 3) + { + std::vector vTargetJointPosition; + vTargetJointPosition.push_back(qi[1]); + vTargetJointPosition.push_back(qpen); + vTargetJointPosition.push_back(qi[2]); + _oDxlHandler.sendTargetJointPosition(vTargetJointPosition); + } + + // waits for awhile depending on the FPS value + char key = (char)cv::waitKey(1000.0/fFPS); + // checks if ESC was pressed to exit + if (key == 27) // if 'esc' key is pressed, break loop + { + std::cout << "[INFO] esc key is pressed by user -> Shutting down!" << std::endl; + break; + } + if (key == 'u') + { + bIsImageUndistorted = !bIsImageUndistorted; + std::cout << "[INFO] Image undistorted: " << bIsImageUndistorted<< std::endl; + } + } + + + + + + + // Closes robot connection + _oDxlHandler.closePort(); + + return 0; +}