diff --git a/ur_rtde_driver/CMakeLists.txt b/ur_rtde_driver/CMakeLists.txt index 8d8e506..b29b9be 100644 --- a/ur_rtde_driver/CMakeLists.txt +++ b/ur_rtde_driver/CMakeLists.txt @@ -96,6 +96,7 @@ add_library(ur_rtde_driver src/rtde/text_message.cpp src/rtde/rtde_client.cpp src/ur/ur_driver.cpp + src/ur/tool_communication.cpp ) target_link_libraries(ur_rtde_driver ${catkin_LIBRARIES}) add_dependencies(ur_rtde_driver ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) diff --git a/ur_rtde_driver/config/ur10e_controllers.yaml b/ur_rtde_driver/config/ur10e_controllers.yaml new file mode 100644 index 0000000..8687fc9 --- /dev/null +++ b/ur_rtde_driver/config/ur10e_controllers.yaml @@ -0,0 +1,113 @@ +# Settings for ros_control control loop +hardware_control_loop: + loop_hz: 125 + +# Settings for ros_control hardware interface +hardware_interface: + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint + +# Publish all joint states ---------------------------------- +joint_state_controller: + type: joint_state_controller/JointStateController + publish_rate: 125 + +# Publish wrench ---------------------------------- +force_torque_sensor_controller: + type: force_torque_sensor_controller/ForceTorqueSensorController + publish_rate: 125 + +# Publish speed_scaling factor +speed_scaling_state_controller: + type: ur_controllers/SpeedScalingStateController + publish_rate: 125 + +# Joint Trajectory Controller - position based ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +pos_based_pos_traj_controller: + #type: position_controllers/JointTrajectoryController + type: position_controllers/ScaledJointTrajectoryController + 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.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.4, goal: 0.1} + wrist_1_joint: {trajectory: 0.4, goal: 0.1} + wrist_2_joint: {trajectory: 0.1, goal: 0.1} + wrist_3_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 500 + action_monitor_rate: 10 + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #stop_trajectory_duration: 0 # Defaults to 0.0 + +# Joint Trajectory Controller ------------------------------- +# For detailed explanations of parameter see http://wiki.ros.org/joint_trajectory_controller +vel_based_pos_traj_controller: + type: velocity_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.1, goal: 0.1} + shoulder_lift_joint: {trajectory: 0.1, goal: 0.1} + elbow_joint: {trajectory: 0.1, goal: 0.1} + wrist_1_joint: {trajectory: 0.1, goal: 0.1} + wrist_2_joint: {trajectory: 0.1, goal: 0.1} + wrist_3_joint: {trajectory: 0.1, goal: 0.1} + stop_trajectory_duration: 0.5 + state_publish_rate: 125 + action_monitor_rate: 10 + gains: + #!!These values have not been optimized!! + shoulder_pan_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + shoulder_lift_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + elbow_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + wrist_1_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + wrist_2_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + wrist_3_joint: {p: 5.0, i: 0.05, d: 0.1, i_clamp: 1} + + # Use a feedforward term to reduce the size of PID gains + velocity_ff: + shoulder_pan_joint: 1.0 + shoulder_lift_joint: 1.0 + elbow_joint: 1.0 + wrist_1_joint: 1.0 + wrist_2_joint: 1.0 + wrist_3_joint: 1.0 + + # state_publish_rate: 50 # Defaults to 50 + # action_monitor_rate: 20 # Defaults to 20 + #stop_trajectory_duration: 0 # Defaults to 0.0 + +# Pass an array of joint velocities directly to the joints +joint_group_vel_controller: + type: velocity_controllers/JointGroupVelocityController + joints: + - shoulder_pan_joint + - shoulder_lift_joint + - elbow_joint + - wrist_1_joint + - wrist_2_joint + - wrist_3_joint diff --git a/ur_rtde_driver/include/ur_rtde_driver/ur/tool_communication.h b/ur_rtde_driver/include/ur_rtde_driver/ur/tool_communication.h new file mode 100644 index 0000000..85b9228 --- /dev/null +++ b/ur_rtde_driver/include/ur_rtde_driver/ur/tool_communication.h @@ -0,0 +1,168 @@ + +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-06-06 + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_RTDE_DRIVER_UR_TOOL_COMMUNICATION_H_INCLUDED +#define UR_RTDE_DRIVER_UR_TOOL_COMMUNICATION_H_INCLUDED + +#include "ur_rtde_driver/types.h" +#include + +namespace ur_driver +{ +enum class ToolVoltage : int +{ + OFF = 0, + _12V = 12, + _24V = 24 +}; + +enum class Parity : int +{ + NONE = 0, + ODD = 1, + EVEN = 2 +}; + +/*! + * \brief Helper class that represents a numeric value with a lower and an upper boundary. + * + * @tparam T any type for which a comparison exists. + */ +template +class Limited +{ +public: + Limited() = delete; + ~Limited() = default; + + using Datatype = T; + + Limited(const T lower, const T upper) : lower_(lower), upper_(upper) + { + data_ = lower_; + } + + void setData(const T data) + { + if (data >= lower_ && data <= upper_) + { + data_ = data; + } + else + { + throw std::runtime_error("Given data is out of range"); + } + } + + T getData() const + { + return data_; + } + +private: + T data_; + const T lower_; + const T upper_; +}; + +/*! + * \brief Class holding a tool communication configuration + */ +class ToolCommSetup +{ +public: + ToolCommSetup(); + ~ToolCommSetup() = default; + + using StopBitsT = Limited; + using RxIdleCharsT = Limited; + using TxIdleCharsT = Limited; + + void setToolVoltage(const ToolVoltage tool_voltage) + { + tool_voltage_ = tool_voltage; + } + ToolVoltage getToolVoltage() const + { + return tool_voltage_; + } + + void setParity(const Parity parity) + { + parity_ = parity; + } + Parity getParity() const + { + return parity_; + } + + void setBaudRate(const uint32_t baud_rate); + uint32_t getBaudRate() const + { + return baud_rate_; + }; + + void setStopBits(const StopBitsT::Datatype stop_bits) + { + stop_bits_.setData(stop_bits); + } + StopBitsT::Datatype getStopBits() const + { + return stop_bits_.getData(); + } + + void setRxIdleChars(const RxIdleCharsT::Datatype rx_idle_chars) + { + rx_idle_chars_.setData(rx_idle_chars); + } + RxIdleCharsT::Datatype getRxIdleChars() const + { + return rx_idle_chars_.getData(); + } + + void setTxIdleChars(const TxIdleCharsT::Datatype tx_idle_chars) + { + tx_idle_chars_.setData(tx_idle_chars); + } + TxIdleCharsT::Datatype getTxIdleChars() const + { + return tx_idle_chars_.getData(); + } + +private: + const std::set baud_rates_{ 9600, 19200, 38400, 57600, 115200, 10000000, 2000000, 5000000 }; + + ToolVoltage tool_voltage_; + Parity parity_; + uint32_t baud_rate_; + StopBitsT stop_bits_; + RxIdleCharsT rx_idle_chars_; + TxIdleCharsT tx_idle_chars_; +}; +} // namespace ur_driver +#endif // ifndef UR_RTDE_DRIVER_UR_TOOL_COMMUNICATION_H_INCLUDED 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 683338c..6f63429 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 @@ -30,6 +30,7 @@ #include "ur_rtde_driver/rtde/rtde_client.h" #include "ur_rtde_driver/comm/reverse_interface.h" #include "ur_rtde_driver/comm/script_sender.h" +#include "ur_rtde_driver/ur/tool_communication.h" namespace ur_driver { @@ -47,9 +48,22 @@ public: * * \param robot_ip IP-address under which the robot is reachable. * \param script_file URScript file that should be sent to the robot + * \param tool_comm_setup Configuration for using the tool communication */ UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file, - std::function handle_program_state); + std::function handle_program_state, std::unique_ptr tool_comm_setup); + /*! + * \brief Constructs a new UrDriver object. + * + * \param robot_ip IP-address under which the robot is reachable. + * \param script_file URScript file that should be sent to the robot + */ + UrDriver(const std::string& robot_ip, const std::string& script_file, const std::string& recipe_file, + std::function handle_program_state) + : UrDriver(robot_ip, script_file, recipe_file, handle_program_state, std::unique_ptr{}) + { + } + virtual ~UrDriver() = default; /*! diff --git a/ur_rtde_driver/launch/ur10_ros_control.launch b/ur_rtde_driver/launch/ur10_ros_control.launch index 2ee6c82..19831dc 100644 --- a/ur_rtde_driver/launch/ur10_ros_control.launch +++ b/ur_rtde_driver/launch/ur10_ros_control.launch @@ -31,6 +31,7 @@ + diff --git a/ur_rtde_driver/launch/ur10e_ros_control.launch b/ur_rtde_driver/launch/ur10e_ros_control.launch new file mode 100644 index 0000000..92bfcc6 --- /dev/null +++ b/ur_rtde_driver/launch/ur10e_ros_control.launch @@ -0,0 +1,65 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ur_rtde_driver/resources/servoj.urscript b/ur_rtde_driver/resources/servoj.urscript index 1c35c26..d44268f 100644 --- a/ur_rtde_driver/resources/servoj.urscript +++ b/ur_rtde_driver/resources/servoj.urscript @@ -1,3 +1,5 @@ +{{BEGIN_REPLACE}} + global steptime = get_steptime() textmsg("steptime=", steptime) global MULT_jointstate = {{JOINT_STATE_REPLACE}} diff --git a/ur_rtde_driver/src/ros/hardware_interface.cpp b/ur_rtde_driver/src/ros/hardware_interface.cpp index 8a7fbbe..b44d7de 100644 --- a/ur_rtde_driver/src/ros/hardware_interface.cpp +++ b/ur_rtde_driver/src/ros/hardware_interface.cpp @@ -26,6 +26,7 @@ //---------------------------------------------------------------------- #include "ur_rtde_driver/ros/hardware_interface.h" +#include "ur_rtde_driver/ur/tool_communication.h" #include @@ -66,9 +67,67 @@ bool HardwareInterface ::init(ros::NodeHandle& root_nh, ros::NodeHandle& robot_h tcp_link_ = robot_hw_nh.param("tcp_link", "tool0"); program_state_pub_ = robot_hw_nh.advertise("robot_program_running", 10, true); + bool use_tool_communication = robot_hw_nh.param("use_tool_communication", "false"); + std::unique_ptr tool_comm_setup; + if (use_tool_communication) + { + tool_comm_setup.reset(new ToolCommSetup()); + + using ToolVoltageT = std::underlying_type::type; + ToolVoltageT tool_voltage; + if (!robot_hw_nh.getParam("tool_voltage", tool_voltage)) + { + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_voltage") << " not given."); + return false; + } + tool_comm_setup->setToolVoltage(static_cast(tool_voltage)); + + using ParityT = std::underlying_type::type; + ParityT parity; + if (!robot_hw_nh.getParam("tool_parity", parity)) + { + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_parity") << " not given."); + return false; + } + + int baud_rate; + if (!robot_hw_nh.getParam("tool_baud_rate", baud_rate)) + { + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_baud_rate") << " not given."); + return false; + } + tool_comm_setup->setBaudRate(baud_rate); + + int stop_bits; + if (!robot_hw_nh.getParam("tool_stop_bits", stop_bits)) + { + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_stop_bits") << " not given."); + return false; + } + tool_comm_setup->setStopBits(stop_bits); + + int rx_idle_chars; + if (!robot_hw_nh.getParam("tool_rx_idle_chars", rx_idle_chars)) + { + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_rx_idle_chars") << " not given."); + return false; + } + tool_comm_setup->setRxIdleChars(rx_idle_chars); + tool_comm_setup->setParity(static_cast(parity)); + + int tx_idle_chars; + if (!robot_hw_nh.getParam("tool_tx_idle_chars", tx_idle_chars)) + { + ROS_ERROR_STREAM("Required parameter " << robot_hw_nh.resolveName("tool_tx_idle_chars") << " not given."); + return false; + } + tool_comm_setup->setTxIdleChars(tx_idle_chars); + } + ROS_INFO_STREAM("Initializing urdriver"); ur_driver_.reset(new UrDriver(robot_ip, script_filename, recipe_filename, - std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1))); + std::bind(&HardwareInterface::handleRobotProgramState, this, std::placeholders::_1), + std::move(tool_comm_setup))); if (!root_nh.getParam("hardware_interface/joints", joint_names_)) { diff --git a/ur_rtde_driver/src/ur/tool_communication.cpp b/ur_rtde_driver/src/ur/tool_communication.cpp new file mode 100644 index 0000000..df87740 --- /dev/null +++ b/ur_rtde_driver/src/ur/tool_communication.cpp @@ -0,0 +1,53 @@ +// this is for emacs file handling -*- mode: c++; indent-tabs-mode: nil -*- + +// -- BEGIN LICENSE BLOCK ---------------------------------------------- +// Copyright 2019 FZI Forschungszentrum Informatik +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. +// -- END LICENSE BLOCK ------------------------------------------------ + +//---------------------------------------------------------------------- +/*!\file + * + * \author Felix Mauch mauch@fzi.de + * \date 2019-06-06 + * + */ +//---------------------------------------------------------------------- + +#include "ur_rtde_driver/ur/tool_communication.h" + +namespace ur_driver +{ +ToolCommSetup::ToolCommSetup() + : tool_voltage_(ToolVoltage::OFF) + , parity_(Parity::ODD) + , baud_rate_(9600) + , stop_bits_(1, 2) + , rx_idle_chars_(1.0, 40.0) + , tx_idle_chars_(0.0, 40.0) +{ +} + +void ToolCommSetup::setBaudRate(const uint32_t baud_rate) +{ + if (baud_rates_.find(baud_rate) != baud_rates_.end()) + { + baud_rate_ = baud_rate; + } + else + { + throw std::runtime_error("Provided baud rate is not allowed"); + } +} +} // namespace ur_driver diff --git a/ur_rtde_driver/src/ur/ur_driver.cpp b/ur_rtde_driver/src/ur/ur_driver.cpp index 81a1a82..94e61bb 100644 --- a/ur_rtde_driver/src/ur/ur_driver.cpp +++ b/ur_rtde_driver/src/ur/ur_driver.cpp @@ -38,13 +38,15 @@ namespace ur_driver { static const int32_t MULT_JOINTSTATE = 1000000; +static const std::string BEGIN_REPLACE("{{BEGIN_REPLACE}}"); static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}"); static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}"); static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}"); static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}"); ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& script_file, - const std::string& recipe_file, std::function handle_program_state) + const std::string& recipe_file, std::function handle_program_state, + std::unique_ptr tool_comm_setup) : servoj_time_(0.008) , servoj_gain_(2000) , servoj_lookahead_time_(0.03) @@ -80,6 +82,20 @@ ur_driver::UrDriver::UrDriver(const std::string& robot_ip, const std::string& sc prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip); prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(reverse_port)); + std::stringstream begin_replace; + if (tool_comm_setup != nullptr) + { + begin_replace << "set_tool_voltage(" + << static_cast::type>(tool_comm_setup->getToolVoltage()) << ")\n"; + begin_replace << "set_tool_communication(" + << "True" + << ", " << tool_comm_setup->getBaudRate() << ", " + << static_cast::type>(tool_comm_setup->getParity()) << ", " + << tool_comm_setup->getStopBits() << ", " << tool_comm_setup->getRxIdleChars() << ", " + << tool_comm_setup->getTxIdleChars() << ")"; + } + prog.replace(prog.find(BEGIN_REPLACE), BEGIN_REPLACE.length(), begin_replace.str()); + script_sender_.reset(new comm::ScriptSender(script_sender_port, prog)); script_sender_->start(); LOG_INFO("Created script sender");