diff --git a/README.md b/README.md
index ec171c7..a48c591 100644
--- a/README.md
+++ b/README.md
@@ -55,10 +55,8 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design
* **servoj_time** - time interval (real time) for which each interpolation step controls the robot (using servoj command). See below on examples of setting different time parameters. Default value is 0.008 (corresponds to expected 125Hz frequency of UR robot control)
* **servoj_time_waiting** - time in seconds (real time) of internal active loop while the robot waits for new instructions in case of delays in communication. The smaller value, the faster robot restarts move after delay (but more stress is put on URControl processor). Default value is 0.001 (1000 Hz check frequency)
* **max_waiting_time** - maximum time in seconds (real time) to wait for instructions from the drive before move is aborted. Defaults to 2 seconds.
- * **debug** - displays (visible in the log of Polyscope/pendant and URControl PC) helpful information about trajectory points messages exchanged Driver <-> URScript. It is safe to be run with real robot and introduces very small overhead on the execution of the moves
- * **more_debug** - displays even more debug information in the logs of Polyscope - details about interpolation loop calculations and execution. It adds a lot of overhead and is not recommended to be used with real robot (It's quite OK to run it with URSim though)
* **servoj_gain** and **servoj_lookahead_time** - useful to control precision and speed of the position moves with servoj command (see URScript documentation for detailes)
-
+ * **max_joint_difference** - maximum allowed difference between target and actual joints - checked at every trajectory step
Here are some examples of manipulating the time flow for **Low Bandwidth Trajectory Follower** mode. You can use other settings but you should do it on your own risk.
* Default mode: *servoj_time* = 0.008, *time_interval* = 0.008 : interpolation time flows with the same speed as real time - moves are executed as planned
* Slow-motion mode: *servoj_time* = 0.008, *time_interval* = 0.004 : interpolation time flows 2x slower than real time, so the move is executed 2x slower than planned. Requires configuring MoveIt to accept much slower moves than expected (otherwise MoveIt cancels such move mid-way)
diff --git a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h
index bd52ee4..003d071 100644
--- a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h
+++ b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h
@@ -22,7 +22,7 @@ private:
URServer server_;
double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \
- servoj_gain_, servoj_lookahead_time_;
+ servoj_gain_, servoj_lookahead_time_, max_joint_difference_;
bool debug_, more_debug_;
std::string program_;
diff --git a/launch/ur_common.launch b/launch/ur_common.launch
index ad77d41..2387b00 100644
--- a/launch/ur_common.launch
+++ b/launch/ur_common.launch
@@ -22,6 +22,7 @@
+
@@ -52,6 +53,7 @@
+
diff --git a/src/ros/lowbandwidth_trajectory_follower.cpp b/src/ros/lowbandwidth_trajectory_follower.cpp
index 1e628b2..c3cdd9f 100644
--- a/src/ros/lowbandwidth_trajectory_follower.cpp
+++ b/src/ros/lowbandwidth_trajectory_follower.cpp
@@ -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())