From 30e4383d3057c8c1f01245454e5b6bedb671870e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 4 Oct 2023 19:54:11 +0000 Subject: [PATCH 1/6] Add scaling factor to JTC This adds a scaling factor between 0 and 1 to the JTC so that the trajectory time inside the controller is extended respectively. A value of 0.5 means that trajectory execution will take twice as long as the trajectory states. The scaling factor itself is read from the hardware for now. --- .../joint_trajectory_controller.hpp | 14 ++++++ .../src/joint_trajectory_controller.cpp | 46 ++++++++++++++++--- ...oint_trajectory_controller_parameters.yaml | 5 ++ 3 files changed, 58 insertions(+), 7 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index ae370df0a6..27cc86d3dd 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -53,6 +53,16 @@ using namespace std::chrono_literals; // NOLINT namespace joint_trajectory_controller { +struct TimeData +{ + TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0) + { + } + rclcpp::Time time; + rclcpp::Duration period; + rclcpp::Time uptime; +}; + class JointTrajectoryController : public controller_interface::ControllerInterface { public: @@ -162,6 +172,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; + // Things around speed scaling + double scaling_factor_{}; + realtime_tools::RealtimeBuffer time_data_; + // Timeout to consider commands old double cmd_timeout_; // True if holding position or repeating last trajectory point in case of success diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4a4b63cb75..4510020224 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -16,6 +16,7 @@ #include #include +#include #include #include @@ -116,12 +117,24 @@ JointTrajectoryController::state_interface_configuration() const conf.names.push_back(joint_name + "/" + interface_type); } } + conf.names.push_back(params_.speed_scaling_interface_name); return conf; } controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { + if (state_interfaces_.back().get_name() == params_.speed_scaling_interface_name) + { + scaling_factor_ = state_interfaces_.back().get_value(); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", + params_.speed_scaling_interface_name.c_str()); + } + if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; @@ -163,6 +176,18 @@ controller_interface::return_type JointTrajectoryController::update( // currently carrying out a trajectory if (has_active_trajectory()) { + // Adjust time with scaling factor + TimeData time_data; + time_data.time = time; + rcl_duration_value_t t_period = (time_data.time - time_data_.readFromRT()->time).nanoseconds(); + // explicitly cast (truncate fraction) + time_data.period = rclcpp::Duration::from_nanoseconds( + static_cast(scaling_factor_ * static_cast(t_period))); + time_data.uptime = time_data_.readFromRT()->uptime + time_data.period; + rclcpp::Time traj_time = + time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period); + time_data_.writeFromNonRT(time_data); + bool first_sample = false; // if sampling the first time, set the point before you sample if (!traj_external_point_ptr_->is_sampled_already()) @@ -171,19 +196,19 @@ controller_interface::return_type JointTrajectoryController::update( if (params_.open_loop_control) { traj_external_point_ptr_->set_point_before_trajectory_msg( - time, last_commanded_state_, joints_angle_wraparound_); + traj_time, last_commanded_state_, joints_angle_wraparound_); } else { traj_external_point_ptr_->set_point_before_trajectory_msg( - time, state_current_, joints_angle_wraparound_); + traj_time, state_current_, joints_angle_wraparound_); } } // find segment for current timestamp TrajectoryPointConstIter start_segment_itr, end_segment_itr; const bool valid_point = traj_external_point_ptr_->sample( - time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + traj_time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); if (valid_point) { @@ -444,7 +469,8 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), [](const auto & interface) + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -514,7 +540,8 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), [](const auto & interface) + joint_interface.begin(), joint_interface.end(), + [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -886,13 +913,18 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { // update the dynamic map parameters param_listener_->refresh_dynamic_parameters(); - // get parameters from the listener in case they were updated params_ = param_listener_->get_params(); - // parse remaining parameters default_tolerances_ = get_segment_tolerances(params_); + // Setup time_data buffer used for scaling + TimeData time_data; + time_data.time = get_node()->now(); + time_data.period = rclcpp::Duration::from_nanoseconds(0); + time_data.uptime = get_node()->now(); + time_data_.initRT(time_data); + // order all joints in the storage for (const auto & interface : params_.command_interfaces) { diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index b17229cab8..0940ca8ab3 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -39,6 +39,11 @@ joint_trajectory_controller: "joint_trajectory_controller::state_interface_type_combinations": null, } } + speed_scaling_interface_name: { + type: string, + default_value: "speed_scaling/speed_scaling_factor", + description: "Fully qualified name of the speed scaling interface name" + } allow_partial_joints_goal: { type: bool, default_value: false, From 1b0ce61520758c41982d5be0d0e301ec4d231e12 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 4 Oct 2023 19:58:51 +0000 Subject: [PATCH 2/6] Fixed formatting --- .../joint_trajectory_controller.hpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 27cc86d3dd..b60158f24c 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -55,9 +55,7 @@ namespace joint_trajectory_controller struct TimeData { - TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0) - { - } + TimeData() : time(0.0), period(rclcpp::Duration::from_nanoseconds(0.0)), uptime(0.0) {} rclcpp::Time time; rclcpp::Duration period; rclcpp::Time uptime; From 0b6823400d48a558fff70e308a84103d12d4d313 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Tue, 10 Oct 2023 14:39:50 +0200 Subject: [PATCH 3/6] use reset+initRT due to missing writeFromRT MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Co-authored-by: Christoph Fröhlich --- .../src/joint_trajectory_controller.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4510020224..110a833616 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -186,7 +186,8 @@ controller_interface::return_type JointTrajectoryController::update( time_data.uptime = time_data_.readFromRT()->uptime + time_data.period; rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period); - time_data_.writeFromNonRT(time_data); + time_data_.reset(); + time_data_.initRT(time_data); bool first_sample = false; // if sampling the first time, set the point before you sample From abbf52f2e9d8b599d5e0f24fcb788dbde7181ff5 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Wed, 13 Mar 2024 15:51:58 +0100 Subject: [PATCH 4/6] synchronization of scaling factor with hw optional, add service for setting of scaling factor --- .../joint_trajectory_controller.hpp | 8 +++ .../src/joint_trajectory_controller.cpp | 62 ++++++++++++++----- ...oint_trajectory_controller_parameters.yaml | 10 +++ 3 files changed, 66 insertions(+), 14 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index b60158f24c..6b90cb07f2 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -24,6 +24,7 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" +#include "control_msgs/srv/set_scaling_factor.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -311,8 +312,15 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + bool set_scaling_factor( + control_msgs::srv::SetScalingFactor::Request::SharedPtr req, + control_msgs::srv::SetScalingFactor::Response::SharedPtr resp); + urdf::Model model_; + realtime_tools::RealtimeBuffer scaling_factor_rt_buff_; + rclcpp::Service::SharedPtr set_scaling_factor_srv_; + /** * @brief Assigns the values from a trajectory point interface to a joint interface. * diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 110a833616..7700b8a508 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -117,22 +117,32 @@ JointTrajectoryController::state_interface_configuration() const conf.names.push_back(joint_name + "/" + interface_type); } } - conf.names.push_back(params_.speed_scaling_interface_name); + if (params_.exchange_scaling_factor_with_hardware) + { + conf.names.push_back(params_.speed_scaling_interface_name); + } return conf; } controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { - if (state_interfaces_.back().get_name() == params_.speed_scaling_interface_name) + if (params_.exchange_scaling_factor_with_hardware) { - scaling_factor_ = state_interfaces_.back().get_value(); + if (state_interfaces_.back().get_name() == params_.speed_scaling_interface_name) + { + scaling_factor_ = state_interfaces_.back().get_value(); + } + else + { + RCLCPP_ERROR( + get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", + params_.speed_scaling_interface_name.c_str()); + } } else { - RCLCPP_ERROR( - get_node()->get_logger(), "Speed scaling interface (%s) not found in hardware interface.", - params_.speed_scaling_interface_name.c_str()); + scaling_factor_ = *(scaling_factor_rt_buff_.readFromRT()); } if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) @@ -180,14 +190,13 @@ controller_interface::return_type JointTrajectoryController::update( TimeData time_data; time_data.time = time; rcl_duration_value_t t_period = (time_data.time - time_data_.readFromRT()->time).nanoseconds(); - // explicitly cast (truncate fraction) + // explicitly cast time_data.period = rclcpp::Duration::from_nanoseconds( - static_cast(scaling_factor_ * static_cast(t_period))); + static_cast(scaling_factor_ * static_cast(t_period))); time_data.uptime = time_data_.readFromRT()->uptime + time_data.period; rclcpp::Time traj_time = time_data_.readFromRT()->uptime + rclcpp::Duration::from_nanoseconds(t_period); - time_data_.reset(); - time_data_.initRT(time_data); + time_data_.writeFromNonRT(time_data); bool first_sample = false; // if sampling the first time, set the point before you sample @@ -470,8 +479,7 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) + joint_interface.begin(), joint_interface.end(), [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -541,8 +549,7 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( auto interface_has_values = [](const auto & joint_interface) { return std::find_if( - joint_interface.begin(), joint_interface.end(), - [](const auto & interface) + joint_interface.begin(), joint_interface.end(), [](const auto & interface) { return std::isnan(interface.get().get_value()); }) == joint_interface.end(); }; @@ -902,10 +909,19 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( resize_joint_trajectory_point(state_error_, dof_); resize_joint_trajectory_point(last_commanded_state_, dof_); + // create services query_state_srv_ = get_node()->create_service( std::string(get_node()->get_name()) + "/query_state", std::bind(&JointTrajectoryController::query_state_service, this, _1, _2)); + set_scaling_factor_srv_ = get_node()->create_service( + "~/set_scaling_factor", std::bind( + &JointTrajectoryController::set_scaling_factor, this, + std::placeholders::_1, std::placeholders::_2)); + + // set scaling factor to low value default + scaling_factor_rt_buff_.writeFromNonRT(params_.scaling_factor_initial_default); + return CallbackReturn::SUCCESS; } @@ -1601,6 +1617,24 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( } } +bool JointTrajectoryController::set_scaling_factor( + control_msgs::srv::SetScalingFactor::Request::SharedPtr req, + control_msgs::srv::SetScalingFactor::Response::SharedPtr resp) +{ + if (req->scaling_factor < 0 && req->scaling_factor > 1) + { + RCLCPP_WARN( + get_node()->get_logger(), "Scaling factor has to be in range [0, 1]. Ignoring input!"); + resp->success = false; + return true; + } + + RCLCPP_INFO(get_node()->get_logger(), "New scaling factor will be %f", req->scaling_factor); + scaling_factor_rt_buff_.writeFromNonRT(req->scaling_factor); + resp->success = true; + return true; +} + bool JointTrajectoryController::has_active_trajectory() const { return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 0940ca8ab3..721e1bc064 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -39,6 +39,16 @@ joint_trajectory_controller: "joint_trajectory_controller::state_interface_type_combinations": null, } } + exchange_scaling_factor_with_hardware: { + type: bool, + default_value: false, + description: "Flag that mark is hardware supports setting and reading of scaling factor. If set to 'true' corresponding command and state interfaces are created." + } + scaling_factor_initial_default: { + type: double, + default_value: 0.1, + description: "The initial value of the scaling factor if not exchange with hardware takes place." + } speed_scaling_interface_name: { type: string, default_value: "speed_scaling/speed_scaling_factor", From a685a5d0c3b38dd5d996148811fdfef9435eb923 Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 21 May 2024 10:51:23 +0200 Subject: [PATCH 5/6] Fix bug in setting scaling factor --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 7700b8a508..6e03e74852 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1621,7 +1621,7 @@ bool JointTrajectoryController::set_scaling_factor( control_msgs::srv::SetScalingFactor::Request::SharedPtr req, control_msgs::srv::SetScalingFactor::Response::SharedPtr resp) { - if (req->scaling_factor < 0 && req->scaling_factor > 1) + if (req->scaling_factor < 0 || req->scaling_factor > 1) { RCLCPP_WARN( get_node()->get_logger(), "Scaling factor has to be in range [0, 1]. Ignoring input!"); From 77e609d2e189e4e2656b2f70bb259438feb82cda Mon Sep 17 00:00:00 2001 From: Manuel M Date: Tue, 21 May 2024 10:52:12 +0200 Subject: [PATCH 6/6] Add service for getting the currently set scaling factor --- .../joint_trajectory_controller.hpp | 7 +++++++ .../src/joint_trajectory_controller.cpp | 14 ++++++++++++++ 2 files changed, 21 insertions(+) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index 6b90cb07f2..33657b7d37 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp @@ -25,6 +25,7 @@ #include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" #include "control_msgs/srv/set_scaling_factor.hpp" +#include "control_msgs/srv/get_scaling_factor.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" @@ -32,6 +33,7 @@ #include "joint_trajectory_controller/tolerances.hpp" #include "joint_trajectory_controller/trajectory.hpp" #include "joint_trajectory_controller/visibility_control.h" + #include "rclcpp/duration.hpp" #include "rclcpp/subscription.hpp" #include "rclcpp/time.hpp" @@ -315,11 +317,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool set_scaling_factor( control_msgs::srv::SetScalingFactor::Request::SharedPtr req, control_msgs::srv::SetScalingFactor::Response::SharedPtr resp); + + bool get_scaling_factor( + control_msgs::srv::GetScalingFactor::Request::SharedPtr req, + control_msgs::srv::GetScalingFactor::Response::SharedPtr resp); urdf::Model model_; realtime_tools::RealtimeBuffer scaling_factor_rt_buff_; rclcpp::Service::SharedPtr set_scaling_factor_srv_; + rclcpp::Service::SharedPtr get_scaling_factor_srv_; /** * @brief Assigns the values from a trajectory point interface to a joint interface. diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 6e03e74852..1421511bcb 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -918,6 +918,11 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "~/set_scaling_factor", std::bind( &JointTrajectoryController::set_scaling_factor, this, std::placeholders::_1, std::placeholders::_2)); + + get_scaling_factor_srv_ = get_node()->create_service( + "~/get_scaling_factor", std::bind( + &JointTrajectoryController::get_scaling_factor, this, + std::placeholders::_1, std::placeholders::_2)); // set scaling factor to low value default scaling_factor_rt_buff_.writeFromNonRT(params_.scaling_factor_initial_default); @@ -1635,6 +1640,15 @@ bool JointTrajectoryController::set_scaling_factor( return true; } +bool JointTrajectoryController::get_scaling_factor( + control_msgs::srv::GetScalingFactor::Request::SharedPtr /*req*/, + control_msgs::srv::GetScalingFactor::Response::SharedPtr resp) +{ + resp->scaling_factor = *(scaling_factor_rt_buff_.readFromNonRT()); + resp->success = true; + return true; +} + bool JointTrajectoryController::has_active_trajectory() const { return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg();