Merge branch 'develop'
This commit is contained in:
ros
commit
c04547d57b
|
|
@ -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,26 @@
|
|||
#include <iostream>
|
||||
#include <vector>
|
||||
|
||||
#include "opencv2/imgcodecs.hpp"
|
||||
#include "opencv2/highgui.hpp"
|
||||
#include "opencv2/imgproc.hpp"
|
||||
|
||||
#include "DynamixelHandler.h"
|
||||
#include "Kinematics.h"
|
||||
|
||||
|
||||
// ImageProcessing
|
||||
#define thresholdValue 200
|
||||
#define maxValue 255
|
||||
//ConvertContours
|
||||
#define drawingAreaWidthInMm 80.0
|
||||
#define drawingAreaHeigthInMm 50.0
|
||||
// SendContours
|
||||
#define deltaTBetweenSamples 500 //in ms
|
||||
#define incrementBetweenSamples 2
|
||||
|
||||
std::vector<std::vector<cv::Point>> imageProcessing(cv::Mat originalImage);
|
||||
|
||||
std::vector<std::vector<float>> convertContoursPixel2Mm(std::vector<std::vector<cv::Point>> vContoursInPixel, float theta, float tx, float ty, int imageWidth, int imageHeight);
|
||||
|
||||
void sendContours(std::vector<std::vector<cv::Point>> vContoursInPixel, std::vector<std::vector<float>> vContoursInMm, DynamixelHandler& dxlHandler, float L1, float L2);
|
||||
|
|
@ -0,0 +1,16 @@
|
|||
#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);
|
||||
|
||||
int computeJacobianMatrixRank(std::vector<float> vJacobianMatrix, float threshold);
|
||||
|
||||
cv::Mat computeInverseJacobianMatrix(std::vector<float> vJacobianMatrix);
|
||||
|
|
@ -0,0 +1,22 @@
|
|||
all: kinematics dynamics joint testKinematics
|
||||
g++ -o bin/jointControl lib/jointControl.o lib/Kinematics.o lib/DynamixelHandler.o -L/usr/local/lib/ -ldxl_x64_cpp -lrt -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
g++ -o bin/testKinematics lib/testKinematics.o lib/Kinematics.o -L/usr/lib/x86_64-linux-gnu `pkg-config --libs opencv4`
|
||||
|
||||
testKinematics: src/testKinematics.cpp
|
||||
g++ -c src/testKinematics.cpp -o lib/testKinematics.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
|
||||
|
||||
joint: src/jointControl.cpp
|
||||
g++ -c src/jointControl.cpp -o lib/jointControl.o -I./include -I/usr/include/opencv4
|
||||
|
||||
image: src/ImageProcessing.cpp
|
||||
g++ -c src/ImageProcessing.cpp -o lib/ImageProcessing.o -I./include -I/usr/include/opencv4
|
||||
|
||||
dynamics: src/DynamixelHandler.cpp
|
||||
g++ -c src/DynamixelHandler.cpp -o lib/DynamixelHandler.o -I./include -I/usr/local/include
|
||||
|
||||
clean:
|
||||
rm lib/*.o
|
||||
rm bin/*
|
||||
|
|
@ -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,149 @@
|
|||
#include "ImageProcessing.h"
|
||||
|
||||
std::vector<std::vector<cv::Point>> imageProcessing(cv::Mat img_original)
|
||||
{
|
||||
// displays the original image
|
||||
cv::imshow( "Original", img_original );
|
||||
|
||||
// converts the image to grayscale
|
||||
cv::Mat img_gray;
|
||||
cv::cvtColor( img_original, img_gray, cv::COLOR_BGR2GRAY );
|
||||
|
||||
// displays the grayscale image
|
||||
cv::imshow( "Grayscale", img_gray );
|
||||
|
||||
cv::Mat img_threshold;
|
||||
cv::threshold(img_gray, img_threshold, thresholdValue, maxValue, cv::THRESH_BINARY);
|
||||
|
||||
if (false)
|
||||
{
|
||||
// blurs it
|
||||
cv::Mat img_blur;
|
||||
blur( img_gray, img_blur, cv::Size(3,3) );
|
||||
|
||||
// displays the blurred image
|
||||
cv::imshow( "Blur", img_blur );
|
||||
|
||||
// applies Canny algorithm on it
|
||||
cv::Mat canny_output;
|
||||
cv::Canny( img_blur, canny_output, thresholdValue, maxValue );
|
||||
|
||||
// displays the canny image
|
||||
cv::imshow( "Canny", canny_output );
|
||||
}
|
||||
|
||||
// finds contours on the canny image
|
||||
std::vector<std::vector<cv::Point> > contours;
|
||||
std::vector<cv::Vec4i> hierarchy;
|
||||
cv::findContours( img_threshold, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE );
|
||||
//cv::findContours( canny_output, contours, hierarchy, cv::RETR_TREE, cv::CHAIN_APPROX_NONE );
|
||||
|
||||
// displays contours on the original image
|
||||
cv::RNG rng(12345);
|
||||
//cv::Mat drawing = cv::Mat::zeros( canny_output.size(), CV_8UC3 );
|
||||
cv::Mat drawing = cv::Mat::zeros( img_threshold.size(), CV_8UC3 );
|
||||
for( size_t i = 0; i< contours.size(); i++ )
|
||||
{
|
||||
cv::Scalar color = cv::Scalar( rng.uniform(0, 256), rng.uniform(0,256), rng.uniform(0,256) );
|
||||
cv::drawContours( drawing, contours, (int)i, color, 2, cv::LINE_8, hierarchy, 0 );
|
||||
}
|
||||
cv::imshow( "Contours", drawing );
|
||||
|
||||
return contours;
|
||||
}
|
||||
|
||||
std::vector<std::vector<float>> convertContoursPixel2Mm(std::vector<std::vector<cv::Point>> vContoursInPixel, float theta, float tx, float ty, int imageWidth, int imageHeight)
|
||||
{
|
||||
std::vector<std::vector<float>> vContoursInMm;
|
||||
|
||||
// pixel / mm ratios
|
||||
float widthRatio = (float)imageWidth / drawingAreaWidthInMm;
|
||||
float heigthRatio = (float)imageHeight / drawingAreaHeigthInMm;
|
||||
float appliedRatio = ceil((widthRatio>=heigthRatio) ? widthRatio : heigthRatio);
|
||||
|
||||
// image to real world reference frame transformation
|
||||
cv::Mat transformationMatrix(3, 3, CV_32FC1);
|
||||
transformationMatrix.at<float>(0,0) = cos(theta); transformationMatrix.at<float>(0,1) = -sin(theta); transformationMatrix.at<float>(0,2) = tx;
|
||||
transformationMatrix.at<float>(1,0) = sin(theta); transformationMatrix.at<float>(1,1) = cos(theta); transformationMatrix.at<float>(1,2) = ty;
|
||||
transformationMatrix.at<float>(2,0) = 0.0; transformationMatrix.at<float>(2,1) = 0.0; transformationMatrix.at<float>(2,2) = 1.0;
|
||||
|
||||
// size the output vector with the same number of contours as the input one
|
||||
vContoursInMm.resize(vContoursInPixel.size());
|
||||
|
||||
// fill the output vector by transforming the data
|
||||
for (int l_contour = 0; l_contour < vContoursInPixel.size(); l_contour++)
|
||||
{
|
||||
std::cout << "[INFO] (convertContoursPixel2Mm) contour #" << l_contour << " , # of pts = " << vContoursInPixel[l_contour].size()<< std::endl;
|
||||
for (int l_point = 0; l_point < vContoursInPixel[l_contour].size(); l_point++)
|
||||
{
|
||||
// get the coordinate of the current point in the pixel reference frame
|
||||
cv::Mat pointCoordinateInPixel(3, 1, CV_32FC1);
|
||||
pointCoordinateInPixel.at<float>(0,0) = vContoursInPixel[l_contour][l_point].y/ appliedRatio; //!! WATCH OUT Points go (x,y); (width,height)
|
||||
pointCoordinateInPixel.at<float>(1,0) = vContoursInPixel[l_contour][l_point].x/ appliedRatio;
|
||||
pointCoordinateInPixel.at<float>(2,0) = 1.0;
|
||||
|
||||
// transform it in the real world reference frame
|
||||
cv::Mat pointCoordinateInMm(3, 1, CV_32FC1);
|
||||
pointCoordinateInMm = transformationMatrix * pointCoordinateInPixel;
|
||||
|
||||
// store the data in the contour in mm
|
||||
vContoursInMm[l_contour].push_back(pointCoordinateInMm.at<float>(0,0) );
|
||||
vContoursInMm[l_contour].push_back(pointCoordinateInMm.at<float>(1,0) );
|
||||
}
|
||||
}
|
||||
|
||||
return vContoursInMm;
|
||||
|
||||
}
|
||||
|
||||
void sendContours(std::vector<std::vector<cv::Point>> vContoursInPixel, std::vector<std::vector<float>> vContoursInMm, DynamixelHandler& dxlHandler, float L1, float L2)
|
||||
{
|
||||
if (vContoursInPixel.size() == 0 || vContoursInMm.size() == 0)
|
||||
{
|
||||
std::cout << "[ERROR](sendContours) Contour vector is empty!" << std::endl;
|
||||
return;
|
||||
}
|
||||
|
||||
cv::Mat drawing = cv::Mat::zeros( 240, 240, CV_8UC3 );
|
||||
cv::Scalar color = cv::Scalar( 0, 0, 255 );
|
||||
cv::imshow( "Current contour", drawing );
|
||||
|
||||
float q_pen = deg2rad(-70.0f);
|
||||
|
||||
for (int l_contour = 0; l_contour < vContoursInMm.size(); l_contour++)
|
||||
{
|
||||
q_pen = deg2rad(-70.0f);
|
||||
std::cout << "[INFO] (sendContours) contour #" << l_contour << " , # of pts = " << vContoursInMm[l_contour].size()<< std::endl;
|
||||
for (int l_point = 0; l_point < vContoursInMm[l_contour].size()/2; l_point=l_point+incrementBetweenSamples)
|
||||
{
|
||||
std::cout << "l_point #" << l_point << std::endl;
|
||||
float targetPointX = vContoursInMm[l_contour][2*l_point]/10; // in cm
|
||||
float targetPointY = vContoursInMm[l_contour][2*l_point+1]/10; // in cm
|
||||
|
||||
float targetPointI = vContoursInPixel[l_contour][l_point].x;
|
||||
float targetPointJ = vContoursInPixel[l_contour][l_point].y;
|
||||
|
||||
// Plots a dot at the current point position
|
||||
cv::circle(drawing, cv::Point(targetPointI ,targetPointJ), 0, cv::Scalar(0,0,255), cv::FILLED, cv::LINE_8);
|
||||
cv::imshow( "Current contour", drawing );
|
||||
std::cout<<"CurrentPoint = (" << targetPointJ << "," << targetPointI << ") --> (" << targetPointX << "," << targetPointY << ")" << std::endl;
|
||||
|
||||
// IK
|
||||
std::vector<float> qi = computeInverseKinematics(targetPointX, targetPointY, L1, L2);
|
||||
if (qi.size() > 3)
|
||||
{
|
||||
std::vector<float> l_vTargetJointPosition;
|
||||
// q1
|
||||
l_vTargetJointPosition.push_back(qi[1]);
|
||||
// fixed joint
|
||||
l_vTargetJointPosition.push_back(q_pen);
|
||||
// q2
|
||||
l_vTargetJointPosition.push_back(qi[2]);
|
||||
|
||||
dxlHandler.sendTargetJointPosition(l_vTargetJointPosition);
|
||||
}
|
||||
q_pen = deg2rad(-90.0f);
|
||||
cv::waitKey(deltaTBetweenSamples);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
|
@ -0,0 +1,90 @@
|
|||
#include "Kinematics.h"
|
||||
|
||||
|
||||
float deg2rad(float angle)
|
||||
{
|
||||
return angle/180.0*M_PI;
|
||||
}
|
||||
|
||||
float rad2deg(float angle)
|
||||
{
|
||||
return angle*180.0/M_PI;
|
||||
}
|
||||
|
||||
std::vector<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;
|
||||
}
|
||||
|
||||
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,74 @@
|
|||
#include <chrono>
|
||||
#include <thread>
|
||||
|
||||
#include "Kinematics.h"
|
||||
#include "DynamixelHandler.h"
|
||||
|
||||
// Global variables
|
||||
DynamixelHandler _oDxlHandler;
|
||||
std::string _robotDxlPortName = "/dev/ttyUSB0";
|
||||
float _robotDxlProtocol = 2.0;
|
||||
int _robotDxlBaudRate = 1000000;
|
||||
|
||||
|
||||
void initRobot(DynamixelHandler& dxlHandler, std::string portName, float protocol, int baudRate)
|
||||
{
|
||||
std::cout << "===Initialization of the Dynamixel Motor communication====" << std::endl;
|
||||
dxlHandler.setDeviceName(portName);
|
||||
dxlHandler.setProtocolVersion(protocol);
|
||||
dxlHandler.openPort();
|
||||
dxlHandler.setBaudRate(baudRate);
|
||||
dxlHandler.enableTorque(true);
|
||||
std::cout << std::endl;
|
||||
|
||||
}
|
||||
|
||||
void closeRobot(DynamixelHandler& dxlHandler)
|
||||
{
|
||||
dxlHandler.enableTorque(false);
|
||||
dxlHandler.closePort();
|
||||
}
|
||||
|
||||
|
||||
int main(int argc, char** argv)
|
||||
{
|
||||
if (argc == 6)
|
||||
{
|
||||
// Retrieves the args and stores them into variables
|
||||
float L1 = atof(argv[1]); // in cm
|
||||
float L2 = atof(argv[2]); // in cm
|
||||
float q1 = deg2rad(atof(argv[3])); // in rad
|
||||
float q2 = deg2rad(atof(argv[4])); // in rad
|
||||
float qpen = deg2rad(atof(argv[5])); // in rad
|
||||
|
||||
// Initializes the robot
|
||||
initRobot(_oDxlHandler, _robotDxlPortName, _robotDxlProtocol, _robotDxlBaudRate);
|
||||
|
||||
// Computes FK
|
||||
computeForwardKinematics(q1, q2, L1, L2);
|
||||
|
||||
// Computes Differential Kinematics
|
||||
//std::vector<float> vJacobianMatrix = computeDifferentialKinematics(q1, q2, L1, L2);
|
||||
//int rank = computeJacobianMatrixRank(vJacobianMatrix, 0.1);
|
||||
|
||||
// Sends the target joint values received as args
|
||||
std::vector<float> vTargetJointPosition;
|
||||
vTargetJointPosition.push_back(q1);
|
||||
vTargetJointPosition.push_back(qpen);
|
||||
vTargetJointPosition.push_back(q2);
|
||||
_oDxlHandler.sendTargetJointPosition(vTargetJointPosition);
|
||||
//std::cout << "vTargetJointPosition= " << vTargetJointPosition[0] << ", " << vTargetJointPosition[1] << ", " << vTargetJointPosition[2]<< std::endl;
|
||||
// Waits 2s
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds(2000));
|
||||
|
||||
// Closes robot connection
|
||||
_oDxlHandler.closePort();
|
||||
}
|
||||
else
|
||||
{
|
||||
std::cout << "[WARNING] Joint control"<< std::endl;
|
||||
std::cout << ">> jointControl L1(cm) L2(cm) q1(deg) q2(deg) qpen(deg)"<< std::endl;
|
||||
}
|
||||
|
||||
return 0;
|
||||
}
|
||||
|
|
@ -0,0 +1,123 @@
|
|||
#include <iostream>
|
||||
|
||||
#include "Kinematics.h"
|
||||
|
||||
#define ERROR_THRESHOLD 0.00001
|
||||
|
||||
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);
|
||||
|
||||
|
||||
bool testFK(float q1, float q2, float L1, float L2, std::vector<float> expectedEndEffectorPosition, float errorThreshold)
|
||||
{
|
||||
std::vector<float> calculatedEndEffectorPosition = computeForwardKinematics(q1, q2, L1, L2);
|
||||
|
||||
if (expectedEndEffectorPosition.size() != calculatedEndEffectorPosition.size())
|
||||
{
|
||||
std::cout << "[ERROR] (testFK) expected results and calculated ones have different sizes!" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
float error = 0.0f;
|
||||
for (int l_coord=0; l_coord < expectedEndEffectorPosition.size(); l_coord++)
|
||||
{
|
||||
error += abs(expectedEndEffectorPosition[l_coord] - calculatedEndEffectorPosition[l_coord]);
|
||||
}
|
||||
std::cout << "[INFO] (testFK) error = " << error << std::endl;
|
||||
|
||||
if (error < errorThreshold)
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
/*
|
||||
bool testIK(float x, float y, float L1, float L2, std::vector<float> expectedJointValues, float errorThreshold)
|
||||
{
|
||||
|
||||
std::vector<float> calculatedJointValues = computeInverseKinematics(x, y, L1, L2);
|
||||
|
||||
if (expectedJointValues.size() != calculatedJointValues.size())
|
||||
{
|
||||
std::cout << "[ERROR] (testIK) expected results and calculated ones have different sizes!" << std::endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
float error = 0.0f;
|
||||
for (int l_coord=0; l_coord < expectedJointValues.size(); l_coord++)
|
||||
{
|
||||
error += abs(expectedJointValues[l_coord] - calculatedJointValues[l_coord]);
|
||||
}
|
||||
std::cout << "[INFO] (testIK) error = " << error << std::endl;
|
||||
|
||||
if (error < errorThreshold)
|
||||
return true;
|
||||
else
|
||||
return false;
|
||||
}
|
||||
*/
|
||||
int main()
|
||||
{
|
||||
// Tests Forward Kinematics
|
||||
std::cout << "====TESTING FORWARD KINEMATICS====" << std::endl;
|
||||
std::cout << "-> Test #1......"<< std::endl;
|
||||
float q1= deg2rad(0.0f); float q2 = deg2rad(0.0f); float L1 = 5.0f; float L2 = 6.0f; std::vector<float> expectedEndEffectorPosition{11.0f, 0.0f};
|
||||
bool isTestPassed = testFK(q1, q2, L1, L2, expectedEndEffectorPosition, ERROR_THRESHOLD);
|
||||
if (isTestPassed)
|
||||
std::cout << "PASSED!" << std::endl;
|
||||
else
|
||||
std::cout << "FAILED!" << std::endl;
|
||||
|
||||
std::cout << "-> Test #2......"<< std::endl;
|
||||
q1= deg2rad(90.0f); q2 = deg2rad(0.0f); L1 = 5.0f; L2 = 6.0f; expectedEndEffectorPosition[0] = 0.0f; expectedEndEffectorPosition[1] = 11.0f;
|
||||
isTestPassed = testFK(q1, q2, L1, L2, expectedEndEffectorPosition, ERROR_THRESHOLD);
|
||||
if (isTestPassed)
|
||||
std::cout << "PASSED!" << std::endl;
|
||||
else
|
||||
std::cout << "FAILED!" << std::endl;
|
||||
|
||||
std::cout << "-> Test #3......"<< std::endl;
|
||||
q1= deg2rad(0.0f); q2 = deg2rad(90.0f); L1 = 5.0f; L2 = 6.0f; expectedEndEffectorPosition[0] = 5.0f; expectedEndEffectorPosition[1] = 6.0f;
|
||||
isTestPassed = testFK(q1, q2, L1, L2, expectedEndEffectorPosition, ERROR_THRESHOLD);
|
||||
if (isTestPassed)
|
||||
std::cout << "PASSED!" << std::endl;
|
||||
else
|
||||
std::cout << "FAILED!" << std::endl;
|
||||
/*
|
||||
// Tests Inverse Kinematics
|
||||
std::cout << "====TESTING INVERSE KINEMATICS====" << std::endl;
|
||||
std::cout << "-> Test #1......"<< std::endl;
|
||||
float x= 11.0f; float y = 0.0f; L1 = 5.0f; L2 = 6.0f; expectedEndEffectorPosition.resize(3); expectedEndEffectorPosition[0] = 1.0f; expectedEndEffectorPosition[1] = deg2rad(0.0f); expectedEndEffectorPosition[2] = deg2rad(0.0f);
|
||||
isTestPassed = testIK(x, y, L1, L2, expectedEndEffectorPosition, ERROR_THRESHOLD);
|
||||
if (isTestPassed)
|
||||
std::cout << "PASSED!" << std::endl;
|
||||
else
|
||||
std::cout << "FAILED!" << std::endl;
|
||||
|
||||
std::cout << "-> Test #2......"<< std::endl;
|
||||
x= 0.0f; y = 11.0f; L1 = 5.0f; L2 = 6.0f; expectedEndEffectorPosition.resize(3); expectedEndEffectorPosition[0] = 1.0f; expectedEndEffectorPosition[1] = deg2rad(90.0f); expectedEndEffectorPosition[2] = deg2rad(0.0f);
|
||||
isTestPassed = testIK(x, y, L1, L2, expectedEndEffectorPosition, ERROR_THRESHOLD);
|
||||
if (isTestPassed)
|
||||
std::cout << "PASSED!" << std::endl;
|
||||
else
|
||||
std::cout << "FAILED!" << std::endl;
|
||||
|
||||
std::cout << "-> Test #3......"<< std::endl;
|
||||
x= 5.0f; y = 6.0f; L1 = 5.0f; L2 = 6.0f; expectedEndEffectorPosition.resize(5); expectedEndEffectorPosition[0] = 2.0f; expectedEndEffectorPosition[1] = deg2rad(0.0f); expectedEndEffectorPosition[2] = deg2rad(90.0f); expectedEndEffectorPosition[3] = deg2rad(100.389f); expectedEndEffectorPosition[4] = deg2rad(-90.0f);
|
||||
isTestPassed = testIK(x, y, L1, L2, expectedEndEffectorPosition, ERROR_THRESHOLD);
|
||||
if (isTestPassed)
|
||||
std::cout << "PASSED!" << std::endl;
|
||||
else
|
||||
std::cout << "FAILED!" << std::endl;
|
||||
|
||||
std::cout << "-> Test #4......"<< std::endl;
|
||||
x= 12.0f; y = 0.0f; L1 = 5.0f; L2 = 6.0f; expectedEndEffectorPosition.resize(1); expectedEndEffectorPosition[0] = 0.0f;
|
||||
isTestPassed = testIK(x, y, L1, L2, expectedEndEffectorPosition, ERROR_THRESHOLD);
|
||||
if (isTestPassed)
|
||||
std::cout << "PASSED!" << std::endl;
|
||||
else
|
||||
std::cout << "FAILED!" << std::endl;
|
||||
|
||||
return 0;
|
||||
*/
|
||||
}
|
||||
Loading…
Reference in New Issue