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

Adds Safe Trajectory Follower implementation

Browse Source 
Safe Trajectory Follower implements different approach for controlling
the robot. Rather than calculate the interpolation steps in the driver
and send the small interpolated steps over the network to the URScript
program with 500Hz frequency, the coarser MoveIt trajectory is sent
(with few Hz) and the interpolation steps are calculated by the
URScript.

The algorithm for time progress has also built-in protection against
any delays induced by load on the driver, network or URControl - it
will never "catch-up" dangerously when such delay are introduced,
It will rather pause and wait for the next small interpolation step
instructions and re-start the move slower - never skipping any
interpolated steps.

Those changes make Safe Trajectory Follower much more resilient to
network communication problems and removes any superficial requirements
for the network setup, kernel latency and no-load-requirement for the
driver's PC - making it much more suitable for research, development
and quick iteration loops. It works reliably even over WiFi.
This commit is contained in:
Jarek Potiuk
2017-12-29 09:37:56 +01:00
parent f71c83c649
commit 5dcef72415
14 changed files with 734 additions and 31 deletions
Download Patch File Download Diff File Expand all files Collapse all files

24
src/ros_main.cpp
View File

@@ -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/safe_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 SAFE_TRAJECTORY_FOLLOWER("~use_safe_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_safe_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(SAFE_TRAJECTORY_FOLLOWER, args.use_safe_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,7 +138,20 @@ int main(int argc, char **argv)
auto rt_commander = factory.getCommander(rt_stream);
vector<IConsumer<RTPacket> *> rt_vec{ &rt_pub };
TrajectoryFollower traj_follower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
TrajectoryFollowerInterface *traj_follower(nullptr);
if (args.use_safe_trajectory_follower && !args.use_ros_control)
{
LOG_INFO("Use safe trajectory follower");
traj_follower = new SafeTrajectoryFollower(*rt_commander, local_ip, args.reverse_port,factory.isVersion3());
// We are leaking the follower here, but it's OK as this is once-a-lifetime event
}
else
{
LOG_INFO("Use standard trajectory follower");
traj_follower = new TrajectoryFollower(*rt_commander, local_ip, args.reverse_port, factory.isVersion3());
// We are leaking the follower here, but it's OK as this is once-a-lifetime event
}
INotifier *notifier(nullptr);
ROSController *controller(nullptr);
@@ -142,14 +159,15 @@ int main(int argc, char **argv)
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);
// Note - we are sure that TrajectoryFollower is used here (see the args.use_ros_control above)
controller = new ROSController(*rt_commander, *((TrajectoryFollower *) 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);
action_server = new ActionServer(*traj_follower, args.joint_names, args.max_velocity);
rt_vec.push_back(action_server);
services.push_back(action_server);
}