297 lines
8.5 KiB
C++
297 lines
8.5 KiB
C++
//#include <boost/asio.hpp>
|
|
//#include <boost/bind.hpp>
|
|
//#include <boost/thread.hpp>
|
|
//#include <boost/signals2.hpp>
|
|
//#include <boost/array.hpp>
|
|
//#include <boost/lexical_cast.hpp>
|
|
#include "UR3_Lib.h"
|
|
|
|
using namespace std;
|
|
|
|
//TODO: COPY UR_DRIVER FUNCTIONS
|
|
//TODO: IMPLEMENT DELAY FOR ALL GET FUNCTIONS BECAUSE OF REFRESH RATE DATA (+/- 128Hz)
|
|
|
|
|
|
template <typename T> std::string UR3::to_string(T const& value){
|
|
stringstream sstr;
|
|
sstr << value;
|
|
return sstr.str();
|
|
}
|
|
|
|
UR3::UR3(){
|
|
condition_variable msg_cond;
|
|
unsigned int safety_count_max = 12;
|
|
rt_interface_ = new UrRealtimeCommunication(msg_cond, "192.168.1.11", safety_count_max);
|
|
sec_interface_ = new UrCommunication(msg_cond, "192.168.1.11");
|
|
if(!sec_interface_->start()){
|
|
cout << "Communication not started" << endl;
|
|
}
|
|
firmware_version_ = sec_interface_->robot_state_->getVersion();
|
|
cout << "Firmware Version: " << firmware_version_ << endl;
|
|
rt_interface_->robot_state_->setVersion(firmware_version_);
|
|
|
|
if(!rt_interface_->start()){
|
|
cout << "RT Communication not started" << endl;
|
|
}else{
|
|
cout << "RT Communication started" << endl;
|
|
}
|
|
|
|
}
|
|
|
|
void UR3::_send(std::string message){
|
|
rt_interface_->addCommandToQueue(message);
|
|
}
|
|
|
|
void UR3::move(double x, double y, double z, double rx, double ry, double rz){
|
|
string xco, yco, zco, rxo, ryo,rzo, message;
|
|
xco = to_string(x);
|
|
yco = to_string(y);
|
|
zco = to_string(z);
|
|
rxo = to_string(rx);
|
|
ryo = to_string(ry);
|
|
rzo = to_string(rz);
|
|
|
|
message = "movej(p[" +xco + ", " + yco + ", " + zco + ", " +rxo+", "+ryo+", "+rzo+"], a=1.0,v=1.0)\nend\n"; //a=1.0,v=1.0
|
|
//message = "movej(p[" +xco + ", " + yco + ", " + zco + "], a=1.0,v=1.0)\nend\n";
|
|
_send(message);
|
|
}
|
|
|
|
void UR3::moveLinear(double x, double y, double z, double rx, double ry, double rz){
|
|
string xco, yco, zco, rxo, ryo,rzo, message;
|
|
xco = to_string(x);
|
|
yco = to_string(y);
|
|
zco = to_string(z);
|
|
rxo = to_string(rx);
|
|
ryo = to_string(ry);
|
|
rzo = to_string(rz);
|
|
|
|
message = "movel(p[" +xco + ", " + yco + ", " + zco + ", " +rxo+", "+ryo+", "+rzo+"], a=0.2,v=0.2)\nend\n";
|
|
_send(message);
|
|
}
|
|
|
|
void UR3::moveHome(){
|
|
string message = "movep(p[0.0,-getActualPosition90.0,0.0,-90.0,0.0,0.0], a=0.2,v=0.2)\nend\n"; //a=0.2,v=0.2
|
|
_send(message);
|
|
}
|
|
|
|
void UR3::moveGripper(bool close){
|
|
string message;
|
|
if(close)
|
|
message = "speedj([0.0,0.0,0.0,0.0,0.0,2.5], 1.0, 3.3)\nend\n"; //a=0.2,v=0.2
|
|
else{
|
|
message = "speedj([0.0,0.0,0.0,0.0,0.0,-2.5], 1.0, 3.3)\nend\n"; //a=0.2,v=0.2
|
|
}
|
|
_send(message);
|
|
usleep(3100000);
|
|
_send("stopj(1.0)\nend\n");
|
|
}
|
|
void UR3::openCloseRoboticGripper(bool close){
|
|
/*
|
|
string message = "rq_reset()\nrq_activate()\n";
|
|
//_send("rq_reset()\nend\n");
|
|
//usleep(500000);
|
|
//_send("rq_activate()\nend\n");
|
|
//usleep(500000);
|
|
if(close)
|
|
message += "rq_move(0)\nend\n";
|
|
else{
|
|
message += "rq_move(255)\nend\n";
|
|
//message = "rqc_open()\nend\n";
|
|
}
|
|
cout << message;
|
|
_send(message);
|
|
usleep(1000000);
|
|
*/
|
|
|
|
/*
|
|
string message = "socket_close(\"gripper_socket\")\nsync()\nsocket_open(\"127.0.0.1\", 63352, \"gripper_socket\")\nsync()\n";
|
|
message += "socket_set_var(\"GTO\", 1, \"gripper_socket\")\n";
|
|
message += "socket_set_var(\"FOR\", 255, \"gripper_socket\")\n";
|
|
message += "socket_set_var(\"SPE\", 150, \"gripper_socket\")\n";
|
|
|
|
if(close)
|
|
message += "socket_set_var(\"POS\", 0, \"gripper_socket\")\n";
|
|
else{
|
|
message += "socket_set_var(\"POS\", 255, \"gripper_socket\")\n";
|
|
//message = "rqc_open()\nend\n";
|
|
}
|
|
|
|
message += "end\n";
|
|
|
|
_send(message);
|
|
*/
|
|
|
|
_send("socket_close(\"gripper_socket\")\n");
|
|
usleep(100000);
|
|
_send("socket_open(\"127.0.0.1\", 63352, \"gripper_socket\")");
|
|
usleep(100000);
|
|
_send("socket_set_var(\"GTO\", 1, \"gripper_socket\")\n");
|
|
usleep(100000);
|
|
_send("sleep(1)\n");
|
|
usleep(1000000);
|
|
_send("socket_set_var(\"FOR\", 255, \"gripper_socket\")\n");
|
|
usleep(100000);
|
|
_send("socket_set_var(\"SPE\", 150, \"gripper_socket\")\n");
|
|
usleep(100000);
|
|
|
|
if(close)
|
|
_send("socket_set_var(\"POS\", 0, \"gripper_socket\")\n");
|
|
else{
|
|
_send("socket_set_var(\"POS\", 255, \"gripper_socket\")\n");
|
|
//message = "rqc_open()\nend\n";
|
|
}
|
|
usleep(100000);
|
|
_send("end\n");
|
|
|
|
usleep(3000000);
|
|
}
|
|
std::vector<bool> UR3::getDigitalInputs(){
|
|
usleep(1000);
|
|
//vector<bool> data = rt_interface_->robot_state_->getDigitalInputBits(); //RT dig in function - High speed
|
|
unsigned int data = sec_interface_->robot_state_->getDigitalInputBits();
|
|
unsigned int check = 1;
|
|
vector<bool> ret;
|
|
for(int i = 0; i < 18; i++){
|
|
if((data & check) == 1){ ret.push_back(true);}
|
|
else{ret.push_back(false);}
|
|
data = data >> 1;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
std::vector<bool> UR3::getDigitalOutputs(){
|
|
usleep(1000);
|
|
unsigned int data = sec_interface_->robot_state_->getDigitalOutputBits();
|
|
//cout << "Digital Outputs: " << data << endl;
|
|
unsigned int check = 1;
|
|
vector<bool> ret;
|
|
for(int i = 0; i < 18; i++){
|
|
if((data & check) == 1){ ret.push_back(true);}
|
|
else{ret.push_back(false);}
|
|
data = data >> 1;
|
|
}
|
|
return ret;
|
|
}
|
|
|
|
void UR3::setDigitalOut(unsigned int pin, bool state){
|
|
string p,s,message;
|
|
p = to_string(pin);
|
|
s = to_string(state ? "True" : "False");
|
|
message = "sec setOut():\n\tset_standard_digital_out("+p+","+s+")\nend\n";
|
|
_send(message);
|
|
}
|
|
|
|
std::vector<float> UR3::getAnalogInputs(){
|
|
vector<float> ret;
|
|
ret.push_back(sec_interface_->robot_state_->getAnalogInput0());
|
|
ret.push_back(sec_interface_->robot_state_->getAnalogInput1());
|
|
return ret;
|
|
}
|
|
|
|
std::vector<float> UR3::getAnalogOutputs(){
|
|
vector<float> ret;
|
|
ret.push_back(sec_interface_->robot_state_->getAnalogOutput0());
|
|
ret.push_back(sec_interface_->robot_state_->getAnalogOutput1());
|
|
return ret;
|
|
}
|
|
|
|
void UR3::setAnalogOut(unsigned int pin, float value){
|
|
string p,v,message;
|
|
p = to_string(pin);
|
|
v = to_string(value);
|
|
message = "sec setOut():\n\tset_analog_out("+p+","+v+")\nend\n";
|
|
_send(message);
|
|
}
|
|
|
|
vector<double> UR3::getActualPosition(){
|
|
vector<double> ret = rt_interface_->robot_state_->getToolVectorActual();
|
|
return ret;
|
|
}
|
|
|
|
bool UR3::getSafety_mode(){
|
|
double ret = rt_interface_->robot_state_->getSafety_mode();
|
|
if(ret == 3.0){
|
|
cout << "Safety: Safety Stop" << endl;
|
|
return false;
|
|
}else if(ret == 2.0){
|
|
cout << "Safety: Limited Operational" << endl;
|
|
return true;
|
|
}else{
|
|
return true;
|
|
}
|
|
}
|
|
|
|
void UR3::IO_test(){
|
|
vector<bool> dig_in = getDigitalInputs();
|
|
for(int i = 0; i < dig_in.size(); i++){
|
|
cout << "Digital Input Pin " << i << " - State: " << dig_in[i] << endl;
|
|
}
|
|
vector<bool> dig_out = getDigitalOutputs();
|
|
for(int i = 0; i < dig_out.size(); i++){
|
|
cout << "Digital Output Pin " << i << " - State: " << dig_out[i] << endl;
|
|
}
|
|
vector<float> analog_in = getAnalogInputs();
|
|
for(int i = 0; i < analog_in.size(); i++){
|
|
cout << "Analog Input Pin " << i << " - Value: " << analog_in[i] << endl;
|
|
}
|
|
vector<float> analog_out = getAnalogOutputs();
|
|
for(int i = 0; i < analog_out.size(); i++){
|
|
cout << "Analog Output Pin " << i << " - Value: " << analog_out[i] << endl;
|
|
}
|
|
}
|
|
|
|
void UR3::move_test(){
|
|
double rx, ry, rz;
|
|
rx = 0.0395;
|
|
ry = 3.1199;
|
|
rz = -0.0102;
|
|
|
|
setDigitalOut(0, true);
|
|
|
|
move(0.43269,-0.05873,0.12940,rx,ry,rz); //move above object at A
|
|
move(0.43269,-0.05873,-0.08640,rx,ry,rz); //touch object at A
|
|
setDigitalOut(0, false); //vacuum at A
|
|
move(0.43269,-0.05873,0.12940,rx,ry,rz); //remove object at A
|
|
//move(-0.04905,-0.33974,0.72268,0.4840,0.5834,-1.5299,time);
|
|
move(-0.30924,-0.26365,0.12940,rx,ry,rz); //move object A to B
|
|
move(-0.30924,-0.26365,-0.08640,rx,ry,rz); //touch object at B
|
|
setDigitalOut(0, true); //release vacuum at B
|
|
move(-0.30924,-0.26365,0.12940,rx,ry,rz); //remove object at B
|
|
move(-0.30924,-0.26365,-0.08640,rx,ry,rz); //touch object at B
|
|
setDigitalOut(0, false); //vacuum at B
|
|
move(-0.30924,-0.26365,0.12940,rx,ry,rz); //remove object at B
|
|
//move(-0.04905,-0.33974,0.72268,0.4840,0.5834,-1.5299,time);
|
|
move(0.43269,-0.05873,0.12940,rx,ry,rz); //move object B to A
|
|
move(0.43269,-0.05873,-0.08640,rx,ry,rz); //touch object at A
|
|
setDigitalOut(0, true); //vacuum at A
|
|
move(0.43269,-0.05873,0.12940,rx,ry,rz); //remove object at A
|
|
|
|
}
|
|
|
|
void UR3::Halt(){
|
|
rt_interface_->halt();
|
|
}
|
|
|
|
void UR3::stop(){
|
|
string message = "sec setOut():\n\tstop()\nend\n";
|
|
_send(message);
|
|
}
|
|
|
|
bool UR3::getEmergencyStop(){
|
|
bool ret = sec_interface_->robot_state_->isEmergencyStopped();
|
|
if(ret){cout << "Emergency Stop" << endl;}
|
|
return ret;
|
|
}
|
|
|
|
bool UR3::getProtectiveStop(){
|
|
bool ret = sec_interface_->robot_state_->isEmergencyStopped();
|
|
if(ret){cout << "Protective Stop" << endl;}
|
|
return sec_interface_->robot_state_->isProtectiveStopped();
|
|
}
|
|
|
|
std::vector<double> UR3::getMotorTemperatures(){
|
|
return rt_interface_->robot_state_->getMotorTemperatures();
|
|
}
|
|
|
|
|