diff --git a/bin/CameraOrientationTracking b/bin/CameraOrientationTracking index d29f141..3f610c4 100755 Binary files a/bin/CameraOrientationTracking and b/bin/CameraOrientationTracking differ diff --git a/bin/RedBallDetection b/bin/RedBallDetection index cae72c4..d0ca646 100755 Binary files a/bin/RedBallDetection and b/bin/RedBallDetection differ diff --git a/bin/RedBallTracking b/bin/RedBallTracking index 5e08137..1a28300 100755 Binary files a/bin/RedBallTracking and b/bin/RedBallTracking differ diff --git a/bin/RobotServoing b/bin/RobotServoing new file mode 100755 index 0000000..e485a3c Binary files /dev/null and b/bin/RobotServoing differ diff --git a/data/camera_chessboard_calibration_params.xml b/data/camera_chessboard_calibration_params.xml index 8fdbebb..c87503e 100644 --- a/data/camera_chessboard_calibration_params.xml +++ b/data/camera_chessboard_calibration_params.xml @@ -11,42 +11,42 @@ 0. 184. 92. 0. 0. 115. 0. 23. 115. 0. 46. 115. 0. 69. 115. 0. 92. 115. 0. 115. 115. 0. 138. 115. 0. 161. 115. 0. 184. 115. 0. - 5.88612000e+02 2.15689667e+02 5.54606873e+02 2.15611755e+02 - 5.20468445e+02 2.15258972e+02 4.85772522e+02 2.15278687e+02 - 4.51321503e+02 2.15120773e+02 4.16495636e+02 2.14783768e+02 - 3.81401917e+02 2.14718674e+02 3.46348175e+02 2.14802200e+02 - 3.10953491e+02 2.15044067e+02 5.88692505e+02 1.81895676e+02 - 5.54564270e+02 1.81450623e+02 5.20432983e+02 1.80957260e+02 - 4.85783722e+02 1.80860062e+02 4.51179932e+02 1.80639709e+02 - 4.16396912e+02 1.80453308e+02 3.81144226e+02 1.80382690e+02 - 3.45956238e+02 1.80322159e+02 3.10507690e+02 1.80347870e+02 - 5.88607361e+02 1.48160278e+02 5.54770813e+02 1.47601761e+02 - 5.20398499e+02 1.47191406e+02 4.85810608e+02 1.46545593e+02 - 4.50779694e+02 1.46543564e+02 4.15969879e+02 1.46196732e+02 - 3.80619965e+02 1.45882202e+02 3.45383270e+02 1.45875610e+02 - 3.09773987e+02 1.46258408e+02 5.89240051e+02 1.14398933e+02 - 5.54773438e+02 1.13560555e+02 5.20508728e+02 1.13239220e+02 - 4.85565887e+02 1.12632996e+02 4.50695831e+02 1.12478821e+02 - 4.15567413e+02 1.12141571e+02 3.80236389e+02 1.11906654e+02 - 3.44660980e+02 1.11700226e+02 3.09211731e+02 1.11827919e+02 - 5.89326904e+02 8.11330795e+01 5.55244568e+02 8.04520645e+01 - 5.20487244e+02 7.96654358e+01 4.85463074e+02 7.93521194e+01 - 4.50391876e+02 7.91115799e+01 4.15237152e+02 7.86507416e+01 - 3.79536346e+02 7.84776230e+01 3.43838593e+02 7.84099808e+01 - 3.08281067e+02 7.82369461e+01 5.89964478e+02 4.79190063e+01 - 5.55360474e+02 4.72442245e+01 5.20573486e+02 4.66231613e+01 - 4.85435333e+02 4.63138351e+01 4.50215057e+02 4.57072220e+01 - 4.14667877e+02 4.53788414e+01 3.78956207e+02 4.50921440e+01 - 3.43222778e+02 4.47188683e+01 3.07135529e+02 4.45427628e+01 + 2.45399948e+02 3.96526428e+02 2.75491119e+02 3.96743744e+02 + 3.05529846e+02 3.97404755e+02 3.35598389e+02 3.97747894e+02 + 3.65435516e+02 3.98247070e+02 3.95277130e+02 3.98678436e+02 + 4.24790131e+02 3.99188873e+02 4.54343109e+02 3.99793427e+02 + 4.83617188e+02 4.00341827e+02 2.43551071e+02 4.26507141e+02 + 2.73815063e+02 4.26920990e+02 3.04072540e+02 4.27350433e+02 + 3.34468323e+02 4.27689362e+02 3.64420837e+02 4.28136353e+02 + 3.94441345e+02 4.28616119e+02 4.24255005e+02 4.28880920e+02 + 4.53849121e+02 4.29541565e+02 4.83251129e+02 4.30148285e+02 + 2.41581970e+02 4.56861023e+02 2.72229858e+02 4.57136444e+02 + 3.02617950e+02 4.57337982e+02 3.33223389e+02 4.57670135e+02 + 3.63343964e+02 4.58243011e+02 3.93526886e+02 4.58544525e+02 + 4.23598541e+02 4.59073547e+02 4.53499939e+02 4.59387024e+02 + 4.83260071e+02 4.59713501e+02 2.39441559e+02 4.87543976e+02 + 2.70317017e+02 4.87674408e+02 3.00955750e+02 4.87926483e+02 + 3.31665344e+02 4.88370178e+02 3.62285126e+02 4.88526459e+02 + 3.92531311e+02 4.88930115e+02 4.22826599e+02 4.89222931e+02 + 4.52897003e+02 4.89462860e+02 4.82814758e+02 4.89736603e+02 + 2.36905212e+02 5.18513794e+02 2.68179993e+02 5.18365417e+02 + 2.99221039e+02 5.18484436e+02 3.30290558e+02 5.18595032e+02 + 3.61055054e+02 5.18633667e+02 3.91492371e+02 5.19132996e+02 + 4.22201019e+02 5.19357239e+02 4.52542633e+02 5.19609009e+02 + 4.82629883e+02 5.19963745e+02 2.33913452e+02 5.49964966e+02 + 2.65571899e+02 5.49619385e+02 2.97115417e+02 5.49631531e+02 + 3.28535370e+02 5.49592285e+02 3.59526276e+02 5.49615601e+02 + 3.90559509e+02 5.49705688e+02 4.21580505e+02 5.49908875e+02 + 4.52188660e+02 5.50283813e+02 4.82645752e+02 5.50572266e+02 4 4
d
- -9.9999815881189502e-01 1.2697535215647785e-03 - 1.4387837969518850e-03 1.2288328754052873e+02 - -1.2718960354282296e-03 -9.9999808231089615e-01 - -1.4891793059053041e-03 -2.7774165634818203e+01 - 1.4368901471439603e-03 -1.4910065474532613e-03 - 9.9999785612079128e-01 5.2883271790406673e+02 0. 0. 0. 1. + 9.9988271709700483e-01 -8.6495461050344179e-03 + -1.2638726315885510e-02 -1.2258087320730928e+02 + 1.2737430271251139e-02 9.2786987434153378e-01 3.7268653605880536e-01 + 1.0601197521882341e+02 8.5035240221915127e-03 + -3.7280381119511524e-01 9.2787122405946010e-01 + 6.1627442399943686e+02 0. 0. 0. 1. diff --git a/data/color_params.xml b/data/color_params.xml index fcedfef..31598ee 100644 --- a/data/color_params.xml +++ b/data/color_params.xml @@ -1,9 +1,9 @@ -0 -179 -105 +14 +19 +119 255 -210 +0 255 diff --git a/data/color_params_data.xml b/data/color_params_data.xml new file mode 100644 index 0000000..31598ee --- /dev/null +++ b/data/color_params_data.xml @@ -0,0 +1,9 @@ + + +14 +19 +119 +255 +0 +255 + diff --git a/data/color_params_data2.xml b/data/color_params_data2.xml new file mode 100644 index 0000000..fcedfef --- /dev/null +++ b/data/color_params_data2.xml @@ -0,0 +1,9 @@ + + +0 +179 +105 +255 +210 +255 + 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/Kinematics.h b/include/Kinematics.h new file mode 100644 index 0000000..04fc5e1 --- /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/CameraOrientationTracking.o b/lib/CameraOrientationTracking.o index 69556e8..a6feb47 100644 Binary files a/lib/CameraOrientationTracking.o and b/lib/CameraOrientationTracking.o differ diff --git a/lib/DynamixelHandler.o b/lib/DynamixelHandler.o new file mode 100644 index 0000000..eb89afc 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/RedBallDetection.o b/lib/RedBallDetection.o index 7ebbe51..1fdfd1d 100644 Binary files a/lib/RedBallDetection.o and b/lib/RedBallDetection.o differ diff --git a/lib/RobotServoing.o b/lib/RobotServoing.o new file mode 100644 index 0000000..24d977b Binary files /dev/null and b/lib/RobotServoing.o differ diff --git a/makefile b/makefile index 2e0036e..53d177c 100644 --- a/makefile +++ b/makefile @@ -1,23 +1,33 @@ -all: ReadWrite MonoCamCalib RedBallDetection RedBallTracking CameraOrientationTracking +all: ReadWrite MonoCamCalib RedBallDetection RedBallTracking CameraOrientationTracking Kinematics Dxl Servoing g++ lib/MonoCameraCalibration.o lib/ReadWriteFunctions.o -o bin/MonoCameraCalibration -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++ lib/CameraOrientationTracking.o lib/ReadWriteFunctions.o -o bin/CameraOrientationTracking -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 -lopencv_core -lopencv_highgui -lopencv_imgproc -lopencv_aruco -lopencv_videoio -lopencv_objdetect -lopencv_imgcodecs -lopencv_features2d -lopencv_calib3d + g++ lib/RedBallTracking.o -o bin/RedBallTracking -L/usr/lib/x86_64-linux-gnu -lopencv_core -lopencv_highgui -lopencv_imgproc -lopencv_aruco -lopencv_videoio -lopencv_objdetect -lopencv_imgcodecs -lopencv_features2d -lopencv_calib3d + g++ lib/CameraOrientationTracking.o lib/ReadWriteFunctions.o -o bin/CameraOrientationTracking -L/usr/lib/x86_64-linux-gnu -lopencv_core -lopencv_highgui -lopencv_imgproc -lopencv_aruco -lopencv_videoio -lopencv_objdetect -lopencv_imgcodecs -lopencv_features2d -lopencv_calib3d + 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` MonoCamCalib: src/MonoCameraCalibration.cpp g++ -c src/MonoCameraCalibration.cpp -o lib/MonoCameraCalibration.o -I./include -I/usr/include/opencv4 - + ReadWrite: src/ReadWriteFunctions.cpp g++ -c src/ReadWriteFunctions.cpp -o lib/ReadWriteFunctions.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 CameraOrientationTracking: src/CameraOrientationTracking.cpp g++ -c src/CameraOrientationTracking.cpp -o lib/CameraOrientationTracking.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 + +Dxl: src/DynamixelHandler.cpp + g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include + +Servoing: src/RobotServoing.cpp + g++ -c src/RobotServoing.cpp -o lib/RobotServoing.o -I./include -I/usr/include/opencv4 clean: rm lib/*.o diff --git a/src/CameraOrientationTracking.cpp b/src/CameraOrientationTracking.cpp index fb2f0ee..275e261 100644 --- a/src/CameraOrientationTracking.cpp +++ b/src/CameraOrientationTracking.cpp @@ -12,7 +12,7 @@ #define CAM_PARAMS_FILENAME "./data/camera_calibration_params.xml" #define CAM_CHESSBOARD_CALIB_PARAMS_FILENAME "./data/camera_chessboard_calibration_params.xml" -#define FPS 5.0 +#define FPS 30.0 #define BOARDSIZE_WIDTH 9 #define BOARDSIZE_HEIGHT 6 #define WIN_SIZE 11 @@ -229,4 +229,4 @@ int main( int argc, char** argv ) } return 0; -} \ 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/Kinematics.cpp b/src/Kinematics.cpp new file mode 100644 index 0000000..db043c4 --- /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/RedBallDetection.cpp b/src/RedBallDetection.cpp index 4b92ed6..d7d97ef 100644 --- a/src/RedBallDetection.cpp +++ b/src/RedBallDetection.cpp @@ -10,8 +10,8 @@ #include #define CAM_PARAMS_FILENAME "./data/camera_calibration_params.xml" -//#define COLOR_PARAMS_FILENAME "./data/color_params.xml" -#define COLOR_PARAMS_FILENAME "./data/color_params_RGB.xml" +#define COLOR_PARAMS_FILENAME "./data/color_params.xml" +//#define COLOR_PARAMS_FILENAME "./data/color_params_RGB.xml" #define FPS 30.0 #define STRUCTURAL_ELEMENTS_SIZE 5 #define RESOLUTION_MAX 800 @@ -31,8 +31,8 @@ bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & di return true; } -//bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV) -bool writeColorParameters(std::string filename, int iLowR, int iHighR, int iLowG, int iHighG, int iLowB, int iHighB) +bool writeColorParameters(std::string filename, int iLowH, int iHighH, int iLowS, int iHighS, int iLowV, int iHighV) +//bool writeColorParameters(std::string filename, int iLowR, int iHighR, int iLowG, int iHighG, int iLowB, int iHighB) { cv::FileStorage fs(filename, cv::FileStorage::WRITE); if (!fs.isOpened()) @@ -41,7 +41,7 @@ bool writeColorParameters(std::string filename, int iLowR, int iHighR, int iLowG return false; } - /* + fs << "lowH" << iLowH; fs << "highH" << iHighH; @@ -52,7 +52,7 @@ bool writeColorParameters(std::string filename, int iLowR, int iHighR, int iLowG fs << "lowV" << iLowV; fs << "highV" << iHighV; - */ + /* fs << "lowR" << iLowR; fs << "highR" << iHighR; @@ -62,7 +62,7 @@ bool writeColorParameters(std::string filename, int iLowR, int iHighR, int iLowG fs << "lowB" << iLowB; fs << "highB" << iHighB; - + */ // releases the writer fs.release(); @@ -141,7 +141,7 @@ int main(int argc, char** argv) cv::namedWindow("Control", cv::WINDOW_AUTOSIZE); //create a window called "Control" // sets min/max value for HSV color representation - /* + int iLowH = 0; int iHighH = 179; @@ -150,7 +150,7 @@ int main(int argc, char** argv) int iLowV = 0; int iHighV = 255; - */ + /* int iLowR = 0; int iHighR = 255; @@ -159,7 +159,8 @@ int main(int argc, char** argv) int iLowB = 0; int iHighB = 255; -/* + */ + // creates trackbars in "Control" window cv::createTrackbar("LowH", "Control", &iLowH, 179); //Hue (0 - 179) cv::createTrackbar("HighH", "Control", &iHighH, 179); @@ -169,7 +170,7 @@ int main(int argc, char** argv) cv::createTrackbar("LowV", "Control", &iLowV, 255); //Value (0 - 255) cv::createTrackbar("HighV", "Control", &iHighV, 255); - */ + /* // creates trackbars in "Control" window cv::createTrackbar("LowR", "Control", &iLowR, 255); //Red (0 - 255) cv::createTrackbar("HighR", "Control", &iHighR, 255); @@ -179,7 +180,7 @@ int main(int argc, char** argv) cv::createTrackbar("LowB", "Control", &iLowB, 255); //Blue (0 - 255) cv::createTrackbar("HighB", "Control", &iHighB, 255); - + */ while (true) @@ -206,8 +207,8 @@ int main(int argc, char** argv) //Threshold the image based on the trackbar values cv::Mat imgThresholded; - //inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded); - inRange(imgOriginal, cv::Scalar(iLowR, iLowG, iLowB), cv::Scalar(iHighR, iHighG, iHighB), imgThresholded); + inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded); + //inRange(imgOriginal, cv::Scalar(iLowR, iLowG, iLowB), cv::Scalar(iHighR, iHighG, iHighB), imgThresholded); //morphological opening (remove small objects from the foreground) cv::erode(imgThresholded, imgThresholded, cv::getStructuringElement(cv::MORPH_ELLIPSE, cv::Size(iStructuralElementSize, iStructuralElementSize)) ); @@ -236,8 +237,8 @@ int main(int argc, char** argv) } if (key == 's') { - //writeColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV); - writeColorParameters(sColorParamFilename, iLowR, iHighR, iLowG, iHighG, iLowB, iHighB); + writeColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV); + //writeColorParameters(sColorParamFilename, iLowR, iHighR, iLowG, iHighG, iLowB, iHighB); std::cout << "[INFO] Color parameters saved to file: " << sColorParamFilename << std::endl; } diff --git a/src/RobotServoing.cpp b/src/RobotServoing.cpp new file mode 100644 index 0000000..7a348d5 --- /dev/null +++ b/src/RobotServoing.cpp @@ -0,0 +1,321 @@ +#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/camera_calibration_params.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 +#define HEIGHT2CM 13.63 +#define WIDTH2CM 13.49 +#define RESOLUTION_MAX 800 + +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; + int iMaxVideoResolution = RESOLUTION_MAX; + + // updates main parameters from arguments + int opt; + while ((opt = getopt (argc, argv, ":c:f:s:a:i:p:l:m:r:")) != -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 'r': + iMaxVideoResolution = atoi(optarg); + break; + case '?': + if (optopt == 'c' || optopt == 'f' || optopt == 's' || optopt == 'a' || optopt == 'p' || optopt == 'l' || optopt == 'm' || optopt == 'i' || optopt == 'r') + 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 + + // changes image resolution to maximum (e.g. 1920x1080 if possible) + cap.set(cv::CAP_PROP_FRAME_HEIGHT, iMaxVideoResolution); cap.set(cv::CAP_PROP_FRAME_WIDTH, iMaxVideoResolution); + + // 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); + + // converts posX, posY in mm in the world reference frame + float img_width = imgTmp.size().width; + float img_height = imgTmp.size().height; + float x = (posY-img_height/2)/img_height*HEIGHT2CM; + float y = (posX-img_width/2)/img_width*WIDTH2CM; + + 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; +}