From 2db6392491542c8b95ba5ce425113b30ed6dcc1a Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Wed, 4 Oct 2023 19:54:11 +0000 Subject: [PATCH] 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 | 31 +++++++++++++++++-- ...oint_trajectory_controller_parameters.yaml | 5 +++ 3 files changed, 47 insertions(+), 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 b3e2f55742..266ea07f29 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 @@ -64,6 +64,16 @@ namespace joint_trajectory_controller { class Trajectory; +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: @@ -174,6 +184,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_; // Are we holding position? diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e39bffc73..9700be035d 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -17,6 +17,7 @@ #include #include #include +#include #include #include #include @@ -114,12 +115,20 @@ 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; @@ -189,6 +198,15 @@ 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(); + time_data.period = rclcpp::Duration::from_nanoseconds(scaling_factor_ * 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()) @@ -196,18 +214,18 @@ controller_interface::return_type JointTrajectoryController::update( first_sample = true; if (params_.open_loop_control) { - traj_external_point_ptr_->set_point_before_trajectory_msg(time, last_commanded_state_); + traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, last_commanded_state_); } else { - traj_external_point_ptr_->set_point_before_trajectory_msg(time, state_current_); + traj_external_point_ptr_->set_point_before_trajectory_msg(traj_time, state_current_); } } // 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) { @@ -934,6 +952,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( controller_interface::CallbackReturn JointTrajectoryController::on_activate( const rclcpp_lifecycle::State &) { + // 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 ce17ba8bf9..4658cf7453 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,