22 static const std::array<double, 6>
EMPTY_VALUES = { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
25 static const std::string
SERVOJ_TIME(
"{{SERVOJ_TIME}}");
28 static const std::string
SERVOJ_GAIN(
"{{SERVOJ_GAIN}}");
30 static const std::string
REVERSE_IP(
"{{REVERSE_IP}}");
31 static const std::string
REVERSE_PORT(
"{{REVERSE_PORT}}");
34 def driveRobotLowBandwidthTrajectory(): 36 global TIME_INTERVAL = {{TIME_INTERVAL}} 37 global SERVOJ_TIME = {{SERVOJ_TIME}} 38 global SERVOJ_TIME_WAITING = {{SERVOJ_TIME_WAITING}} 39 global MAX_WAITING_TIME = {{MAX_WAITING_TIME}} 40 global EMPTY_VALUES = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0] 41 global SERVOJ_GAIN = {{SERVOJ_GAIN}} 42 global SERVOJ_LOOKAHEAD_TIME = {{SERVOJ_LOOKAHEAD_TIME}} 43 global CONNECTION_NAME = "reverse_connection" 44 global REVERSE_IP = "{{REVERSE_IP}}" 45 global REVERSE_PORT = {{REVERSE_PORT}} 46 global MAX_JOINT_DIFFERENCE = {{MAX_JOINT_DIFFERENCE}} 47 global g_position_previous = EMPTY_VALUES 48 global g_position_target = EMPTY_VALUES 49 global g_position_next = EMPTY_VALUES 50 global g_velocity_previous = EMPTY_VALUES 51 global g_velocity_target = EMPTY_VALUES 52 global g_velocity_next = EMPTY_VALUES 53 global g_time_previous = 0.0 54 global g_time_target = 0.0 55 global g_time_next = 0.0 56 global g_num_previous = -1 57 global g_num_target = -1 58 global g_num_next = -1 59 global g_received_waypoints_number = -1 60 global g_requested_waypoints_number = -1 61 global g_total_elapsed_time = 0 62 global g_stopping = False 63 def send_message(message): 64 socket_send_string(message, CONNECTION_NAME) 65 socket_send_byte(10, CONNECTION_NAME) 68 def is_waypoint_sentinel(waypoint): 69 local l_previous_index = 2 70 while l_previous_index < 1 + JOINT_NUM * 2 + 2: 71 if waypoint[l_previous_index] != 0.0: 74 l_previous_index = l_previous_index + 1 79 def is_final_position_reached(position): 80 local l_current_position = get_actual_joint_positions() 82 while l_index < JOINT_NUM: 83 if norm(position[l_index] - l_current_position[l_index]) > MAX_JOINT_DIFFERENCE: 91 def interpolate(time_within_segment, total_segment_time, start_pos, l_end_pos, l_start_vel, end_vel): 94 local c = (-3 * a + 3 * l_end_pos - 2 * total_segment_time * b - total_segment_time * end_vel) / pow(total_segment_time, 2) 95 local d = (2 * a - 2 * l_end_pos + total_segment_time * b + total_segment_time * end_vel) / pow(total_segment_time, 3) 96 return a + b * time_within_segment + c * pow(time_within_segment, 2) + d * pow(time_within_segment, 3) 99 def add_next_waypoint(waypoint): 101 g_position_previous = g_position_target 102 g_velocity_previous = g_velocity_target 103 g_time_previous = g_time_target 104 g_num_previous = g_num_target 105 g_position_target = g_position_next 106 g_velocity_target = g_velocity_next 107 g_time_target = g_time_next 108 g_num_target = g_num_next 109 g_num_next = waypoint[1] 110 g_position_next = [waypoint[2], waypoint[3], waypoint[4], waypoint[5], waypoint[6], waypoint[7]] 111 g_velocity_next = [waypoint[8], waypoint[9], waypoint[10], waypoint[11], waypoint[12], waypoint[13]] 112 g_time_next = waypoint[14] 113 g_received_waypoints_number = g_num_next 117 thread controllingThread(): 118 local l_received_waypoints_number = -1 119 local l_requested_waypoints_number = -1 120 local l_stopped = False 121 local l_current_position = get_actual_joint_positions() 123 g_requested_waypoints_number = 2 127 l_requested_waypoints_number = g_requested_waypoints_number 129 local l_max_waiting_time_left = MAX_WAITING_TIME 130 while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0: 131 servoj(l_current_position,t=SERVOJ_TIME_WAITING,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) 133 l_received_waypoints_number = g_received_waypoints_number 135 l_max_waiting_time_left = l_max_waiting_time_left - SERVOJ_TIME_WAITING 137 if l_max_waiting_time_left <= 0: 138 textmsg("Closing the connection on waiting too long.") 139 socket_close(CONNECTION_NAME) 143 local l_start_pos = g_position_previous 144 local l_start_vel = g_velocity_previous 145 local l_start_time = g_time_previous 146 local l_start_num= g_num_previous 147 local l_end_pos = g_position_target 148 local l_end_vel = g_velocity_target 149 local l_end_time = g_time_target 150 local l_end_num = g_num_target 151 local l_total_elapsed_time = g_total_elapsed_time 152 local l_stopping_after_next_interpolation = g_stopping 153 g_requested_waypoints_number = g_requested_waypoints_number + 1 156 l_current_position = l_start_pos 158 local l_total_segment_time = l_end_time - l_start_time 160 while l_total_elapsed_time <= l_end_time: 161 local l_segment_elapsed_time = l_total_elapsed_time - l_start_time 164 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]) 167 servoj(l_current_position,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) 169 g_total_elapsed_time = g_total_elapsed_time + TIME_INTERVAL 170 l_total_elapsed_time = g_total_elapsed_time 174 if l_stopping_after_next_interpolation: 175 while not is_final_position_reached(l_end_pos): 176 textmsg("Catching up on final position not reached first time.") 177 servoj(l_end_pos,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) 179 textmsg("Finishing the controlling thread. Final position reached.") 185 thread sendingThread(): 186 local controlling_thread = run controllingThread() 187 local l_sent_waypoints_number = -1 188 local l_requested_waypoints_number = -1 189 local l_stopping = False 192 l_requested_waypoints_number = g_requested_waypoints_number 193 l_stopping = g_stopping 195 while not l_stopping: 196 while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping: 197 sleep(SERVOJ_TIME_WAITING) 200 l_requested_waypoints_number = g_requested_waypoints_number 201 l_stopping = g_stopping 208 send_message(l_sent_waypoints_number + 1) 209 l_sent_waypoints_number = l_sent_waypoints_number + 1 211 join controlling_thread 214 thread receivingThread(): 215 local sending_thread = run sendingThread() 217 waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME) 218 if waypoint_received[0] == 0: 219 textmsg("Not received trajectory for the last 2 seconds. Quitting") 224 elif waypoint_received[0] != JOINT_NUM * 2 + 2: 225 textmsg("Received wrong number of floats in trajectory. This is certainly not OK.") 226 textmsg(waypoint_received[0]) 231 elif is_waypoint_sentinel(waypoint_received): 232 add_next_waypoint(waypoint_received) 235 g_received_waypoints_number = g_received_waypoints_number + 1 239 add_next_waypoint(waypoint_received) 243 socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME) 244 receiving_thread = run receivingThread() 245 join receiving_thread 246 socket_close(CONNECTION_NAME) 247 textmsg("Exiting the program") 253 int reverse_port,
bool version_3)
255 , commander_(commander)
256 , server_(reverse_port)
257 , time_interval_(0.008)
258 , servoj_time_(0.008)
259 , servoj_time_waiting_(0.001)
260 , max_waiting_time_(2.0)
261 , servoj_gain_(300.0)
262 , servoj_lookahead_time_(0.03)
263 , max_joint_difference_(0.01)
274 std::ostringstream out;
277 LOG_ERROR(
"Low Bandwidth Trajectory Follower only works for interface version > 3");
293 LOG_ERROR(
"Failed to bind server, the port %d is likely already in use", reverse_port);
296 LOG_INFO(
"Low Bandwidth Trajectory Follower is initialized!");
301 LOG_INFO(
"Starting LowBandwidthTrajectoryFollower");
306 LOG_INFO(
"Uploading trajectory program to robot");
314 LOG_DEBUG(
"Awaiting incoming robot connection");
318 LOG_ERROR(
"Failed to accept incoming robot connection");
322 LOG_DEBUG(
"Robot successfully connected");
327 const std::array<double, 6> &velocities,
double sample_number,
328 double time_in_seconds)
333 std::ostringstream out;
336 out << sample_number <<
",";
337 for (
auto const &pos : positions)
341 for (
auto const &vel : velocities)
345 out << time_in_seconds <<
")\r\n";
349 const std::string tmp = out.str();
350 const char *formatted_message = tmp.c_str();
351 const uint8_t *buf = (uint8_t *)formatted_message;
354 LOG_DEBUG(
"Sending message %s", formatted_message);
356 return server_.
write(buf, strlen(formatted_message) + 1, written);
364 bool finished =
false;
370 while (!finished && !interrupt)
374 LOG_DEBUG(
"Connection closed. Finishing!");
378 unsigned int message_num = atoi((
const char *)line);
379 LOG_DEBUG(
"Received request %i", message_num);
380 if (message_num < trajectory.size())
382 res =
execute(trajectory[message_num].positions, trajectory[message_num].velocities, message_num,
383 trajectory[message_num].time_from_start.count() / 1e6);
static const std::string MAX_JOINT_DIFFERENCE("{{MAX_JOINT_DIFFERENCE}}")
static const std::string POSITION_PROGRAM
std::atomic< bool > running_
double servoj_time_waiting_
static const std::string REVERSE_IP("{{REVERSE_IP}}")
bool uploadProg(const std::string &s)
bool execute(const std::array< double, 6 > &positions, const std::array< double, 6 > &velocities, double sample_number, double time_in_seconds)
static const std::string SERVOJ_GAIN("{{SERVOJ_GAIN}}")
static const std::string MAX_WAITING_TIME("{{MAX_WAITING_TIME}}")
static const std::string TIME_INTERVAL("{{TIME_INTERVAL}}")
#define LOG_DEBUG(format,...)
bool readLine(char *buffer, size_t buf_len)
bool write(const uint8_t *buf, size_t buf_len, size_t &written)
ROSCPP_DECL bool get(const std::string &key, std::string &s)
#define MAX_SERVER_BUF_LEN
static const std::string SERVOJ_TIME_WAITING("{{SERVOJ_TIME_WAITING}}")
#define LOG_INFO(format,...)
static const std::string SERVOJ_TIME("{{SERVOJ_TIME}}")
static const std::string SERVOJ_LOOKAHEAD_TIME("{{SERVOJ_LOOKAHEAD_TIME}}")
LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3)
double max_joint_difference_
double servoj_lookahead_time_
#define LOG_ERROR(format,...)
static const std::array< double, 6 > EMPTY_VALUES
static const std::string REVERSE_PORT("{{REVERSE_PORT}}")