Finished the project
This commit is contained in:
Thierry Bissem
parent
8426e4722a
commit
cc7842e99d
Binary file not shown.
|
|
@ -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.
14
makefile
14
makefile
|
|
@ -1,15 +1,27 @@
|
|||
all: CameraCalibration RedBallDetection RedBallTracking
|
||||
all: CameraCalibration RedBallDetection RedBallTracking kinematics dynamics Robotservoing
|
||||
g++ lib/CameraCalibration.o -o bin/CameraCalibration -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
g++ lib/RedBallDetection.o -o bin/RedBallDetection -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
g++ lib/RedBallTracking.o -o bin/RedBallTracking -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
g++ -o bin/RobotServoing lib/RobotServoing.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
|
||||
CameraCalibration: src/CameraCalibration.cpp
|
||||
g++ -c src/CameraCalibration.cpp -o lib/CameraCalibration.o -I./include -I/usr/include/opencv4
|
||||
|
||||
RedBallDetection: src/RedBallDetection.cpp
|
||||
g++ -c src/RedBallDetection.cpp -o lib/RedBallDetection.o -I./include -I/usr/include/opencv4
|
||||
|
||||
RedBallTracking: src/RedBallTracking.cpp
|
||||
g++ -c src/RedBallTracking.cpp -o lib/RedBallTracking.o -I./include -I/usr/include/opencv4
|
||||
|
||||
kinematics: src/Kinematics.cpp
|
||||
g++ -c src/Kinematics.cpp -o lib/Kinematics.o -I./include -I/usr/include/opencv4
|
||||
|
||||
dynamics: src/DynamixelHandler.cpp
|
||||
g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include
|
||||
|
||||
Robotservoing: src/RobotServoing.cpp
|
||||
g++ -c src/RobotServoing.cpp -o lib/RobotServoing.o -I./include -I/usr/include/opencv4
|
||||
|
||||
clean:
|
||||
rm lib/*.o
|
||||
rm bin/*
|
||||
|
|
|
|||
|
|
@ -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;
|
||||
}
|
||||
|
|
@ -0,0 +1,317 @@
|
|||
#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/microsoft_livecam_hd3000.xml"
|
||||
#define COLOR_PARAMS_FILENAME "./data/color_params_data.xml"
|
||||
#define FPS 30.0
|
||||
#define STRUCTURAL_ELEMENTS_SIZE 5
|
||||
#define AREA_THRESOLD 1000
|
||||
#define ROBOT_L1 5
|
||||
#define ROBOT_L2 6
|
||||
|
||||
|
||||
using namespace cv;
|
||||
using namespace std;
|
||||
|
||||
DynamixelHandler _oDxlHandler;
|
||||
std::string _robotDxlPortName = "/dev/ttyUSB0";
|
||||
float _robotDxlProtocol = 2.0;
|
||||
int _robotDxlBaudRate = 1000000;
|
||||
|
||||
|
||||
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
{
|
||||
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
|
||||
dxlHandler.setDeviceName(portName);
|
||||
dxlHandler.setProtocolVersion(protocol);
|
||||
dxlHandler.openPort();
|
||||
dxlHandler.setBaudRate(baudRate);
|
||||
dxlHandler.enableTorque(true);
|
||||
std::cout << std::endl;
|
||||
}
|
||||
|
||||
void closeRobot(DynamixelHandler& dxlHandler)
|
||||
{
|
||||
dxlHandler.enableTorque(false);
|
||||
dxlHandler.closePort();
|
||||
}
|
||||
|
||||
bool readCameraParameters(std::string filename, cv::Mat &camMatrix, cv::Mat & distCoeffs)
|
||||
{
|
||||
cv::FileStorage fs(filename, cv::FileStorage::READ);
|
||||
if (!fs.isOpened())
|
||||
{
|
||||
std::cout << "[ERROR] Could not open the camera parameter file storage: " << filename << " !"<< std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
fs["camera_matrix"] >> camMatrix;
|
||||
fs["distortion_coefficients"] >> distCoeffs;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
bool readColorParameters(std::string filename, int& iLowH, int& iHighH, int& iLowS, int& iHighS, int& iLowV, int& iHighV)
|
||||
{
|
||||
cv::FileStorage fs(filename, cv::FileStorage::READ);
|
||||
if (!fs.isOpened())
|
||||
{
|
||||
std::cout << "[ERROR] Could not open the color paramter file storage: " << filename << " !"<< std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
fs["lowH"] >> iLowH;
|
||||
fs["highH"] >> iHighH;
|
||||
|
||||
fs["lowS"] >> iLowS;
|
||||
fs["highS"] >> iHighS;
|
||||
|
||||
fs["lowV"] >> iLowV;
|
||||
fs["highV"] >> iHighV;
|
||||
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
// initializes main parameters
|
||||
float L1 = ROBOT_L1;
|
||||
float L2 = ROBOT_L2;
|
||||
float qpen = deg2rad(-90); // in rad
|
||||
std::string sCameraParamFilename = CAM_PARAMS_FILENAME;
|
||||
std::string sColorParamFilename = COLOR_PARAMS_FILENAME;
|
||||
float fFPS = FPS;
|
||||
int iStructuralElementSize = STRUCTURAL_ELEMENTS_SIZE;
|
||||
int iAreaThresold = AREA_THRESOLD;
|
||||
|
||||
// updates main parameters from arguments
|
||||
int opt;
|
||||
while ((opt = getopt (argc, argv, ":c:f:s:a:i:p:l:m:")) != -1)
|
||||
{
|
||||
switch (opt)
|
||||
{
|
||||
case 'c':
|
||||
sColorParamFilename = optarg;
|
||||
break;
|
||||
case 'f':
|
||||
fFPS = atof(optarg);
|
||||
break;
|
||||
case 'p':
|
||||
qpen = atof(optarg);
|
||||
break;
|
||||
case 'l':
|
||||
L1 = atof(optarg);
|
||||
break;
|
||||
case 'm':
|
||||
L2 = atof(optarg);
|
||||
break;
|
||||
case 's':
|
||||
iStructuralElementSize = atoi(optarg);
|
||||
break;
|
||||
case 'a':
|
||||
iAreaThresold = atoi(optarg);
|
||||
break;
|
||||
case 'i':
|
||||
sCameraParamFilename = optarg;
|
||||
break;
|
||||
case '?':
|
||||
if (optopt == 'c' || optopt == 'f' || optopt == 's' || optopt == 'a' || optopt == 'p' || optopt == 'l' || optopt == 'm' || optopt == 'i')
|
||||
fprintf (stderr, "Option -%c requires an argument.\n", optopt);
|
||||
else if (isprint (optopt))
|
||||
fprintf (stderr, "Unknown option `-%c'.\n", optopt);
|
||||
else
|
||||
fprintf (stderr, "Unknown option character `\\x%x'.\n", optopt);
|
||||
return 1;
|
||||
default:
|
||||
abort ();
|
||||
}
|
||||
}
|
||||
|
||||
// Initializes the robot
|
||||
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
|
||||
|
||||
|
||||
// reads color parameters from the file storage
|
||||
int iLowH, iHighH, iLowS, iHighS, iLowV, iHighV;
|
||||
bool isColorParamsSet = readColorParameters(sColorParamFilename, iLowH, iHighH, iLowS, iHighS, iLowV, iHighV);
|
||||
|
||||
// checks if the color parameters were successfully read
|
||||
if (!isColorParamsSet)
|
||||
{
|
||||
std::cout << "[ERROR] Color parameters could not be loaded!" << std::endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// distorted/undistorted image
|
||||
bool bIsImageUndistorted = true;
|
||||
|
||||
// reads camera intrinsic parameters
|
||||
cv::Mat cameraMatrix, distCoeffs;
|
||||
bool isCamParamsSet = readCameraParameters(sCameraParamFilename, cameraMatrix, distCoeffs);
|
||||
|
||||
// checks if the camera parameters were successfully read
|
||||
if (!isCamParamsSet)
|
||||
{
|
||||
std::cout << "[WARNING] Camera intrinsic parameters could not be loaded!" << std::endl;
|
||||
}
|
||||
|
||||
// creates a camera grabber
|
||||
VideoCapture cap(0, cv::CAP_V4L2); //capture the video from webcam
|
||||
|
||||
// checks if the camera was successfully opened
|
||||
if ( !cap.isOpened() ) // if not success, exit program
|
||||
{
|
||||
cout << "[ERROR] Could not open the camera!" << endl;
|
||||
return -1;
|
||||
}
|
||||
|
||||
// inits previous x,y location of the ball
|
||||
int iLastX = -1;
|
||||
int iLastY = -1;
|
||||
|
||||
// captures a temporary image from the camera
|
||||
Mat imgTmp;
|
||||
cap.read(imgTmp);
|
||||
|
||||
// main loop launched every FPS
|
||||
while (true)
|
||||
{
|
||||
// creates a black image with the size as the camera output
|
||||
Mat imgLines = Mat::zeros( imgTmp.size(), CV_8UC3 );
|
||||
|
||||
// reads a new frame from video
|
||||
cv::Mat imgOriginal;
|
||||
bool bSuccess = cap.read(imgOriginal);
|
||||
|
||||
// checks if a new frame was grabbed
|
||||
if (!bSuccess) //if not success, break loop
|
||||
{
|
||||
std::cout << "[WARNING] Could not read a new frame from video stream" << std::endl;
|
||||
break;
|
||||
}
|
||||
|
||||
if (bIsImageUndistorted && isCamParamsSet)
|
||||
{
|
||||
cv::Mat temp = imgOriginal.clone();
|
||||
cv::undistort(temp, imgOriginal, cameraMatrix, distCoeffs);
|
||||
}
|
||||
|
||||
// converts the captured frame from BGR to HSV
|
||||
cv::Mat imgHSV;
|
||||
cvtColor(imgOriginal, imgHSV, cv::COLOR_BGR2HSV);
|
||||
|
||||
// thresholds the image based on the trackbar values
|
||||
cv::Mat imgThresholded;
|
||||
inRange(imgHSV, cv::Scalar(iLowH, iLowS, iLowV), cv::Scalar(iHighH, iHighS, iHighV), imgThresholded);
|
||||
|
||||
// applies morphological opening (removes small objects from the foreground)
|
||||
cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
|
||||
cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
|
||||
|
||||
// applies morphological closing (removes small holes from the foreground)
|
||||
cv::dilate( imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
|
||||
cv::erode(imgThresholded, imgThresholded, getStructuringElement(MORPH_ELLIPSE, Size(iStructuralElementSize, iStructuralElementSize)) );
|
||||
|
||||
// calculates the moments of the thresholded image
|
||||
Moments oMoments = moments(imgThresholded);
|
||||
double dM01 = oMoments.m01;
|
||||
double dM10 = oMoments.m10;
|
||||
double dArea = oMoments.m00;
|
||||
|
||||
// if the area <= iAreaThresold, considers that the there are no object in the image and it's because of the noise, the area is not zero
|
||||
int posX, posY;
|
||||
|
||||
if (dArea > iAreaThresold)
|
||||
{
|
||||
// calculates the position of the ball
|
||||
posX = dM10 / dArea;
|
||||
posY = dM01 / dArea;
|
||||
|
||||
if (iLastX >= 0 && iLastY >= 0 && posX >= 0 && posY >= 0)
|
||||
{
|
||||
// draww a red line from the previous point to the current point
|
||||
line(imgLines, Point(posX, posY), Point(iLastX, iLastY), Scalar(0,0,255), 2);
|
||||
}
|
||||
|
||||
// stores the current position for enxt frame
|
||||
iLastX = posX;
|
||||
iLastY = posY;
|
||||
}
|
||||
|
||||
// displays the thresholded image
|
||||
imshow("Thresholded Image", imgThresholded);
|
||||
|
||||
// adds a cross at the centre of the image
|
||||
cv::drawMarker(imgOriginal, cv::Point(imgTmp.size().width/2, imgTmp.size().height/2), 10, cv::MARKER_CROSS, cv::LINE_8);
|
||||
|
||||
// shows the original image with the tracking (red) lines
|
||||
imgOriginal = imgOriginal + imgLines;
|
||||
imshow("Original", imgOriginal);
|
||||
float h = 167;
|
||||
float w = 239;
|
||||
// converts posX, posY in mm in the world reference frame
|
||||
float img_width = imgTmp.size().width;
|
||||
float img_height = imgTmp.size().height;
|
||||
/////////////////////////////////
|
||||
// TODO
|
||||
float x = (posY - (480/2))*(0.05) + 7;
|
||||
float y = (posX - (640/2))*(0.05);
|
||||
|
||||
//std::cout << "(pixel -> cm) = (" << posX << ", " << posY << ") - > (" << x << ", " << y << ")" << std::endl;
|
||||
/////////////////////////////////
|
||||
|
||||
|
||||
// Computes IK
|
||||
std::vector<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