Unverified Commit 5cc6fdb2 authored by Tom Queen's avatar Tom Queen Committed by GitHub
Browse files

Merge branch 'master' into adding_non_blocking_read

parents 654622f9 f2db8f3d
......@@ -208,7 +208,7 @@ the output `Robot ready to receive control commands.`
**Note:** When interacting with the teach pendant, or sending other primary programs to the robot, the
program will be stopped. On the ROS terminal you will see an output `Connection to robot dropped,
waiting for new connection`. In those cases, restart program execution (e.g. by pressing the play
button on the TP).
button on the TP, or calling `rosservice call /ur_hardware_interface/dashboard/play` as explained [here](https://github.com/UniversalRobots/Universal_Robots_ROS_Driver/issues/33) and [here](ur_robot_driver/doc/ROS_INTERFACE.md#ur_robot_driver_node)).
In general, make sure you've completed the following tasks:
......
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 125
loop_hz: &loop_hz 125
# Settings for ros_control hardware interface
hardware_interface:
......@@ -15,17 +15,17 @@ hardware_interface:
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 125
publish_rate: *loop_hz
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 125
publish_rate: *loop_hz
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 125
publish_rate: *loop_hz
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
......@@ -42,7 +42,7 @@ scaled_pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
state_publish_rate: *loop_hz
action_monitor_rate: 10
pos_traj_controller:
......@@ -58,5 +58,5 @@ pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
state_publish_rate: *loop_hz
action_monitor_rate: 10
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 500
loop_hz: &loop_hz 500
# Settings for ros_control hardware interface
hardware_interface:
......@@ -15,17 +15,17 @@ hardware_interface:
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 500
publish_rate: *loop_hz
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 500
publish_rate: *loop_hz
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 500
publish_rate: *loop_hz
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
......@@ -42,7 +42,7 @@ scaled_pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
state_publish_rate: *loop_hz
action_monitor_rate: 10
pos_traj_controller:
......@@ -58,5 +58,5 @@ pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
state_publish_rate: *loop_hz
action_monitor_rate: 10
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 125
loop_hz: &loop_hz 125
# Settings for ros_control hardware interface
hardware_interface:
......@@ -15,17 +15,17 @@ hardware_interface:
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 125
publish_rate: *loop_hz
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 125
publish_rate: *loop_hz
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 125
publish_rate: *loop_hz
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
......@@ -42,7 +42,7 @@ scaled_pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
state_publish_rate: *loop_hz
action_monitor_rate: 10
pos_traj_controller:
......@@ -58,5 +58,5 @@ pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
state_publish_rate: *loop_hz
action_monitor_rate: 10
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 500
loop_hz: &loop_hz 500
# Settings for ros_control hardware interface
hardware_interface:
......@@ -15,17 +15,17 @@ hardware_interface:
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 500
publish_rate: *loop_hz
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 500
publish_rate: *loop_hz
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 500
publish_rate: *loop_hz
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
......@@ -42,7 +42,7 @@ scaled_pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
state_publish_rate: *loop_hz
action_monitor_rate: 10
pos_traj_controller:
......@@ -58,5 +58,5 @@ pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
state_publish_rate: *loop_hz
action_monitor_rate: 10
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 125
loop_hz: &loop_hz 125
# Settings for ros_control hardware interface
hardware_interface:
......@@ -15,17 +15,17 @@ hardware_interface:
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 125
publish_rate: *loop_hz
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 125
publish_rate: *loop_hz
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 125
publish_rate: *loop_hz
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
......@@ -42,7 +42,7 @@ scaled_pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
state_publish_rate: *loop_hz
action_monitor_rate: 10
pos_traj_controller:
......@@ -58,5 +58,5 @@ pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 125
state_publish_rate: *loop_hz
action_monitor_rate: 10
# Settings for ros_control control loop
hardware_control_loop:
loop_hz: 500
loop_hz: &loop_hz 500
# Settings for ros_control hardware interface
hardware_interface:
......@@ -15,17 +15,17 @@ hardware_interface:
# Publish all joint states ----------------------------------
joint_state_controller:
type: joint_state_controller/JointStateController
publish_rate: 500
publish_rate: *loop_hz
# Publish wrench ----------------------------------
force_torque_sensor_controller:
type: force_torque_sensor_controller/ForceTorqueSensorController
publish_rate: 500
publish_rate: *loop_hz
# Publish speed_scaling factor
speed_scaling_state_controller:
type: ur_controllers/SpeedScalingStateController
publish_rate: 500
publish_rate: *loop_hz
# Joint Trajectory Controller - position based -------------------------------
# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller
......@@ -42,7 +42,7 @@ scaled_pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
state_publish_rate: *loop_hz
action_monitor_rate: 10
pos_traj_controller:
......@@ -58,5 +58,5 @@ pos_traj_controller:
wrist_2_joint: {trajectory: 0.2, goal: 0.1}
wrist_3_joint: {trajectory: 0.2, goal: 0.1}
stop_trajectory_duration: 0.5
state_publish_rate: 500
state_publish_rate: *loop_hz
action_monitor_rate: 10
......@@ -822,6 +822,14 @@ This is the actual driver node containing the ROS-Control stack. Interfaces docu
Should the tool's RS485 interface be forwarded to the ROS machine? This is only available on e-Series models. Setting this parameter to TRUE requires multiple other parameters to be set,as well.
* "**servoj_gain**" (default: "2000")
Specify gain for servoing to position in joint space. A higher gain can sharpen the trajectory.
* "**servoj_gain**" (default: "0.03")
Specify lookahead time for servoing to position in joint space. A longer lookahead time can smooth the trajectory.
#### Published topics
* "**robot_program_running**" ([std_msgs/Bool](http://docs.ros.org/api/std_msgs/html/msg/Bool.html))
......
......@@ -63,12 +63,14 @@ public:
* \param script_sending_port The driver will offer an interface to receive the program's URScript on this port. If
* the robot cannot connect to this port, `External Control` will stop immediately.
* \param non_blocking_read Enable non-blocking mode for read (useful when used with combined_robot_hw)
* \param servoj_gain Proportional gain for arm joints following target position, range [100,2000]
* \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
*/
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum = "",
const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002,
bool non_blocking_read = false);
const uint32_t reverse_port = 50001, const uint32_t script_sender_port = 50002, int servoj_gain = 2000,
double servoj_lookahead_time = 0.03, bool non_blocking_read = false);
/*!
* \brief Constructs a new UrDriver object.
*
......@@ -85,14 +87,17 @@ public:
* \param script_sending_port The driver will offer an interface to receive the program's URScript on this port.
* If the robot cannot connect to this port, `External Control` will stop immediately.
* \param non_blocking_read Enable non-blocking mode for read (useful when used with combined_robot_hw)
* \param servoj_gain Proportional gain for arm joints following target position, range [100,2000]
* \param servoj_lookahead_time Time [S], range [0.03,0.2] smoothens the trajectory with this lookahead time
*/
UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& output_recipe_file,
const std::string& input_recipe_file, std::function<void(bool)> handle_program_state, bool headless_mode,
const std::string& calibration_checksum = "", const uint32_t reverse_port = 50001,
const uint32_t script_sender_port = 50002, bool non_blocking_read = false)
const uint32_t script_sender_port = 50002, int servoj_gain = 2000, double servoj_lookahead_time = 0.03,
bool non_blocking_read = false)
: UrDriver(robot_ip, script_file, output_recipe_file, input_recipe_file, handle_program_state, headless_mode,
std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port,
non_blocking_read)
std::unique_ptr<ToolCommSetup>{}, calibration_checksum, reverse_port, script_sender_port, servoj_gain,
servoj_lookahead_time, non_blocking_read)
{
}
......
......@@ -109,6 +109,24 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
// synchronises read/write operations.
robot_hw_nh.param("non_blocking_read", non_blocking_read_, false);
// Specify gain for servoing to position in joint space.
// A higher gain can sharpen the trajectory.
int servoj_gain = robot_hw_nh.param("servoj_gain", 2000);
if ((servoj_gain > 2000) || (servoj_gain < 100))
{
ROS_ERROR_STREAM("servoj_gain is " << servoj_gain << ", must be in range [100, 2000]");
return false;
}
// Specify lookahead time for servoing to position in joint space.
// A longer lookahead time can smooth the trajectory.
double servoj_lookahead_time = robot_hw_nh.param("servoj_lookahead_time", 0.03);
if ((servoj_lookahead_time > 0.2) || (servoj_lookahead_time < 0.03))
{
ROS_ERROR_STREAM("servoj_lookahead_time is " << servoj_lookahead_time << ", must be in range [0.03, 0.2]");
return false;
}
// Whenever the runtime state of the "External Control" program node in the UR-program changes, a
// message gets published here. So this is equivalent to the information whether the robot accepts
// commands from ROS side.
......@@ -219,7 +237,8 @@ bool HardwareInterface::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_hw
ur_driver_.reset(new UrDriver(robot_ip_, script_filename, output_recipe_filename, input_recipe_filename,
std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1),
headless_mode, std::move(tool_comm_setup), calibration_checksum,
(uint32_t)reverse_port, (uint32_t)script_sender_port, non_blocking_read_));
(uint32_t)reverse_port, (uint32_t)script_sender_port, servoj_gain,
servoj_lookahead_time, non_blocking_read_));
}
catch (ur_driver::ToolCommNotAvailable& e)
{
......
......@@ -51,10 +51,11 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc
const std::string& output_recipe_file, const std::string& input_recipe_file,
std::function<void(bool)> handle_program_state, bool headless_mode,
std::unique_ptr<ToolCommSetup> tool_comm_setup, const std::string& calibration_checksum,
const uint32_t reverse_port, const uint32_t script_sender_port, bool non_blocking_read)
const uint32_t reverse_port, const uint32_t script_sender_port, int servoj_gain,
double servoj_lookahead_time, bool non_blocking_read)
: servoj_time_(0.008)
, servoj_gain_(2000)
, servoj_lookahead_time_(0.03)
, servoj_gain_(servoj_gain)
, servoj_lookahead_time_(servoj_lookahead_time)
, reverse_interface_active_(false)
, reverse_port_(reverse_port)
, handle_program_state_(handle_program_state)
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment