mirror of
https://gitlab.com/obbart/universal_robots_ros_driver.git
synced 2026-04-10 10:00:48 +02:00
339 lines
10 KiB
C++
339 lines
10 KiB
C++
/*
|
|
* ur_driver.cpp
|
|
*
|
|
* Copyright 2015 Thomas Timm Andersen
|
|
*
|
|
* Licensed under the Apache License, Version 2.0 (the "License");
|
|
* you may not use this file except in compliance with the License.
|
|
* You may obtain a copy of the License at
|
|
*
|
|
* http://www.apache.org/licenses/LICENSE-2.0
|
|
*
|
|
* Unless required by applicable law or agreed to in writing, software
|
|
* distributed under the License is distributed on an "AS IS" BASIS,
|
|
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
|
|
* See the License for the specific language governing permissions and
|
|
* limitations under the License.
|
|
*/
|
|
|
|
#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) :
|
|
REVERSE_PORT_(reverse_port), maximum_time_step_(max_time_step), minimum_payload_(
|
|
min_payload), maximum_payload_(max_payload), servoj_time_(
|
|
servoj_time) {
|
|
char buffer[256];
|
|
struct sockaddr_in serv_addr;
|
|
int n, flag;
|
|
|
|
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));
|
|
|
|
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) {
|
|
/*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) {
|
|
std::chrono::high_resolution_clock::time_point t0, t;
|
|
std::vector<double> positions;
|
|
unsigned int j;
|
|
|
|
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;
|
|
}
|
|
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;
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
void UrDriver::stopTraj() {
|
|
executing_traj_ = false;
|
|
rt_interface_->addCommandToQueue("stopj(10)\n");
|
|
}
|
|
|
|
bool UrDriver::uploadProg() {
|
|
std::string cmd_str;
|
|
char buf[128];
|
|
cmd_str = "def driverProg():\n";
|
|
|
|
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";
|
|
|
|
if (sec_interface_->robot_state_->getVersion() >= 3.1)
|
|
sprintf(buf, "\t\t\t\tservoj(q, t=%.4f, lookahead_time=0.03)\n", servoj_time_);
|
|
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";
|
|
|
|
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 += "end\n";
|
|
|
|
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;
|
|
}
|
|
void UrDriver::closeServo(std::vector<double> positions) {
|
|
if (positions.size() != 6)
|
|
UrDriver::servoj(rt_interface_->robot_state_->getQActual(), 0);
|
|
else
|
|
UrDriver::servoj(positions, 0);
|
|
|
|
reverse_connected_ = false;
|
|
close(new_sockfd_);
|
|
}
|
|
|
|
bool UrDriver::start() {
|
|
if (!sec_interface_->start())
|
|
return false;
|
|
rt_interface_->robot_state_->setVersion(
|
|
sec_interface_->robot_state_->getVersion());
|
|
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_);
|
|
}
|
|
|
|
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);
|
|
}
|
|
|
|
std::vector<std::string> UrDriver::getJointNames() {
|
|
return joint_names_;
|
|
}
|
|
|
|
void UrDriver::setJointNames(std::vector<std::string> 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);
|
|
}
|
|
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);
|
|
}
|
|
void UrDriver::setDigitalOut(unsigned int n, bool b) {
|
|
char buf[256];
|
|
sprintf(buf, "sec setOut():\n\tset_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];
|
|
sprintf(buf, "sec setOut():\n\tset_analog_out(%d, %1.4f)\nend\n", n, f);
|
|
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;
|
|
}
|
|
|
|
void UrDriver::setMinPayload(double m) {
|
|
if (m > 0) {
|
|
minimum_payload_ = m;
|
|
} else {
|
|
minimum_payload_ = 0;
|
|
}
|
|
|
|
}
|
|
void UrDriver::setMaxPayload(double m) {
|
|
maximum_payload_ = m;
|
|
}
|
|
void UrDriver::setServojTime(double t) {
|
|
if (t > 0.008) {
|
|
servoj_time_ = t;
|
|
} else {
|
|
servoj_time_ = 0.008;
|
|
}
|
|
}
|