From 76bee81769a85bd982d84ae85d54ad1476587072 Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 11 Mar 2024 09:35:20 +0000 Subject: [PATCH 01/50] Started work on forward position controller Creates errors on compilation due to multiple definition of funtions. --- ur_controllers/CMakeLists.txt | 9 ++- ur_controllers/controller_plugins.xml | 5 ++ .../forward_position_controller.hpp | 72 ++++++++++++++++++ .../src/forward_position_controller.cpp | 76 +++++++++++++++++++ ...orward_position_controller_parameters.yaml | 6 ++ .../ur_robot_driver/hardware_interface.hpp | 4 +- ur_robot_driver/src/hardware_interface.cpp | 3 + 7 files changed, 173 insertions(+), 2 deletions(-) create mode 100644 ur_controllers/include/ur_controllers/forward_position_controller.hpp create mode 100644 ur_controllers/src/forward_position_controller.cpp create mode 100644 ur_controllers/src/forward_position_controller_parameters.yaml diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a2b72e599..a11c27960 100644 --- a/ur_controllers/CMakeLists.txt +++ b/ur_controllers/CMakeLists.txt @@ -54,10 +54,16 @@ generate_parameter_library( src/scaled_joint_trajectory_controller_parameters.yaml ) +generate_parameter_library( + forward_position_controller_parameters + src/forward_position_controller_parameters.yaml +) + add_library(${PROJECT_NAME} SHARED src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp - src/gpio_controller.cpp) + src/gpio_controller.cpp + src/forward_position_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE include @@ -66,6 +72,7 @@ target_link_libraries(${PROJECT_NAME} gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters + forward_position_controller_parameters ) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index f0058ab55..8b66cbb91 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,4 +14,9 @@ This controller publishes the Tool IO. + + + This controller forwards a cartesian trajectory to the robot controller for interpolation. + + diff --git a/ur_controllers/include/ur_controllers/forward_position_controller.hpp b/ur_controllers/include/ur_controllers/forward_position_controller.hpp new file mode 100644 index 000000000..da200c1f6 --- /dev/null +++ b/ur_controllers/include/ur_controllers/forward_position_controller.hpp @@ -0,0 +1,72 @@ +// 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__FORWARD_POSITION_CONTROLLER_HPP_ +#define UR_CONTROLLERS__FORWARD_POSITION_CONTROLLER_HPP_ + +#include +// #include "controller_interface/controller_interface.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "forward_position_controller_parameters.hpp" + +namespace forward_controller +{ +class ForwardPositionController : public joint_trajectory_controller::JointTrajectoryController +{ +public: + ForwardPositionController() = default; + ~ForwardPositionController() override = default; + + controller_interface::InterfaceConfiguration state_interface_configuration() const override; + + controller_interface::CallbackReturn on_activate(const rclcpp_lifecycle::State& state) override; + + controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; + + controller_interface::CallbackReturn on_init() override; + + void start_interpolation(); + +private: + std::shared_ptr forward_param_listener_; + forward_position_controller::Params forward_params_; +}; +} // namespace forward_controller +#endif // UR_CONTROLLERS__FORWARD_POSITION_CONTROLLER_HPP_ diff --git a/ur_controllers/src/forward_position_controller.cpp b/ur_controllers/src/forward_position_controller.cpp new file mode 100644 index 000000000..498e8a345 --- /dev/null +++ b/ur_controllers/src/forward_position_controller.cpp @@ -0,0 +1,76 @@ +// 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 "ur_controllers/forward_position_controller.hpp" + +namespace forward_controller +{ +controller_interface::CallbackReturn ForwardPositionController::on_init() +{ + forward_param_listener_ = std::make_shared(get_node()); + forward_params_ = forward_param_listener_->get_params(); + + return ControllerInterface::on_init(); +} + +controller_interface::InterfaceConfiguration ForwardPositionController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + conf.names.push_back(forward_params_.speed_scaling_interface_name); + + return conf; +} + +controller_interface::CallbackReturn ForwardPositionController::on_activate(const rclcpp_lifecycle::State& state) +{ + return ControllerInterface::on_activate(state); +} + +controller_interface::return_type ForwardPositionController::update(const rclcpp::Time& time, + const rclcpp::Duration& period) +{ + std::cout << state_interfaces_.back().get_value() << std::endl; + return controller_interface::return_type::OK; +} + +} // namespace forward_controller diff --git a/ur_controllers/src/forward_position_controller_parameters.yaml b/ur_controllers/src/forward_position_controller_parameters.yaml new file mode 100644 index 000000000..55a862f3d --- /dev/null +++ b/ur_controllers/src/forward_position_controller_parameters.yaml @@ -0,0 +1,6 @@ +forward_position_controller: + speed_scaling_interface_name: { + type: string, + default_value: "speed_scaling/speed_scaling_factor", + description: "Fully qualified name of the speed scaling interface name" + } 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 67e27a222..6225ee408 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_FORWARD }; /*! @@ -221,6 +222,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::vector start_modes_; bool position_controller_running_; bool velocity_controller_running_; + bool forward_position_controller_running_; std::unique_ptr ur_driver_; std::shared_ptr async_thread_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 5826cf595..3d8df6ce3 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -85,6 +85,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys start_modes_ = {}; position_controller_running_ = false; velocity_controller_running_ = false; + forward_position_controller_running_ = false; runtime_state_ = static_cast(rtde::RUNTIME_STATE::STOPPED); pausing_state_ = PausingState::RUNNING; pausing_ramp_up_increment_ = 0.01; @@ -631,6 +632,8 @@ 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 (forward_position_controller_running_) { + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } else { ur_driver_->writeKeepalive(); } From 686ddbbecc438bf658f430571e9b43c29cbc0e2f Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 11 Mar 2024 14:58:41 +0000 Subject: [PATCH 02/50] Controller is now able to run and read from speed_scale --- ur_controllers/CMakeLists.txt | 15 +- ur_controllers/controller_plugins.xml | 2 +- ... => passthrough_trajectory_controller.hpp} | 57 ++++-- ur_controllers/package.xml | 4 + .../src/forward_position_controller.cpp | 76 -------- .../src/passthrough_trajectory_controller.cpp | 180 ++++++++++++++++++ ...ugh_trajectory_controller_parameters.yaml} | 7 +- .../scaled_joint_trajectory_controller.cpp | 1 + ur_robot_driver/config/ur_controllers.yaml | 3 + .../ur_robot_driver/hardware_interface.hpp | 12 +- ur_robot_driver/launch/ur5e.launch.py | 3 +- ur_robot_driver/launch/ur_control.launch.py | 2 + ur_robot_driver/src/hardware_interface.cpp | 53 +++++- 13 files changed, 312 insertions(+), 103 deletions(-) rename ur_controllers/include/ur_controllers/{forward_position_controller.hpp => passthrough_trajectory_controller.hpp} (53%) delete mode 100644 ur_controllers/src/forward_position_controller.cpp create mode 100644 ur_controllers/src/passthrough_trajectory_controller.cpp rename ur_controllers/src/{forward_position_controller_parameters.yaml => passthrough_trajectory_controller_parameters.yaml} (55%) diff --git a/ur_controllers/CMakeLists.txt b/ur_controllers/CMakeLists.txt index a11c27960..5f7e36925 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) @@ -55,15 +62,15 @@ generate_parameter_library( ) generate_parameter_library( - forward_position_controller_parameters - src/forward_position_controller_parameters.yaml + passthrough_trajectory_controller_parameters + src/passthrough_trajectory_controller_parameters.yaml ) add_library(${PROJECT_NAME} SHARED src/scaled_joint_trajectory_controller.cpp src/speed_scaling_state_broadcaster.cpp src/gpio_controller.cpp - src/forward_position_controller.cpp) + src/passthrough_trajectory_controller.cpp) target_include_directories(${PROJECT_NAME} PRIVATE include @@ -72,7 +79,7 @@ target_link_libraries(${PROJECT_NAME} gpio_controller_parameters speed_scaling_state_broadcaster_parameters scaled_joint_trajectory_controller_parameters - forward_position_controller_parameters + passthrough_trajectory_controller_parameters ) ament_target_dependencies(${PROJECT_NAME} ${THIS_PACKAGE_INCLUDE_DEPENDS} diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index 8b66cbb91..097f898b3 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -14,7 +14,7 @@ This controller publishes the Tool IO. - + This controller forwards a cartesian trajectory to the robot controller for interpolation. diff --git a/ur_controllers/include/ur_controllers/forward_position_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp similarity index 53% rename from ur_controllers/include/ur_controllers/forward_position_controller.hpp rename to ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index da200c1f6..6d95ecfea 100644 --- a/ur_controllers/include/ur_controllers/forward_position_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -38,35 +38,60 @@ */ //---------------------------------------------------------------------- -#ifndef UR_CONTROLLERS__FORWARD_POSITION_CONTROLLER_HPP_ -#define UR_CONTROLLERS__FORWARD_POSITION_CONTROLLER_HPP_ +#ifndef UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ +#define UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ #include -// #include "controller_interface/controller_interface.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" -#include "forward_position_controller_parameters.hpp" +#include +#include -namespace forward_controller +#include "controller_interface/controller_interface.hpp" +#include "rclcpp_action/server.hpp" +#include "rclcpp_action/create_server.hpp" +#include "rclcpp/rclcpp.hpp" +#include "rclcpp_action/server_goal_handle.hpp" + +#include "trajectory_msgs/msg/joint_trajectory.hpp" +#include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "control_msgs/action/follow_joint_trajectory.hpp" +#include "control_msgs/action/joint_trajectory.hpp" + +#include "passthrough_trajectory_controller_parameters.hpp" + +namespace ur_controllers { -class ForwardPositionController : public joint_trajectory_controller::JointTrajectoryController +class PassthroughTrajectoryController : public controller_interface::ControllerInterface { public: - ForwardPositionController() = default; - ~ForwardPositionController() override = default; + 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::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; - controller_interface::CallbackReturn on_init() override; +private: + std::shared_ptr passthrough_param_listener_; + passthrough_trajectory_controller::Params passthrough_params_; - void start_interpolation(); + rclcpp_action::Server::SharedPtr send_trajectory_action_server_; -private: - std::shared_ptr forward_param_listener_; - forward_position_controller::Params forward_params_; + 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( + const std::shared_ptr> goal_handle); }; -} // namespace forward_controller -#endif // UR_CONTROLLERS__FORWARD_POSITION_CONTROLLER_HPP_ +} // namespace ur_controllers +#endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/package.xml b/ur_controllers/package.xml index b21490d5d..5bf7df31b 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/forward_position_controller.cpp b/ur_controllers/src/forward_position_controller.cpp deleted file mode 100644 index 498e8a345..000000000 --- a/ur_controllers/src/forward_position_controller.cpp +++ /dev/null @@ -1,76 +0,0 @@ -// 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 "ur_controllers/forward_position_controller.hpp" - -namespace forward_controller -{ -controller_interface::CallbackReturn ForwardPositionController::on_init() -{ - forward_param_listener_ = std::make_shared(get_node()); - forward_params_ = forward_param_listener_->get_params(); - - return ControllerInterface::on_init(); -} - -controller_interface::InterfaceConfiguration ForwardPositionController::state_interface_configuration() const -{ - controller_interface::InterfaceConfiguration conf; - - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - - conf.names.push_back(forward_params_.speed_scaling_interface_name); - - return conf; -} - -controller_interface::CallbackReturn ForwardPositionController::on_activate(const rclcpp_lifecycle::State& state) -{ - return ControllerInterface::on_activate(state); -} - -controller_interface::return_type ForwardPositionController::update(const rclcpp::Time& time, - const rclcpp::Duration& period) -{ - std::cout << state_interfaces_.back().get_value() << std::endl; - return controller_interface::return_type::OK; -} - -} // namespace forward_controller diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp new file mode 100644 index 000000000..ae0d15e8f --- /dev/null +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -0,0 +1,180 @@ +// 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 "ur_controllers/passthrough_trajectory_controller.hpp" +#include "lifecycle_msgs/msg/state.hpp" + +namespace ur_controllers +{ +controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() +{ + passthrough_param_listener_ = std::make_shared(get_node()); + passthrough_params_ = passthrough_param_listener_->get_params(); + + std::cout << "-------------------------------Initialised passthrough " + "controller-------------------------------------------------" + << std::endl; + + return controller_interface::CallbackReturn::SUCCESS; +} + +controller_interface::CallbackReturn +PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + send_trajectory_action_server_ = rclcpp_action::create_server( + get_node(), std::string(get_node()->get_name()) + "/forward_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)); + + std::cout << "-------------------------------Configured passthrough " + "controller-------------------------------------------------" + << std::endl; + + return ControllerInterface::on_configure(previous_state); +} + +controller_interface::InterfaceConfiguration PassthroughTrajectoryController::state_interface_configuration() const +{ + controller_interface::InterfaceConfiguration conf; + + conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + + conf.names.push_back(passthrough_params_.speed_scaling_interface_name); + + std::cout << conf.names[0] << " / " << conf.names.size() << std::endl; + + std::cout << "------------------------------- Configured passthrough " + "controller state interface -------------------------------------------------" + << std::endl; + + 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; + + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_present"); + + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_number_of_points_"); + + for (size_t i = 0; i < 6; ++i) { + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_positions_" + + std::to_string(i)); + } + for (size_t i = 0; i < 6; ++i) { + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_velocities_" + + std::to_string(i)); + } + for (size_t i = 0; i < 6; ++i) { + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_accelerations_" + + std::to_string(i)); + } + + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_time_from_start"); + + std::cout << "------------------------------- Configured passthrough " + "controller command interface -------------------------------------------------" + << std::endl; + + return config; +} + +controller_interface::CallbackReturn PassthroughTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) +{ + std::cout << "-------------------------------Activated passthrough " + "controller-------------------------------------------------" + << std::endl; + return ControllerInterface::on_activate(state); +} + +controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& /*period*/) +{ + static int delay = 500; + if (!delay) { + std::cout << "-------------------------------Updated passthrough " + "controller-------------------------------------------------" + << std::endl; + + // std::cout << command_interfaces_.at(2).get_name() << " = " << command_interfaces_.at(2).get_value() << std::endl; + delay = 500; + } + delay--; + 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 to forward to robot"); + + // Precondition: Running controller + if (get_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; + } + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} + +rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_callback( + const std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Canceling active trajectory because cancel callback received."); + return rclcpp_action::CancelResponse::ACCEPT; +} + +void PassthroughTrajectoryController::goal_accepted_callback( + const std::shared_ptr> goal_handle) +{ + RCLCPP_INFO(get_node()->get_logger(), "Accepted new trajectory."); + return; +} + +} // namespace ur_controllers + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS(ur_controllers::PassthroughTrajectoryController, controller_interface::ControllerInterface) diff --git a/ur_controllers/src/forward_position_controller_parameters.yaml b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml similarity index 55% rename from ur_controllers/src/forward_position_controller_parameters.yaml rename to ur_controllers/src/passthrough_trajectory_controller_parameters.yaml index 55a862f3d..83a184650 100644 --- a/ur_controllers/src/forward_position_controller_parameters.yaml +++ b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml @@ -1,6 +1,11 @@ -forward_position_controller: +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" + } diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 93ffa0a94..9f0dbe9ef 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -76,6 +76,7 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time, const rclcpp::Duration& period) { + // std::cout << state_interfaces_.back().get_name() << " = " << state_interfaces_.back().get_value() << std::endl; if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) { scaling_factor_ = state_interfaces_.back().get_value(); } else { diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index a512dc1ca..3908d947e 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -24,6 +24,9 @@ controller_manager: forward_position_controller: type: position_controllers/JointGroupPositionController + passthrough_trajectory_controller: + type: ur_controllers/PassthroughTrajectoryController + speed_scaling_state_broadcaster: ros__parameters: 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 6225ee408..d7e5775ae 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -78,7 +78,7 @@ enum StoppingInterface NONE, STOP_POSITION, STOP_VELOCITY, - STOP_FORWARD + STOP_PASSTHROUGH }; /*! @@ -136,6 +136,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void updateNonDoubleValues(); void extractToolPose(); void transformForceTorque(); + void check_passthrough_trajectory_controller(); urcl::vector6d_t urcl_position_commands_; urcl::vector6d_t urcl_position_commands_old_; @@ -192,6 +193,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool initialized_; double system_interface_initialized_; bool async_thread_shutdown_; + double passthrough_trajectory_present_; + double passthrough_trajectory_number_of_points_; + std::array passthrough_trajectory_positions_ = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 }; + std::array passthrough_trajectory_velocities_; + std::array passthrough_trajectory_accelerations_; + double passthrough_trajectory_time_from_start_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; @@ -213,6 +220,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool robot_program_running_; bool non_blocking_read_; double robot_program_running_copy_; + bool passthrough_trajectory_executing_; PausingState pausing_state_; double pausing_ramp_up_increment_; @@ -222,7 +230,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::vector start_modes_; bool position_controller_running_; bool velocity_controller_running_; - bool forward_position_controller_running_; + bool passthrough_trajectory_controller_running_; std::unique_ptr ur_driver_; std::shared_ptr async_thread_; diff --git a/ur_robot_driver/launch/ur5e.launch.py b/ur_robot_driver/launch/ur5e.launch.py index df36d7e1d..5b98023a1 100644 --- a/ur_robot_driver/launch/ur5e.launch.py +++ b/ur_robot_driver/launch/ur5e.launch.py @@ -62,13 +62,14 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "initial_joint_controller", - default_value="scaled_joint_trajectory_controller", + default_value="passthrough_trajectory_controller", description="Initially loaded robot controller.", choices=[ "scaled_joint_trajectory_controller", "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 c884086fa..b10c9cf5f 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -162,6 +162,8 @@ def controller_spawner(controllers, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", + "passthrough_trajectory_controller", + "scaled_joint_trajectory_controller", ] controllers_inactive = ["forward_position_controller"] diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 3d8df6ce3..51dc34ab4 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -85,7 +85,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys start_modes_ = {}; position_controller_running_ = false; velocity_controller_running_ = false; - forward_position_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 +94,7 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys initialized_ = false; async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; + passthrough_trajectory_executing_ = false; for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -299,6 +300,35 @@ 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_controller", "passthrough_trajectory_present", &passthrough_trajectory_present_)); + + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller", + "passthrough_trajectory_number_of_points", + &passthrough_trajectory_number_of_points_)); + + for (size_t i = 0; i < 6; ++i) { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "passthrough_controller", "passthrough_trajectory_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_controller", "passthrough_trajectory_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_controller", "passthrough_trajectory_accelerations_" + std::to_string(i), + &passthrough_trajectory_accelerations_[i])); + } + + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller", + "passthrough_trajectory_time_from_start", + &passthrough_trajectory_time_from_start_)); + return command_interfaces; } @@ -517,6 +547,9 @@ void URPositionHardwareInterface::asyncThread() if (initialized_) { // RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initialized in async thread"); checkAsyncIO(); + if (passthrough_trajectory_controller_running_) { + check_passthrough_trajectory_controller(); + } } std::this_thread::sleep_for(std::chrono::nanoseconds(20000000)); } @@ -632,7 +665,7 @@ 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 (forward_position_controller_running_) { + } else if (passthrough_trajectory_controller_running_) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } else { ur_driver_->writeKeepalive(); @@ -882,6 +915,22 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod return ret_val; } + +void URPositionHardwareInterface::check_passthrough_trajectory_controller() +{ + if (!std::isnan(passthrough_trajectory_present_)) { + if (!passthrough_trajectory_executing_) { + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + static_cast(passthrough_trajectory_number_of_points_)); + passthrough_trajectory_executing_ = true; + } + if (passthrough_trajectory_executing_) { + ur_driver_->writeTrajectoryPoint(passthrough_trajectory_positions_, false, + passthrough_trajectory_time_from_start_); + } + } +} + } // namespace ur_robot_driver #include "pluginlib/class_list_macros.hpp" From b8ef0a703c61dc062e2beb4aa162458b70ef468a Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 25 Mar 2024 09:41:34 +0000 Subject: [PATCH 03/50] Communication between robot and passthrough controller is now functional. --- examples/examples.py | 234 ++++++++++++++++++ ur_controllers/controller_plugins.xml | 2 +- .../passthrough_trajectory_controller.hpp | 24 ++ .../src/passthrough_trajectory_controller.cpp | 147 +++++++++-- ur_robot_driver/CMakeLists.txt | 1 + .../ur_robot_driver/hardware_interface.hpp | 7 +- ur_robot_driver/launch/ur5e.launch.py | 2 +- ur_robot_driver/launch/ur_control.launch.py | 8 +- ur_robot_driver/src/controller_stopper.cpp | 1 + ur_robot_driver/src/hardware_interface.cpp | 65 ++++- .../__pycache__/robot_driver.cpython-310.pyc | Bin 0 -> 3187 bytes .../__pycache__/test_common.cpython-310.pyc | Bin 0 -> 9398 bytes ur_robot_driver/test/robot_driver.py | 38 +-- ur_robot_driver/test/test_common.py | 48 ++-- 14 files changed, 509 insertions(+), 68 deletions(-) create mode 100644 examples/examples.py create mode 100644 ur_robot_driver/test/__pycache__/robot_driver.cpython-310.pyc create mode 100644 ur_robot_driver/test/__pycache__/test_common.cpython-310.pyc diff --git a/examples/examples.py b/examples/examples.py new file mode 100644 index 000000000..e097f068d --- /dev/null +++ b/examples/examples.py @@ -0,0 +1,234 @@ +#!/usr/bin/env python3 +# Copyright 2024, Universal Robots +# +# 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 rclpy +from builtin_interfaces.msg import Duration +from control_msgs.action import JointTrajectory + +from rclpy.action import ActionClient +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint +from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import ( + UnloadController, + LoadController, + ConfigureController, + SwitchController, + ListControllers, +) +import time + +TIMEOUT_WAIT_SERVICE = 10 +TIMEOUT_WAIT_SERVICE_INITIAL = 60 +TIMEOUT_WAIT_ACTION = 10 + +ROBOT_JOINTS = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + + +# Helper functions +def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): + client = node.create_client(srv_type, srv_name) + if client.wait_for_service(timeout) is False: + raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") + + node.get_logger().info(f"Successfully connected to service '{srv_name}'") + return client + + +def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): + client = ActionClient(node, action_type, action_name) + if client.wait_for_server(timeout) is False: + raise Exception( + f"Could not reach action server '{action_name}' within timeout of {timeout}" + ) + + node.get_logger().info(f"Successfully connected to action '{action_name}'") + return client + + +class Robot: + def __init__(self, node): + self.node = node + self.init_robot() + + def init_robot(self): + service_interfaces = { + "/io_and_status_controller/set_io": SetIO, + "/controller_manager/load_controller": LoadController, + "/controller_manager/unload_controller": UnloadController, + "/controller_manager/configure_controller": ConfigureController, + "/controller_manager/switch_controller": SwitchController, + "/controller_manager/list_controllers": ListControllers, + } + self.service_clients = { + srv_name: waitForService(self.node, srv_name, srv_type) + for (srv_name, srv_type) in service_interfaces.items() + } + + def set_io(self, pin, value): + """Test to set an IO.""" + set_io_req = SetIO.Request() + set_io_req.fun = 1 + set_io_req.pin = pin + set_io_req.state = value + + self.call_service("/io_and_status_controller/set_io", set_io_req) + + def send_trajectory(self, waypts, time_vec): + """Send robot trajectory.""" + if len(waypts) != len(time_vec): + raise Exception("waypoints vector and time vec should be same length") + + # Construct test trajectory + joint_trajectory = JTmsg() + joint_trajectory.joint_names = ROBOT_JOINTS + for i in range(len(waypts)): + point = JointTrajectoryPoint() + point.positions = waypts[i] + point.time_from_start = time_vec[i] + joint_trajectory.points.append(point) + + # Sending trajectory goal + goal_response = self.call_action( + self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + ) + if goal_response.accepted is False: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) + return result + + def call_service(self, srv_name, request): + future = self.service_clients[srv_name].call_async(request) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling service: {future.exception()}") + + def call_action(self, ac_client, g): + future = ac_client.send_goal_async(g) + rclpy.spin_until_future_complete(self.node, future) + + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling action: {future.exception()}") + + def get_result(self, ac_client, goal_response): + future_res = ac_client._get_result_async(goal_response) + rclpy.spin_until_future_complete(self.node, future_res) + if future_res.result() is not None: + return future_res.result().result + else: + raise Exception(f"Exception while calling action: {future_res.exception()}") + + def load_passthrough_controller(self): + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names = [] + # Find loaded controllers + for controller in list_response.controller: + names.append(controller.name) + # Check whether the passthrough controller is already loaded + try: + names.index("passthrough_trajectory_controller") + except ValueError: + print("Loading controller") + load_request = LoadController.Request() + load_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/load_controller", load_request) + configure_request = ConfigureController.Request() + configure_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/configure_controller", configure_request) + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names.clear() + # Update the list of controller names. + for controller in list_response.controller: + names.append(controller.name) + id = names.index("passthrough_trajectory_controller") + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + if list_response.controller[id].state == "inactive": + switch_request.activate_controllers = ["passthrough_trajectory_controller"] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + try: + id = names.index("scaled_joint_trajectory_controller") + if list_response.controller[id].state == "active": + switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] + except ValueError: + switch_request.deactivate_controllers = [] + finally: + switch_request.strictness = 1 # Best effort switching, will not terminate program if controller is already running + switch_request.activate_asap = False + switch_request.timeout = Duration(sec=2, nanosec=0) + self.call_service("/controller_manager/switch_controller", switch_request) + # Try unloading the scaled joint trajectory controller + try: + names.index("scaled_joint_trajectory_controller") + unload_request = UnloadController.Request() + unload_request.name = "scaled_joint_trajectory_controller" + self.call_service("/controller_manager/unload_controller", unload_request) + except ValueError: + print("scaled_joint_trajectory_controller not loaded, skipping unload") + # Connect to the passthrough controller action + finally: + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + robot.load_passthrough_controller() + + # The following list are arbitrary joint positions, change according to your own needs + waypts = [ + [-0.47, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], + [-0.47, -2.5998, -1.004, -2.676, -0.992, -1.5406], + ] + time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] + + # Execute trajectory on robot, make sure that the robot is booted and the control script is running + robot.send_trajectory(waypts, time_vec) diff --git a/ur_controllers/controller_plugins.xml b/ur_controllers/controller_plugins.xml index 097f898b3..d5119da18 100644 --- a/ur_controllers/controller_plugins.xml +++ b/ur_controllers/controller_plugins.xml @@ -16,7 +16,7 @@ - This controller forwards a cartesian trajectory to the robot controller for interpolation. + This controller forwards a joint-based trajectory to the robot controller for interpolation. diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 6d95ecfea..52acb9fae 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -41,6 +41,7 @@ #ifndef UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ #define UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ +#include #include #include #include @@ -60,6 +61,18 @@ namespace ur_controllers { +enum CommandInterfaces +{ + PASSTHROUGH_TRAJECTORY_PRESENT = 0, + PASSTHROUGH_POINT_WRITTEN = 1, + PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS = 2, + PASSTHROUGH_TRAJECTORY_CANCEL = 3, + PASSTHROUGH_TRAJECTORY_POSITIONS_ = 4, + PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 10, + PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 16, + PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 22 +}; + class PassthroughTrajectoryController : public controller_interface::ControllerInterface { public: @@ -79,6 +92,8 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: + void start_action_server(void); + std::shared_ptr passthrough_param_listener_; passthrough_trajectory_controller::Params passthrough_params_; @@ -92,6 +107,15 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI void goal_accepted_callback( const std::shared_ptr> goal_handle); + + void + execute(const std::shared_ptr> goal_handle); + trajectory_msgs::msg::JointTrajectory active_joint_traj_; + uint32_t current_point_; + bool trajectory_active_; + uint64_t active_trajectory_elapsed_time_; + uint64_t max_trajectory_time_; + double scaling_factor_; }; } // namespace ur_controllers #endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index ae0d15e8f..375cf889f 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -38,6 +38,7 @@ */ //---------------------------------------------------------------------- +#include #include "ur_controllers/passthrough_trajectory_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" @@ -48,15 +49,26 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() passthrough_param_listener_ = std::make_shared(get_node()); passthrough_params_ = passthrough_param_listener_->get_params(); - std::cout << "-------------------------------Initialised passthrough " + /*std::cout << "-------------------------------Initialised passthrough " "controller-------------------------------------------------" - << std::endl; + << std::endl;*/ return controller_interface::CallbackReturn::SUCCESS; } controller_interface::CallbackReturn PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& previous_state) +{ + start_action_server(); + trajectory_active_ = false; + /*std::cout << "-------------------------------Configured passthrough " + "controller-------------------------------------------------" + << std::endl;*/ + + 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()) + "/forward_joint_trajectory", @@ -64,12 +76,7 @@ PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& pre std::placeholders::_2), std::bind(&PassthroughTrajectoryController::goal_cancelled_callback, this, std::placeholders::_1), std::bind(&PassthroughTrajectoryController::goal_accepted_callback, this, std::placeholders::_1)); - - std::cout << "-------------------------------Configured passthrough " - "controller-------------------------------------------------" - << std::endl; - - return ControllerInterface::on_configure(previous_state); + return; } controller_interface::InterfaceConfiguration PassthroughTrajectoryController::state_interface_configuration() const @@ -80,11 +87,11 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st conf.names.push_back(passthrough_params_.speed_scaling_interface_name); - std::cout << conf.names[0] << " / " << conf.names.size() << std::endl; + // std::cout << conf.names[0] << " / " << conf.names.size() << std::endl; - std::cout << "------------------------------- Configured passthrough " + /*std::cout << "------------------------------- Configured passthrough " "controller state interface -------------------------------------------------" - << std::endl; + << std::endl;*/ return conf; } @@ -98,7 +105,11 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_present"); - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_number_of_points_"); + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_point_written"); + + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_number_of_points"); + + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_cancel"); for (size_t i = 0; i < 6; ++i) { config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_positions_" + @@ -115,34 +126,45 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_time_from_start"); - std::cout << "------------------------------- Configured passthrough " + /*std::cout << "------------------------------- Configured passthrough " "controller command interface -------------------------------------------------" - << std::endl; + << std::endl;*/ return config; } controller_interface::CallbackReturn PassthroughTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { - std::cout << "-------------------------------Activated passthrough " + /*std::cout << "-------------------------------Activated passthrough " "controller-------------------------------------------------" - << std::endl; + << std::endl;*/ return ControllerInterface::on_activate(state); } controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, - const rclcpp::Duration& /*period*/) + const rclcpp::Duration& period) { static int delay = 500; + // Debug message if (!delay) { std::cout << "-------------------------------Updated passthrough " "controller-------------------------------------------------" << std::endl; - - // std::cout << command_interfaces_.at(2).get_name() << " = " << command_interfaces_.at(2).get_value() << std::endl; - delay = 500; + delay--; + } + if (delay) + delay--; + if (trajectory_active_) { + scaling_factor_ = state_interfaces_[0].get_value(); + active_trajectory_elapsed_time_ += + static_cast(scaling_factor_ * ((period.seconds() * pow(10, 9)) + period.nanoseconds())); + + if (active_trajectory_elapsed_time_ > max_trajectory_time_) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory should be finished by now. You may want to cancel this goal, " + "if it is not."); + trajectory_active_ = false; + } } - delay--; return controller_interface::return_type::OK; } @@ -156,6 +178,14 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectories. Controller is not running."); return rclcpp_action::GoalResponse::REJECT; } + // Check dimensions of the trajectory + for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + if (goal->trajectory.points[i].positions.size() != 6) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have positions " + "for all joints of the robot. (6 joint positions per point)"); + return rclcpp_action::GoalResponse::REJECT; + } + } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -163,6 +193,12 @@ rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_ca const std::shared_ptr> goal_handle) { RCLCPP_INFO(get_node()->get_logger(), "Canceling active trajectory because cancel callback received."); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(0.0); + current_point_ = 0; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(1.0); + std::shared_ptr result = + std::make_shared(); + goal_handle->canceled(result); return rclcpp_action::CancelResponse::ACCEPT; } @@ -170,9 +206,78 @@ void PassthroughTrajectoryController::goal_accepted_callback( const std::shared_ptr> goal_handle) { RCLCPP_INFO(get_node()->get_logger(), "Accepted new trajectory."); + trajectory_active_ = true; + active_trajectory_elapsed_time_ = 0; + current_point_ = 0; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(0.0); + active_joint_traj_ = goal_handle->get_goal()->trajectory; + max_trajectory_time_ = (active_joint_traj_.points.back().time_from_start.sec * pow(10, 9)) + + active_joint_traj_.points.back().time_from_start.nanosec; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS].set_value( + active_joint_traj_.points.size()); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(1.0); + for (int i = 0; i < 6; i++) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( + active_joint_traj_.points[current_point_].positions[i]); + } + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( + active_joint_traj_.points[current_point_].time_from_start.sec + + (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); + command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); + current_point_++; + std::thread{ std::bind(&PassthroughTrajectoryController::execute, this, std::placeholders::_1), goal_handle } + .detach(); return; } +void PassthroughTrajectoryController::execute( + const std::shared_ptr> goal_handle) +{ + std::cout << "------------------Entered Execute loop--------------------------" << std::endl; + rclcpp::Rate loop_rate(500); + while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].get_value() == 1.0) { + if (command_interfaces_[PASSTHROUGH_TRAJECTORY_CANCEL].get_value() == 1.0) { + std::cout << "------------------Entered cancel--------------------------" << std::endl; + std::shared_ptr result = + std::make_shared(); + goal_handle->abort(result); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(0.0); + trajectory_active_ = false; + return; + } + if (goal_handle->is_canceling()) { + std::shared_ptr result = + std::make_shared(); + goal_handle->canceled(result); + } else { + // Write a new point to the command interface, if the previous point has been written to the hardware. + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && + current_point_ < active_joint_traj_.points.size()) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( + active_joint_traj_.points[current_point_].time_from_start.sec + + (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); + for (int i = 0; i < 6; i++) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( + active_joint_traj_.points[current_point_].positions[i]); + } + command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); + current_point_++; + } + } + // Check if all points have been written to the hardware + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && + current_point_ == active_joint_traj_.points.size() && + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "All points sent to the robot!"); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(0.0); + std::shared_ptr result = + std::make_shared(); + goal_handle->succeed(result); + } + loop_rate.sleep(); + } + return; +} } // namespace ur_controllers #include "pluginlib/class_list_macros.hpp" diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index c47a83452..8e87fd1f5 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -148,6 +148,7 @@ install(PROGRAMS scripts/wait_for_robot_description scripts/example_move.py scripts/start_ursim.sh + ../examples/examples.py DESTINATION lib/${PROJECT_NAME} ) 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 d7e5775ae..612d2c1bc 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -137,6 +137,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface void extractToolPose(); void transformForceTorque(); void check_passthrough_trajectory_controller(); + void trajectory_done_callback(urcl::control::TrajectoryResult result); urcl::vector6d_t urcl_position_commands_; urcl::vector6d_t urcl_position_commands_old_; @@ -194,8 +195,10 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double system_interface_initialized_; bool async_thread_shutdown_; double passthrough_trajectory_present_; + double passthrough_trajectory_cancel_; + double passthrough_point_written_; double passthrough_trajectory_number_of_points_; - std::array passthrough_trajectory_positions_ = { 1.0, 2.0, 3.0, 4.0, 5.0, 6.0 }; + std::array passthrough_trajectory_positions_; std::array passthrough_trajectory_velocities_; std::array passthrough_trajectory_accelerations_; double passthrough_trajectory_time_from_start_; @@ -238,6 +241,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_TRAJECTORY_CONTROLLER = "passthrough_controller/passthrough_trajectory_positions_"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/launch/ur5e.launch.py b/ur_robot_driver/launch/ur5e.launch.py index 5b98023a1..433f5e701 100644 --- a/ur_robot_driver/launch/ur5e.launch.py +++ b/ur_robot_driver/launch/ur5e.launch.py @@ -62,7 +62,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "initial_joint_controller", - default_value="passthrough_trajectory_controller", + default_value="scaled_joint_trajectory_controller", description="Initially loaded robot controller.", choices=[ "scaled_joint_trajectory_controller", diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index b10c9cf5f..22f84cd8d 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -155,6 +155,7 @@ def controller_spawner(controllers, active=True): ] + inactive_flags + controllers, + # ros_arguments=['--log-level', 'debug'] ) controllers_active = [ @@ -163,9 +164,8 @@ def controller_spawner(controllers, active=True): "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", "passthrough_trajectory_controller", - "scaled_joint_trajectory_controller", ] - controllers_inactive = ["forward_position_controller"] + controllers_inactive = ["forward_position_controller", "scaled_joint_trajectory_controller"] controller_spawners = [controller_spawner(controllers_active)] + [ controller_spawner(controllers_inactive, active=False) @@ -183,6 +183,7 @@ def controller_spawner(controllers, active=True): controller_spawner_timeout, ], condition=IfCondition(activate_joint_controller), + # ros_arguments=['--log-level', 'debug'] ) initial_joint_controller_spawner_stopped = Node( package="controller_manager", @@ -196,6 +197,7 @@ def controller_spawner(controllers, active=True): "--inactive", ], condition=UnlessCondition(activate_joint_controller), + # ros_arguments=['--log-level', 'debug'] ) rsp = IncludeLaunchDescription( @@ -329,7 +331,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "initial_joint_controller", - default_value="scaled_joint_trajectory_controller", + default_value="passthrough_trajectory_controller", description="Initially loaded robot controller.", ) ) diff --git a/ur_robot_driver/src/controller_stopper.cpp b/ur_robot_driver/src/controller_stopper.cpp index 0fc37267b..0ba15d881 100644 --- a/ur_robot_driver/src/controller_stopper.cpp +++ b/ur_robot_driver/src/controller_stopper.cpp @@ -88,6 +88,7 @@ ControllerStopper::ControllerStopper(const rclcpp::Node::SharedPtr& node, bool s auto it = std::find(consistent_controllers_.begin(), consistent_controllers_.end(), controller.name); if (it == consistent_controllers_.end()) { stopped_controllers_.push_back(controller.name); + std::cout << "Stopping controller with name: " << controller.name << std::endl; } } } diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 51dc34ab4..9a882842a 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -145,7 +145,6 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys return hardware_interface::CallbackReturn::ERROR; } } - return hardware_interface::CallbackReturn::SUCCESS; } @@ -303,10 +302,16 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "passthrough_controller", "passthrough_trajectory_present", &passthrough_trajectory_present_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "passthrough_controller", "passthrough_point_written", &passthrough_point_written_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller", "passthrough_trajectory_number_of_points", &passthrough_trajectory_number_of_points_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "passthrough_controller", "passthrough_trajectory_cancel", &passthrough_trajectory_cancel_)); + for (size_t i = 0; i < 6; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "passthrough_controller", "passthrough_trajectory_positions_" + std::to_string(i), @@ -489,6 +494,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; } @@ -665,7 +673,7 @@ 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_) { + } else if (passthrough_trajectory_controller_running_ && passthrough_trajectory_executing_ != 1.0) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } else { ur_driver_->writeKeepalive(); @@ -848,6 +856,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } + if (key == PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) { + start_modes_.push_back(PASSTHROUGH_TRAJECTORY_CONTROLLER); + } } } // set new mode to all interfaces at the same time @@ -870,6 +881,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); } + if (key == PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) { + stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH); + } } } // stop all interfaces at the same time @@ -895,19 +909,31 @@ 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; + std::cout << "---------------------Stopped passthrough controller---------------" << std::endl; } 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_TRAJECTORY_CONTROLLER) != start_modes_.end()) { + velocity_controller_running_ = false; + position_controller_running_ = false; + passthrough_trajectory_controller_running_ = true; + std::cout << "---------------------Started passthrough controller---------------" << std::endl; } start_modes_.clear(); @@ -918,19 +944,46 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod void URPositionHardwareInterface::check_passthrough_trajectory_controller() { - if (!std::isnan(passthrough_trajectory_present_)) { + static double last_time = 0.0; + if (passthrough_trajectory_present_ == 1.0) { if (!passthrough_trajectory_executing_) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, static_cast(passthrough_trajectory_number_of_points_)); passthrough_trajectory_executing_ = true; + std::cout << "Received points to passthrough: " << passthrough_trajectory_number_of_points_ << std::endl; } - if (passthrough_trajectory_executing_) { - ur_driver_->writeTrajectoryPoint(passthrough_trajectory_positions_, false, - passthrough_trajectory_time_from_start_); + if (passthrough_trajectory_executing_ && passthrough_point_written_ == 0.0) { + std::cout << "Writing point to robot with time parameter: " << passthrough_trajectory_time_from_start_ + << std::endl; + + bool status = ur_driver_->writeTrajectoryPoint(passthrough_trajectory_positions_, false, + passthrough_trajectory_time_from_start_ - last_time); + + std::cout << "Status of write: " << status << std::endl; + if (!status) { + std::cout << "Write failed, cancelling trajectory" << std::endl; + passthrough_trajectory_cancel_ = 1.0; + std::cout << "----------------------------Cancelling trajectory in hardware interface------------------" + << std::endl; + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); + } else { + last_time = passthrough_trajectory_time_from_start_; + passthrough_point_written_ = 1.0; + } } + } else { + passthrough_trajectory_executing_ = false; } } +void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result) +{ + std::cout << "-------------------Triggered trajectory callback!--------------------------" << std::endl; + std::cout << "Result is: " << int(result) << std::endl; + passthrough_trajectory_executing_ = false; + return; +} + } // namespace ur_robot_driver #include "pluginlib/class_list_macros.hpp" diff --git a/ur_robot_driver/test/__pycache__/robot_driver.cpython-310.pyc b/ur_robot_driver/test/__pycache__/robot_driver.cpython-310.pyc new file mode 100644 index 0000000000000000000000000000000000000000..d58deb25cb98c8636cf2ce4cccd549c7915ddaa7 GIT binary patch literal 3187 zcmai0-E$i?5I>#I=kwXlcLPn6HvIz3M+_~)$G|X@BxTZ}4wE81YiPnHhCvNVV#CWh@rWLqo~e1kD`;+cmgYsTsJZRgnwPv1a5Hw2vR5X!M@!{OeS|IiwWC5o`+4M$+oAty{ zwA^MX1J9UGdGv^JUos(m%0$ScE}r)6GDM(ms|t%BWcz*x?NYO1FYCn=)Z=JZ_JuP8 zVK+$ovf4h6V84%E9^QI+c)LF=%U!?A*>3c!GL)Sh_&x520LZ})`vqvL&;%k{8b<)u zr+8In^OW#;kktvF0LCu@IW_nKu)lt2>}UL;=t9s$V=EK48iI$7ofJJ5LF{j34>BqI zt5oacPW7R=Nl3;vjnCVbdm>aJFmnMPMPcmwxES*U-co5N2SS zc_u4RLQMpvXFnay#^N5}jRS09DZzd;~aFHvR-07~yfe`m$EF2(C!6h!4 zAcqTMo~;a(&p3%u*WpJr92uQ~o5uxomm1&@Y1S=%4dzu9unyUJ;G@la8AXoR>q(mJ zr~EA#j@wz2EZ}P}t4-Yj4PgOs3srKsG;Zcgx@RoKb=uO;po3WMMpOs@p&}neEOnn_>ky% zo#7RjS4$&%`AJ@GMs#YZbH$G`U+5Ausy*BZla?d&LA7u4E6f6vSc1j7W8vpFVe!w< z#1xP!!Ot`;V#4SCa~3B<*u6UXm<$lt5u8x{xIhwEATJ)4cQUuevQd2)DR9AsMSjC;5O!SyG?99+@x>HxGlR;Nfw8w$-+vttk3f*im0dK8_S zkf@SJp-jPQH5b5>^a!7iA<)EWiQk4Z`3FGMw9s$j$UH~btI`(xr3a~}D&MScxBT_H zcQ?0M%FG^~8;NCIU-L-6Fj9sin`W@5z`Ab{8$OFz&&`7K_u+@*!Ft_BuZ$!A6n4i- zc(n^xEBAQTYww+(YW#Ch?^>^p*f6@+FogTCR6lT2=*hpP={!ID=zlypf0t(6 zRgUkcLBf1rxxSxdv=?K)>ifU+g1A3Z39;xVOzvfrzk^o4hXgZ?e}d!=62#I7Ks8jc zaPuV~-$4^IKpX?|keDTxI7O%8l&g-Ouh&&6&f4uLZNnW4RS*Y4CyR1=@GxLN)I=Qg z(hx!l1Jtv-2iVPP^8ABd6oYtJal}d@+8m3$n(ar<2Z>lgNM>nXWiAb4I;dG%N$^KG z4qg3vt?25nAFoNsQjzg|FFXPhNzIPbRJ{StlwgMAJEbb{cVQqdqr~b4a!=Vow+l(H zN;KjaZM=`&NZ&z4qae(Z1dP>n5zA~|i?NvMe=%yIwQ+xaa|aUS$Mw~n*1F%?x^r)R zwY9nRld5iQezV!~?`>{0TieP?0{&1{MuMm+-2iF~gLFM)`d^50S`g9I04fvS+IiQh soG){je1*@u_!x+@=#c(rBG0QX(kW<;lS$K-5RQ~A5H4B?9(Iu&v^)$*+$3Ij0P(~F|G|0T0r40Y|d0>WIg2{YR*<>WjXC1Zq8NaWI5v>Y0g*XWjX5~ zZ62!}ljWTMMzdHcHjh`1H&0YfG*4DeHcwSf$-caQx_PE@2IYy$0xML`;$32sZ0dWh^e>?Q zEGv0u(DNp(F8eRAx7azH?IJtR%6MO5i);z+x7h{n?7G1&vP)0&%4MdnYNfZoAj+T9}_jo7@tB)E{ZDZI}(#7QMYNu7(xaoy8-e^aSpjEQO!JA&quX0cI zUFY>q(`!W{f9o@^)``52c~JAhP|Pm3YJP{s!$y`6(|5f(Huw0ghnVwuQ%!)UZoR?>5r??#AcP<7(}GwT?r2>y6JuVWeqw1K)&AGk3izd%PB`c_E!4d%Z^UZ}<(I74_sA zZ`AAZys5jvy&(E1U|uO93LkjU?V!p$hHt3zYEw+0B`&W<)yNZBGz_MQ!g3h*-tDwn zjaFS)G#^_eR=sHX&KD$=Uwj5ovxsQ%FM&5bH}HhF0eaew)-!tg3uDWQ^c{ogJ-uhX zFmR?qx__6OFs{%g7%pP;!k8vJ3ebkBXK+K6zR;(EY~ zp~oLKYF=^SbhuCie{2+^M$-#AQStP8ST2{}?;gGpbbMB91yPZE)!N2z%FAgrrO_Hp z3f7C=v&G_Sr$#KY-tqm%#ahs6d9}!6#V8m~#(qF*u7))u&#g)9i=-da>k?Cq)_Ncc zk8n~K$8_Ukiqx&oYM$cLgs@tmjBrB!&}~(laVdJ-_CzxN2w|&*KiBvfVDjZBi;n_+ zKWtZVy2ZOecyTf0wZ%_bjfW7Bs_#nTh3?%us~6pyoLUz<+~vdtE>ragT(d~Ptp&|y z&?>hdi%ECemRiPqcZ{|~J%j?%g+8C{aV<6lGZ zH^wPGHjy8}cXZ2NN9M5^lGwa}Y5XXGV*p}e57Io&i)bw+_;KpYs1+1p`xT-sKZS07 zngEehfd!gU7*at^UPfz3n<^q0dS177vzDdvb7-uY0R0~cpp8cg9YZ0~c61W7O`~T> zQPUx6#tVJRjI5mmh-6BUGI#73FohkRpO!O>o@^QRt!hz{#B$S6g1O_o(E7qzO00Cx z->CXN31eRn6~m5DhkFk?U^m(F@?f!e*QMo-41NVoH9UKM zeH9sAM6;)nV^T3Z)3aazp#Wh36U=OA4Q<_gs;%qHVu>fIiv3ixv`Uf`AEb0uDZAws zOw&4az#dto4M^`2TZA71^z>)Ct3B18>hTuNGkp{K_l0&-+onDfb#uphsy{U_!o(qH zM4~_9g-+fFwzgJkaI6MD+Q7kc08dCf+mJ#wyhqP{=V_R zSXG?GnYj(o;GOruD@e_NuTi&-_*>!1M*Db&22Ow&AwLDzrr~F(k{#`t4JnLqOSv0=$l=OiV@-y<+MamiE-!|#x4%jsj1VRSeJV4GRPNjU#wC$qX^ zSU+)gyL0=t91uY4H7SIMB%Tbd_6#7>wt--l1eaMq(tis~DdmG?#_1V-X6m4?ze%od(Y)w(MtcISy&P#35}f8QIW{iCz*Cm|*r37&2XZ za9)amvtJBI=12^FOa?)UK}LxImejB$w&}&}|GEBBUsRMQe)wD$DKh%fmX9$+msTWg zZfjFIxc8mcA}{RRgQpvY|3^0NAD6$6#rX#WBrDQ^I9Z#qgKSaL&k^_#pk@H{f3y^l z$;A^QC#BnKJ{j(!+1*kP)xvT<13gz*7>Od{a637ulDaMCGX5CG4S%9KpcucIWA zca_nAGp~mtM}C+)aFeVmuY;NIKccrf@j#%2^PXRA-ec9PPq9u&9;%^P@Lpzh>#OeN zpAM1nN^*-*QY69-%s14q(i}tF^K00K|15zU0K)PSJcwlI!KsdT$q{C=7V>XVSD~-W z2J-9lOZ?|)0kUjT^YJ?<5tq1-UrAy`p#>VS2Ji?^NPIV?rwv-bd8y6%Hc)sK7T*OouJb?1{c_dC~@*WA@x zcfY-Sm8J8c%(XXG|Ncu&L80Y zR&)%1WQDHb3C{uy?J49|SyKcP%tDZvkaDb5Y(`ep5fj&?7df<{WHb9eD$_$`l%@@4 z3@i?hR&x=v02&dTA=J&D1#J)IB2|OFf>>b29a~zNG;wr)`>1pH`flkilB*})cUtcO3MjdY1Grt(|o2doIaY2EswK4Et7< z`(dth&#ZbaMqXw;sQMI!DFlv&0^~>X^Egu}!#%u&^#R6?_>6R3zD_+8Ay#u~Ddj!# zCF;6BfS8Yyht|jF$J9WQDJ+`6FH?OlN=0h0-tVJVdhAnr!62_)`Gl6PpFHB628^9Zirenr5g9OvRX51J!>{YadRlt3n~!ZE2yGIWcBP8S$!wI; zqEPoDSM3qg<0v7SR2!E>dj)x<7tb%z0?J_#8A;C#m8&_04v~%{04zODvGj1uA7N-X z1K=RS#j`sF?*%FqN^}~v?%X)xhErEcN&a2T-_?j}E+kV{Q4)ic*1_G$tT0 zSi_ExEEA8 z8|piMFBtibfRJprbKnD2l=WsAOPZzdyWGj-qO zOHrB`R0fMt=!l+v$QZAfQ7gvL1;34*yKf*J7jTYDOHn2)iZDXnyWpa>O1^aMa}fWi zSjviwTiftz_hm39oObZY<1X@QxqG~N-vcZ8UN!WDMJx%c?R#EZ^-m?UJE%Dpz7SiqqcCL@?O%85y=87ti*t8wm&Vp5sk3I}N` z#7)V#CO@VB5v1D2K7te71)k6bfD1uf|ElATARXpzjPrSqR;Hy!mLKvr#W;V3NABTh z&P;ZKQ(Jd(k@ZCcHeFDAOc4qh2Ro-L>}@jsSsAz zz34F~zer5_$W+2lLyzyPtZP;_VYbYDW@>&AdTu*tciLi#n1i}KM$5|qxwwE$C}x`I zS9w1d+V-oD`LEDfP3hb5UqG4vBEUY8HPLttkJK{*kxeiJ-H7|)r$}L)8)mjYsHYv@A-X2>)f=eq)Hla{0B?T55A9lEBBEFf}vU(Od;r9)G2qoAGDygvkfkT-f zhMW*0932k%nv_E%8VcmUB5^WIy{Cs2WGyLInUHAV9c;@V0>t)GObkb=+*cN~A@nLV zW`^A%{|%Zv)P_=`4h)iN!;$}tanh=iPwPOUY~A?Pv|+sLmTwFWIgYaOD;8QQ>HK?i zSZkbne|PUuhKx%Q=s(i_7J#GWZB8xS1B=7sxlKhaF&WAWiV@85U&r8m>_8g$8XlQo z9k2u4e&7yZIame5b;1GdSHNF;t;`6{h6nvV7W@}lR2|f^Ikj{TFE#=fMp5Fw6ONos z*a%SEMj)mkK;#f9jTrO!o z;5xjnK(lxF#U(j%sbq>A^VX}#B)Si)e#a9THoOcH$=XJM>wyrMwX5wmK9iH!z+stU z>XL}L;7@r$w9iX8?8akUU*0keNB$SqkhZJ3TABUlzwAY9hV*yuk^IK#gfcnspk|dr&svzVY-+ELY2es`VAO6{#ckO zbn_4{n4u028_jaK@lr2#&GXG--#p27kQ;XuUJDsEv({w%H)%l|7bWdZ1bN7y%ISF`NGl#1mJ~n;v*BC#A$h&fW%gwO4kUG z@Jagf2`bSygVze5ni zHzz~OZvpJ_Du}prTFHb@P>7JF0TUXE332w+Tn>aY>qbnpNyXay z&u}3A=K$SlVsfR{hZO3jFT4Y~yi;B(UlNIR}q69GSRBAi^NcJljvzsP%^4njEceY%MeA@g=zf>~CC(+xdyQ6iR@$rK8k83DIHXw|d}R;+WBQ~&0f27H z#YR@iq(}xW*P|Qiyprozu&Xe`h%26zQcnWUNyqaPfxUc;g!JE1>t7R)Sj;McQFn^` z@6aytszQc_<9=eC#fw80mlu)lIDcOx{(c`TIi|{AA&&*#hDwpTRdGE*>p!iu{jbK9 zfe2fUNI<{*w~5cyb3DAb11d?-aJu}UZt2jejw{kwo0$DpCaq&Ob;^_gU7oZL|Ump~{LiI|SZ zAt^WbpMX%2kEB-2#tZNOXWcVKl8#i6n2E;_YLP7|_s>kaNMz*|S==V?GDS9w;27|S z>gy-vr|eOb5;FiQI6An$kFa(C^Ed@Q{2+lD0*3%Z2IB{~ou%eqCO|1;-X?ICz&Qd} z2)sk!9)Sl0o)I7?Mk!R%rAoCCWk=ZS3{@8Zko<)}B5`>YtfYlq0OyE Date: Thu, 11 Apr 2024 13:43:37 +0000 Subject: [PATCH 04/50] Controller will now send over all received points, before sending them to the robot. --- .../passthrough_trajectory_controller.hpp | 2 +- .../src/passthrough_trajectory_controller.cpp | 55 +++++++------- .../ur_robot_driver/hardware_interface.hpp | 4 +- ur_robot_driver/src/hardware_interface.cpp | 76 +++++++++++-------- 4 files changed, 76 insertions(+), 61 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 52acb9fae..78eddc92f 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -63,7 +63,7 @@ namespace ur_controllers { enum CommandInterfaces { - PASSTHROUGH_TRAJECTORY_PRESENT = 0, + PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0, PASSTHROUGH_POINT_WRITTEN = 1, PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS = 2, PASSTHROUGH_TRAJECTORY_CANCEL = 3, diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 375cf889f..049a11052 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -193,7 +193,7 @@ rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_ca const std::shared_ptr> goal_handle) { RCLCPP_INFO(get_node()->get_logger(), "Canceling active trajectory because cancel callback received."); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(0.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); current_point_ = 0; command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(1.0); std::shared_ptr result = @@ -215,16 +215,7 @@ void PassthroughTrajectoryController::goal_accepted_callback( active_joint_traj_.points.back().time_from_start.nanosec; command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS].set_value( active_joint_traj_.points.size()); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(1.0); - for (int i = 0; i < 6; i++) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( - active_joint_traj_.points[current_point_].positions[i]); - } - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( - active_joint_traj_.points[current_point_].time_from_start.sec + - (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); - command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); - current_point_++; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(1.0); std::thread{ std::bind(&PassthroughTrajectoryController::execute, this, std::placeholders::_1), goal_handle } .detach(); return; @@ -235,44 +226,50 @@ void PassthroughTrajectoryController::execute( { std::cout << "------------------Entered Execute loop--------------------------" << std::endl; rclcpp::Rate loop_rate(500); - while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].get_value() == 1.0) { + while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { if (command_interfaces_[PASSTHROUGH_TRAJECTORY_CANCEL].get_value() == 1.0) { std::cout << "------------------Entered cancel--------------------------" << std::endl; std::shared_ptr result = std::make_shared(); goal_handle->abort(result); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(0.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); trajectory_active_ = false; return; } + if (goal_handle->is_canceling()) { std::shared_ptr result = std::make_shared(); goal_handle->canceled(result); - } else { - // Write a new point to the command interface, if the previous point has been written to the hardware. - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && - current_point_ < active_joint_traj_.points.size()) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( - active_joint_traj_.points[current_point_].time_from_start.sec + - (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); - for (int i = 0; i < 6; i++) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( - active_joint_traj_.points[current_point_].positions[i]); - } - command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); - current_point_++; + return; + } + + // Write a new point to the command interface, if the previous point has been written to the hardware interface. + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && + current_point_ < active_joint_traj_.points.size()) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( + active_joint_traj_.points[current_point_].time_from_start.sec + + (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); + for (int i = 0; i < 6; i++) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( + active_joint_traj_.points[current_point_].positions[i]); } + command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); + current_point_++; } // Check if all points have been written to the hardware if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && current_point_ == active_joint_traj_.points.size() && - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "All points sent to the robot!"); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_PRESENT].set_value(0.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajecotry will now execute!"); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); + } + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) { std::shared_ptr result = std::make_shared(); goal_handle->succeed(result); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); + return; } loop_rate.sleep(); } 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 612d2c1bc..acb9cd4e7 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -194,7 +194,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool initialized_; double system_interface_initialized_; bool async_thread_shutdown_; - double passthrough_trajectory_present_; + double passthrough_trajectory_transfer_state_; double passthrough_trajectory_cancel_; double passthrough_point_written_; double passthrough_trajectory_number_of_points_; @@ -224,6 +224,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool non_blocking_read_; double robot_program_running_copy_; bool passthrough_trajectory_executing_; + std::vector> trajectory_joint_positions_; + std::vector trajectory_times_; PausingState pausing_state_; double pausing_ramp_up_increment_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 9a882842a..05e83b58b 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -299,8 +299,9 @@ 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_controller", "passthrough_trajectory_present", &passthrough_trajectory_present_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller", + "passthrough_trajectory_transfer_state", + &passthrough_trajectory_transfer_state_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "passthrough_controller", "passthrough_point_written", &passthrough_point_written_)); @@ -945,40 +946,55 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod void URPositionHardwareInterface::check_passthrough_trajectory_controller() { static double last_time = 0.0; - if (passthrough_trajectory_present_ == 1.0) { - if (!passthrough_trajectory_executing_) { - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - static_cast(passthrough_trajectory_number_of_points_)); - passthrough_trajectory_executing_ = true; - std::cout << "Received points to passthrough: " << passthrough_trajectory_number_of_points_ << std::endl; + if (passthrough_trajectory_transfer_state_ == 1.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_; + passthrough_point_written_ = 1.0; + } else if (passthrough_trajectory_transfer_state_ == 2.0) { + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + trajectory_joint_positions_.size()); + for (int i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectoryPoint(trajectory_joint_positions_[i], false, trajectory_times_[i]); } - if (passthrough_trajectory_executing_ && passthrough_point_written_ == 0.0) { - std::cout << "Writing point to robot with time parameter: " << passthrough_trajectory_time_from_start_ - << std::endl; - - bool status = ur_driver_->writeTrajectoryPoint(passthrough_trajectory_positions_, false, - passthrough_trajectory_time_from_start_ - last_time); - - std::cout << "Status of write: " << status << std::endl; - if (!status) { - std::cout << "Write failed, cancelling trajectory" << std::endl; - passthrough_trajectory_cancel_ = 1.0; - std::cout << "----------------------------Cancelling trajectory in hardware interface------------------" - << std::endl; - ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); - } else { - last_time = passthrough_trajectory_time_from_start_; - passthrough_point_written_ = 1.0; - } - } - } else { - passthrough_trajectory_executing_ = false; - } + passthrough_trajectory_transfer_state_ = 3.0; + } + + // if (passthrough_trajectory_transfer_state_ == 1.0) { + // if (!passthrough_trajectory_executing_) { + // ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, + // static_cast(passthrough_trajectory_number_of_points_)); + // passthrough_trajectory_executing_ = true; + // std::cout << "Received points to passthrough: " << passthrough_trajectory_number_of_points_ << std::endl; + // } + // if (passthrough_trajectory_executing_ && passthrough_point_written_ == 0.0) { + // std::cout << "Writing point to robot with time parameter: " << passthrough_trajectory_time_from_start_ + // << std::endl; + + // bool status = ur_driver_->writeTrajectoryPoint(passthrough_trajectory_positions_, false, + // passthrough_trajectory_time_from_start_ - last_time); + + // std::cout << "Status of write: " << status << std::endl; + // if (!status) { + // std::cout << "Write failed, cancelling trajectory" << std::endl; + // passthrough_trajectory_cancel_ = 1.0; + // std::cout << "----------------------------Cancelling trajectory in hardware interface------------------" + // << std::endl; + // ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); + // } else { + // last_time = passthrough_trajectory_time_from_start_; + // passthrough_point_written_ = 1.0; + // } + // } + // } else { + // passthrough_trajectory_executing_ = false; + // } } void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result) { std::cout << "-------------------Triggered trajectory callback!--------------------------" << std::endl; + passthrough_trajectory_transfer_state_ = 4.0; std::cout << "Result is: " << int(result) << std::endl; passthrough_trajectory_executing_ = false; return; From 95db48b97fb3f52c48669ab5c04abf06b36320ad Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 2 May 2024 11:52:05 +0000 Subject: [PATCH 05/50] Created more examples for the use of the controller. Controller now supports passing through accelerations and velocities. --- examples/example_movej.py | 252 +++++++++++++++++ examples/example_spline.py | 264 ++++++++++++++++++ examples/examples.py | 17 +- examples/load_switch_controller_example.py | 236 ++++++++++++++++ .../passthrough_trajectory_controller.hpp | 11 +- .../src/passthrough_trajectory_controller.cpp | 189 +++++++++---- ur_robot_driver/CMakeLists.txt | 3 + .../ur_robot_driver/hardware_interface.hpp | 3 + ur_robot_driver/launch/ur_control.launch.py | 7 +- ur_robot_driver/src/hardware_interface.cpp | 84 +++--- 10 files changed, 964 insertions(+), 102 deletions(-) create mode 100644 examples/example_movej.py create mode 100644 examples/example_spline.py create mode 100644 examples/load_switch_controller_example.py diff --git a/examples/example_movej.py b/examples/example_movej.py new file mode 100644 index 000000000..2376212e5 --- /dev/null +++ b/examples/example_movej.py @@ -0,0 +1,252 @@ +#!/usr/bin/env python3 +# Copyright 2024, Universal Robots +# +# 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 rclpy +from builtin_interfaces.msg import Duration +from control_msgs.action import JointTrajectory + +from rclpy.action import ActionClient +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint +from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import ( + UnloadController, + LoadController, + ConfigureController, + SwitchController, + ListControllers, +) +import time + +TIMEOUT_WAIT_SERVICE = 10 +TIMEOUT_WAIT_SERVICE_INITIAL = 60 +TIMEOUT_WAIT_ACTION = 10 + +ROBOT_JOINTS = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + + +# Helper functions +def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): + client = node.create_client(srv_type, srv_name) + if client.wait_for_service(timeout) is False: + raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") + + node.get_logger().info(f"Successfully connected to service '{srv_name}'") + return client + + +def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): + client = ActionClient(node, action_type, action_name) + if client.wait_for_server(timeout) is False: + raise Exception( + f"Could not reach action server '{action_name}' within timeout of {timeout}" + ) + + node.get_logger().info(f"Successfully connected to action '{action_name}'") + return client + + +class Robot: + def __init__(self, node): + self.node = node + self.init_robot() + + def init_robot(self): + service_interfaces = { + "/io_and_status_controller/set_io": SetIO, + "/controller_manager/load_controller": LoadController, + "/controller_manager/unload_controller": UnloadController, + "/controller_manager/configure_controller": ConfigureController, + "/controller_manager/switch_controller": SwitchController, + "/controller_manager/list_controllers": ListControllers, + } + self.service_clients = { + srv_name: waitForService(self.node, srv_name, srv_type) + for (srv_name, srv_type) in service_interfaces.items() + } + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + def set_io(self, pin, value): + """Test to set an IO.""" + set_io_req = SetIO.Request() + set_io_req.fun = 1 + set_io_req.pin = pin + set_io_req.state = value + + self.call_service("/io_and_status_controller/set_io", set_io_req) + + def send_trajectory(self, waypts, time_vec): + """Send robot trajectory.""" + if len(waypts) != len(time_vec): + raise Exception("waypoints vector and time vec should be same length") + + # Construct test trajectory + joint_trajectory = JTmsg() + joint_trajectory.joint_names = ROBOT_JOINTS + for i in range(len(waypts)): + point = JointTrajectoryPoint() + point.positions = waypts[i] + point.time_from_start = time_vec[i] + joint_trajectory.points.append(point) + + # Sending trajectory goal + goal_response = self.call_action( + self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + ) + if goal_response.accepted is False: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) + return result + + def call_service(self, srv_name, request): + future = self.service_clients[srv_name].call_async(request) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling service: {future.exception()}") + + def call_action(self, ac_client, g): + future = ac_client.send_goal_async(g) + rclpy.spin_until_future_complete(self.node, future) + + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling action: {future.exception()}") + + def get_result(self, ac_client, goal_response): + future_res = ac_client._get_result_async(goal_response) + rclpy.spin_until_future_complete(self.node, future_res) + if future_res.result() is not None: + return future_res.result().result + else: + raise Exception(f"Exception while calling action: {future_res.exception()}") + + def load_passthrough_controller(self): + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names = [] + # Find loaded controllers + for controller in list_response.controller: + names.append(controller.name) + # Check whether the passthrough controller is already loaded + try: + names.index("passthrough_trajectory_controller") + except ValueError: + print("Loading controller") + load_request = LoadController.Request() + load_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/load_controller", load_request) + configure_request = ConfigureController.Request() + configure_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/configure_controller", configure_request) + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names.clear() + # Update the list of controller names. + for controller in list_response.controller: + names.append(controller.name) + id = names.index("passthrough_trajectory_controller") + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + if list_response.controller[id].state == "inactive": + switch_request.activate_controllers = ["passthrough_trajectory_controller"] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + try: + id = names.index("scaled_joint_trajectory_controller") + if list_response.controller[id].state == "active": + switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] + except ValueError: + switch_request.deactivate_controllers = [] + finally: + switch_request.strictness = 1 # Best effort switching, will not terminate program if controller is already running + switch_request.activate_asap = False + switch_request.timeout = Duration(sec=2, nanosec=0) + self.call_service("/controller_manager/switch_controller", switch_request) + # Try unloading the scaled joint trajectory controller + try: + names.index("scaled_joint_trajectory_controller") + unload_request = UnloadController.Request() + unload_request.name = "scaled_joint_trajectory_controller" + self.call_service("/controller_manager/unload_controller", unload_request) + except ValueError: + print("scaled_joint_trajectory_controller not loaded, skipping unload") + # Connect to the passthrough controller action + finally: + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + def switch_controller(self, active, inactive): + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + switch_request.activate_controllers = [active] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + switch_request.deactivate_controllers = [inactive] + switch_request.strictness = ( + 1 # Best effort switching, will not terminate program if controller is already running + ) + switch_request.activate_asap = False + switch_request.timeout = Duration(sec=2, nanosec=0) + self.call_service("/controller_manager/switch_controller", switch_request) + + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + + # The following list are arbitrary joint positions, change according to your own needs + waypts = [ + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + ] + time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] + + # Execute trajectory on robot, make sure that the robot is booted and the control script is running + robot.send_trajectory(waypts, time_vec) diff --git a/examples/example_spline.py b/examples/example_spline.py new file mode 100644 index 000000000..059333da7 --- /dev/null +++ b/examples/example_spline.py @@ -0,0 +1,264 @@ +#!/usr/bin/env python3 +# Copyright 2024, Universal Robots +# +# 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 rclpy +from builtin_interfaces.msg import Duration +from control_msgs.action import JointTrajectory + +from rclpy.action import ActionClient +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint +from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import ( + UnloadController, + LoadController, + ConfigureController, + SwitchController, + ListControllers, +) +import time + +TIMEOUT_WAIT_SERVICE = 10 +TIMEOUT_WAIT_SERVICE_INITIAL = 60 +TIMEOUT_WAIT_ACTION = 10 + +ROBOT_JOINTS = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + + +# Helper functions +def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): + client = node.create_client(srv_type, srv_name) + if client.wait_for_service(timeout) is False: + raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") + + node.get_logger().info(f"Successfully connected to service '{srv_name}'") + return client + + +def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): + client = ActionClient(node, action_type, action_name) + if client.wait_for_server(timeout) is False: + raise Exception( + f"Could not reach action server '{action_name}' within timeout of {timeout}" + ) + + node.get_logger().info(f"Successfully connected to action '{action_name}'") + return client + + +class Robot: + def __init__(self, node): + self.node = node + self.init_robot() + + def init_robot(self): + service_interfaces = { + "/io_and_status_controller/set_io": SetIO, + "/controller_manager/load_controller": LoadController, + "/controller_manager/unload_controller": UnloadController, + "/controller_manager/configure_controller": ConfigureController, + "/controller_manager/switch_controller": SwitchController, + "/controller_manager/list_controllers": ListControllers, + } + self.service_clients = { + srv_name: waitForService(self.node, srv_name, srv_type) + for (srv_name, srv_type) in service_interfaces.items() + } + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + def set_io(self, pin, value): + """Test to set an IO.""" + set_io_req = SetIO.Request() + set_io_req.fun = 1 + set_io_req.pin = pin + set_io_req.state = value + + self.call_service("/io_and_status_controller/set_io", set_io_req) + + def send_trajectory(self, waypts, time_vec, vels, accels): + """Send robot trajectory.""" + if len(waypts) != len(time_vec): + raise Exception("waypoints vector and time vec should be same length") + + # Construct test trajectory + joint_trajectory = JTmsg() + joint_trajectory.joint_names = ROBOT_JOINTS + for i in range(len(waypts)): + point = JointTrajectoryPoint() + point.positions = waypts[i] + point.velocities = vels[i] + point.accelerations = accels[i] + point.time_from_start = time_vec[i] + joint_trajectory.points.append(point) + + # Sending trajectory goal + goal_response = self.call_action( + self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + ) + if goal_response.accepted is False: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) + return result + + def call_service(self, srv_name, request): + future = self.service_clients[srv_name].call_async(request) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling service: {future.exception()}") + + def call_action(self, ac_client, g): + future = ac_client.send_goal_async(g) + rclpy.spin_until_future_complete(self.node, future) + + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling action: {future.exception()}") + + def get_result(self, ac_client, goal_response): + future_res = ac_client._get_result_async(goal_response) + rclpy.spin_until_future_complete(self.node, future_res) + if future_res.result() is not None: + return future_res.result().result + else: + raise Exception(f"Exception while calling action: {future_res.exception()}") + + def load_passthrough_controller(self): + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names = [] + # Find loaded controllers + for controller in list_response.controller: + names.append(controller.name) + # Check whether the passthrough controller is already loaded + try: + names.index("passthrough_trajectory_controller") + except ValueError: + print("Loading controller") + load_request = LoadController.Request() + load_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/load_controller", load_request) + configure_request = ConfigureController.Request() + configure_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/configure_controller", configure_request) + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names.clear() + # Update the list of controller names. + for controller in list_response.controller: + names.append(controller.name) + id = names.index("passthrough_trajectory_controller") + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + if list_response.controller[id].state == "inactive": + switch_request.activate_controllers = ["passthrough_trajectory_controller"] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + try: + id = names.index("scaled_joint_trajectory_controller") + if list_response.controller[id].state == "active": + switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] + except ValueError: + switch_request.deactivate_controllers = [] + finally: + switch_request.strictness = 1 # Best effort switching, will not terminate program if controller is already running + switch_request.activate_asap = False + switch_request.timeout = Duration(sec=2, nanosec=0) + self.call_service("/controller_manager/switch_controller", switch_request) + # Try unloading the scaled joint trajectory controller + try: + names.index("scaled_joint_trajectory_controller") + unload_request = UnloadController.Request() + unload_request.name = "scaled_joint_trajectory_controller" + self.call_service("/controller_manager/unload_controller", unload_request) + except ValueError: + print("scaled_joint_trajectory_controller not loaded, skipping unload") + # Connect to the passthrough controller action + finally: + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + def switch_controller(self, active, inactive): + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + switch_request.activate_controllers = [active] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + switch_request.deactivate_controllers = [inactive] + switch_request.strictness = ( + 1 # Best effort switching, will not terminate program if controller is already running + ) + switch_request.activate_asap = False + switch_request.timeout = Duration(sec=2, nanosec=0) + self.call_service("/controller_manager/switch_controller", switch_request) + + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + + # The following list are arbitrary joint positions, change according to your own needs + waypts = [ + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + ] + vels = [ + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + ] + accels = [ + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + ] + time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] + + # Execute trajectory on robot, make sure that the robot is booted and the control script is running + robot.send_trajectory(waypts, time_vec, vels, accels) diff --git a/examples/examples.py b/examples/examples.py index e097f068d..79364c97a 100644 --- a/examples/examples.py +++ b/examples/examples.py @@ -215,6 +215,19 @@ def load_passthrough_controller(self): ) time.sleep(2) + def switch_controller(self, active, inactive): + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + switch_request.activate_controllers = [active] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + switch_request.deactivate_controllers = [inactive] + switch_request.strictness = ( + 1 # Best effort switching, will not terminate program if controller is already running + ) + switch_request.activate_asap = False + switch_request.timeout = Duration(sec=2, nanosec=0) + self.call_service("/controller_manager/switch_controller", switch_request) + if __name__ == "__main__": rclpy.init() @@ -224,9 +237,9 @@ def load_passthrough_controller(self): # The following list are arbitrary joint positions, change according to your own needs waypts = [ - [-0.47, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], - [-0.47, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], ] time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] diff --git a/examples/load_switch_controller_example.py b/examples/load_switch_controller_example.py new file mode 100644 index 000000000..c5a5a6ceb --- /dev/null +++ b/examples/load_switch_controller_example.py @@ -0,0 +1,236 @@ +#!/usr/bin/env python3 +# Copyright 2024, Universal Robots +# +# 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 rclpy +from builtin_interfaces.msg import Duration +from control_msgs.action import JointTrajectory + +from rclpy.action import ActionClient +from rclpy.node import Node +from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint +from ur_msgs.srv import SetIO +from controller_manager_msgs.srv import ( + UnloadController, + LoadController, + ConfigureController, + SwitchController, + ListControllers, +) +import time + +TIMEOUT_WAIT_SERVICE = 10 +TIMEOUT_WAIT_SERVICE_INITIAL = 60 +TIMEOUT_WAIT_ACTION = 10 + +ROBOT_JOINTS = [ + "shoulder_pan_joint", + "shoulder_lift_joint", + "elbow_joint", + "wrist_1_joint", + "wrist_2_joint", + "wrist_3_joint", +] + + +# Helper functions +def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): + client = node.create_client(srv_type, srv_name) + if client.wait_for_service(timeout) is False: + raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") + + node.get_logger().info(f"Successfully connected to service '{srv_name}'") + return client + + +def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): + client = ActionClient(node, action_type, action_name) + if client.wait_for_server(timeout) is False: + raise Exception( + f"Could not reach action server '{action_name}' within timeout of {timeout}" + ) + + node.get_logger().info(f"Successfully connected to action '{action_name}'") + return client + + +class Robot: + def __init__(self, node): + self.node = node + self.init_robot() + + def init_robot(self): + service_interfaces = { + "/io_and_status_controller/set_io": SetIO, + "/controller_manager/load_controller": LoadController, + "/controller_manager/unload_controller": UnloadController, + "/controller_manager/configure_controller": ConfigureController, + "/controller_manager/switch_controller": SwitchController, + "/controller_manager/list_controllers": ListControllers, + } + self.service_clients = { + srv_name: waitForService(self.node, srv_name, srv_type) + for (srv_name, srv_type) in service_interfaces.items() + } + + def set_io(self, pin, value): + """Test to set an IO.""" + set_io_req = SetIO.Request() + set_io_req.fun = 1 + set_io_req.pin = pin + set_io_req.state = value + + self.call_service("/io_and_status_controller/set_io", set_io_req) + + def send_trajectory(self, waypts, time_vec): + """Send robot trajectory.""" + if len(waypts) != len(time_vec): + raise Exception("waypoints vector and time vec should be same length") + + # Construct test trajectory + joint_trajectory = JTmsg() + joint_trajectory.joint_names = ROBOT_JOINTS + for i in range(len(waypts)): + point = JointTrajectoryPoint() + point.positions = waypts[i] + point.time_from_start = time_vec[i] + joint_trajectory.points.append(point) + + # Sending trajectory goal + goal_response = self.call_action( + self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + ) + if goal_response.accepted is False: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) + return result + + def call_service(self, srv_name, request): + future = self.service_clients[srv_name].call_async(request) + rclpy.spin_until_future_complete(self.node, future) + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling service: {future.exception()}") + + def call_action(self, ac_client, g): + future = ac_client.send_goal_async(g) + rclpy.spin_until_future_complete(self.node, future) + + if future.result() is not None: + return future.result() + else: + raise Exception(f"Exception while calling action: {future.exception()}") + + def get_result(self, ac_client, goal_response): + future_res = ac_client._get_result_async(goal_response) + rclpy.spin_until_future_complete(self.node, future_res) + if future_res.result() is not None: + return future_res.result().result + else: + raise Exception(f"Exception while calling action: {future_res.exception()}") + + def load_passthrough_controller(self): + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names = [] + # Find loaded controllers + for controller in list_response.controller: + names.append(controller.name) + # Check whether the passthrough controller is already loaded + try: + names.index("passthrough_trajectory_controller") + except ValueError: + print("Loading controller") + load_request = LoadController.Request() + load_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/load_controller", load_request) + configure_request = ConfigureController.Request() + configure_request.name = "passthrough_trajectory_controller" + self.call_service("/controller_manager/configure_controller", configure_request) + list_request = ListControllers.Request() + list_response = robot.call_service("/controller_manager/list_controllers", list_request) + names.clear() + # Update the list of controller names. + for controller in list_response.controller: + names.append(controller.name) + id = names.index("passthrough_trajectory_controller") + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + if list_response.controller[id].state == "inactive": + switch_request.activate_controllers = ["passthrough_trajectory_controller"] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + try: + id = names.index("scaled_joint_trajectory_controller") + if list_response.controller[id].state == "active": + switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] + except ValueError: + switch_request.deactivate_controllers = [] + finally: + switch_request.strictness = 1 # Best effort switching, will not terminate program if controller is already running + switch_request.activate_asap = False + switch_request.timeout = Duration(sec=2, nanosec=0) + self.call_service("/controller_manager/switch_controller", switch_request) + # Try unloading the scaled joint trajectory controller + try: + names.index("scaled_joint_trajectory_controller") + unload_request = UnloadController.Request() + unload_request.name = "scaled_joint_trajectory_controller" + self.call_service("/controller_manager/unload_controller", unload_request) + except ValueError: + print("scaled_joint_trajectory_controller not loaded, skipping unload") + # Connect to the passthrough controller action + finally: + self.jtc_action_client = waitForAction( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + JointTrajectory, + ) + time.sleep(2) + + def switch_controller(self, active, inactive): + switch_request = SwitchController.Request() + # Check if passthrough controller is inactive, and activate it if it is. + switch_request.activate_controllers = [active] + # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. + switch_request.deactivate_controllers = [inactive] + switch_request.strictness = ( + 1 # Best effort switching, will not terminate program if controller is already running + ) + switch_request.activate_asap = False + switch_request.timeout = Duration(sec=2, nanosec=0) + self.call_service("/controller_manager/switch_controller", switch_request) + + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + robot.load_passthrough_controller() diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 78eddc92f..c6cf9f269 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -67,10 +67,11 @@ enum CommandInterfaces PASSTHROUGH_POINT_WRITTEN = 1, PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS = 2, PASSTHROUGH_TRAJECTORY_CANCEL = 3, - PASSTHROUGH_TRAJECTORY_POSITIONS_ = 4, - PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 10, - PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 16, - PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 22 + PASSTHROUGH_TRAJECTORY_DIMENSIONS = 4, + PASSTHROUGH_TRAJECTORY_POSITIONS_ = 5, + PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 11, + PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 17, + PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 23 }; class PassthroughTrajectoryController : public controller_interface::ControllerInterface @@ -110,12 +111,14 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI void execute(const std::shared_ptr> goal_handle); + int check_dimensions(std::shared_ptr goal); trajectory_msgs::msg::JointTrajectory active_joint_traj_; uint32_t current_point_; bool trajectory_active_; uint64_t active_trajectory_elapsed_time_; uint64_t max_trajectory_time_; double scaling_factor_; + std::shared_ptr> current_handle; }; } // namespace ur_controllers #endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 049a11052..81935f287 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -48,10 +48,7 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() { passthrough_param_listener_ = std::make_shared(get_node()); passthrough_params_ = passthrough_param_listener_->get_params(); - - /*std::cout << "-------------------------------Initialised passthrough " - "controller-------------------------------------------------" - << std::endl;*/ + current_point_ = 0; return controller_interface::CallbackReturn::SUCCESS; } @@ -61,9 +58,6 @@ PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& pre { start_action_server(); trajectory_active_ = false; - /*std::cout << "-------------------------------Configured passthrough " - "controller-------------------------------------------------" - << std::endl;*/ return ControllerInterface::on_configure(previous_state); } @@ -87,12 +81,6 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st conf.names.push_back(passthrough_params_.speed_scaling_interface_name); - // std::cout << conf.names[0] << " / " << conf.names.size() << std::endl; - - /*std::cout << "------------------------------- Configured passthrough " - "controller state interface -------------------------------------------------" - << std::endl;*/ - return conf; } @@ -103,7 +91,7 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co const std::string tf_prefix = passthrough_params_.tf_prefix; - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_present"); + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_point_written"); @@ -111,6 +99,8 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_cancel"); + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_dimensions"); + for (size_t i = 0; i < 6; ++i) { config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_positions_" + std::to_string(i)); @@ -126,42 +116,35 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_time_from_start"); - /*std::cout << "------------------------------- Configured passthrough " - "controller command interface -------------------------------------------------" - << std::endl;*/ - return config; } controller_interface::CallbackReturn PassthroughTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { - /*std::cout << "-------------------------------Activated passthrough " - "controller-------------------------------------------------" - << std::endl;*/ return ControllerInterface::on_activate(state); } controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& period) { - static int delay = 500; - // Debug message - if (!delay) { - std::cout << "-------------------------------Updated passthrough " - "controller-------------------------------------------------" - << std::endl; - delay--; - } - if (delay) - delay--; - if (trajectory_active_) { + // static int delay = 1500; + // if (!delay) { + // std::cout << "Speed scaling : " << state_interfaces_[0].get_value() << std::endl; + // std::cout << "Trajectory executing : " << trajectory_active_ << std::endl; + // delay = 1500; + // } + + // if (delay) + // delay--; + + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 3.0) { scaling_factor_ = state_interfaces_[0].get_value(); active_trajectory_elapsed_time_ += static_cast(scaling_factor_ * ((period.seconds() * pow(10, 9)) + period.nanoseconds())); - if (active_trajectory_elapsed_time_ > max_trajectory_time_) { - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory should be finished by now. You may want to cancel this goal, " - "if it is not."); + if (active_trajectory_elapsed_time_ > max_trajectory_time_ && trajectory_active_) { + RCLCPP_WARN(get_node()->get_logger(), "Trajectory should be finished by now. You may want to cancel this goal, " + "if it is not."); trajectory_active_ = false; } } @@ -172,50 +155,118 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) { RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory to forward to robot"); - // Precondition: Running controller if (get_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 (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. A trajectory is already executing."); + return rclcpp_action::GoalResponse::REJECT; + } // Check dimensions of the trajectory + if (check_dimensions(goal) == 0) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, dimensions of trajectory are wrong."); + return rclcpp_action::GoalResponse::REJECT; + } else { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_DIMENSIONS].set_value( + static_cast(check_dimensions(goal))); + } + + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; +} +// This function should be told how many robot joints there are. Not just run with 6 or 0. +int PassthroughTrajectoryController::check_dimensions( + std::shared_ptr goal) +{ for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { + std::string msg; if (goal->trajectory.points[i].positions.size() != 6) { RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have positions " "for all joints of the robot. (6 joint positions per point)"); - return rclcpp_action::GoalResponse::REJECT; + 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(), msg.c_str()); + return 0; + } + if (goal->trajectory.points[i].velocities.size() != 0 && goal->trajectory.points[i].velocities.size() != 6) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have velocities " + "for all joints of the robot. (6 joint velocities per point)"); + 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(), msg.c_str()); + return 0; + } + if (goal->trajectory.points[i].accelerations.size() != 0 && goal->trajectory.points[i].accelerations.size() != 6) { + RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have " + "accelerations " + "for all joints of the robot. (6 joint accelerations per point)"); + msg = "Point nr " + std::to_string(i + 1) + + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; + RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); + return 0; } } - return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; + if (goal->trajectory.points[0].velocities.size() == 6 && goal->trajectory.points[0].accelerations.size() == 0) { + /*If there are only positions and velocities defined */ + return 2; + } else if (goal->trajectory.points[0].velocities.size() == 0 && + goal->trajectory.points[0].accelerations.size() == 6) { + /*If there are only positions and accelerations defined */ + return 3; + } else if (goal->trajectory.points[0].velocities.size() == 6 && + goal->trajectory.points[0].accelerations.size() == 6) { + /*If there are both positions, velocities and accelerations defined */ + return 4; + } else { + /*If there are only positions defined */ + return 1; + } } rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_callback( const std::shared_ptr> goal_handle) { - RCLCPP_INFO(get_node()->get_logger(), "Canceling active trajectory because cancel callback received."); + RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory because cancel callback received."); command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); current_point_ = 0; command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(1.0); std::shared_ptr result = std::make_shared(); goal_handle->canceled(result); + trajectory_active_ = false; return rclcpp_action::CancelResponse::ACCEPT; } void PassthroughTrajectoryController::goal_accepted_callback( const std::shared_ptr> goal_handle) { + current_handle = goal_handle; RCLCPP_INFO(get_node()->get_logger(), "Accepted new trajectory."); trajectory_active_ = true; active_trajectory_elapsed_time_ = 0; current_point_ = 0; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(0.0); + active_joint_traj_ = goal_handle->get_goal()->trajectory; + max_trajectory_time_ = (active_joint_traj_.points.back().time_from_start.sec * pow(10, 9)) + active_joint_traj_.points.back().time_from_start.nanosec; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS].set_value( active_joint_traj_.points.size()); + + std::cout << "Size of positions: " << active_joint_traj_.points[0].positions.size() << std::endl; + + std::cout << "Size of velocities: " << active_joint_traj_.points[0].velocities.size() << std::endl; + + std::cout << "Size of accelerations: " << active_joint_traj_.points[0].accelerations.size() << std::endl; + + command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(1.0); command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(1.0); + std::thread{ std::bind(&PassthroughTrajectoryController::execute, this, std::placeholders::_1), goal_handle } .detach(); return; @@ -224,11 +275,10 @@ void PassthroughTrajectoryController::goal_accepted_callback( void PassthroughTrajectoryController::execute( const std::shared_ptr> goal_handle) { - std::cout << "------------------Entered Execute loop--------------------------" << std::endl; - rclcpp::Rate loop_rate(500); + rclcpp::Rate loop_rate(200); while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { if (command_interfaces_[PASSTHROUGH_TRAJECTORY_CANCEL].get_value() == 1.0) { - std::cout << "------------------Entered cancel--------------------------" << std::endl; + RCLCPP_INFO(get_node()->get_logger(), "Trajectory cancelled from hardware interface, aborting action."); std::shared_ptr result = std::make_shared(); goal_handle->abort(result); @@ -244,31 +294,50 @@ void PassthroughTrajectoryController::execute( return; } - // Write a new point to the command interface, if the previous point has been written to the hardware interface. - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && - current_point_ < active_joint_traj_.points.size()) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( - active_joint_traj_.points[current_point_].time_from_start.sec + - (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); - for (int i = 0; i < 6; i++) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( - active_joint_traj_.points[current_point_].positions[i]); + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { + // Write a new point to the command interface, if the previous point has been written to the hardware interface. + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && + current_point_ < active_joint_traj_.points.size()) { + // Write the time_from_start parameter. + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( + active_joint_traj_.points[current_point_].time_from_start.sec + + (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); + // Write the positions for each joint of the robot + for (int i = 0; i < 6; i++) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( + active_joint_traj_.points[current_point_].positions[i]); + // Optionally, also write velocities and accelerations for each joint. + if (active_joint_traj_.points[current_point_].velocities.size() > 0) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value( + active_joint_traj_.points[current_point_].velocities[i]); + } + + if (active_joint_traj_.points[current_point_].accelerations.size() > 0) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value( + active_joint_traj_.points[current_point_].accelerations[i]); + } + } + // Tell hardware interface that this point is ready to be read. + command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); + current_point_++; + } + // Check if all points have been written to the hardware + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && + current_point_ == active_joint_traj_.points.size()) { + RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " + "execute!"); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); } - command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); - current_point_++; - } - // Check if all points have been written to the hardware - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && - current_point_ == active_joint_traj_.points.size() && - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajecotry will now execute!"); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); } + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) { std::shared_ptr result = std::make_shared(); goal_handle->succeed(result); + RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); + std::cout << "It took this long: " << active_trajectory_elapsed_time_ << std::endl; command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); + trajectory_active_ = false; return; } loop_rate.sleep(); diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 8e87fd1f5..c2e7f5036 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -149,6 +149,9 @@ install(PROGRAMS scripts/example_move.py scripts/start_ursim.sh ../examples/examples.py + ../examples/load_switch_controller_example.py + ../examples/example_movej.py + ../examples/example_spline.py DESTINATION lib/${PROJECT_NAME} ) 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 acb9cd4e7..f3497ca47 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -198,6 +198,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double passthrough_trajectory_cancel_; double passthrough_point_written_; double passthrough_trajectory_number_of_points_; + double passthrough_trajectory_dimensions_; std::array passthrough_trajectory_positions_; std::array passthrough_trajectory_velocities_; std::array passthrough_trajectory_accelerations_; @@ -225,6 +226,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface double robot_program_running_copy_; bool passthrough_trajectory_executing_; std::vector> trajectory_joint_positions_; + std::vector> trajectory_joint_velocities_; + std::vector> trajectory_joint_accelerations_; std::vector trajectory_times_; PausingState pausing_state_; diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 22f84cd8d..f02acbe20 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -163,9 +163,12 @@ def controller_spawner(controllers, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", + "scaled_joint_trajectory_controller", + ] + controllers_inactive = [ + "forward_position_controller", "passthrough_trajectory_controller", ] - controllers_inactive = ["forward_position_controller", "scaled_joint_trajectory_controller"] controller_spawners = [controller_spawner(controllers_active)] + [ controller_spawner(controllers_inactive, active=False) @@ -331,7 +334,7 @@ def generate_launch_description(): declared_arguments.append( DeclareLaunchArgument( "initial_joint_controller", - default_value="passthrough_trajectory_controller", + default_value="scaled_joint_trajectory_controller", description="Initially loaded robot controller.", ) ) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 05e83b58b..2b71e4b79 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -95,6 +95,10 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; passthrough_trajectory_executing_ = false; + passthrough_trajectory_transfer_state_ = 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) { @@ -313,6 +317,9 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "passthrough_controller", "passthrough_trajectory_cancel", &passthrough_trajectory_cancel_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "passthrough_controller", "passthrough_trajectory_dimensions", &passthrough_trajectory_dimensions_)); + for (size_t i = 0; i < 6; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "passthrough_controller", "passthrough_trajectory_positions_" + std::to_string(i), @@ -913,6 +920,10 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod } 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_cancel_ = 1; + trajectory_joint_positions_.clear(); + trajectory_joint_accelerations_.clear(); + trajectory_joint_velocities_.clear(); std::cout << "---------------------Stopped passthrough controller---------------" << std::endl; } @@ -946,55 +957,60 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod void URPositionHardwareInterface::check_passthrough_trajectory_controller() { static double last_time = 0.0; - if (passthrough_trajectory_transfer_state_ == 1.0) { + if (passthrough_trajectory_transfer_state_ == 1.0 && passthrough_point_written_ == 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_; passthrough_point_written_ = 1.0; + if (passthrough_trajectory_dimensions_ == 2.0 || passthrough_trajectory_dimensions_ == 4.0) { + trajectory_joint_velocities_.push_back(passthrough_trajectory_velocities_); + } + if (passthrough_trajectory_dimensions_ == 3.0 || passthrough_trajectory_dimensions_ == 4.0) { + trajectory_joint_accelerations_.push_back(passthrough_trajectory_accelerations_); + } } else if (passthrough_trajectory_transfer_state_ == 2.0) { + std::cout << " Passing through " + std::to_string(trajectory_joint_positions_.size()) + " Points to the robot." + << std::endl; + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, trajectory_joint_positions_.size()); - for (int i = 0; i < trajectory_joint_positions_.size(); i++) { - ur_driver_->writeTrajectoryPoint(trajectory_joint_positions_[i], false, trajectory_times_[i]); + if (passthrough_trajectory_dimensions_ == 1.0) { + for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { + ur_driver_->writeTrajectoryPoint(trajectory_joint_positions_[i], false, trajectory_times_[i]); + } + } else if (passthrough_trajectory_dimensions_ == 2.0) { + 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 (passthrough_trajectory_dimensions_ == 3.0) { + 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 (passthrough_trajectory_dimensions_ == 4.0) { + 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(); passthrough_trajectory_transfer_state_ = 3.0; } - - // if (passthrough_trajectory_transfer_state_ == 1.0) { - // if (!passthrough_trajectory_executing_) { - // ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_START, - // static_cast(passthrough_trajectory_number_of_points_)); - // passthrough_trajectory_executing_ = true; - // std::cout << "Received points to passthrough: " << passthrough_trajectory_number_of_points_ << std::endl; - // } - // if (passthrough_trajectory_executing_ && passthrough_point_written_ == 0.0) { - // std::cout << "Writing point to robot with time parameter: " << passthrough_trajectory_time_from_start_ - // << std::endl; - - // bool status = ur_driver_->writeTrajectoryPoint(passthrough_trajectory_positions_, false, - // passthrough_trajectory_time_from_start_ - last_time); - - // std::cout << "Status of write: " << status << std::endl; - // if (!status) { - // std::cout << "Write failed, cancelling trajectory" << std::endl; - // passthrough_trajectory_cancel_ = 1.0; - // std::cout << "----------------------------Cancelling trajectory in hardware interface------------------" - // << std::endl; - // ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_CANCEL); - // } else { - // last_time = passthrough_trajectory_time_from_start_; - // passthrough_point_written_ = 1.0; - // } - // } - // } else { - // passthrough_trajectory_executing_ = false; - // } } void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result) { std::cout << "-------------------Triggered trajectory callback!--------------------------" << std::endl; - passthrough_trajectory_transfer_state_ = 4.0; + if (result == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) { + passthrough_trajectory_transfer_state_ = 4.0; + std::cout << "-------------------Trajectory was successful!--------------------------" << std::endl; + } else { + passthrough_trajectory_cancel_ = 1.0; + } std::cout << "Result is: " << int(result) << std::endl; passthrough_trajectory_executing_ = false; return; From fab43166706d8c9582cc04c57f32a9b6dcb181f9 Mon Sep 17 00:00:00 2001 From: URJala Date: Thu, 2 May 2024 14:27:54 +0000 Subject: [PATCH 06/50] Finished the passthrough controller The number of joints in the controller is variable and received from the hardware interface. (It is not variable there though). The internal action server uses the action type FollowJointTrajectory. Both goal time tolerance and goal position tolerance are checked upon completion of trajectory, and feedback is sent to the user. The integration test has also been updated to test the controller. --- examples/example_movej.py | 24 +- examples/example_spline.py | 8 +- examples/examples.py | 6 +- examples/load_switch_controller_example.py | 6 +- .../passthrough_trajectory_controller.hpp | 93 +++++-- .../src/passthrough_trajectory_controller.cpp | 238 +++++++++++------- ur_robot_driver/config/ur_controllers.yaml | 25 ++ .../ur_robot_driver/hardware_interface.hpp | 13 +- ur_robot_driver/src/hardware_interface.cpp | 62 +++-- .../__pycache__/robot_driver.cpython-312.pyc | Bin 0 -> 17214 bytes .../__pycache__/test_common.cpython-312.pyc | Bin 0 -> 13206 bytes ur_robot_driver/test/robot_driver.py | 95 ++++++- ur_robot_driver/test/test_common.py | 51 ++-- 13 files changed, 434 insertions(+), 187 deletions(-) create mode 100644 ur_robot_driver/test/__pycache__/robot_driver.cpython-312.pyc create mode 100644 ur_robot_driver/test/__pycache__/test_common.cpython-312.pyc diff --git a/examples/example_movej.py b/examples/example_movej.py index 2376212e5..d6a8ee071 100644 --- a/examples/example_movej.py +++ b/examples/example_movej.py @@ -29,7 +29,8 @@ import rclpy from builtin_interfaces.msg import Duration -from control_msgs.action import JointTrajectory +from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance from rclpy.action import ActionClient from rclpy.node import Node @@ -100,7 +101,7 @@ def init_robot(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) @@ -127,14 +128,23 @@ def send_trajectory(self, waypts, time_vec): point.time_from_start = time_vec[i] joint_trajectory.points.append(point) + tolerances = [JointTolerance(position=0.001) for i in range(6)] + time_tolerance = Duration() + time_tolerance.sec = 1 # Sending trajectory goal goal_response = self.call_action( - self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + self.jtc_action_client, + FollowJointTrajectory.Goal( + trajectory=joint_trajectory, + goal_tolerance=tolerances, + goal_time_tolerance=time_tolerance, + ), ) if goal_response.accepted is False: raise Exception("trajectory was not accepted") # Verify execution + result = self.get_result(self.jtc_action_client, goal_response) return result @@ -217,7 +227,7 @@ def load_passthrough_controller(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) @@ -242,9 +252,9 @@ def switch_controller(self, active, inactive): # The following list are arbitrary joint positions, change according to your own needs waypts = [ - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], - [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-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)] diff --git a/examples/example_spline.py b/examples/example_spline.py index 059333da7..e00c4bb98 100644 --- a/examples/example_spline.py +++ b/examples/example_spline.py @@ -29,7 +29,7 @@ import rclpy from builtin_interfaces.msg import Duration -from control_msgs.action import JointTrajectory +from control_msgs.action import FollowJointTrajectory from rclpy.action import ActionClient from rclpy.node import Node @@ -100,7 +100,7 @@ def init_robot(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) @@ -131,7 +131,7 @@ def send_trajectory(self, waypts, time_vec, vels, accels): # Sending trajectory goal goal_response = self.call_action( - self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) ) if goal_response.accepted is False: raise Exception("trajectory was not accepted") @@ -219,7 +219,7 @@ def load_passthrough_controller(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) diff --git a/examples/examples.py b/examples/examples.py index 79364c97a..9d5f4a49d 100644 --- a/examples/examples.py +++ b/examples/examples.py @@ -29,7 +29,7 @@ import rclpy from builtin_interfaces.msg import Duration -from control_msgs.action import JointTrajectory +from control_msgs.action import FollowJointTrajectory from rclpy.action import ActionClient from rclpy.node import Node @@ -123,7 +123,7 @@ def send_trajectory(self, waypts, time_vec): # Sending trajectory goal goal_response = self.call_action( - self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) ) if goal_response.accepted is False: raise Exception("trajectory was not accepted") @@ -211,7 +211,7 @@ def load_passthrough_controller(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) diff --git a/examples/load_switch_controller_example.py b/examples/load_switch_controller_example.py index c5a5a6ceb..aa0fd8b52 100644 --- a/examples/load_switch_controller_example.py +++ b/examples/load_switch_controller_example.py @@ -29,7 +29,7 @@ import rclpy from builtin_interfaces.msg import Duration -from control_msgs.action import JointTrajectory +from control_msgs.action import FollowJointTrajectory from rclpy.action import ActionClient from rclpy.node import Node @@ -123,7 +123,7 @@ def send_trajectory(self, waypts, time_vec): # Sending trajectory goal goal_response = self.call_action( - self.jtc_action_client, JointTrajectory.Goal(trajectory=joint_trajectory) + self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) ) if goal_response.accepted is False: raise Exception("trajectory was not accepted") @@ -211,7 +211,7 @@ def load_passthrough_controller(self): self.jtc_action_client = waitForAction( self.node, "/passthrough_trajectory_controller/forward_joint_trajectory", - JointTrajectory, + FollowJointTrajectory, ) time.sleep(2) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index c6cf9f269..468201f11 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -51,6 +51,8 @@ #include "rclcpp_action/create_server.hpp" #include "rclcpp/rclcpp.hpp" #include "rclcpp_action/server_goal_handle.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp/duration.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" @@ -63,15 +65,36 @@ namespace ur_controllers { enum CommandInterfaces { + /* The PASSTHROUGH_TRAJECTORY_TRANSFER_STATE value is used to keep track of which stage the transfer is in. + 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 trajecotry. 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 robot driver. + 4.0: The robot is moving through the trajectory. + 5.0: The robot finished executing the trajectory. */ PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0, - PASSTHROUGH_POINT_WRITTEN = 1, - PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS = 2, - PASSTHROUGH_TRAJECTORY_CANCEL = 3, - PASSTHROUGH_TRAJECTORY_DIMENSIONS = 4, - PASSTHROUGH_TRAJECTORY_POSITIONS_ = 5, - PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 11, - PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 17, - PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 23 + /* The PASSTHROUGH_TRAJECTORY_CANCEL value is used to indicate whether the trajectory has been cancelled from the + * hardware interface.*/ + PASSTHROUGH_TRAJECTORY_CANCEL = 1, + /* The PASSTHROUGH_TRAJECTORY_DIMENSIONS is used to indicate what combination of joint positions, velocities and + * accelerations the trajectory consists of, see check_dimensions() for a description of what the values mean. */ + PASSTHROUGH_TRAJECTORY_DIMENSIONS = 2, + /* Arrays to hold the values of a point. */ + PASSTHROUGH_TRAJECTORY_POSITIONS_ = 3, + PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 9, + PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 15, + PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 21 +}; + +enum StateInterfaces +{ + /* State interface 0 - 17 are joint state interfaces*/ + + SPEED_SCALING_FACTOR = 18, + NUMBER_OF_JOINTS = 19, + CONTROLLER_RUNNING = 20 }; class PassthroughTrajectoryController : public controller_interface::ControllerInterface @@ -93,32 +116,64 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI controller_interface::return_type update(const rclcpp::Time& time, const rclcpp::Duration& period) override; private: + /* 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(); + std::shared_ptr passthrough_param_listener_; passthrough_trajectory_controller::Params passthrough_params_; - rclcpp_action::Server::SharedPtr send_trajectory_action_server_; + 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::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); + const std::shared_ptr> goal_handle); void goal_accepted_callback( - const std::shared_ptr> goal_handle); + const std::shared_ptr> goal_handle); + + void execute( + const std::shared_ptr> goal_handle); + + std::vector joint_names_; + std::vector state_interface_types_; + + std::vector> joint_position_state_interface_; + std::vector> joint_velocity_state_interface_; + + /* + If there are values in the velocities and accelerations vectors, they should have as many elements as there are + joints, and all be the same size. + The return value indicates what combination of joint positions, velocities and accelerations is present; + 0: The trajectory is invalid, and will be rejected. + 1: Only positions are defined for the trajectory. + 2: Positions and velocities are defined for the trajectory. + 3: Positions and accelerations are defined for the trajectory. + 4: Both positions, velocities and accelerations are defined for the trajectory. + */ + int check_dimensions(std::shared_ptr goal); - void - execute(const std::shared_ptr> goal_handle); - int check_dimensions(std::shared_ptr goal); trajectory_msgs::msg::JointTrajectory active_joint_traj_; + std::vector path_tolerance_; + std::vector goal_tolerance_; + rclcpp::Duration goal_time_tolerance_ = rclcpp::Duration::from_nanoseconds(0); + uint32_t current_point_; bool trajectory_active_; - uint64_t active_trajectory_elapsed_time_; - uint64_t max_trajectory_time_; + uint64_t period_ns; + uint64_t last_time_ns; + uint64_t now_ns; + double active_trajectory_elapsed_time_; + double max_trajectory_time_; double scaling_factor_; - std::shared_ptr> current_handle; + uint32_t number_of_joints_; + std::shared_ptr> active_goal_; }; } // namespace ur_controllers #endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 81935f287..092d6aa5e 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -49,7 +49,8 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() passthrough_param_listener_ = std::make_shared(get_node()); passthrough_params_ = passthrough_param_listener_->get_params(); current_point_ = 0; - + joint_names_ = auto_declare>("joints", joint_names_); + state_interface_types_ = auto_declare("state_interfaces", state_interface_types_); return controller_interface::CallbackReturn::SUCCESS; } @@ -64,7 +65,7 @@ PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& pre void PassthroughTrajectoryController::start_action_server(void) { - send_trajectory_action_server_ = rclcpp_action::create_server( + send_trajectory_action_server_ = rclcpp_action::create_server( get_node(), std::string(get_node()->get_name()) + "/forward_joint_trajectory", std::bind(&PassthroughTrajectoryController::goal_received_callback, this, std::placeholders::_1, std::placeholders::_2), @@ -76,10 +77,18 @@ void PassthroughTrajectoryController::start_action_server(void) controller_interface::InterfaceConfiguration PassthroughTrajectoryController::state_interface_configuration() const { controller_interface::InterfaceConfiguration conf; - conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; + const std::string tf_prefix = passthrough_params_.tf_prefix; - conf.names.push_back(passthrough_params_.speed_scaling_interface_name); + conf.names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto& joint_name : joint_names_) { + for (const auto& interface_type : state_interface_types_) { + conf.names.emplace_back(joint_name + "/" + interface_type); + } + } + conf.names.emplace_back(passthrough_params_.speed_scaling_interface_name); + conf.names.emplace_back(tf_prefix + "passthrough_controller/number_of_joints"); + conf.names.emplace_back(tf_prefix + "passthrough_controller/running"); return conf; } @@ -93,10 +102,6 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_point_written"); - - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_number_of_points"); - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_cancel"); config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_dimensions"); @@ -121,53 +126,73 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co controller_interface::CallbackReturn PassthroughTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { + number_of_joints_ = joint_names_.size(); + // clear out vectors in case of restart + joint_position_state_interface_.clear(); + joint_velocity_state_interface_.clear(); + + for (uint32_t i = 0; i < state_interface_types_.size() * joint_names_.size(); i++) { + if (state_interfaces_[i].get_interface_name() == "position") { + joint_position_state_interface_.emplace_back(state_interfaces_[i]); + } else if (state_interfaces_[i].get_interface_name() == "velocity") { + joint_velocity_state_interface_.emplace_back(state_interfaces_[i]); + } + } + return ControllerInterface::on_activate(state); } -controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, - const rclcpp::Duration& period) +controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& time, + const rclcpp::Duration& /* period */) { - // static int delay = 1500; - // if (!delay) { - // std::cout << "Speed scaling : " << state_interfaces_[0].get_value() << std::endl; - // std::cout << "Trajectory executing : " << trajectory_active_ << std::endl; - // delay = 1500; - // } - - // if (delay) - // delay--; - - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 3.0) { - scaling_factor_ = state_interfaces_[0].get_value(); - active_trajectory_elapsed_time_ += - static_cast(scaling_factor_ * ((period.seconds() * pow(10, 9)) + period.nanoseconds())); - - if (active_trajectory_elapsed_time_ > max_trajectory_time_ && trajectory_active_) { + static bool firstpass = true; + if (firstpass) { + now_ns = time.nanoseconds(); + firstpass = false; + } else { + last_time_ns = now_ns; + now_ns = time.nanoseconds(); + period_ns = now_ns - last_time_ns; + } + /* Keep track of how long the trajectory has been executing, if it takes too long, send a warning. */ + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) { + scaling_factor_ = state_interfaces_[StateInterfaces::SPEED_SCALING_FACTOR].get_value(); + active_trajectory_elapsed_time_ += static_cast(scaling_factor_ * (period_ns / pow(10, 9))); + if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds()) && + trajectory_active_) { RCLCPP_WARN(get_node()->get_logger(), "Trajectory should be finished by now. You may want to cancel this goal, " "if it is not."); trajectory_active_ = false; } } + return controller_interface::return_type::OK; } rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callback( - const rclcpp_action::GoalUUID& uuid, std::shared_ptr goal) + const rclcpp_action::GoalUUID& /*uuid*/, + std::shared_ptr goal) { - RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory to forward to robot"); + RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory."); // Precondition: Running controller if (get_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 (state_interfaces_[StateInterfaces::CONTROLLER_RUNNING].get_value() != 1.0) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, controller not running in hardware interface."); + return rclcpp_action::GoalResponse::REJECT; + } + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. A trajectory is already executing."); return rclcpp_action::GoalResponse::REJECT; } + // Check dimensions of the trajectory if (check_dimensions(goal) == 0) { - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, dimensions of trajectory are wrong."); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, dimensions of trajectory are incorrect."); return rclcpp_action::GoalResponse::REJECT; } else { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_DIMENSIONS].set_value( @@ -176,32 +201,39 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -// This function should be told how many robot joints there are. Not just run with 6 or 0. + int PassthroughTrajectoryController::check_dimensions( - std::shared_ptr goal) + std::shared_ptr goal) { for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { std::string msg; - if (goal->trajectory.points[i].positions.size() != 6) { - RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have positions " - "for all joints of the robot. (6 joint positions per point)"); + if (goal->trajectory.points[i].positions.size() != number_of_joints_) { + 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(), 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(), msg.c_str()); return 0; } - if (goal->trajectory.points[i].velocities.size() != 0 && goal->trajectory.points[i].velocities.size() != 6) { - RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have velocities " - "for all joints of the robot. (6 joint velocities per point)"); + if ((goal->trajectory.points[i].velocities.size() != 0 && + goal->trajectory.points[i].velocities.size() != number_of_joints_) || + goal->trajectory.points[i].velocities.size() != goal->trajectory.points[0].velocities.size()) { + 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(), 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(), msg.c_str()); return 0; } - if (goal->trajectory.points[i].accelerations.size() != 0 && goal->trajectory.points[i].accelerations.size() != 6) { - RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. All trajectory points must have " - "accelerations " - "for all joints of the robot. (6 joint accelerations per point)"); + if ((goal->trajectory.points[i].accelerations.size() != 0 && + goal->trajectory.points[i].accelerations.size() != number_of_joints_) || + goal->trajectory.points[i].accelerations.size() != goal->trajectory.points[0].accelerations.size()) { + 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(), msg.c_str()); msg = "Point nr " + std::to_string(i + 1) + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); @@ -225,24 +257,19 @@ int PassthroughTrajectoryController::check_dimensions( } } +// Called when the action is cancelled by the action client. rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_callback( - const std::shared_ptr> goal_handle) + const std::shared_ptr< + rclcpp_action::ServerGoalHandle> /* goal_handle */) { RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory because cancel callback received."); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); - current_point_ = 0; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(1.0); - std::shared_ptr result = - std::make_shared(); - goal_handle->canceled(result); - trajectory_active_ = false; return rclcpp_action::CancelResponse::ACCEPT; } +// Action goal was accepted, initialise values for a new trajectory. void PassthroughTrajectoryController::goal_accepted_callback( - const std::shared_ptr> goal_handle) + const std::shared_ptr> goal_handle) { - current_handle = goal_handle; RCLCPP_INFO(get_node()->get_logger(), "Accepted new trajectory."); trajectory_active_ = true; active_trajectory_elapsed_time_ = 0; @@ -251,53 +278,50 @@ void PassthroughTrajectoryController::goal_accepted_callback( command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(0.0); active_joint_traj_ = goal_handle->get_goal()->trajectory; + goal_tolerance_ = goal_handle->get_goal()->goal_tolerance; + path_tolerance_ = goal_handle->get_goal()->path_tolerance; + goal_time_tolerance_ = goal_handle->get_goal()->goal_time_tolerance; + active_goal_ = goal_handle; - max_trajectory_time_ = (active_joint_traj_.points.back().time_from_start.sec * pow(10, 9)) + - active_joint_traj_.points.back().time_from_start.nanosec; - - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_NUMBER_OF_POINTS].set_value( - active_joint_traj_.points.size()); + max_trajectory_time_ = active_joint_traj_.points.back().time_from_start.sec + + (active_joint_traj_.points.back().time_from_start.nanosec / pow(10, 9)); - std::cout << "Size of positions: " << active_joint_traj_.points[0].positions.size() << std::endl; - - std::cout << "Size of velocities: " << active_joint_traj_.points[0].velocities.size() << std::endl; - - std::cout << "Size of accelerations: " << active_joint_traj_.points[0].accelerations.size() << std::endl; - - command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(1.0); command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(1.0); + /* Start the executing thread of the action server, this will run until the trajectory is finished or cancelled. */ std::thread{ std::bind(&PassthroughTrajectoryController::execute, this, std::placeholders::_1), goal_handle } .detach(); return; } void PassthroughTrajectoryController::execute( - const std::shared_ptr> goal_handle) + const std::shared_ptr> goal_handle) { + /* While loop should execute 200 times pr second. Can be made slower if needed, but it will affect how fast the points + * of the trajectory are transferred to the hardware interface. */ rclcpp::Rate loop_rate(200); while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { + /* Check if the trajectory has been cancelled from the hardware interface. E.g. the robot was stopped on the teach + * pendant. */ if (command_interfaces_[PASSTHROUGH_TRAJECTORY_CANCEL].get_value() == 1.0) { RCLCPP_INFO(get_node()->get_logger(), "Trajectory cancelled from hardware interface, aborting action."); - std::shared_ptr result = - std::make_shared(); + std::shared_ptr result = + std::make_shared(); goal_handle->abort(result); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); - trajectory_active_ = false; + end_goal(); return; } - + /* Check if the goal has been cancelled by the ROS user. */ if (goal_handle->is_canceling()) { - std::shared_ptr result = - std::make_shared(); + std::shared_ptr result = + std::make_shared(); goal_handle->canceled(result); + end_goal(); return; } - + // Write a new point to the command interface, if the previous point has been read by the hardware interface. if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { - // Write a new point to the command interface, if the previous point has been written to the hardware interface. - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && - current_point_ < active_joint_traj_.points.size()) { + if (current_point_ < active_joint_traj_.points.size()) { // Write the time_from_start parameter. command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( active_joint_traj_.points[current_point_].time_from_start.sec + @@ -318,32 +342,64 @@ void PassthroughTrajectoryController::execute( } } // Tell hardware interface that this point is ready to be read. - command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].set_value(0.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); current_point_++; - } - // Check if all points have been written to the hardware - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_POINT_WRITTEN].get_value() == 1.0 && - current_point_ == active_joint_traj_.points.size()) { + + // Check if all points have been written to the hardware interface. + } else if (current_point_ == active_joint_traj_.points.size()) { RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " "execute!"); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(3.0); } - } - - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) { - std::shared_ptr result = - std::make_shared(); - goal_handle->succeed(result); - RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); - std::cout << "It took this long: " << active_trajectory_elapsed_time_ << std::endl; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); - trajectory_active_ = false; + // When the trajectory is finished, report the goal as successful to the client. + } else if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 5.0) { + std::shared_ptr result = + std::make_shared(); + // 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."; + goal_handle->abort(result); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); + // Check if the goal time tolerance was complied with. + } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds())) { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = "Goal not reached within time tolerance"; + goal_handle->abort(result); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal time tolerance not met."); + } else { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; + goal_handle->succeed(result); + RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); + } + end_goal(); return; } loop_rate.sleep(); } return; } + +bool PassthroughTrajectoryController::check_goal_tolerance() +{ + for (uint32_t i = 0; i < goal_tolerance_.size(); i++) { + if (std::abs(joint_position_state_interface_[i].get().get_value() - active_joint_traj_.points.back().positions[i]) > + goal_tolerance_[i].position) { + return false; + } + } + return true; +} + +void PassthroughTrajectoryController::end_goal() +{ + trajectory_active_ = false; + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); + active_goal_ = NULL; + current_point_ = 0; + goal_tolerance_.clear(); + path_tolerance_.clear(); +} } // namespace ur_controllers #include "pluginlib/class_list_macros.hpp" diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 3908d947e..40b3f5025 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -107,6 +107,31 @@ 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 + - effort + constraints: + stopped_velocity_tolerance: 0.2 + goal_time: 0.0 + $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } + $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } + 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 f3497ca47..f0be556a8 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -196,13 +196,14 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool async_thread_shutdown_; double passthrough_trajectory_transfer_state_; double passthrough_trajectory_cancel_; - double passthrough_point_written_; - double passthrough_trajectory_number_of_points_; double passthrough_trajectory_dimensions_; - std::array passthrough_trajectory_positions_; - std::array passthrough_trajectory_velocities_; - std::array passthrough_trajectory_accelerations_; + double passthrough_trajectory_controller_running_; + // TODO(URJala): The size of these arrays should be dependent on the number of joints on the physical robot. + urcl::vector6d_t passthrough_trajectory_positions_; + urcl::vector6d_t passthrough_trajectory_velocities_; + urcl::vector6d_t passthrough_trajectory_accelerations_; double passthrough_trajectory_time_from_start_; + double number_of_joints_; // payload stuff urcl::vector3d_t payload_center_of_gravity_; @@ -224,7 +225,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool robot_program_running_; bool non_blocking_read_; double robot_program_running_copy_; - bool passthrough_trajectory_executing_; std::vector> trajectory_joint_positions_; std::vector> trajectory_joint_velocities_; std::vector> trajectory_joint_accelerations_; @@ -238,7 +238,6 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface std::vector start_modes_; bool position_controller_running_; bool velocity_controller_running_; - bool passthrough_trajectory_controller_running_; std::unique_ptr ur_driver_; std::shared_ptr async_thread_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 2b71e4b79..3ff309955 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -94,11 +94,12 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys initialized_ = false; async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; - passthrough_trajectory_executing_ = false; passthrough_trajectory_transfer_state_ = 0.0; + passthrough_trajectory_cancel_ = 0.0; trajectory_joint_positions_.clear(); trajectory_joint_velocities_.clear(); trajectory_joint_accelerations_.clear(); + number_of_joints_ = info_.joints.size(); for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -236,6 +237,12 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); + state_interfaces.emplace_back( + hardware_interface::StateInterface(tf_prefix + "passthrough_controller", "number_of_joints", &number_of_joints_)); + + state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + "passthrough_controller", "running", + &passthrough_trajectory_controller_running_)); + return state_interfaces; } @@ -307,13 +314,6 @@ std::vector URPositionHardwareInterface::e "passthrough_trajectory_transfer_state", &passthrough_trajectory_transfer_state_)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "passthrough_controller", "passthrough_point_written", &passthrough_point_written_)); - - command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller", - "passthrough_trajectory_number_of_points", - &passthrough_trajectory_number_of_points_)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( tf_prefix + "passthrough_controller", "passthrough_trajectory_cancel", &passthrough_trajectory_cancel_)); @@ -681,7 +681,7 @@ 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_ && passthrough_trajectory_executing_ != 1.0) { + } else if (passthrough_trajectory_controller_running_) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } else { ur_driver_->writeKeepalive(); @@ -850,10 +850,9 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod const std::vector& start_interfaces, const std::vector& stop_interfaces) { hardware_interface::return_type ret_val = hardware_interface::return_type::OK; - start_modes_.clear(); stop_modes_.clear(); - + const std::string tf_prefix = info_.hardware_parameters.at("tf_prefix"); // Starting interfaces // add start interface per joint in tmp var for later check for (const auto& key : start_interfaces) { @@ -864,7 +863,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } - if (key == PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) { + if (key == tf_prefix + PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) { start_modes_.push_back(PASSTHROUGH_TRAJECTORY_CONTROLLER); } } @@ -919,33 +918,31 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod 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_cancel_ = 1; + passthrough_trajectory_controller_running_ = 0.0; + passthrough_trajectory_cancel_ = 1.0; trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); - std::cout << "---------------------Stopped passthrough controller---------------" << std::endl; } 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; + passthrough_trajectory_controller_running_ = 0.0; 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; + passthrough_trajectory_controller_running_ = 0.0; 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_TRAJECTORY_CONTROLLER) != start_modes_.end()) { velocity_controller_running_ = false; position_controller_running_ = false; - passthrough_trajectory_controller_running_ = true; - std::cout << "---------------------Started passthrough controller---------------" << std::endl; + passthrough_trajectory_controller_running_ = 1.0; } start_modes_.clear(); @@ -957,27 +954,32 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod void URPositionHardwareInterface::check_passthrough_trajectory_controller() { static double last_time = 0.0; - if (passthrough_trajectory_transfer_state_ == 1.0 && passthrough_point_written_ == 0.0) { + /* See passthrough_trajectory_controller.hpp for an explanation of the passthrough_trajectory_transfer_state_ values. + */ + if (passthrough_trajectory_transfer_state_ == 2.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_; - passthrough_point_written_ = 1.0; + if (passthrough_trajectory_dimensions_ == 2.0 || passthrough_trajectory_dimensions_ == 4.0) { trajectory_joint_velocities_.push_back(passthrough_trajectory_velocities_); } if (passthrough_trajectory_dimensions_ == 3.0 || passthrough_trajectory_dimensions_ == 4.0) { trajectory_joint_accelerations_.push_back(passthrough_trajectory_accelerations_); } - } else if (passthrough_trajectory_transfer_state_ == 2.0) { - std::cout << " Passing through " + std::to_string(trajectory_joint_positions_.size()) + " Points to the robot." - << std::endl; - + passthrough_trajectory_transfer_state_ = 1.0; + /* When all points have been read, write them to the robot driver.*/ + } else if (passthrough_trajectory_transfer_state_ == 3.0) { + /* Tell robot driver 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 according to the dimensions of the trajectory. See + * passthrough_trajectory_controller.hpp - check_dimensions() for an explanation of the values. */ if (passthrough_trajectory_dimensions_ == 1.0) { for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { - ur_driver_->writeTrajectoryPoint(trajectory_joint_positions_[i], false, trajectory_times_[i]); + ur_driver_->writeTrajectorySplinePoint(trajectory_joint_positions_[i], urcl::vector6d_t{ 0, 0, 0, 0, 0, 0 }, + trajectory_times_[i]); } } else if (passthrough_trajectory_dimensions_ == 2.0) { for (size_t i = 0; i < trajectory_joint_positions_.size(); i++) { @@ -998,21 +1000,17 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller() trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); - passthrough_trajectory_transfer_state_ = 3.0; + passthrough_trajectory_transfer_state_ = 4.0; } } void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result) { - std::cout << "-------------------Triggered trajectory callback!--------------------------" << std::endl; if (result == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) { - passthrough_trajectory_transfer_state_ = 4.0; - std::cout << "-------------------Trajectory was successful!--------------------------" << std::endl; + passthrough_trajectory_transfer_state_ = 5.0; } else { passthrough_trajectory_cancel_ = 1.0; } - std::cout << "Result is: " << int(result) << std::endl; - passthrough_trajectory_executing_ = false; return; } diff --git a/ur_robot_driver/test/__pycache__/robot_driver.cpython-312.pyc b/ur_robot_driver/test/__pycache__/robot_driver.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..3dbbf549a4db99353fcd9b4a62e432585dcf9af9 GIT binary patch literal 17214 zcmeG@dvFxzeY^MEyW4x{eR{!xc^wEzfO(kMMpzOc86<-Qwhz1KL%WjhxYNnst{_rQ zEl%88a!Ik1y5J<8khC)an{-I04`;?1+i|D0)5-J>2~J+ijorB8@}G=w65Q#ezu&jF zx2Iz<;5hYUns4rY`|bCB{Py?!zJGPQoeYGx?uqyRay!HP9AC6x%SGmY!ZOTBMqmUs z&VsODYjl4 zzW$7D)Zt>0MYEa~T0P1J%P!+fSCCb>wh@;WOCm9` zPfn6i$ndC1$+$=&LwzEo_$rykp>SXBSg^CN;;C* z*odSj1qQ?+7{4rr1rj|XlCUgFa##?hJ`x?q-YzeKH3`}jml!{kJQ|MSxRf$!FgX$z zpm;pmFXs|V^NGWeplVJQU--Hq_v(=Q!^1@EkA6{M6kB36JVL^KITWMY zgqwx``5FKx89ASqV$QJ|6811bQ`cFR_@PXYB|dnNIsg)u4t#u|>1dK1mWE;dq^5mI zX?>GK`kM9+;pC-AJiIS?C@D+fece6l!)KGofz`Lp7 z-71)$WPqkv0NMpBKu)j$bO?5UPJsjH5*z?|!3oeUxBz+t9-w#B9rP$(I&*c(y)Z-L zl|MxXrLTNU8?H8dcxs;?%g9zOS;+5CiV5dFI);%QT7Ha@TkTDK3MOq&StiBA+zWH@ zEmc5zMwy_wODQSb7mCc6ao>Y>#Y+0(uuqogP*f&%NK-6B@M$U*Ihqg^s}vW-Va18L z>TaK96?0!)QqhLgV+E%q%KL{~;}J<3t0`KpMm-Oym9!23?6r+?cuqJcoyYmvvZ}Oy z%~V;#g!wo(%kh}{%(~<58P0XWGwFFoy2#a%#ZYU(!sGH%lz$8&Cvh3zSsY^d1=Vwm zHX){QF1=X6W{omu*{-04tb|;J2ND28Y&48(M3!R`Ck>9sLh|SkSp%*AKh2x)^H zb!qC0mu5ls0j!Ll#4%$YTKEu^dbBkv=H!j9S&VT(C zFh?B#-|{#r^RLE}VW;2kRKBKR>Y^#ub7CsYnrPzD!KSsacg!xH6Mg{AeDcQ5Sv9zJ}FB*yy-Fr2K0_OJisbz<=d70jO~=&QeVDL%wyEXS@_(s*t$M2IeoSb1zvt`r5G@B=EFy2vuf4|3#AcqlHw29hJvWEG^4ASOEjT}1*S+K5OtVL~|( zC2>f|@$O-g93YW|VwXnxfb~n{E+|v5#lQiOxQ|s76EB*KTPw8yIL6Fye3o01;g)2% zwHa>hG}p9%nDZyRlipL7tpDze|L&}RZ^plO%HNaqAI$gj=4wV2V~23w8sT?9!Xl6_9bAz(AsX z@(?VxQXT=(^$3cYI_Z(l+Q~i86G1IRDUqn`T#!$2CW?KZ`1lC$BE@xIdrxn;eaDXO zeZ7h)dHA{w3{b&KMGkF%a$iA%W6i~Td$SrNFaoI!0Nr-nS@;yQd}D@h%Ej=+}{6-Z`^scY5WXIlGF47C=Id zBH>k+&EcH$Fb?-EAo8mF(<^u9(D}NSKEjr+N9Q~x`voBNQ#P=Suut12Z2c@~D%>~= z$OBU;4o0%8Y|%;y<`jGT6z#=a{c)3E88^#$#leWv2-cKYvmsEPmKV&lF#z`})8kr9 zu%*m`{T!#ECD=C{;}*F>%a4_7G5x7=Ji$2ziR0FkMM~USjg<8;h^yp)F2Ker^>($G z@d^vBbG$yoN!t_EleQRY$3-D!u7;Et>efYJ+yW!@-_}Uk;|E|gs5KRZltr-Xqq)i6 zxPODa@f7aOt&LsZ7xFu^FXXe^7mvO##+VdC^|UCY3Y2uPp?SydMB_m~P6j}53Pgqi z9o+zhK;NL)cQ|l#P?QHn5{Sxy!H5(%B#J{=uCZ%@(u-UWKe>67B%fnZD0-S5Wk?ge zo&|YOF(;$}#RiH2JSEiQ0>@Mh-Jzz1`wopY8k1X528E*W?syVZBkA78eEF|ID-udf zzdFXeTj8JFLFwDr9ZgUI#(%vEf*Ginh!VjOj8*iAvK$>62n?E@MCE52Yz(Xw6|ohE1Vaud!Op3a3ekb645l(VMs)=qhOQKsDuK5@J`#y5<(dJZ za7aNr*@}Zg8-U`@FCY$E;V1{ig>hh4ba5081$bN!vLC8QC@=+RXi@jurLtW*(6QQ2 z*zvDo^^<^Du)Sb*D6O2ZUA34T)pHzEUH@&z^Ny^4W5&O6mM@>-*Uj)1vwR8aV_CjA z!#B@6tkup5_neO@ubAN8=e#HQNj_cCaAt6t+f4O=h78w`|I~~!s(J{w@q+wmDc{X<$I3rIR46YX7EB_6L!@-*?p?*+a1q$ zoZoPf-#p{3NPCye)Gf`{ZOqhdJU^JJ+m@|s&(yU)(}c~)vdpNcxd) zS<`Ww1>VO^{U+UHDOhi@zU>C}?YCH;E37|Y21z8ZuPqq0B{yooZs4?R%7W4X)OFCY zTT*72ne~LN&rIi5$}o?>oX4%>HeDJ>*;3YX4sA`KZaLI7kK4wr=bT#Vxcw&PR+p~l z0sS}717mr!mbOV?g0+KmkyBg7IjC2n=V`GN7lTn32dx&Bx{P|u%TjvH6es89p8UKV z&?|~AMIq%dl4WG{1<(*fBVud?ZZ%*O&b>G+B8}97x=<4kuI1+-qm^_9cf&d+Yz!i`_V>?unSGWk+R)%F! zP@DrmlfeLk%%!HcV^4eceo)>YYH!`&+aB)Sx4pBywYPiU!$ibpoFXAf@b#dn8AW%+ zwok3;?AhPi+TPQ%WB=~6PL*|_;Ht1Om^6qP1$Ifrs=B5eYC78Va~vfp5mclU7mhPL z2+TY#DmHa&63$BHChx^Ou6{=hwZk>_V_T&F{dBrKcm)?o}_>U0S{4{Pq{_ys&yl zn%_BRx70c(yx=6NTt2&{?UKL!%m|S6{ZrMeFZkPmbT;1eORKru^Im!InTJnvSItbx z@-tt2qxzDsXWk6ySFN^6=Y;cWX`ThFp61rfRMuoGw`MB0W-Hq=m2K$KiLkJ*xqnUam!l9o(K%M4#NUAgk5<*@j)2hF#f)o=ii}MXvYSm9@<3cJ`VQY`>N~wRsZ^P{I%JIY+z2 z_VFj49XF+`##G#?)%&zmeAIOtG?5=W=G4ABN*whs3lstZ&e%q$USKdP1>l!R5;2hM z4@3e-BJrp|J@nM-7mx=dpmPk0^o^3Zk*F*Ngrq3ZWD*@1!~&3&Zk9@-%`!MQ|Jek}g7}S8QHeGL=#(Rg~D}hE+fe$5t9ruj>oG1#L{&6HRE$$()&~Y6pv|smeLwDikPdv(;-e)oU+ujn}TM%#)l2 z80I2K%Rk{SLgX;ZM=@a=wto9TDBS}Z8 zEE1w}4%D#~hU7yh(YbQJz;?@SO$K^++THX|{H1k!URw9J%Py?jljgfVd+M=yn%h!D zJ#JgnndMi^Tg_C|;dI6N=AzEJJqyJgmF~1H1G>`|M7q<~h;*lI!$P`)ay?aZOF=n& z%J=zGoπcUI9ATu=KZeBgNC!SR4fsgWxbMa!Zf=haZbo#F&f-jU!)!Qo8(R7T9e zNg#L^P$&b1H)f>J+Ryp4dgD$QX;smX;5FqmGS_RhC_Oalt=6a|UH#1qlrHGi$gt}& zQikQFTC1^TT1YtRmRd^wE3eUn`*P4Tg2y>%@X+n~kt=Zm?hKEs=|U6yix`nHX6L*A&3N6sJ7 zHSWcDUfS5f#N;{Q&6${hbWszNXWT2_rL~{(q`YIGE^8|*lph9336vB+H8pw1Jz9?T z1cS*ZaX?evh*D%Qp=yMA99jh9NyXR!RX@-qe3j4jB?*C(xbe|I)Vcra6`k?kx}$>hK<%K3 z^uj~&!I{C_IoX`k0F_+LYhjH^=&|UyHLe?0XAH*pusRtq0_hG7+SDrG$r9=dzXE5* z;-wkBPW5KEn^e0{&ToNVFhTz^6&7-{Wm?7-b)vy2pq2tB4Tu+FfF6+C>If{C#_AX3 zNBZGVS3&j%Un>8bo$rl3xh+_4s2r`t4j)B;Yg}YfBF8Z89t5Z(kuM|o3IengktY#Y z5d1L!rRst0y}QD_-MicOZSQJr4HKDy2&oMkG{s3oK$KY8qXwuzK2*jVSCP z@;ua#&}y1j?sAcwk7Kp5kY)%?MP_bslfzz_0&DH%SNX?Sd!i@aOEHqF)G7~J%%?}p>vcRh>J zOYgkkxpRS2Wtv|B3a)oCWKDVQe7CM4%~j`}OqCL;coA+im@6B$0D6{#Ysm-Bj?%=zsQE!&_LibCSrI&5YF1t6g?A~n<;%gASW-G~wz@qZne7hewJYGD zMT_1Q8&k6O+JAsuuTaUpHouRB^3qbk;=gco1a1G|=m2TA($TTO`Pw?ZV+;3VOI61` z{F`iTr`fZ>*NX;ND*w)aIsEfF{&qWOpx_FN<-06ED}@+0kFz@G7i{!c2-0^51?g}U zAC4qkpYAM7hqg>|-i0FMT{6I*%{e!b2b@1<8U5ol2>dy>#&Llg`z+u@g8aOZ53b1+ z^gC|Vk3ytODOMW~IFs^{XUYb>*-vmq4k!CKCs%8|rZ{l?c(vMjQzbWED9@r}#^5-K z;js`$3Xa2^E76YAP+1r#QgHxT*uZV0%^zf8!+g?vgOMyLZXcy>UB=bsH;yv$a)Yui zBZ*fAsZ|X|GH$Fv%hjJ+KcHjW-rkoQ?fnhrTCew~#Zv!&R^32z_|&Uz_{9nKlr@|` zRwb0doBp-@{yWf6F8?&r<@6l^95(v;4R39HcE_M8A1K#okdX^qr!=FQ3)q}@_X73H z>oi&_IE+w>=_0#f=JeDl*#ohlZD7}XV<0GBd*y?lb$|8bQ0afa^1+XOUS9v^fBxc? z56*1-=@+^(&%W}(AJ%UfXp3Ik_Q6kHzwi6efB(+5pzZSi{QB3wCTFp=7Z9KpO3oqp zK7h+qI7TL;IyFvWOhzz-;F|#8Qrur+oJ#*k)PyhL!>syA%c!4}`jkOVRxdXy9yL*W zA2`=~A@e9%e|Pl1fO(T{tX`v^<|aF^1M1)2iLqS>IuIbMhpSkTA-Jpv1}}!3Mu4h{ z>R{f2v116HK=5S*UqN6X{*ym}45eyk_x9aJI>0bidmKQJr-Hp=I~o}smL>2DqrdzJ zINaGN`2m(Xk6;pjV#PcO4rt}P#=1~1p>3#8L|0EB)l%ua@cgc>g^sN;6vvGVhXLQE z4X{3+-C^G=KE{2o!f#dhd%&%XCQWcFgX6j6^mdRr;jB|ZtQ6eW{7NWR?wsJ@+DRMR z!M;)Q`j$)8yI+cYtMPo>rJB3a)w?G=0A?4hIJ542WO~t-G*>%YQ-5mkjBTo>X~H{O zu?+mf%kRv2r~}XJKh>&wsHIoiFZnuE54G8f9_l%ssjAIZwqz<>vXz~g%FgtG$L1L3 zDBEd*$3aunl&6zF8gK?K+`8rU5PaVkCH)iS^r<>ce&AT(ryR*#?W||+I<{x^0?}7A#4^8cT_)>M~ zjqTrROt0T@sb*)oI)rl#QFHR=I7lJWEKKtv$np2FJK0wsf8Bqny7MI%=J{opYBr~< zJ8_hqGfVG)t$+)3Gb>ip$Q^T~pZD_SMxHxhaFdG@H$QKF?aD!xso4Xk2sk)JaP3MB zQ*#jXk#eTyepW)h`(t0(o+``hT-BaB=RbSO_AIr$=_uQ?($W?5DO{MIg$aihXE>Zl z3L|g?GOY07FrHP!Q!?=8>cfkj2{@pd6bPCe$$A86rX(#0IuSg8;4lKD7vyUQo!vG@u{GBH?3FFG)}||4)#%+;=X}W2&RW%@`LxU68iF0d zeHox>aOP7z%*l)QDWF05b~j>v?=YesrCuVZYzQ6wiX{;thZR@B+9|H#2;43c;l|ULNH2UVTrVtS zYXqGA6af|@#mC&;Di+>LrlCb3{fzV)fH^bEvh!OR*84W&c$;y)&A8rXygz3u-e%ly zGyFTuns=Cu6mEHkseOl8@iXS3cbLGtjQJ_<3GONH6W(V`C+*MLPkNvAp5~@}!D(jo zFHJVq^brF9tS5^CX{P3i#qKj5t<8 literal 0 HcmV?d00001 diff --git a/ur_robot_driver/test/__pycache__/test_common.cpython-312.pyc b/ur_robot_driver/test/__pycache__/test_common.cpython-312.pyc new file mode 100644 index 0000000000000000000000000000000000000000..ead048f623372b2c75aaba307ecb91c376a09032 GIT binary patch literal 13206 zcmdU0Yj7Lab>0OQ?>7m)K#>F~z95ODM7?6!k}Oj%N)%;_vMocF*AR9|g2IEo3s@!s z+H#^9ON~;=lcrMYq*YsIO2?kD(xem5B-1LH%v5>IWDzg}WGg3XqfW=aN{jL+_OG6^ z4=e~la@(2y=*m2}d+)iAz4x5&o^$T?FI+A=1rPu1;mOt^iuwb-XitNhSpGRpQI{!> z;^;UPR(|O)Ex#MW2KY6^jR_{qBursb!W=dyEMW_ce8#vnVGG;jG!wTc9AQVo8FnUI zVV9g|in|k@ut!dt`T;z>*RM!yguO%`x6b}2Kn6@Z%j0Wo8+`D-kfL&x5#OG zJdkJ&x5{Zpd{v??+$N`;@%BVVxFZn^2NRv)&O}$ZE3rDfS}t?NyAwU(9!R^xy__f9 z2fzKC_be3-aXz4HxH_O~4b(o0t3OL|{&x+^T!h!jc@2=)Sk4=e^O_*9nP11XZ~?wG zM)R%ezfOKN{A=Yq_#oefd0gu^UGJ@W*Q~ZUDA#U-+U-!ggE#SJ-ogdHY2-S2CPr6q z4#}Kdz_}VYySW~|Cq{F<*jokHdae(~zJcopI?RQDZsgYReUk=m?O8g!X^sx9`v|)p z4>6KwEGDE!Qpq$)#p65?B=5xOSUNhbXG!j{RD`Q|_dcDBugD3}l5Z@MNk*r4^Foxw zX4A1$GGvq*cJtAAgz!q)E;5x#@X55~-t!6{&7}D!Nh-<3VP=!W3NcwimZug7%Y5@;|L$&=8mS0@`JC6X{5rmzm8$ zoS;JH@$9;1Be670MQ}1jf&x!ojz#%kUysli1hJeBreg^{l}QJCCWXPl!M)j*kyIwm z1(T_CknoY{v|i;izfvWZELTcR2DAOa;6x^hayOZY$LE64R5Hm&(>xbUr}WCuA1tUV z3T>u&HY%^YWQnJyrsP$MB_~so_cV-@g<-PlFeTfbSE9Tk{3eM>f>cOWfxOHnBMCK? zo}1+*i~13gS!q}>!9)h}`RwVT(yhmNF#;UR%Uhn`NxUIx{S#93K(1op_` ziD7m(!Q7z?VF?N$%PH?^5ZWOmY&4Zfq>_WPbCQp(9xDk!pS$5tmr2UBoLP|0Kb~l@c70p+godo;*a3JF4Zlsv>EF`NpmT2?CKkE98JD zqziIGCbAj}9ZCy|xN0vbv7{G@NgohhR7pQp@#=!5R3>0SP=}E-2ILHN)75gN`>JE% zc)|5Zg-q?a{zdWV@kQo&S*Y6bmbNQX1xxQjPu?;FLRE4PESy?6A#NMLar#C~bRJta z8a(zT%3!nKwNfUhBAC0bKf1^qE}>wNHPBnM407;-0EX|&aoKXw1#3p#$DyqJAOO|^ z#+;!76zQ6vXwcwp-lzlOjBHVro0k(H&_)fA0q%@5u9fBZ_O6=s`linrui@eK|GWK6 zP-%}g3Iy?5bD#kG3<^@!qm6bY=!Z<>*@2NrJdPl)0k{Hm74lE`mok7+a?WQB!Qc@- z$^)EnfZ{?Xp57Mh5wdN22!T!D^mHuF2cuOj_I+mT+ZGJjbm1o5Fh0qSjBG@hOGYIV ziN*o_0>W%8$!3!2Se%{Aq%#B%J(ZY^^ROYCmHuQskZgPzzZqF2WSN&N>R=_aQd?l4 zBZO;32VAV&rTVF48x%edf5KORfC6YR`1tkSe;)erkT?-uWR90y^+i{E-i27|>O;k_ z6`Afl(|z^m!j?s5_?E@>#{BE^Uw%!rbltRiE;`RU#fEhYlkX?qNxYj{wC*T*8ZOSC zpa05hqP6|DyGH}VXai!_*vCe9F}V$`JZ95qRE#0OuB5aK&w@?@ioWfOrXuUa`MYZydRb1*Zs`c(Ll&v&zV%{R*-%+sqac<9Qw*#Q zRkQ`k6if37LAD*Ft{N3oM%7>vBy(n#L$kN_!MsAqEHMHfpH%iBkJhJZBPm-?H%v3> z0V1I7i>jw7pOs~?2Fqpx_w_NTm7zcg*MXd&mdsRB>pA9vW64eRtu6L$&-ZRG_KxIx zM+&`r?m5g|_K)q9Z}lbXmHs6gY8cZoTE893!UUlGBtx9d1XB^gC0_>1}V71JzSIjH?5boG+oxmhku2p z85R5w9hb(B-?p1N0allcG53AhO@M8FI?zgbZvyWDQlyg4l(1O z(L>Qn{6lvNEw$xKKq}X+Hcmorc{ZSe`sC;qNO3YXAeKFt$AFwOY79cFkH0wp{G1VG zMEe?mdgD03!G44WdWeyCIkFSd<%qJM!;U};k_%lcw(R*Z5i$i>aOyPvuG~C7 zvpyD&Bu;XX9ofwf)L$WU62b_Az_Npn#Z$1^3OgX7RKrbHI0)nn^-EXD-*9>VoBQFX z=`xabz|CVbn<^7M8{JSpgs?IF>cjf)Nu5S|kM{aD%`;RPJm{Gd!3?=WA?@)l2 zCLsgXFKluO$mjzhyYSN=N3t7;#Ka+rB3T3;y!uzga-JK)}Q2jBIj{ry(nQKVoZur;l?U7=?cW@<*^ltgiEx zytOlLU3+!!udQnVpvLIu>Egji{$QkVFnZ5u=(7KI^S*1xK6vHa=oRDn!&j#B&fW!U z-nky^Z@@Vt;+z#~9i@M@<7Ybx2aiLYz(eW;pw0z1nCz?j#J;V?zU}$G?S;Obg|5ec zy6J}XSMHy=3t**;JeTi!?hfU!x%1BUbNeq%msa&G7{76NVLHER%eh0ho%JOz{MTIS z-+YH+e085#tSQK zW7^5GFJ&TewPe-N(L;L zU8X1x0?&qMB3pi_BabU9^%$}uL3c>#1oFqT)UEEv#RD%a8+|7GQZKb;-3KRc4Bw*~ zOt7Z_k;PB069xA2qd~=T7i3jhY5+WFr!fFO6j*CUpyoLS!WW)hvh%51W|eqc4k8_u zo2#q(SVW zWselyOsV66O(^~bIJSF$Pch{OFg1t-z1b>9R$kIQO(4!IYiF8E^HNWy86yMlMs~j50IY zfkFl2yJ{Mo-iLpe0pgMmssG>YX9Bim*v_)O6MT|GCv_?niDUFn5x>e<4Z5LZ1B@@^ zAQ&f;n0>%o;c`wvCPCGfJOZ@FVpEFqNj728W+anHwgA!O@kz`;)R!2nKpuf)l|V|i za(mg9YOyRAeG4nT2?XTQ(|aym^mOJuokdS?-qX9lh@P$2%z4k&bIfhch1~u@y14VH z{LZJuiDwtB$4b72qOT|K>nZv+=Y5;6aRuL_=ghaQV9568t^EuB_gmj-y*9aM-45Zq zi`&m{|H@9$(RHo(ZhFF9m=oQ@0_arZO%-Qdizca0dbG*dSB3XPh%G4&hk zGq)TKMaQbVV^zV?E;`m!sMVU{@JN1mq%gdvI6Rgg9$R!CD%H0Y>$~&y-G%zzV*NnA zen4~%+^%aa)eT?URNS~Hzj2Q^y!U$a#+u@R7xD*QC>(fE9F2(HlVG~lyCHPyQVa_S z0+16heDVSk48}J~7M%j#E=8WJ<)a{N4(M*6()~%vi(Ibx5IiVki-jbHm z#xswbcjY~*GY=E6wnTF_MsOw#3TdZxzKdVxzN8DHf&~`i~&ow0}hK*guszqP(Jnwf?dyV16-L^PVutlb+DnQ zgCVEnV58G~^pqT)m8`R=(>!6}aM*Q{L{9MlE^$60@DhU{1)g4<=Vv9itmyJ_8n!Fh zm7_H|%^)QRoZ#+0Jw84MUBD^Va4o{=aJbM%!B*i`Gy@Ua@6uzGZEpWnj@dc-!i{Xg_biwBds5 zRzpj%p+DcyUual!&aU$Z7OmYSgqvXA5iB}-@{XSOT8g1<`OvmsJGR|K7}@askb*km zhTYfq-q=<=_+tLxi^YR{{vaA5AKj z{g71w`9|4yHO?6T?gRQAlS7dtT+tvp?4uNE52o>N`TIaFQ?>^{w+17%ys3Gx%pknC z8jlaat6;RI_1_$Q3U)eyzEBV4H7!lG9Ig}1QOxI9o4Q=n)P-Xp*D{zv=-Z2dBRG!I z@88(<7q~-!HwR4e+>;y(j$lAXbt5E?&Z(T)Iza$%4VATW=*0s0GpJYXV$_!pgLTgA z;7EBa)r&A#ga1FV-DiP-*0}3Rf%alxC?6Oq1U3`{kL3f8ojX`+58VOlqxF)#`2&|;x^QU8MS1E~m7^sy zT8J<6VA)kE-(NuI)q0065kd0r@F$?>ahAI4x0nKo%3(>ien|gnHlfHH+*$Go^7W^R zC6PR`%IMsZ4|Na}OV1)-gC?W!SAMiTd>OS~7kIdP1=o;9Qxl+08KErS={l274J{iG!)WwI%sgeBRQp;(FqV^~H~a@!fa?e7tLA1U~^mHaIw ze_zSpy<{`_>XwZbmwU-exx6K(ujH&>YI3^lO99H=TJkjA?eIFyOO)4SzJKz|^8a(_ zu5$7}XTr+{x~vQ3yKm8QYGA?z~k&z6FJYstT=-tIfXTr(8{9 zbN><^V*H2QbH3**eRo@2&1RAD-)*Oypt_v}>*}|^__y=omc74p{lX*m9?M(tG7?IU zL6sh|A39xel83@A>+cdIDn+P5e+>^8Cp~zzLX(-P`vnW`HE?EKV02FrHJme}wsW>` z+G+)NHDP(rUKTy0E?#;tN#jeS_ai@APR=-Gs1h`Xf+OS~fsyvIOK?xxu^iOhhBn<8^sZHVH4zeX}V2-$UP9-%{W zs3Hh=MYB>s-;`N6dOHom!>VB@saZROQ`*-#CD%0E_>DtM2#<+)sYz|IS}>(X1q1J^ zZ%a>t$&43@$&X=3A-e)~H55TI3TaYH8vq`Eg`b45qkNZBQ9I>cXH~Q+sH=s1?UTJv zCw>j>Jcc^)1|-f<5QOq{X~0>(R?&}}zD}`gW5KuS+O+uiGotrd5N%Wr29(1|we*yl z+DolnH`fe^gZm3>Moax;rMA_jzQI!OK&hqc<9d6*e$P)iyt3wW`uyfIV|RCe;w-jp zDYSjzM+bj+=*DB>qsQ`X#|n<;MCLgeZ9|)t0J?0?Zi2x{t@=u6mKd85ri4K_LMEt4 zgh7IV7=os_EL8--L~^SrrR)`gyqPOI49>mOsTOPNsapc9Am1PtWSo-xDj%GjmhX!T z${M0FOU-HnBn5+7!Gn7kL4=bURUTXkI93?chJ;WjIpwQpYMy*6Q*sJvu++%Q%4cGH zMrv4r!}1gWQDP1?%5Zx8*guAFNF$OaBp6nc9MD#I5?e4kg5*Uc86+VjLrAtE!9z+V zu7@6}Y(2~Nrh?bF3Cl1`P_C-%Z8*gTwSm*s9praV3PMK-TYxSZX_{XC0!4fNg>w9X za^5qTXamFuX~QQ@itdqLKbBv;6g@`&o;vV*YX67S;D=Pthg9%GYVAFP8F(oqZi;Td z)zl(V{!&MmNUbV0Hj7mK9kZ3LyVGa~60p-R(Dwo%8ctDx+&f0E#w3utV{{+=Xo+bk zxjS!J>n`mBz;84|45P8F)DkRpg-U_WQlPig)>&#^g(BZRSZe7nwXQ9Bo8Z5XTitH+ zCoX8>?orxD2jqrU-)b8uwT4P<10P!$Xv|J|IP^q!~s!43< vExEg|PKmC;Pi#Y-)XO>&3D$-(dMOfl*cCyjNUbN!~awxE6INW|2HWc literal 0 HcmV?d00001 diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 0805acbff..8182b0c03 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -37,10 +37,11 @@ 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 -from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint +from trajectory_msgs.msg import JointTrajectory as JointTrajectory, JointTrajectoryPoint from ur_msgs.msg import IOStates sys.path.append(os.path.dirname(__file__)) @@ -98,6 +99,11 @@ def init_robot(self): "/scaled_joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectory, ) + self._passthrough_forward_joint_trajectory = ActionInterface( + self.node, + "/passthrough_trajectory_controller/forward_joint_trajectory", + FollowJointTrajectory, + ) def setUp(self): self._dashboard_interface.start_robot() @@ -121,6 +127,14 @@ def test_start_passthrough_controller(self): 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 ) @@ -333,3 +347,82 @@ def js_cb(msg): # 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): + 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) for i in range(6)] + 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 + ], + ) + 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.000000000000000001) for i in range(6)] + 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) for i in range(6)] + goal_time_tolerance.sec = 0 + goal_time_tolerance.nanosec = 1000 + 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 960965aee..146a6dcbb 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -29,16 +29,20 @@ 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, ExecuteProcess, IncludeLaunchDescription, - # RegisterEventHandler, + RegisterEventHandler, ) - -# from launch.event_handlers import OnProcessExit +from launch.event_handlers import OnProcessExit from launch.launch_description_sources import PythonLaunchDescriptionSource from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.substitutions import FindPackagePrefix, FindPackageShare @@ -224,7 +228,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"): @@ -328,7 +336,7 @@ def generate_driver_test_description( ) ), launch_arguments={ - "robot_ip": "172.17.0.3", + "robot_ip": "192.168.56.101", "ur_type": ur_type, "launch_rviz": "false", "controller_spawner_timeout": str(controller_spawner_timeout), @@ -339,17 +347,20 @@ def generate_driver_test_description( "tf_prefix": tf_prefix, }.items(), ) - # wait_dashboard_server = ExecuteProcess( - # cmd=[ - # PathJoinSubstitution( - # [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] - # ) - # ], - # name="wait_dashboard_server", - # output="screen", - # ) - # driver_starter = RegisterEventHandler( - # OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) - # ) - - return LaunchDescription(_declare_launch_arguments() + [ReadyToTest(), robot_driver]) + wait_dashboard_server = ExecuteProcess( + cmd=[ + PathJoinSubstitution( + [FindPackagePrefix("ur_robot_driver"), "bin", "wait_dashboard_server.sh"] + ) + ], + name="wait_dashboard_server", + output="screen", + ) + driver_starter = RegisterEventHandler( + OnProcessExit(target_action=wait_dashboard_server, on_exit=robot_driver) + ) + + return LaunchDescription( + _declare_launch_arguments() + + [ReadyToTest(), wait_dashboard_server, _ursim_action(), driver_starter] + ) From 7042157975fd5d3998dc5f50a4dd50b599a4eb09 Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 15 Jul 2024 10:43:54 +0000 Subject: [PATCH 07/50] Added passthrough controller to "initially loaded controller" options. Did some cleanup of comments and print statements. Improved the examples, so they are easier to use. --- examples/example_movej.py | 262 ------------------ examples/examples.py | 247 ----------------- examples/load_switch_controller_example.py | 236 ---------------- examples/passthrough_example.py | 69 +++++ .../{example_spline.py => robot_class.py} | 200 +++++++------ examples/scaled_joint_example.py | 51 ++++ .../scaled_joint_trajectory_controller.cpp | 1 - ur_robot_driver/CMakeLists.txt | 7 +- .../ur_robot_driver/hardware_interface.hpp | 6 +- ur_robot_driver/launch/ur10.launch.py | 1 + ur_robot_driver/launch/ur10e.launch.py | 1 + ur_robot_driver/launch/ur16e.launch.py | 1 + ur_robot_driver/launch/ur20.launch.py | 1 + ur_robot_driver/launch/ur3.launch.py | 1 + ur_robot_driver/launch/ur30.launch.py | 1 + ur_robot_driver/launch/ur3e.launch.py | 1 + ur_robot_driver/launch/ur5.launch.py | 1 + ur_robot_driver/launch/ur_control.launch.py | 3 - ur_robot_driver/src/controller_stopper.cpp | 1 - .../__pycache__/robot_driver.cpython-310.pyc | Bin 3187 -> 0 bytes .../__pycache__/robot_driver.cpython-312.pyc | Bin 17214 -> 0 bytes .../__pycache__/test_common.cpython-310.pyc | Bin 9398 -> 0 bytes .../__pycache__/test_common.cpython-312.pyc | Bin 13206 -> 0 bytes ur_robot_driver/test/robot_driver.py | 2 +- 24 files changed, 252 insertions(+), 841 deletions(-) delete mode 100644 examples/example_movej.py delete mode 100644 examples/examples.py delete mode 100644 examples/load_switch_controller_example.py create mode 100644 examples/passthrough_example.py rename examples/{example_spline.py => robot_class.py} (56%) create mode 100644 examples/scaled_joint_example.py delete mode 100644 ur_robot_driver/test/__pycache__/robot_driver.cpython-310.pyc delete mode 100644 ur_robot_driver/test/__pycache__/robot_driver.cpython-312.pyc delete mode 100644 ur_robot_driver/test/__pycache__/test_common.cpython-310.pyc delete mode 100644 ur_robot_driver/test/__pycache__/test_common.cpython-312.pyc diff --git a/examples/example_movej.py b/examples/example_movej.py deleted file mode 100644 index d6a8ee071..000000000 --- a/examples/example_movej.py +++ /dev/null @@ -1,262 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2024, Universal Robots -# -# 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 rclpy -from builtin_interfaces.msg import Duration -from control_msgs.action import FollowJointTrajectory -from control_msgs.msg import JointTolerance - -from rclpy.action import ActionClient -from rclpy.node import Node -from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint -from ur_msgs.srv import SetIO -from controller_manager_msgs.srv import ( - UnloadController, - LoadController, - ConfigureController, - SwitchController, - ListControllers, -) -import time - -TIMEOUT_WAIT_SERVICE = 10 -TIMEOUT_WAIT_SERVICE_INITIAL = 60 -TIMEOUT_WAIT_ACTION = 10 - -ROBOT_JOINTS = [ - "shoulder_pan_joint", - "shoulder_lift_joint", - "elbow_joint", - "wrist_1_joint", - "wrist_2_joint", - "wrist_3_joint", -] - - -# Helper functions -def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): - client = node.create_client(srv_type, srv_name) - if client.wait_for_service(timeout) is False: - raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") - - node.get_logger().info(f"Successfully connected to service '{srv_name}'") - return client - - -def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): - client = ActionClient(node, action_type, action_name) - if client.wait_for_server(timeout) is False: - raise Exception( - f"Could not reach action server '{action_name}' within timeout of {timeout}" - ) - - node.get_logger().info(f"Successfully connected to action '{action_name}'") - return client - - -class Robot: - def __init__(self, node): - self.node = node - self.init_robot() - - def init_robot(self): - service_interfaces = { - "/io_and_status_controller/set_io": SetIO, - "/controller_manager/load_controller": LoadController, - "/controller_manager/unload_controller": UnloadController, - "/controller_manager/configure_controller": ConfigureController, - "/controller_manager/switch_controller": SwitchController, - "/controller_manager/list_controllers": ListControllers, - } - self.service_clients = { - srv_name: waitForService(self.node, srv_name, srv_type) - for (srv_name, srv_type) in service_interfaces.items() - } - self.jtc_action_client = waitForAction( - self.node, - "/passthrough_trajectory_controller/forward_joint_trajectory", - FollowJointTrajectory, - ) - time.sleep(2) - - def set_io(self, pin, value): - """Test to set an IO.""" - set_io_req = SetIO.Request() - set_io_req.fun = 1 - set_io_req.pin = pin - set_io_req.state = value - - self.call_service("/io_and_status_controller/set_io", set_io_req) - - def send_trajectory(self, waypts, time_vec): - """Send robot trajectory.""" - if len(waypts) != len(time_vec): - raise Exception("waypoints vector and time vec should be same length") - - # Construct test trajectory - joint_trajectory = JTmsg() - joint_trajectory.joint_names = ROBOT_JOINTS - for i in range(len(waypts)): - point = JointTrajectoryPoint() - point.positions = waypts[i] - point.time_from_start = time_vec[i] - joint_trajectory.points.append(point) - - tolerances = [JointTolerance(position=0.001) for i in range(6)] - time_tolerance = Duration() - time_tolerance.sec = 1 - # Sending trajectory goal - goal_response = self.call_action( - self.jtc_action_client, - FollowJointTrajectory.Goal( - trajectory=joint_trajectory, - goal_tolerance=tolerances, - goal_time_tolerance=time_tolerance, - ), - ) - if goal_response.accepted is False: - raise Exception("trajectory was not accepted") - - # Verify execution - - result = self.get_result(self.jtc_action_client, goal_response) - return result - - def call_service(self, srv_name, request): - future = self.service_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - def call_action(self, ac_client, g): - future = ac_client.send_goal_async(g) - rclpy.spin_until_future_complete(self.node, future) - - if future.result() is not None: - return future.result() - else: - raise Exception(f"Exception while calling action: {future.exception()}") - - def get_result(self, ac_client, goal_response): - future_res = ac_client._get_result_async(goal_response) - rclpy.spin_until_future_complete(self.node, future_res) - if future_res.result() is not None: - return future_res.result().result - else: - raise Exception(f"Exception while calling action: {future_res.exception()}") - - def load_passthrough_controller(self): - list_request = ListControllers.Request() - list_response = robot.call_service("/controller_manager/list_controllers", list_request) - names = [] - # Find loaded controllers - for controller in list_response.controller: - names.append(controller.name) - # Check whether the passthrough controller is already loaded - try: - names.index("passthrough_trajectory_controller") - except ValueError: - print("Loading controller") - load_request = LoadController.Request() - load_request.name = "passthrough_trajectory_controller" - self.call_service("/controller_manager/load_controller", load_request) - configure_request = ConfigureController.Request() - configure_request.name = "passthrough_trajectory_controller" - self.call_service("/controller_manager/configure_controller", configure_request) - list_request = ListControllers.Request() - list_response = robot.call_service("/controller_manager/list_controllers", list_request) - names.clear() - # Update the list of controller names. - for controller in list_response.controller: - names.append(controller.name) - id = names.index("passthrough_trajectory_controller") - switch_request = SwitchController.Request() - # Check if passthrough controller is inactive, and activate it if it is. - if list_response.controller[id].state == "inactive": - switch_request.activate_controllers = ["passthrough_trajectory_controller"] - # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. - try: - id = names.index("scaled_joint_trajectory_controller") - if list_response.controller[id].state == "active": - switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] - except ValueError: - switch_request.deactivate_controllers = [] - finally: - switch_request.strictness = 1 # Best effort switching, will not terminate program if controller is already running - switch_request.activate_asap = False - switch_request.timeout = Duration(sec=2, nanosec=0) - self.call_service("/controller_manager/switch_controller", switch_request) - # Try unloading the scaled joint trajectory controller - try: - names.index("scaled_joint_trajectory_controller") - unload_request = UnloadController.Request() - unload_request.name = "scaled_joint_trajectory_controller" - self.call_service("/controller_manager/unload_controller", unload_request) - except ValueError: - print("scaled_joint_trajectory_controller not loaded, skipping unload") - # Connect to the passthrough controller action - finally: - self.jtc_action_client = waitForAction( - self.node, - "/passthrough_trajectory_controller/forward_joint_trajectory", - FollowJointTrajectory, - ) - time.sleep(2) - - def switch_controller(self, active, inactive): - switch_request = SwitchController.Request() - # Check if passthrough controller is inactive, and activate it if it is. - switch_request.activate_controllers = [active] - # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. - switch_request.deactivate_controllers = [inactive] - switch_request.strictness = ( - 1 # Best effort switching, will not terminate program if controller is already running - ) - switch_request.activate_asap = False - switch_request.timeout = Duration(sec=2, nanosec=0) - self.call_service("/controller_manager/switch_controller", switch_request) - - -if __name__ == "__main__": - rclpy.init() - node = Node("robot_driver_test") - robot = Robot(node) - - # The following list are arbitrary joint positions, change according to your own needs - 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)] - - # Execute trajectory on robot, make sure that the robot is booted and the control script is running - robot.send_trajectory(waypts, time_vec) diff --git a/examples/examples.py b/examples/examples.py deleted file mode 100644 index 9d5f4a49d..000000000 --- a/examples/examples.py +++ /dev/null @@ -1,247 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2024, Universal Robots -# -# 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 rclpy -from builtin_interfaces.msg import Duration -from control_msgs.action import FollowJointTrajectory - -from rclpy.action import ActionClient -from rclpy.node import Node -from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint -from ur_msgs.srv import SetIO -from controller_manager_msgs.srv import ( - UnloadController, - LoadController, - ConfigureController, - SwitchController, - ListControllers, -) -import time - -TIMEOUT_WAIT_SERVICE = 10 -TIMEOUT_WAIT_SERVICE_INITIAL = 60 -TIMEOUT_WAIT_ACTION = 10 - -ROBOT_JOINTS = [ - "shoulder_pan_joint", - "shoulder_lift_joint", - "elbow_joint", - "wrist_1_joint", - "wrist_2_joint", - "wrist_3_joint", -] - - -# Helper functions -def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): - client = node.create_client(srv_type, srv_name) - if client.wait_for_service(timeout) is False: - raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") - - node.get_logger().info(f"Successfully connected to service '{srv_name}'") - return client - - -def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): - client = ActionClient(node, action_type, action_name) - if client.wait_for_server(timeout) is False: - raise Exception( - f"Could not reach action server '{action_name}' within timeout of {timeout}" - ) - - node.get_logger().info(f"Successfully connected to action '{action_name}'") - return client - - -class Robot: - def __init__(self, node): - self.node = node - self.init_robot() - - def init_robot(self): - service_interfaces = { - "/io_and_status_controller/set_io": SetIO, - "/controller_manager/load_controller": LoadController, - "/controller_manager/unload_controller": UnloadController, - "/controller_manager/configure_controller": ConfigureController, - "/controller_manager/switch_controller": SwitchController, - "/controller_manager/list_controllers": ListControllers, - } - self.service_clients = { - srv_name: waitForService(self.node, srv_name, srv_type) - for (srv_name, srv_type) in service_interfaces.items() - } - - def set_io(self, pin, value): - """Test to set an IO.""" - set_io_req = SetIO.Request() - set_io_req.fun = 1 - set_io_req.pin = pin - set_io_req.state = value - - self.call_service("/io_and_status_controller/set_io", set_io_req) - - def send_trajectory(self, waypts, time_vec): - """Send robot trajectory.""" - if len(waypts) != len(time_vec): - raise Exception("waypoints vector and time vec should be same length") - - # Construct test trajectory - joint_trajectory = JTmsg() - joint_trajectory.joint_names = ROBOT_JOINTS - for i in range(len(waypts)): - point = JointTrajectoryPoint() - point.positions = waypts[i] - point.time_from_start = time_vec[i] - joint_trajectory.points.append(point) - - # Sending trajectory goal - goal_response = self.call_action( - self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) - ) - if goal_response.accepted is False: - raise Exception("trajectory was not accepted") - - # Verify execution - result = self.get_result(self.jtc_action_client, goal_response) - return result - - def call_service(self, srv_name, request): - future = self.service_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - def call_action(self, ac_client, g): - future = ac_client.send_goal_async(g) - rclpy.spin_until_future_complete(self.node, future) - - if future.result() is not None: - return future.result() - else: - raise Exception(f"Exception while calling action: {future.exception()}") - - def get_result(self, ac_client, goal_response): - future_res = ac_client._get_result_async(goal_response) - rclpy.spin_until_future_complete(self.node, future_res) - if future_res.result() is not None: - return future_res.result().result - else: - raise Exception(f"Exception while calling action: {future_res.exception()}") - - def load_passthrough_controller(self): - list_request = ListControllers.Request() - list_response = robot.call_service("/controller_manager/list_controllers", list_request) - names = [] - # Find loaded controllers - for controller in list_response.controller: - names.append(controller.name) - # Check whether the passthrough controller is already loaded - try: - names.index("passthrough_trajectory_controller") - except ValueError: - print("Loading controller") - load_request = LoadController.Request() - load_request.name = "passthrough_trajectory_controller" - self.call_service("/controller_manager/load_controller", load_request) - configure_request = ConfigureController.Request() - configure_request.name = "passthrough_trajectory_controller" - self.call_service("/controller_manager/configure_controller", configure_request) - list_request = ListControllers.Request() - list_response = robot.call_service("/controller_manager/list_controllers", list_request) - names.clear() - # Update the list of controller names. - for controller in list_response.controller: - names.append(controller.name) - id = names.index("passthrough_trajectory_controller") - switch_request = SwitchController.Request() - # Check if passthrough controller is inactive, and activate it if it is. - if list_response.controller[id].state == "inactive": - switch_request.activate_controllers = ["passthrough_trajectory_controller"] - # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. - try: - id = names.index("scaled_joint_trajectory_controller") - if list_response.controller[id].state == "active": - switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] - except ValueError: - switch_request.deactivate_controllers = [] - finally: - switch_request.strictness = 1 # Best effort switching, will not terminate program if controller is already running - switch_request.activate_asap = False - switch_request.timeout = Duration(sec=2, nanosec=0) - self.call_service("/controller_manager/switch_controller", switch_request) - # Try unloading the scaled joint trajectory controller - try: - names.index("scaled_joint_trajectory_controller") - unload_request = UnloadController.Request() - unload_request.name = "scaled_joint_trajectory_controller" - self.call_service("/controller_manager/unload_controller", unload_request) - except ValueError: - print("scaled_joint_trajectory_controller not loaded, skipping unload") - # Connect to the passthrough controller action - finally: - self.jtc_action_client = waitForAction( - self.node, - "/passthrough_trajectory_controller/forward_joint_trajectory", - FollowJointTrajectory, - ) - time.sleep(2) - - def switch_controller(self, active, inactive): - switch_request = SwitchController.Request() - # Check if passthrough controller is inactive, and activate it if it is. - switch_request.activate_controllers = [active] - # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. - switch_request.deactivate_controllers = [inactive] - switch_request.strictness = ( - 1 # Best effort switching, will not terminate program if controller is already running - ) - switch_request.activate_asap = False - switch_request.timeout = Duration(sec=2, nanosec=0) - self.call_service("/controller_manager/switch_controller", switch_request) - - -if __name__ == "__main__": - rclpy.init() - node = Node("robot_driver_test") - robot = Robot(node) - robot.load_passthrough_controller() - - # The following list are arbitrary joint positions, change according to your own needs - waypts = [ - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], - [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], - ] - time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] - - # Execute trajectory on robot, make sure that the robot is booted and the control script is running - robot.send_trajectory(waypts, time_vec) diff --git a/examples/load_switch_controller_example.py b/examples/load_switch_controller_example.py deleted file mode 100644 index aa0fd8b52..000000000 --- a/examples/load_switch_controller_example.py +++ /dev/null @@ -1,236 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2024, Universal Robots -# -# 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 rclpy -from builtin_interfaces.msg import Duration -from control_msgs.action import FollowJointTrajectory - -from rclpy.action import ActionClient -from rclpy.node import Node -from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint -from ur_msgs.srv import SetIO -from controller_manager_msgs.srv import ( - UnloadController, - LoadController, - ConfigureController, - SwitchController, - ListControllers, -) -import time - -TIMEOUT_WAIT_SERVICE = 10 -TIMEOUT_WAIT_SERVICE_INITIAL = 60 -TIMEOUT_WAIT_ACTION = 10 - -ROBOT_JOINTS = [ - "shoulder_pan_joint", - "shoulder_lift_joint", - "elbow_joint", - "wrist_1_joint", - "wrist_2_joint", - "wrist_3_joint", -] - - -# Helper functions -def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): - client = node.create_client(srv_type, srv_name) - if client.wait_for_service(timeout) is False: - raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") - - node.get_logger().info(f"Successfully connected to service '{srv_name}'") - return client - - -def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): - client = ActionClient(node, action_type, action_name) - if client.wait_for_server(timeout) is False: - raise Exception( - f"Could not reach action server '{action_name}' within timeout of {timeout}" - ) - - node.get_logger().info(f"Successfully connected to action '{action_name}'") - return client - - -class Robot: - def __init__(self, node): - self.node = node - self.init_robot() - - def init_robot(self): - service_interfaces = { - "/io_and_status_controller/set_io": SetIO, - "/controller_manager/load_controller": LoadController, - "/controller_manager/unload_controller": UnloadController, - "/controller_manager/configure_controller": ConfigureController, - "/controller_manager/switch_controller": SwitchController, - "/controller_manager/list_controllers": ListControllers, - } - self.service_clients = { - srv_name: waitForService(self.node, srv_name, srv_type) - for (srv_name, srv_type) in service_interfaces.items() - } - - def set_io(self, pin, value): - """Test to set an IO.""" - set_io_req = SetIO.Request() - set_io_req.fun = 1 - set_io_req.pin = pin - set_io_req.state = value - - self.call_service("/io_and_status_controller/set_io", set_io_req) - - def send_trajectory(self, waypts, time_vec): - """Send robot trajectory.""" - if len(waypts) != len(time_vec): - raise Exception("waypoints vector and time vec should be same length") - - # Construct test trajectory - joint_trajectory = JTmsg() - joint_trajectory.joint_names = ROBOT_JOINTS - for i in range(len(waypts)): - point = JointTrajectoryPoint() - point.positions = waypts[i] - point.time_from_start = time_vec[i] - joint_trajectory.points.append(point) - - # Sending trajectory goal - goal_response = self.call_action( - self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) - ) - if goal_response.accepted is False: - raise Exception("trajectory was not accepted") - - # Verify execution - result = self.get_result(self.jtc_action_client, goal_response) - return result - - def call_service(self, srv_name, request): - future = self.service_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - def call_action(self, ac_client, g): - future = ac_client.send_goal_async(g) - rclpy.spin_until_future_complete(self.node, future) - - if future.result() is not None: - return future.result() - else: - raise Exception(f"Exception while calling action: {future.exception()}") - - def get_result(self, ac_client, goal_response): - future_res = ac_client._get_result_async(goal_response) - rclpy.spin_until_future_complete(self.node, future_res) - if future_res.result() is not None: - return future_res.result().result - else: - raise Exception(f"Exception while calling action: {future_res.exception()}") - - def load_passthrough_controller(self): - list_request = ListControllers.Request() - list_response = robot.call_service("/controller_manager/list_controllers", list_request) - names = [] - # Find loaded controllers - for controller in list_response.controller: - names.append(controller.name) - # Check whether the passthrough controller is already loaded - try: - names.index("passthrough_trajectory_controller") - except ValueError: - print("Loading controller") - load_request = LoadController.Request() - load_request.name = "passthrough_trajectory_controller" - self.call_service("/controller_manager/load_controller", load_request) - configure_request = ConfigureController.Request() - configure_request.name = "passthrough_trajectory_controller" - self.call_service("/controller_manager/configure_controller", configure_request) - list_request = ListControllers.Request() - list_response = robot.call_service("/controller_manager/list_controllers", list_request) - names.clear() - # Update the list of controller names. - for controller in list_response.controller: - names.append(controller.name) - id = names.index("passthrough_trajectory_controller") - switch_request = SwitchController.Request() - # Check if passthrough controller is inactive, and activate it if it is. - if list_response.controller[id].state == "inactive": - switch_request.activate_controllers = ["passthrough_trajectory_controller"] - # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. - try: - id = names.index("scaled_joint_trajectory_controller") - if list_response.controller[id].state == "active": - switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] - except ValueError: - switch_request.deactivate_controllers = [] - finally: - switch_request.strictness = 1 # Best effort switching, will not terminate program if controller is already running - switch_request.activate_asap = False - switch_request.timeout = Duration(sec=2, nanosec=0) - self.call_service("/controller_manager/switch_controller", switch_request) - # Try unloading the scaled joint trajectory controller - try: - names.index("scaled_joint_trajectory_controller") - unload_request = UnloadController.Request() - unload_request.name = "scaled_joint_trajectory_controller" - self.call_service("/controller_manager/unload_controller", unload_request) - except ValueError: - print("scaled_joint_trajectory_controller not loaded, skipping unload") - # Connect to the passthrough controller action - finally: - self.jtc_action_client = waitForAction( - self.node, - "/passthrough_trajectory_controller/forward_joint_trajectory", - FollowJointTrajectory, - ) - time.sleep(2) - - def switch_controller(self, active, inactive): - switch_request = SwitchController.Request() - # Check if passthrough controller is inactive, and activate it if it is. - switch_request.activate_controllers = [active] - # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. - switch_request.deactivate_controllers = [inactive] - switch_request.strictness = ( - 1 # Best effort switching, will not terminate program if controller is already running - ) - switch_request.activate_asap = False - switch_request.timeout = Duration(sec=2, nanosec=0) - self.call_service("/controller_manager/switch_controller", switch_request) - - -if __name__ == "__main__": - rclpy.init() - node = Node("robot_driver_test") - robot = Robot(node) - robot.load_passthrough_controller() diff --git a/examples/passthrough_example.py b/examples/passthrough_example.py new file mode 100644 index 000000000..954f9659c --- /dev/null +++ b/examples/passthrough_example.py @@ -0,0 +1,69 @@ +#!/usr/bin/env python3 +# Copyright 2024, Universal Robots +# +# 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 rclpy +from builtin_interfaces.msg import Duration +from rclpy.node import Node +from robot_class import Robot + + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + + # The following list are arbitrary joint positions, change according to your own needs + waypts = [ + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], + [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], + ] + # Velocities and accelerations can be omitted + vels = [ + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + ] + accels = [ + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], + ] + time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] + + # Execute trajectory on robot, make sure that the robot is booted and the control script is running + # Make sure that the pasthrough controller is loaded and activated + robot.load_controller("passthrough_trajectory_controller") + robot.switch_controllers( + ["passthrough_trajectory_controller"], ["scaled_joint_trajectory_controller"] + ) + # Trajectory using positions, velocities and accelerations + robot.passthrough_trajectory(waypts, time_vec, vels, accels) + # Trajectory using only positions + robot.passthrough_trajectory(waypts, time_vec) diff --git a/examples/example_spline.py b/examples/robot_class.py similarity index 56% rename from examples/example_spline.py rename to examples/robot_class.py index e00c4bb98..70146c066 100644 --- a/examples/example_spline.py +++ b/examples/robot_class.py @@ -42,7 +42,9 @@ SwitchController, ListControllers, ) -import time +from std_srvs.srv import Trigger +from enum import Enum +from collections import namedtuple TIMEOUT_WAIT_SERVICE = 10 TIMEOUT_WAIT_SERVICE_INITIAL = 60 @@ -56,6 +58,30 @@ "wrist_2_joint", "wrist_3_joint", ] +Action_tuple = namedtuple("Actions", ["name", "action_type"]) + + +class Actions(Enum): + PASSTHROUGH_TRAJECTORY = Action_tuple( + "/passthrough_trajectory_controller/forward_joint_trajectory", FollowJointTrajectory + ) + FOLLOW_TRAJECTORY = Action_tuple( + "/scaled_joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectory + ) + + +Service_tuple = namedtuple("Services", ["name", "service_type"]) + + +class Services(Enum): + Set_IO = Service_tuple("/io_and_status_controller/set_io", SetIO) + Load_Controller = Service_tuple("/controller_manager/load_controller", LoadController) + Unload_Controller = Service_tuple("/controller_manager/unload_controller", UnloadController) + Configure_Controller = Service_tuple( + "/controller_manager/configure_controller", ConfigureController + ) + Switch_Controller = Service_tuple("/controller_manager/switch_controller", SwitchController) + List_Controllers = Service_tuple("/controller_manager/list_controllers", ListControllers) # Helper functions @@ -85,24 +111,26 @@ def __init__(self, node): self.init_robot() def init_robot(self): - service_interfaces = { - "/io_and_status_controller/set_io": SetIO, - "/controller_manager/load_controller": LoadController, - "/controller_manager/unload_controller": UnloadController, - "/controller_manager/configure_controller": ConfigureController, - "/controller_manager/switch_controller": SwitchController, - "/controller_manager/list_controllers": ListControllers, - } + self.service_clients = { - srv_name: waitForService(self.node, srv_name, srv_type) - for (srv_name, srv_type) in service_interfaces.items() + service.name: waitForService(self.node, service.value.name, service.value.service_type) + for (service) in Services + } + + self.action_clients = { + action.name: waitForAction(self.node, action.value.name, action.value.action_type) + for (action) in Actions } - self.jtc_action_client = waitForAction( - self.node, - "/passthrough_trajectory_controller/forward_joint_trajectory", - FollowJointTrajectory, + + def add_service(self, dict_key: str, service_tuple: Service_tuple): + self.service_clients.update( + {dict_key: waitForService(self.node, service_tuple.name, service_tuple.service_type)} + ) + + def add_action(self, dict_key: str, action_tuple: Action_tuple): + self.action_clients.update( + {dict_key: waitForAction(self.node, action_tuple.name, action_tuple.action_type)} ) - time.sleep(2) def set_io(self, pin, value): """Test to set an IO.""" @@ -111,9 +139,9 @@ def set_io(self, pin, value): set_io_req.pin = pin set_io_req.state = value - self.call_service("/io_and_status_controller/set_io", set_io_req) + self.call_service(Services.Set_IO.name, set_io_req) - def send_trajectory(self, waypts, time_vec, vels, accels): + def follow_trajectory(self, waypts, time_vec): """Send robot trajectory.""" if len(waypts) != len(time_vec): raise Exception("waypoints vector and time vec should be same length") @@ -124,20 +152,56 @@ def send_trajectory(self, waypts, time_vec, vels, accels): for i in range(len(waypts)): point = JointTrajectoryPoint() point.positions = waypts[i] - point.velocities = vels[i] - point.accelerations = accels[i] point.time_from_start = time_vec[i] joint_trajectory.points.append(point) # Sending trajectory goal goal_response = self.call_action( - self.jtc_action_client, FollowJointTrajectory.Goal(trajectory=joint_trajectory) + self.action_clients[Actions.FOLLOW_TRAJECTORY.name], + FollowJointTrajectory.Goal(trajectory=joint_trajectory), ) if goal_response.accepted is False: raise Exception("trajectory was not accepted") # Verify execution - result = self.get_result(self.jtc_action_client, goal_response) + result = self.get_result(self.action_clients[Actions.FOLLOW_TRAJECTORY.name], goal_response) + return result + + def passthrough_trajectory( + self, + waypts: list[float], + time_vec: list[float], + vels: list[float] = [], + accels: list[float] = [], + goal_time_tolerance=Duration(sec=1), + ): + """Send trajectory through the passthrough controller.""" + if len(waypts) != len(time_vec): + raise Exception("waypoints vector and time vec should be same length.") + trajectory = JTmsg() + trajectory.joint_names = ROBOT_JOINTS + for i in range(len(waypts)): + point = JointTrajectoryPoint() + point.positions = waypts[i] + point.time_from_start = time_vec[i] + if len(vels) != 0: + point.velocities = vels[i] + if len(accels) != 0: + point.accelerations = accels[i] + trajectory.points.append(point) + goal_response = self.call_action( + self.action_clients[Actions.PASSTHROUGH_TRAJECTORY.name], + FollowJointTrajectory.Goal( + trajectory=trajectory, goal_time_tolerance=goal_time_tolerance + ), + ) + if goal_response.accepted is False: + raise Exception("trajectory was not accepted") + + # Verify execution + result = self.get_result( + self.action_clients[Actions.PASSTHROUGH_TRAJECTORY.name], goal_response + ) return result def call_service(self, srv_name, request): @@ -165,82 +229,58 @@ def get_result(self, ac_client, goal_response): else: raise Exception(f"Exception while calling action: {future_res.exception()}") - def load_passthrough_controller(self): - list_request = ListControllers.Request() - list_response = robot.call_service("/controller_manager/list_controllers", list_request) + def load_controller(self, controller_name: str): + list_response = self.call_service(Services.List_Controllers.name, ListControllers.Request()) names = [] # Find loaded controllers for controller in list_response.controller: names.append(controller.name) # Check whether the passthrough controller is already loaded try: - names.index("passthrough_trajectory_controller") + names.index(controller_name) except ValueError: print("Loading controller") - load_request = LoadController.Request() - load_request.name = "passthrough_trajectory_controller" - self.call_service("/controller_manager/load_controller", load_request) - configure_request = ConfigureController.Request() - configure_request.name = "passthrough_trajectory_controller" - self.call_service("/controller_manager/configure_controller", configure_request) - list_request = ListControllers.Request() - list_response = robot.call_service("/controller_manager/list_controllers", list_request) + load_request = LoadController.Request(name=controller_name) + self.call_service(Services.Load_Controller.name, load_request) + configure_request = ConfigureController.Request(name=controller_name) + self.call_service(Services.Configure_Controller.name, configure_request) + list_response = robot.call_service( + Services.List_Controllers.name, ListControllers.Request() + ) names.clear() # Update the list of controller names. for controller in list_response.controller: names.append(controller.name) - id = names.index("passthrough_trajectory_controller") - switch_request = SwitchController.Request() - # Check if passthrough controller is inactive, and activate it if it is. - if list_response.controller[id].state == "inactive": - switch_request.activate_controllers = ["passthrough_trajectory_controller"] - # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. - try: - id = names.index("scaled_joint_trajectory_controller") - if list_response.controller[id].state == "active": - switch_request.deactivate_controllers = ["scaled_joint_trajectory_controller"] - except ValueError: - switch_request.deactivate_controllers = [] - finally: - switch_request.strictness = 1 # Best effort switching, will not terminate program if controller is already running - switch_request.activate_asap = False - switch_request.timeout = Duration(sec=2, nanosec=0) - self.call_service("/controller_manager/switch_controller", switch_request) - # Try unloading the scaled joint trajectory controller - try: - names.index("scaled_joint_trajectory_controller") - unload_request = UnloadController.Request() - unload_request.name = "scaled_joint_trajectory_controller" - self.call_service("/controller_manager/unload_controller", unload_request) - except ValueError: - print("scaled_joint_trajectory_controller not loaded, skipping unload") - # Connect to the passthrough controller action + else: + print("Controller already loaded") finally: - self.jtc_action_client = waitForAction( - self.node, - "/passthrough_trajectory_controller/forward_joint_trajectory", - FollowJointTrajectory, - ) - time.sleep(2) + print(f"Currently loaded controllers: {names}") - def switch_controller(self, active, inactive): + def switch_controllers(self, active: list[str], inactive: list[str]) -> bool: switch_request = SwitchController.Request() - # Check if passthrough controller is inactive, and activate it if it is. - switch_request.activate_controllers = [active] - # Check that the scaled joint trajectory controller is loaded and active, deactivate if it is. - switch_request.deactivate_controllers = [inactive] + switch_request.activate_controllers = active + switch_request.deactivate_controllers = inactive switch_request.strictness = ( - 1 # Best effort switching, will not terminate program if controller is already running - ) + SwitchController.Request.BEST_EFFORT + ) # Best effort switching, will not terminate program if controller is already running switch_request.activate_asap = False switch_request.timeout = Duration(sec=2, nanosec=0) - self.call_service("/controller_manager/switch_controller", switch_request) + return self.call_service(Services.Switch_Controller.name, switch_request) if __name__ == "__main__": rclpy.init() node = Node("robot_driver_test") + for action in Actions: + print(action.name) + print(action.value.name) robot = Robot(node) + robot.switch_controllers( + ["passthrough_trajectory_controller"], ["scaled_joint_trajectory_controller"] + ) + robot.add_service( + "Zero_sensor", Service_tuple("/io_and_status_controller/zero_ftsensor", Trigger) + ) # The following list are arbitrary joint positions, change according to your own needs waypts = [ @@ -248,17 +288,7 @@ def switch_controller(self, active, inactive): [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], ] - vels = [ - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - ] - accels = [ - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - ] time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] # Execute trajectory on robot, make sure that the robot is booted and the control script is running - robot.send_trajectory(waypts, time_vec, vels, accels) + robot.passthrough_trajectory(waypts, time_vec) diff --git a/examples/scaled_joint_example.py b/examples/scaled_joint_example.py new file mode 100644 index 000000000..ce1cab3d4 --- /dev/null +++ b/examples/scaled_joint_example.py @@ -0,0 +1,51 @@ +#!/usr/bin/env python3 +# Copyright 2024, Universal Robots +# +# 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 rclpy +from builtin_interfaces.msg import Duration +from rclpy.node import Node +from robot_class import Robot + +if __name__ == "__main__": + rclpy.init() + node = Node("robot_driver_test") + robot = Robot(node) + + # The following list are arbitrary joint positions, change according to your own needs + 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)] + robot.switch_controllers( + ["scaled_joint_trajectory_controller"], ["passthrough_trajectory_controller"] + ) + # Execute trajectory on robot, make sure that the robot is booted and the control script is running + robot.follow_trajectory(waypts, time_vec) diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index 9f0dbe9ef..93ffa0a94 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -76,7 +76,6 @@ controller_interface::CallbackReturn ScaledJointTrajectoryController::on_activat controller_interface::return_type ScaledJointTrajectoryController::update(const rclcpp::Time& time, const rclcpp::Duration& period) { - // std::cout << state_interfaces_.back().get_name() << " = " << state_interfaces_.back().get_value() << std::endl; if (state_interfaces_.back().get_name() == scaled_params_.speed_scaling_interface_name) { scaling_factor_ = state_interfaces_.back().get_value(); } else { diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index c2e7f5036..476f37494 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -148,10 +148,9 @@ install(PROGRAMS scripts/wait_for_robot_description scripts/example_move.py scripts/start_ursim.sh - ../examples/examples.py - ../examples/load_switch_controller_example.py - ../examples/example_movej.py - ../examples/example_spline.py + ../examples/robot_class.py + ../examples/scaled_joint_example.py + ../examples/passthrough_example.py DESTINATION lib/${PROJECT_NAME} ) 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 f0be556a8..94179be03 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -194,11 +194,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool initialized_; double system_interface_initialized_; bool async_thread_shutdown_; + + // Passthrough trajectory controller interface values double passthrough_trajectory_transfer_state_; double passthrough_trajectory_cancel_; double passthrough_trajectory_dimensions_; double passthrough_trajectory_controller_running_; - // TODO(URJala): The size of these arrays should be dependent on the number of joints on the physical robot. urcl::vector6d_t passthrough_trajectory_positions_; urcl::vector6d_t passthrough_trajectory_velocities_; urcl::vector6d_t passthrough_trajectory_accelerations_; @@ -225,6 +226,9 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface bool robot_program_running_; 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_; 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/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index f02acbe20..1796b459e 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -155,7 +155,6 @@ def controller_spawner(controllers, active=True): ] + inactive_flags + controllers, - # ros_arguments=['--log-level', 'debug'] ) controllers_active = [ @@ -186,7 +185,6 @@ def controller_spawner(controllers, active=True): controller_spawner_timeout, ], condition=IfCondition(activate_joint_controller), - # ros_arguments=['--log-level', 'debug'] ) initial_joint_controller_spawner_stopped = Node( package="controller_manager", @@ -200,7 +198,6 @@ def controller_spawner(controllers, active=True): "--inactive", ], condition=UnlessCondition(activate_joint_controller), - # ros_arguments=['--log-level', 'debug'] ) rsp = IncludeLaunchDescription( diff --git a/ur_robot_driver/src/controller_stopper.cpp b/ur_robot_driver/src/controller_stopper.cpp index 0ba15d881..0fc37267b 100644 --- a/ur_robot_driver/src/controller_stopper.cpp +++ b/ur_robot_driver/src/controller_stopper.cpp @@ -88,7 +88,6 @@ ControllerStopper::ControllerStopper(const rclcpp::Node::SharedPtr& node, bool s auto it = std::find(consistent_controllers_.begin(), consistent_controllers_.end(), controller.name); if (it == consistent_controllers_.end()) { stopped_controllers_.push_back(controller.name); - std::cout << "Stopping controller with name: " << controller.name << std::endl; } } } diff --git a/ur_robot_driver/test/__pycache__/robot_driver.cpython-310.pyc b/ur_robot_driver/test/__pycache__/robot_driver.cpython-310.pyc deleted file mode 100644 index d58deb25cb98c8636cf2ce4cccd549c7915ddaa7..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 3187 zcmai0-E$i?5I>#I=kwXlcLPn6HvIz3M+_~)$G|X@BxTZ}4wE81YiPnHhCvNVV#CWh@rWLqo~e1kD`;+cmgYsTsJZRgnwPv1a5Hw2vR5X!M@!{OeS|IiwWC5o`+4M$+oAty{ zwA^MX1J9UGdGv^JUos(m%0$ScE}r)6GDM(ms|t%BWcz*x?NYO1FYCn=)Z=JZ_JuP8 zVK+$ovf4h6V84%E9^QI+c)LF=%U!?A*>3c!GL)Sh_&x520LZ})`vqvL&;%k{8b<)u zr+8In^OW#;kktvF0LCu@IW_nKu)lt2>}UL;=t9s$V=EK48iI$7ofJJ5LF{j34>BqI zt5oacPW7R=Nl3;vjnCVbdm>aJFmnMPMPcmwxES*U-co5N2SS zc_u4RLQMpvXFnay#^N5}jRS09DZzd;~aFHvR-07~yfe`m$EF2(C!6h!4 zAcqTMo~;a(&p3%u*WpJr92uQ~o5uxomm1&@Y1S=%4dzu9unyUJ;G@la8AXoR>q(mJ zr~EA#j@wz2EZ}P}t4-Yj4PgOs3srKsG;Zcgx@RoKb=uO;po3WMMpOs@p&}neEOnn_>ky% zo#7RjS4$&%`AJ@GMs#YZbH$G`U+5Ausy*BZla?d&LA7u4E6f6vSc1j7W8vpFVe!w< z#1xP!!Ot`;V#4SCa~3B<*u6UXm<$lt5u8x{xIhwEATJ)4cQUuevQd2)DR9AsMSjC;5O!SyG?99+@x>HxGlR;Nfw8w$-+vttk3f*im0dK8_S zkf@SJp-jPQH5b5>^a!7iA<)EWiQk4Z`3FGMw9s$j$UH~btI`(xr3a~}D&MScxBT_H zcQ?0M%FG^~8;NCIU-L-6Fj9sin`W@5z`Ab{8$OFz&&`7K_u+@*!Ft_BuZ$!A6n4i- zc(n^xEBAQTYww+(YW#Ch?^>^p*f6@+FogTCR6lT2=*hpP={!ID=zlypf0t(6 zRgUkcLBf1rxxSxdv=?K)>ifU+g1A3Z39;xVOzvfrzk^o4hXgZ?e}d!=62#I7Ks8jc zaPuV~-$4^IKpX?|keDTxI7O%8l&g-Ouh&&6&f4uLZNnW4RS*Y4CyR1=@GxLN)I=Qg z(hx!l1Jtv-2iVPP^8ABd6oYtJal}d@+8m3$n(ar<2Z>lgNM>nXWiAb4I;dG%N$^KG z4qg3vt?25nAFoNsQjzg|FFXPhNzIPbRJ{StlwgMAJEbb{cVQqdqr~b4a!=Vow+l(H zN;KjaZM=`&NZ&z4qae(Z1dP>n5zA~|i?NvMe=%yIwQ+xaa|aUS$Mw~n*1F%?x^r)R zwY9nRld5iQezV!~?`>{0TieP?0{&1{MuMm+-2iF~gLFM)`d^50S`g9I04fvS+IiQh soG){je1*@u_!x+@=#c(rBG0QX(kW2~J+ijorB8@}G=w65Q#ezu&jF zx2Iz<;5hYUns4rY`|bCB{Py?!zJGPQoeYGx?uqyRay!HP9AC6x%SGmY!ZOTBMqmUs z&VsODYjl4 zzW$7D)Zt>0MYEa~T0P1J%P!+fSCCb>wh@;WOCm9` zPfn6i$ndC1$+$=&LwzEo_$rykp>SXBSg^CN;;C* z*odSj1qQ?+7{4rr1rj|XlCUgFa##?hJ`x?q-YzeKH3`}jml!{kJQ|MSxRf$!FgX$z zpm;pmFXs|V^NGWeplVJQU--Hq_v(=Q!^1@EkA6{M6kB36JVL^KITWMY zgqwx``5FKx89ASqV$QJ|6811bQ`cFR_@PXYB|dnNIsg)u4t#u|>1dK1mWE;dq^5mI zX?>GK`kM9+;pC-AJiIS?C@D+fece6l!)KGofz`Lp7 z-71)$WPqkv0NMpBKu)j$bO?5UPJsjH5*z?|!3oeUxBz+t9-w#B9rP$(I&*c(y)Z-L zl|MxXrLTNU8?H8dcxs;?%g9zOS;+5CiV5dFI);%QT7Ha@TkTDK3MOq&StiBA+zWH@ zEmc5zMwy_wODQSb7mCc6ao>Y>#Y+0(uuqogP*f&%NK-6B@M$U*Ihqg^s}vW-Va18L z>TaK96?0!)QqhLgV+E%q%KL{~;}J<3t0`KpMm-Oym9!23?6r+?cuqJcoyYmvvZ}Oy z%~V;#g!wo(%kh}{%(~<58P0XWGwFFoy2#a%#ZYU(!sGH%lz$8&Cvh3zSsY^d1=Vwm zHX){QF1=X6W{omu*{-04tb|;J2ND28Y&48(M3!R`Ck>9sLh|SkSp%*AKh2x)^H zb!qC0mu5ls0j!Ll#4%$YTKEu^dbBkv=H!j9S&VT(C zFh?B#-|{#r^RLE}VW;2kRKBKR>Y^#ub7CsYnrPzD!KSsacg!xH6Mg{AeDcQ5Sv9zJ}FB*yy-Fr2K0_OJisbz<=d70jO~=&QeVDL%wyEXS@_(s*t$M2IeoSb1zvt`r5G@B=EFy2vuf4|3#AcqlHw29hJvWEG^4ASOEjT}1*S+K5OtVL~|( zC2>f|@$O-g93YW|VwXnxfb~n{E+|v5#lQiOxQ|s76EB*KTPw8yIL6Fye3o01;g)2% zwHa>hG}p9%nDZyRlipL7tpDze|L&}RZ^plO%HNaqAI$gj=4wV2V~23w8sT?9!Xl6_9bAz(AsX z@(?VxQXT=(^$3cYI_Z(l+Q~i86G1IRDUqn`T#!$2CW?KZ`1lC$BE@xIdrxn;eaDXO zeZ7h)dHA{w3{b&KMGkF%a$iA%W6i~Td$SrNFaoI!0Nr-nS@;yQd}D@h%Ej=+}{6-Z`^scY5WXIlGF47C=Id zBH>k+&EcH$Fb?-EAo8mF(<^u9(D}NSKEjr+N9Q~x`voBNQ#P=Suut12Z2c@~D%>~= z$OBU;4o0%8Y|%;y<`jGT6z#=a{c)3E88^#$#leWv2-cKYvmsEPmKV&lF#z`})8kr9 zu%*m`{T!#ECD=C{;}*F>%a4_7G5x7=Ji$2ziR0FkMM~USjg<8;h^yp)F2Ker^>($G z@d^vBbG$yoN!t_EleQRY$3-D!u7;Et>efYJ+yW!@-_}Uk;|E|gs5KRZltr-Xqq)i6 zxPODa@f7aOt&LsZ7xFu^FXXe^7mvO##+VdC^|UCY3Y2uPp?SydMB_m~P6j}53Pgqi z9o+zhK;NL)cQ|l#P?QHn5{Sxy!H5(%B#J{=uCZ%@(u-UWKe>67B%fnZD0-S5Wk?ge zo&|YOF(;$}#RiH2JSEiQ0>@Mh-Jzz1`wopY8k1X528E*W?syVZBkA78eEF|ID-udf zzdFXeTj8JFLFwDr9ZgUI#(%vEf*Ginh!VjOj8*iAvK$>62n?E@MCE52Yz(Xw6|ohE1Vaud!Op3a3ekb645l(VMs)=qhOQKsDuK5@J`#y5<(dJZ za7aNr*@}Zg8-U`@FCY$E;V1{ig>hh4ba5081$bN!vLC8QC@=+RXi@jurLtW*(6QQ2 z*zvDo^^<^Du)Sb*D6O2ZUA34T)pHzEUH@&z^Ny^4W5&O6mM@>-*Uj)1vwR8aV_CjA z!#B@6tkup5_neO@ubAN8=e#HQNj_cCaAt6t+f4O=h78w`|I~~!s(J{w@q+wmDc{X<$I3rIR46YX7EB_6L!@-*?p?*+a1q$ zoZoPf-#p{3NPCye)Gf`{ZOqhdJU^JJ+m@|s&(yU)(}c~)vdpNcxd) zS<`Ww1>VO^{U+UHDOhi@zU>C}?YCH;E37|Y21z8ZuPqq0B{yooZs4?R%7W4X)OFCY zTT*72ne~LN&rIi5$}o?>oX4%>HeDJ>*;3YX4sA`KZaLI7kK4wr=bT#Vxcw&PR+p~l z0sS}717mr!mbOV?g0+KmkyBg7IjC2n=V`GN7lTn32dx&Bx{P|u%TjvH6es89p8UKV z&?|~AMIq%dl4WG{1<(*fBVud?ZZ%*O&b>G+B8}97x=<4kuI1+-qm^_9cf&d+Yz!i`_V>?unSGWk+R)%F! zP@DrmlfeLk%%!HcV^4eceo)>YYH!`&+aB)Sx4pBywYPiU!$ibpoFXAf@b#dn8AW%+ zwok3;?AhPi+TPQ%WB=~6PL*|_;Ht1Om^6qP1$Ifrs=B5eYC78Va~vfp5mclU7mhPL z2+TY#DmHa&63$BHChx^Ou6{=hwZk>_V_T&F{dBrKcm)?o}_>U0S{4{Pq{_ys&yl zn%_BRx70c(yx=6NTt2&{?UKL!%m|S6{ZrMeFZkPmbT;1eORKru^Im!InTJnvSItbx z@-tt2qxzDsXWk6ySFN^6=Y;cWX`ThFp61rfRMuoGw`MB0W-Hq=m2K$KiLkJ*xqnUam!l9o(K%M4#NUAgk5<*@j)2hF#f)o=ii}MXvYSm9@<3cJ`VQY`>N~wRsZ^P{I%JIY+z2 z_VFj49XF+`##G#?)%&zmeAIOtG?5=W=G4ABN*whs3lstZ&e%q$USKdP1>l!R5;2hM z4@3e-BJrp|J@nM-7mx=dpmPk0^o^3Zk*F*Ngrq3ZWD*@1!~&3&Zk9@-%`!MQ|Jek}g7}S8QHeGL=#(Rg~D}hE+fe$5t9ruj>oG1#L{&6HRE$$()&~Y6pv|smeLwDikPdv(;-e)oU+ujn}TM%#)l2 z80I2K%Rk{SLgX;ZM=@a=wto9TDBS}Z8 zEE1w}4%D#~hU7yh(YbQJz;?@SO$K^++THX|{H1k!URw9J%Py?jljgfVd+M=yn%h!D zJ#JgnndMi^Tg_C|;dI6N=AzEJJqyJgmF~1H1G>`|M7q<~h;*lI!$P`)ay?aZOF=n& z%J=zGoπcUI9ATu=KZeBgNC!SR4fsgWxbMa!Zf=haZbo#F&f-jU!)!Qo8(R7T9e zNg#L^P$&b1H)f>J+Ryp4dgD$QX;smX;5FqmGS_RhC_Oalt=6a|UH#1qlrHGi$gt}& zQikQFTC1^TT1YtRmRd^wE3eUn`*P4Tg2y>%@X+n~kt=Zm?hKEs=|U6yix`nHX6L*A&3N6sJ7 zHSWcDUfS5f#N;{Q&6${hbWszNXWT2_rL~{(q`YIGE^8|*lph9336vB+H8pw1Jz9?T z1cS*ZaX?evh*D%Qp=yMA99jh9NyXR!RX@-qe3j4jB?*C(xbe|I)Vcra6`k?kx}$>hK<%K3 z^uj~&!I{C_IoX`k0F_+LYhjH^=&|UyHLe?0XAH*pusRtq0_hG7+SDrG$r9=dzXE5* z;-wkBPW5KEn^e0{&ToNVFhTz^6&7-{Wm?7-b)vy2pq2tB4Tu+FfF6+C>If{C#_AX3 zNBZGVS3&j%Un>8bo$rl3xh+_4s2r`t4j)B;Yg}YfBF8Z89t5Z(kuM|o3IengktY#Y z5d1L!rRst0y}QD_-MicOZSQJr4HKDy2&oMkG{s3oK$KY8qXwuzK2*jVSCP z@;ua#&}y1j?sAcwk7Kp5kY)%?MP_bslfzz_0&DH%SNX?Sd!i@aOEHqF)G7~J%%?}p>vcRh>J zOYgkkxpRS2Wtv|B3a)oCWKDVQe7CM4%~j`}OqCL;coA+im@6B$0D6{#Ysm-Bj?%=zsQE!&_LibCSrI&5YF1t6g?A~n<;%gASW-G~wz@qZne7hewJYGD zMT_1Q8&k6O+JAsuuTaUpHouRB^3qbk;=gco1a1G|=m2TA($TTO`Pw?ZV+;3VOI61` z{F`iTr`fZ>*NX;ND*w)aIsEfF{&qWOpx_FN<-06ED}@+0kFz@G7i{!c2-0^51?g}U zAC4qkpYAM7hqg>|-i0FMT{6I*%{e!b2b@1<8U5ol2>dy>#&Llg`z+u@g8aOZ53b1+ z^gC|Vk3ytODOMW~IFs^{XUYb>*-vmq4k!CKCs%8|rZ{l?c(vMjQzbWED9@r}#^5-K z;js`$3Xa2^E76YAP+1r#QgHxT*uZV0%^zf8!+g?vgOMyLZXcy>UB=bsH;yv$a)Yui zBZ*fAsZ|X|GH$Fv%hjJ+KcHjW-rkoQ?fnhrTCew~#Zv!&R^32z_|&Uz_{9nKlr@|` zRwb0doBp-@{yWf6F8?&r<@6l^95(v;4R39HcE_M8A1K#okdX^qr!=FQ3)q}@_X73H z>oi&_IE+w>=_0#f=JeDl*#ohlZD7}XV<0GBd*y?lb$|8bQ0afa^1+XOUS9v^fBxc? z56*1-=@+^(&%W}(AJ%UfXp3Ik_Q6kHzwi6efB(+5pzZSi{QB3wCTFp=7Z9KpO3oqp zK7h+qI7TL;IyFvWOhzz-;F|#8Qrur+oJ#*k)PyhL!>syA%c!4}`jkOVRxdXy9yL*W zA2`=~A@e9%e|Pl1fO(T{tX`v^<|aF^1M1)2iLqS>IuIbMhpSkTA-Jpv1}}!3Mu4h{ z>R{f2v116HK=5S*UqN6X{*ym}45eyk_x9aJI>0bidmKQJr-Hp=I~o}smL>2DqrdzJ zINaGN`2m(Xk6;pjV#PcO4rt}P#=1~1p>3#8L|0EB)l%ua@cgc>g^sN;6vvGVhXLQE z4X{3+-C^G=KE{2o!f#dhd%&%XCQWcFgX6j6^mdRr;jB|ZtQ6eW{7NWR?wsJ@+DRMR z!M;)Q`j$)8yI+cYtMPo>rJB3a)w?G=0A?4hIJ542WO~t-G*>%YQ-5mkjBTo>X~H{O zu?+mf%kRv2r~}XJKh>&wsHIoiFZnuE54G8f9_l%ssjAIZwqz<>vXz~g%FgtG$L1L3 zDBEd*$3aunl&6zF8gK?K+`8rU5PaVkCH)iS^r<>ce&AT(ryR*#?W||+I<{x^0?}7A#4^8cT_)>M~ zjqTrROt0T@sb*)oI)rl#QFHR=I7lJWEKKtv$np2FJK0wsf8Bqny7MI%=J{opYBr~< zJ8_hqGfVG)t$+)3Gb>ip$Q^T~pZD_SMxHxhaFdG@H$QKF?aD!xso4Xk2sk)JaP3MB zQ*#jXk#eTyepW)h`(t0(o+``hT-BaB=RbSO_AIr$=_uQ?($W?5DO{MIg$aihXE>Zl z3L|g?GOY07FrHP!Q!?=8>cfkj2{@pd6bPCe$$A86rX(#0IuSg8;4lKD7vyUQo!vG@u{GBH?3FFG)}||4)#%+;=X}W2&RW%@`LxU68iF0d zeHox>aOP7z%*l)QDWF05b~j>v?=YesrCuVZYzQ6wiX{;thZR@B+9|H#2;43c;l|ULNH2UVTrVtS zYXqGA6af|@#mC&;Di+>LrlCb3{fzV)fH^bEvh!OR*84W&c$;y)&A8rXygz3u-e%ly zGyFTuns=Cu6mEHkseOl8@iXS3cbLGtjQJ_<3GONH6W(V`C+*MLPkNvAp5~@}!D(jo zFHJVq^brF9tS5^CX{P3i#qKj5t<8 diff --git a/ur_robot_driver/test/__pycache__/test_common.cpython-310.pyc b/ur_robot_driver/test/__pycache__/test_common.cpython-310.pyc deleted file mode 100644 index c48b72a88671f156693e87f400c9a770c7450acd..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 9398 zcmb_i+ix6MTCZDIS6|(B9LI5-Oy}zKOwx9eNoIF6ghWYZ#!JQ-V$UwLb|^Z1%I$Wl zyE><;lS$K-5RQ~A5H4B?9(Iu&v^)$*+$3Ij0P(~F|G|0T0r40Y|d0>WIg2{YR*<>WjXC1Zq8NaWI5v>Y0g*XWjX5~ zZ62!}ljWTMMzdHcHjh`1H&0YfG*4DeHcwSf$-caQx_PE@2IYy$0xML`;$32sZ0dWh^e>?Q zEGv0u(DNp(F8eRAx7azH?IJtR%6MO5i);z+x7h{n?7G1&vP)0&%4MdnYNfZoAj+T9}_jo7@tB)E{ZDZI}(#7QMYNu7(xaoy8-e^aSpjEQO!JA&quX0cI zUFY>q(`!W{f9o@^)``52c~JAhP|Pm3YJP{s!$y`6(|5f(Huw0ghnVwuQ%!)UZoR?>5r??#AcP<7(}GwT?r2>y6JuVWeqw1K)&AGk3izd%PB`c_E!4d%Z^UZ}<(I74_sA zZ`AAZys5jvy&(E1U|uO93LkjU?V!p$hHt3zYEw+0B`&W<)yNZBGz_MQ!g3h*-tDwn zjaFS)G#^_eR=sHX&KD$=Uwj5ovxsQ%FM&5bH}HhF0eaew)-!tg3uDWQ^c{ogJ-uhX zFmR?qx__6OFs{%g7%pP;!k8vJ3ebkBXK+K6zR;(EY~ zp~oLKYF=^SbhuCie{2+^M$-#AQStP8ST2{}?;gGpbbMB91yPZE)!N2z%FAgrrO_Hp z3f7C=v&G_Sr$#KY-tqm%#ahs6d9}!6#V8m~#(qF*u7))u&#g)9i=-da>k?Cq)_Ncc zk8n~K$8_Ukiqx&oYM$cLgs@tmjBrB!&}~(laVdJ-_CzxN2w|&*KiBvfVDjZBi;n_+ zKWtZVy2ZOecyTf0wZ%_bjfW7Bs_#nTh3?%us~6pyoLUz<+~vdtE>ragT(d~Ptp&|y z&?>hdi%ECemRiPqcZ{|~J%j?%g+8C{aV<6lGZ zH^wPGHjy8}cXZ2NN9M5^lGwa}Y5XXGV*p}e57Io&i)bw+_;KpYs1+1p`xT-sKZS07 zngEehfd!gU7*at^UPfz3n<^q0dS177vzDdvb7-uY0R0~cpp8cg9YZ0~c61W7O`~T> zQPUx6#tVJRjI5mmh-6BUGI#73FohkRpO!O>o@^QRt!hz{#B$S6g1O_o(E7qzO00Cx z->CXN31eRn6~m5DhkFk?U^m(F@?f!e*QMo-41NVoH9UKM zeH9sAM6;)nV^T3Z)3aazp#Wh36U=OA4Q<_gs;%qHVu>fIiv3ixv`Uf`AEb0uDZAws zOw&4az#dto4M^`2TZA71^z>)Ct3B18>hTuNGkp{K_l0&-+onDfb#uphsy{U_!o(qH zM4~_9g-+fFwzgJkaI6MD+Q7kc08dCf+mJ#wyhqP{=V_R zSXG?GnYj(o;GOruD@e_NuTi&-_*>!1M*Db&22Ow&AwLDzrr~F(k{#`t4JnLqOSv0=$l=OiV@-y<+MamiE-!|#x4%jsj1VRSeJV4GRPNjU#wC$qX^ zSU+)gyL0=t91uY4H7SIMB%Tbd_6#7>wt--l1eaMq(tis~DdmG?#_1V-X6m4?ze%od(Y)w(MtcISy&P#35}f8QIW{iCz*Cm|*r37&2XZ za9)amvtJBI=12^FOa?)UK}LxImejB$w&}&}|GEBBUsRMQe)wD$DKh%fmX9$+msTWg zZfjFIxc8mcA}{RRgQpvY|3^0NAD6$6#rX#WBrDQ^I9Z#qgKSaL&k^_#pk@H{f3y^l z$;A^QC#BnKJ{j(!+1*kP)xvT<13gz*7>Od{a637ulDaMCGX5CG4S%9KpcucIWA zca_nAGp~mtM}C+)aFeVmuY;NIKccrf@j#%2^PXRA-ec9PPq9u&9;%^P@Lpzh>#OeN zpAM1nN^*-*QY69-%s14q(i}tF^K00K|15zU0K)PSJcwlI!KsdT$q{C=7V>XVSD~-W z2J-9lOZ?|)0kUjT^YJ?<5tq1-UrAy`p#>VS2Ji?^NPIV?rwv-bd8y6%Hc)sK7T*OouJb?1{c_dC~@*WA@x zcfY-Sm8J8c%(XXG|Ncu&L80Y zR&)%1WQDHb3C{uy?J49|SyKcP%tDZvkaDb5Y(`ep5fj&?7df<{WHb9eD$_$`l%@@4 z3@i?hR&x=v02&dTA=J&D1#J)IB2|OFf>>b29a~zNG;wr)`>1pH`flkilB*})cUtcO3MjdY1Grt(|o2doIaY2EswK4Et7< z`(dth&#ZbaMqXw;sQMI!DFlv&0^~>X^Egu}!#%u&^#R6?_>6R3zD_+8Ay#u~Ddj!# zCF;6BfS8Yyht|jF$J9WQDJ+`6FH?OlN=0h0-tVJVdhAnr!62_)`Gl6PpFHB628^9Zirenr5g9OvRX51J!>{YadRlt3n~!ZE2yGIWcBP8S$!wI; zqEPoDSM3qg<0v7SR2!E>dj)x<7tb%z0?J_#8A;C#m8&_04v~%{04zODvGj1uA7N-X z1K=RS#j`sF?*%FqN^}~v?%X)xhErEcN&a2T-_?j}E+kV{Q4)ic*1_G$tT0 zSi_ExEEA8 z8|piMFBtibfRJprbKnD2l=WsAOPZzdyWGj-qO zOHrB`R0fMt=!l+v$QZAfQ7gvL1;34*yKf*J7jTYDOHn2)iZDXnyWpa>O1^aMa}fWi zSjviwTiftz_hm39oObZY<1X@QxqG~N-vcZ8UN!WDMJx%c?R#EZ^-m?UJE%Dpz7SiqqcCL@?O%85y=87ti*t8wm&Vp5sk3I}N` z#7)V#CO@VB5v1D2K7te71)k6bfD1uf|ElATARXpzjPrSqR;Hy!mLKvr#W;V3NABTh z&P;ZKQ(Jd(k@ZCcHeFDAOc4qh2Ro-L>}@jsSsAz zz34F~zer5_$W+2lLyzyPtZP;_VYbYDW@>&AdTu*tciLi#n1i}KM$5|qxwwE$C}x`I zS9w1d+V-oD`LEDfP3hb5UqG4vBEUY8HPLttkJK{*kxeiJ-H7|)r$}L)8)mjYsHYv@A-X2>)f=eq)Hla{0B?T55A9lEBBEFf}vU(Od;r9)G2qoAGDygvkfkT-f zhMW*0932k%nv_E%8VcmUB5^WIy{Cs2WGyLInUHAV9c;@V0>t)GObkb=+*cN~A@nLV zW`^A%{|%Zv)P_=`4h)iN!;$}tanh=iPwPOUY~A?Pv|+sLmTwFWIgYaOD;8QQ>HK?i zSZkbne|PUuhKx%Q=s(i_7J#GWZB8xS1B=7sxlKhaF&WAWiV@85U&r8m>_8g$8XlQo z9k2u4e&7yZIame5b;1GdSHNF;t;`6{h6nvV7W@}lR2|f^Ikj{TFE#=fMp5Fw6ONos z*a%SEMj)mkK;#f9jTrO!o z;5xjnK(lxF#U(j%sbq>A^VX}#B)Si)e#a9THoOcH$=XJM>wyrMwX5wmK9iH!z+stU z>XL}L;7@r$w9iX8?8akUU*0keNB$SqkhZJ3TABUlzwAY9hV*yuk^IK#gfcnspk|dr&svzVY-+ELY2es`VAO6{#ckO zbn_4{n4u028_jaK@lr2#&GXG--#p27kQ;XuUJDsEv({w%H)%l|7bWdZ1bN7y%ISF`NGl#1mJ~n;v*BC#A$h&fW%gwO4kUG z@Jagf2`bSygVze5ni zHzz~OZvpJ_Du}prTFHb@P>7JF0TUXE332w+Tn>aY>qbnpNyXay z&u}3A=K$SlVsfR{hZO3jFT4Y~yi;B(UlNIR}q69GSRBAi^NcJljvzsP%^4njEceY%MeA@g=zf>~CC(+xdyQ6iR@$rK8k83DIHXw|d}R;+WBQ~&0f27H z#YR@iq(}xW*P|Qiyprozu&Xe`h%26zQcnWUNyqaPfxUc;g!JE1>t7R)Sj;McQFn^` z@6aytszQc_<9=eC#fw80mlu)lIDcOx{(c`TIi|{AA&&*#hDwpTRdGE*>p!iu{jbK9 zfe2fUNI<{*w~5cyb3DAb11d?-aJu}UZt2jejw{kwo0$DpCaq&Ob;^_gU7oZL|Ump~{LiI|SZ zAt^WbpMX%2kEB-2#tZNOXWcVKl8#i6n2E;_YLP7|_s>kaNMz*|S==V?GDS9w;27|S z>gy-vr|eOb5;FiQI6An$kFa(C^Ed@Q{2+lD0*3%Z2IB{~ou%eqCO|1;-X?ICz&Qd} z2)sk!9)Sl0o)I7?Mk!R%rAoCCWk=ZS3{@8Zko<)}B5`>YtfYlq0OyE0OQ?>7m)K#>F~z95ODM7?6!k}Oj%N)%;_vMocF*AR9|g2IEo3s@!s z+H#^9ON~;=lcrMYq*YsIO2?kD(xem5B-1LH%v5>IWDzg}WGg3XqfW=aN{jL+_OG6^ z4=e~la@(2y=*m2}d+)iAz4x5&o^$T?FI+A=1rPu1;mOt^iuwb-XitNhSpGRpQI{!> z;^;UPR(|O)Ex#MW2KY6^jR_{qBursb!W=dyEMW_ce8#vnVGG;jG!wTc9AQVo8FnUI zVV9g|in|k@ut!dt`T;z>*RM!yguO%`x6b}2Kn6@Z%j0Wo8+`D-kfL&x5#OG zJdkJ&x5{Zpd{v??+$N`;@%BVVxFZn^2NRv)&O}$ZE3rDfS}t?NyAwU(9!R^xy__f9 z2fzKC_be3-aXz4HxH_O~4b(o0t3OL|{&x+^T!h!jc@2=)Sk4=e^O_*9nP11XZ~?wG zM)R%ezfOKN{A=Yq_#oefd0gu^UGJ@W*Q~ZUDA#U-+U-!ggE#SJ-ogdHY2-S2CPr6q z4#}Kdz_}VYySW~|Cq{F<*jokHdae(~zJcopI?RQDZsgYReUk=m?O8g!X^sx9`v|)p z4>6KwEGDE!Qpq$)#p65?B=5xOSUNhbXG!j{RD`Q|_dcDBugD3}l5Z@MNk*r4^Foxw zX4A1$GGvq*cJtAAgz!q)E;5x#@X55~-t!6{&7}D!Nh-<3VP=!W3NcwimZug7%Y5@;|L$&=8mS0@`JC6X{5rmzm8$ zoS;JH@$9;1Be670MQ}1jf&x!ojz#%kUysli1hJeBreg^{l}QJCCWXPl!M)j*kyIwm z1(T_CknoY{v|i;izfvWZELTcR2DAOa;6x^hayOZY$LE64R5Hm&(>xbUr}WCuA1tUV z3T>u&HY%^YWQnJyrsP$MB_~so_cV-@g<-PlFeTfbSE9Tk{3eM>f>cOWfxOHnBMCK? zo}1+*i~13gS!q}>!9)h}`RwVT(yhmNF#;UR%Uhn`NxUIx{S#93K(1op_` ziD7m(!Q7z?VF?N$%PH?^5ZWOmY&4Zfq>_WPbCQp(9xDk!pS$5tmr2UBoLP|0Kb~l@c70p+godo;*a3JF4Zlsv>EF`NpmT2?CKkE98JD zqziIGCbAj}9ZCy|xN0vbv7{G@NgohhR7pQp@#=!5R3>0SP=}E-2ILHN)75gN`>JE% zc)|5Zg-q?a{zdWV@kQo&S*Y6bmbNQX1xxQjPu?;FLRE4PESy?6A#NMLar#C~bRJta z8a(zT%3!nKwNfUhBAC0bKf1^qE}>wNHPBnM407;-0EX|&aoKXw1#3p#$DyqJAOO|^ z#+;!76zQ6vXwcwp-lzlOjBHVro0k(H&_)fA0q%@5u9fBZ_O6=s`linrui@eK|GWK6 zP-%}g3Iy?5bD#kG3<^@!qm6bY=!Z<>*@2NrJdPl)0k{Hm74lE`mok7+a?WQB!Qc@- z$^)EnfZ{?Xp57Mh5wdN22!T!D^mHuF2cuOj_I+mT+ZGJjbm1o5Fh0qSjBG@hOGYIV ziN*o_0>W%8$!3!2Se%{Aq%#B%J(ZY^^ROYCmHuQskZgPzzZqF2WSN&N>R=_aQd?l4 zBZO;32VAV&rTVF48x%edf5KORfC6YR`1tkSe;)erkT?-uWR90y^+i{E-i27|>O;k_ z6`Afl(|z^m!j?s5_?E@>#{BE^Uw%!rbltRiE;`RU#fEhYlkX?qNxYj{wC*T*8ZOSC zpa05hqP6|DyGH}VXai!_*vCe9F}V$`JZ95qRE#0OuB5aK&w@?@ioWfOrXuUa`MYZydRb1*Zs`c(Ll&v&zV%{R*-%+sqac<9Qw*#Q zRkQ`k6if37LAD*Ft{N3oM%7>vBy(n#L$kN_!MsAqEHMHfpH%iBkJhJZBPm-?H%v3> z0V1I7i>jw7pOs~?2Fqpx_w_NTm7zcg*MXd&mdsRB>pA9vW64eRtu6L$&-ZRG_KxIx zM+&`r?m5g|_K)q9Z}lbXmHs6gY8cZoTE893!UUlGBtx9d1XB^gC0_>1}V71JzSIjH?5boG+oxmhku2p z85R5w9hb(B-?p1N0allcG53AhO@M8FI?zgbZvyWDQlyg4l(1O z(L>Qn{6lvNEw$xKKq}X+Hcmorc{ZSe`sC;qNO3YXAeKFt$AFwOY79cFkH0wp{G1VG zMEe?mdgD03!G44WdWeyCIkFSd<%qJM!;U};k_%lcw(R*Z5i$i>aOyPvuG~C7 zvpyD&Bu;XX9ofwf)L$WU62b_Az_Npn#Z$1^3OgX7RKrbHI0)nn^-EXD-*9>VoBQFX z=`xabz|CVbn<^7M8{JSpgs?IF>cjf)Nu5S|kM{aD%`;RPJm{Gd!3?=WA?@)l2 zCLsgXFKluO$mjzhyYSN=N3t7;#Ka+rB3T3;y!uzga-JK)}Q2jBIj{ry(nQKVoZur;l?U7=?cW@<*^ltgiEx zytOlLU3+!!udQnVpvLIu>Egji{$QkVFnZ5u=(7KI^S*1xK6vHa=oRDn!&j#B&fW!U z-nky^Z@@Vt;+z#~9i@M@<7Ybx2aiLYz(eW;pw0z1nCz?j#J;V?zU}$G?S;Obg|5ec zy6J}XSMHy=3t**;JeTi!?hfU!x%1BUbNeq%msa&G7{76NVLHER%eh0ho%JOz{MTIS z-+YH+e085#tSQK zW7^5GFJ&TewPe-N(L;L zU8X1x0?&qMB3pi_BabU9^%$}uL3c>#1oFqT)UEEv#RD%a8+|7GQZKb;-3KRc4Bw*~ zOt7Z_k;PB069xA2qd~=T7i3jhY5+WFr!fFO6j*CUpyoLS!WW)hvh%51W|eqc4k8_u zo2#q(SVW zWselyOsV66O(^~bIJSF$Pch{OFg1t-z1b>9R$kIQO(4!IYiF8E^HNWy86yMlMs~j50IY zfkFl2yJ{Mo-iLpe0pgMmssG>YX9Bim*v_)O6MT|GCv_?niDUFn5x>e<4Z5LZ1B@@^ zAQ&f;n0>%o;c`wvCPCGfJOZ@FVpEFqNj728W+anHwgA!O@kz`;)R!2nKpuf)l|V|i za(mg9YOyRAeG4nT2?XTQ(|aym^mOJuokdS?-qX9lh@P$2%z4k&bIfhch1~u@y14VH z{LZJuiDwtB$4b72qOT|K>nZv+=Y5;6aRuL_=ghaQV9568t^EuB_gmj-y*9aM-45Zq zi`&m{|H@9$(RHo(ZhFF9m=oQ@0_arZO%-Qdizca0dbG*dSB3XPh%G4&hk zGq)TKMaQbVV^zV?E;`m!sMVU{@JN1mq%gdvI6Rgg9$R!CD%H0Y>$~&y-G%zzV*NnA zen4~%+^%aa)eT?URNS~Hzj2Q^y!U$a#+u@R7xD*QC>(fE9F2(HlVG~lyCHPyQVa_S z0+16heDVSk48}J~7M%j#E=8WJ<)a{N4(M*6()~%vi(Ibx5IiVki-jbHm z#xswbcjY~*GY=E6wnTF_MsOw#3TdZxzKdVxzN8DHf&~`i~&ow0}hK*guszqP(Jnwf?dyV16-L^PVutlb+DnQ zgCVEnV58G~^pqT)m8`R=(>!6}aM*Q{L{9MlE^$60@DhU{1)g4<=Vv9itmyJ_8n!Fh zm7_H|%^)QRoZ#+0Jw84MUBD^Va4o{=aJbM%!B*i`Gy@Ua@6uzGZEpWnj@dc-!i{Xg_biwBds5 zRzpj%p+DcyUual!&aU$Z7OmYSgqvXA5iB}-@{XSOT8g1<`OvmsJGR|K7}@askb*km zhTYfq-q=<=_+tLxi^YR{{vaA5AKj z{g71w`9|4yHO?6T?gRQAlS7dtT+tvp?4uNE52o>N`TIaFQ?>^{w+17%ys3Gx%pknC z8jlaat6;RI_1_$Q3U)eyzEBV4H7!lG9Ig}1QOxI9o4Q=n)P-Xp*D{zv=-Z2dBRG!I z@88(<7q~-!HwR4e+>;y(j$lAXbt5E?&Z(T)Iza$%4VATW=*0s0GpJYXV$_!pgLTgA z;7EBa)r&A#ga1FV-DiP-*0}3Rf%alxC?6Oq1U3`{kL3f8ojX`+58VOlqxF)#`2&|;x^QU8MS1E~m7^sy zT8J<6VA)kE-(NuI)q0065kd0r@F$?>ahAI4x0nKo%3(>ien|gnHlfHH+*$Go^7W^R zC6PR`%IMsZ4|Na}OV1)-gC?W!SAMiTd>OS~7kIdP1=o;9Qxl+08KErS={l274J{iG!)WwI%sgeBRQp;(FqV^~H~a@!fa?e7tLA1U~^mHaIw ze_zSpy<{`_>XwZbmwU-exx6K(ujH&>YI3^lO99H=TJkjA?eIFyOO)4SzJKz|^8a(_ zu5$7}XTr+{x~vQ3yKm8QYGA?z~k&z6FJYstT=-tIfXTr(8{9 zbN><^V*H2QbH3**eRo@2&1RAD-)*Oypt_v}>*}|^__y=omc74p{lX*m9?M(tG7?IU zL6sh|A39xel83@A>+cdIDn+P5e+>^8Cp~zzLX(-P`vnW`HE?EKV02FrHJme}wsW>` z+G+)NHDP(rUKTy0E?#;tN#jeS_ai@APR=-Gs1h`Xf+OS~fsyvIOK?xxu^iOhhBn<8^sZHVH4zeX}V2-$UP9-%{W zs3Hh=MYB>s-;`N6dOHom!>VB@saZROQ`*-#CD%0E_>DtM2#<+)sYz|IS}>(X1q1J^ zZ%a>t$&43@$&X=3A-e)~H55TI3TaYH8vq`Eg`b45qkNZBQ9I>cXH~Q+sH=s1?UTJv zCw>j>Jcc^)1|-f<5QOq{X~0>(R?&}}zD}`gW5KuS+O+uiGotrd5N%Wr29(1|we*yl z+DolnH`fe^gZm3>Moax;rMA_jzQI!OK&hqc<9d6*e$P)iyt3wW`uyfIV|RCe;w-jp zDYSjzM+bj+=*DB>qsQ`X#|n<;MCLgeZ9|)t0J?0?Zi2x{t@=u6mKd85ri4K_LMEt4 zgh7IV7=os_EL8--L~^SrrR)`gyqPOI49>mOsTOPNsapc9Am1PtWSo-xDj%GjmhX!T z${M0FOU-HnBn5+7!Gn7kL4=bURUTXkI93?chJ;WjIpwQpYMy*6Q*sJvu++%Q%4cGH zMrv4r!}1gWQDP1?%5Zx8*guAFNF$OaBp6nc9MD#I5?e4kg5*Uc86+VjLrAtE!9z+V zu7@6}Y(2~Nrh?bF3Cl1`P_C-%Z8*gTwSm*s9praV3PMK-TYxSZX_{XC0!4fNg>w9X za^5qTXamFuX~QQ@itdqLKbBv;6g@`&o;vV*YX67S;D=Pthg9%GYVAFP8F(oqZi;Td z)zl(V{!&MmNUbV0Hj7mK9kZ3LyVGa~60p-R(Dwo%8ctDx+&f0E#w3utV{{+=Xo+bk zxjS!J>n`mBz;84|45P8F)DkRpg-U_WQlPig)>&#^g(BZRSZe7nwXQ9Bo8Z5XTitH+ zCoX8>?orxD2jqrU-)b8uwT4P<10P!$Xv|J|IP^q!~s!43< vExEg|PKmC;Pi#Y-)XO>&3D$-(dMOfl*cCyjNUbN!~awxE6INW|2HWc diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 8182b0c03..0fa2f2c50 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -41,7 +41,7 @@ from controller_manager_msgs.srv import SwitchController from rclpy.node import Node from sensor_msgs.msg import JointState -from trajectory_msgs.msg import JointTrajectory as JointTrajectory, JointTrajectoryPoint +from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from ur_msgs.msg import IOStates sys.path.append(os.path.dirname(__file__)) From 95173afa8dbfbffe438c0db2b0d87bf4be320aae Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 15 Jul 2024 10:54:32 +0000 Subject: [PATCH 08/50] Fix test_common --- ur_robot_driver/test/test_common.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/ur_robot_driver/test/test_common.py b/ur_robot_driver/test/test_common.py index 146a6dcbb..ce06e1f90 100644 --- a/ur_robot_driver/test/test_common.py +++ b/ur_robot_driver/test/test_common.py @@ -335,17 +335,7 @@ def generate_driver_test_description( [FindPackageShare("ur_robot_driver"), "launch", "ur_control.launch.py"] ) ), - launch_arguments={ - "robot_ip": "192.168.56.101", - "ur_type": ur_type, - "launch_rviz": "false", - "controller_spawner_timeout": str(controller_spawner_timeout), - "initial_joint_controller": "scaled_joint_trajectory_controller", - "headless_mode": "true", - "launch_dashboard_client": "false", - "start_joint_controller": "false", - "tf_prefix": tf_prefix, - }.items(), + launch_arguments=launch_arguments.items(), ) wait_dashboard_server = ExecuteProcess( cmd=[ From 4c9d9b65126e2f63ff0f73f40e8bf0af508be287 Mon Sep 17 00:00:00 2001 From: URJala Date: Tue, 3 Sep 2024 02:05:29 +0000 Subject: [PATCH 09/50] Move functionality to update method Moved functionality of passthrough controller from separate execute loop in to controller's update method. Also changed how the controller checks validity of a received trajectory, and how it communicates that to the hardware interface. --- .../passthrough_trajectory_controller.hpp | 41 +-- .../src/passthrough_trajectory_controller.cpp | 276 ++++++++++-------- .../ur_robot_driver/hardware_interface.hpp | 7 +- ur_robot_driver/src/hardware_interface.cpp | 46 +-- 4 files changed, 188 insertions(+), 182 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 468201f11..a62d6e729 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -71,21 +71,17 @@ enum CommandInterfaces 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 robot driver. - 4.0: The robot is moving through the trajectory. - 5.0: The robot finished executing the trajectory. */ + 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. */ PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0, - /* The PASSTHROUGH_TRAJECTORY_CANCEL value is used to indicate whether the trajectory has been cancelled from the + /* The PASSTHROUGH_TRAJECTORY_ABORT value is used to indicate whether the trajectory has been cancelled from the * hardware interface.*/ - PASSTHROUGH_TRAJECTORY_CANCEL = 1, - /* The PASSTHROUGH_TRAJECTORY_DIMENSIONS is used to indicate what combination of joint positions, velocities and - * accelerations the trajectory consists of, see check_dimensions() for a description of what the values mean. */ - PASSTHROUGH_TRAJECTORY_DIMENSIONS = 2, + PASSTHROUGH_TRAJECTORY_ABORT = 1, /* Arrays to hold the values of a point. */ - PASSTHROUGH_TRAJECTORY_POSITIONS_ = 3, - PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 9, - PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 15, - PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 21 + PASSTHROUGH_TRAJECTORY_POSITIONS_ = 2, + PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 8, + PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 14, + PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 20 }; enum StateInterfaces @@ -93,8 +89,7 @@ enum StateInterfaces /* State interface 0 - 17 are joint state interfaces*/ SPEED_SCALING_FACTOR = 18, - NUMBER_OF_JOINTS = 19, - CONTROLLER_RUNNING = 20 + CONTROLLER_RUNNING = 19 }; class PassthroughTrajectoryController : public controller_interface::ControllerInterface @@ -138,26 +133,15 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI void goal_accepted_callback( const std::shared_ptr> goal_handle); - void execute( - const std::shared_ptr> goal_handle); - std::vector joint_names_; std::vector state_interface_types_; std::vector> joint_position_state_interface_; std::vector> joint_velocity_state_interface_; - /* - If there are values in the velocities and accelerations vectors, they should have as many elements as there are - joints, and all be the same size. - The return value indicates what combination of joint positions, velocities and accelerations is present; - 0: The trajectory is invalid, and will be rejected. - 1: Only positions are defined for the trajectory. - 2: Positions and velocities are defined for the trajectory. - 3: Positions and accelerations are defined for the trajectory. - 4: Both positions, velocities and accelerations are defined for the trajectory. - */ - int check_dimensions(std::shared_ptr goal); + bool check_positions(std::shared_ptr goal); + bool check_velocities(std::shared_ptr goal); + bool check_accelerations(std::shared_ptr goal); trajectory_msgs::msg::JointTrajectory active_joint_traj_; std::vector path_tolerance_; @@ -174,6 +158,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI double scaling_factor_; uint32_t number_of_joints_; std::shared_ptr> active_goal_; + static constexpr double NO_VAL = std::numeric_limits::quiet_NaN(); }; } // namespace ur_controllers #endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 092d6aa5e..4514fb113 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -87,7 +87,6 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st } } conf.names.emplace_back(passthrough_params_.speed_scaling_interface_name); - conf.names.emplace_back(tf_prefix + "passthrough_controller/number_of_joints"); conf.names.emplace_back(tf_prefix + "passthrough_controller/running"); return conf; @@ -102,9 +101,7 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_cancel"); - - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_dimensions"); + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_abort"); for (size_t i = 0; i < 6; ++i) { config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_positions_" + @@ -145,6 +142,85 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& time, const rclcpp::Duration& /* period */) { + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { + /* Check if the trajectory has been aborted from the hardware interface. E.g. the robot was stopped on the teach + * pendant. */ + if (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "Trajectory cancelled from hardware interface, aborting action."); + std::shared_ptr result = + std::make_shared(); + active_goal_->abort(result); + end_goal(); + return controller_interface::return_type::OK; + } + /* Check if the goal has been cancelled by the ROS user. */ + if (active_goal_->is_canceling()) { + std::shared_ptr result = + std::make_shared(); + active_goal_->canceled(result); + end_goal(); + return controller_interface::return_type::OK; + } + // Write a new point to the command interface, if the previous point has been read by the hardware interface. + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { + if (current_point_ < active_joint_traj_.points.size()) { + // Write the time_from_start parameter. + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( + active_joint_traj_.points[current_point_].time_from_start.sec + + (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); + // Write the positions for each joint of the robot + for (int i = 0; i < 6; i++) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( + active_joint_traj_.points[current_point_].positions[i]); + // Optionally, also write velocities and accelerations for each joint. + if (active_joint_traj_.points[current_point_].velocities.size() > 0) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value( + active_joint_traj_.points[current_point_].velocities[i]); + } else { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_].set_value(NO_VAL); + } + + if (active_joint_traj_.points[current_point_].accelerations.size() > 0) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value( + active_joint_traj_.points[current_point_].accelerations[i]); + } else { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value(NO_VAL); + } + } + // Tell hardware interface that this point is ready to be read. + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); + current_point_++; + + // Check if all points have been written to the hardware interface. + } else if (current_point_ == active_joint_traj_.points.size()) { + RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " + "execute!"); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(3.0); + } + // When the trajectory is finished, report the goal as successful to the client. + } else if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 5.0) { + std::shared_ptr result = + std::make_shared(); + // 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_->abort(result); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); + // Check if the goal time tolerance was complied with. + } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds())) { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; + result->error_string = "Goal not reached within time tolerance"; + active_goal_->abort(result); + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal time tolerance not met."); + } else { + result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; + active_goal_->succeed(result); + RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); + } + end_goal(); + } + } static bool firstpass = true; if (firstpass) { now_ns = time.nanoseconds(); @@ -190,71 +266,102 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb return rclcpp_action::GoalResponse::REJECT; } - // Check dimensions of the trajectory - if (check_dimensions(goal) == 0) { - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, dimensions of trajectory are incorrect."); + // Check that all parts of the trajectory are valid. + if (!check_positions(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_velocities(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); + return rclcpp_action::GoalResponse::REJECT; + } + if (!check_accelerations(goal)) { + RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); return rclcpp_action::GoalResponse::REJECT; - } else { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_DIMENSIONS].set_value( - static_cast(check_dimensions(goal))); } return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -int PassthroughTrajectoryController::check_dimensions( +bool PassthroughTrajectoryController::check_positions( std::shared_ptr goal) { for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { - std::string msg; 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(), 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(), msg.c_str()); - return 0; + return false; } - if ((goal->trajectory.points[i].velocities.size() != 0 && - goal->trajectory.points[i].velocities.size() != number_of_joints_) || - goal->trajectory.points[i].velocities.size() != goal->trajectory.points[0].velocities.size()) { - msg = "Can't accept new trajectory. All trajectory points must have velocities for all joints of the robot. (" + + } + return true; +} + +bool PassthroughTrajectoryController::check_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(), 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(), msg.c_str()); - return 0; + return false; } - if ((goal->trajectory.points[i].accelerations.size() != 0 && - goal->trajectory.points[i].accelerations.size() != number_of_joints_) || - goal->trajectory.points[i].accelerations.size() != goal->trajectory.points[0].accelerations.size()) { - msg = "Can't accept new trajectory. All trajectory points must have accelerations for all joints of the robot. " + 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(), 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(), msg.c_str()); + return false; + } + } + return true; +} + +bool PassthroughTrajectoryController::check_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(), msg.c_str()); - msg = "Point nr " + std::to_string(i + 1) + + msg = "Point nr " + std::to_string(i) + " has: " + std::to_string(goal->trajectory.points[i].accelerations.size()) + " accelerations."; RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); - return 0; + 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(), 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(), msg.c_str()); + return false; } } - if (goal->trajectory.points[0].velocities.size() == 6 && goal->trajectory.points[0].accelerations.size() == 0) { - /*If there are only positions and velocities defined */ - return 2; - } else if (goal->trajectory.points[0].velocities.size() == 0 && - goal->trajectory.points[0].accelerations.size() == 6) { - /*If there are only positions and accelerations defined */ - return 3; - } else if (goal->trajectory.points[0].velocities.size() == 6 && - goal->trajectory.points[0].accelerations.size() == 6) { - /*If there are both positions, velocities and accelerations defined */ - return 4; - } else { - /*If there are only positions defined */ - return 1; - } + return true; } // Called when the action is cancelled by the action client. @@ -275,7 +382,7 @@ void PassthroughTrajectoryController::goal_accepted_callback( active_trajectory_elapsed_time_ = 0; current_point_ = 0; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_CANCEL].set_value(0.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ABORT].set_value(0.0); active_joint_traj_ = goal_handle->get_goal()->trajectory; goal_tolerance_ = goal_handle->get_goal()->goal_tolerance; @@ -288,95 +395,6 @@ void PassthroughTrajectoryController::goal_accepted_callback( command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(1.0); - /* Start the executing thread of the action server, this will run until the trajectory is finished or cancelled. */ - std::thread{ std::bind(&PassthroughTrajectoryController::execute, this, std::placeholders::_1), goal_handle } - .detach(); - return; -} - -void PassthroughTrajectoryController::execute( - const std::shared_ptr> goal_handle) -{ - /* While loop should execute 200 times pr second. Can be made slower if needed, but it will affect how fast the points - * of the trajectory are transferred to the hardware interface. */ - rclcpp::Rate loop_rate(200); - while (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { - /* Check if the trajectory has been cancelled from the hardware interface. E.g. the robot was stopped on the teach - * pendant. */ - if (command_interfaces_[PASSTHROUGH_TRAJECTORY_CANCEL].get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "Trajectory cancelled from hardware interface, aborting action."); - std::shared_ptr result = - std::make_shared(); - goal_handle->abort(result); - end_goal(); - return; - } - /* Check if the goal has been cancelled by the ROS user. */ - if (goal_handle->is_canceling()) { - std::shared_ptr result = - std::make_shared(); - goal_handle->canceled(result); - end_goal(); - return; - } - // Write a new point to the command interface, if the previous point has been read by the hardware interface. - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { - if (current_point_ < active_joint_traj_.points.size()) { - // Write the time_from_start parameter. - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( - active_joint_traj_.points[current_point_].time_from_start.sec + - (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); - // Write the positions for each joint of the robot - for (int i = 0; i < 6; i++) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( - active_joint_traj_.points[current_point_].positions[i]); - // Optionally, also write velocities and accelerations for each joint. - if (active_joint_traj_.points[current_point_].velocities.size() > 0) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value( - active_joint_traj_.points[current_point_].velocities[i]); - } - - if (active_joint_traj_.points[current_point_].accelerations.size() > 0) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value( - active_joint_traj_.points[current_point_].accelerations[i]); - } - } - // Tell hardware interface that this point is ready to be read. - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); - current_point_++; - - // Check if all points have been written to the hardware interface. - } else if (current_point_ == active_joint_traj_.points.size()) { - RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " - "execute!"); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(3.0); - } - // When the trajectory is finished, report the goal as successful to the client. - } else if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 5.0) { - std::shared_ptr result = - std::make_shared(); - // 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."; - goal_handle->abort(result); - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); - // Check if the goal time tolerance was complied with. - } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds())) { - result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; - result->error_string = "Goal not reached within time tolerance"; - goal_handle->abort(result); - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal time tolerance not met."); - } else { - result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; - goal_handle->succeed(result); - RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); - } - end_goal(); - return; - } - loop_rate.sleep(); - } return; } 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 94179be03..d007e0383 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -138,6 +138,8 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface 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_; @@ -197,15 +199,12 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // Passthrough trajectory controller interface values double passthrough_trajectory_transfer_state_; - double passthrough_trajectory_cancel_; - double passthrough_trajectory_dimensions_; + double passthrough_trajectory_abort_; double 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_; - double number_of_joints_; - // payload stuff urcl::vector3d_t payload_center_of_gravity_; double payload_mass_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 3ff309955..c36c83f95 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -95,11 +95,10 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys async_thread_shutdown_ = false; system_interface_initialized_ = 0.0; passthrough_trajectory_transfer_state_ = 0.0; - passthrough_trajectory_cancel_ = 0.0; + passthrough_trajectory_abort_ = 0.0; trajectory_joint_positions_.clear(); trajectory_joint_velocities_.clear(); trajectory_joint_accelerations_.clear(); - number_of_joints_ = info_.joints.size(); for (const hardware_interface::ComponentInfo& joint : info_.joints) { if (joint.command_interfaces.size() != 2) { @@ -237,9 +236,6 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); - state_interfaces.emplace_back( - hardware_interface::StateInterface(tf_prefix + "passthrough_controller", "number_of_joints", &number_of_joints_)); - state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + "passthrough_controller", "running", &passthrough_trajectory_controller_running_)); @@ -315,10 +311,7 @@ std::vector URPositionHardwareInterface::e &passthrough_trajectory_transfer_state_)); command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "passthrough_controller", "passthrough_trajectory_cancel", &passthrough_trajectory_cancel_)); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "passthrough_controller", "passthrough_trajectory_dimensions", &passthrough_trajectory_dimensions_)); + tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_)); for (size_t i = 0; i < 6; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -919,7 +912,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod } else if (stop_modes_.size() != 0 && std::find(stop_modes_.begin(), stop_modes_.end(), StoppingInterface::STOP_PASSTHROUGH) != stop_modes_.end()) { passthrough_trajectory_controller_running_ = 0.0; - passthrough_trajectory_cancel_ = 1.0; + passthrough_trajectory_abort_ = 1.0; trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); @@ -962,36 +955,35 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller() trajectory_times_.push_back(passthrough_trajectory_time_from_start_ - last_time); last_time = passthrough_trajectory_time_from_start_; - if (passthrough_trajectory_dimensions_ == 2.0 || passthrough_trajectory_dimensions_ == 4.0) { + if (!std::isnan(passthrough_trajectory_velocities_[0])) { trajectory_joint_velocities_.push_back(passthrough_trajectory_velocities_); } - if (passthrough_trajectory_dimensions_ == 3.0 || passthrough_trajectory_dimensions_ == 4.0) { + 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 robot driver.*/ + /* When all points have been read, write them to the physical robot controller.*/ } else if (passthrough_trajectory_transfer_state_ == 3.0) { - /* Tell robot driver how many points are in the trajectory. */ + /* 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 according to the dimensions of the trajectory. See - * passthrough_trajectory_controller.hpp - check_dimensions() for an explanation of the values. */ - if (passthrough_trajectory_dimensions_ == 1.0) { + /* 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 (passthrough_trajectory_dimensions_ == 2.0) { + } 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 (passthrough_trajectory_dimensions_ == 3.0) { + } 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 (passthrough_trajectory_dimensions_ == 4.0) { + } 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]); @@ -1000,6 +992,8 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller() trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); trajectory_joint_velocities_.clear(); + trajectory_times_.clear(); + last_time = 0.0; passthrough_trajectory_transfer_state_ = 4.0; } } @@ -1009,11 +1003,21 @@ void URPositionHardwareInterface::trajectory_done_callback(urcl::control::Trajec if (result == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) { passthrough_trajectory_transfer_state_ = 5.0; } else { - passthrough_trajectory_cancel_ = 1.0; + passthrough_trajectory_abort_ = 1.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" From 67e5c0df4ed5f7f9be394ed51d41ca1cb42b7a4c Mon Sep 17 00:00:00 2001 From: URJala Date: Wed, 18 Sep 2024 00:20:15 +0000 Subject: [PATCH 10/50] Removing examples from this PR --- examples/passthrough_example.py | 69 -------- examples/robot_class.py | 294 ------------------------------- examples/scaled_joint_example.py | 51 ------ ur_robot_driver/CMakeLists.txt | 3 - 4 files changed, 417 deletions(-) delete mode 100644 examples/passthrough_example.py delete mode 100644 examples/robot_class.py delete mode 100644 examples/scaled_joint_example.py diff --git a/examples/passthrough_example.py b/examples/passthrough_example.py deleted file mode 100644 index 954f9659c..000000000 --- a/examples/passthrough_example.py +++ /dev/null @@ -1,69 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2024, Universal Robots -# -# 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 rclpy -from builtin_interfaces.msg import Duration -from rclpy.node import Node -from robot_class import Robot - - -if __name__ == "__main__": - rclpy.init() - node = Node("robot_driver_test") - robot = Robot(node) - - # The following list are arbitrary joint positions, change according to your own needs - waypts = [ - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], - [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], - ] - # Velocities and accelerations can be omitted - vels = [ - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - ] - accels = [ - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - [0.1, 0.1, 0.1, 0.1, 0.1, 0.1], - ] - time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] - - # Execute trajectory on robot, make sure that the robot is booted and the control script is running - # Make sure that the pasthrough controller is loaded and activated - robot.load_controller("passthrough_trajectory_controller") - robot.switch_controllers( - ["passthrough_trajectory_controller"], ["scaled_joint_trajectory_controller"] - ) - # Trajectory using positions, velocities and accelerations - robot.passthrough_trajectory(waypts, time_vec, vels, accels) - # Trajectory using only positions - robot.passthrough_trajectory(waypts, time_vec) diff --git a/examples/robot_class.py b/examples/robot_class.py deleted file mode 100644 index 70146c066..000000000 --- a/examples/robot_class.py +++ /dev/null @@ -1,294 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2024, Universal Robots -# -# 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 rclpy -from builtin_interfaces.msg import Duration -from control_msgs.action import FollowJointTrajectory - -from rclpy.action import ActionClient -from rclpy.node import Node -from trajectory_msgs.msg import JointTrajectory as JTmsg, JointTrajectoryPoint -from ur_msgs.srv import SetIO -from controller_manager_msgs.srv import ( - UnloadController, - LoadController, - ConfigureController, - SwitchController, - ListControllers, -) -from std_srvs.srv import Trigger -from enum import Enum -from collections import namedtuple - -TIMEOUT_WAIT_SERVICE = 10 -TIMEOUT_WAIT_SERVICE_INITIAL = 60 -TIMEOUT_WAIT_ACTION = 10 - -ROBOT_JOINTS = [ - "shoulder_pan_joint", - "shoulder_lift_joint", - "elbow_joint", - "wrist_1_joint", - "wrist_2_joint", - "wrist_3_joint", -] -Action_tuple = namedtuple("Actions", ["name", "action_type"]) - - -class Actions(Enum): - PASSTHROUGH_TRAJECTORY = Action_tuple( - "/passthrough_trajectory_controller/forward_joint_trajectory", FollowJointTrajectory - ) - FOLLOW_TRAJECTORY = Action_tuple( - "/scaled_joint_trajectory_controller/follow_joint_trajectory", FollowJointTrajectory - ) - - -Service_tuple = namedtuple("Services", ["name", "service_type"]) - - -class Services(Enum): - Set_IO = Service_tuple("/io_and_status_controller/set_io", SetIO) - Load_Controller = Service_tuple("/controller_manager/load_controller", LoadController) - Unload_Controller = Service_tuple("/controller_manager/unload_controller", UnloadController) - Configure_Controller = Service_tuple( - "/controller_manager/configure_controller", ConfigureController - ) - Switch_Controller = Service_tuple("/controller_manager/switch_controller", SwitchController) - List_Controllers = Service_tuple("/controller_manager/list_controllers", ListControllers) - - -# Helper functions -def waitForService(node, srv_name, srv_type, timeout=TIMEOUT_WAIT_SERVICE): - client = node.create_client(srv_type, srv_name) - if client.wait_for_service(timeout) is False: - raise Exception(f"Could not reach service '{srv_name}' within timeout of {timeout}") - - node.get_logger().info(f"Successfully connected to service '{srv_name}'") - return client - - -def waitForAction(node, action_name, action_type, timeout=TIMEOUT_WAIT_ACTION): - client = ActionClient(node, action_type, action_name) - if client.wait_for_server(timeout) is False: - raise Exception( - f"Could not reach action server '{action_name}' within timeout of {timeout}" - ) - - node.get_logger().info(f"Successfully connected to action '{action_name}'") - return client - - -class Robot: - def __init__(self, node): - self.node = node - self.init_robot() - - def init_robot(self): - - self.service_clients = { - service.name: waitForService(self.node, service.value.name, service.value.service_type) - for (service) in Services - } - - self.action_clients = { - action.name: waitForAction(self.node, action.value.name, action.value.action_type) - for (action) in Actions - } - - def add_service(self, dict_key: str, service_tuple: Service_tuple): - self.service_clients.update( - {dict_key: waitForService(self.node, service_tuple.name, service_tuple.service_type)} - ) - - def add_action(self, dict_key: str, action_tuple: Action_tuple): - self.action_clients.update( - {dict_key: waitForAction(self.node, action_tuple.name, action_tuple.action_type)} - ) - - def set_io(self, pin, value): - """Test to set an IO.""" - set_io_req = SetIO.Request() - set_io_req.fun = 1 - set_io_req.pin = pin - set_io_req.state = value - - self.call_service(Services.Set_IO.name, set_io_req) - - def follow_trajectory(self, waypts, time_vec): - """Send robot trajectory.""" - if len(waypts) != len(time_vec): - raise Exception("waypoints vector and time vec should be same length") - - # Construct test trajectory - joint_trajectory = JTmsg() - joint_trajectory.joint_names = ROBOT_JOINTS - for i in range(len(waypts)): - point = JointTrajectoryPoint() - point.positions = waypts[i] - point.time_from_start = time_vec[i] - joint_trajectory.points.append(point) - - # Sending trajectory goal - goal_response = self.call_action( - self.action_clients[Actions.FOLLOW_TRAJECTORY.name], - FollowJointTrajectory.Goal(trajectory=joint_trajectory), - ) - if goal_response.accepted is False: - raise Exception("trajectory was not accepted") - - # Verify execution - result = self.get_result(self.action_clients[Actions.FOLLOW_TRAJECTORY.name], goal_response) - return result - - def passthrough_trajectory( - self, - waypts: list[float], - time_vec: list[float], - vels: list[float] = [], - accels: list[float] = [], - goal_time_tolerance=Duration(sec=1), - ): - """Send trajectory through the passthrough controller.""" - if len(waypts) != len(time_vec): - raise Exception("waypoints vector and time vec should be same length.") - trajectory = JTmsg() - trajectory.joint_names = ROBOT_JOINTS - for i in range(len(waypts)): - point = JointTrajectoryPoint() - point.positions = waypts[i] - point.time_from_start = time_vec[i] - if len(vels) != 0: - point.velocities = vels[i] - if len(accels) != 0: - point.accelerations = accels[i] - trajectory.points.append(point) - goal_response = self.call_action( - self.action_clients[Actions.PASSTHROUGH_TRAJECTORY.name], - FollowJointTrajectory.Goal( - trajectory=trajectory, goal_time_tolerance=goal_time_tolerance - ), - ) - if goal_response.accepted is False: - raise Exception("trajectory was not accepted") - - # Verify execution - result = self.get_result( - self.action_clients[Actions.PASSTHROUGH_TRAJECTORY.name], goal_response - ) - return result - - def call_service(self, srv_name, request): - future = self.service_clients[srv_name].call_async(request) - rclpy.spin_until_future_complete(self.node, future) - if future.result() is not None: - return future.result() - else: - raise Exception(f"Exception while calling service: {future.exception()}") - - def call_action(self, ac_client, g): - future = ac_client.send_goal_async(g) - rclpy.spin_until_future_complete(self.node, future) - - if future.result() is not None: - return future.result() - else: - raise Exception(f"Exception while calling action: {future.exception()}") - - def get_result(self, ac_client, goal_response): - future_res = ac_client._get_result_async(goal_response) - rclpy.spin_until_future_complete(self.node, future_res) - if future_res.result() is not None: - return future_res.result().result - else: - raise Exception(f"Exception while calling action: {future_res.exception()}") - - def load_controller(self, controller_name: str): - list_response = self.call_service(Services.List_Controllers.name, ListControllers.Request()) - names = [] - # Find loaded controllers - for controller in list_response.controller: - names.append(controller.name) - # Check whether the passthrough controller is already loaded - try: - names.index(controller_name) - except ValueError: - print("Loading controller") - load_request = LoadController.Request(name=controller_name) - self.call_service(Services.Load_Controller.name, load_request) - configure_request = ConfigureController.Request(name=controller_name) - self.call_service(Services.Configure_Controller.name, configure_request) - list_response = robot.call_service( - Services.List_Controllers.name, ListControllers.Request() - ) - names.clear() - # Update the list of controller names. - for controller in list_response.controller: - names.append(controller.name) - else: - print("Controller already loaded") - finally: - print(f"Currently loaded controllers: {names}") - - def switch_controllers(self, active: list[str], inactive: list[str]) -> bool: - switch_request = SwitchController.Request() - switch_request.activate_controllers = active - switch_request.deactivate_controllers = inactive - switch_request.strictness = ( - SwitchController.Request.BEST_EFFORT - ) # Best effort switching, will not terminate program if controller is already running - switch_request.activate_asap = False - switch_request.timeout = Duration(sec=2, nanosec=0) - return self.call_service(Services.Switch_Controller.name, switch_request) - - -if __name__ == "__main__": - rclpy.init() - node = Node("robot_driver_test") - for action in Actions: - print(action.name) - print(action.value.name) - robot = Robot(node) - robot.switch_controllers( - ["passthrough_trajectory_controller"], ["scaled_joint_trajectory_controller"] - ) - robot.add_service( - "Zero_sensor", Service_tuple("/io_and_status_controller/zero_ftsensor", Trigger) - ) - - # The following list are arbitrary joint positions, change according to your own needs - waypts = [ - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], - [-0.1, -2.6998, -1.104, -2.676, -0.992, -1.5406], - [-1, -2.5998, -1.004, -2.676, -0.992, -1.5406], - ] - time_vec = [Duration(sec=4, nanosec=0), Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0)] - - # Execute trajectory on robot, make sure that the robot is booted and the control script is running - robot.passthrough_trajectory(waypts, time_vec) diff --git a/examples/scaled_joint_example.py b/examples/scaled_joint_example.py deleted file mode 100644 index ce1cab3d4..000000000 --- a/examples/scaled_joint_example.py +++ /dev/null @@ -1,51 +0,0 @@ -#!/usr/bin/env python3 -# Copyright 2024, Universal Robots -# -# 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 rclpy -from builtin_interfaces.msg import Duration -from rclpy.node import Node -from robot_class import Robot - -if __name__ == "__main__": - rclpy.init() - node = Node("robot_driver_test") - robot = Robot(node) - - # The following list are arbitrary joint positions, change according to your own needs - 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)] - robot.switch_controllers( - ["scaled_joint_trajectory_controller"], ["passthrough_trajectory_controller"] - ) - # Execute trajectory on robot, make sure that the robot is booted and the control script is running - robot.follow_trajectory(waypts, time_vec) diff --git a/ur_robot_driver/CMakeLists.txt b/ur_robot_driver/CMakeLists.txt index 476f37494..c47a83452 100644 --- a/ur_robot_driver/CMakeLists.txt +++ b/ur_robot_driver/CMakeLists.txt @@ -148,9 +148,6 @@ install(PROGRAMS scripts/wait_for_robot_description scripts/example_move.py scripts/start_ursim.sh - ../examples/robot_class.py - ../examples/scaled_joint_example.py - ../examples/passthrough_example.py DESTINATION lib/${PROJECT_NAME} ) From 95da8692c82f161ee2f8404bb24284aea5c5dab2 Mon Sep 17 00:00:00 2001 From: URJala Date: Wed, 18 Sep 2024 02:14:39 +0000 Subject: [PATCH 11/50] Implemented some realtime boxes Command and state interfaces are now only interacted with within the update method. The update method still triggers some action server callbacks, so it is not threadsafe yet, but i dont know how to trigger the action server without having them in the update method. --- .../passthrough_trajectory_controller.hpp | 19 +++++++++++++- .../src/passthrough_trajectory_controller.cpp | 26 ++++++++++++++----- 2 files changed, 38 insertions(+), 7 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index a62d6e729..f8d391f07 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -46,6 +46,9 @@ #include #include +#include +#include + #include "controller_interface/controller_interface.hpp" #include "rclcpp_action/server.hpp" #include "rclcpp_action/create_server.hpp" @@ -72,7 +75,9 @@ enum CommandInterfaces 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. */ + controller. + 4.0: The robot is moving through the trajectory. + 5.0: The robot finished executing the trajectory. */ PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0, /* The PASSTHROUGH_TRAJECTORY_ABORT value is used to indicate whether the trajectory has been cancelled from the * hardware interface.*/ @@ -92,6 +97,15 @@ enum StateInterfaces CONTROLLER_RUNNING = 19 }; +// Struct to hold data that has to be transferred between realtime thread and non-realtime threads +struct AsyncInfo +{ + double transfer_state = 0; + double abort = 0; + double controller_running = 0; + bool info_updated = false; +}; + class PassthroughTrajectoryController : public controller_interface::ControllerInterface { public: @@ -148,6 +162,9 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI std::vector goal_tolerance_; rclcpp::Duration goal_time_tolerance_ = rclcpp::Duration::from_nanoseconds(0); + realtime_tools::RealtimeBoxBestEffort info_from_realtime_; + realtime_tools::RealtimeBoxBestEffort info_to_realtime_; + uint32_t current_point_; bool trajectory_active_; uint64_t period_ns; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 4514fb113..c99b425a9 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -142,6 +142,14 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& time, const rclcpp::Duration& /* period */) { + AsyncInfo temp = info_to_realtime_.get(); + if (temp.info_updated) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(temp.transfer_state); + command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].set_value(temp.abort); + temp.info_updated = false; + info_to_realtime_.set(temp); + } + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { /* Check if the trajectory has been aborted from the hardware interface. E.g. the robot was stopped on the teach * pendant. */ @@ -233,7 +241,9 @@ controller_interface::return_type PassthroughTrajectoryController::update(const /* Keep track of how long the trajectory has been executing, if it takes too long, send a warning. */ if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) { scaling_factor_ = state_interfaces_[StateInterfaces::SPEED_SCALING_FACTOR].get_value(); + active_trajectory_elapsed_time_ += static_cast(scaling_factor_ * (period_ns / pow(10, 9))); + if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds()) && trajectory_active_) { RCLCPP_WARN(get_node()->get_logger(), "Trajectory should be finished by now. You may want to cancel this goal, " @@ -241,9 +251,13 @@ controller_interface::return_type PassthroughTrajectoryController::update(const trajectory_active_ = false; } } + AsyncInfo info = { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value(), + command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value(), + state_interfaces_[StateInterfaces::CONTROLLER_RUNNING].get_value(), true }; + info_from_realtime_.set(info); return controller_interface::return_type::OK; -} +} // namespace ur_controllers rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callback( const rclcpp_action::GoalUUID& /*uuid*/, @@ -256,12 +270,12 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb return rclcpp_action::GoalResponse::REJECT; } - if (state_interfaces_[StateInterfaces::CONTROLLER_RUNNING].get_value() != 1.0) { + if (info_from_realtime_.get().controller_running != 1.0) { RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, controller not running in hardware interface."); return rclcpp_action::GoalResponse::REJECT; } - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { + if (info_from_realtime_.get().transfer_state != 0.0) { RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. A trajectory is already executing."); return rclcpp_action::GoalResponse::REJECT; } @@ -382,8 +396,6 @@ void PassthroughTrajectoryController::goal_accepted_callback( active_trajectory_elapsed_time_ = 0; current_point_ = 0; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ABORT].set_value(0.0); - active_joint_traj_ = goal_handle->get_goal()->trajectory; goal_tolerance_ = goal_handle->get_goal()->goal_tolerance; path_tolerance_ = goal_handle->get_goal()->path_tolerance; @@ -393,7 +405,9 @@ void PassthroughTrajectoryController::goal_accepted_callback( max_trajectory_time_ = active_joint_traj_.points.back().time_from_start.sec + (active_joint_traj_.points.back().time_from_start.nanosec / pow(10, 9)); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(1.0); + // Communicate to update() method, that the action has been triggered. + AsyncInfo temp = { 1.0, 0.0, 0.0, true }; + info_to_realtime_.set(temp); return; } From a61bd79be96b98b658b0ab80416d746dcf42b052 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 25 Sep 2024 21:37:51 +0200 Subject: [PATCH 12/50] Updated to latest ControllerInterface API --- ur_controllers/src/passthrough_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index c99b425a9..19f435556 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -265,7 +265,7 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb { RCLCPP_INFO(get_node()->get_logger(), "Received new trajectory."); // Precondition: Running controller - if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { + 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; } From caa8941dd801df2846285e282c4dada08561916d Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 25 Sep 2024 21:40:20 +0200 Subject: [PATCH 13/50] Rename controller's action topic to match the JTC This should making it drop in easier --- ur_controllers/src/passthrough_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 19f435556..705fd35de 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -66,7 +66,7 @@ PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& pre void PassthroughTrajectoryController::start_action_server(void) { send_trajectory_action_server_ = rclcpp_action::create_server( - get_node(), std::string(get_node()->get_name()) + "/forward_joint_trajectory", + 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), From 3d461ae127ca41e9ff6e6b6710aaaff2179e4b60 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 25 Sep 2024 21:41:05 +0200 Subject: [PATCH 14/50] Remove duplicate controller entry This gets started by the initial joint_controller --- ur_robot_driver/launch/ur_control.launch.py | 1 - 1 file changed, 1 deletion(-) diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 1796b459e..db8cf834f 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -162,7 +162,6 @@ def controller_spawner(controllers, active=True): "io_and_status_controller", "speed_scaling_state_broadcaster", "force_torque_sensor_broadcaster", - "scaled_joint_trajectory_controller", ] controllers_inactive = [ "forward_position_controller", From 1ab4ed2ad7d6b8e920f70c09facf8bb29093f4ff Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 26 Sep 2024 11:55:20 +0200 Subject: [PATCH 15/50] Rename controller action in tests --- ur_robot_driver/test/robot_driver.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 0fa2f2c50..9197e759f 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -101,7 +101,7 @@ def init_robot(self): ) self._passthrough_forward_joint_trajectory = ActionInterface( self.node, - "/passthrough_trajectory_controller/forward_joint_trajectory", + "/passthrough_trajectory_controller/follow_joint_trajectory", FollowJointTrajectory, ) From e1263a94e2f854af3cd72e53813c0afc5cf21f6e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 27 Sep 2024 11:48:44 +0200 Subject: [PATCH 16/50] Use generate_parameters_library for joints and state interfaces --- .../src/passthrough_trajectory_controller.cpp | 4 ++-- ...ough_trajectory_controller_parameters.yaml | 19 +++++++++++++++++++ 2 files changed, 21 insertions(+), 2 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 705fd35de..37f8da80e 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -49,8 +49,8 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() passthrough_param_listener_ = std::make_shared(get_node()); passthrough_params_ = passthrough_param_listener_->get_params(); current_point_ = 0; - joint_names_ = auto_declare>("joints", joint_names_); - state_interface_types_ = auto_declare("state_interfaces", state_interface_types_); + joint_names_ = passthrough_params_.joints; + state_interface_types_ = passthrough_params_.state_interfaces; return controller_interface::CallbackReturn::SUCCESS; } diff --git a/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml index 83a184650..2a4b87bab 100644 --- a/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml +++ b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml @@ -9,3 +9,22 @@ passthrough_trajectory_controller: 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",]], + } + } From cf7aeaa8122ab112e2cd84ffdad704769244bcc0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 27 Sep 2024 12:05:38 +0200 Subject: [PATCH 17/50] Use constants for transfer state in controller --- .../passthrough_trajectory_controller.hpp | 16 +++++++++++++--- .../src/passthrough_trajectory_controller.cpp | 17 +++++++++++------ 2 files changed, 24 insertions(+), 9 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index f8d391f07..ec2f6f39e 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -42,13 +42,15 @@ #define UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ #include + +#include + +#include +#include #include #include #include -#include -#include - #include "controller_interface/controller_interface.hpp" #include "rclcpp_action/server.hpp" #include "rclcpp_action/create_server.hpp" @@ -66,6 +68,14 @@ namespace ur_controllers { + +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; + enum CommandInterfaces { /* The PASSTHROUGH_TRAJECTORY_TRANSFER_STATE value is used to keep track of which stage the transfer is in. diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 37f8da80e..6a1073570 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -150,7 +150,8 @@ controller_interface::return_type PassthroughTrajectoryController::update(const info_to_realtime_.set(temp); } - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != 0.0) { + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != + 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 (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0) { @@ -170,7 +171,8 @@ controller_interface::return_type PassthroughTrajectoryController::update(const return controller_interface::return_type::OK; } // Write a new point to the command interface, if the previous point has been read by the hardware interface. - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 1.0) { + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == + TRANSFER_STATE_WAITING_FOR_POINT) { if (current_point_ < active_joint_traj_.points.size()) { // Write the time_from_start parameter. command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( @@ -196,17 +198,19 @@ controller_interface::return_type PassthroughTrajectoryController::update(const } } // Tell hardware interface that this point is ready to be read. - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(2.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value( + TRANSFER_STATE_TRANSFERRING); current_point_++; // Check if all points have been written to the hardware interface. } else if (current_point_ == active_joint_traj_.points.size()) { RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " "execute!"); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(3.0); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(TRANSFER_STATE_DONE); } // When the trajectory is finished, report the goal as successful to the client. - } else if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 5.0) { + } else if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == + TRANSFER_STATE_DONE) { std::shared_ptr result = std::make_shared(); // Check if the actual position complies with the tolerances given. @@ -239,7 +243,8 @@ controller_interface::return_type PassthroughTrajectoryController::update(const period_ns = now_ns - last_time_ns; } /* Keep track of how long the trajectory has been executing, if it takes too long, send a warning. */ - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == 4.0) { + if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == + TRANSFER_STATE_IN_MOTION) { scaling_factor_ = state_interfaces_[StateInterfaces::SPEED_SCALING_FACTOR].get_value(); active_trajectory_elapsed_time_ += static_cast(scaling_factor_ * (period_ns / pow(10, 9))); From dc85692f546614d219b26562f5d4304d84ce9f7e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 27 Sep 2024 12:06:33 +0200 Subject: [PATCH 18/50] Use period directly instead of calculating it by hand --- .../passthrough_trajectory_controller.hpp | 3 --- .../src/passthrough_trajectory_controller.cpp | 17 +++++------------ 2 files changed, 5 insertions(+), 15 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index ec2f6f39e..9d6034f02 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -177,9 +177,6 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI uint32_t current_point_; bool trajectory_active_; - uint64_t period_ns; - uint64_t last_time_ns; - uint64_t now_ns; double active_trajectory_elapsed_time_; double max_trajectory_time_; double scaling_factor_; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 6a1073570..f56f5ff99 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -139,8 +139,8 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat return ControllerInterface::on_activate(state); } -controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& time, - const rclcpp::Duration& /* period */) +controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, + const rclcpp::Duration& period) { AsyncInfo temp = info_to_realtime_.get(); if (temp.info_updated) { @@ -233,21 +233,14 @@ controller_interface::return_type PassthroughTrajectoryController::update(const end_goal(); } } - static bool firstpass = true; - if (firstpass) { - now_ns = time.nanoseconds(); - firstpass = false; - } else { - last_time_ns = now_ns; - now_ns = time.nanoseconds(); - period_ns = now_ns - last_time_ns; - } + /* Keep track of how long the trajectory has been executing, if it takes too long, send a warning. */ if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == TRANSFER_STATE_IN_MOTION) { scaling_factor_ = state_interfaces_[StateInterfaces::SPEED_SCALING_FACTOR].get_value(); - active_trajectory_elapsed_time_ += static_cast(scaling_factor_ * (period_ns / pow(10, 9))); + active_trajectory_elapsed_time_ += + static_cast(scaling_factor_ * (static_cast(period.nanoseconds()) / pow(10, 9))); if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds()) && trajectory_active_) { From a050290cb87774a247b4794cadcb69bcbdef5726 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 27 Sep 2024 12:07:07 +0200 Subject: [PATCH 19/50] Change some stdout strings --- ur_controllers/src/passthrough_trajectory_controller.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index f56f5ff99..763c0afbd 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -155,7 +155,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const /* Check if the trajectory has been aborted from the hardware interface. E.g. the robot was stopped on the teach * pendant. */ if (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "Trajectory cancelled from hardware interface, aborting action."); + RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted hardware, aborting action."); std::shared_ptr result = std::make_shared(); active_goal_->abort(result); @@ -381,7 +381,7 @@ rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_ca const std::shared_ptr< rclcpp_action::ServerGoalHandle> /* goal_handle */) { - RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory because cancel callback received."); + RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory requested."); return rclcpp_action::CancelResponse::ACCEPT; } From 569e31980ba36dea83b486fcdaf85d80fea0bded Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 27 Sep 2024 12:07:24 +0200 Subject: [PATCH 20/50] Remove unnecessary check --- ur_controllers/src/passthrough_trajectory_controller.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 763c0afbd..d49bfa93c 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -268,11 +268,6 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb return rclcpp_action::GoalResponse::REJECT; } - if (info_from_realtime_.get().controller_running != 1.0) { - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected, controller not running in hardware interface."); - return rclcpp_action::GoalResponse::REJECT; - } - if (info_from_realtime_.get().transfer_state != 0.0) { RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. A trajectory is already executing."); return rclcpp_action::GoalResponse::REJECT; From c319be7924cbb0cba4655a67c80fdd2cf07695f2 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 27 Sep 2024 12:07:48 +0200 Subject: [PATCH 21/50] Remove an unnecessary comment --- ur_controllers/src/passthrough_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index d49bfa93c..f16ef0285 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -255,7 +255,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const info_from_realtime_.set(info); return controller_interface::return_type::OK; -} // namespace ur_controllers +} rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callback( const rclcpp_action::GoalUUID& /*uuid*/, From 7b5f095620cc7454183ac9cd326259e2c808e998 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Fri, 27 Sep 2024 12:08:36 +0200 Subject: [PATCH 22/50] Do not require a controller active state inteface --- .../passthrough_trajectory_controller.hpp | 1 - .../src/passthrough_trajectory_controller.cpp | 18 ++++++++---------- 2 files changed, 8 insertions(+), 11 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 9d6034f02..de21e73e9 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -104,7 +104,6 @@ enum StateInterfaces /* State interface 0 - 17 are joint state interfaces*/ SPEED_SCALING_FACTOR = 18, - CONTROLLER_RUNNING = 19 }; // Struct to hold data that has to be transferred between realtime thread and non-realtime threads diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index f16ef0285..3be993e7b 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -87,7 +87,6 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st } } conf.names.emplace_back(passthrough_params_.speed_scaling_interface_name); - conf.names.emplace_back(tf_prefix + "passthrough_controller/running"); return conf; } @@ -142,13 +141,13 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& period) { - AsyncInfo temp = info_to_realtime_.get(); - if (temp.info_updated) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(temp.transfer_state); - command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].set_value(temp.abort); - temp.info_updated = false; - info_to_realtime_.set(temp); - } + // AsyncInfo temp = info_to_realtime_.get(); + // if (temp.info_updated) { + // command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(temp.transfer_state); + // command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].set_value(temp.abort); + // temp.info_updated = false; + // info_to_realtime_.set(temp); + //} if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != TRANSFER_STATE_IDLE) { @@ -250,8 +249,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const } } AsyncInfo info = { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value(), - command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value(), - state_interfaces_[StateInterfaces::CONTROLLER_RUNNING].get_value(), true }; + command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value(), true, true }; info_from_realtime_.set(info); return controller_interface::return_type::OK; From c4a366fe65500ab61ea2c7df1b7ffdc3a9bab281 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 30 Sep 2024 22:26:18 +0200 Subject: [PATCH 23/50] Make action handling realtime-thread-safe --- .../passthrough_trajectory_controller.hpp | 32 ++- .../src/passthrough_trajectory_controller.cpp | 238 ++++++++++-------- ...ough_trajectory_controller_parameters.yaml | 55 ++-- ur_robot_driver/config/ur_controllers.yaml | 3 +- ur_robot_driver/launch/ur_control.launch.py | 1 - ur_robot_driver/src/hardware_interface.cpp | 1 + 6 files changed, 188 insertions(+), 142 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index de21e73e9..c3ad63427 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -43,8 +43,6 @@ #include -#include - #include #include #include @@ -66,6 +64,9 @@ #include "passthrough_trajectory_controller_parameters.hpp" +#include "realtime_tools/realtime_buffer.h" +#include "realtime_tools/realtime_server_goal_handle.h" + namespace ur_controllers { @@ -75,6 +76,7 @@ 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 enum CommandInterfaces { @@ -134,6 +136,16 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI 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 + + 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); @@ -154,7 +166,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI const std::shared_ptr> goal_handle); void goal_accepted_callback( - const std::shared_ptr> goal_handle); + std::shared_ptr> goal_handle); std::vector joint_names_; std::vector state_interface_types_; @@ -171,17 +183,15 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI std::vector goal_tolerance_; rclcpp::Duration goal_time_tolerance_ = rclcpp::Duration::from_nanoseconds(0); - realtime_tools::RealtimeBoxBestEffort info_from_realtime_; - realtime_tools::RealtimeBoxBestEffort info_to_realtime_; - - uint32_t current_point_; - bool trajectory_active_; - double active_trajectory_elapsed_time_; - double max_trajectory_time_; + 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_; uint32_t number_of_joints_; - std::shared_ptr> active_goal_; static constexpr double NO_VAL = std::numeric_limits::quiet_NaN(); + + std::optional> scaling_state_interface_; }; } // namespace ur_controllers #endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 3be993e7b..fe878976f 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -39,18 +39,28 @@ //---------------------------------------------------------------------- #include +#include +#include #include "ur_controllers/passthrough_trajectory_controller.hpp" +#include "builtin_interfaces/msg/duration.hpp" #include "lifecycle_msgs/msg/state.hpp" namespace ur_controllers { + +double duration_to_double(const builtin_interfaces::msg::Duration& duration) +{ + return duration.sec + (duration.nanosec / 1000000000); +} + controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() { passthrough_param_listener_ = std::make_shared(get_node()); passthrough_params_ = passthrough_param_listener_->get_params(); - current_point_ = 0; + current_index_ = 0; joint_names_ = passthrough_params_.joints; state_interface_types_ = passthrough_params_.state_interfaces; + scaling_factor_ = 1.0; return controller_interface::CallbackReturn::SUCCESS; } @@ -127,71 +137,97 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat joint_position_state_interface_.clear(); joint_velocity_state_interface_.clear(); - for (uint32_t i = 0; i < state_interface_types_.size() * joint_names_.size(); i++) { - if (state_interfaces_[i].get_interface_name() == "position") { - joint_position_state_interface_.emplace_back(state_interfaces_[i]); - } else if (state_interfaces_[i].get_interface_name() == "velocity") { - joint_velocity_state_interface_.emplace_back(state_interfaces_[i]); + auto find_joint = [&](auto& first, auto& last) { + return std::find_if(first, last, [&](auto& interface) { + return std::find(joint_names_.begin(), joint_names_.end(), interface.get_name()) != joint_names_.end(); + }); + }; + + auto first = state_interfaces_.begin(); + auto last = state_interfaces_.end(); + + auto interface_it = find_joint(first, last); + while (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_position_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; + } + return ControllerInterface::on_activate(state); } controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Time& /*time*/, const rclcpp::Duration& period) { - // AsyncInfo temp = info_to_realtime_.get(); - // if (temp.info_updated) { - // command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(temp.transfer_state); - // command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].set_value(temp.abort); - // temp.info_updated = false; - // info_to_realtime_.set(temp); - //} - - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() != - 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 (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0) { - RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted hardware, aborting action."); - std::shared_ptr result = - std::make_shared(); - active_goal_->abort(result); - end_goal(); - return controller_interface::return_type::OK; + const auto active_goal = *rt_active_goal_.readFromRT(); + + const auto current_transfer_state = + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].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 (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0) { + RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted hardware, aborting action."); + std::shared_ptr result = + std::make_shared(); + active_goal->setAborted(result); + end_goal(); + return controller_interface::return_type::OK; + } } - /* Check if the goal has been cancelled by the ROS user. */ - if (active_goal_->is_canceling()) { - std::shared_ptr result = - std::make_shared(); - active_goal_->canceled(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); + rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); + max_trajectory_time_ = + rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); + goal_tolerance_ = active_goal->gh_->get_goal()->goal_tolerance; + path_tolerance_ = active_goal->gh_->get_goal()->path_tolerance; + goal_time_tolerance_ = active_goal->gh_->get_goal()->goal_time_tolerance; + + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value( + TRANSFER_STATE_WAITING_FOR_POINT); } + // Write a new point to the command interface, if the previous point has been read by the hardware interface. - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == - TRANSFER_STATE_WAITING_FOR_POINT) { - if (current_point_ < active_joint_traj_.points.size()) { + if (current_transfer_state == TRANSFER_STATE_WAITING_FOR_POINT) { + if (current_index_ < active_joint_traj_.points.size()) { // Write the time_from_start parameter. command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( - active_joint_traj_.points[current_point_].time_from_start.sec + - (active_joint_traj_.points[current_point_].time_from_start.nanosec / 1000000000)); + duration_to_double(active_joint_traj_.points[current_index_].time_from_start)); + // Write the positions for each joint of the robot - for (int i = 0; i < 6; i++) { + for (size_t i = 0; i < joint_names_.size(); i++) { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( - active_joint_traj_.points[current_point_].positions[i]); + active_joint_traj_.points[current_index_].positions[i]); // Optionally, also write velocities and accelerations for each joint. - if (active_joint_traj_.points[current_point_].velocities.size() > 0) { + if (active_joint_traj_.points[current_index_].velocities.size() > 0) { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value( - active_joint_traj_.points[current_point_].velocities[i]); + active_joint_traj_.points[current_index_].velocities[i]); } else { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_].set_value(NO_VAL); } - if (active_joint_traj_.points[current_point_].accelerations.size() > 0) { + if (active_joint_traj_.points[current_index_].accelerations.size() > 0) { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value( - active_joint_traj_.points[current_point_].accelerations[i]); + active_joint_traj_.points[current_index_].accelerations[i]); } else { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value(NO_VAL); } @@ -199,59 +235,58 @@ controller_interface::return_type PassthroughTrajectoryController::update(const // Tell hardware interface that this point is ready to be read. command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value( TRANSFER_STATE_TRANSFERRING); - current_point_++; - + current_index_++; // Check if all points have been written to the hardware interface. - } else if (current_point_ == active_joint_traj_.points.size()) { - RCLCPP_INFO(get_node()->get_logger(), "All points sent to the hardware interface, trajectory will now " - "execute!"); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(TRANSFER_STATE_DONE); + } else if (current_index_ == active_joint_traj_.points.size()) { + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].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 (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == - TRANSFER_STATE_DONE) { + } else if (current_transfer_state == TRANSFER_STATE_DONE) { std::shared_ptr result = std::make_shared(); // 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_->abort(result); + active_goal->setAborted(result); RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); // Check if the goal time tolerance was complied with. - } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_.seconds())) { + } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_)) { result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; result->error_string = "Goal not reached within time tolerance"; - active_goal_->abort(result); - RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal time tolerance not met."); + active_goal->setAborted(result); + end_goal(); + RCLCPP_ERROR(get_node()->get_logger(), + "Trajectory failed, goal time tolerance not met. Missed goal time by %f seconds.", + (active_trajectory_elapsed_time_ - max_trajectory_time_ - goal_time_tolerance_).seconds()); } else { result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; - active_goal_->succeed(result); + active_goal->setSucceeded(result); + end_goal(); RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); } - end_goal(); - } - } + } 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(); + } - /* Keep track of how long the trajectory has been executing, if it takes too long, send a warning. */ - if (command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value() == - TRANSFER_STATE_IN_MOTION) { - scaling_factor_ = state_interfaces_[StateInterfaces::SPEED_SCALING_FACTOR].get_value(); + active_trajectory_elapsed_time_ += period * scaling_factor_; - active_trajectory_elapsed_time_ += - static_cast(scaling_factor_ * (static_cast(period.nanoseconds()) / pow(10, 9))); + // 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_ + goal_time_tolerance_.seconds()) && - trajectory_active_) { - RCLCPP_WARN(get_node()->get_logger(), "Trajectory should be finished by now. You may want to cancel this goal, " - "if it is not."); - trajectory_active_ = false; + if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_) && trajectory_active_) { + RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *clk, 1000, + "Trajectory should be finished by now. You may want to cancel this goal, if it is not."); + } } } - AsyncInfo info = { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value(), - command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value(), true, true }; - info_from_realtime_.set(info); return controller_interface::return_type::OK; } @@ -266,7 +301,7 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb return rclcpp_action::GoalResponse::REJECT; } - if (info_from_realtime_.get().transfer_state != 0.0) { + if (trajectory_active_) { RCLCPP_ERROR(get_node()->get_logger(), "Can't accept new trajectory. A trajectory is already executing."); return rclcpp_action::GoalResponse::REJECT; } @@ -369,37 +404,42 @@ bool PassthroughTrajectoryController::check_accelerations( return true; } -// Called when the action is cancelled by the action client. rclcpp_action::CancelResponse PassthroughTrajectoryController::goal_cancelled_callback( - const std::shared_ptr< - rclcpp_action::ServerGoalHandle> /* goal_handle */) + const std::shared_ptr> goal_handle) { - RCLCPP_INFO(get_node()->get_logger(), "Cancelling active trajectory requested."); + // 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( - const std::shared_ptr> goal_handle) + std::shared_ptr> goal_handle) { - RCLCPP_INFO(get_node()->get_logger(), "Accepted new trajectory."); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "Accepted new trajectory with " + << goal_handle->get_goal()->trajectory.points.size() << " points."); + current_index_ = 0; + + // 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->preallocated_feedback_->joint_names = joint_names_; + 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; - active_trajectory_elapsed_time_ = 0; - current_point_ = 0; - - active_joint_traj_ = goal_handle->get_goal()->trajectory; - goal_tolerance_ = goal_handle->get_goal()->goal_tolerance; - path_tolerance_ = goal_handle->get_goal()->path_tolerance; - goal_time_tolerance_ = goal_handle->get_goal()->goal_time_tolerance; - active_goal_ = goal_handle; - - max_trajectory_time_ = active_joint_traj_.points.back().time_from_start.sec + - (active_joint_traj_.points.back().time_from_start.nanosec / pow(10, 9)); - - // Communicate to update() method, that the action has been triggered. - AsyncInfo temp = { 1.0, 0.0, 0.0, true }; - info_to_realtime_.set(temp); - return; } @@ -417,11 +457,7 @@ bool PassthroughTrajectoryController::check_goal_tolerance() void PassthroughTrajectoryController::end_goal() { trajectory_active_ = false; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(0.0); - active_goal_ = NULL; - current_point_ = 0; - goal_tolerance_.clear(); - path_tolerance_.clear(); + command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(TRANSFER_STATE_IDLE); } } // namespace ur_controllers diff --git a/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml index 2a4b87bab..db8844338 100644 --- a/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml +++ b/ur_controllers/src/passthrough_trajectory_controller_parameters.yaml @@ -1,30 +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" + 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, } - 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",]], - } + } + 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/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index 40b3f5025..a707019cb 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -120,10 +120,9 @@ passthrough_trajectory_controller: state_interfaces: - position - velocity - - effort constraints: stopped_velocity_tolerance: 0.2 - goal_time: 0.0 + goal_time: 0.5 $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index db8cf834f..6849ddad7 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -165,7 +165,6 @@ def controller_spawner(controllers, active=True): ] controllers_inactive = [ "forward_position_controller", - "passthrough_trajectory_controller", ] controller_spawners = [controller_spawner(controllers_active)] + [ diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index c36c83f95..191c308bf 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -994,6 +994,7 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller() trajectory_joint_velocities_.clear(); trajectory_times_.clear(); last_time = 0.0; + passthrough_trajectory_abort_ = 0.0; passthrough_trajectory_transfer_state_ = 4.0; } } From 416e190a34b364574308682490f379e3f95653a0 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 1 Oct 2024 00:59:04 +0200 Subject: [PATCH 24/50] Fix tolerances handling --- .../passthrough_trajectory_controller.hpp | 30 +++++----- .../src/passthrough_trajectory_controller.cpp | 58 ++++++++++++------- ur_robot_driver/launch/ur_control.launch.py | 1 + ur_robot_driver/scripts/example_move.py | 2 + ur_robot_driver/test/robot_driver.py | 17 ++++-- 5 files changed, 68 insertions(+), 40 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index c3ad63427..d2a3bca13 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -43,30 +43,29 @@ #include +#include +#include + #include #include #include #include #include -#include "controller_interface/controller_interface.hpp" -#include "rclcpp_action/server.hpp" -#include "rclcpp_action/create_server.hpp" -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/server_goal_handle.hpp" -#include "rclcpp/time.hpp" -#include "rclcpp/duration.hpp" +#include +#include +#include +#include +#include +#include +#include +#include -#include "trajectory_msgs/msg/joint_trajectory.hpp" -#include "trajectory_msgs/msg/joint_trajectory_point.hpp" -#include "control_msgs/action/follow_joint_trajectory.hpp" -#include "control_msgs/action/joint_trajectory.hpp" +#include +#include #include "passthrough_trajectory_controller_parameters.hpp" -#include "realtime_tools/realtime_buffer.h" -#include "realtime_tools/realtime_server_goal_handle.h" - namespace ur_controllers { @@ -171,6 +170,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI std::vector 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_; @@ -192,6 +192,8 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI static constexpr double NO_VAL = std::numeric_limits::quiet_NaN(); std::optional> scaling_state_interface_; + + rclcpp::Clock::SharedPtr clock_; }; } // namespace ur_controllers #endif // UR_CONTROLLERS__PASSTHROUGH_TRAJECTORY_CONTROLLER_HPP_ diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index fe878976f..4bea63036 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -38,12 +38,14 @@ */ //---------------------------------------------------------------------- +#include #include #include #include +#include +#include + #include "ur_controllers/passthrough_trajectory_controller.hpp" -#include "builtin_interfaces/msg/duration.hpp" -#include "lifecycle_msgs/msg/state.hpp" namespace ur_controllers { @@ -61,6 +63,7 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() joint_names_ = passthrough_params_.joints; state_interface_types_ = passthrough_params_.state_interfaces; scaling_factor_ = 1.0; + clock_ = get_node()->get_clock(); return controller_interface::CallbackReturn::SUCCESS; } @@ -70,6 +73,15 @@ PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& pre start_action_server(); trajectory_active_ = false; + joint_state_interface_names_.clear(); + + joint_state_interface_names_.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto& joint_name : joint_names_) { + for (const auto& interface_type : state_interface_types_) { + joint_state_interface_names_.emplace_back(joint_name + "/" + interface_type); + } + } + return ControllerInterface::on_configure(previous_state); } @@ -137,22 +149,16 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat joint_position_state_interface_.clear(); joint_velocity_state_interface_.clear(); - auto find_joint = [&](auto& first, auto& last) { - return std::find_if(first, last, [&](auto& interface) { - return std::find(joint_names_.begin(), joint_names_.end(), interface.get_name()) != joint_names_.end(); - }); - }; - - auto first = state_interfaces_.begin(); - auto last = state_interfaces_.end(); - - auto interface_it = find_joint(first, last); - while (interface_it != state_interfaces_.end()) { - if (interface_it->get_interface_name() == "position") { - joint_position_state_interface_.emplace_back(*interface_it); + 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_position_state_interface_.emplace_back(*interface_it); + } else if (interface_it->get_interface_name() == "velocity") { + joint_position_state_interface_.emplace_back(*interface_it); + } } } @@ -198,6 +204,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); max_trajectory_time_ = rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); + // TODO(fexner): Merge goal tolerances with default tolerances goal_tolerance_ = active_goal->gh_->get_goal()->goal_tolerance; path_tolerance_ = active_goal->gh_->get_goal()->path_tolerance; goal_time_tolerance_ = active_goal->gh_->get_goal()->goal_time_tolerance; @@ -253,6 +260,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const 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."); // Check if the goal time tolerance was complied with. } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_)) { @@ -281,7 +289,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const // active_trajectory_elapsed_time_.seconds(), scaling_factor_, period.seconds()); if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_) && trajectory_active_) { - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *clk, 1000, + 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."); } } @@ -429,6 +437,8 @@ void PassthroughTrajectoryController::goal_accepted_callback( << goal_handle->get_goal()->trajectory.points.size() << " points."); current_index_ = 0; + // TODO(fexner): Make sure the trajectory is correctly ordered! + // 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. @@ -445,9 +455,15 @@ void PassthroughTrajectoryController::goal_accepted_callback( bool PassthroughTrajectoryController::check_goal_tolerance() { - for (uint32_t i = 0; i < goal_tolerance_.size(); i++) { - if (std::abs(joint_position_state_interface_[i].get().get_value() - active_joint_traj_.points.back().positions[i]) > - goal_tolerance_[i].position) { + for (size_t i = 0; i < active_joint_traj_.joint_names.size(); ++i) { + const std::string joint_name = active_joint_traj_.joint_names[i]; + auto joint_tolerance_it = std::find_if(goal_tolerance_.begin(), goal_tolerance_.end(), + [&](auto entry) { return entry.name == joint_name; }); + auto position_state_it = + std::find_if(joint_position_state_interface_.begin(), joint_position_state_interface_.end(), + [&](auto entry) { return entry.get().get_name() == joint_name + "/" + "position"; }); + if (std::abs(position_state_it->get().get_value() - active_joint_traj_.points.back().positions[i]) > + joint_tolerance_it->position) { return false; } } diff --git a/ur_robot_driver/launch/ur_control.launch.py b/ur_robot_driver/launch/ur_control.launch.py index 6849ddad7..db8cf834f 100644 --- a/ur_robot_driver/launch/ur_control.launch.py +++ b/ur_robot_driver/launch/ur_control.launch.py @@ -165,6 +165,7 @@ def controller_spawner(controllers, active=True): ] controllers_inactive = [ "forward_position_controller", + "passthrough_trajectory_controller", ] controller_spawners = [controller_spawner(controllers_active)] + [ diff --git a/ur_robot_driver/scripts/example_move.py b/ur_robot_driver/scripts/example_move.py index 51ec5c855..9c11e63f6 100755 --- a/ur_robot_driver/scripts/example_move.py +++ b/ur_robot_driver/scripts/example_move.py @@ -41,6 +41,7 @@ from builtin_interfaces.msg import Duration from trajectory_msgs.msg import JointTrajectory, JointTrajectoryPoint from control_msgs.action import FollowJointTrajectory +from control_msgs.msg import JointTolerance TRAJECTORIES = { "traj0": [ @@ -134,6 +135,7 @@ def execute_trajectory(self, traj_name): goal.trajectory = self.goals[traj_name] goal.goal_time_tolerance = Duration(sec=0, nanosec=1000) + goal.goal_tolerance = [JointTolerance(position=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) diff --git a/ur_robot_driver/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 9197e759f..4938bc899 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -56,9 +56,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", @@ -348,7 +348,7 @@ def js_cb(msg): # 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): + def test_passthrough_trajectory(self, tf_prefix): self.assertTrue( self._controller_manager_interface.switch_controller( strictness=SwitchController.Request.BEST_EFFORT, @@ -366,7 +366,10 @@ def test_passthrough_trajectory(self): Duration(sec=8, nanosec=0), Duration(sec=12, nanosec=0), ] - goal_tolerance = [JointTolerance(position=0.01) for i in range(6)] + 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( @@ -374,6 +377,7 @@ def test_passthrough_trajectory(self): 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, @@ -387,7 +391,10 @@ def test_passthrough_trajectory(self): ) self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) # Test impossible goal tolerance, should fail. - goal_tolerance = [JointTolerance(position=0.000000000000000001) for i in range(6)] + goal_tolerance = [ + JointTolerance(position=0.000000000000000001, name=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, From e513fc62b2c7e8a0e4bab98a15e5369bb50da698 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 1 Oct 2024 00:59:42 +0200 Subject: [PATCH 25/50] String formatting --- .../src/passthrough_trajectory_controller.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 4bea63036..af1624fe5 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -339,10 +339,10 @@ bool PassthroughTrajectoryController::check_positions( 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(), msg.c_str()); + 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(), msg.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); return false; } } @@ -359,10 +359,10 @@ bool PassthroughTrajectoryController::check_velocities( 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(), msg.c_str()); + 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(), msg.c_str()); + 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()) { @@ -370,10 +370,10 @@ bool PassthroughTrajectoryController::check_velocities( 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(), msg.c_str()); + 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(), msg.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); return false; } } @@ -390,10 +390,10 @@ bool PassthroughTrajectoryController::check_accelerations( 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(), msg.c_str()); + 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(), msg.c_str()); + 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()) { @@ -402,10 +402,10 @@ bool PassthroughTrajectoryController::check_accelerations( "robot. " "(" + std::to_string(number_of_joints_) + " joint accelerations per point)"; - RCLCPP_ERROR(get_node()->get_logger(), msg.c_str()); + 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(), msg.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "%s", msg.c_str()); return false; } } From a5957fcbead34f516031c189b4f706555f141dd9 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 1 Oct 2024 01:00:11 +0200 Subject: [PATCH 26/50] Remove unused interfaces --- .../include/ur_robot_driver/hardware_interface.hpp | 2 +- ur_robot_driver/src/hardware_interface.cpp | 11 ++++------- 2 files changed, 5 insertions(+), 8 deletions(-) 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 d007e0383..e06da693d 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -200,7 +200,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface // Passthrough trajectory controller interface values double passthrough_trajectory_transfer_state_; double passthrough_trajectory_abort_; - double passthrough_trajectory_controller_running_; + bool passthrough_trajectory_controller_running_; urcl::vector6d_t passthrough_trajectory_positions_; urcl::vector6d_t passthrough_trajectory_velocities_; urcl::vector6d_t passthrough_trajectory_accelerations_; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 191c308bf..bc6e873c5 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -236,9 +236,6 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); - state_interfaces.emplace_back(hardware_interface::StateInterface(tf_prefix + "passthrough_controller", "running", - &passthrough_trajectory_controller_running_)); - return state_interfaces; } @@ -911,7 +908,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod 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_ = 0.0; + passthrough_trajectory_controller_running_ = false; passthrough_trajectory_abort_ = 1.0; trajectory_joint_positions_.clear(); trajectory_joint_accelerations_.clear(); @@ -921,21 +918,21 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod 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_ = 0.0; + 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_ = 0.0; + 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_TRAJECTORY_CONTROLLER) != start_modes_.end()) { velocity_controller_running_ = false; position_controller_running_ = false; - passthrough_trajectory_controller_running_ = 1.0; + passthrough_trajectory_controller_running_ = true; } start_modes_.clear(); From 5b1f7f3e007836b16e57acce215db5825a208e8a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 1 Oct 2024 10:54:11 +0200 Subject: [PATCH 27/50] Deactivate goal time check for now since ros2_control can report wrong periods --- .../src/passthrough_trajectory_controller.cpp | 23 +++++++++++-------- 1 file changed, 14 insertions(+), 9 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index af1624fe5..4e336373a 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -262,15 +262,20 @@ controller_interface::return_type PassthroughTrajectoryController::update(const active_goal->setAborted(result); end_goal(); RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); - // Check if the goal time tolerance was complied with. - } else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_)) { - result->error_code = control_msgs::action::FollowJointTrajectory::Result::GOAL_TOLERANCE_VIOLATED; - result->error_string = "Goal not reached within time tolerance"; - active_goal->setAborted(result); - end_goal(); - RCLCPP_ERROR(get_node()->get_logger(), - "Trajectory failed, goal time tolerance not met. Missed goal time by %f seconds.", - (active_trajectory_elapsed_time_ - max_trajectory_time_ - goal_time_tolerance_).seconds()); + // TODO(fexner): Goal time check currently disabled due to + // https://github.com/ros-controls/ros2_control/issues/1769 + //} else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_)) { + //// 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_ - goal_time_tolerance_).seconds()) + + // " seconds."; + // active_goal->setAborted(result); + // end_goal(); + // RCLCPP_ERROR(get_node()->get_logger(), + // "Trajectory failed, goal time tolerance not met. Missed goal time by %f seconds.", + // (active_trajectory_elapsed_time_ - max_trajectory_time_ - goal_time_tolerance_).seconds()); } else { result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; active_goal->setSucceeded(result); From 5078c60605ee7ef258617276f7af71293ea69e30 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Tue, 1 Oct 2024 11:39:32 +0200 Subject: [PATCH 28/50] Simplify joint state configuration --- .../src/passthrough_trajectory_controller.cpp | 15 ++++++--------- 1 file changed, 6 insertions(+), 9 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 4e336373a..7e68c0bdf 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -100,15 +100,10 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - const std::string tf_prefix = passthrough_params_.tf_prefix; - conf.names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto& joint_name : joint_names_) { - for (const auto& interface_type : state_interface_types_) { - conf.names.emplace_back(joint_name + "/" + interface_type); - } - } - conf.names.emplace_back(passthrough_params_.speed_scaling_interface_name); + 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; } @@ -216,7 +211,9 @@ controller_interface::return_type PassthroughTrajectoryController::update(const // 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. + // TODO(fexner): Make sure that we are writing to the correct interface. Joints could be + // ordered differently inside the trajectory! + // Write the time_from_start parameter. command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( duration_to_double(active_joint_traj_.points[current_index_].time_from_start)); From 51fd0028cfa0d82062fb26963eb903791ee8fe16 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Oct 2024 15:10:03 +0200 Subject: [PATCH 29/50] Make controller robust against joint ordering --- .../passthrough_trajectory_controller.hpp | 16 +- .../src/passthrough_trajectory_controller.cpp | 147 ++++++++++++------ 2 files changed, 112 insertions(+), 51 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index d2a3bca13..4b7b97349 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -50,6 +50,7 @@ #include #include #include +#include #include #include @@ -142,6 +143,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI 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); @@ -152,6 +154,9 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI 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_; @@ -167,28 +172,29 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI void goal_accepted_callback( std::shared_ptr> goal_handle); - std::vector joint_names_; + 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_positions(std::shared_ptr goal); bool check_velocities(std::shared_ptr goal); bool check_accelerations(std::shared_ptr goal); trajectory_msgs::msg::JointTrajectory active_joint_traj_; - std::vector path_tolerance_; - std::vector goal_tolerance_; - rclcpp::Duration goal_time_tolerance_ = rclcpp::Duration::from_nanoseconds(0); + // 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_; - uint32_t number_of_joints_; + std::atomic number_of_joints_; static constexpr double NO_VAL = std::numeric_limits::quiet_NaN(); std::optional> scaling_state_interface_; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 7e68c0bdf..806d1e34e 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -52,7 +52,7 @@ namespace ur_controllers double duration_to_double(const builtin_interfaces::msg::Duration& duration) { - return duration.sec + (duration.nanosec / 1000000000); + return duration.sec + (duration.nanosec / 1000000000.0); } controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() @@ -60,7 +60,9 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_init() passthrough_param_listener_ = std::make_shared(get_node()); passthrough_params_ = passthrough_param_listener_->get_params(); current_index_ = 0; - joint_names_ = passthrough_params_.joints; + 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(); @@ -75,8 +77,10 @@ PassthroughTrajectoryController::on_configure(const rclcpp_lifecycle::State& pre joint_state_interface_names_.clear(); - joint_state_interface_names_.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto& joint_name : joint_names_) { + 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); } @@ -119,15 +123,15 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_abort"); - for (size_t i = 0; i < 6; ++i) { + for (size_t i = 0; i < number_of_joints_; ++i) { config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_positions_" + std::to_string(i)); } - for (size_t i = 0; i < 6; ++i) { + for (size_t i = 0; i < number_of_joints_; ++i) { config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_velocities_" + std::to_string(i)); } - for (size_t i = 0; i < 6; ++i) { + for (size_t i = 0; i < number_of_joints_; ++i) { config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_accelerations_" + std::to_string(i)); } @@ -139,10 +143,10 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co controller_interface::CallbackReturn PassthroughTrajectoryController::on_activate(const rclcpp_lifecycle::State& state) { - number_of_joints_ = joint_names_.size(); // 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(), @@ -152,8 +156,10 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat joint_position_state_interface_.emplace_back(*interface_it); } else if (interface_it->get_interface_name() == "velocity") { - joint_position_state_interface_.emplace_back(*interface_it); + joint_velocity_state_interface_.emplace_back(*interface_it); } + } else if (interface_it->get_interface_name() == "acceleration") { + joint_acceleration_state_interface_.emplace_back(*interface_it); } } @@ -199,39 +205,36 @@ controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); max_trajectory_time_ = rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); - // TODO(fexner): Merge goal tolerances with default tolerances - goal_tolerance_ = active_goal->gh_->get_goal()->goal_tolerance; - path_tolerance_ = active_goal->gh_->get_goal()->path_tolerance; - goal_time_tolerance_ = active_goal->gh_->get_goal()->goal_time_tolerance; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].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()) { - // TODO(fexner): Make sure that we are writing to the correct interface. Joints could be - // ordered differently inside the trajectory! // Write the time_from_start parameter. command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( duration_to_double(active_joint_traj_.points[current_index_].time_from_start)); // Write the positions for each joint of the robot - for (size_t i = 0; i < joint_names_.size(); i++) { + auto joint_names_internal = joint_names_.readFromRT(); + for (size_t i = 0; i < number_of_joints_; i++) { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( - active_joint_traj_.points[current_index_].positions[i]); + 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_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value( - active_joint_traj_.points[current_index_].velocities[i]); + active_joint_traj_.points[current_index_].velocities[joint_mapping->at(joint_names_internal->at(i))]); } else { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_].set_value(NO_VAL); } if (active_joint_traj_.points[current_index_].accelerations.size() > 0) { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value( - active_joint_traj_.points[current_index_].accelerations[i]); + active_joint_traj_.points[current_index_] + .accelerations[joint_mapping->at(joint_names_internal->at(i))]); } else { command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value(NO_VAL); } @@ -259,20 +262,16 @@ controller_interface::return_type PassthroughTrajectoryController::update(const active_goal->setAborted(result); end_goal(); RCLCPP_ERROR(get_node()->get_logger(), "Trajectory failed, goal tolerances not met."); - // TODO(fexner): Goal time check currently disabled due to - // https://github.com/ros-controls/ros2_control/issues/1769 - //} else if (active_trajectory_elapsed_time_ > (max_trajectory_time_ + goal_time_tolerance_)) { - //// 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_ - goal_time_tolerance_).seconds()) + - // " seconds."; - // active_goal->setAborted(result); - // end_goal(); - // RCLCPP_ERROR(get_node()->get_logger(), - // "Trajectory failed, goal time tolerance not met. Missed goal time by %f seconds.", - // (active_trajectory_elapsed_time_ - max_trajectory_time_ - goal_time_tolerance_).seconds()); + } 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; active_goal->setSucceeded(result); @@ -290,7 +289,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const // 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_ + goal_time_tolerance_) && trajectory_active_) { + 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."); } @@ -330,6 +329,10 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb return rclcpp_action::GoalResponse::REJECT; } + // TODO(fexner): Validate goal + // - Do the tolerances contain correct joint names? + // - Do the tolerances contain correct values? + return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } @@ -439,13 +442,29 @@ void PassthroughTrajectoryController::goal_accepted_callback( << goal_handle->get_goal()->trajectory.points.size() << " points."); current_index_ = 0; - // TODO(fexner): Make sure the trajectory is correctly ordered! + // 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; + auto joint_names_internal = joint_names_.readFromRT(); + 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); + } + } + goal_tolerance_.writeFromNonRT(goal_tolerances); + goal_time_tolerance_.writeFromNonRT(goal_handle->get_goal()->goal_time_tolerance); // 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->preallocated_feedback_->joint_names = joint_names_; rt_goal->execute(); rt_active_goal_.writeFromNonRT(rt_goal); goal_handle_timer_.reset(); @@ -457,18 +476,40 @@ void PassthroughTrajectoryController::goal_accepted_callback( bool PassthroughTrajectoryController::check_goal_tolerance() { - for (size_t i = 0; i < active_joint_traj_.joint_names.size(); ++i) { - const std::string joint_name = active_joint_traj_.joint_names[i]; - auto joint_tolerance_it = std::find_if(goal_tolerance_.begin(), goal_tolerance_.end(), - [&](auto entry) { return entry.name == joint_name; }); - auto position_state_it = - std::find_if(joint_position_state_interface_.begin(), joint_position_state_interface_.end(), - [&](auto entry) { return entry.get().get_name() == joint_name + "/" + "position"; }); - if (std::abs(position_state_it->get().get_value() - active_joint_traj_.points.back().positions[i]) > - joint_tolerance_it->position) { + auto goal_tolerance = goal_tolerance_.readFromRT(); + auto joint_mapping = joint_trajectory_mapping_.readFromRT(); + auto joint_names_internal = joint_names_.readFromRT(); + 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", + joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos); 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) { + RCLCPP_ERROR(get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f", + joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos); + 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) { + RCLCPP_ERROR(get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f", + joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos); + return false; + } + } } + return true; } @@ -477,6 +518,20 @@ void PassthroughTrajectoryController::end_goal() trajectory_active_ = false; command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].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" From bd9652e272a5696cb4a64951dd27a4dfdb706c87 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Oct 2024 15:11:14 +0200 Subject: [PATCH 30/50] Make default config reflect the actual parameters --- ur_robot_driver/config/ur_controllers.yaml | 9 --------- 1 file changed, 9 deletions(-) diff --git a/ur_robot_driver/config/ur_controllers.yaml b/ur_robot_driver/config/ur_controllers.yaml index a707019cb..930aec37b 100644 --- a/ur_robot_driver/config/ur_controllers.yaml +++ b/ur_robot_driver/config/ur_controllers.yaml @@ -120,15 +120,6 @@ passthrough_trajectory_controller: state_interfaces: - position - velocity - constraints: - stopped_velocity_tolerance: 0.2 - goal_time: 0.5 - $(var tf_prefix)shoulder_pan_joint: { trajectory: 0.2, goal: 0.1 } - $(var tf_prefix)shoulder_lift_joint: { trajectory: 0.2, goal: 0.1 } - $(var tf_prefix)elbow_joint: { trajectory: 0.2, goal: 0.1 } - $(var tf_prefix)wrist_1_joint: { trajectory: 0.2, goal: 0.1 } - $(var tf_prefix)wrist_2_joint: { trajectory: 0.2, goal: 0.1 } - $(var tf_prefix)wrist_3_joint: { trajectory: 0.2, goal: 0.1 } speed_scaling_interface_name: $(var tf_prefix)speed_scaling/speed_scaling_factor forward_velocity_controller: From 1389c3c73fea6a7bde1f4987e7bdb14ebd0b3ea7 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Oct 2024 16:16:44 +0200 Subject: [PATCH 31/50] Fix tolerance handling --- .../passthrough_trajectory_controller.hpp | 7 +- .../src/passthrough_trajectory_controller.cpp | 82 +++++++++++++------ ur_robot_driver/scripts/example_move.py | 6 +- ur_robot_driver/test/robot_driver.py | 13 ++- 4 files changed, 76 insertions(+), 32 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 4b7b97349..1133686af 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -180,9 +180,10 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI std::vector> joint_velocity_state_interface_; std::vector> joint_acceleration_state_interface_; - bool check_positions(std::shared_ptr goal); - bool check_velocities(std::shared_ptr goal); - bool check_accelerations(std::shared_ptr goal); + 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_; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 806d1e34e..4991e9b45 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -40,6 +40,8 @@ #include #include +#include + #include #include #include @@ -316,27 +318,50 @@ rclcpp_action::GoalResponse PassthroughTrajectoryController::goal_received_callb } // Check that all parts of the trajectory are valid. - if (!check_positions(goal)) { + if (!check_goal_positions(goal)) { RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); return rclcpp_action::GoalResponse::REJECT; } - if (!check_velocities(goal)) { + if (!check_goal_velocities(goal)) { RCLCPP_ERROR(get_node()->get_logger(), "Trajectory rejected"); return rclcpp_action::GoalResponse::REJECT; } - if (!check_accelerations(goal)) { + 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; } - - // TODO(fexner): Validate goal - // - Do the tolerances contain correct joint names? - // - Do the tolerances contain correct values? return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; } -bool PassthroughTrajectoryController::check_positions( +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++) { @@ -354,7 +379,7 @@ bool PassthroughTrajectoryController::check_positions( return true; } -bool PassthroughTrajectoryController::check_velocities( +bool PassthroughTrajectoryController::check_goal_velocities( std::shared_ptr goal) { for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { @@ -385,7 +410,7 @@ bool PassthroughTrajectoryController::check_velocities( return true; } -bool PassthroughTrajectoryController::check_accelerations( +bool PassthroughTrajectoryController::check_goal_accelerations( std::shared_ptr goal) { for (uint32_t i = 0; i < goal->trajectory.points.size(); i++) { @@ -448,17 +473,27 @@ void PassthroughTrajectoryController::goal_accepted_callback( // sort goal tolerances to match internal joint order std::vector goal_tolerances; - auto joint_names_internal = joint_names_.readFromRT(); - 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); + 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 @@ -479,14 +514,19 @@ 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", - joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos); + // 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; } @@ -494,8 +534,6 @@ bool PassthroughTrajectoryController::check_goal_tolerance() 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) { - RCLCPP_ERROR(get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f", - joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos); return false; } } @@ -503,8 +541,6 @@ bool PassthroughTrajectoryController::check_goal_tolerance() 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) { - RCLCPP_ERROR(get_node()->get_logger(), "Joint %s should be at position %f, but is at position %f", - joint_position_state_interface_[i].get().get_name().c_str(), setpoint, joint_pos); return false; } } diff --git a/ur_robot_driver/scripts/example_move.py b/ur_robot_driver/scripts/example_move.py index 9c11e63f6..28c12fa46 100755 --- a/ur_robot_driver/scripts/example_move.py +++ b/ur_robot_driver/scripts/example_move.py @@ -134,7 +134,7 @@ 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, name=self.joints[i]) for i in range(6)] self._send_goal_future = self._action_client.send_goal_async(goal) @@ -158,7 +158,7 @@ def get_result_callback(self, future): time.sleep(2) self.execute_next_trajectory() else: - raise RuntimeError("Executing trajectory failed") + raise RuntimeError("Executing trajectory failed. " + result.error_string) @staticmethod def error_code_to_str(error_code): @@ -182,6 +182,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/test/robot_driver.py b/ur_robot_driver/test/robot_driver.py index 4938bc899..9e6464316 100755 --- a/ur_robot_driver/test/robot_driver.py +++ b/ur_robot_driver/test/robot_driver.py @@ -280,7 +280,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( @@ -392,7 +395,7 @@ def test_passthrough_trajectory(self, tf_prefix): self.assertEqual(result.error_code, FollowJointTrajectory.Result.SUCCESSFUL) # Test impossible goal tolerance, should fail. goal_tolerance = [ - JointTolerance(position=0.000000000000000001, name=ROBOT_JOINTS[i]) + 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( @@ -410,9 +413,11 @@ def test_passthrough_trajectory(self, tf_prefix): ) # Test impossible goal time - goal_tolerance = [JointTolerance(position=0.01) for i in range(6)] + 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 = 1000 + goal_time_tolerance.nanosec = 10 goal_handle = self._passthrough_forward_joint_trajectory.send_goal( trajectory=trajectory, goal_time_tolerance=goal_time_tolerance, From bc76a615f4a1d25fb357d2823e5e27c5fd955854 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Oct 2024 17:10:13 +0200 Subject: [PATCH 32/50] REVERT_ME: use fixed ros2_control until merged --- Universal_Robots_ROS2_Driver.jazzy.repos | 4 ++-- Universal_Robots_ROS2_Driver.rolling.repos | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Universal_Robots_ROS2_Driver.jazzy.repos b/Universal_Robots_ROS2_Driver.jazzy.repos index f7dd6bee1..c9780c3dc 100644 --- a/Universal_Robots_ROS2_Driver.jazzy.repos +++ b/Universal_Robots_ROS2_Driver.jazzy.repos @@ -13,8 +13,8 @@ repositories: version: foxy-devel ros2_control: type: git - url: https://github.com/ros-controls/ros2_control.git - version: master + url: https://github.com/pal-robotics-forks/ros2_control.git + version: fix/controller_update_period ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers diff --git a/Universal_Robots_ROS2_Driver.rolling.repos b/Universal_Robots_ROS2_Driver.rolling.repos index f7dd6bee1..c9780c3dc 100644 --- a/Universal_Robots_ROS2_Driver.rolling.repos +++ b/Universal_Robots_ROS2_Driver.rolling.repos @@ -13,8 +13,8 @@ repositories: version: foxy-devel ros2_control: type: git - url: https://github.com/ros-controls/ros2_control.git - version: master + url: https://github.com/pal-robotics-forks/ros2_control.git + version: fix/controller_update_period ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers From 934ccc5663dd31d9369ea2bcaaef229988788049 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 2 Oct 2024 18:07:32 +0200 Subject: [PATCH 33/50] Make sure the abort state is reset when sending new trajectories --- ur_controllers/src/passthrough_trajectory_controller.cpp | 5 +++-- ur_robot_driver/src/hardware_interface.cpp | 2 ++ 2 files changed, 5 insertions(+), 2 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 4991e9b45..4b4c5bed0 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -190,7 +190,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const 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 (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0) { + if (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0 && current_index_ > 0) { RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted hardware, aborting action."); std::shared_ptr result = std::make_shared(); @@ -278,7 +278,8 @@ controller_interface::return_type PassthroughTrajectoryController::update(const result->error_code = control_msgs::action::FollowJointTrajectory::Result::SUCCESSFUL; active_goal->setSucceeded(result); end_goal(); - RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully!"); + RCLCPP_INFO(get_node()->get_logger(), "Trajectory executed successfully in %f seconds!", + active_trajectory_elapsed_time_.seconds()); } } 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. diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index bc6e873c5..e103f26dc 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -933,6 +933,7 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod velocity_controller_running_ = false; position_controller_running_ = false; passthrough_trajectory_controller_running_ = true; + passthrough_trajectory_abort_ = 0.0; } start_modes_.clear(); @@ -947,6 +948,7 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller() /* See passthrough_trajectory_controller.hpp for an explanation of the passthrough_trajectory_transfer_state_ values. */ 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); From 0e9b37dacc308e7e8f482bd740c48888fc32aef2 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 11:21:09 +0200 Subject: [PATCH 34/50] Restructure command interface exporting Make passthrough command interfaces part of the joints and fix claiming conflicts --- .../passthrough_trajectory_controller.hpp | 53 ++++----- .../src/passthrough_trajectory_controller.cpp | 65 ++++++----- .../ur_robot_driver/hardware_interface.hpp | 2 +- ur_robot_driver/src/hardware_interface.cpp | 101 ++++++++++++------ ur_robot_driver/urdf/ur.ros2_control.xacro | 8 ++ 5 files changed, 130 insertions(+), 99 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 1133686af..080d9d17d 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -70,51 +70,33 @@ 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 trajecotry. 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 enum CommandInterfaces { - /* The PASSTHROUGH_TRAJECTORY_TRANSFER_STATE value is used to keep track of which stage the transfer is in. - 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 trajecotry. 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. */ - PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 0, - /* The PASSTHROUGH_TRAJECTORY_ABORT value is used to indicate whether the trajectory has been cancelled from the - * hardware interface.*/ - PASSTHROUGH_TRAJECTORY_ABORT = 1, - /* Arrays to hold the values of a point. */ - PASSTHROUGH_TRAJECTORY_POSITIONS_ = 2, - PASSTHROUGH_TRAJECTORY_VELOCITIES_ = 8, - PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ = 14, - PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 20 -}; - -enum StateInterfaces -{ - /* State interface 0 - 17 are joint state interfaces*/ - - SPEED_SCALING_FACTOR = 18, -}; - -// Struct to hold data that has to be transferred between realtime thread and non-realtime threads -struct AsyncInfo -{ - double transfer_state = 0; - double abort = 0; - double controller_running = 0; - bool info_updated = false; + // The PASSTHROUGH_TRAJECTORY_TRANSFER_STATE value is used to keep track of which stage the transfer is in. + PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 18, + // The PASSTHROUGH_TRAJECTORY_ABORT value is used to indicate whether the trajectory has been cancelled from the + // hardware interface./ + PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 19 }; class PassthroughTrajectoryController : public controller_interface::ControllerInterface @@ -199,6 +181,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI static constexpr double NO_VAL = std::numeric_limits::quiet_NaN(); std::optional> scaling_state_interface_; + std::optional> abort_state_interface_; rclcpp::Clock::SharedPtr clock_; }; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 4b4c5bed0..f49bc509c 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -111,6 +111,9 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st conf.names.push_back(passthrough_params_.speed_scaling_interface_name); + const std::string tf_prefix = passthrough_params_.tf_prefix; + conf.names.push_back(tf_prefix + "passthrough_controller/passthrough_trajectory_abort"); + return conf; } @@ -121,23 +124,14 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co const std::string tf_prefix = passthrough_params_.tf_prefix; - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); - - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_abort"); - - for (size_t i = 0; i < number_of_joints_; ++i) { - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_positions_" + - std::to_string(i)); - } - for (size_t i = 0; i < number_of_joints_; ++i) { - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_velocities_" + - std::to_string(i)); - } - for (size_t i = 0; i < number_of_joints_; ++i) { - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_accelerations_" + - std::to_string(i)); + auto joint_names = passthrough_params_.joints; + for (auto& joint_name : joint_names) { + config.names.emplace_back(joint_name + "/passthrough_position"); + config.names.emplace_back(joint_name + "/passthrough_velocity"); + config.names.emplace_back(joint_name + "/passthrough_acceleration"); } + config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_time_from_start"); return config; @@ -175,6 +169,19 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat return controller_interface::CallbackReturn::ERROR; } + { + const std::string interface_name = passthrough_params_.tf_prefix + "passthrough_controller/" + "passthrough_trajectory_abort"; + auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + [&](auto& interface) { return (interface.get_name() == interface_name); }); + if (it != state_interfaces_.end()) { + abort_state_interface_ = *it; + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str()); + return controller_interface::CallbackReturn::ERROR; + } + } + return ControllerInterface::on_activate(state); } @@ -190,8 +197,8 @@ controller_interface::return_type PassthroughTrajectoryController::update(const 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 (command_interfaces_[PASSTHROUGH_TRAJECTORY_ABORT].get_value() == 1.0 && current_index_ > 0) { - RCLCPP_INFO(get_node()->get_logger(), "Trajectory aborted hardware, aborting action."); + if (abort_state_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); @@ -222,23 +229,25 @@ controller_interface::return_type PassthroughTrajectoryController::update(const // 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_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_POSITIONS_ + i].set_value( + 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_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_ + i].set_value( + 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_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_VELOCITIES_].set_value(NO_VAL); - } - - if (active_joint_traj_.points[current_index_].accelerations.size() > 0) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value( - active_joint_traj_.points[current_index_] - .accelerations[joint_mapping->at(joint_names_internal->at(i))]); - } else { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_ACCELERATIONS_ + i].set_value(NO_VAL); + 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. 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 e06da693d..c6a3fc061 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -249,7 +249,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); - const std::string PASSTHROUGH_TRAJECTORY_CONTROLLER = "passthrough_controller/passthrough_trajectory_positions_"; + const std::string PASSTHROUGH_TRAJECTORY_CONTROLLER = "passthrough"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index e103f26dc..554d785cf 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -45,7 +45,7 @@ #include "ur_client_library/exceptions.h" #include "ur_client_library/ur/tool_communication.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" @@ -101,9 +101,9 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys trajectory_joint_accelerations_.clear(); for (const hardware_interface::ComponentInfo& joint : info_.joints) { - if (joint.command_interfaces.size() != 2) { + if (joint.command_interfaces.size() != 5) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' has %zu command interfaces found. 2 expected.", joint.name.c_str(), + "Joint '%s' has %zu command interfaces found. 5 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -236,6 +236,9 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_)); + return state_interfaces; } @@ -248,6 +251,15 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &urcl_velocity_commands_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, "passthrough_position", + &passthrough_trajectory_positions_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, "passthrough_velocity", + &passthrough_trajectory_velocities_[i])); + + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, "passthrough_acceleration", &passthrough_trajectory_accelerations_[i])); } // Obtain the tf_prefix from the urdf so that we can have the general interface multiple times // NOTE using the tf_prefix at this point is some kind of workaround. One should actually go through the list of gpio @@ -283,6 +295,8 @@ std::vector URPositionHardwareInterface::e hardware_interface::CommandInterface(tf_prefix + "payload", "cog.z", &payload_center_of_gravity_[2])); command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_)); for (size_t i = 0; i < 18; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -307,27 +321,6 @@ std::vector URPositionHardwareInterface::e "passthrough_trajectory_transfer_state", &passthrough_trajectory_transfer_state_)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_)); - - for (size_t i = 0; i < 6; ++i) { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "passthrough_controller", "passthrough_trajectory_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_controller", "passthrough_trajectory_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_controller", "passthrough_trajectory_accelerations_" + std::to_string(i), - &passthrough_trajectory_accelerations_[i])); - } - command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller", "passthrough_trajectory_time_from_start", &passthrough_trajectory_time_from_start_)); @@ -853,20 +846,11 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } - if (key == tf_prefix + PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) { + if (key == info_.joints[i].name + "/" + PASSTHROUGH_TRAJECTORY_CONTROLLER + "_position") { start_modes_.push_back(PASSTHROUGH_TRAJECTORY_CONTROLLER); } } } - // 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 @@ -878,14 +862,61 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); } - if (key == PASSTHROUGH_TRAJECTORY_CONTROLLER + std::to_string(i)) { + if (key == info_.joints[i].name + "/" + PASSTHROUGH_TRAJECTORY_CONTROLLER + "_position") { stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH); } } } + + // set new mode to all interfaces at the same time + if (start_modes_.size() != 0 && start_modes_.size() != 6) { + RCLCPP_ERROR(get_logger(), "Either none or all joint interfaces must be started at once."); + ret_val = hardware_interface::return_type::ERROR; + } + + if (position_controller_running_ && + std::none_of(stop_modes_.begin(), stop_modes_.end(), + [](auto item) { return item == StoppingInterface::STOP_POSITION; }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_TRAJECTORY_CONTROLLER); + })) { + RCLCPP_ERROR(get_logger(), "Start of velocity or passthrough interface requested while there is the position " + "interface running."); + ret_val = hardware_interface::return_type::ERROR; + } + + if (velocity_controller_running_ && + std::none_of(stop_modes_.begin(), stop_modes_.end(), + [](auto item) { return item == StoppingInterface::STOP_VELOCITY; }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { + return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_TRAJECTORY_CONTROLLER); + })) { + RCLCPP_ERROR(get_logger(), "Start of position or passthrough interface requested while there is the velocity " + "interface running."); + ret_val = hardware_interface::return_type::ERROR; + } + + if (passthrough_trajectory_controller_running_ && + std::none_of(stop_modes_.begin(), stop_modes_.end(), + [](auto item) { return item == StoppingInterface::STOP_PASSTHROUGH; }) && + std::any_of(start_modes_.begin(), start_modes_.end(), [](auto& item) { + return (item == hardware_interface::HW_IF_VELOCITY || item == hardware_interface::HW_IF_POSITION); + })) { + RCLCPP_ERROR(get_logger(), "Start of position or velocity interface requested while there is the passthrough " + "interface running."); + 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())) { + RCLCPP_ERROR(get_logger(), "Can't mix different control domains for joint control."); + ret_val = hardware_interface::return_type::ERROR; + } + // 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()))) { + RCLCPP_ERROR(get_logger(), "Either none or all joint interfaces must be stopped at once."); ret_val = hardware_interface::return_type::ERROR; } diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 580107df6..374b4a120 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -227,6 +227,14 @@ + + + + + + + + From e224dfad8ad801bd0dc8a501299ea30f3bbd589c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 11:23:11 +0200 Subject: [PATCH 35/50] REVERT_ME: Use passthrough branch of description --- Universal_Robots_ROS2_Driver.jazzy.repos | 2 +- Universal_Robots_ROS2_Driver.rolling.repos | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Universal_Robots_ROS2_Driver.jazzy.repos b/Universal_Robots_ROS2_Driver.jazzy.repos index c9780c3dc..10997e996 100644 --- a/Universal_Robots_ROS2_Driver.jazzy.repos +++ b/Universal_Robots_ROS2_Driver.jazzy.repos @@ -6,7 +6,7 @@ repositories: Universal_Robots_ROS2_Description: type: git url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git - version: rolling + version: passthrough ur_msgs: type: git url: https://github.com/ros-industrial/ur_msgs.git diff --git a/Universal_Robots_ROS2_Driver.rolling.repos b/Universal_Robots_ROS2_Driver.rolling.repos index c9780c3dc..10997e996 100644 --- a/Universal_Robots_ROS2_Driver.rolling.repos +++ b/Universal_Robots_ROS2_Driver.rolling.repos @@ -6,7 +6,7 @@ repositories: Universal_Robots_ROS2_Description: type: git url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git - version: rolling + version: passthrough ur_msgs: type: git url: https://github.com/ros-industrial/ur_msgs.git From 6eb6def1310e61df9701cb0df0768d68692d6330 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 11:32:47 +0200 Subject: [PATCH 36/50] Add documentation for the passthrough controller --- ur_controllers/doc/index.rst | 114 +++++++++++++++++++++++++++++++++++ 1 file changed, 114 insertions(+) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index 099b3f8f4..7ac5afcdb 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -133,3 +133,117 @@ 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_controlers/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. + +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. + +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. It always has +to export position, velocity and acceleration interfaces in order to be able to project the full +JointTrajectory definition: + +.. 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. + +Additionally, the following interfaces are necessary to handle the control flow: + +.. code:: xml + + + + + + + +Implementation details / dataflow +""""""""""""""""""""""""""""""""" + +* A trajectory passed to the controller will be sent to the hardware component one by one. +* The controller will send one setpooint 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`` + state interface), the action will be aborted. +* When the action is preempted, execution on the hardware is preempted. From 3a0f5623c67b6aadd8c9fe6188a1a877405719cb Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 12:10:40 +0200 Subject: [PATCH 37/50] Store command interface instead of using indices --- .../passthrough_trajectory_controller.hpp | 11 +---- .../src/passthrough_trajectory_controller.cpp | 40 ++++++++++++++----- 2 files changed, 32 insertions(+), 19 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 080d9d17d..1d6026ea2 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -90,15 +90,6 @@ const double TRANSFER_STATE_DONE = 5.0; using namespace std::chrono_literals; // NOLINT -enum CommandInterfaces -{ - // The PASSTHROUGH_TRAJECTORY_TRANSFER_STATE value is used to keep track of which stage the transfer is in. - PASSTHROUGH_TRAJECTORY_TRANSFER_STATE = 18, - // The PASSTHROUGH_TRAJECTORY_ABORT value is used to indicate whether the trajectory has been cancelled from the - // hardware interface./ - PASSTHROUGH_TRAJECTORY_TIME_FROM_START = 19 -}; - class PassthroughTrajectoryController : public controller_interface::ControllerInterface { public: @@ -182,6 +173,8 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI std::optional> scaling_state_interface_; std::optional> abort_state_interface_; + std::optional> transfer_command_interface_; + std::optional> time_from_start_command_interface_; rclcpp::Clock::SharedPtr clock_; }; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index f49bc509c..46c9e13da 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -182,6 +182,30 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat } } + const std::string tf_prefix = passthrough_params_.tf_prefix; + { + const std::string interface_name = tf_prefix + "passthrough_controller/passthrough_trajectory_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 + "passthrough_controller/passthrough_trajectory_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); } @@ -190,8 +214,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const { const auto active_goal = *rt_active_goal_.readFromRT(); - const auto current_transfer_state = - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].get_value(); + const auto current_transfer_state = transfer_command_interface_->get().get_value(); if (active_goal && trajectory_active_) { if (current_transfer_state != TRANSFER_STATE_IDLE) { @@ -214,8 +237,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); max_trajectory_time_ = rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start)); - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value( - TRANSFER_STATE_WAITING_FOR_POINT); + 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(); @@ -224,7 +246,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const if (current_transfer_state == TRANSFER_STATE_WAITING_FOR_POINT) { if (current_index_ < active_joint_traj_.points.size()) { // Write the time_from_start parameter. - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TIME_FROM_START].set_value( + 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 @@ -251,13 +273,11 @@ controller_interface::return_type PassthroughTrajectoryController::update(const } } // Tell hardware interface that this point is ready to be read. - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value( - TRANSFER_STATE_TRANSFERRING); + 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()) { - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value( - TRANSFER_STATE_TRANSFER_DONE); + 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!"); } @@ -562,7 +582,7 @@ bool PassthroughTrajectoryController::check_goal_tolerance() void PassthroughTrajectoryController::end_goal() { trajectory_active_ = false; - command_interfaces_[CommandInterfaces::PASSTHROUGH_TRAJECTORY_TRANSFER_STATE].set_value(TRANSFER_STATE_IDLE); + transfer_command_interface_->get().set_value(TRANSFER_STATE_IDLE); } std::unordered_map From df7c9ab7c26f25285c715901c79248ee6aac1671 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 13:23:23 +0200 Subject: [PATCH 38/50] Correctly handle preemptions --- .../passthrough_trajectory_controller.hpp | 2 +- .../src/passthrough_trajectory_controller.cpp | 22 +++++++++++------- ur_robot_driver/src/hardware_interface.cpp | 23 ++++++++++--------- ur_robot_driver/urdf/ur.ros2_control.xacro | 2 +- 4 files changed, 28 insertions(+), 21 deletions(-) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 1d6026ea2..84d7c7e72 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -172,7 +172,7 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI static constexpr double NO_VAL = std::numeric_limits::quiet_NaN(); std::optional> scaling_state_interface_; - std::optional> abort_state_interface_; + std::optional> abort_command_interface_; std::optional> transfer_command_interface_; std::optional> time_from_start_command_interface_; diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 46c9e13da..041f4cc0e 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -111,9 +111,6 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::st conf.names.push_back(passthrough_params_.speed_scaling_interface_name); - const std::string tf_prefix = passthrough_params_.tf_prefix; - conf.names.push_back(tf_prefix + "passthrough_controller/passthrough_trajectory_abort"); - return conf; } @@ -131,6 +128,7 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co config.names.emplace_back(joint_name + "/passthrough_acceleration"); } + config.names.push_back(tf_prefix + "passthrough_controller/passthrough_trajectory_abort"); config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_time_from_start"); @@ -172,12 +170,12 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat { const std::string interface_name = passthrough_params_.tf_prefix + "passthrough_controller/" "passthrough_trajectory_abort"; - auto it = std::find_if(state_interfaces_.begin(), state_interfaces_.end(), + auto it = std::find_if(command_interfaces_.begin(), command_interfaces_.end(), [&](auto& interface) { return (interface.get_name() == interface_name); }); - if (it != state_interfaces_.end()) { - abort_state_interface_ = *it; + if (it != command_interfaces_.end()) { + abort_command_interface_ = *it; } else { - RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in state interfaces.", interface_name.c_str()); + RCLCPP_ERROR(get_node()->get_logger(), "Did not find '%s' in command interfaces.", interface_name.c_str()); return controller_interface::CallbackReturn::ERROR; } } @@ -220,7 +218,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const 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_state_interface_->get().get_value() == 1.0 && current_index_ > 0) { + 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(); @@ -326,6 +324,14 @@ controller_interface::return_type PassthroughTrajectoryController::update(const "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; diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 554d785cf..2d8ad5301 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -236,9 +236,6 @@ std::vector URPositionHardwareInterface::exp state_interfaces.emplace_back( hardware_interface::StateInterface(tf_prefix + "gpio", "program_running", &robot_program_running_copy_)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_)); - return state_interfaces; } @@ -295,8 +292,6 @@ std::vector URPositionHardwareInterface::e hardware_interface::CommandInterface(tf_prefix + "payload", "cog.z", &payload_center_of_gravity_[2])); command_interfaces.emplace_back( hardware_interface::CommandInterface(tf_prefix + "payload", "payload_async_success", &payload_async_success_)); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_)); for (size_t i = 0; i < 18; ++i) { command_interfaces.emplace_back(hardware_interface::CommandInterface( @@ -324,6 +319,8 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface(tf_prefix + "passthrough_controller", "passthrough_trajectory_time_from_start", &passthrough_trajectory_time_from_start_)); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + tf_prefix + "passthrough_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_)); return command_interfaces; } @@ -976,9 +973,12 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod 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. - */ - if (passthrough_trajectory_transfer_state_ == 2.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_); @@ -1031,11 +1031,12 @@ void URPositionHardwareInterface::check_passthrough_trajectory_controller() void URPositionHardwareInterface::trajectory_done_callback(urcl::control::TrajectoryResult result) { - if (result == urcl::control::TrajectoryResult::TRAJECTORY_RESULT_SUCCESS) { - passthrough_trajectory_transfer_state_ = 5.0; - } else { + 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; } diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 374b4a120..7ef20ea38 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -231,7 +231,7 @@ - + From 197552aa8e32f5cdbc5db584a79be93536255f14 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 13:44:17 +0200 Subject: [PATCH 39/50] Correctly abort and act on deactivate --- .../passthrough_trajectory_controller.hpp | 2 ++ .../src/passthrough_trajectory_controller.cpp | 15 +++++++++++++++ 2 files changed, 17 insertions(+) diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index 84d7c7e72..f0c7a95f1 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -106,6 +106,8 @@ class PassthroughTrajectoryController : public controller_interface::ControllerI 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: diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 041f4cc0e..85da201c2 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -207,6 +207,21 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat 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) { From 63bce673bc879b6d8ee5a2612614589e91213711 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 14:13:47 +0200 Subject: [PATCH 40/50] Update documentation --- ur_controllers/doc/index.rst | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index 7ac5afcdb..e0ab4993f 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -204,9 +204,11 @@ The trajectory passthrough controller uses the following parameters: Interfaces """""""""" -In order to use this, the hardware has to export a command interface for passthrough. It always has +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: +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 @@ -230,7 +232,7 @@ Additionally, the following interfaces are necessary to handle the control flow: - + Implementation details / dataflow @@ -245,5 +247,5 @@ Implementation details / dataflow * 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`` - state interface), the action will be aborted. + command interface), the action will be aborted. * When the action is preempted, execution on the hardware is preempted. From d6df554c9bbd56f7aa90220fb93561caa3e5c54a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 3 Oct 2024 14:18:26 +0200 Subject: [PATCH 41/50] Improve result representation in example_move script --- ur_robot_driver/scripts/example_move.py | 31 ++++++++++++++++++++++--- 1 file changed, 28 insertions(+), 3 deletions(-) diff --git a/ur_robot_driver/scripts/example_move.py b/ur_robot_driver/scripts/example_move.py index 28c12fa46..0f32146b5 100755 --- a/ur_robot_driver/scripts/example_move.py +++ b/ur_robot_driver/scripts/example_move.py @@ -39,6 +39,7 @@ 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 @@ -135,7 +136,9 @@ def execute_trajectory(self, traj_name): goal.trajectory = self.goals[traj_name] goal.goal_time_tolerance = Duration(sec=0, nanosec=500000000) - goal.goal_tolerance = [JointTolerance(position=0.01, name=self.joints[i]) for i in range(6)] + 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) @@ -153,11 +156,16 @@ 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: + 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 @@ -175,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) From 2121cf8efd62577c95b1e31d95d93f35c0f3df71 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 14 Oct 2024 11:35:01 +0200 Subject: [PATCH 42/50] Re-Restructure command interface exporting If we want to use other types for forwarding such as Cartesian setpoints or motion primitives it doesn't really make sense to have this as a part of the joints. --- ur_controllers/doc/index.rst | 40 ++++++++------ .../src/passthrough_trajectory_controller.cpp | 23 ++++---- .../ur_robot_driver/hardware_interface.hpp | 2 +- ur_robot_driver/src/hardware_interface.cpp | 55 +++++++++++-------- ur_robot_driver/urdf/ur.ros2_control.xacro | 26 +++++++-- 5 files changed, 88 insertions(+), 58 deletions(-) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index e0ab4993f..3fec6b801 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -212,29 +212,35 @@ 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. -Additionally, the following interfaces are necessary to handle the control flow: - -.. code:: xml - - - - - - - Implementation details / dataflow """"""""""""""""""""""""""""""""" diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 85da201c2..33a1e2447 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -121,16 +121,15 @@ controller_interface::InterfaceConfiguration PassthroughTrajectoryController::co const std::string tf_prefix = passthrough_params_.tf_prefix; - auto joint_names = passthrough_params_.joints; - for (auto& joint_name : joint_names) { - config.names.emplace_back(joint_name + "/passthrough_position"); - config.names.emplace_back(joint_name + "/passthrough_velocity"); - config.names.emplace_back(joint_name + "/passthrough_acceleration"); + 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 + "passthrough_controller/passthrough_trajectory_abort"); - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"); - config.names.emplace_back(tf_prefix + "passthrough_controller/passthrough_trajectory_time_from_start"); + 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; } @@ -168,8 +167,8 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat } { - const std::string interface_name = passthrough_params_.tf_prefix + "passthrough_controller/" - "passthrough_trajectory_abort"; + 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()) { @@ -182,7 +181,7 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat const std::string tf_prefix = passthrough_params_.tf_prefix; { - const std::string interface_name = tf_prefix + "passthrough_controller/passthrough_trajectory_transfer_state"; + 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()) { @@ -193,7 +192,7 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat } } { - const std::string interface_name = tf_prefix + "passthrough_controller/passthrough_trajectory_time_from_start"; + 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()) { 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 c6a3fc061..9241382bf 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -249,7 +249,7 @@ class URPositionHardwareInterface : public hardware_interface::SystemInterface urcl::RobotReceiveTimeout receive_timeout_ = urcl::RobotReceiveTimeout::millisec(20); - const std::string PASSTHROUGH_TRAJECTORY_CONTROLLER = "passthrough"; + const std::string PASSTHROUGH_GPIO = "trajectory_passthrough"; }; } // namespace ur_robot_driver diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 2d8ad5301..5fdcab847 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -101,9 +101,9 @@ URPositionHardwareInterface::on_init(const hardware_interface::HardwareInfo& sys trajectory_joint_accelerations_.clear(); for (const hardware_interface::ComponentInfo& joint : info_.joints) { - if (joint.command_interfaces.size() != 5) { + if (joint.command_interfaces.size() != 2) { RCLCPP_FATAL(rclcpp::get_logger("URPositionHardwareInterface"), - "Joint '%s' has %zu command interfaces found. 5 expected.", joint.name.c_str(), + "Joint '%s' has %zu command interfaces found. 2 expected.", joint.name.c_str(), joint.command_interfaces.size()); return hardware_interface::CallbackReturn::ERROR; } @@ -248,15 +248,6 @@ std::vector URPositionHardwareInterface::e command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &urcl_velocity_commands_[i])); - - command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, "passthrough_position", - &passthrough_trajectory_positions_[i])); - - command_interfaces.emplace_back(hardware_interface::CommandInterface(info_.joints[i].name, "passthrough_velocity", - &passthrough_trajectory_velocities_[i])); - - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, "passthrough_acceleration", &passthrough_trajectory_accelerations_[i])); } // Obtain the tf_prefix from the urdf so that we can have the general interface multiple times // NOTE using the tf_prefix at this point is some kind of workaround. One should actually go through the list of gpio @@ -312,15 +303,31 @@ 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_controller", - "passthrough_trajectory_transfer_state", + 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_controller", - "passthrough_trajectory_time_from_start", + 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_controller", "passthrough_trajectory_abort", &passthrough_trajectory_abort_)); + 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; } @@ -843,8 +850,8 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { start_modes_.push_back(hardware_interface::HW_IF_VELOCITY); } - if (key == info_.joints[i].name + "/" + PASSTHROUGH_TRAJECTORY_CONTROLLER + "_position") { - start_modes_.push_back(PASSTHROUGH_TRAJECTORY_CONTROLLER); + if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { + start_modes_.push_back(PASSTHROUGH_GPIO); } } } @@ -859,7 +866,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) { stop_modes_.push_back(StoppingInterface::STOP_VELOCITY); } - if (key == info_.joints[i].name + "/" + PASSTHROUGH_TRAJECTORY_CONTROLLER + "_position") { + if (key == tf_prefix + PASSTHROUGH_GPIO + "/setpoint_positions_" + std::to_string(i)) { stop_modes_.push_back(StoppingInterface::STOP_PASSTHROUGH); } } @@ -875,7 +882,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod std::none_of(stop_modes_.begin(), stop_modes_.end(), [](auto item) { return item == StoppingInterface::STOP_POSITION; }) && std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_TRAJECTORY_CONTROLLER); + return (item == hardware_interface::HW_IF_VELOCITY || item == PASSTHROUGH_GPIO); })) { RCLCPP_ERROR(get_logger(), "Start of velocity or passthrough interface requested while there is the position " "interface running."); @@ -886,7 +893,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod std::none_of(stop_modes_.begin(), stop_modes_.end(), [](auto item) { return item == StoppingInterface::STOP_VELOCITY; }) && std::any_of(start_modes_.begin(), start_modes_.end(), [this](auto& item) { - return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_TRAJECTORY_CONTROLLER); + return (item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO); })) { RCLCPP_ERROR(get_logger(), "Start of position or passthrough interface requested while there is the velocity " "interface running."); @@ -956,8 +963,8 @@ hardware_interface::return_type URPositionHardwareInterface::perform_command_mod 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_TRAJECTORY_CONTROLLER) != start_modes_.end()) { + } 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; diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 7ef20ea38..8c390af89 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -228,10 +228,28 @@ - - - - + + + + + + + + + + + + + + + + + + + + + + From 17ab6d0e6361906fb6a4b6bc92284617e8073f3c Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Mon, 14 Oct 2024 11:39:51 +0200 Subject: [PATCH 43/50] Revert "REVERT_ME: Use passthrough branch of description" This reverts commit e224dfad8ad801bd0dc8a501299ea30f3bbd589c. --- Universal_Robots_ROS2_Driver.jazzy.repos | 2 +- Universal_Robots_ROS2_Driver.rolling.repos | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/Universal_Robots_ROS2_Driver.jazzy.repos b/Universal_Robots_ROS2_Driver.jazzy.repos index 10997e996..c9780c3dc 100644 --- a/Universal_Robots_ROS2_Driver.jazzy.repos +++ b/Universal_Robots_ROS2_Driver.jazzy.repos @@ -6,7 +6,7 @@ repositories: Universal_Robots_ROS2_Description: type: git url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git - version: passthrough + version: rolling ur_msgs: type: git url: https://github.com/ros-industrial/ur_msgs.git diff --git a/Universal_Robots_ROS2_Driver.rolling.repos b/Universal_Robots_ROS2_Driver.rolling.repos index 10997e996..c9780c3dc 100644 --- a/Universal_Robots_ROS2_Driver.rolling.repos +++ b/Universal_Robots_ROS2_Driver.rolling.repos @@ -6,7 +6,7 @@ repositories: Universal_Robots_ROS2_Description: type: git url: https://github.com/UniversalRobots/Universal_Robots_ROS2_Description.git - version: passthrough + version: rolling ur_msgs: type: git url: https://github.com/ros-industrial/ur_msgs.git From ca7aff9bae8ea08a58f39822c18731cb614390b2 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 16 Oct 2024 16:20:27 +0200 Subject: [PATCH 44/50] Fix command interface names --- ur_robot_driver/urdf/ur.ros2_control.xacro | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ur_robot_driver/urdf/ur.ros2_control.xacro b/ur_robot_driver/urdf/ur.ros2_control.xacro index 72cf56e49..0d41c644d 100644 --- a/ur_robot_driver/urdf/ur.ros2_control.xacro +++ b/ur_robot_driver/urdf/ur.ros2_control.xacro @@ -236,12 +236,12 @@ - - - - - - + + + + + + From f887c09000a22e6f482c4e98d24cbcc70bc142c5 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 16 Oct 2024 20:00:26 +0200 Subject: [PATCH 45/50] Check passthrough trajectory controller from write function This should be thread-safe in contrast to calling it from the AsyncIO thread. --- ur_robot_driver/src/hardware_interface.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ur_robot_driver/src/hardware_interface.cpp b/ur_robot_driver/src/hardware_interface.cpp index 7152ebcd2..3fd9db044 100644 --- a/ur_robot_driver/src/hardware_interface.cpp +++ b/ur_robot_driver/src/hardware_interface.cpp @@ -572,9 +572,6 @@ void URPositionHardwareInterface::asyncThread() if (initialized_) { // RCLCPP_INFO(rclcpp::get_logger("URPositionHardwareInterface"), "Initialized in async thread"); checkAsyncIO(); - if (passthrough_trajectory_controller_running_) { - check_passthrough_trajectory_controller(); - } } std::this_thread::sleep_for(std::chrono::nanoseconds(20000000)); } @@ -692,6 +689,7 @@ hardware_interface::return_type URPositionHardwareInterface::write(const rclcpp: } else if (passthrough_trajectory_controller_running_) { ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); + check_passthrough_trajectory_controller(); } else { ur_driver_->writeKeepalive(); } From c7ebfad907673e114f01bed49c2fcd98bcdb18ff Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Sat, 26 Oct 2024 13:46:41 +0200 Subject: [PATCH 46/50] Apply suggestions from code review Co-authored-by: Vincenzo Di Pentima --- ur_controllers/doc/index.rst | 16 ++++++++-------- .../passthrough_trajectory_controller.hpp | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index a760c8439..8d8039da7 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -138,7 +138,7 @@ Advertised services .. _passthrough_trajectory_controller: -ur_controlers/PassthroughTrajectoryController +ur_controllers/PassthroughTrajectoryController ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ This controller uses a ``control_msgs/FollowJointTrajectory`` action but instead of interpolating @@ -221,12 +221,12 @@ directly to their joints. - - - - - - + + + + + + @@ -247,7 +247,7 @@ Implementation details / dataflow """"""""""""""""""""""""""""""""" * A trajectory passed to the controller will be sent to the hardware component one by one. -* The controller will send one setpooint and then wait for the hardware to acknowledge that it can +* 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 diff --git a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp index f0c7a95f1..a3c91d10f 100644 --- a/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp +++ b/ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp @@ -72,7 +72,7 @@ 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 trajecotry. When the state is 1.0, the controller will write a + * 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. From 5cca2f9d9a6c2f2df809a3fa6217e8e8c0183f29 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Sat, 26 Oct 2024 13:54:09 +0200 Subject: [PATCH 47/50] Add more docs --- ur_controllers/doc/index.rst | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/ur_controllers/doc/index.rst b/ur_controllers/doc/index.rst index 8d8039da7..a488d07e5 100644 --- a/ur_controllers/doc/index.rst +++ b/ur_controllers/doc/index.rst @@ -139,11 +139,13 @@ Advertised services .. _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. +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 @@ -165,6 +167,11 @@ controller basically tracks the scaled time instead of the real time. 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 """""""""" From e2343c91d6470cc94ddd2dc1b133011099a45a77 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Sat, 26 Oct 2024 14:23:23 +0200 Subject: [PATCH 48/50] Improve duration output --- ur_controllers/src/passthrough_trajectory_controller.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index 33a1e2447..f52cbfd6e 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -296,8 +296,7 @@ controller_interface::return_type PassthroughTrajectoryController::update(const // When the trajectory is finished, report the goal as successful to the client. } else if (current_transfer_state == TRANSFER_STATE_DONE) { - std::shared_ptr result = - std::make_shared(); + 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; @@ -317,10 +316,12 @@ controller_interface::return_type PassthroughTrajectoryController::update(const 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(), "Trajectory executed successfully in %f seconds!", - active_trajectory_elapsed_time_.seconds()); + 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. From 6b4ca4dc2371ddb25b697a52cc14796b593bca25 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 6 Nov 2024 13:53:51 +0100 Subject: [PATCH 49/50] Revert "REVERT_ME: use fixed ros2_control until merged" This reverts commit bc76a615f4a1d25fb357d2823e5e27c5fd955854. --- Universal_Robots_ROS2_Driver.jazzy.repos | 4 ++-- Universal_Robots_ROS2_Driver.rolling.repos | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/Universal_Robots_ROS2_Driver.jazzy.repos b/Universal_Robots_ROS2_Driver.jazzy.repos index d8bf87ce3..c85d2328f 100644 --- a/Universal_Robots_ROS2_Driver.jazzy.repos +++ b/Universal_Robots_ROS2_Driver.jazzy.repos @@ -13,8 +13,8 @@ repositories: version: humble-devel ros2_control: type: git - url: https://github.com/pal-robotics-forks/ros2_control.git - version: fix/controller_update_period + url: https://github.com/ros-controls/ros2_control.git + version: master ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers diff --git a/Universal_Robots_ROS2_Driver.rolling.repos b/Universal_Robots_ROS2_Driver.rolling.repos index d8bf87ce3..c85d2328f 100644 --- a/Universal_Robots_ROS2_Driver.rolling.repos +++ b/Universal_Robots_ROS2_Driver.rolling.repos @@ -13,8 +13,8 @@ repositories: version: humble-devel ros2_control: type: git - url: https://github.com/pal-robotics-forks/ros2_control.git - version: fix/controller_update_period + url: https://github.com/ros-controls/ros2_control.git + version: master ros2_controllers: type: git url: https://github.com/ros-controls/ros2_controllers From d706fa0c0980ef882ec9461dd00baf82605d6b50 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Thu, 7 Nov 2024 09:56:54 +0100 Subject: [PATCH 50/50] Fix if statement for claimed interfaces --- ur_controllers/src/passthrough_trajectory_controller.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/ur_controllers/src/passthrough_trajectory_controller.cpp b/ur_controllers/src/passthrough_trajectory_controller.cpp index f52cbfd6e..b0515539a 100644 --- a/ur_controllers/src/passthrough_trajectory_controller.cpp +++ b/ur_controllers/src/passthrough_trajectory_controller.cpp @@ -147,12 +147,11 @@ controller_interface::CallbackReturn PassthroughTrajectoryController::on_activat 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); } - } else if (interface_it->get_interface_name() == "acceleration") { - joint_acceleration_state_interface_.emplace_back(*interface_it); } }