From 98a6e05d185d483cc14259cf744cf0db6fb44620 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 24 Jun 2019 11:13:04 +0200 Subject: [PATCH 01/10] added socat as exec_dependency socat was missing as an execution dependency, although is is needed by the tool_communication script. This solves #2 --- ur_rtde_driver/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/ur_rtde_driver/package.xml b/ur_rtde_driver/package.xml index 4d47acb..f9ae0b2 100644 --- a/ur_rtde_driver/package.xml +++ b/ur_rtde_driver/package.xml @@ -39,6 +39,7 @@ joint_state_controller joint_trajectory_controller robot_state_publisher + socat ur_description velocity_controllers From 792c621f82352ba95d5daa6aa6892e42b06cd00f Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Mon, 24 Jun 2019 12:10:26 +0200 Subject: [PATCH 02/10] correct installation location of tool_comm script --- ur_rtde_driver/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt index c832a47..6032624 100644 --- a/ur_rtde_driver/CMakeLists.txt +++ b/ur_rtde_driver/CMakeLists.txt @@ -119,7 +119,7 @@ install(TARGETS ur_rtde_driver ur_rtde_driver_node ) install(PROGRAMS scripts/tool_communication - DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION}) + DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) install(DIRECTORY config launch DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) From bd4df1ff5e2eead50c608e1fbc503a008e97a20d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rune=20S=C3=B8e-Knudsen?= Date: Mon, 24 Jun 2019 12:04:17 +0200 Subject: [PATCH 03/10] Adjust critical sections Optimised the used of the the enter_critical and exit_critical sections --- ur_rtde_driver/resources/servoj.urscript | 12 ++++-------- 1 file changed, 4 insertions(+), 8 deletions(-) diff --git a/ur_rtde_driver/resources/servoj.urscript b/ur_rtde_driver/resources/servoj.urscript index d44268f..de285a0 100644 --- a/ur_rtde_driver/resources/servoj.urscript +++ b/ur_rtde_driver/resources/servoj.urscript @@ -12,19 +12,15 @@ global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_q = get_actual_joint_positions() global cmd_servo_q_last = get_actual_joint_positions() def set_servo_setpoint(q): - enter_critical cmd_servo_state = SERVO_RUNNING cmd_servo_q_last = cmd_servo_q cmd_servo_q = q - exit_critical end def extrapolate(): - enter_critical diff = [cmd_servo_q[0] - cmd_servo_q_last[0], cmd_servo_q[1] - cmd_servo_q_last[1], cmd_servo_q[2] - cmd_servo_q_last[2], cmd_servo_q[3] - cmd_servo_q_last[3], cmd_servo_q[4] - cmd_servo_q_last[4], cmd_servo_q[5] - cmd_servo_q_last[5]] cmd_servo_q_last = cmd_servo_q cmd_servo_q = [cmd_servo_q[0] + diff[0], cmd_servo_q[1] + diff[1], cmd_servo_q[2] + diff[2], cmd_servo_q[3] + diff[3], cmd_servo_q[4] + diff[4], cmd_servo_q[5] + diff[5]] - exit_critical return cmd_servo_q end @@ -42,7 +38,6 @@ thread servoThread(): if cmd_servo_state > SERVO_UNINITIALIZED: cmd_servo_state = SERVO_IDLE end - exit_critical if do_extrapolate: textmsg("No new setpoint received. Extrapolating.") q = extrapolate() @@ -52,6 +47,7 @@ thread servoThread(): else: sync() end + exit_critical end textmsg("servo thread ended") stopj(0.1) @@ -63,6 +59,7 @@ keepalive = -2 params_mult = socket_read_binary_integer(6+1, "reverse_socket") keepalive = params_mult[7] while keepalive > 0: + enter_critical socket_send_line(1, "reverse_socket") params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation if params_mult[0] > 0: @@ -72,11 +69,10 @@ while keepalive > 0: else: keepalive = keepalive - 1 end + exit_critical end textmsg("Stopping communication and servoing") -enter_critical - cmd_servo_state = SERVO_STOPPED -exit_critical +cmd_servo_state = SERVO_STOPPED sleep(.1) socket_close() kill thread_servo From 72330cb98b937cfaaa430c18907de1a936266b17 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Rune=20S=C3=B8e-Knudsen?= Date: Mon, 24 Jun 2019 12:09:23 +0200 Subject: [PATCH 04/10] Change extrapolation log messages strategy Change the log strategy regarding extrapolation from make a message every time, to show the maximum number of extrapolation steps observed in a row and the current count In this way the log will not be filled up with identical messages, hiding other messages. Valuable when running on an unstable network like WIFI --- ur_rtde_driver/resources/servoj.urscript | 30 ++++++++++++++++++------ 1 file changed, 23 insertions(+), 7 deletions(-) diff --git a/ur_rtde_driver/resources/servoj.urscript b/ur_rtde_driver/resources/servoj.urscript index de285a0..0811219 100644 --- a/ur_rtde_driver/resources/servoj.urscript +++ b/ur_rtde_driver/resources/servoj.urscript @@ -1,16 +1,22 @@ {{BEGIN_REPLACE}} -global steptime = get_steptime() +steptime = get_steptime() textmsg("steptime=", steptime) -global MULT_jointstate = {{JOINT_STATE_REPLACE}} +MULT_jointstate = {{JOINT_STATE_REPLACE}} -global SERVO_STOPPED = -2 -global SERVO_UNINITIALIZED = -1 -global SERVO_IDLE = 0 -global SERVO_RUNNING = 1 +#Constants +SERVO_STOPPED = -2 +SERVO_UNINITIALIZED = -1 +SERVO_IDLE = 0 +SERVO_RUNNING = 1 + +#Global variables are also showed in the Teach pendants variable list global cmd_servo_state = SERVO_UNINITIALIZED global cmd_servo_q = get_actual_joint_positions() global cmd_servo_q_last = get_actual_joint_positions() +global extrapolate_count = 0 +global extrapolate_max_count = 0 + def set_servo_setpoint(q): cmd_servo_state = SERVO_RUNNING cmd_servo_q_last = cmd_servo_q @@ -38,13 +44,22 @@ thread servoThread(): if cmd_servo_state > SERVO_UNINITIALIZED: cmd_servo_state = SERVO_IDLE end + if do_extrapolate: - textmsg("No new setpoint received. Extrapolating.") + extrapolate_count = extrapolate_count + 1 + if extrapolate_count > extrapolate_max_count: + extrapolate_max_count = extrapolate_count + end + q = extrapolate() servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + elif state == SERVO_RUNNING: + extrapolate_count = 0 servoj(q, t=steptime, {{SERVO_J_REPLACE}}) + else: + extrapolate_count = 0 sync() end exit_critical @@ -71,6 +86,7 @@ while keepalive > 0: end exit_critical end + textmsg("Stopping communication and servoing") cmd_servo_state = SERVO_STOPPED sleep(.1) From 1caa2c4a97dc4a879e89127f7574d9d3de539dc7 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 26 Jun 2019 08:44:02 +0200 Subject: [PATCH 05/10] make ScaledJointHandle inherit from JointHandle This reduces code duplication --- .../scaled_joint_command_interface.h | 25 +++++-------------- 1 file changed, 6 insertions(+), 19 deletions(-) diff --git a/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h b/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h index 7a828f1..edec13e 100644 --- a/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h +++ b/ur_controllers/include/ur_controllers/scaled_joint_command_interface.h @@ -44,28 +44,26 @@ #include #include +#include namespace ur_controllers { -class ScaledJointHandle : public hardware_interface::JointStateHandle +class ScaledJointHandle : public hardware_interface::JointHandle { public: - ScaledJointHandle() : cmd_(0), scaling_factor_(0) + ScaledJointHandle() : hardware_interface::JointHandle(), scaling_factor_(0) + { } /** * \param js This joint's state handle * \param cmd A pointer to the storage for this joint's output command + * \param scaling_factor A pointer to the storage for this joint's scaling factor */ ScaledJointHandle(const hardware_interface::JointStateHandle& js, double* cmd, double* scaling_factor) - : hardware_interface::JointStateHandle(js), cmd_(cmd), scaling_factor_(scaling_factor) + : hardware_interface::JointHandle(js, cmd), scaling_factor_(scaling_factor) { - if (cmd_ == nullptr) - { - throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + - "'. Command data pointer is null."); - } if (scaling_factor_ == nullptr) { throw hardware_interface::HardwareInterfaceException("Cannot create handle '" + js.getName() + @@ -75,16 +73,6 @@ public: virtual ~ScaledJointHandle() = default; - void setCommand(double command) - { - assert(cmd_); - *cmd_ = command; - } - double getCommand() const - { - assert(cmd_); - return *cmd_; - } void setScalingFactor(double scaling_factor) { assert(scaling_factor_); @@ -97,7 +85,6 @@ public: } private: - double* cmd_; double* scaling_factor_; }; From 8c9f79bc3b53ccf829961337fc7b4c479543d8ad Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 26 Jun 2019 08:46:21 +0200 Subject: [PATCH 06/10] turn around order of commands and keepalive This way we can leave the commands empty very easily --- .../ur_rtde_driver/comm/reverse_interface.h | 23 +++++++++++-------- ur_rtde_driver/resources/servoj.urscript | 16 ++++++++----- 2 files changed, 23 insertions(+), 16 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h index 24a190b..d8ecfe1 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/comm/reverse_interface.h @@ -57,20 +57,23 @@ public: server_.disconnectClient(); } - bool write(const vector6d_t& positions) + bool write(const vector6d_t* positions, const int32_t type = 2) { uint8_t buffer[sizeof(uint32_t) * 7]; uint8_t* b_pos = buffer; - for (auto const& pos : positions) - { - int32_t val = static_cast(pos * MULT_JOINTSTATE); - val = htobe32(val); - b_pos += append(b_pos, val); - } - // TODO: Make this a parameter: Number of allowed missed packages - int32_t val = htobe32(5); - append(b_pos, val); + int32_t val = htobe32(type); + b_pos += append(b_pos, val); + + if (positions != nullptr) + { + for (auto const& pos : *positions) + { + int32_t val = static_cast(pos * MULT_JOINTSTATE); + val = htobe32(val); + b_pos += append(b_pos, val); + } + } size_t written; diff --git a/ur_rtde_driver/resources/servoj.urscript b/ur_rtde_driver/resources/servoj.urscript index 0811219..f12b785 100644 --- a/ur_rtde_driver/resources/servoj.urscript +++ b/ur_rtde_driver/resources/servoj.urscript @@ -71,16 +71,20 @@ socket_open("{{SERVER_IP_REPLACE}}", {{SERVER_PORT_REPLACE}}, "reverse_socket") thread_servo = run servoThread() keepalive = -2 -params_mult = socket_read_binary_integer(6+1, "reverse_socket") -keepalive = params_mult[7] +params_mult = socket_read_binary_integer(1+6, "reverse_socket") +keepalive = params_mult[1] while keepalive > 0: enter_critical socket_send_line(1, "reverse_socket") - params_mult = socket_read_binary_integer(6+1, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation + params_mult = socket_read_binary_integer(1+6, "reverse_socket", 0.02) # steptime could work as well, but does not work in simulation if params_mult[0] > 0: - keepalive = params_mult[7] - q = [params_mult[1] / MULT_jointstate, params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate] - set_servo_setpoint(q) + if params_mult[1] > 1: + keepalive = params_mult[1] + q = [params_mult[2] / MULT_jointstate, params_mult[3] / MULT_jointstate, params_mult[4] / MULT_jointstate, params_mult[5] / MULT_jointstate, params_mult[6] / MULT_jointstate, params_mult[7] / MULT_jointstate] + set_servo_setpoint(q) + else: + keepalive = 1 + end else: keepalive = keepalive - 1 end From 14b9dd4f1b0e04f3087879163600f4f21329b0c7 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 26 Jun 2019 08:48:24 +0200 Subject: [PATCH 07/10] send keepalive signal when no (commanding) controller is active Otherwise the hardware disconnects as soon as we stop a controller by hand. --- .../include/ur_rtde_driver/ur/ur_driver.h | 18 ++++++++++++++++++ ur_rtde_driver/src/ros/hardware_interface.cpp | 4 ++++ ur_rtde_driver/src/ur/ur_driver.cpp | 17 +++++++++++------ 3 files changed, 33 insertions(+), 6 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h index a376286..ce29f37 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ur/ur_driver.h @@ -81,8 +81,26 @@ public: return rtde_frequency_; } + /*! + * \brief Writes a joint command together with a keepalive signal onto the socket being sent to + * the robot. + * + * \param values Desired joint positions + * + * \returns True on successful write. + */ bool writeJointCommand(const vector6d_t& values); + /*! + * \brief Write a keepalive signal only. + * + * This signals the robot that the connection is still + * active in times when no commands are to be sent (e.g. no controller is active.) + * + * \returns True on successful write. + */ + bool writeKeepalive(); + void startWatchdog(); private: diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index c78cc03..1384455 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -292,6 +292,10 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio { ur_driver_->writeJointCommand(joint_position_command_); } + else + { + ur_driver_->writeKeepalive(); + } } bool HardwareInterface ::prepareSwitch(const std::list& start_list, diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index 479fe09..d8df01b 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -138,14 +138,19 @@ bool UrDriver::writeJointCommand(const vector6d_t& values) { if (reverse_interface_active_) { - reverse_interface_->write(values); - } - else - { - return false; + return reverse_interface_->write(&values); } + return false; +} - return true; +bool UrDriver::writeKeepalive() +{ + if (reverse_interface_active_) + { + vector6d_t* fake = nullptr; + return reverse_interface_->write(fake, 1); + } + return false; } void UrDriver::startWatchdog() From 5934de2772d165843cbb4db342982c994bbfe497 Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Wed, 26 Jun 2019 08:49:51 +0200 Subject: [PATCH 08/10] re-added a non-scaled position interface for position controllers As this interface uses the same joint names as the scaled interface, resourced conflicts are already managed by ros_control's resource claim mechanisms. --- ur_rtde_driver/src/ros/hardware_interface.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 1384455..2ea6bfc 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -171,8 +171,8 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h &joint_velocities_[i], &joint_efforts_[i])); // Create joint position control interface - // pj_interface_.registerHandle( - // hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); + pj_interface_.registerHandle( + hardware_interface::JointHandle(js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i])); spj_interface_.registerHandle(ur_controllers::ScaledJointHandle( js_interface_.getHandle(joint_names_[i]), &joint_position_command_[i], &speed_scaling_combined_)); } @@ -186,7 +186,7 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h // Register interfaces registerInterface(&js_interface_); registerInterface(&spj_interface_); - // registerInterface(&pj_interface_); + registerInterface(&pj_interface_); registerInterface(&speedsc_interface_); registerInterface(&fts_interface_); @@ -317,6 +317,10 @@ void HardwareInterface ::doSwitch(const std::list Date: Wed, 26 Jun 2019 08:54:29 +0200 Subject: [PATCH 09/10] define a non-scaled position trajectory controller by default This way, users can choose whether they want to use non-scaled controllers as used by older drivers. --- ur_rtde_driver/config/ur10_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur10e_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur3_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur3e_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur5_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/config/ur5e_controllers.yaml | 24 +++++++++++++++++++- ur_rtde_driver/launch/ur10_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur10e_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur3_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur3e_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur5_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur5e_bringup.launch | 4 ++-- ur_rtde_driver/launch/ur_common.launch | 4 ++-- ur_rtde_driver/launch/ur_control.launch | 4 ++-- 14 files changed, 154 insertions(+), 22 deletions(-) diff --git a/ur_rtde_driver/config/ur10_controllers.yaml b/ur_rtde_driver/config/ur10_controllers.yaml index 42bb62d..ebbd6be 100644 --- a/ur_rtde_driver/config/ur10_controllers.yaml +++ b/ur_rtde_driver/config/ur10_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + 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 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur10e_controllers.yaml b/ur_rtde_driver/config/ur10e_controllers.yaml index 345f4f5..c100dfc 100644 --- a/ur_rtde_driver/config/ur10e_controllers.yaml +++ b/ur_rtde_driver/config/ur10e_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + 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 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur3_controllers.yaml b/ur_rtde_driver/config/ur3_controllers.yaml index 42bb62d..ebbd6be 100644 --- a/ur_rtde_driver/config/ur3_controllers.yaml +++ b/ur_rtde_driver/config/ur3_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + 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 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur3e_controllers.yaml b/ur_rtde_driver/config/ur3e_controllers.yaml index 345f4f5..c100dfc 100644 --- a/ur_rtde_driver/config/ur3e_controllers.yaml +++ b/ur_rtde_driver/config/ur3e_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + 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 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur5_controllers.yaml b/ur_rtde_driver/config/ur5_controllers.yaml index 42bb62d..ebbd6be 100644 --- a/ur_rtde_driver/config/ur5_controllers.yaml +++ b/ur_rtde_driver/config/ur5_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + 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 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/config/ur5e_controllers.yaml b/ur_rtde_driver/config/ur5e_controllers.yaml index 345f4f5..c100dfc 100644 --- a/ur_rtde_driver/config/ur5e_controllers.yaml +++ b/ur_rtde_driver/config/ur5e_controllers.yaml @@ -29,7 +29,7 @@ speed_scaling_state_controller: # Joint Trajectory Controller - position based ------------------------------- # For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller -pos_based_pos_traj_controller: +scaled_pos_traj_controller: type: position_controllers/ScaledJointTrajectoryController joints: - shoulder_pan_joint @@ -50,3 +50,25 @@ pos_based_pos_traj_controller: stop_trajectory_duration: 0.5 state_publish_rate: 500 action_monitor_rate: 10 + +pos_traj_controller: + type: position_controllers/JointTrajectoryController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + constraints: + goal_time: 0.6 + stopped_velocity_tolerance: 0.05 + shoulder_pan_joint: {trajectory: 0.2, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.2, goal: 0.1} + elbow_joint: {trajectory: 0.2, goal: 0.1} + wrist_1_joint: {trajectory: 0.2, goal: 0.1} + 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 + action_monitor_rate: 10 diff --git a/ur_rtde_driver/launch/ur10_bringup.launch b/ur_rtde_driver/launch/ur10_bringup.launch index b60464c..ef2d990 100644 --- a/ur_rtde_driver/launch/ur10_bringup.launch +++ b/ur_rtde_driver/launch/ur10_bringup.launch @@ -10,8 +10,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur10e_bringup.launch b/ur_rtde_driver/launch/ur10e_bringup.launch index dadd662..317b44c 100644 --- a/ur_rtde_driver/launch/ur10e_bringup.launch +++ b/ur_rtde_driver/launch/ur10e_bringup.launch @@ -11,8 +11,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur3_bringup.launch b/ur_rtde_driver/launch/ur3_bringup.launch index ea0ea84..3ecd9eb 100644 --- a/ur_rtde_driver/launch/ur3_bringup.launch +++ b/ur_rtde_driver/launch/ur3_bringup.launch @@ -10,8 +10,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur3e_bringup.launch b/ur_rtde_driver/launch/ur3e_bringup.launch index aef3632..02ff890 100644 --- a/ur_rtde_driver/launch/ur3e_bringup.launch +++ b/ur_rtde_driver/launch/ur3e_bringup.launch @@ -11,8 +11,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur5_bringup.launch b/ur_rtde_driver/launch/ur5_bringup.launch index a28b4ee..60a2363 100644 --- a/ur_rtde_driver/launch/ur5_bringup.launch +++ b/ur_rtde_driver/launch/ur5_bringup.launch @@ -10,8 +10,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur5e_bringup.launch b/ur_rtde_driver/launch/ur5e_bringup.launch index 783f916..ad46d76 100644 --- a/ur_rtde_driver/launch/ur5e_bringup.launch +++ b/ur_rtde_driver/launch/ur5e_bringup.launch @@ -11,8 +11,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur_common.launch b/ur_rtde_driver/launch/ur_common.launch index d591941..646d378 100644 --- a/ur_rtde_driver/launch/ur_common.launch +++ b/ur_rtde_driver/launch/ur_common.launch @@ -8,8 +8,8 @@ - - + + diff --git a/ur_rtde_driver/launch/ur_control.launch b/ur_rtde_driver/launch/ur_control.launch index 7c98798..e2a6d40 100644 --- a/ur_rtde_driver/launch/ur_control.launch +++ b/ur_rtde_driver/launch/ur_control.launch @@ -11,8 +11,8 @@ - - + + From a6a06526ac36225ddaa6e5b05385f7079693243b Mon Sep 17 00:00:00 2001 From: Felix Mauch Date: Thu, 27 Jun 2019 13:25:07 +0200 Subject: [PATCH 10/10] disable starting controllers when robot control is inactive Since we use the external controller stopper to take down controllers when robot control is inactive, we also do not want that users can start controllers while robot control is deactivated. The only exception is at startup time. --- .../ur_rtde_driver/ros/hardware_interface.h | 1 + ur_rtde_driver/src/ros/hardware_interface.cpp | 20 +++++++++++++++++-- 2 files changed, 19 insertions(+), 2 deletions(-) diff --git a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h index a4f0605..a8f8d58 100644 --- a/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h +++ b/ur_rtde_driver/include/ur_rtde_driver/ros/hardware_interface.h @@ -133,6 +133,7 @@ protected: ros::Publisher program_state_pub_; bool controller_reset_necessary_; + bool controllers_initialized_; std::string robot_ip_; }; diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 2ea6bfc..e65edf6 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -47,6 +47,7 @@ HardwareInterface::HardwareInterface() , position_controller_running_(false) , pausing_state_(PausingState::RUNNING) , pausing_ramp_up_increment_(0.01) + , controllers_initialized_(false) { } @@ -301,8 +302,23 @@ void HardwareInterface ::write(const ros::Time& time, const ros::Duration& perio bool HardwareInterface ::prepareSwitch(const std::list& start_list, const std::list& stop_list) { - // TODO: Implement - return true; + bool ret_val = true; + if (controllers_initialized_ && !isRobotProgramRunning() && !start_list.empty()) + { + for (auto& controller : start_list) + { + if (!controller.claimed_resources.empty()) + { + ROS_ERROR_STREAM("Robot control is currently inactive. Starting controllers that claim resources is currently " + "not possible. Not starting controller '" + << controller.name << "'"); + ret_val = false; + } + } + } + + controllers_initialized_ = true; + return ret_val; } void HardwareInterface ::doSwitch(const std::list& start_list,