mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 10:00:48 +02:00
Adopted roscpp code style and naming convention
This commit is contained in:
Simon Rasmussen
@@ -18,397 +18,403 @@
|
||||
|
||||
#include "ur_modern_driver/ur_driver.h"
|
||||
|
||||
UrDriver::UrDriver(std::condition_variable& rt_msg_cond,
|
||||
std::condition_variable& msg_cond, std::string host,
|
||||
unsigned int reverse_port, double servoj_time,
|
||||
unsigned int safety_count_max, double max_time_step, double min_payload,
|
||||
double max_payload, double servoj_lookahead_time, double servoj_gain)
|
||||
: REVERSE_PORT_(reverse_port)
|
||||
, maximum_time_step_(max_time_step)
|
||||
, minimum_payload_(
|
||||
min_payload)
|
||||
, maximum_payload_(max_payload)
|
||||
, servoj_time_(
|
||||
servoj_time)
|
||||
, servoj_lookahead_time_(servoj_lookahead_time)
|
||||
, servoj_gain_(servoj_gain)
|
||||
UrDriver::UrDriver(std::condition_variable& rt_msg_cond, std::condition_variable& msg_cond, std::string host,
|
||||
unsigned int reverse_port, double servoj_time, unsigned int safety_count_max, double max_time_step,
|
||||
double min_payload, double max_payload, double servoj_lookahead_time, double servoj_gain)
|
||||
: REVERSE_PORT_(reverse_port)
|
||||
, maximum_time_step_(max_time_step)
|
||||
, minimum_payload_(min_payload)
|
||||
, maximum_payload_(max_payload)
|
||||
, servoj_time_(servoj_time)
|
||||
, servoj_lookahead_time_(servoj_lookahead_time)
|
||||
, servoj_gain_(servoj_gain)
|
||||
{
|
||||
char buffer[256];
|
||||
struct sockaddr_in serv_addr;
|
||||
int n, flag;
|
||||
char buffer[256];
|
||||
struct sockaddr_in serv_addr;
|
||||
int n, flag;
|
||||
|
||||
firmware_version_ = 0;
|
||||
reverse_connected_ = false;
|
||||
executing_traj_ = false;
|
||||
rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host,
|
||||
safety_count_max);
|
||||
new_sockfd_ = -1;
|
||||
sec_interface_ = new UrCommunication(msg_cond, host);
|
||||
firmware_version_ = 0;
|
||||
reverse_connected_ = false;
|
||||
executing_traj_ = false;
|
||||
rt_interface_ = new UrRealtimeCommunication(rt_msg_cond, host, safety_count_max);
|
||||
new_sockfd_ = -1;
|
||||
sec_interface_ = new UrCommunication(msg_cond, host);
|
||||
|
||||
incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (incoming_sockfd_ < 0) {
|
||||
print_fatal("ERROR opening socket for reverse communication");
|
||||
}
|
||||
bzero((char*)&serv_addr, sizeof(serv_addr));
|
||||
incoming_sockfd_ = socket(AF_INET, SOCK_STREAM, 0);
|
||||
if (incoming_sockfd_ < 0)
|
||||
{
|
||||
print_fatal("ERROR opening socket for reverse communication");
|
||||
}
|
||||
bzero((char*)&serv_addr, sizeof(serv_addr));
|
||||
|
||||
serv_addr.sin_family = AF_INET;
|
||||
serv_addr.sin_addr.s_addr = INADDR_ANY;
|
||||
serv_addr.sin_port = htons(REVERSE_PORT_);
|
||||
flag = 1;
|
||||
setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag,
|
||||
sizeof(int));
|
||||
setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
|
||||
if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr,
|
||||
sizeof(serv_addr))
|
||||
< 0) {
|
||||
print_fatal("ERROR on binding socket for reverse communication");
|
||||
}
|
||||
listen(incoming_sockfd_, 5);
|
||||
serv_addr.sin_family = AF_INET;
|
||||
serv_addr.sin_addr.s_addr = INADDR_ANY;
|
||||
serv_addr.sin_port = htons(REVERSE_PORT_);
|
||||
flag = 1;
|
||||
setsockopt(incoming_sockfd_, IPPROTO_TCP, TCP_NODELAY, (char*)&flag, sizeof(int));
|
||||
setsockopt(incoming_sockfd_, SOL_SOCKET, SO_REUSEADDR, &flag, sizeof(int));
|
||||
if (bind(incoming_sockfd_, (struct sockaddr*)&serv_addr, sizeof(serv_addr)) < 0)
|
||||
{
|
||||
print_fatal("ERROR on binding socket for reverse communication");
|
||||
}
|
||||
listen(incoming_sockfd_, 5);
|
||||
}
|
||||
|
||||
std::vector<double> UrDriver::interp_cubic(double t, double T,
|
||||
std::vector<double> p0_pos, std::vector<double> p1_pos,
|
||||
std::vector<double> p0_vel, std::vector<double> p1_vel)
|
||||
std::vector<double> UrDriver::interp_cubic(double t, double T, std::vector<double> p0_pos, std::vector<double> p1_pos,
|
||||
std::vector<double> p0_vel, std::vector<double> p1_vel)
|
||||
{
|
||||
/*Returns positions of the joints at time 't' */
|
||||
std::vector<double> positions;
|
||||
for (unsigned int i = 0; i < p0_pos.size(); i++) {
|
||||
double a = p0_pos[i];
|
||||
double b = p0_vel[i];
|
||||
double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i]
|
||||
- T * p1_vel[i])
|
||||
/ pow(T, 2);
|
||||
double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i]
|
||||
+ T * p1_vel[i])
|
||||
/ pow(T, 3);
|
||||
positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
|
||||
}
|
||||
return positions;
|
||||
/*Returns positions of the joints at time 't' */
|
||||
std::vector<double> positions;
|
||||
for (unsigned int i = 0; i < p0_pos.size(); i++)
|
||||
{
|
||||
double a = p0_pos[i];
|
||||
double b = p0_vel[i];
|
||||
double c = (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - T * p1_vel[i]) / pow(T, 2);
|
||||
double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + T * p1_vel[i]) / pow(T, 3);
|
||||
positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3));
|
||||
}
|
||||
return positions;
|
||||
}
|
||||
|
||||
bool UrDriver::doTraj(std::vector<double> inp_timestamps,
|
||||
std::vector<std::vector<double> > inp_positions,
|
||||
std::vector<std::vector<double> > inp_velocities)
|
||||
bool UrDriver::doTraj(std::vector<double> inp_timestamps, std::vector<std::vector<double>> inp_positions,
|
||||
std::vector<std::vector<double>> inp_velocities)
|
||||
{
|
||||
std::chrono::high_resolution_clock::time_point t0, t;
|
||||
std::vector<double> positions;
|
||||
unsigned int j;
|
||||
std::chrono::high_resolution_clock::time_point t0, t;
|
||||
std::vector<double> positions;
|
||||
unsigned int j;
|
||||
|
||||
if (!UrDriver::uploadProg()) {
|
||||
return false;
|
||||
if (!UrDriver::uploadProg())
|
||||
{
|
||||
return false;
|
||||
}
|
||||
executing_traj_ = true;
|
||||
t0 = std::chrono::high_resolution_clock::now();
|
||||
t = t0;
|
||||
j = 0;
|
||||
while ((inp_timestamps[inp_timestamps.size() - 1] >=
|
||||
std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count()) and
|
||||
executing_traj_)
|
||||
{
|
||||
while (inp_timestamps[j] <= std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() &&
|
||||
j < inp_timestamps.size() - 1)
|
||||
{
|
||||
j += 1;
|
||||
}
|
||||
executing_traj_ = true;
|
||||
t0 = std::chrono::high_resolution_clock::now();
|
||||
t = t0;
|
||||
j = 0;
|
||||
while ((inp_timestamps[inp_timestamps.size() - 1]
|
||||
>= std::chrono::duration_cast<std::chrono::duration<double> >(t - t0).count())
|
||||
and executing_traj_) {
|
||||
while (inp_timestamps[j]
|
||||
<= std::chrono::duration_cast<std::chrono::duration<double> >(
|
||||
t - t0)
|
||||
.count()
|
||||
&& j < inp_timestamps.size() - 1) {
|
||||
j += 1;
|
||||
}
|
||||
positions = UrDriver::interp_cubic(
|
||||
std::chrono::duration_cast<std::chrono::duration<double> >(
|
||||
t - t0)
|
||||
.count()
|
||||
- inp_timestamps[j - 1],
|
||||
inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
|
||||
inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
|
||||
UrDriver::servoj(positions);
|
||||
positions = UrDriver::interp_cubic(std::chrono::duration_cast<std::chrono::duration<double>>(t - t0).count() -
|
||||
inp_timestamps[j - 1],
|
||||
inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1],
|
||||
inp_positions[j], inp_velocities[j - 1], inp_velocities[j]);
|
||||
UrDriver::servoj(positions);
|
||||
|
||||
// oversample with 4 * sample_time
|
||||
std::this_thread::sleep_for(
|
||||
std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
|
||||
t = std::chrono::high_resolution_clock::now();
|
||||
}
|
||||
executing_traj_ = false;
|
||||
//Signal robot to stop driverProg()
|
||||
UrDriver::closeServo(positions);
|
||||
return true;
|
||||
// oversample with 4 * sample_time
|
||||
std::this_thread::sleep_for(std::chrono::milliseconds((int)((servoj_time_ * 1000) / 4.)));
|
||||
t = std::chrono::high_resolution_clock::now();
|
||||
}
|
||||
executing_traj_ = false;
|
||||
// Signal robot to stop driverProg()
|
||||
UrDriver::closeServo(positions);
|
||||
return true;
|
||||
}
|
||||
|
||||
void UrDriver::servoj(std::vector<double> positions, int keepalive)
|
||||
{
|
||||
if (!reverse_connected_) {
|
||||
print_error(
|
||||
"UrDriver::servoj called without a reverse connection present. Keepalive: "
|
||||
+ std::to_string(keepalive));
|
||||
return;
|
||||
}
|
||||
unsigned int bytes_written;
|
||||
int tmp;
|
||||
unsigned char buf[28];
|
||||
for (int i = 0; i < 6; i++) {
|
||||
tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_));
|
||||
buf[i * 4] = tmp & 0xff;
|
||||
buf[i * 4 + 1] = (tmp >> 8) & 0xff;
|
||||
buf[i * 4 + 2] = (tmp >> 16) & 0xff;
|
||||
buf[i * 4 + 3] = (tmp >> 24) & 0xff;
|
||||
}
|
||||
tmp = htonl((int)keepalive);
|
||||
buf[6 * 4] = tmp & 0xff;
|
||||
buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
|
||||
buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
|
||||
buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
|
||||
bytes_written = write(new_sockfd_, buf, 28);
|
||||
if (!reverse_connected_)
|
||||
{
|
||||
print_error("UrDriver::servoj called without a reverse connection present. Keepalive: " +
|
||||
std::to_string(keepalive));
|
||||
return;
|
||||
}
|
||||
unsigned int bytes_written;
|
||||
int tmp;
|
||||
unsigned char buf[28];
|
||||
for (int i = 0; i < 6; i++)
|
||||
{
|
||||
tmp = htonl((int)(positions[i] * MULT_JOINTSTATE_));
|
||||
buf[i * 4] = tmp & 0xff;
|
||||
buf[i * 4 + 1] = (tmp >> 8) & 0xff;
|
||||
buf[i * 4 + 2] = (tmp >> 16) & 0xff;
|
||||
buf[i * 4 + 3] = (tmp >> 24) & 0xff;
|
||||
}
|
||||
tmp = htonl((int)keepalive);
|
||||
buf[6 * 4] = tmp & 0xff;
|
||||
buf[6 * 4 + 1] = (tmp >> 8) & 0xff;
|
||||
buf[6 * 4 + 2] = (tmp >> 16) & 0xff;
|
||||
buf[6 * 4 + 3] = (tmp >> 24) & 0xff;
|
||||
bytes_written = write(new_sockfd_, buf, 28);
|
||||
}
|
||||
|
||||
void UrDriver::stopTraj()
|
||||
{
|
||||
executing_traj_ = false;
|
||||
rt_interface_->addCommandToQueue("stopj(10)\n");
|
||||
executing_traj_ = false;
|
||||
rt_interface_->addCommandToQueue("stopj(10)\n");
|
||||
}
|
||||
|
||||
bool UrDriver::uploadProg()
|
||||
{
|
||||
std::string cmd_str;
|
||||
char buf[128];
|
||||
cmd_str = "def driverProg():\n";
|
||||
std::string cmd_str;
|
||||
char buf[128];
|
||||
cmd_str = "def driverProg():\n";
|
||||
|
||||
sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
|
||||
cmd_str += buf;
|
||||
sprintf(buf, "\tMULT_jointstate = %i\n", MULT_JOINTSTATE_);
|
||||
cmd_str += buf;
|
||||
|
||||
cmd_str += "\tSERVO_IDLE = 0\n";
|
||||
cmd_str += "\tSERVO_RUNNING = 1\n";
|
||||
cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
|
||||
cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
|
||||
cmd_str += "\tdef set_servo_setpoint(q):\n";
|
||||
cmd_str += "\t\tenter_critical\n";
|
||||
cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
|
||||
cmd_str += "\t\tcmd_servo_q = q\n";
|
||||
cmd_str += "\t\texit_critical\n";
|
||||
cmd_str += "\tend\n";
|
||||
cmd_str += "\tthread servoThread():\n";
|
||||
cmd_str += "\t\tstate = SERVO_IDLE\n";
|
||||
cmd_str += "\t\twhile True:\n";
|
||||
cmd_str += "\t\t\tenter_critical\n";
|
||||
cmd_str += "\t\t\tq = cmd_servo_q\n";
|
||||
cmd_str += "\t\t\tdo_brake = False\n";
|
||||
cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
|
||||
cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
|
||||
cmd_str += "\t\t\t\tdo_brake = True\n";
|
||||
cmd_str += "\t\t\tend\n";
|
||||
cmd_str += "\t\t\tstate = cmd_servo_state\n";
|
||||
cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
|
||||
cmd_str += "\t\t\texit_critical\n";
|
||||
cmd_str += "\t\t\tif do_brake:\n";
|
||||
cmd_str += "\t\t\t\tstopj(1.0)\n";
|
||||
cmd_str += "\t\t\t\tsync()\n";
|
||||
cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
|
||||
cmd_str += "\tSERVO_IDLE = 0\n";
|
||||
cmd_str += "\tSERVO_RUNNING = 1\n";
|
||||
cmd_str += "\tcmd_servo_state = SERVO_IDLE\n";
|
||||
cmd_str += "\tcmd_servo_q = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]\n";
|
||||
cmd_str += "\tdef set_servo_setpoint(q):\n";
|
||||
cmd_str += "\t\tenter_critical\n";
|
||||
cmd_str += "\t\tcmd_servo_state = SERVO_RUNNING\n";
|
||||
cmd_str += "\t\tcmd_servo_q = q\n";
|
||||
cmd_str += "\t\texit_critical\n";
|
||||
cmd_str += "\tend\n";
|
||||
cmd_str += "\tthread servoThread():\n";
|
||||
cmd_str += "\t\tstate = SERVO_IDLE\n";
|
||||
cmd_str += "\t\twhile True:\n";
|
||||
cmd_str += "\t\t\tenter_critical\n";
|
||||
cmd_str += "\t\t\tq = cmd_servo_q\n";
|
||||
cmd_str += "\t\t\tdo_brake = False\n";
|
||||
cmd_str += "\t\t\tif (state == SERVO_RUNNING) and ";
|
||||
cmd_str += "(cmd_servo_state == SERVO_IDLE):\n";
|
||||
cmd_str += "\t\t\t\tdo_brake = True\n";
|
||||
cmd_str += "\t\t\tend\n";
|
||||
cmd_str += "\t\t\tstate = cmd_servo_state\n";
|
||||
cmd_str += "\t\t\tcmd_servo_state = SERVO_IDLE\n";
|
||||
cmd_str += "\t\t\texit_critical\n";
|
||||
cmd_str += "\t\t\tif do_brake:\n";
|
||||
cmd_str += "\t\t\t\tstopj(1.0)\n";
|
||||
cmd_str += "\t\t\t\tsync()\n";
|
||||
cmd_str += "\t\t\telif state == SERVO_RUNNING:\n";
|
||||
|
||||
if (sec_interface_->robot_state_->getVersion() >= 3.1)
|
||||
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n",
|
||||
servoj_time_, servoj_lookahead_time_, servoj_gain_);
|
||||
else
|
||||
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
|
||||
cmd_str += buf;
|
||||
if (sec_interface_->robot_state_->getVersion() >= 3.1)
|
||||
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=%.4f, gain=%.0f)\n", servoj_time_, servoj_lookahead_time_,
|
||||
servoj_gain_);
|
||||
else
|
||||
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f)\n", servoj_time_);
|
||||
cmd_str += buf;
|
||||
|
||||
cmd_str += "\t\t\telse:\n";
|
||||
cmd_str += "\t\t\t\tsync()\n";
|
||||
cmd_str += "\t\t\tend\n";
|
||||
cmd_str += "\t\tend\n";
|
||||
cmd_str += "\tend\n";
|
||||
cmd_str += "\t\t\telse:\n";
|
||||
cmd_str += "\t\t\t\tsync()\n";
|
||||
cmd_str += "\t\t\tend\n";
|
||||
cmd_str += "\t\tend\n";
|
||||
cmd_str += "\tend\n";
|
||||
|
||||
sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(),
|
||||
REVERSE_PORT_);
|
||||
cmd_str += buf;
|
||||
sprintf(buf, "\tsocket_open(\"%s\", %i)\n", ip_addr_.c_str(), REVERSE_PORT_);
|
||||
cmd_str += buf;
|
||||
|
||||
cmd_str += "\tthread_servo = run servoThread()\n";
|
||||
cmd_str += "\tkeepalive = 1\n";
|
||||
cmd_str += "\twhile keepalive > 0:\n";
|
||||
cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
|
||||
cmd_str += "\t\tif params_mult[0] > 0:\n";
|
||||
cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[2] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[3] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[4] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[5] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[6] / MULT_jointstate]\n";
|
||||
cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
|
||||
cmd_str += "\t\t\tset_servo_setpoint(q)\n";
|
||||
cmd_str += "\t\tend\n";
|
||||
cmd_str += "\tend\n";
|
||||
cmd_str += "\tsleep(.1)\n";
|
||||
cmd_str += "\tsocket_close()\n";
|
||||
cmd_str += "\tkill thread_servo\n";
|
||||
cmd_str += "end\n";
|
||||
cmd_str += "\tthread_servo = run servoThread()\n";
|
||||
cmd_str += "\tkeepalive = 1\n";
|
||||
cmd_str += "\twhile keepalive > 0:\n";
|
||||
cmd_str += "\t\tparams_mult = socket_read_binary_integer(6+1)\n";
|
||||
cmd_str += "\t\tif params_mult[0] > 0:\n";
|
||||
cmd_str += "\t\t\tq = [params_mult[1] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[2] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[3] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[4] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[5] / MULT_jointstate, ";
|
||||
cmd_str += "params_mult[6] / MULT_jointstate]\n";
|
||||
cmd_str += "\t\t\tkeepalive = params_mult[7]\n";
|
||||
cmd_str += "\t\t\tset_servo_setpoint(q)\n";
|
||||
cmd_str += "\t\tend\n";
|
||||
cmd_str += "\tend\n";
|
||||
cmd_str += "\tsleep(.1)\n";
|
||||
cmd_str += "\tsocket_close()\n";
|
||||
cmd_str += "\tkill thread_servo\n";
|
||||
cmd_str += "end\n";
|
||||
|
||||
rt_interface_->addCommandToQueue(cmd_str);
|
||||
return UrDriver::openServo();
|
||||
rt_interface_->addCommandToQueue(cmd_str);
|
||||
return UrDriver::openServo();
|
||||
}
|
||||
|
||||
bool UrDriver::openServo()
|
||||
{
|
||||
struct sockaddr_in cli_addr;
|
||||
socklen_t clilen;
|
||||
clilen = sizeof(cli_addr);
|
||||
new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr,
|
||||
&clilen);
|
||||
if (new_sockfd_ < 0) {
|
||||
print_fatal("ERROR on accepting reverse communication");
|
||||
return false;
|
||||
}
|
||||
reverse_connected_ = true;
|
||||
return true;
|
||||
struct sockaddr_in cli_addr;
|
||||
socklen_t clilen;
|
||||
clilen = sizeof(cli_addr);
|
||||
new_sockfd_ = accept(incoming_sockfd_, (struct sockaddr*)&cli_addr, &clilen);
|
||||
if (new_sockfd_ < 0)
|
||||
{
|
||||
print_fatal("ERROR on accepting reverse communication");
|
||||
return false;
|
||||
}
|
||||
reverse_connected_ = true;
|
||||
return true;
|
||||
}
|
||||
void UrDriver::closeServo(std::vector<double> positions)
|
||||
{
|
||||
if (positions.size() != 6)
|
||||
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
|
||||
else
|
||||
UrDriver::servoj(positions, 0);
|
||||
if (positions.size() != 6)
|
||||
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
|
||||
else
|
||||
UrDriver::servoj(positions, 0);
|
||||
|
||||
reverse_connected_ = false;
|
||||
close(new_sockfd_);
|
||||
reverse_connected_ = false;
|
||||
close(new_sockfd_);
|
||||
}
|
||||
|
||||
bool UrDriver::start()
|
||||
{
|
||||
if (!sec_interface_->start())
|
||||
return false;
|
||||
firmware_version_ = sec_interface_->robot_state_->getVersion();
|
||||
rt_interface_->robot_state_->setVersion(firmware_version_);
|
||||
if (!rt_interface_->start())
|
||||
return false;
|
||||
ip_addr_ = rt_interface_->getLocalIp();
|
||||
print_debug(
|
||||
"Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_)
|
||||
+ "\n");
|
||||
return true;
|
||||
if (!sec_interface_->start())
|
||||
return false;
|
||||
firmware_version_ = sec_interface_->robot_state_->getVersion();
|
||||
rt_interface_->robot_state_->setVersion(firmware_version_);
|
||||
if (!rt_interface_->start())
|
||||
return false;
|
||||
ip_addr_ = rt_interface_->getLocalIp();
|
||||
print_debug("Listening on " + ip_addr_ + ":" + std::to_string(REVERSE_PORT_) + "\n");
|
||||
return true;
|
||||
}
|
||||
|
||||
void UrDriver::halt()
|
||||
{
|
||||
if (executing_traj_) {
|
||||
UrDriver::stopTraj();
|
||||
}
|
||||
sec_interface_->halt();
|
||||
rt_interface_->halt();
|
||||
close(incoming_sockfd_);
|
||||
if (executing_traj_)
|
||||
{
|
||||
UrDriver::stopTraj();
|
||||
}
|
||||
sec_interface_->halt();
|
||||
rt_interface_->halt();
|
||||
close(incoming_sockfd_);
|
||||
}
|
||||
|
||||
void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4,
|
||||
double q5, double acc)
|
||||
void UrDriver::setSpeed(double q0, double q1, double q2, double q3, double q4, double q5, double acc)
|
||||
{
|
||||
rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
|
||||
rt_interface_->setSpeed(q0, q1, q2, q3, q4, q5, acc);
|
||||
}
|
||||
|
||||
std::vector<std::string> UrDriver::getJointNames()
|
||||
{
|
||||
return joint_names_;
|
||||
return joint_names_;
|
||||
}
|
||||
|
||||
void UrDriver::setJointNames(std::vector<std::string> jn)
|
||||
{
|
||||
joint_names_ = jn;
|
||||
joint_names_ = jn;
|
||||
}
|
||||
|
||||
void UrDriver::setToolVoltage(unsigned int v)
|
||||
{
|
||||
char buf[256];
|
||||
sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
char buf[256];
|
||||
sprintf(buf, "sec setOut():\n\tset_tool_voltage(%d)\nend\n", v);
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
}
|
||||
void UrDriver::setFlag(unsigned int n, bool b)
|
||||
{
|
||||
char buf[256];
|
||||
sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n,
|
||||
b ? "True" : "False");
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
char buf[256];
|
||||
sprintf(buf, "sec setOut():\n\tset_flag(%d, %s)\nend\n", n, b ? "True" : "False");
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
}
|
||||
void UrDriver::setDigitalOut(unsigned int n, bool b)
|
||||
{
|
||||
char buf[256];
|
||||
if (firmware_version_ < 2) {
|
||||
sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n,
|
||||
b ? "True" : "False");
|
||||
} else if (n > 15) {
|
||||
sprintf(buf,
|
||||
"sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n",
|
||||
n - 16, b ? "True" : "False");
|
||||
} else if (n > 7) {
|
||||
sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n",
|
||||
n - 8, b ? "True" : "False");
|
||||
|
||||
} else {
|
||||
sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n",
|
||||
n, b ? "True" : "False");
|
||||
}
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
char buf[256];
|
||||
if (firmware_version_ < 2)
|
||||
{
|
||||
sprintf(buf, "sec setOut():\n\tset_digital_out(%d, %s)\nend\n", n, b ? "True" : "False");
|
||||
}
|
||||
else if (n > 15)
|
||||
{
|
||||
sprintf(buf, "sec setOut():\n\tset_tool_digital_out(%d, %s)\nend\n", n - 16, b ? "True" : "False");
|
||||
}
|
||||
else if (n > 7)
|
||||
{
|
||||
sprintf(buf, "sec setOut():\n\tset_configurable_digital_out(%d, %s)\nend\n", n - 8, b ? "True" : "False");
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf, "sec setOut():\n\tset_standard_digital_out(%d, %s)\nend\n", n, b ? "True" : "False");
|
||||
}
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
}
|
||||
void UrDriver::setAnalogOut(unsigned int n, double f)
|
||||
{
|
||||
char buf[256];
|
||||
if (firmware_version_ < 2) {
|
||||
sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
|
||||
} else {
|
||||
sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
|
||||
}
|
||||
char buf[256];
|
||||
if (firmware_version_ < 2)
|
||||
{
|
||||
sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
|
||||
}
|
||||
else
|
||||
{
|
||||
sprintf(buf, "sec setOut():\n\tset_standard_analog_out(%d, %1.4f)\nend\n", n, f);
|
||||
}
|
||||
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
}
|
||||
|
||||
bool UrDriver::setPayload(double m)
|
||||
{
|
||||
if ((m < maximum_payload_) && (m > minimum_payload_)) {
|
||||
char buf[256];
|
||||
sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
return true;
|
||||
} else
|
||||
return false;
|
||||
if ((m < maximum_payload_) && (m > minimum_payload_))
|
||||
{
|
||||
char buf[256];
|
||||
sprintf(buf, "sec setOut():\n\tset_payload(%1.3f)\nend\n", m);
|
||||
rt_interface_->addCommandToQueue(buf);
|
||||
print_debug(buf);
|
||||
return true;
|
||||
}
|
||||
else
|
||||
return false;
|
||||
}
|
||||
|
||||
void UrDriver::setMinPayload(double m)
|
||||
{
|
||||
if (m > 0) {
|
||||
minimum_payload_ = m;
|
||||
} else {
|
||||
minimum_payload_ = 0;
|
||||
}
|
||||
if (m > 0)
|
||||
{
|
||||
minimum_payload_ = m;
|
||||
}
|
||||
else
|
||||
{
|
||||
minimum_payload_ = 0;
|
||||
}
|
||||
}
|
||||
void UrDriver::setMaxPayload(double m)
|
||||
{
|
||||
maximum_payload_ = m;
|
||||
maximum_payload_ = m;
|
||||
}
|
||||
void UrDriver::setServojTime(double t)
|
||||
{
|
||||
if (t > 0.008) {
|
||||
servoj_time_ = t;
|
||||
} else {
|
||||
servoj_time_ = 0.008;
|
||||
}
|
||||
if (t > 0.008)
|
||||
{
|
||||
servoj_time_ = t;
|
||||
}
|
||||
else
|
||||
{
|
||||
servoj_time_ = 0.008;
|
||||
}
|
||||
}
|
||||
void UrDriver::setServojLookahead(double t)
|
||||
{
|
||||
if (t > 0.03) {
|
||||
if (t < 0.2) {
|
||||
servoj_lookahead_time_ = t;
|
||||
} else {
|
||||
servoj_lookahead_time_ = 0.2;
|
||||
}
|
||||
} else {
|
||||
servoj_lookahead_time_ = 0.03;
|
||||
if (t > 0.03)
|
||||
{
|
||||
if (t < 0.2)
|
||||
{
|
||||
servoj_lookahead_time_ = t;
|
||||
}
|
||||
else
|
||||
{
|
||||
servoj_lookahead_time_ = 0.2;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
servoj_lookahead_time_ = 0.03;
|
||||
}
|
||||
}
|
||||
void UrDriver::setServojGain(double g)
|
||||
{
|
||||
if (g > 100) {
|
||||
if (g < 2000) {
|
||||
servoj_gain_ = g;
|
||||
} else {
|
||||
servoj_gain_ = 2000;
|
||||
}
|
||||
} else {
|
||||
servoj_gain_ = 100;
|
||||
if (g > 100)
|
||||
{
|
||||
if (g < 2000)
|
||||
{
|
||||
servoj_gain_ = g;
|
||||
}
|
||||
else
|
||||
{
|
||||
servoj_gain_ = 2000;
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
servoj_gain_ = 100;
|
||||
}
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user