Obbart/universal_robots_ros_driver
1
0
Fork 0
mirror of https://gitlab.com/obbart/universal_robots_ros_driver.git synced 2026-04-12 11:00:47 +02:00
Code Releases Activity

Faster loading speed and better precision for LBTF

Browse Source 
Low Bandwidth Trajectory Follower has now much less comments
and debugging information - which means that it will parse on UR robot
in around 400 ms instead of 700 ms. It also has an adjustment loop
in case the robot does not reach any of the trajectory points specified.

It will catch-up at every trajectory point sent by MoveIt and will try
to get as close as possible to the desired pointi (it will check if all
joints are within MAX_JOINT_DIFFERENCE from the desired positions)
This commit is contained in:
Jarek Potiuk
2018-01-27 13:14:38 +01:00
parent a54b97e939
commit 42b336bbf5
4 changed files with 24 additions and 153 deletions
Download Patch File Download Diff File Expand all files Collapse all files

169
src/ros/lowbandwidth_trajectory_follower.cpp
View File

@@ -9,65 +9,41 @@ static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}");
static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}");
static const std::string SERVOJ_TIME_WAITING("{{SERVOJ_TIME_WAITING}}");
static const std::string MAX_WAITING_TIME("{{MAX_WAITING_TIME}}");
static const std::string DEBUG("{{DEBUG}}");
static const std::string MORE_DEBUG("{{MORE_DEBUG}}");
static const std::string SERVOJ_GAIN("{{SERVOJ_GAIN}}");
static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}");
static const std::string REVERSE_IP("{{REVERSE_IP}}");
static const std::string REVERSE_PORT("{{REVERSE_PORT}}");
static const std::string MAX_JOINT_DIFFERENCE("{{MAX_JOINT_DIFFERENCE}}");
static const std::string POSITION_PROGRAM = R"(
def driveRobotLowBandwidthTrajectory():
global JOINT_NUM = 6
global TIME_INTERVAL = {{TIME_INTERVAL}}
global SERVOJ_TIME = {{SERVOJ_TIME}}
global SERVOJ_TIME_WAITING = {{SERVOJ_TIME_WAITING}}
global MAX_WAITING_TIME = {{MAX_WAITING_TIME}}
global DEBUG = {{DEBUG}}
global MORE_DEBUG = {{MORE_DEBUG}}
global EMPTY_VALUES = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
global SERVOJ_GAIN = {{SERVOJ_GAIN}}
global SERVOJ_LOOKAHEAD_TIME = {{SERVOJ_LOOKAHEAD_TIME}}
global CONNECTION_NAME = "reverse_connection"
global REVERSE_IP = "{{REVERSE_IP}}"
global REVERSE_PORT = {{REVERSE_PORT}}
# NOTE All the global variables here are accessed by different threads
# therefore they should be accessed within critical section. Those variables
# are all prefixed with g_ . Whenever their values are needed they are copied
# to similarly named l_ variable. Copying happens inside the critical section
# and l_values might be used outside of it. This needs to be confirmed with UR
# about the semantics of assignment operator (copying or by reference?).
# Hopefully it's copying :).
#
# Please make sure to keep that pattern and do not access the global variables
# outside of the critical section
#
# TO DO: Are those assignments by references or copies? We will find out soon
# If not then assigning positions would not work either
global MAX_JOINT_DIFFERENCE = {{MAX_JOINT_DIFFERENCE}}
global g_position_previous = EMPTY_VALUES
global g_position_target = EMPTY_VALUES
global g_position_next = EMPTY_VALUES
global g_velocity_previous = EMPTY_VALUES
global g_velocity_target = EMPTY_VALUES
global g_velocity_next = EMPTY_VALUES
global g_time_previous = 0.0
global g_time_target = 0.0
global g_time_next = 0.0
global g_num_previous = -1
global g_num_target = -1
global g_num_next = -1
global g_received_waypoints_number = -1
global g_requested_waypoints_number = -1
global g_total_elapsed_time = 0
global g_stopping = False
def send_message(message):
socket_send_string(message, CONNECTION_NAME)
socket_send_byte(10, CONNECTION_NAME)
@@ -75,7 +51,7 @@ def driveRobotLowBandwidthTrajectory():
def is_waypoint_sentinel(waypoint):
local l_previous_index = 2
while l_previous_index < 1 + JOINT_NUM * 2 + 2: # Note - we do not check first two which are non-zero
while l_previous_index < 1 + JOINT_NUM * 2 + 2:
if waypoint[l_previous_index] != 0.0:
return False
end
@@ -84,6 +60,18 @@ def driveRobotLowBandwidthTrajectory():
return True
end
def is_final_position_reached(position):
local l_current_position = get_actual_joint_positions()
local l_index = 0
while l_index < JOINT_NUM:
if norm(position[l_index] - l_current_position[l_index]) > MAX_JOINT_DIFFERENCE:
return False
end
l_index = l_index + 1
end
return True
end
def interpolate(time_within_segment, total_segment_time, start_pos, l_end_pos, l_start_vel, end_vel):
local a = start_pos
local b = l_start_vel
@@ -94,96 +82,48 @@ def driveRobotLowBandwidthTrajectory():
def add_next_waypoint(waypoint):
enter_critical
# Rotate the values received so far:
# target -> previous
g_position_previous = g_position_target
g_velocity_previous = g_velocity_target
g_time_previous = g_time_target
g_num_previous = g_num_target
# next -> previous
g_position_target = g_position_next
g_velocity_target = g_velocity_next
g_time_target = g_time_next
g_num_target = g_num_next
# Decode the array received into next
# waypoint 0 is number of entries in the received array
g_num_next = waypoint[1]
g_position_next = [waypoint[2], waypoint[3], waypoint[4], waypoint[5], waypoint[6], waypoint[7]]
g_velocity_next = [waypoint[8], waypoint[9], waypoint[10], waypoint[11], waypoint[12], waypoint[13]]
g_time_next = waypoint[14]
# store latest received waypoint number so that controlling thread knows it's been received already
g_received_waypoints_number = g_num_next
if DEBUG:
local l_received_waypoints_number = g_received_waypoints_number
local l_waypoint = waypoint
end
exit_critical
if DEBUG:
textmsg("Received waypoint:")
textmsg(l_received_waypoints_number)
textmsg(l_waypoint)
end
end
# Thread controlling the motor. In the loop it checks first if it received the
# requested waypoints and until it does, it syncs doing noting to the motor
# once it received all up to requested waypoints it executes interpolation
# between PREVIOUS AND TARGET points received and rquests the next waypoint request
# to be sent.
thread controllingThread():
local l_received_waypoints_number = -1
local l_requested_waypoints_number = -1
local l_stopped = False
local l_current_position = get_actual_joint_positions()
enter_critical
g_requested_waypoints_number = 2
exit_critical
while True:
enter_critical
l_requested_waypoints_number = g_requested_waypoints_number
l_received_waypoints_number = g_received_waypoints_number
exit_critical
local l_max_waiting_time_left = MAX_WAITING_TIME
# if expected waypoint number not yet received wait so that receiving thread has time to receive it
while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0:
if DEBUG:
textmsg("Waiting for the received waypoints number to catch up (received/requested):")
textmsg(l_received_waypoints_number)
textmsg(l_requested_waypoints_number)
end
# Keep robot in l_current position for short time and check if the next waipoint arrived
servoj(l_current_position,t=SERVOJ_TIME_WAITING,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
enter_critical
l_received_waypoints_number = g_received_waypoints_number
exit_critical
l_max_waiting_time_left = l_max_waiting_time_left - SERVOJ_TIME_WAITING
end
if l_max_waiting_time_left <= 0:
textmsg("Closing the connection on waiting too long.")
socket_close(CONNECTION_NAME)
halt
end
# OK. We received next point, copy the required global variables into local ones
enter_critical
local l_start_pos = g_position_previous
local l_start_vel = g_velocity_previous
local l_start_time = g_time_previous
@@ -193,81 +133,39 @@ def driveRobotLowBandwidthTrajectory():
local l_end_time = g_time_target
local l_end_num = g_num_target
local l_total_elapsed_time = g_total_elapsed_time
# Note we deliberately only read "stopping" state here and not update it below
# so that stopping flag takes effect only after one additional interpolation loop is complete
# and all points of the trajectory are processeed
local l_stopping_after_next_interpolation = g_stopping
# And increasing the global requested number - informs sender thread that it needs to ask for it
g_requested_waypoints_number = g_requested_waypoints_number + 1
exit_critical
if DEBUG:
textmsg("Starting interpolation. Segment from/to:")
textmsg(l_start_num)
textmsg(l_end_num)
textmsg("Current time:")
textmsg(l_total_elapsed_time)
textmsg("Starting/Ending segment time:")
textmsg(l_start_time)
textmsg(l_end_time)
end
l_current_position = l_start_pos
local l_total_segment_time = l_end_time - l_start_time
# Here perform the interpolation loop
while l_total_elapsed_time <= l_end_time:
if MORE_DEBUG:
textmsg("Next step of interpolation:")
end
local l_segment_elapsed_time = l_total_elapsed_time - l_start_time
# Calculate interpolation for all joints
j = 0
while j < JOINT_NUM:
l_current_position[j] = interpolate(l_segment_elapsed_time, l_total_segment_time, l_start_pos[j], l_end_pos[j], l_start_vel[j], l_end_vel[j])
j = j + 1
end
if MORE_DEBUG:
textmsg("Next step of interpolated position:")
textmsg(l_current_position)
textmsg("Current time:")
textmsg(l_total_elapsed_time)
textmsg("Running servoj command:")
end
servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
enter_critical
g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL
l_total_elapsed_time = g_total_elapsed_time
exit_critical
if MORE_DEBUG:
textmsg("Finishing interpolation step at time:")
textmsg(l_total_elapsed_time)
end
end
if DEBUG:
textmsg("Ending interpolation segment at time:")
textmsg(l_total_elapsed_time)
while not is_final_position_reached(l_current_position):
servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN)
end
if l_stopping_after_next_interpolation:
textmsg("Stopping the controlling thread on signal from receiving thread after one interpolation loop.")
break
end
end
textmsg("Ending controlling thread")
end
# This thread sends requested waypoints number to the client when requested number is changed
# It will send all the numbers from [already sent + 1, g_requested_waypoints_number] and waits
# until requested waypoints number increases
thread sendingThread():
local controlling_thread = run controllingThread()
local l_sent_waypoints_number = -1
@@ -279,7 +177,6 @@ def driveRobotLowBandwidthTrajectory():
l_stopping = g_stopping
exit_critical
while not l_stopping:
# wait until we have more requested waypoints to send than actually sent ones
while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping:
sleep(SERVOJ_TIME_WAITING)
@@ -294,65 +191,42 @@ def driveRobotLowBandwidthTrajectory():
end
send_message(l_sent_waypoints_number + 1)
l_sent_waypoints_number = l_sent_waypoints_number + 1
if DEBUG:
textmsg("Sent waypoint request number:")
textmsg(l_sent_waypoints_number)
end
end
textmsg("Joining controlling thread")
join controlling_thread
textmsg("Ending Sending thread")
end
# Receiving thread - it will receive the next trajectory point over the TCP connection
# It will increase the received waipoints_number on each request.
thread receivingThread():
local sending_thread = run sendingThread()
while True:
waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME)
if waypoint_received[0] == 0:
# No new waypoint requested for the last 2 seconds
textmsg("Not received trajectory for the last 2 seconds. Quitting")
enter_critical
g_stopping = True
exit_critical
break
elif waypoint_received[0] != JOINT_NUM * 2 + 2:
textmsg("Received wrong number of floats in trajectory. This is certainly not OK.")
textmsg(waypoint_received[0])
enter_critical
g_stopping = True
exit_critical
break
elif is_waypoint_sentinel(waypoint_received):
add_next_waypoint(waypoint_received)
textmsg("Received sentinel waypoint. Finishing.")
enter_critical
g_stopping = True
g_received_waypoints_number = g_received_waypoints_number + 1
exit_critical
break
end
add_next_waypoint(waypoint_received)
end
textmsg("Joining sendingThread")
join sending_thread
textmsg("Ending Receiving thread")
end
textmsg("Opening socket")
socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME)
textmsg("Socket opened")
receiving_thread = run receivingThread()
textmsg("Joining receiving_thread")
join receiving_thread
textmsg("Closing reverse connection")
socket_close(CONNECTION_NAME)
textmsg("Exiting the program")
end
@@ -370,8 +244,7 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm
, max_waiting_time_(2.0)
, servoj_gain_(300.0)
, servoj_lookahead_time_(0.03)
, debug_(false)
, more_debug_(false)
, max_joint_difference_(0.01)
{
ros::param::get("~time_interval", time_interval_);
ros::param::get("~servoj_time", servoj_time_);
@@ -379,8 +252,7 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm
ros::param::get("~max_waiting_time", max_waiting_time_);
ros::param::get("~servoj_gain", servoj_gain_);
ros::param::get("~servoj_lookahead_time", servoj_lookahead_time_);
ros::param::get("~debug", debug_);
ros::param::get("~more_debug", more_debug_);
ros::param::get("~max_joint_difference", max_joint_difference_);
std::string res(POSITION_PROGRAM);
std::ostringstream out;
@@ -394,10 +266,9 @@ LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &comm
res.replace(res.find(MAX_WAITING_TIME), MAX_WAITING_TIME.length(), std::to_string(max_waiting_time_));
res.replace(res.find(SERVOJ_GAIN), SERVOJ_GAIN.length(), std::to_string(servoj_gain_));
res.replace(res.find(SERVOJ_LOOKAHEAD_TIME), SERVOJ_LOOKAHEAD_TIME.length(), std::to_string(servoj_lookahead_time_));
res.replace(res.find(DEBUG), DEBUG.length(), debug_ ? "True" : "False");
res.replace(res.find(MORE_DEBUG), MORE_DEBUG.length(), more_debug_ ? "True" : "False");
res.replace(res.find(REVERSE_IP), REVERSE_IP.length(), reverse_ip);
res.replace(res.find(REVERSE_PORT), REVERSE_PORT.length(), std::to_string(reverse_port));
res.replace(res.find(MAX_JOINT_DIFFERENCE), MAX_JOINT_DIFFERENCE.length(), std::to_string(max_joint_difference_));
program_ = res;
if (!server_.bind())