diff --git a/.gitignore b/.gitignore index 6faf4e1..1e69c9f 100644 --- a/.gitignore +++ b/.gitignore @@ -4,3 +4,4 @@ Makefile cmake_install.cmake install_manifest.txt *~ +.idea diff --git a/CMakeLists.txt b/CMakeLists.txt index 22cf821..bc43d3f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -172,6 +172,7 @@ set(${PROJECT_NAME}_SOURCES src/ros/rt_publisher.cpp src/ros/service_stopper.cpp src/ros/trajectory_follower.cpp + src/ros/lowbandwidth_trajectory_follower.cpp src/ros/urscript_handler.cpp src/ur/stream.cpp src/ur/server.cpp diff --git a/README.md b/README.md index 51427a0..3682d70 100644 --- a/README.md +++ b/README.md @@ -1,7 +1,7 @@ # ur_modern_driver - Refactored [![Build Status](https://travis-ci.org/Zagitta/ur_modern_driver.svg?branch=master)](https://travis-ci.org/Zagitta/ur_modern_driver) -A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is designed to replace the old driver transparently, while solving some issues, improving usability as well as enabling compatibility of ros_control. +A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is designed to replace the old driver transparently, while solving some issues, improving usability as well as enabling compatibility of ros_control. ## Improvements @@ -22,12 +22,12 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Besides this, the driver subscribes to two new topics: - * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Inteded for sending movel/movej commands directly to the robot, conveyor tracking and the like. + * */ur\_driver/URScript* : Takes messages of type _std\_msgs/String_ and directly forwards it to the robot. Note that no control is done on the input, so use at your own risk! Intended for sending movel/movej commands directly to the robot, conveyor tracking and the like. - * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTracetoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. + * */joint\_speed* : Takes messages of type _trajectory\_msgs/JointTrajectory_. Parses the first JointTrajectoryPoint and sends the specified joint speeds and accelerations to the robot. This interface is intended for doing visual servoing and other kind of control that requires speed control rather than position control of the robot. Remember to set values for all 6 joints. Ignores the field joint\_names, so set the values in the correct order. -* Added support for ros_control. - * As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time. +* Added support for ros_control. + * As ros_control wants to have control over the robot at all times, ros_control compatibility is set via a parameter at launch-time. * With ros_control active, the driver doesn't open the action_lib interface nor publish joint_states or wrench msgs. This is handled by ros_control instead. * Currently two controllers are available, both controlling the joint position of the robot, useable for trajectroy execution * The velocity based controller sends joint speed commands to the robot, using the speedj command @@ -39,9 +39,42 @@ A new driver for the UR3/UR5/UR10 robot arms from Universal Robots. It is design * Added activation modes: `Never`, `Always` and `OnStartup`. Sets wether a call to the `ur_driver/robot_enable` service is required at startup, at startup + on errors or never. Is intended as a safety feature to require some sort of manual intervention in case of driver crashes and robot safety faults. +* **Low Bandwidth Trajectory Follower** mode of execution. (only works when `ros_control` is set to `false`)In this mode the real-time control loop for the robot is shifted from the client PC running the driver to URscript executed on the URControl PC of the robot with the following features: + * It works only with */follow\_joint\_trajectory* action for MoveIt integration. It is mutually exclusive with *ros_control* + * It only implements "position + velocity" based control - it uses coarse positions and velocities calculated by MoveIt and performs cubic interpolation of joint positions (Bezier' curve) of positions. The positions are set using servoj command of URScript. + * It is much more resilient to connectivity problems than other methods of ur_modern_driver. It is resilient to additional load on client PC, latency of network and kernel of the PC the driver runs on. This makes it much better suitable for development/research quick iteration loops without separate dedicated physical setup. + * Other methods of controlling the robot by ur_modern_driver are very fragile and require low-latency kernel, separated wired network and ideally no other network activity on the PC where ur_modern_driver runs. + * **Low Bandwidth Trajectory Follower** will never make dangerous "catch-up" when move is delayed. Other methods prioritise execution time of a move which sometimes might lead to situation where robot gets out of control and speeds up to catch-up - often resulting in Protective Stop. For Low Bandwith Trajectory Follower predictability of the move has priority over move execution time. + * The amount of TCP/IP communication between the driver and UR robot (URControl PC) is two orders of magnitude (around 100x) less with **Low Bandwidth Trajectory Follower** than with other methods- coarse MoveIt generated trajectory is sent to robot and cubic interpolation of the move is calculated by the URScript program run on URControl PC. + * Due to communication optimisations and **Low Bandwidth Trajectory Follower** works reliably even over WiFi connection (!) which is impossible for other methods. + * Time flow for the URScript program might be independent from "real time" and can be controlled to speed up or slow down execution of planned moves. + * The Low Bandwidth Trajectory Follower requires 3.0+ version firmware of the robot (servoj command must support lookahead_time and servoj_gain parameters) + * There are several parameters that you can use to control Low Bandwidth Trajectory Follower's behaviour: + * **use_lowbandwidth_trajectory_follower** - should be set to `true` to enable the follower + * **time_interval** - time interval (in seconds) this is 'simulated' time for interpolation steps of the move. Together with *servoj_time* it can be used to change speed of moves even after they are planned by MoveIt. Default value is 0.008 + * **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. + * **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) + * Fast-forward mode: *servoj_time* = 0.004, *time_interval* = 0.012 : interpolation time flows 3x faster than real time, so the move is 3x faster than planned. Might violate limits of the robot speed so use carefully and make sure you gradually increase the speed in-between to check how safe it is. + +NOTE! In case you use Low Bandwidth Trajectory Follower and you experience MoveIt to cancel robot moves prematurely +because of too long move duration, you should increase tolerance of duration monitoring of MoveIt trajectory execution +You can find the configuration usually in trajectory_execution.launch.xml in generated moveit config - there are +parameters that configure scaling and margin for allowed execution time among others. +The relevant parameters are `trajectory_execution/allowed_execution_duration_scaling` (default 1.2) and +`trajectory_execution/allowed_goal_duration_margin` (default 0.5). The first one is factor that scales execution time, +the second is margin that is added on top of the scaled one. You can increase either of those values to make moveit +executor more "tolerant" to execution delays. There is also another parameter: +`trajectory_execution/execution_duration_monitoring`. You can set it to false to disable duration monitoring completely. + ## Installation -**As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.** +**As the driver communicates with the robot via ethernet and depends on reliable continous communication, it is not possible to reliably control a UR from a virtual machine.** Just clone the repository into your catkin working directory and make it with ```catkin_make```. @@ -89,7 +122,7 @@ The position based controller *should* stay closer to the commanded path, while **Note** that the PID values are not optimally tweaked as of this moment. -To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following +To use ros_control together with MoveIt, be sure to add the desired controller to the ```controllers.yaml``` in the urXX_moveit_config/config folder. Add the following ``` controller_list: - name: /vel_based_pos_traj_controller #or /pos_based_pos_traj_controller @@ -110,7 +143,7 @@ controller_list: Each robot from UR is calibrated individually, so there is a small error (in the order of millimeters) between the end-effector reported by the URDF models in https://github.com/ros-industrial/universal_robot/tree/indigo-devel/ur_description and the end-effector as reported by the controller itself. -This driver broadcasts a transformation between the base link and the end-effector as reported by the UR. The default frame names are: *base* and *tool0_controller*. +This driver broadcasts a transformation between the base link and the end-effector as reported by the UR. The default frame names are: *base* and *tool0_controller*. To use the *tool0_controller* frame in a URDF, there needs to be a link with that name connected to *base*. For example: @@ -148,16 +181,16 @@ Should be compatible with all robots and control boxes with the newest firmware. *Simulated UR5 running 1.6.08725 - + #Credits Please cite the following report if using this driver -@techreport{andersen2015optimizing, - title = {Optimizing the Universal Robots ROS driver.}, - institution = {Technical University of Denmark, Department of Electrical Engineering}, - author = {Andersen, Thomas Timm}, - year = {2015}, - url = {http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html} +@techreport{andersen2015optimizing, + title = {Optimizing the Universal Robots ROS driver.}, + institution = {Technical University of Denmark, Department of Electrical Engineering}, + author = {Andersen, Thomas Timm}, + year = {2015}, + url = {http://orbit.dtu.dk/en/publications/optimizing-the-universal-robots-ros-driver(20dde139-7e87-4552-8658-dbf2cdaab24b).html} } diff --git a/include/ur_modern_driver/ros/action_server.h b/include/ur_modern_driver/ros/action_server.h index 1d62b8f..d741714 100644 --- a/include/ur_modern_driver/ros/action_server.h +++ b/include/ur_modern_driver/ros/action_server.h @@ -39,7 +39,7 @@ private: std::condition_variable tj_cv_; std::thread tj_thread_; - TrajectoryFollower& follower_; + ActionTrajectoryFollowerInterface& follower_; RobotState state_; std::array q_actual_, qd_actual_; @@ -61,7 +61,7 @@ private: bool updateState(RTShared& data); public: - ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity); + ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity); void start(); virtual void onRobotStateChange(RobotState state); @@ -70,4 +70,4 @@ public: virtual bool consume(RTState_V1_8& state); virtual bool consume(RTState_V3_0__1& state); virtual bool consume(RTState_V3_2__3& state); -}; \ No newline at end of file +}; diff --git a/include/ur_modern_driver/ros/action_trajectory_follower_interface.h b/include/ur_modern_driver/ros/action_trajectory_follower_interface.h new file mode 100644 index 0000000..9b5427f --- /dev/null +++ b/include/ur_modern_driver/ros/action_trajectory_follower_interface.h @@ -0,0 +1,32 @@ +#pragma once + +#include +#include +#include +#include +#include + +struct TrajectoryPoint +{ + std::array positions; + std::array velocities; + std::chrono::microseconds time_from_start; + + TrajectoryPoint() + { + } + + TrajectoryPoint(std::array &pos, std::array &vel, std::chrono::microseconds tfs) + : positions(pos), velocities(vel), time_from_start(tfs) + { + } +}; + +class ActionTrajectoryFollowerInterface +{ +public: + virtual bool start() = 0; + virtual bool execute(std::vector &trajectory, std::atomic &interrupt) = 0; + virtual void stop() = 0; + virtual ~ActionTrajectoryFollowerInterface() {}; +}; diff --git a/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h new file mode 100644 index 0000000..6442dcb --- /dev/null +++ b/include/ur_modern_driver/ros/lowbandwidth_trajectory_follower.h @@ -0,0 +1,41 @@ +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include "ur_modern_driver/log.h" +#include "ur_modern_driver/ur/commander.h" +#include "ur_modern_driver/ur/server.h" +#include "ur_modern_driver/ros/action_trajectory_follower_interface.h" + +class LowBandwidthTrajectoryFollower: public ActionTrajectoryFollowerInterface +{ +private: + std::atomic running_; + std::array last_positions_; + URCommander &commander_; + URServer server_; + + double time_interval_, servoj_time_, servoj_time_waiting_, max_waiting_time_, \ + servoj_gain_, servoj_lookahead_time_, max_joint_difference_; + + std::string program_; + + bool execute(const std::array &positions, + const std::array &velocities, + double sample_number, double time_in_seconds); + +public: + LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, bool version_3); + + bool start(); + bool execute(std::vector &trajectory, std::atomic &interrupt); + void stop(); + + virtual ~LowBandwidthTrajectoryFollower() {}; +}; diff --git a/include/ur_modern_driver/ros/trajectory_follower.h b/include/ur_modern_driver/ros/trajectory_follower.h index d6c4789..af37474 100644 --- a/include/ur_modern_driver/ros/trajectory_follower.h +++ b/include/ur_modern_driver/ros/trajectory_follower.h @@ -11,24 +11,9 @@ #include "ur_modern_driver/log.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/server.h" +#include "ur_modern_driver/ros/action_trajectory_follower_interface.h" -struct TrajectoryPoint -{ - std::array positions; - std::array velocities; - std::chrono::microseconds time_from_start; - - TrajectoryPoint() - { - } - - TrajectoryPoint(std::array &pos, std::array &vel, std::chrono::microseconds tfs) - : positions(pos), velocities(vel), time_from_start(tfs) - { - } -}; - -class TrajectoryFollower +class TrajectoryFollower : public ActionTrajectoryFollowerInterface { private: std::atomic running_; @@ -57,5 +42,6 @@ public: bool execute(std::array &positions); bool execute(std::vector &trajectory, std::atomic &interrupt); void stop(); - void interrupt(); -}; \ No newline at end of file + + virtual ~TrajectoryFollower() {}; +}; diff --git a/include/ur_modern_driver/tcp_socket.h b/include/ur_modern_driver/tcp_socket.h index 99427b9..4ff2f86 100644 --- a/include/ur_modern_driver/tcp_socket.h +++ b/include/ur_modern_driver/tcp_socket.h @@ -46,6 +46,7 @@ public: std::string getIP(); + bool read(char *character); bool read(uint8_t *buf, size_t buf_len, size_t &read); bool write(const uint8_t *buf, size_t buf_len, size_t &written); diff --git a/include/ur_modern_driver/ur/server.h b/include/ur_modern_driver/ur/server.h index c240ba5..1809237 100644 --- a/include/ur_modern_driver/ur/server.h +++ b/include/ur_modern_driver/ur/server.h @@ -8,6 +8,8 @@ #include #include "ur_modern_driver/tcp_socket.h" +#define MAX_SERVER_BUF_LEN 50 + class URServer : private TCPSocket { private: @@ -24,5 +26,6 @@ public: bool bind(); bool accept(); void disconnectClient(); + bool readLine(char* buffer, size_t buf_len); bool write(const uint8_t *buf, size_t buf_len, size_t &written); -}; \ No newline at end of file +}; diff --git a/launch/ur10_bringup.launch b/launch/ur10_bringup.launch index f27efb3..34582d9 100644 --- a/launch/ur10_bringup.launch +++ b/launch/ur10_bringup.launch @@ -7,24 +7,45 @@ ur10_bringup.launch robot_ip:= --> - + + + + + + + + + + + - + + + + + + + + + + + + diff --git a/launch/ur10_bringup_compatible.launch b/launch/ur10_bringup_compatible.launch index 44387a4..45e0ebd 100644 --- a/launch/ur10_bringup_compatible.launch +++ b/launch/ur10_bringup_compatible.launch @@ -7,25 +7,46 @@ ur10_bringup.launch robot_ip:= --> - + + + + + + + + + + + + - + - + + + + + + + + + + + diff --git a/launch/ur10_bringup_joint_limited.launch b/launch/ur10_bringup_joint_limited.launch index 1ca2d8b..756807b 100644 --- a/launch/ur10_bringup_joint_limited.launch +++ b/launch/ur10_bringup_joint_limited.launch @@ -2,17 +2,27 @@ - + + + + + + + + + + + @@ -21,6 +31,16 @@ + + + + + + + + + + diff --git a/launch/ur3_bringup.launch b/launch/ur3_bringup.launch index f4bc006..22e426b 100644 --- a/launch/ur3_bringup.launch +++ b/launch/ur3_bringup.launch @@ -14,6 +14,16 @@ + + + + + + + + + + @@ -26,6 +36,16 @@ + + + + + + + + + + diff --git a/launch/ur3_bringup_joint_limited.launch b/launch/ur3_bringup_joint_limited.launch index 9c70c52..5f994f1 100644 --- a/launch/ur3_bringup_joint_limited.launch +++ b/launch/ur3_bringup_joint_limited.launch @@ -13,6 +13,16 @@ + + + + + + + + + + @@ -21,6 +31,16 @@ + + + + + + + + + + diff --git a/launch/ur5_bringup.launch b/launch/ur5_bringup.launch index 303d253..eafad38 100644 --- a/launch/ur5_bringup.launch +++ b/launch/ur5_bringup.launch @@ -7,25 +7,45 @@ ur5_bringup.launch robot_ip:= --> - + + + + + + + + + + + - + - + + + + + + + + + + + diff --git a/launch/ur5_bringup_compatible.launch b/launch/ur5_bringup_compatible.launch index c8383bf..ddd0b92 100644 --- a/launch/ur5_bringup_compatible.launch +++ b/launch/ur5_bringup_compatible.launch @@ -7,25 +7,46 @@ ur5_bringup.launch robot_ip:= --> - + + + + + + + + + + + + - + - + + + + + + + + + + + diff --git a/launch/ur5_bringup_joint_limited.launch b/launch/ur5_bringup_joint_limited.launch index 3640a2d..09a0022 100644 --- a/launch/ur5_bringup_joint_limited.launch +++ b/launch/ur5_bringup_joint_limited.launch @@ -13,6 +13,16 @@ + + + + + + + + + + @@ -21,6 +31,16 @@ + + + + + + + + + + diff --git a/launch/ur_common.launch b/launch/ur_common.launch index c8cb7bd..37eea20 100644 --- a/launch/ur_common.launch +++ b/launch/ur_common.launch @@ -2,7 +2,7 @@ @@ -12,14 +12,22 @@ + + + + + + + + - + @@ -30,10 +38,18 @@ + + + + + + + + diff --git a/src/ros/action_server.cpp b/src/ros/action_server.cpp index 494d932..8616c35 100644 --- a/src/ros/action_server.cpp +++ b/src/ros/action_server.cpp @@ -1,7 +1,7 @@ #include "ur_modern_driver/ros/action_server.h" #include -ActionServer::ActionServer(TrajectoryFollower& follower, std::vector& joint_names, double max_velocity) +ActionServer::ActionServer(ActionTrajectoryFollowerInterface& follower, std::vector& joint_names, double max_velocity) : as_(nh_, "follow_joint_trajectory", boost::bind(&ActionServer::onGoal, this, _1), boost::bind(&ActionServer::onCancel, this, _1), false) , joint_names_(joint_names) @@ -307,6 +307,7 @@ void ActionServer::trajectoryThread() Result res; + LOG_INFO("Attempting to start follower %p", &follower_); if (follower_.start()) { if (follower_.execute(trajectory, interrupt_traj_)) diff --git a/src/ros/lowbandwidth_trajectory_follower.cpp b/src/ros/lowbandwidth_trajectory_follower.cpp new file mode 100644 index 0000000..75c47f7 --- /dev/null +++ b/src/ros/lowbandwidth_trajectory_follower.cpp @@ -0,0 +1,386 @@ +#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h" +#include +#include +#include + +static const std::array EMPTY_VALUES = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + +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 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 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}} + 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) + end + + def is_waypoint_sentinel(waypoint): + local l_previous_index = 2 + while l_previous_index < 1 + JOINT_NUM * 2 + 2: + if waypoint[l_previous_index] != 0.0: + return False + end + l_previous_index = l_previous_index + 1 + end + 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 + local c = (-3 * a + 3 * l_end_pos - 2 * total_segment_time * b - total_segment_time * end_vel) / pow(total_segment_time, 2) + local d = (2 * a - 2 * l_end_pos + total_segment_time * b + total_segment_time * end_vel) / pow(total_segment_time, 3) + return a + b * time_within_segment + c * pow(time_within_segment, 2) + d * pow(time_within_segment, 3) + end + + def add_next_waypoint(waypoint): + enter_critical + g_position_previous = g_position_target + g_velocity_previous = g_velocity_target + g_time_previous = g_time_target + g_num_previous = g_num_target + g_position_target = g_position_next + g_velocity_target = g_velocity_next + g_time_target = g_time_next + g_num_target = g_num_next + 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] + g_received_waypoints_number = g_num_next + exit_critical + end + + 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 + exit_critical + local l_max_waiting_time_left = MAX_WAITING_TIME + while l_received_waypoints_number < l_requested_waypoints_number and l_max_waiting_time_left > 0: + 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 + enter_critical + local l_start_pos = g_position_previous + local l_start_vel = g_velocity_previous + local l_start_time = g_time_previous + local l_start_num= g_num_previous + local l_end_pos = g_position_target + local l_end_vel = g_velocity_target + local l_end_time = g_time_target + local l_end_num = g_num_target + local l_total_elapsed_time = g_total_elapsed_time + local l_stopping_after_next_interpolation = g_stopping + g_requested_waypoints_number = g_requested_waypoints_number + 1 + exit_critical + + l_current_position = l_start_pos + + local l_total_segment_time = l_end_time - l_start_time + + while l_total_elapsed_time <= l_end_time: + local l_segment_elapsed_time = l_total_elapsed_time - l_start_time + 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 + 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 + + end + if l_stopping_after_next_interpolation: + while not is_final_position_reached(l_end_pos): + textmsg("Catching up on final position not reached first time.") + servoj(l_end_pos,t=SERVOJ_TIME,lookahead_time=SERVOJ_LOOKAHEAD_TIME,gain=SERVOJ_GAIN) + end + textmsg("Finishing the controlling thread. Final position reached.") + break + end + end + end + + thread sendingThread(): + local controlling_thread = run controllingThread() + local l_sent_waypoints_number = -1 + local l_requested_waypoints_number = -1 + local l_stopping = False + + enter_critical + l_requested_waypoints_number = g_requested_waypoints_number + l_stopping = g_stopping + exit_critical + while not l_stopping: + while l_sent_waypoints_number == l_requested_waypoints_number and not l_stopping: + sleep(SERVOJ_TIME_WAITING) + + enter_critical + l_requested_waypoints_number = g_requested_waypoints_number + l_stopping = g_stopping + exit_critical + + end + if l_stopping: + break + end + send_message(l_sent_waypoints_number + 1) + l_sent_waypoints_number = l_sent_waypoints_number + 1 + end + join controlling_thread + end + + thread receivingThread(): + local sending_thread = run sendingThread() + while True: + waypoint_received = socket_read_ascii_float(14, CONNECTION_NAME) + if waypoint_received[0] == 0: + 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) + enter_critical + g_stopping = True + g_received_waypoints_number = g_received_waypoints_number + 1 + exit_critical + break + end + add_next_waypoint(waypoint_received) + end + join sending_thread + end + socket_open(REVERSE_IP, REVERSE_PORT, CONNECTION_NAME) + receiving_thread = run receivingThread() + join receiving_thread + socket_close(CONNECTION_NAME) + textmsg("Exiting the program") +end + +)"; + +LowBandwidthTrajectoryFollower::LowBandwidthTrajectoryFollower(URCommander &commander, std::string &reverse_ip, int reverse_port, + bool version_3) + : running_(false) + , commander_(commander) + , server_(reverse_port) + , time_interval_(0.008) + , servoj_time_(0.008) + , servoj_time_waiting_(0.001) + , max_waiting_time_(2.0) + , servoj_gain_(300.0) + , servoj_lookahead_time_(0.03) + , max_joint_difference_(0.01) +{ + ros::param::get("~time_interval", time_interval_); + ros::param::get("~servoj_time", servoj_time_); + ros::param::get("~servoj_time_waiting", servoj_time_waiting_); + 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("~max_joint_difference", max_joint_difference_); + + std::string res(POSITION_PROGRAM); + std::ostringstream out; + if (!version_3) { + LOG_ERROR("Low Bandwidth Trajectory Follower only works for interface version > 3"); + std::exit(-1); + } + res.replace(res.find(TIME_INTERVAL), TIME_INTERVAL.length(), std::to_string(time_interval_)); + res.replace(res.find(SERVOJ_TIME_WAITING), SERVOJ_TIME_WAITING.length(), std::to_string(servoj_time_waiting_)); + res.replace(res.find(SERVOJ_TIME), SERVOJ_TIME.length(), std::to_string(servoj_time_)); + 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(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()) + { + LOG_ERROR("Failed to bind server, the port %d is likely already in use", reverse_port); + std::exit(-1); + } + LOG_INFO("Low Bandwidth Trajectory Follower is initialized!"); +} + +bool LowBandwidthTrajectoryFollower::start() +{ + LOG_INFO("Starting LowBandwidthTrajectoryFollower"); + + if (running_) + return true; // not sure + + LOG_INFO("Uploading trajectory program to robot"); + + if (!commander_.uploadProg(program_)) + { + LOG_ERROR("Program upload failed!"); + return false; + } + + LOG_DEBUG("Awaiting incoming robot connection"); + + if (!server_.accept()) + { + LOG_ERROR("Failed to accept incoming robot connection"); + return false; + } + + LOG_DEBUG("Robot successfully connected"); + return (running_ = true); +} + +bool LowBandwidthTrajectoryFollower::execute(const std::array &positions, + const std::array &velocities, + double sample_number, double time_in_seconds) +{ + if (!running_) + return false; + + std::ostringstream out; + + out << "("; + out << sample_number << ","; + for (auto const &pos: positions) + { + out << pos << ","; + } + for (auto const &vel: velocities) + { + out << vel << ","; + } + out << time_in_seconds << ")\r\n"; + + // I know it's ugly but it's the most efficient and fastest way + // We have only ASCII characters and we can cast char -> uint_8 + const std::string tmp = out.str(); + const char *formatted_message = tmp.c_str(); + const uint8_t *buf = (uint8_t *) formatted_message; + + size_t written; + LOG_DEBUG("Sending message %s", formatted_message); + + return server_.write(buf, strlen(formatted_message) + 1, written); +} + +bool LowBandwidthTrajectoryFollower::execute(std::vector &trajectory, std::atomic &interrupt) +{ + if (!running_) + return false; + + bool finished = false; + + char* line[MAX_SERVER_BUF_LEN]; + + bool res = true; + + while (!finished && !interrupt) + { + if (!server_.readLine((char *)line, MAX_SERVER_BUF_LEN)) + { + LOG_DEBUG("Connection closed. Finishing!"); + finished = true; + break; + } + unsigned int message_num=atoi((const char *) line); + LOG_DEBUG("Received request %i", message_num); + if (message_num < trajectory.size()) + { + res = execute(trajectory[message_num].positions, trajectory[message_num].velocities, + message_num, trajectory[message_num].time_from_start.count() / 1e6); + } else + { + res = execute(EMPTY_VALUES, EMPTY_VALUES, message_num, 0.0); + } + if (!res) + { + finished = true; + } + } + return res; +} + +void LowBandwidthTrajectoryFollower::stop() +{ + if (!running_) + return; + + server_.disconnectClient(); + running_ = false; +} diff --git a/src/ros_main.cpp b/src/ros_main.cpp index 123ecc4..5343334 100644 --- a/src/ros_main.cpp +++ b/src/ros_main.cpp @@ -13,6 +13,7 @@ #include "ur_modern_driver/ros/rt_publisher.h" #include "ur_modern_driver/ros/service_stopper.h" #include "ur_modern_driver/ros/trajectory_follower.h" +#include "ur_modern_driver/ros/lowbandwidth_trajectory_follower.h" #include "ur_modern_driver/ros/urscript_handler.h" #include "ur_modern_driver/ur/commander.h" #include "ur_modern_driver/ur/factory.h" @@ -25,6 +26,7 @@ static const std::string IP_ADDR_ARG("~robot_ip_address"); static const std::string REVERSE_PORT_ARG("~reverse_port"); static const std::string ROS_CONTROL_ARG("~use_ros_control"); +static const std::string LOW_BANDWIDTH_TRAJECTORY_FOLLOWER("~use_lowbandwidth_trajectory_follower"); static const std::string MAX_VEL_CHANGE_ARG("~max_vel_change"); static const std::string PREFIX_ARG("~prefix"); static const std::string BASE_FRAME_ARG("~base_frame"); @@ -53,6 +55,7 @@ public: double max_vel_change; int32_t reverse_port; bool use_ros_control; + bool use_lowbandwidth_trajectory_follower; bool shutdown_on_disconnect; }; @@ -92,6 +95,7 @@ bool parse_args(ProgArgs &args) ros::param::param(MAX_VEL_CHANGE_ARG, args.max_vel_change, 15.0); // rad/s ros::param::param(MAX_VEL_CHANGE_ARG, args.max_velocity, 10.0); ros::param::param(ROS_CONTROL_ARG, args.use_ros_control, false); + ros::param::param(LOW_BANDWIDTH_TRAJECTORY_FOLLOWER, args.use_lowbandwidth_trajectory_follower, false); ros::param::param(PREFIX_ARG, args.prefix, std::string()); ros::param::param(BASE_FRAME_ARG, args.base_frame, args.prefix + "base_link"); ros::param::param(TOOL_FRAME_ARG, args.tool_frame, args.prefix + "tool0_controller"); @@ -134,22 +138,34 @@ int main(int argc, char **argv) auto rt_commander = factory.getCommander(rt_stream); vector *> rt_vec{ &rt_pub }; - TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); - INotifier *notifier(nullptr); ROSController *controller(nullptr); ActionServer *action_server(nullptr); if (args.use_ros_control) { LOG_INFO("ROS control enabled"); - controller = new ROSController(*rt_commander, traj_follower, args.joint_names, args.max_vel_change, args.tcp_link); + TrajectoryFollower *traj_follower = new TrajectoryFollower( + *rt_commander, local_ip, args.reverse_port, factory.isVersion3()); + controller = new ROSController(*rt_commander, *traj_follower, args.joint_names, args.max_vel_change, args.tcp_link); rt_vec.push_back(controller); services.push_back(controller); } else { LOG_INFO("ActionServer enabled"); - action_server = new ActionServer(traj_follower, args.joint_names, args.max_velocity); + ActionTrajectoryFollowerInterface *traj_follower(nullptr); + if (args.use_lowbandwidth_trajectory_follower) + { + LOG_INFO("Use low bandwidth trajectory follower"); + traj_follower = new LowBandwidthTrajectoryFollower(*rt_commander, + local_ip, args.reverse_port,factory.isVersion3()); + } + else + { + LOG_INFO("Use standard trajectory follower"); + traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3()); + } + action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity); rt_vec.push_back(action_server); services.push_back(action_server); } diff --git a/src/tcp_socket.cpp b/src/tcp_socket.cpp index 0bd390a..6a057bd 100644 --- a/src/tcp_socket.cpp +++ b/src/tcp_socket.cpp @@ -90,7 +90,7 @@ void TCPSocket::close() if (state_ != SocketState::Connected) return; state_ = SocketState::Closed; - ::shutdown(socket_fd_, SHUT_RDWR); + ::close(socket_fd_); socket_fd_ = -1; } @@ -111,6 +111,16 @@ std::string TCPSocket::getIP() return std::string(buf); } +bool TCPSocket::read(char *character) +{ + size_t read_chars; + // It's inefficient, but in our case we read very small messages + // and the overhead connected with reading character by character is + // negligible - adding buffering would complicate the code needlessly. + return read((uint8_t *) character, 1, read_chars); +} + + bool TCPSocket::read(uint8_t *buf, size_t buf_len, size_t &read) { read = 0; @@ -154,4 +164,4 @@ bool TCPSocket::write(const uint8_t *buf, size_t buf_len, size_t &written) } return true; -} \ No newline at end of file +} diff --git a/src/ur/server.cpp b/src/ur/server.cpp index 1d54869..557ed5d 100644 --- a/src/ur/server.cpp +++ b/src/ur/server.cpp @@ -86,3 +86,44 @@ bool URServer::write(const uint8_t* buf, size_t buf_len, size_t& written) { return client_.write(buf, buf_len, written); } + +bool URServer::readLine(char* buffer, size_t buf_len) +{ + char *current_pointer = buffer; + char ch; + size_t total_read; + + if (buf_len <= 0 || buffer == NULL) { + return false; + } + + total_read = 0; + for (;;) { + if (client_.read(&ch)) + { + if (total_read < buf_len - 1) // just in case ... + { + total_read ++; + *current_pointer++ = ch; + } + if (ch == '\n') + { + break; + } + } + else + { + if (total_read == 0) + { + return false; + } + else + { + break; + } + } + } + + *current_pointer = '\0'; + return true; +}