From bbeaa65635cb5e5a45b4dd62ec9d9e1822c66af9 Mon Sep 17 00:00:00 2001 From: URJala Date: Mon, 11 Mar 2024 09:35:20 +0000 Subject: [PATCH] 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 fe1ede6f7..9bc22a267 100644 --- a/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp +++ b/ur_robot_driver/include/ur_robot_driver/hardware_interface.hpp @@ -76,7 +76,8 @@ enum StoppingInterface { NONE, STOP_POSITION, - STOP_VELOCITY + STOP_VELOCITY, + STOP_FORWARD }; /*! @@ -220,6 +221,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 8ff61080e..a33b0a82b 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; @@ -632,6 +633,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); + } else if (forward_position_controller_running_) { + ur_driver_->writeTrajectoryControlMessage(urcl::control::TrajectoryControlMessage::TRAJECTORY_NOOP); } else { ur_driver_->writeKeepalive(); }