From 57dd8e502177557f1983b11fb92abc11e051fe22 Mon Sep 17 00:00:00 2001 From: ros Date: Mon, 31 Mar 2025 14:01:45 +0200 Subject: [PATCH] Added Starting Code for Lab 1. --- include/DynamixelHandler.h | 88 ++++++++ include/ImageProcessing.h | 26 +++ include/Kinematics.h | 16 ++ makefile | 19 +- src/DynamixelHandler.cpp | 407 +++++++++++++++++++++++++++++++++++++ src/ImageProcessing.cpp | 149 ++++++++++++++ src/Kinematics.cpp | 90 ++++++++ src/jointControl.cpp | 74 +++++++ 8 files changed, 868 insertions(+), 1 deletion(-) create mode 100644 include/DynamixelHandler.h create mode 100644 include/ImageProcessing.h create mode 100644 include/Kinematics.h create mode 100644 src/DynamixelHandler.cpp create mode 100644 src/ImageProcessing.cpp create mode 100644 src/Kinematics.cpp create mode 100644 src/jointControl.cpp diff --git a/include/DynamixelHandler.h b/include/DynamixelHandler.h new file mode 100644 index 0000000..da1a9a6 --- /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/ImageProcessing.h b/include/ImageProcessing.h new file mode 100644 index 0000000..210aca3 --- /dev/null +++ b/include/ImageProcessing.h @@ -0,0 +1,26 @@ +#include +#include + +#include "opencv2/imgcodecs.hpp" +#include "opencv2/highgui.hpp" +#include "opencv2/imgproc.hpp" + +#include "DynamixelHandler.h" +#include "Kinematics.h" + + +// ImageProcessing +#define thresholdValue 200 +#define maxValue 255 +//ConvertContours +#define drawingAreaWidthInMm 80.0 +#define drawingAreaHeigthInMm 50.0 +// SendContours +#define deltaTBetweenSamples 500 //in ms +#define incrementBetweenSamples 2 + +std::vector> imageProcessing(cv::Mat originalImage); + +std::vector> convertContoursPixel2Mm(std::vector> vContoursInPixel, float theta, float tx, float ty, int imageWidth, int imageHeight); + +void sendContours(std::vector> vContoursInPixel, std::vector> vContoursInMm, DynamixelHandler& dxlHandler, float L1, float L2); diff --git a/include/Kinematics.h b/include/Kinematics.h new file mode 100644 index 0000000..246781a --- /dev/null +++ b/include/Kinematics.h @@ -0,0 +1,16 @@ +#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); + +int computeJacobianMatrixRank(std::vector vJacobianMatrix, float threshold); + +cv::Mat computeInverseJacobianMatrix(std::vector vJacobianMatrix); diff --git a/makefile b/makefile index 9daeafb..f7500c4 100644 --- a/makefile +++ b/makefile @@ -1 +1,18 @@ -test +all: kinematics dynamics joint + g++ -o bin/jointControl lib/jointControl.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4` + +kinematics: src/Kinematics.cpp + g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4 + +joint: src/jointControl.cpp + g++ -c src/jointControl.cpp -o lib/jointControl.o -I./include -I/usr/include/opencv4 + +image: src/ImageProcessing.cpp + g++ -c src/ImageProcessing.cpp -o lib/ImageProcessing.o -I./include -I/usr/include/opencv4 + +dynamics: src/DynamixelHandler.cpp + g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include + +clean: + rm lib/*.o + rm bin/* \ No newline at end of file diff --git a/src/DynamixelHandler.cpp b/src/DynamixelHandler.cpp new file mode 100644 index 0000000..a97b5fd --- /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/ImageProcessing.cpp b/src/ImageProcessing.cpp new file mode 100644 index 0000000..e5d8e66 --- /dev/null +++ b/src/ImageProcessing.cpp @@ -0,0 +1,149 @@ +#include "ImageProcessing.h" + +std::vector> imageProcessing(cv::Mat img_original) +{ + // displays the original image + cv::imshow( "Original", img_original ); + + // converts the image to grayscale + cv::Mat img_gray; + cv::cvtColor( img_original, img_gray, cv::COLOR_BGR2GRAY ); + + // displays the grayscale image + cv::imshow( "Grayscale", img_gray ); + + cv::Mat img_threshold; + cv::threshold(img_gray, img_threshold, thresholdValue, maxValue, cv::THRESH_BINARY); + + if (false) + { + // blurs it + cv::Mat img_blur; + blur( img_gray, img_blur, cv::Size(3,3) ); + + // displays the blurred image + cv::imshow( "Blur", img_blur ); + + // applies Canny algorithm on it + cv::Mat canny_output; + cv::Canny( img_blur, canny_output, thresholdValue, maxValue ); + + // displays the canny image + cv::imshow( "Canny", canny_output ); + } + + // finds contours on the canny image + std::vector > contours; + std::vector hierarchy; + cv::findContours( img_threshold, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE ); + //cv::findContours( canny_output, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE ); + + // displays contours on the original image + cv::RNG rng(12345); + //cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 ); + cv::Mat drawing = cv::Mat::zeros( img_threshold.size(), CV_8UC3 ); + for( size_t i = 0; i< contours.size(); i++ ) + { + cv::Scalar color = cv::Scalar( rng.uniform(0, 256), rng.uniform(0,256), rng.uniform(0,256) ); + cv::drawContours( drawing, contours, (int)i, color, 2, cv::LINE_8, hierarchy, 0 ); + } + cv::imshow( "Contours", drawing ); + + return contours; +} + +std::vector> convertContoursPixel2Mm(std::vector> vContoursInPixel, float theta, float tx, float ty, int imageWidth, int imageHeight) +{ + std::vector> vContoursInMm; + + // pixel / mm ratios + float widthRatio = (float)imageWidth / drawingAreaWidthInMm; + float heigthRatio = (float)imageHeight / drawingAreaHeigthInMm; + float appliedRatio = ceil((widthRatio>=heigthRatio) ? widthRatio : heigthRatio); + + // image to real world reference frame transformation + cv::Mat transformationMatrix(3, 3, CV_32FC1); + transformationMatrix.at(0,0) = cos(theta); transformationMatrix.at(0,1) = -sin(theta); transformationMatrix.at(0,2) = tx; + transformationMatrix.at(1,0) = sin(theta); transformationMatrix.at(1,1) = cos(theta); transformationMatrix.at(1,2) = ty; + transformationMatrix.at(2,0) = 0.0; transformationMatrix.at(2,1) = 0.0; transformationMatrix.at(2,2) = 1.0; + + // size the output vector with the same number of contours as the input one + vContoursInMm.resize(vContoursInPixel.size()); + + // fill the output vector by transforming the data + for (int l_contour = 0; l_contour < vContoursInPixel.size(); l_contour++) + { + std::cout << "[INFO] (convertContoursPixel2Mm) contour #" << l_contour << " , # of pts = " << vContoursInPixel[l_contour].size()<< std::endl; + for (int l_point = 0; l_point < vContoursInPixel[l_contour].size(); l_point++) + { + // get the coordinate of the current point in the pixel reference frame + cv::Mat pointCoordinateInPixel(3, 1, CV_32FC1); + pointCoordinateInPixel.at(0,0) = vContoursInPixel[l_contour][l_point].y/ appliedRatio; //!! WATCH OUT Points go (x,y); (width,height) + pointCoordinateInPixel.at(1,0) = vContoursInPixel[l_contour][l_point].x/ appliedRatio; + pointCoordinateInPixel.at(2,0) = 1.0; + + // transform it in the real world reference frame + cv::Mat pointCoordinateInMm(3, 1, CV_32FC1); + pointCoordinateInMm = transformationMatrix * pointCoordinateInPixel; + + // store the data in the contour in mm + vContoursInMm[l_contour].push_back(pointCoordinateInMm.at(0,0) ); + vContoursInMm[l_contour].push_back(pointCoordinateInMm.at(1,0) ); + } + } + + return vContoursInMm; + +} + +void sendContours(std::vector> vContoursInPixel, std::vector> vContoursInMm, DynamixelHandler& dxlHandler, float L1, float L2) +{ + if (vContoursInPixel.size() == 0 || vContoursInMm.size() == 0) + { + std::cout << "[ERROR](sendContours) Contour vector is empty!" << std::endl; + return; + } + + cv::Mat drawing = cv::Mat::zeros( 240, 240, CV_8UC3 ); + cv::Scalar color = cv::Scalar( 0, 0, 255 ); + cv::imshow( "Current contour", drawing ); + + float q_pen = deg2rad(-70.0f); + + for (int l_contour = 0; l_contour < vContoursInMm.size(); l_contour++) + { + q_pen = deg2rad(-70.0f); + std::cout << "[INFO] (sendContours) contour #" << l_contour << " , # of pts = " << vContoursInMm[l_contour].size()<< std::endl; + for (int l_point = 0; l_point < vContoursInMm[l_contour].size()/2; l_point=l_point+incrementBetweenSamples) + { + std::cout << "l_point #" << l_point << std::endl; + float targetPointX = vContoursInMm[l_contour][2*l_point]/10; // in cm + float targetPointY = vContoursInMm[l_contour][2*l_point+1]/10; // in cm + + float targetPointI = vContoursInPixel[l_contour][l_point].x; + float targetPointJ = vContoursInPixel[l_contour][l_point].y; + + // Plots a dot at the current point position + cv::circle(drawing, cv::Point(targetPointI ,targetPointJ), 0, cv::Scalar(0,0,255), cv::FILLED, cv::LINE_8); + cv::imshow( "Current contour", drawing ); + std::cout<<"CurrentPoint = (" << targetPointJ << "," << targetPointI << ") --> (" << targetPointX << "," << targetPointY << ")" << std::endl; + + // IK + std::vector qi = computeInverseKinematics(targetPointX, targetPointY, L1, L2); + if (qi.size() > 3) + { + std::vector l_vTargetJointPosition; + // q1 + l_vTargetJointPosition.push_back(qi[1]); + // fixed joint + l_vTargetJointPosition.push_back(q_pen); + // q2 + l_vTargetJointPosition.push_back(qi[2]); + + dxlHandler.sendTargetJointPosition(l_vTargetJointPosition); + } + q_pen = deg2rad(-90.0f); + cv::waitKey(deltaTBetweenSamples); + } + } +} diff --git a/src/Kinematics.cpp b/src/Kinematics.cpp new file mode 100644 index 0000000..0ef3916 --- /dev/null +++ b/src/Kinematics.cpp @@ -0,0 +1,90 @@ +#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; +} + +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/jointControl.cpp b/src/jointControl.cpp new file mode 100644 index 0000000..1a00795 --- /dev/null +++ b/src/jointControl.cpp @@ -0,0 +1,74 @@ +#include +#include + +#include "Kinematics.h" +#include "DynamixelHandler.h" + +// Global variables +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(); +} + + +int main(int argc, char** argv) +{ + if (argc == 6) + { + // Retrieves the args and stores them into variables + float L1 = atof(argv[1]); // in cm + float L2 = atof(argv[2]); // in cm + float q1 = deg2rad(atof(argv[3])); // in rad + float q2 = deg2rad(atof(argv[4])); // in rad + float qpen = deg2rad(atof(argv[5])); // in rad + + // Initializes the robot + initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate); + + // Computes FK + computeForwardKinematics(q1, q2, L1, L2); + + // Computes Differential Kinematics + //std::vector vJacobianMatrix = computeDifferentialKinematics(q1, q2, L1, L2); + //int rank = computeJacobianMatrixRank(vJacobianMatrix, 0.1); + + // Sends the target joint values received as args + std::vector vTargetJointPosition; + vTargetJointPosition.push_back(q1); + vTargetJointPosition.push_back(qpen); + vTargetJointPosition.push_back(q2); + _oDxlHandler.sendTargetJointPosition(vTargetJointPosition); + //std::cout << "vTargetJointPosition= " << vTargetJointPosition[0] << ", " << vTargetJointPosition[1] << ", " << vTargetJointPosition[2]<< std::endl; + // Waits 2s + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + + // Closes robot connection + _oDxlHandler.closePort(); + } + else + { + std::cout << "[WARNING] Joint control"<< std::endl; + std::cout << ">> jointControl L1(cm) L2(cm) q1(deg) q2(deg) qpen(deg)"<< std::endl; + } + + return 0; +} \ No newline at end of file