Universal_Robots_Client_Library
Universal_Robots_Client_Library copied to clipboard
Keepalive freezes trajectory execution
During the integration of urcl with my custom control software for UR robots I faced a problem with trajectory interface and keepalive. Sending keepalive to the robot during trajectory execution stops the move. Moreover, if I freeze the keepalive until the trajectory is finished, the program on the robot is stopped by the socket error. Is this a bug?
Could you please provide a short code snippet that explains what you are trying to do? That would help very much in answering your question.
This snippet is a part of the project, but I think that it should be enough. Method connect() is responsible for acquiring a connection with RTDE and also for starting the keep-alive thread. This thread writes a keep-alive thread when the robot is not moving. Method handle_trajectory() is responsible for sending a trajectory to the robot. When the trajectory is received, this method blocks the keep-alive thread.
#include <robotic_manager_core/ur/ur_interface.hpp>
#include <tf2/LinearMath/Matrix3x3.h>
#include <tf2/LinearMath/Quaternion.h>
#include <tf2/LinearMath/Vector3.h>
namespace roboticmanager {
URInterface::URInterface(std::string robot_ip, std::string &host_ip,
std::string script_file, std::string &output_recipe,
std::string input_recipe, bool headless_mode,
bool use_tool_comm, std::string calibration_checksum,
double servoj_gain, double servoj_lookahead_time,
bool non_blocking_read,
UrToolParameters tool_parameters,
uint32_t reverse_port, uint32_t script_sender_port,
uint32_t trajectory_port)
: commonlibraries::LoggingInterface("URInterface",
spdlog::get("rm_logger")),
robot_ip_(std::move(robot_ip)), host_ip_(std::move(host_ip)),
script_file_(std::move(script_file)),
output_recipe_(std::move(output_recipe)),
input_recipe_(std::move(input_recipe)), headless_mode_(headless_mode),
use_tool_com_(use_tool_comm),
calibration_checksum_(std::move(calibration_checksum)),
servoj_gain_(servoj_gain), servoj_lookahead_time_(servoj_lookahead_time),
tool_parameters_(std::move(tool_parameters)), reverse_port_(reverse_port),
script_sender_port_(script_sender_port),
trajectory_port_(trajectory_port), non_blocking_read_(non_blocking_read),
trajectory_received_(false),
trajectory_status_(
urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE),
joints_pos_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
joints_vel_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
joints_effort_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
tcp_pose_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}},
tcp_vel_cur_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}, tcp_ft_cur_{{0.0, 0.0, 0.0,
0.0, 0.0,
0.0}} {}
URInterface::URInterface(std::string robot_ip, std::string host_ip,
std::string script_file, std::string output_recipe,
std::string input_recipe, bool headless_mode,
bool use_tool_comm, std::string calibration_checksum,
double servoj_gain, double servoj_lookahead_time,
bool non_blocking_read,
UrToolParameters tool_parameters)
: URInterface(robot_ip, host_ip, script_file, output_recipe, input_recipe,
headless_mode, use_tool_comm, calibration_checksum,
servoj_gain, servoj_lookahead_time, non_blocking_read,
tool_parameters, 50001, 50002, 50003){};
URInterface::~URInterface() { disconnect(); }
std::unique_ptr<urcl::ToolCommSetup> URInterface::create_tool_communication() {
std::unique_ptr<urcl::ToolCommSetup> tool_comm_setup;
if (use_tool_com_) {
tool_comm_setup = std::make_unique<urcl::ToolCommSetup>();
using ToolVoltageT = std::underlying_type<urcl::ToolVoltage>::type;
ToolVoltageT tool_voltage;
tool_voltage = tool_parameters_.tool_voltage;
tool_comm_setup->setToolVoltage(
static_cast<urcl::ToolVoltage>(tool_voltage));
using ParityT = std::underlying_type<urcl::Parity>::type;
ParityT parity;
parity = tool_parameters_.tool_parity;
tool_comm_setup->setParity(static_cast<urcl::Parity>(parity));
tool_comm_setup->setBaudRate(
static_cast<uint32_t>(tool_parameters_.tool_baud_rate));
tool_comm_setup->setStopBits(
static_cast<uint32_t>(tool_parameters_.tool_stop_bits));
tool_comm_setup->setRxIdleChars(tool_parameters_.tool_rx_idle_chars);
tool_comm_setup->setTxIdleChars(tool_parameters_.tool_tx_idle_chars);
}
return tool_comm_setup;
}
RMStatusCode URInterface::move(const JointTarget &target) {
vector6d_t joints;
trajectory_t traj;
std::copy_n(target.joints.begin(), 6, joints.begin());
traj.push_back(joints);
return handle_trajectory(
traj, std::vector<bool>{false},
std::vector<float>{static_cast<float>(target.goal_time)},
std::vector<float>{static_cast<float>(target.radius)});
}
RMStatusCode URInterface::handle_trajectory(
const trajectory_t &target, const std::vector<bool> cartesian,
const std::vector<float> goal_time, const std::vector<float> blend_radius) {
trajectory_received_ = true;
trajectory_status_ =
urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE;
auto ctrl_msg_res = ur_driver_->writeTrajectoryControlMessage(
urcl::control::TrajectoryControlMessage::TRAJECTORY_START, target.size());
if (!ctrl_msg_res) {
return RMStatusCode::FAILED;
}
for (size_t i = 0; i < target.size(); i++) {
auto exec_res = ur_driver_->writeTrajectoryPoint(
target[i], cartesian[i], goal_time[i], blend_radius[i]);
if (!exec_res) {
return RMStatusCode::FAILED;
}
}
while (trajectory_status_ !=
urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) {
std::this_thread::sleep_for(std::chrono::milliseconds(1));
}
trajectory_received_ = false;
return RMStatusCode::SUCCESS;
}
void URInterface::trajectory_callabck(urcl::control::TrajectoryResult res) {
trajectory_status_ = res;
}
RMStatusCode URInterface::connect() {
auto tool_comm_setup = create_tool_communication();
ur_driver_ = std::make_unique<urcl::UrDriver>(
robot_ip_, script_file_, output_recipe_, input_recipe_,
std::bind(&URInterface::handle_robot_program, this,
std::placeholders::_1),
headless_mode_, std::move(tool_comm_setup), calibration_checksum_,
reverse_port_, script_sender_port_, servoj_gain_, servoj_lookahead_time_,
non_blocking_read_, host_ip_, trajectory_port_);
ur_driver_->registerTrajectoryDoneCallback(
std::bind(&URInterface::trajectory_callabck, this, _1));
driver_stopped_ = false;
std::thread t([&]() {
while (!driver_stopped_) {
if (!trajectory_received_) {
ur_driver_->writeKeepalive();
}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
}
});
t.detach();
ur_driver_->startRTDECommunication();
return RMStatusCode::SUCCESS;
}
RMStatusCode URInterface::disconnect() {
driver_stopped_ = true;
// Wait for joining detached thread.
std::this_thread::sleep_for(std::chrono::milliseconds(100));
ur_driver_->stopControl();
ur_driver_.reset();
return RMStatusCode::SUCCESS;
}
RMStatusCode URInterface::reconnect() {
log_info("Reconnecting to UR Robot");
auto disconnected = disconnect();
if (disconnected != RMStatusCode::SUCCESS)
return RMStatusCode::FAILED;
return connect();
}
void URInterface::handle_robot_program(bool program_running) {
robot_program_running_ = program_running;
}
} // namespace roboticmanager
std::thread t([&]() { while (!driver_stopped_) { if (!trajectory_received_) { ur_driver_->writeKeepalive(); } std::this_thread::sleep_for(std::chrono::milliseconds(2)); } });
is it really necessary to send keepalive msgs at 2 ms intervals?
I think not, but I was experimenting with different interval values. The same behaviour was with a 100 ms interval etc.
I'll try to have a look at this once I'm back in the office. @stefanscherzinger assigning this to me so it pops up in my To-Do-List. If you want to give it a shot before me, please reassign.
@fmauch Sorry for the late resposne, holidays etc. I can try to look at this bug, but it will not be as quick as possible.
@gbartyzel @fmauch Any update on this?