Added full servoeing.
This commit is contained in:
ros
parent
a28b91f204
commit
60ea6f36bd
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
|
|
@ -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.</objPoints>
|
||||
<imagePoints>
|
||||
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</imagePoints>
|
||||
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</imagePoints>
|
||||
<cameraChessboardTransform type_id="opencv-matrix">
|
||||
<rows>4</rows>
|
||||
<cols>4</cols>
|
||||
<dt>d</dt>
|
||||
<data>
|
||||
-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.</data></cameraChessboardTransform>
|
||||
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.</data></cameraChessboardTransform>
|
||||
</opencv_storage>
|
||||
|
|
|
|||
|
|
@ -1,9 +1,9 @@
|
|||
<?xml version="1.0"?>
|
||||
<opencv_storage>
|
||||
<lowH>0</lowH>
|
||||
<highH>179</highH>
|
||||
<lowS>105</lowS>
|
||||
<lowH>14</lowH>
|
||||
<highH>19</highH>
|
||||
<lowS>119</lowS>
|
||||
<highS>255</highS>
|
||||
<lowV>210</lowV>
|
||||
<lowV>0</lowV>
|
||||
<highV>255</highV>
|
||||
</opencv_storage>
|
||||
|
|
|
|||
|
|
@ -0,0 +1,9 @@
|
|||
<?xml version="1.0"?>
|
||||
<opencv_storage>
|
||||
<lowH>14</lowH>
|
||||
<highH>19</highH>
|
||||
<lowS>119</lowS>
|
||||
<highS>255</highS>
|
||||
<lowV>0</lowV>
|
||||
<highV>255</highV>
|
||||
</opencv_storage>
|
||||
|
|
@ -0,0 +1,9 @@
|
|||
<?xml version="1.0"?>
|
||||
<opencv_storage>
|
||||
<lowH>0</lowH>
|
||||
<highH>179</highH>
|
||||
<lowS>105</lowS>
|
||||
<highS>255</highS>
|
||||
<lowV>210</lowV>
|
||||
<highV>255</highV>
|
||||
</opencv_storage>
|
||||
|
|
@ -0,0 +1,88 @@
|
|||
#if defined(__linux__) || defined(__APPLE__)
|
||||
#include <fcntl.h>
|
||||
#include <getopt.h>
|
||||
#include <termios.h>
|
||||
#define STDIN_FILENO 0
|
||||
#elif defined(_WIN32) || defined(_WIN64)
|
||||
#include <conio.h>
|
||||
#endif
|
||||
|
||||
#define _USE_MATH_DEFINES
|
||||
#include <cmath>
|
||||
|
||||
// standard includes
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <string>
|
||||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
// 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<uint16_t>& vCurrentJointPosition);
|
||||
bool readCurrentJointPosition(std::vector<float>& vCurrentJointPosition);
|
||||
bool sendTargetJointPosition(std::vector<uint16_t>& vTargetJointPosition);
|
||||
bool sendTargetJointPosition(std::vector<float>& vTargetJointPosition);
|
||||
bool sendTargetJointVelocity(std::vector<uint16_t>& vTargetJointVelocity);
|
||||
bool sendTargetJointVelocity(std::vector<float>& 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<uint16_t> 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;
|
||||
|
||||
};
|
||||
|
|
@ -0,0 +1,20 @@
|
|||
#define _USE_MATH_DEFINES
|
||||
#include <cmath>
|
||||
#include <vector>
|
||||
#include <iostream>
|
||||
|
||||
#include "opencv2/opencv.hpp"
|
||||
|
||||
float deg2rad(float angle);
|
||||
|
||||
float rad2deg(float angle);
|
||||
|
||||
std::vector<float> computeForwardKinematics(float q1, float q2, float L1, float L2);
|
||||
|
||||
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2);
|
||||
|
||||
std::vector<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2);
|
||||
|
||||
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
|
||||
|
||||
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);
|
||||
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
Binary file not shown.
24
makefile
24
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
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
}
|
||||
|
|
|
|||
|
|
@ -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<float>& vCurrentJointPosition)
|
||||
{
|
||||
// Creates a vector of joint position
|
||||
std::vector<uint16_t> l_vCurrentJointPosition;
|
||||
// Reads the current joint positions in motor command unit
|
||||
bool bIsReadSuccessfull = this->readCurrentJointPosition(l_vCurrentJointPosition);
|
||||
//std::cout << "l_vCurrentJointPosition= " << l_vCurrentJointPosition[0] << ", " << l_vCurrentJointPosition[1] << ", " << l_vCurrentJointPosition[2]<< std::endl;
|
||||
|
||||
// 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<uint16_t>& 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<float>& 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<uint16_t> l_vTargetJointPosition;
|
||||
// q1
|
||||
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q1*vTargetJointPosition[0]));
|
||||
// qpen
|
||||
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_QPEN*vTargetJointPosition[1]));
|
||||
// q2
|
||||
l_vTargetJointPosition.push_back(convertAngleToJointCmd(ROT_DIRECTION_Q2*vTargetJointPosition[2]));
|
||||
|
||||
//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<uint16_t>& 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<float>& 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<uint16_t> l_vTargetJointVelocity;
|
||||
// q1
|
||||
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q1*vTargetJointVelocity[0]));
|
||||
// qpen
|
||||
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_QPEN*vTargetJointVelocity[1]));
|
||||
// q2
|
||||
l_vTargetJointVelocity.push_back(convertJointVelocityToJointCmd(ROT_DIRECTION_Q2*vTargetJointVelocity[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<uint16_t>& 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;
|
||||
}
|
||||
|
|
@ -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<float> 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<float> X;
|
||||
X.push_back(x);
|
||||
X.push_back(y);
|
||||
|
||||
return X;
|
||||
}
|
||||
|
||||
std::vector<float> computeInverseKinematics(float x, float y, float L1, float L2)
|
||||
{
|
||||
std::vector<float> qi;
|
||||
|
||||
float cos_q2 = (x*x+y*y-(L1*L1+L2*L2)) / (2.0 * L1 * L2);
|
||||
|
||||
std::cout << "[INFO] cos_q2= " << cos_q2 << std::endl;
|
||||
|
||||
if (cos_q2 >1 | cos_q2 <-1)
|
||||
{
|
||||
qi.push_back(0.0);
|
||||
std::cout << "[INFO] Inverse Kinematics: No solution!" << std::endl;
|
||||
}
|
||||
else if (cos_q2 == 1)
|
||||
{
|
||||
qi.push_back(1.0);
|
||||
float q1 = atan2(y, x);
|
||||
float q2 = 0;
|
||||
std::cout << "[INFO] Inverse Kinematics: One solution: (x, y)->(q1, q2) = (" << x << ", " << y << ")->(" << rad2deg(q1) << ", " << rad2deg(q2) << ")" << std::endl;
|
||||
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<float> computeDifferentialKinematics(float q1, float q2, float L1, float L2)
|
||||
{
|
||||
std::vector<float> jacobian;
|
||||
|
||||
float j11 = -L2*sin(q1+q2) - L1*sin(q1);
|
||||
float j12 = -L2*sin(q1+q2);
|
||||
float j21 = L2*cos(q1+q2) + L1*cos(q1);
|
||||
float j22 = L2*cos(q1+q2);
|
||||
|
||||
jacobian.push_back(j11);
|
||||
jacobian.push_back(j12);
|
||||
jacobian.push_back(j21);
|
||||
jacobian.push_back(j22);
|
||||
|
||||
return jacobian;
|
||||
}
|
||||
|
||||
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold)
|
||||
{
|
||||
int rank = -1;
|
||||
cv::Mat1f oJacobianMatrix(2, 2);
|
||||
|
||||
if (vJacobianMatrix.size() == 4)
|
||||
{
|
||||
// Converts the Jacobian matrix from std::vector to cv::Mat
|
||||
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
|
||||
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
|
||||
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
|
||||
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
|
||||
std::cout << "=====Jacobian Matrix=====" << std::endl;
|
||||
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << " ]" << std::endl;
|
||||
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << " ]" << std::endl;
|
||||
// Computes the determinant of the Jacobian matrix
|
||||
float determinant = abs(vJacobianMatrix[0] * vJacobianMatrix[3] - vJacobianMatrix[1]*vJacobianMatrix[2]);
|
||||
std::cout << "=====Determinant of the Jacobian matrix=====" << std::endl << determinant << std::endl;
|
||||
// Computes SVD
|
||||
cv::Mat1f w, u, vt;
|
||||
cv::SVD::compute(oJacobianMatrix, w, u, vt);
|
||||
// Finds non zero singular values
|
||||
cv::Mat1f nonZeroSingularValues = w/w.at<float>(0,0) > threshold;
|
||||
// Counts the number of non zero singular values
|
||||
rank = cv::countNonZero(nonZeroSingularValues);
|
||||
std::cout << "=====Rank of the Jacobian matrix=====" << std::endl << rank << " / " << oJacobianMatrix.rows << std::endl;
|
||||
// Determines the inverse of the Jacobian matrix
|
||||
cv::Mat oJacobianInverse = oJacobianMatrix.inv();
|
||||
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
|
||||
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1) << " ]" << std::endl;
|
||||
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << " ]" << std::endl;
|
||||
}
|
||||
else
|
||||
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
|
||||
|
||||
return rank;
|
||||
}
|
||||
|
||||
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix)
|
||||
{
|
||||
cv::Mat1f oJacobianMatrix(2, 2);
|
||||
cv::Mat oJacobianInverse;
|
||||
|
||||
if (vJacobianMatrix.size() == 4)
|
||||
{
|
||||
// Converts the Jacobian matrix from std::vector to cv::Mat
|
||||
oJacobianMatrix.at<float>(0, 0) = vJacobianMatrix[0];
|
||||
oJacobianMatrix.at<float>(0, 1) = vJacobianMatrix[1];
|
||||
oJacobianMatrix.at<float>(1, 0) = vJacobianMatrix[2];
|
||||
oJacobianMatrix.at<float>(1, 1) = vJacobianMatrix[3];
|
||||
std::cout << "=====Jacobian Matrix=====" << std::endl;
|
||||
std::cout << "[ " << oJacobianMatrix.at<float>(0,0) << ", " << oJacobianMatrix.at<float>(0,1) << " ]" << std::endl;
|
||||
std::cout << "[ " << oJacobianMatrix.at<float>(1,0) << ", " << oJacobianMatrix.at<float>(1,1) << " ]" << std::endl;
|
||||
// Determines the inverse of the Jacobian matrix
|
||||
cv::invert(oJacobianMatrix, oJacobianInverse, cv::DECOMP_SVD);
|
||||
//oJacobianInverse = oJacobianMatrix.inv();
|
||||
std::cout << "=====Inverse of the Jacobian Matrix=====" << std::endl;
|
||||
std::cout << "[ " << oJacobianInverse.at<float>(0,0) << ", " << oJacobianInverse.at<float>(0,1) << " ]" << std::endl;
|
||||
std::cout << "[ " << oJacobianInverse.at<float>(1,0) << ", " << oJacobianInverse.at<float>(1,1) << " ]" << std::endl;
|
||||
}
|
||||
else
|
||||
std::cout << "[ERROR] Jacobian matrix has a size of "<< vJacobianMatrix.size() << " instead of 4" << std::endl;
|
||||
|
||||
return oJacobianInverse;
|
||||
}
|
||||
|
|
@ -10,8 +10,8 @@
|
|||
#include <opencv2/calib3d.hpp>
|
||||
|
||||
#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;
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -0,0 +1,321 @@
|
|||
#include <chrono>
|
||||
#include <thread>
|
||||
#include <iostream>
|
||||
#include <ctype.h>
|
||||
#include <stdio.h>
|
||||
#include <stdlib.h>
|
||||
#include <unistd.h>
|
||||
|
||||
#include "opencv2/highgui/highgui.hpp"
|
||||
#include "opencv2/imgproc/imgproc.hpp"
|
||||
#include <opencv2/calib3d.hpp>
|
||||
|
||||
#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<float> 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<float> 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;
|
||||
}
|
||||
Loading…
Reference in New Issue