diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index 5ea7bbe86..e94be4f0f 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -19,6 +19,10 @@ find_package(std_srvs REQUIRED) find_package(ur_dashboard_msgs REQUIRED) find_package(ur_msgs REQUIRED) find_package(generate_parameter_library REQUIRED) +find_package(trajectory_msgs REQUIRED) +find_package(control_msgs REQUIRED) +find_package(action_msgs REQUIRED) + set(THIS_PACKAGE_INCLUDE_DEPENDS angles @@ -34,6 +38,9 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS ur_dashboard_msgs ur_msgs generate_parameter_library + control_msgs + trajectory_msgs + action_msgs ) include_directories(include) @@ -54,6 +61,11 @@ generate_parameter_library( src/scaled_joint_trajectory_controller_parameters.yaml ) +generate_parameter_library( + passthrough_trajectory_controller_parameters + src/passthrough_trajectory_controller_parameters.yaml +) + generate_parameter_library( ur_configuration_controller_parameters src/ur_configuration_controller_parameters.yaml @@ -63,6 +75,7 @@ add_library(${PROJECT_NAME} SHARED src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp src/gpio_controller.cpp + src/passthrough_trajectory_controller.cpp src/ur_configuration_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE @@ -72,6 +85,7 @@ target_link_libraries(${PROJECT_NAME} gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters + passthrough_trajectory_controller_parameters ur_configuration_controller_parameters ) ament_target_dependencies(${PROJECT_NAME} diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index fa4b63987..c784b35d0 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,6 +14,11 @@ This controller publishes the Tool IO. + + + This controller forwards a joint-based trajectory to the robot controller for interpolation. + + Controller used to get and change the configuration of the robot diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index bc4d7c147..a488d07e5 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -135,3 +135,132 @@ Advertised services * ``~/set_speed_slider [ur_msgs/srv/SetSpeedSliderFraction]``: Set the value of the speed slider. * ``~/zero_ftsensor [std_srvs/srv/Trigger]``: Zeroes the reported wrench of the force torque sensor. + +.. _passthrough_trajectory_controller: + +ur_controllers/PassthroughTrajectoryController +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +This controller uses a ``control_msgs/FollowJointTrajectory`` action but instead of interpolating +the trajectory on the ROS pc it forwards the complete trajectory to the robot controller for +interpolation and execution. This way, the realtime requirements for the control PC can be +massively decreased, since the robot always "knows" what to do next. That means that you should be +able to run a stable driver connection also without a real-time patched kernel. + +Interpolation depends on the robot controller's implementation, but in conjunction with the +ur_robot_driver it defaults to mimicking ros2_control's spline interpolation. So, any trajectory +planned e.g. with MoveIt! will be executed following the trajectory exactly. + +A trajectory sent to the controller's action server will be forwarded to the robot controller and +executed there. Once all setpoints are transferred to the robot, the controller goes into a waiting +state where it waits for the trajectory to be finished. While waiting, the controller tracks the +time spent on the trajectory to ensure the robot isn't stuck during execution. + +This controller also supports **speed scaling** such that and scaling down of the trajectory done +by the robot, for example due to safety settings on the robot or simply because a slower execution +is configured on the teach pendant. This will be considered, during execution monitoring, so the +controller basically tracks the scaled time instead of the real time. + +.. note:: + + When using this controller with the URSim simulator execution times can be slightly larger than + the expected time depending on the simulation host's resources. This effect will not be present + when using a real UR arm. + +.. note:: + + This controller can currently only be used with URSim or a real UR robot. Neither mock hardware + nor gazebo support this type of trajectory interfaces at the time being. + +Tolerances +"""""""""" + +Currently, the trajectory passthrough controller only supports goal tolerances and goal time +tolerances passed in the action directly. Please make sure that the tolerances are completely +filled with all joint names. + +A **goal time tolerance** of ``0.0`` means that no goal time tolerance is set and the action will +not fail when execution takes too long. + +Action interface / usage +"""""""""""""""""""""""" + +To use this controller, publish a goal to the ``~/follow_joint_trajectory`` action interface +similar to the `joint_trajectory_controller `_. + +Currently, the controller doesn't support replacing a running trajectory action. While a trajectory +is being executed, goals will be rejected until the action has finished. If you want to replace it, +first cancel the running action and then send a new one. + +Parameters +"""""""""" + +The trajectory passthrough controller uses the following parameters: + ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ +| Parameter name | Type | Default value | Description | +| | | | | ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ +| ``joints`` (required) | string_array | | Joint names to listen to | ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ +| ``state_interfaces`` (required) | string_array | | State interfaces provided by the hardware for all joints. Subset of ``["position", "velocity", "acceleration"]`` | ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ +| ``speed_scaling_interface_name`` | string | ``speed_scaling/speed_scaling_factor`` | Fully qualified name of the speed scaling interface name. | ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ +| ``tf_prefix`` | string | | Urdf prefix of the corresponding arm | ++----------------------------------+--------------+----------------------------------------+------------------------------------------------------------------------------------------------------------------+ + +Interfaces +"""""""""" + +In order to use this, the hardware has to export a command interface for passthrough operations for each joint. It always has +to export position, velocity and acceleration interfaces in order to be able to project the full +JointTrajectory definition. This is why there are separate fields used, as for passthrough mode +accelerations might be relevant also for robots that don't support commanding accelerations +directly to their joints. + +.. code:: xml + + + + + + + + + + + + + + + + + + + + + + + + + +.. note:: + + The hardware component has to take care that the passthrough command interfaces cannot be + activated in parallel to the streaming command interfaces. + +Implementation details / dataflow +""""""""""""""""""""""""""""""""" + +* A trajectory passed to the controller will be sent to the hardware component one by one. +* The controller will send one setpoint and then wait for the hardware to acknowledge that it can + take a new setpoint. +* This happens until all setpoints have been transferred to the hardware. Then, the controller goes + into a waiting state where it monitors execution time and waits for the hardware to finish + execution. +* If execution takes longer than anticipated, a warning will be printed. +* If execution finished taking longer than expected (plus the goal time tolerance), the action will fail. +* When the hardware reports that execution has been aborted (The ``passthrough_trajectory_abort`` + command interface), the action will be aborted. +* When the action is preempted, execution on the hardware is preempted. diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp new file mode 100644 index 000000000..a3c91d10f --- /dev/null +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -0,0 +1,184 @@ +// Copyright 2024, Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Jacob Larsen jala@universal-robots.com + * \date 2024-03-11 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#ifndef UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ +#define UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ + +#include + +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include "passthrough_trajectory_controller_parameters.hpp" + +namespace ur_controllers +{ + +/* + * 0.0: No trajectory to forward, the controller is idling and ready to receive a new trajectory. + * 1.0: The controller has received and accepted a new trajectory. When the state is 1.0, the controller will write a + * point to the hardware interface. + * 2.0: The hardware interface will read the point written from the controller. The state will switch between 1.0 + * and 2.0 until all points have been read by the hardware interface. + * 3.0: The hardware interface has read all the points, and will now write all the points to the physical robot + * controller. + * 4.0: The robot is moving through the trajectory. + * 5.0: The robot finished executing the trajectory. + */ +const double TRANSFER_STATE_IDLE = 0.0; +const double TRANSFER_STATE_WAITING_FOR_POINT = 1.0; +const double TRANSFER_STATE_TRANSFERRING = 2.0; +const double TRANSFER_STATE_TRANSFER_DONE = 3.0; +const double TRANSFER_STATE_IN_MOTION = 4.0; +const double TRANSFER_STATE_DONE = 5.0; + +using namespace std::chrono_literals; // NOLINT + +class PassthroughTrajectoryController : public controller_interface::ControllerInterface +{ +public: + PassthroughTrajectoryController() = default; + ~PassthroughTrajectoryController() override = default; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::InterfaceConfiguration command_interface_configuration() const override; + + controller_interface::CallbackReturn on_init() override; + + controller_interface::CallbackReturn on_configure(const rclcpp_lifecycle::State& previous_state) override; + + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override; + + controller_interface::CallbackReturn on_deactivate(const rclcpp_lifecycle::State& state) override; + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + +private: + using FollowJTrajAction = control_msgs::action::FollowJointTrajectory; + using RealtimeGoalHandle = realtime_tools::RealtimeServerGoalHandle; + using RealtimeGoalHandlePtr = std::shared_ptr; + using RealtimeGoalHandleBuffer = realtime_tools::RealtimeBuffer; + + RealtimeGoalHandleBuffer rt_active_goal_; ///< Currently active action goal, if any. + rclcpp::TimerBase::SharedPtr goal_handle_timer_; ///< Timer to frequently check on the running goal + realtime_tools::RealtimeBuffer> joint_trajectory_mapping_; + + rclcpp::Duration action_monitor_period_ = rclcpp::Duration(50ms); + + /* Start an action server with an action called: /passthrough_trajectory_controller/forward_joint_trajectory. */ + void start_action_server(void); + + void end_goal(); + + bool check_goal_tolerance(); + + // Get a mapping between the trajectory's joint order and the internal one + std::unordered_map create_joint_mapping(const std::vector& joint_names) const; + + std::shared_ptr passthrough_param_listener_; + passthrough_trajectory_controller::Params passthrough_params_; + + rclcpp_action::Server::SharedPtr send_trajectory_action_server_; + + rclcpp_action::GoalResponse + goal_received_callback(const rclcpp_action::GoalUUID& uuid, + std::shared_ptr goal); + + rclcpp_action::CancelResponse goal_cancelled_callback( + const std::shared_ptr> goal_handle); + + void goal_accepted_callback( + std::shared_ptr> goal_handle); + + realtime_tools::RealtimeBuffer> joint_names_; + std::vector state_interface_types_; + + std::vector joint_state_interface_names_; + std::vector> joint_position_state_interface_; + std::vector> joint_velocity_state_interface_; + std::vector> joint_acceleration_state_interface_; + + bool check_goal_tolerances(std::shared_ptr goal); + bool check_goal_positions(std::shared_ptr goal); + bool check_goal_velocities(std::shared_ptr goal); + bool check_goal_accelerations(std::shared_ptr goal); + + trajectory_msgs::msg::JointTrajectory active_joint_traj_; + // std::vector path_tolerance_; + realtime_tools::RealtimeBuffer> goal_tolerance_; + realtime_tools::RealtimeBuffer goal_time_tolerance_{ rclcpp::Duration(0, 0) }; + + std::atomic current_index_; + std::atomic trajectory_active_; + rclcpp::Duration active_trajectory_elapsed_time_ = rclcpp::Duration::from_nanoseconds(0); + rclcpp::Duration max_trajectory_time_ = rclcpp::Duration::from_nanoseconds(0); + double scaling_factor_; + std::atomic number_of_joints_; + static constexpr double NO_VAL = std::numeric_limits::quiet_NaN(); + + std::optional> scaling_state_interface_; + std::optional> abort_command_interface_; + std::optional> transfer_command_interface_; + std::optional> time_from_start_command_interface_; + + rclcpp::Clock::SharedPtr clock_; +}; +} // namespace ur_controllers +#endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index d5235c27d..cae303417 100644 --- a/ur_controllers/package.xml +++ b/ur_controllers/package.xml @@ -33,6 +33,10 @@ std_srvs ur_dashboard_msgs ur_msgs + control_msgs + trajectory_msgs + action_msgs + ament_cmake diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp new file mode 100644 index 000000000..0b09bb6fe --- /dev/null +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -0,0 +1,624 @@ +// Copyright 2024, Universal Robots A/S +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the {copyright_holder} nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +//---------------------------------------------------------------------- +/*!\file + * + * \author Jacob Larsen jala@universal-robots.com + * \date 2024-03-11 + * + * + * + * + */ +//---------------------------------------------------------------------- + +#include +#include +#include + +#include +#include +#include +#include + +#include "ur_controllers/passthrough_trajectory_controller.hpp" + +namespace ur_controllers +{ + +double duration_to_double(const builtin_interfaces::msg::Duration& duration) +{ + return duration.sec + (duration.nanosec / 1000000000.0); +} + +controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() +{ + passthrough_param_listener_ = std::make_shared(get_node()); + passthrough_params_ = passthrough_param_listener_->get_params(); + current_index_ = 0; + auto joint_names = passthrough_params_.joints; + joint_names_.writeFromNonRT(joint_names); + number_of_joints_ = joint_names.size(); + state_interface_types_ = passthrough_params_.state_interfaces; + scaling_factor_ = 1.0; + clock_ = get_node()->get_clock(); + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + start_action_server(); + trajectory_active_ = false; + + joint_state_interface_names_.clear(); + + joint_state_interface_names_.reserve(number_of_joints_ * state_interface_types_.size()); + + auto joint_names_internal = joint_names_.readFromRT(); + for (const auto& joint_name : *joint_names_internal) { + for (const auto& interface_type : state_interface_types_) { + joint_state_interface_names_.emplace_back(joint_name + "/" + interface_type); + } + } + + return ControllerInterface::on_configure(previous_state); +} + +void PassthroughTrajectoryController::start_action_server(void) +{ + send_trajectory_action_server_ = rclcpp_action::create_server( + get_node(), std::string(get_node()->get_name()) + "/follow_joint_trajectory", + std::bind(&PassthroughTrajectoryController::goal_received_callback, this, std::placeholders::_1, + std::placeholders::_2), + std::bind(&PassthroughTrajectoryController::goal_cancelled_callback, this, std::placeholders::_1), + std::bind(&PassthroughTrajectoryController::goal_accepted_callback, this, std::placeholders::_1)); + return; +} + +controller_interface::InterfaceConfiguration PassthroughTrajectoryController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + std::copy(joint_state_interface_names_.cbegin(), joint_state_interface_names_.cend(), std::back_inserter(conf.names)); + + conf.names.push_back(passthrough_params_.speed_scaling_interface_name); + + return conf; +} + +controller_interface::InterfaceConfiguration PassthroughTrajectoryController::command_interface_configuration() const +{ + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + const std::string tf_prefix = passthrough_params_.tf_prefix; + + for (size_t i = 0; i < number_of_joints_; ++i) { + config.names.emplace_back(tf_prefix + "trajectory_passthrough/setpoint_positions_" + std::to_string(i)); + config.names.emplace_back(tf_prefix + "trajectory_passthrough/setpoint_velocities_" + std::to_string(i)); + config.names.emplace_back(tf_prefix + "trajectory_passthrough/setpoint_accelerations_" + std::to_string(i)); + } + + config.names.push_back(tf_prefix + "trajectory_passthrough/abort"); + config.names.emplace_back(tf_prefix + "trajectory_passthrough/transfer_state"); + config.names.emplace_back(tf_prefix + "trajectory_passthrough/time_from_start"); + + return config; +} + +controller_interface::CallbackReturn PassthroughTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) +{ + // clear out vectors in case of restart + joint_position_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + joint_acceleration_state_interface_.clear(); + + for (auto& interface_name : joint_state_interface_names_) { + auto interface_it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (interface_it != state_interfaces_.end()) { + if (interface_it->get_interface_name() == "position") { + joint_position_state_interface_.emplace_back(*interface_it); + } else if (interface_it->get_interface_name() == "velocity") { + joint_velocity_state_interface_.emplace_back(*interface_it); + } else if (interface_it->get_interface_name() == "acceleration") { + joint_acceleration_state_interface_.emplace_back(*interface_it); + } + } + } + + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), [&](auto& interface) { + return (interface.get_name() == passthrough_params_.speed_scaling_interface_name); + }); + if (it != state_interfaces_.end()) { + scaling_state_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find speed scaling interface in state interfaces."); + return controller_interface::CallbackReturn::ERROR; + } + + { + const std::string interface_name = passthrough_params_.tf_prefix + "trajectory_passthrough/" + "abort"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + abort_command_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + const std::string tf_prefix = passthrough_params_.tf_prefix; + { + const std::string interface_name = tf_prefix + "trajectory_passthrough/transfer_state"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + transfer_command_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + { + const std::string interface_name = tf_prefix + "trajectory_passthrough/time_from_start"; + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != command_interfaces_.end()) { + time_from_start_command_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + + return ControllerInterface::on_activate(state); +} + +controller_interface::CallbackReturn PassthroughTrajectoryController::on_deactivate(const rclcpp_lifecycle::State&) +{ + abort_command_interface_->get().set_value(1.0); + if (trajectory_active_) { + const auto active_goal = *rt_active_goal_.readFromRT(); + std::shared_ptr result = + std::make_shared(); + result->set__error_string("Aborting current goal, since the controller is being deactivated."); + active_goal->setAborted(result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + end_goal(); + } + return CallbackReturn::SUCCESS; +} + +controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& period) +{ + const auto active_goal = *rt_active_goal_.readFromRT(); + + const auto current_transfer_state = transfer_command_interface_->get().get_value(); + + if (active_goal && trajectory_active_) { + if (current_transfer_state != TRANSFER_STATE_IDLE) { + // Check if the trajectory has been aborted from the hardware interface. E.g. the robot was stopped on the teach + // pendant. + if (abort_command_interface_->get().get_value() == 1.0 && current_index_ > 0) { + RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted by hardware, aborting action."); + std::shared_ptr result = + std::make_shared(); + active_goal->setAborted(result); + end_goal(); + return controller_interface::return_type::OK; + } + } + + active_joint_traj_ = active_goal->gh_->get_goal()->trajectory; + + if (current_index_ == 0 && current_transfer_state == TRANSFER_STATE_IDLE) { + active_trajectory_elapsed_time_ = rclcpp::Duration(0, 0); + max_trajectory_time_ = + rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); + transfer_command_interface_->get().set_value(TRANSFER_STATE_WAITING_FOR_POINT); + } + auto active_goal_time_tol = goal_time_tolerance_.readFromRT(); + auto joint_mapping = joint_trajectory_mapping_.readFromRT(); + + // Write a new point to the command interface, if the previous point has been read by the hardware interface. + if (current_transfer_state == TRANSFER_STATE_WAITING_FOR_POINT) { + if (current_index_ < active_joint_traj_.points.size()) { + // Write the time_from_start parameter. + time_from_start_command_interface_->get().set_value( + duration_to_double(active_joint_traj_.points[current_index_].time_from_start)); + + // Write the positions for each joint of the robot + auto joint_names_internal = joint_names_.readFromRT(); + // We've added the joint interfaces matching the order of the joint names so we can safely access + // them by the index. + for (size_t i = 0; i < number_of_joints_; i++) { + command_interfaces_[i * 3].set_value( + active_joint_traj_.points[current_index_].positions[joint_mapping->at(joint_names_internal->at(i))]); + // Optionally, also write velocities and accelerations for each joint. + if (active_joint_traj_.points[current_index_].velocities.size() > 0) { + command_interfaces_[i * 3 + 1].set_value( + active_joint_traj_.points[current_index_].velocities[joint_mapping->at(joint_names_internal->at(i))]); + if (active_joint_traj_.points[current_index_].accelerations.size() > 0) { + command_interfaces_[i * 3 + 2].set_value( + active_joint_traj_.points[current_index_] + .accelerations[joint_mapping->at(joint_names_internal->at(i))]); + } else { + command_interfaces_[i * 3 + 2].set_value(NO_VAL); + } + } else { + command_interfaces_[i * 3 + 1].set_value(NO_VAL); + command_interfaces_[i * 3 + 2].set_value(NO_VAL); + } + } + // Tell hardware interface that this point is ready to be read. + transfer_command_interface_->get().set_value(TRANSFER_STATE_TRANSFERRING); + current_index_++; + // Check if all points have been written to the hardware interface. + } else if (current_index_ == active_joint_traj_.points.size()) { + transfer_command_interface_->get().set_value(TRANSFER_STATE_TRANSFER_DONE); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Hardware waiting for trajectory point while none is present!"); + } + + // When the trajectory is finished, report the goal as successful to the client. + } else if (current_transfer_state == TRANSFER_STATE_DONE) { + auto result = active_goal->preallocated_result_; + // Check if the actual position complies with the tolerances given. + if (!check_goal_tolerance()) { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = "Robot not within tolerances at end of trajectory."; + active_goal->setAborted(result); + end_goal(); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); + } else if (active_goal_time_tol->nanoseconds() > 0 && + active_trajectory_elapsed_time_ > (max_trajectory_time_ + *active_goal_time_tol)) { + // Check if the goal time tolerance was complied with. + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = + "Goal not reached within time tolerance. Missed goal time by " + + std::to_string((active_trajectory_elapsed_time_ - max_trajectory_time_ - *active_goal_time_tol).seconds()) + + " seconds."; + active_goal->setAborted(result); + end_goal(); + } else { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; + result->error_string = "Trajectory executed successfully in " + + std::to_string(active_trajectory_elapsed_time_.seconds()) + + " (scaled) seconds! The real time needed for execution could be longer."; + active_goal->setSucceeded(result); + end_goal(); + RCLCPP_INFO(get_node()->get_logger(), "%s", result->error_string.c_str()); + } + } else if (current_transfer_state == TRANSFER_STATE_IN_MOTION) { + // Keep track of how long the trajectory has been executing, if it takes too long, send a warning. + if (scaling_state_interface_.has_value()) { + scaling_factor_ = scaling_state_interface_->get().get_value(); + } + + active_trajectory_elapsed_time_ += period * scaling_factor_; + + // RCLCPP_INFO(get_node()->get_logger(), "Elapsed trajectory time: %f. Scaling factor: %f, period: %f", + // active_trajectory_elapsed_time_.seconds(), scaling_factor_, period.seconds()); + + if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + *active_goal_time_tol) && trajectory_active_) { + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *clock_, 1000, + "Trajectory should be finished by now. You may want to cancel this goal, if it is not."); + } + } + } else if (current_transfer_state != TRANSFER_STATE_IDLE && current_transfer_state != TRANSFER_STATE_DONE) { + // No goal is active, but we are not in IDLE, either. We have been canceled. + abort_command_interface_->get().set_value(1.0); + + } else if (current_transfer_state == TRANSFER_STATE_DONE) { + // We have been informed about the finished trajectory. Let's reset things. + transfer_command_interface_->get().set_value(TRANSFER_STATE_IDLE); + abort_command_interface_->get().set_value(0.0); + } + + return controller_interface::return_type::OK; +} + +rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callback( + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr goal) +{ + RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory."); + // Precondition: Running controller + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectories. Controller is not running."); + return rclcpp_action::GoalResponse::REJECT; + } + + if (trajectory_active_) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. A trajectory is already executing."); + return rclcpp_action::GoalResponse::REJECT; + } + + // Check that all parts of the trajectory are valid. + if (!check_goal_positions(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_goal_velocities(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_goal_accelerations(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_goal_tolerances(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +bool PassthroughTrajectoryController::check_goal_tolerances( + std::shared_ptr goal) +{ + auto& tolerances = goal->goal_tolerance; + auto joint_names_internal = joint_names_.readFromRT(); + if (!tolerances.empty()) { + for (auto& tol : tolerances) { + auto found_it = std::find(joint_names_internal->begin(), joint_names_internal->end(), tol.name); + if (found_it == joint_names_internal->end()) { + RCLCPP_ERROR(get_node()->get_logger(), + "Tolerance for joint '%s' given. This joint is not known to this controller.", tol.name.c_str()); + return false; + } + } + if (tolerances.size() != number_of_joints_) { + RCLCPP_ERROR(get_node()->get_logger(), "Tolerances for %lu joints given. This controller knows %lu joints.", + tolerances.size(), number_of_joints_.load()); + return false; + } + } + return true; +} + +bool PassthroughTrajectoryController::check_goal_positions( + std::shared_ptr goal) +{ + for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + if (goal->trajectory.points[i].positions.size() != number_of_joints_) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must have positions for all joints of the robot. (" + + std::to_string(number_of_joints_) + " joint positions per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i + 1) + + " has: " + std::to_string(goal->trajectory.points[i].positions.size()) + " positions."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + } + return true; +} + +bool PassthroughTrajectoryController::check_goal_velocities( + std::shared_ptr goal) +{ + for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + if (goal->trajectory.points[i].velocities.size() != number_of_joints_ && + goal->trajectory.points[i].velocities.size() != 0) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must either not have velocities or have them for all " + "joints of the robot. (" + + std::to_string(number_of_joints_) + " joint velocities per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i + 1) + + " has: " + std::to_string(goal->trajectory.points[i].velocities.size()) + " velocities."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + if (goal->trajectory.points[i].velocities.size() != goal->trajectory.points[0].velocities.size()) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must have velocities for all joints of the robot. " + "(" + + std::to_string(number_of_joints_) + " joint velocities per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i) + " has: " + std::to_string(goal->trajectory.points[i].velocities.size()) + + " velocities."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + } + return true; +} + +bool PassthroughTrajectoryController::check_goal_accelerations( + std::shared_ptr goal) +{ + for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + if (goal->trajectory.points[i].accelerations.size() != 0 && + goal->trajectory.points[i].accelerations.size() != number_of_joints_) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must either not have accelerations or have them for " + "all joints of the robot. (" + + std::to_string(number_of_joints_) + " joint accelerations per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i) + + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + if (goal->trajectory.points[i].accelerations.size() != goal->trajectory.points[0].accelerations.size()) { + std::string msg; + msg = "Can't accept new trajectory. All trajectory points must have accelerations for all joints of the " + "robot. " + "(" + + std::to_string(number_of_joints_) + " joint accelerations per point)"; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + msg = "Point nr " + std::to_string(i) + + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); + return false; + } + } + return true; +} + +rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_callback( + const std::shared_ptr> goal_handle) +{ + // Check that cancel request refers to currently active goal (if any) + const auto active_goal = *rt_active_goal_.readFromNonRT(); + if (active_goal && active_goal->gh_ == goal_handle) { + RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory requested."); + + // Mark the current goal as canceled + auto result = std::make_shared(); + active_goal->setCanceled(result); + rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); + trajectory_active_ = false; + } + return rclcpp_action::CancelResponse::ACCEPT; +} + +// Action goal was accepted, initialise values for a new trajectory. +void PassthroughTrajectoryController::goal_accepted_callback( + std::shared_ptr> goal_handle) +{ + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Accepted new trajectory with " + << goal_handle->get_goal()->trajectory.points.size() << " points."); + current_index_ = 0; + + // TODO(fexner): Merge goal tolerances with default tolerances + + joint_trajectory_mapping_.writeFromNonRT(create_joint_mapping(goal_handle->get_goal()->trajectory.joint_names)); + + // sort goal tolerances to match internal joint order + std::vector goal_tolerances; + if (!goal_handle->get_goal()->goal_tolerance.empty()) { + auto joint_names_internal = joint_names_.readFromRT(); + std::stringstream ss; + ss << "Using goal tolerances\n"; + for (auto& joint_name : *joint_names_internal) { + auto found_it = + std::find_if(goal_handle->get_goal()->goal_tolerance.begin(), goal_handle->get_goal()->goal_tolerance.end(), + [&joint_name](auto& tol) { return tol.name == joint_name; }); + if (found_it != goal_handle->get_goal()->goal_tolerance.end()) { + goal_tolerances.push_back(*found_it); + ss << joint_name << " -- position: " << found_it->position << ", velocity: " << found_it->velocity + << ", acceleration: " << found_it->acceleration << std::endl; + } + } + RCLCPP_INFO_STREAM(get_node()->get_logger(), ss.str()); + } + goal_tolerance_.writeFromNonRT(goal_tolerances); + goal_time_tolerance_.writeFromNonRT(goal_handle->get_goal()->goal_time_tolerance); + RCLCPP_INFO_STREAM(get_node()->get_logger(), + "Goal time tolerance: " << duration_to_double(goal_handle->get_goal()->goal_time_tolerance) + << " sec"); + + // Action handling will be done from the timer callback to avoid those things in the realtime + // thread. First, we delete the existing (if any) timer by resetting the pointer and then create a new + // one. + // + RealtimeGoalHandlePtr rt_goal = std::make_shared(goal_handle); + rt_goal->execute(); + rt_active_goal_.writeFromNonRT(rt_goal); + goal_handle_timer_.reset(); + goal_handle_timer_ = get_node()->create_wall_timer(action_monitor_period_.to_chrono(), + std::bind(&RealtimeGoalHandle::runNonRealtime, rt_goal)); + trajectory_active_ = true; + return; +} + +bool PassthroughTrajectoryController::check_goal_tolerance() +{ + auto goal_tolerance = goal_tolerance_.readFromRT(); + auto joint_mapping = joint_trajectory_mapping_.readFromRT(); + auto joint_names_internal = joint_names_.readFromRT(); + if (goal_tolerance->empty()) { + return true; + } + + for (size_t i = 0; i < number_of_joints_; ++i) { + const std::string joint_name = joint_names_internal->at(i); + const auto& joint_tol = goal_tolerance->at(i); + const auto& setpoint = active_joint_traj_.points.back().positions[joint_mapping->at(joint_name)]; + const double joint_pos = joint_position_state_interface_[i].get().get_value(); + if (std::abs(joint_pos - setpoint) > joint_tol.position) { + // RCLCPP_ERROR( + // get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f, where tolerance is %f", + // joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos, joint_tol.position); + return false; + } + + if (!active_joint_traj_.points.back().velocities.empty()) { + const double joint_vel = joint_velocity_state_interface_[i].get().get_value(); + const auto& expected_vel = active_joint_traj_.points.back().velocities[joint_mapping->at(joint_name)]; + if (std::abs(joint_vel - expected_vel) > joint_tol.velocity) { + return false; + } + } + if (!active_joint_traj_.points.back().accelerations.empty()) { + const double joint_vel = joint_acceleration_state_interface_[i].get().get_value(); + const auto& expected_vel = active_joint_traj_.points.back().accelerations[joint_mapping->at(joint_name)]; + if (std::abs(joint_vel - expected_vel) > joint_tol.acceleration) { + return false; + } + } + } + + return true; +} + +void PassthroughTrajectoryController::end_goal() +{ + trajectory_active_ = false; + transfer_command_interface_->get().set_value(TRANSFER_STATE_IDLE); +} + +std::unordered_map +PassthroughTrajectoryController::create_joint_mapping(const std::vector& joint_names) const +{ + std::unordered_map joint_mapping; + auto joint_names_internal = joint_names_.readFromNonRT(); + for (auto& joint_name : *joint_names_internal) { + auto found_it = std::find(joint_names.begin(), joint_names.end(), joint_name); + if (found_it != joint_names.end()) { + joint_mapping.insert({ joint_name, found_it - joint_names.begin() }); + } + } + return joint_mapping; +} +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::PassthroughTrajectoryController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml new file mode 100644 index 000000000..db8844338 --- /dev/null +++ b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml @@ -0,0 +1,31 @@ +--- +passthrough_trajectory_controller: + speed_scaling_interface_name: { + type: string, + default_value: "speed_scaling/speed_scaling_factor", + description: "Fully qualified name of the speed scaling interface name" + } + tf_prefix: { + type: string, + default_value: "", + description: "Urdf prefix of the corresponding arm" + } + joints: { + type: string_array, + default_value: [], + description: "Joint names to claim and listen to", + read_only: true, + validation: { + unique<>: null, + } + } + state_interfaces: { + type: string_array, + default_value: [], + description: "State interfaces provided by the hardware for all joints.", + read_only: true, + validation: { + unique<>: null, + subset_of<>: [["position", "velocity", "acceleration"]], + } + } diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index c47a83452..0d0487a03 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -184,6 +184,10 @@ if(BUILD_TESTING) TIMEOUT 800 ) + add_launch_test(test/integration_test_controller_switch.py + TIMEOUT + 800 + ) add_launch_test(test/urscript_interface.py TIMEOUT 500 diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index ceb56e5ef..182d6eac5 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,8 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + passthrough_trajectory_controller: + type: ur_controllers/PassthroughTrajectoryController tcp_pose_broadcaster: type: pose_broadcaster/PoseBroadcaster @@ -113,6 +115,21 @@ scaled_joint_trajectory_controller: $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor +passthrough_trajectory_controller: + ros__parameters: + tf_prefix: "$(var tf_prefix)" + joints: + - $(var tf_prefix)shoulder_pan_joint + - $(var tf_prefix)shoulder_lift_joint + - $(var tf_prefix)elbow_joint + - $(var tf_prefix)wrist_1_joint + - $(var tf_prefix)wrist_2_joint + - $(var tf_prefix)wrist_3_joint + state_interfaces: + - position + - velocity + speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor + forward_velocity_controller: ros__parameters: joints: diff --git a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp index 515f19999..428456113 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -77,7 +77,8 @@ enum StoppingInterface { NONE, STOP_POSITION, - STOP_VELOCITY + STOP_VELOCITY, + STOP_PASSTHROUGH }; // We define our own quaternion to use it as a buffer, since we need to pass pointers to the state @@ -157,6 +158,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void updateNonDoubleValues(); void extractToolPose(); void transformForceTorque(); + void check_passthrough_trajectory_controller(); + void trajectory_done_callback(urcl::control::TrajectoryResult result); + bool has_accelerations(std::vector> accelerations); + bool has_velocities(std::vector> velocities); urcl::vector6d_t urcl_position_commands_; urcl::vector6d_t urcl_position_commands_old_; @@ -220,6 +225,14 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double get_robot_software_version_bugfix_; double get_robot_software_version_build_; + // Passthrough trajectory controller interface values + double passthrough_trajectory_transfer_state_; + double passthrough_trajectory_abort_; + bool passthrough_trajectory_controller_running_; + urcl::vector6d_t passthrough_trajectory_positions_; + urcl::vector6d_t passthrough_trajectory_velocities_; + urcl::vector6d_t passthrough_trajectory_accelerations_; + double passthrough_trajectory_time_from_start_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; @@ -241,6 +254,13 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool non_blocking_read_; double robot_program_running_copy_; + /* Vectors used to store the trajectory received from the passthrough trajectory controller. The whole trajectory is + * received before it is sent to the robot. */ + std::vector> trajectory_joint_positions_; + std::vector> trajectory_joint_velocities_; + std::vector> trajectory_joint_accelerations_; + std::vector trajectory_times_; + PausingState pausing_state_; double pausing_ramp_up_increment_; @@ -256,6 +276,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::atomic_bool rtde_comm_has_been_started_ = false; urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); + + const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur10.launch.py b/ur_robot_driver/launch/ur10.launch.py index 8bbc698db..2bfde7780 100644 --- a/ur_robot_driver/launch/ur10.launch.py +++ b/ur_robot_driver/launch/ur10.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur10e.launch.py b/ur_robot_driver/launch/ur10e.launch.py index 024712440..18c909225 100644 --- a/ur_robot_driver/launch/ur10e.launch.py +++ b/ur_robot_driver/launch/ur10e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur16e.launch.py b/ur_robot_driver/launch/ur16e.launch.py index f10d56c3b..82850e5ee 100644 --- a/ur_robot_driver/launch/ur16e.launch.py +++ b/ur_robot_driver/launch/ur16e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur20.launch.py b/ur_robot_driver/launch/ur20.launch.py index a5f8afc4d..547c9293d 100644 --- a/ur_robot_driver/launch/ur20.launch.py +++ b/ur_robot_driver/launch/ur20.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur3.launch.py b/ur_robot_driver/launch/ur3.launch.py index d37e50dd9..38de8f5f4 100644 --- a/ur_robot_driver/launch/ur3.launch.py +++ b/ur_robot_driver/launch/ur3.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur30.launch.py b/ur_robot_driver/launch/ur30.launch.py index fcbac536b..95478caa1 100644 --- a/ur_robot_driver/launch/ur30.launch.py +++ b/ur_robot_driver/launch/ur30.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur3e.launch.py b/ur_robot_driver/launch/ur3e.launch.py index 87e5a949d..78c145a87 100644 --- a/ur_robot_driver/launch/ur3e.launch.py +++ b/ur_robot_driver/launch/ur3e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur5.launch.py b/ur_robot_driver/launch/ur5.launch.py index 6a1439693..c70a6bc39 100644 --- a/ur_robot_driver/launch/ur5.launch.py +++ b/ur_robot_driver/launch/ur5.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur5e.launch.py b/ur_robot_driver/launch/ur5e.launch.py index df36d7e1d..433f5e701 100644 --- a/ur_robot_driver/launch/ur5e.launch.py +++ b/ur_robot_driver/launch/ur5e.launch.py @@ -69,6 +69,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], ) ) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 3561d1c87..9e1ef380b 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -175,6 +175,7 @@ def controller_spawner(controllers, active=True): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ] if activate_joint_controller.perform(context) == "true": controllers_active.append(initial_joint_controller.perform(context)) @@ -320,6 +321,7 @@ def generate_launch_description(): "joint_trajectory_controller", "forward_velocity_controller", "forward_position_controller", + "passthrough_trajectory_controller", ], description="Initially loaded robot controller.", ) diff --git a/ur_robot_driver/scripts/example_move.py b/ur_robot_driver/scripts/example_move.py index 51ec5c855..0f32146b5 100755 --- a/ur_robot_driver/scripts/example_move.py +++ b/ur_robot_driver/scripts/example_move.py @@ -39,8 +39,10 @@ from rclpy.action import ActionClient from builtin_interfaces.msg import Duration +from action_msgs.msg import GoalStatus from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance TRAJECTORIES = { "traj0": [ @@ -133,7 +135,10 @@ def execute_trajectory(self, traj_name): goal = FollowJointTrajectory.Goal() goal.trajectory = self.goals[traj_name] - goal.goal_time_tolerance = Duration(sec=0, nanosec=1000) + goal.goal_time_tolerance = Duration(sec=0, nanosec=500000000) + goal.goal_tolerance = [ + JointTolerance(position=0.01, velocity=0.01, name=self.joints[i]) for i in range(6) + ] self._send_goal_future = self._action_client.send_goal_async(goal) self._send_goal_future.add_done_callback(self.goal_response_callback) @@ -151,12 +156,17 @@ def goal_response_callback(self, future): def get_result_callback(self, future): result = future.result().result - self.get_logger().info(f"Done with result: {self.error_code_to_str(result.error_code)}") - if result.error_code == FollowJointTrajectory.Result.SUCCESSFUL: + status = future.result().status + self.get_logger().info(f"Done with result: {self.status_to_str(status)}") + if status == GoalStatus.STATUS_SUCCEEDED: time.sleep(2) self.execute_next_trajectory() else: - raise RuntimeError("Executing trajectory failed") + if result.error_code != FollowJointTrajectory.Result.SUCCESSFUL: + self.get_logger().error( + f"Done with result: {self.error_code_to_str(result.error_code)}" + ) + raise RuntimeError("Executing trajectory failed. " + result.error_string) @staticmethod def error_code_to_str(error_code): @@ -173,6 +183,23 @@ def error_code_to_str(error_code): if error_code == FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED: return "GOAL_TOLERANCE_VIOLATED" + @staticmethod + def status_to_str(error_code): + if error_code == GoalStatus.STATUS_UNKNOWN: + return "UNKNOWN" + if error_code == GoalStatus.STATUS_ACCEPTED: + return "ACCEPTED" + if error_code == GoalStatus.STATUS_EXECUTING: + return "EXECUTING" + if error_code == GoalStatus.STATUS_CANCELING: + return "CANCELING" + if error_code == GoalStatus.STATUS_SUCCEEDED: + return "SUCCEEDED" + if error_code == GoalStatus.STATUS_CANCELED: + return "CANCELED" + if error_code == GoalStatus.STATUS_ABORTED: + return "ABORTED" + def main(args=None): rclpy.init(args=args) @@ -180,6 +207,8 @@ def main(args=None): jtc_client = JTCClient() try: rclpy.spin(jtc_client) + except RuntimeError as err: + jtc_client.get_logger().error(str(err)) except SystemExit: rclpy.logging.get_logger("jtc_client").info("Done") diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index fb92fc4bc..3fe6dbec3 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -46,7 +46,7 @@ #include "ur_client_library/ur/tool_communication.h" #include "ur_client_library/ur/version_information.h" -#include "rclcpp/rclcpp.hpp" +#include #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "ur_robot_driver/hardware_interface.hpp" #include "ur_robot_driver/urcl_log_handler.hpp" @@ -86,6 +86,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys start_modes_ = {}; position_controller_running_ = false; velocity_controller_running_ = false; + passthrough_trajectory_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; pausing_ramp_up_increment_ = 0.01; @@ -94,6 +95,11 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys initialized_ = false; async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; + passthrough_trajectory_transfer_state_ = 0.0; + passthrough_trajectory_abort_ = 0.0; + trajectory_joint_positions_.clear(); + trajectory_joint_velocities_.clear(); + trajectory_joint_accelerations_.clear(); for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -144,7 +150,6 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys return hardware_interface::CallbackReturn::ERROR; } } - return hardware_interface::CallbackReturn::SUCCESS; } @@ -333,6 +338,32 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "zero_ftsensor", "zero_ftsensor_async_success", &zero_ftsensor_async_success_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "transfer_state", + &passthrough_trajectory_transfer_state_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "time_from_start", + &passthrough_trajectory_time_from_start_)); + command_interfaces.emplace_back( + hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, "abort", &passthrough_trajectory_abort_)); + + for (size_t i = 0; i < 6; ++i) { + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, + "setpoint_positions_" + std::to_string(i), + &passthrough_trajectory_positions_[i])); + } + + for (size_t i = 0; i < 6; ++i) { + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, + "setpoint_velocities_" + std::to_string(i), + &passthrough_trajectory_velocities_[i])); + } + + for (size_t i = 0; i < 6; ++i) { + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + PASSTHROUGH_GPIO, + "setpoint_accelerations_" + std::to_string(i), + &passthrough_trajectory_accelerations_[i])); + } + return command_interfaces; } @@ -500,6 +531,9 @@ URPositionHardwareInterface::on_configure(const rclcpp_lifecycle::State& previou RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "System successfully started!"); + ur_driver_->registerTrajectoryDoneCallback( + std::bind(&URPositionHardwareInterface::trajectory_done_callback, this, std::placeholders::_1)); + return hardware_interface::CallbackReturn::SUCCESS; } @@ -674,6 +708,9 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (velocity_controller_running_) { ur_driver_->writeJointCommand(urcl_velocity_commands_, urcl::comm::ControlMode::MODE_SPEEDJ, receive_timeout_); + } else if (passthrough_trajectory_controller_running_) { + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + check_passthrough_trajectory_controller(); } else { ur_driver_->writeKeepalive(); } @@ -843,30 +880,53 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - start_modes_.clear(); + start_modes_ = std::vector(info_.joints.size(), "UNDEFINED"); stop_modes_.clear(); + std::vector control_modes(info_.joints.size()); + const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); + + // Assess current state + for (auto i = 0u; i < info_.joints.size(); i++) { + if (position_controller_running_) { + control_modes[i] = hardware_interface::HW_IF_POSITION; + } + if (velocity_controller_running_) { + control_modes[i] = hardware_interface::HW_IF_VELOCITY; + } + if (passthrough_trajectory_controller_running_) { + control_modes[i] = PASSTHROUGH_GPIO; + } + } + + if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(), + [&](const std::string& other) { return other == start_modes_[0]; })) { + RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same."); + return hardware_interface::return_type::ERROR; + } // Starting interfaces - // add start interface per joint in tmp var for later check + // If a joint has been reserved already, raise an error. + // Modes that are not directly mapped to a single joint such as force_mode reserve all joints. for (const auto& key : start_interfaces) { for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { - start_modes_.push_back(hardware_interface::HW_IF_POSITION); - } - if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { - start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = hardware_interface::HW_IF_POSITION; + } else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = hardware_interface::HW_IF_VELOCITY; + } else if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { + if (start_modes_[i] != "UNDEFINED") { + return hardware_interface::return_type::ERROR; + } + start_modes_[i] = PASSTHROUGH_GPIO; } } } - // set new mode to all interfaces at the same time - if (start_modes_.size() != 0 && start_modes_.size() != 6) { - ret_val = hardware_interface::return_type::ERROR; - } - - // all start interfaces must be the same - can't mix position and velocity control - if (start_modes_.size() != 0 && !std::equal(start_modes_.begin() + 1, start_modes_.end(), start_modes_.begin())) { - ret_val = hardware_interface::return_type::ERROR; - } // Stopping interfaces // add stop interface per joint in tmp var for later check @@ -874,15 +934,55 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod for (auto i = 0u; i < info_.joints.size(); i++) { if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_POSITION) { stop_modes_.push_back(StoppingInterface::STOP_POSITION); + if (control_modes[i] == hardware_interface::HW_IF_POSITION) { + control_modes[i] = "UNDEFINED"; + } } if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); + if (control_modes[i] == hardware_interface::HW_IF_VELOCITY) { + control_modes[i] = "UNDEFINED"; + } + } + if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { + stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH); + if (control_modes[i] == PASSTHROUGH_GPIO) { + control_modes[i] = "UNDEFINED"; + } } } } - // stop all interfaces at the same time - if (stop_modes_.size() != 0 && - (stop_modes_.size() != 6 || !std::equal(stop_modes_.begin() + 1, stop_modes_.end(), stop_modes_.begin()))) { + + // Do not start conflicting controllers + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [this](auto& item) { return (item == PASSTHROUGH_GPIO); }) && + (std::any_of(start_modes_.begin(), start_modes_.end(), + [](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + }) || + std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); + }))) { + ret_val = hardware_interface::return_type::ERROR; + } + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [](auto& item) { return (item == hardware_interface::HW_IF_POSITION); }) && + (std::any_of( + start_modes_.begin(), start_modes_.end(), + [this](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); }) || + std::any_of(control_modes.begin(), control_modes.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); + }))) { + ret_val = hardware_interface::return_type::ERROR; + } + if (std::any_of(start_modes_.begin(), start_modes_.end(), + [](auto& item) { return (item == hardware_interface::HW_IF_VELOCITY); }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION || + item == PASSTHROUGH_GPIO); + })) { ret_val = hardware_interface::return_type::ERROR; } @@ -903,19 +1003,34 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_VELOCITY) != stop_modes_.end()) { velocity_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; + } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), + StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { + passthrough_trajectory_controller_running_ = false; + passthrough_trajectory_abort_ = 1.0; + trajectory_joint_positions_.clear(); + trajectory_joint_accelerations_.clear(); + trajectory_joint_velocities_.clear(); } if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_POSITION) != start_modes_.end()) { velocity_controller_running_ = false; + passthrough_trajectory_controller_running_ = false; urcl_position_commands_ = urcl_position_commands_old_ = urcl_joint_positions_; position_controller_running_ = true; } else if (start_modes_.size() != 0 && std::find(start_modes_.begin(), start_modes_.end(), hardware_interface::HW_IF_VELOCITY) != start_modes_.end()) { position_controller_running_ = false; + passthrough_trajectory_controller_running_ = false; urcl_velocity_commands_ = { { 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 } }; velocity_controller_running_ = true; + } else if (start_modes_.size() != 0 && + std::find(start_modes_.begin(), start_modes_.end(), PASSTHROUGH_GPIO) != start_modes_.end()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + passthrough_trajectory_controller_running_ = true; + passthrough_trajectory_abort_ = 0.0; } start_modes_.clear(); @@ -923,6 +1038,87 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod return ret_val; } + +void URPositionHardwareInterface::check_passthrough_trajectory_controller() +{ + static double last_time = 0.0; + // See passthrough_trajectory_controller.hpp for an explanation of the passthrough_trajectory_transfer_state_ values. + + // We should abort and are not in state IDLE + if (passthrough_trajectory_abort_ == 1.0 && passthrough_trajectory_transfer_state_ != 0.0) { + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); + } else if (passthrough_trajectory_transfer_state_ == 2.0) { + passthrough_trajectory_abort_ = 0.0; + trajectory_joint_positions_.push_back(passthrough_trajectory_positions_); + + trajectory_times_.push_back(passthrough_trajectory_time_from_start_ - last_time); + last_time = passthrough_trajectory_time_from_start_; + + if (!std::isnan(passthrough_trajectory_velocities_[0])) { + trajectory_joint_velocities_.push_back(passthrough_trajectory_velocities_); + } + if (!std::isnan(passthrough_trajectory_accelerations_[0])) { + trajectory_joint_accelerations_.push_back(passthrough_trajectory_accelerations_); + } + passthrough_trajectory_transfer_state_ = 1.0; + /* When all points have been read, write them to the physical robot controller.*/ + } else if (passthrough_trajectory_transfer_state_ == 3.0) { + /* Tell robot controller how many points are in the trajectory. */ + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + trajectory_joint_positions_.size()); + /* Write the appropriate type of point depending on the combination of positions, velocities and accelerations. */ + if (!has_velocities(trajectory_joint_velocities_) && !has_accelerations(trajectory_joint_accelerations_)) { + for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], urcl::vector6d_t{ 0, 0, 0, 0, 0, 0 }, + trajectory_times_[i]); + } + } else if (has_velocities(trajectory_joint_velocities_) && !has_accelerations(trajectory_joint_accelerations_)) { + for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], + trajectory_times_[i]); + } + } else if (!has_velocities(trajectory_joint_velocities_) && has_accelerations(trajectory_joint_accelerations_)) { + for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_accelerations_[i], + trajectory_times_[i]); + } + } else if (has_velocities(trajectory_joint_velocities_) && has_accelerations(trajectory_joint_accelerations_)) { + for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], trajectory_joint_velocities_[i], + trajectory_joint_accelerations_[i], trajectory_times_[i]); + } + } + trajectory_joint_positions_.clear(); + trajectory_joint_accelerations_.clear(); + trajectory_joint_velocities_.clear(); + trajectory_times_.clear(); + last_time = 0.0; + passthrough_trajectory_abort_ = 0.0; + passthrough_trajectory_transfer_state_ = 4.0; + } +} + +void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result) +{ + if (result == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_FAILURE) { + passthrough_trajectory_abort_ = 1.0; + } else { + passthrough_trajectory_abort_ = 0.0; + } + passthrough_trajectory_transfer_state_ = 5.0; + return; +} + +bool URPositionHardwareInterface::has_velocities(std::vector> velocities) +{ + return (velocities.size() > 0); +} + +bool URPositionHardwareInterface::has_accelerations(std::vector> accelerations) +{ + return (accelerations.size() > 0); +} + } // namespace ur_robot_driver #include "pluginlib/class_list_macros.hpp" diff --git a/ur_robot_driver/test/integration_test_controller_switch.py b/ur_robot_driver/test/integration_test_controller_switch.py new file mode 100644 index 000000000..20ea69190 --- /dev/null +++ b/ur_robot_driver/test/integration_test_controller_switch.py @@ -0,0 +1,287 @@ +#!/usr/bin/env python +# Copyright 2019, Universal Robots A/S +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# * Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# +# * Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in the +# documentation and/or other materials provided with the distribution. +# +# * Neither the name of the {copyright_holder} nor the names of its +# contributors may be used to endorse or promote products derived from +# this software without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. + +import os +import sys +import time +import unittest + +import pytest + +import launch_testing +import rclpy +from rclpy.node import Node + +from controller_manager_msgs.srv import SwitchController + +sys.path.append(os.path.dirname(__file__)) +from test_common import ( # noqa: E402 + ControllerManagerInterface, + DashboardInterface, + IoStatusInterface, + generate_driver_test_description, +) + + +@pytest.mark.launch_test +@launch_testing.parametrize( + "tf_prefix", + [(""), ("my_ur_")], +) +def generate_test_description(tf_prefix): + return generate_driver_test_description(tf_prefix=tf_prefix) + + +class RobotDriverTest(unittest.TestCase): + @classmethod + def setUpClass(cls): + # Initialize the ROS context + rclpy.init() + cls.node = Node("robot_driver_test") + time.sleep(1) + cls.init_robot(cls) + + @classmethod + def tearDownClass(cls): + # Shutdown the ROS context + cls.node.destroy_node() + rclpy.shutdown() + + def init_robot(self): + self._dashboard_interface = DashboardInterface(self.node) + self._controller_manager_interface = ControllerManagerInterface(self.node) + self._io_status_controller_interface = IoStatusInterface(self.node) + + def setUp(self): + self._dashboard_interface.start_robot() + time.sleep(1) + self.assertTrue(self._io_status_controller_interface.resend_robot_program().success) + + def test_activating_multiple_controllers_same_interface_fails(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + + # Activating different motion controllers should not be possible + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_position_controller", + ], + ).ok + ) + + def test_activating_multiple_controllers_different_interface_fails(self): + # Deactivate all writing controllers + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_position_controller", + "forward_velocity_controller", + ], + ).ok + ) + + def test_activating_controller_with_running_position_controller_fails(self): + # Having a position-based controller active, no other controller should be able to + # activate. + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + ], + deactivate_controllers=[ + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + "passthrough_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "passthrough_trajectory_controller", + ], + ).ok + ) + # Stop controller again + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + ], + ).ok + ) + + def test_activating_controller_with_running_passthrough_trajectory_controller_fails(self): + # Having a position-based controller active, no other controller should be able to + # activate. + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=[ + "scaled_joint_trajectory_controller", + "joint_trajectory_controller", + "forward_position_controller", + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "scaled_joint_trajectory_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_position_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "forward_velocity_controller", + ], + ).ok + ) + self.assertFalse( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + activate_controllers=[ + "joint_trajectory_controller", + ], + ).ok + ) + # Stop the controller again + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.STRICT, + deactivate_controllers=[ + "passthrough_trajectory_controller", + ], + ).ok + ) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 88fcffce3..6350ea78d 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -1,4 +1,4 @@ -#!/usr/bin/env python +#!/usr/bin/env python3 # Copyright 2019, FZI Forschungszentrum Informatik # # Redistribution and use in source and binary forms, with or without @@ -37,6 +37,7 @@ import rclpy from builtin_interfaces.msg import Duration from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance from controller_manager_msgs.srv import SwitchController from rclpy.node import Node from sensor_msgs.msg import JointState @@ -56,9 +57,9 @@ TIMEOUT_EXECUTE_TRAJECTORY = 30 ROBOT_JOINTS = [ - "elbow_joint", - "shoulder_lift_joint", "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", "wrist_1_joint", "wrist_2_joint", "wrist_3_joint", @@ -100,6 +101,11 @@ def init_robot(self): "/scaled_joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectory, ) + self._passthrough_forward_joint_trajectory = ActionInterface( + self.node, + "/passthrough_trajectory_controller/follow_joint_trajectory", + FollowJointTrajectory, + ) def setUp(self): self._dashboard_interface.start_robot() @@ -123,6 +129,22 @@ def test_start_scaled_jtc_controller(self): ).ok ) + def test_start_passthrough_controller(self): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + def test_set_io(self): """Test to set an IO and check whether it has been set.""" # Create io callback to verify result @@ -265,7 +287,10 @@ def test_trajectory_scaled_aborts_on_violation(self, tf_prefix): Duration(sec=6, nanosec=50000000), [-1.0 for j in ROBOT_JOINTS], ), # physically unfeasible - (Duration(sec=8, nanosec=0), [-1.5 for j in ROBOT_JOINTS]), # physically unfeasible + ( + Duration(sec=8, nanosec=0), + [-1.5 for j in ROBOT_JOINTS], + ), # physically unfeasible ] trajectory = JointTrajectory( @@ -318,17 +343,105 @@ def js_cb(msg): ) ) - # TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message - # see https://github.com/ros-controls/ros2_controllers/issues/249 - # Now do the same again, but with a goal time constraint - # self.node.get_logger().info("Sending scaled goal with time restrictions") - # - # goal.goal_time_tolerance = Duration(nanosec=10000000) - # goal_response = self.call_action("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal) - # - # self.assertEqual(goal_response.accepted, True) - # - # if goal_response.accepted: - # result = self.get_result("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal_response, TIMEOUT_EXECUTE_TRAJECTORY) - # self.assertEqual(result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED) - # self.node.get_logger().info("Received result GOAL_TOLERANCE_VIOLATED") + # TODO: uncomment when JTC starts taking into account goal_time_tolerance from goal message + # see https://github.com/ros-controls/ros2_controllers/issues/249 + # Now do the same again, but with a goal time constraint + # self.node.get_logger().info("Sending scaled goal with time restrictions") + # + # goal.goal_time_tolerance = Duration(nanosec=10000000) + # goal_response = self.call_action("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal) + # + # self.assertEqual(goal_response.accepted, True) + # + # if goal_response.accepted: + # result = self.get_result("/scaled_joint_trajectory_controller/follow_joint_trajectory", goal_response, TIMEOUT_EXECUTE_TRAJECTORY) + # self.assertEqual(result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED) + # self.node.get_logger().info("Received result GOAL_TOLERANCE_VIOLATED") + + def test_passthrough_trajectory(self, tf_prefix): + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + activate_controllers=["passthrough_trajectory_controller"], + deactivate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) + waypts = [ + [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + [-3, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + [-1.58, -1.692, -1.4311, -0.0174, 1.5882, 0.0349], + ] + time_vec = [ + Duration(sec=4, nanosec=0), + Duration(sec=8, nanosec=0), + Duration(sec=12, nanosec=0), + ] + goal_tolerance = [ + JointTolerance(position=0.01, name=tf_prefix + ROBOT_JOINTS[i]) + for i in range(len(ROBOT_JOINTS)) + ] + goal_time_tolerance = Duration(sec=1, nanosec=0) + test_trajectory = zip(time_vec, waypts) + trajectory = JointTrajectory( + points=[ + JointTrajectoryPoint(positions=pos, time_from_start=times) + for (times, pos) in test_trajectory + ], + joint_names=[tf_prefix + ROBOT_JOINTS[i] for i in range(len(ROBOT_JOINTS))], + ) + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) + # Test impossible goal tolerance, should fail. + goal_tolerance = [ + JointTolerance(position=0.000000001, name=tf_prefix + ROBOT_JOINTS[i]) + for i in range(len(ROBOT_JOINTS)) + ] + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual( + result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED + ) + + # Test impossible goal time + goal_tolerance = [ + JointTolerance(position=0.01, name=tf_prefix + ROBOT_JOINTS[i]) for i in range(6) + ] + goal_time_tolerance.sec = 0 + goal_time_tolerance.nanosec = 10 + goal_handle = self._passthrough_forward_joint_trajectory.send_goal( + trajectory=trajectory, + goal_time_tolerance=goal_time_tolerance, + goal_tolerance=goal_tolerance, + ) + self.assertTrue(goal_handle.accepted) + if goal_handle.accepted: + result = self._passthrough_forward_joint_trajectory.get_result( + goal_handle, TIMEOUT_EXECUTE_TRAJECTORY + ) + self.assertEqual( + result.error_code, FollowJointTrajectory.Result.GOAL_TOLERANCE_VIOLATED + ) + self.assertTrue( + self._controller_manager_interface.switch_controller( + strictness=SwitchController.Request.BEST_EFFORT, + deactivate_controllers=["passthrough_trajectory_controller"], + activate_controllers=["scaled_joint_trajectory_controller"], + ).ok + ) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 8db7b6835..a8a1a8ffb 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -29,7 +29,12 @@ import time import rclpy -from controller_manager_msgs.srv import ListControllers, SwitchController +from controller_manager_msgs.srv import ( + ListControllers, + SwitchController, + LoadController, + UnloadController, +) from launch import LaunchDescription from launch.actions import ( DeclareLaunchArgument, @@ -58,6 +63,15 @@ TIMEOUT_WAIT_SERVICE_INITIAL = 120 # If we download the docker image simultaneously to the tests, it can take quite some time until the dashboard server is reachable and usable. TIMEOUT_WAIT_ACTION = 10 +ROBOT_JOINTS = [ + "elbow_joint", + "shoulder_lift_joint", + "shoulder_pan_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + def _wait_for_service(node, srv_name, srv_type, timeout): client = node.create_client(srv_type, srv_name) @@ -223,7 +237,11 @@ def _check_call(self, result): class ControllerManagerInterface( _ServiceInterface, namespace="/controller_manager", - initial_services={"switch_controller": SwitchController}, + initial_services={ + "switch_controller": SwitchController, + "load_controller": LoadController, + "unload_controller": UnloadController, + }, services={"list_controllers": ListControllers}, ): def wait_for_controller(self, controller_name, target_state="active"): diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 9d5755f34..d5df9b2b8 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -229,6 +229,31 @@ + + + + + + + + + + + + + + + + + + + + + + + + +