diff --git a/joint_trajectory_controller/doc/speed_scaling.rst b/joint_trajectory_controller/doc/speed_scaling.rst new file mode 100644 index 0000000000..e920e16a83 --- /dev/null +++ b/joint_trajectory_controller/doc/speed_scaling.rst @@ -0,0 +1,104 @@ +Speed scaling +============= + +The ``joint_trajectory_controller`` (JTC) supports dynamically scaling its trajectory execution speed. +That means, when specifying a scaling factor :math:`{f}` of less than 1, execution will proceed only +:math:`{f \cdot \Delta_t}` per control step where :math:`{\Delta_t}` is the controller's cycle time. + +Methods of speed scaling +------------------------ + +Generally, the speed scaling feature has two separate scaling approaches in mind: On-Robot scaling +and On-Controller scaling. They are both conceptually different and to correctly configure speed +scaling it is important to understand the differences. + +On-Robot speed scaling +~~~~~~~~~~~~~~~~~~~~~~ + +This scaling method is intended for robots that provide a scaling feature directly on the robot's +teach pendant and / or through a safety feature. One example of such robots are the `Universal +Robots manipulators `_. + +For the scope of this documentation a user-defined scaling and a safety-limited scaling will be +treated the same resulting in a "hardware scaling factor". + +In this setup, the hardware will treat the command sent from the ROS controller (e.g. Reach joint +configuration :math:`{\theta}` within :math:`{\Delta_t}` seconds.). This effectively means that the +robot will only make half of the way towards the target configuration when a scaling factor of 0.5 +is given (neglectling acceleration and deceleration influcences during this time period). + +The following plot shows trajectory execution (for one joint) with a hardware-scaled execution and +a controller that is **not** aware of speed scaling: + +.. image:: traj_without_speed_scaling.png + :alt: Trajectory with a hardware-scaled-down execution with a non-scaled controller + +The graph shows a trajectory with one joint being moved to a target point and back to its starting +point. As the joint's speed is limited to a very low setting on the teach pendant, speed scaling +(black line) activates and limits the joint speed (green line). As a result, the target trajectory +(light blue) doesn't get executed by the robot, but instead the pink trajectory is executed. The +vertical distance between the light blue line and the pink line is the path error in each control +cycle. We can see that the path deviation gets above 300 degrees at some point and the target point +at -6 radians never gets reached. + +With the scaled version of the trajectory controller the example motion shown in the previous diagram becomes: + +.. image:: traj_with_speed_scaling.png + :alt: Trajectory with a hardware-scaled-down execution with a scaled controller + +The deviation between trajectory interpolation on the ROS side and actual robot execution stays +minimal and the robot reaches the intermediate setpoint instead of returning "too early" as in the +example above. + +Scaling is done in such a way, that the time in the trajectory is virtually scaled. For example, if +a controller runs with a cycle time of 100 Hz, each control cycle is 10 ms long. A speed scaling of +0.5 means that in each time step the trajectory is moved forward by 5 ms instead. +So, the beginning of the 3rd timestep is 15 ms instead of 30 ms in the trajectory. + +Command sampling is performed as in the unscaled case, with the timestep's start plus the **full** +cycle time of 10 ms. The robot will scale down the motion command by 50% resulting in only half of +the distance being executed, which is why the next control cycle will be started at the current +start plus half of the step time. + + +On-Controller speed scaling +~~~~~~~~~~~~~~~~~~~~~~~~~~~ + +Conceptionally, with this scaling the robot hardware isn't aware of any scaling happening. The JTC +generates commands to be sent to the robot that are already scaled down accordingly, so they can be +directly executed by the robot. + +Since the hardware isn't aware of speed scaling, the speed-scaling related command and state +interfaces should not be specified and the scaling factor will be set through the +``~/speed_scaling_input`` topic directly: + +.. code:: console + + $ ros2 topic pub --qos-durability transient_local --once \ + /joint_trajectory_controller/speed_scaling_input control_msgs/msg/SpeedScalingFactor "{factor: 0.5}" + + +.. note:: + The ``~/speed_scaling_input`` topic uses the QoS durability profile ``transient_local``. This + means you can restart the controller while still having a publisher on that topic active. + +.. note:: + The current implementation only works for position-based interfaces. + + +Effect on tolerances +-------------------- + +When speed scaling is used while executing a trajectory, the tolerances configured for execution +will be scaled, as well. + +Since commands are generated from the scaled trajectory time, **path errors** will also be compared +to the scaled trajectory. + +The **goal time tolerance** also uses the virtual trajectory time. This means that a trajectory +being executed with a constant scaling factor of 0.5 will take twice as long for execution than the +``time_from_start`` value of the last trajectory point specifies. As long as the robot doesn't take +longer than that the goal time tolerance is considered to be met. + +If an application relies on the actual execution time as set in the ``time_from_start`` fields, an +external monitoring has to be wrapped around the trajectory execution action. diff --git a/joint_trajectory_controller/doc/traj_with_speed_scaling.png b/joint_trajectory_controller/doc/traj_with_speed_scaling.png new file mode 100644 index 0000000000..ce7fcd389d Binary files /dev/null and b/joint_trajectory_controller/doc/traj_with_speed_scaling.png differ diff --git a/joint_trajectory_controller/doc/traj_without_speed_scaling.png b/joint_trajectory_controller/doc/traj_without_speed_scaling.png new file mode 100644 index 0000000000..ed0b35dee6 Binary files /dev/null and b/joint_trajectory_controller/doc/traj_without_speed_scaling.png differ diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 4dcb71a064..6727ef7de7 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -206,6 +206,7 @@ Further information :titlesonly: Trajectory Representation + Speed scaling joint_trajectory_controller Parameters rqt_joint_trajectory_controller <../../rqt_joint_trajectory_controller/doc/userdoc.rst> 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 af0f35be18..0648d1c42f 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 @@ -22,9 +22,11 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" +#include "control_msgs/msg/speed_scaling_factor.hpp" #include "control_msgs/srv/query_trajectory_state.hpp" #include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" +#include "hardware_interface/loaned_command_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/interpolation_methods.hpp" #include "joint_trajectory_controller/tolerances.hpp" @@ -97,6 +99,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa trajectory_msgs::msg::JointTrajectoryPoint command_next_; trajectory_msgs::msg::JointTrajectoryPoint state_desired_; trajectory_msgs::msg::JointTrajectoryPoint state_error_; + trajectory_msgs::msg::JointTrajectoryPoint goal_error_; // Degrees of freedom size_t dof_; @@ -124,6 +127,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa InterfaceReferences joint_command_interface_; InterfaceReferences joint_state_interface_; + std::optional> + scaling_state_interface_; + std::optional> + scaling_command_interface_; bool has_position_state_interface_ = false; bool has_velocity_state_interface_ = false; @@ -145,6 +152,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 + std::atomic scaling_factor_{1.0}; + std::atomic scaling_factor_cmd_{1.0}; + // Timeout to consider commands old double cmd_timeout_; // True if holding position or repeating last trajectory point in case of success @@ -269,6 +280,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void resize_joint_trajectory_point_command( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + bool set_scaling_factor(const double scaling_factor); + using SpeedScalingMsg = control_msgs::msg::SpeedScalingFactor; + rclcpp::Subscription::SharedPtr scaling_factor_sub_; /** * @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 a4ad5210be..014eb8c370 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -122,12 +122,17 @@ JointTrajectoryController::command_interface_configuration() const conf.names.push_back(joint_name + "/" + interface_type); } } + if (!params_.speed_scaling_command_interface_name.empty()) + { + conf.names.push_back(params_.speed_scaling_command_interface_name); + } return conf; } controller_interface::InterfaceConfiguration JointTrajectoryController::state_interface_configuration() const { + const auto logger = get_node()->get_logger(); controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; conf.names.reserve(dof_ * params_.state_interfaces.size()); @@ -138,16 +143,39 @@ JointTrajectoryController::state_interface_configuration() const conf.names.push_back(joint_name + "/" + interface_type); } } + if (!params_.speed_scaling_state_interface_name.empty()) + { + conf.names.push_back(params_.speed_scaling_state_interface_name); + } return conf; } controller_interface::return_type JointTrajectoryController::update( const rclcpp::Time & time, const rclcpp::Duration & period) { + if (scaling_state_interface_.has_value()) + { + scaling_factor_ = scaling_state_interface_->get().get_value(); + } + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { return controller_interface::return_type::OK; } + + if (get_lifecycle_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_ACTIVE) + { + if (scaling_command_interface_.has_value()) + { + if (!scaling_command_interface_->get().set_value(scaling_factor_cmd_.load())) + { + RCLCPP_ERROR( + get_node()->get_logger(), + "Could not set speed scaling factor through command interfaces."); + } + } + } + auto logger = this->get_node()->get_logger(); // update dynamic parameters if (param_listener_->is_old(params_)) @@ -206,18 +234,21 @@ controller_interface::return_type JointTrajectoryController::update( } else { - traj_time_ += period; + traj_time_ += period * scaling_factor_.load(); } // Sample expected state from the trajectory traj_external_point_ptr_->sample( traj_time_, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + state_desired_.time_from_start = traj_time_ - traj_external_point_ptr_->time_from_start(); // Sample setpoint for next control cycle const bool valid_point = traj_external_point_ptr_->sample( traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr, end_segment_itr, false); + state_current_.time_from_start = time - traj_external_point_ptr_->time_from_start(); + if (valid_point) { const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start(); @@ -263,23 +294,26 @@ controller_interface::return_type JointTrajectoryController::update( tolerance_violated_while_moving = true; } // past the final point, check that we end up inside goal tolerance - if ( - !before_last_point && *(rt_is_holding_.readFromRT()) == false && - !check_state_tolerance_per_joint( - state_error_, index, active_tol->goal_state_tolerance[index], false /* show_errors */)) + if (!before_last_point && *(rt_is_holding_.readFromRT()) == false) { - outside_goal_tolerance = true; - - if (active_tol->goal_time_tolerance != 0.0) + compute_error_for_joint(goal_error_, index, state_current_, command_next_); + if (!check_state_tolerance_per_joint( + goal_error_, index, active_tol->goal_state_tolerance[index], + false /* show_errors */)) { - // if we exceed goal_time_tolerance set it to aborted - if (time_difference > active_tol->goal_time_tolerance) + outside_goal_tolerance = true; + + if (active_tol->goal_time_tolerance != 0.0) { - within_goal_time = false; - // print once, goal will be aborted afterwards - check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], - true /* show_errors */); + // if we exceed goal_time_tolerance set it to aborted + if (time_difference > active_tol->goal_time_tolerance) + { + within_goal_time = false; + // print once, goal will be aborted afterwards + check_state_tolerance_per_joint( + goal_error_, index, default_tolerances_.goal_state_tolerance[index], + true /* show_errors */); + } } } } @@ -335,7 +369,6 @@ controller_interface::return_type JointTrajectoryController::update( auto feedback = std::make_shared(); feedback->header.stamp = time; feedback->joint_names = params_.joints; - feedback->actual = state_current_; feedback->desired = state_desired_; feedback->error = state_error_; @@ -878,12 +911,49 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( resize_joint_trajectory_point_command(command_current_, dof_); resize_joint_trajectory_point(state_desired_, dof_); resize_joint_trajectory_point(state_error_, dof_); + resize_joint_trajectory_point(goal_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)); + if ( + !has_velocity_command_interface_ && !has_acceleration_command_interface_ && + !has_effort_command_interface_) + { + auto qos = rclcpp::SystemDefaultsQoS(); + qos.transient_local(); + scaling_factor_sub_ = get_node()->create_subscription( + "~/speed_scaling_input", qos, + [&](const SpeedScalingMsg & msg) { set_scaling_factor(msg.factor); }); + RCLCPP_INFO( + logger, "Setting initial scaling factor to %2f", params_.scaling_factor_initial_default); + scaling_factor_ = params_.scaling_factor_initial_default; + } + else + { + RCLCPP_WARN( + logger, + "Speed scaling is currently only supported for position interfaces. If you want to make use " + "of speed scaling, please only use a position interface when configuring this controller."); + scaling_factor_ = 1.0; + } + if (!params_.speed_scaling_state_interface_name.empty()) + { + RCLCPP_INFO( + logger, "Using scaling state from the hardware from interface %s.", + params_.speed_scaling_state_interface_name.c_str()); + } + else + { + RCLCPP_INFO( + get_node()->get_logger(), + "No scaling interface set. This controller will not read speed scaling from the hardware."); + } + scaling_factor_cmd_.store(scaling_factor_.load()); + if (get_update_rate() == 0) { throw std::runtime_error("Controller's update rate is set to 0. This should not happen!"); @@ -908,6 +978,42 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // parse remaining parameters default_tolerances_ = get_segment_tolerances(logger, params_); + // Set scaling interfaces + if (!params_.speed_scaling_state_interface_name.empty()) + { + auto it = std::find_if( + state_interfaces_.begin(), state_interfaces_.end(), [&](auto & interface) + { return (interface.get_name() == params_.speed_scaling_state_interface_name); }); + if (it != state_interfaces_.end()) + { + scaling_state_interface_ = *it; + } + else + { + RCLCPP_ERROR( + logger, "Did not find speed scaling interface '%s' in state interfaces.", + params_.speed_scaling_state_interface_name.c_str()); + return CallbackReturn::ERROR; + } + } + if (!params_.speed_scaling_command_interface_name.empty()) + { + auto it = std::find_if( + command_interfaces_.begin(), command_interfaces_.end(), [&](auto & interface) + { return (interface.get_name() == params_.speed_scaling_command_interface_name); }); + if (it != command_interfaces_.end()) + { + scaling_command_interface_ = *it; + } + else + { + RCLCPP_ERROR( + logger, "Did not find speed scaling interface '%s' in command interfaces.", + params_.speed_scaling_command_interface_name.c_str()); + return CallbackReturn::ERROR; + } + } + // order all joints in the storage for (const auto & interface : params_.command_interfaces) { @@ -1097,6 +1203,7 @@ void JointTrajectoryController::publish_state( { state_publisher_->msg_.output = command_current_; } + state_publisher_->msg_.speed_scaling_factor = scaling_factor_; state_publisher_->unlockAndPublish(); } @@ -1579,6 +1686,41 @@ void JointTrajectoryController::resize_joint_trajectory_point_command( } } +bool JointTrajectoryController::set_scaling_factor(const double scaling_factor) +{ + if (scaling_factor < 0) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Scaling factor has to be greater or equal to 0.0 - Ignoring input!"); + return false; + } + + if (scaling_factor != scaling_factor_.load()) + { + RCLCPP_INFO( + get_node()->get_logger().get_child("speed_scaling"), "New scaling factor will be %f", + scaling_factor); + } + scaling_factor_.store(scaling_factor); + if (params_.speed_scaling_command_interface_name.empty()) + { + if (!params_.speed_scaling_state_interface_name.empty()) + { + RCLCPP_WARN( + get_node()->get_logger(), + "Setting the scaling factor while only one-way communication with the hardware is setup. " + "This will likely get overwritten by the hardware again. If available, please also setup " + "the speed_scaling_command_interface_name"); + } + } + else + { + scaling_factor_cmd_.store(scaling_factor); + } + 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 14b71f0711..e7120c4ec5 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -39,6 +39,27 @@ joint_trajectory_controller: "joint_trajectory_controller::state_interface_type_combinations": null, } } + scaling_factor_initial_default: { + type: double, + default_value: 1.0, + read_only: true, + description: "The initial value of the scaling factor if no exchange with hardware takes place.", + validation: { + gt_eq<>: 0.0, + } + } + speed_scaling_state_interface_name: { + type: string, + default_value: "", + read_only: true, + description: "Fully qualified name of the speed scaling state interface name" + } + speed_scaling_command_interface_name: { + type: string, + default_value: "", + read_only: true, + description: "Command interface name used for setting the speed scaling factor on the hardware." + } allow_partial_joints_goal: { type: bool, default_value: false, diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index d4979deef8..1ecf5826d2 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -219,6 +219,15 @@ class TestTrajectoryActionsTestParameterized static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } }; +class TestTrajectoryActionsTestScalingFactor : public TestTrajectoryActions, + public ::testing::WithParamInterface +{ +public: + virtual void SetUp() { TestTrajectoryActions::SetUp(); } + + static void TearDownTestCase() { TrajectoryControllerTest::TearDownTestCase(); } +}; + TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal) { // deactivate velocity tolerance @@ -431,7 +440,6 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) // i.e., active but trivial trajectory (one point only) expectCommandPoint(points_positions.at(0)); } - /** * Makes sense with position command interface only, * because no integration to position state interface is implemented @@ -1012,3 +1020,164 @@ INSTANTIATE_TEST_SUITE_P( std::make_tuple( std::vector({"effort"}), std::vector({"position", "velocity", "acceleration"})))); + +TEST_P(TestTrajectoryActionsTestScalingFactor, test_scaling_execution_time_succeeds) +{ + double scaling_factor = GetParam(); + const double state_tol = 0.3; // Since we use a common buffer for cmd and state in these tests, + const double goal_tol = 1e-10; + // the error will be whatever the command diff is. + std::vector params = { + rclcpp::Parameter("open_loop_control", false), + rclcpp::Parameter("scaling_factor_initial_default", scaling_factor), + rclcpp::Parameter("constraints.joint1.trajectory", state_tol), + rclcpp::Parameter("constraints.joint2.trajectory", state_tol), + rclcpp::Parameter("constraints.joint3.trajectory", state_tol), + rclcpp::Parameter("constraints.joint1.goal", goal_tol), + rclcpp::Parameter("constraints.joint2.goal", goal_tol), + rclcpp::Parameter("constraints.joint3.goal", goal_tol), + // the test hw does not report velocity, so this constraint will not do anything + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.01), + }; + SetUpExecutor({params}, false, 1.0, 0.0); + SetUpControllerHardware(); + + // add feedback + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr feedback_msg) + { + auto time_diff_sec = [](const builtin_interfaces::msg::Duration & msg) + { return static_cast(msg.sec) + static_cast(msg.nanosec) * 1e-9; }; + + // Since we are summing up scaled periods, the scale of the period sum will not be the same + // due to numerical errors. + EXPECT_NEAR( + time_diff_sec(feedback_msg->desired.time_from_start), + time_diff_sec(feedback_msg->actual.time_from_start) * scaling_factor, + 1e-3 * time_diff_sec(feedback_msg->actual.time_from_start)); + }; + + std::shared_future gh_future; + // send goal + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.1); + point1.positions.resize(joint_names_.size()); + + point1.positions = points_positions.at(0); + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.2); + point2.positions.resize(joint_names_.size()); + + point2.positions = points_positions.at(1); + points.push_back(point2); + + gh_future = sendActionGoal(points, 0.1, goal_options_); + controller_hw_thread_.join(); + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); + + // run an update + updateControllerAsync(rclcpp::Duration::from_seconds(0.01)); + + // it should be holding the last position goal + // i.e., active but trivial trajectory (one point only) + // note: the action goal also is a trivial trajectory + expectCommandPoint(points_positions[1]); + + // Run a second trajectory + SetUpControllerHardware(); + gh_future = sendActionGoal(points, 1.0, goal_options_); + std::cout << "Waiting for another trajectory to finish" << std::endl; + controller_hw_thread_.join(); + std::cout << "trajectory_done" << std::endl; + + EXPECT_TRUE(gh_future.get()); + EXPECT_EQ(rclcpp_action::ResultCode::SUCCEEDED, common_resultcode_); +} + +TEST_P(TestTrajectoryActionsTestScalingFactor, test_scaling_sampling_is_correct) +{ + double scaling_factor = GetParam(); + std::vector params = { + rclcpp::Parameter("scaling_factor_initial_default", scaling_factor), + rclcpp::Parameter("constraints.joint1.goal", 1e-3), + rclcpp::Parameter("constraints.joint2.goal", 1e-3), + rclcpp::Parameter("constraints.joint3.goal", 1e-3), + rclcpp::Parameter("constraints.goal_time", 0.1), + }; + SetUpExecutor(params, true, 1.0, 0.0); + // SetUpControllerHardware(); + + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.1); + point1.positions.resize(joint_names_.size()); + + point1.positions = points_positions.at(0); + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.2); + point2.positions.resize(joint_names_.size()); + + point2.positions = points_positions.at(1); + points.push_back(point2); + + std::shared_future gh_future = + sendActionGoal(points, 1.0, goal_options_); + // sometimes doesn't receive calls when we don't sleep + std::this_thread::sleep_for(std::chrono::milliseconds(300)); + + auto trajectory_msg = std::make_shared(); + trajectory_msg->joint_names = joint_names_; + trajectory_msg->points = points; + + joint_trajectory_controller::Trajectory trajectory(trajectory_msg); + + auto clock = rclcpp::Clock(RCL_STEADY_TIME); + rclcpp::Time sample_time = clock.now(); + rclcpp::Time scaled_sample_time = sample_time; + const rclcpp::Duration controller_period = + rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); + + trajectory_msgs::msg::JointTrajectoryPoint initial_pt; + initial_pt.positions = INITIAL_POS_JOINTS; + trajectory.set_point_before_trajectory_msg(sample_time, initial_pt, {false, false, false}); + + const auto end_time = sample_time + points.back().time_from_start; + + // Stop earlier, as we will set the + // final joint value once we reached the last segment. The assumption that the reference is the + // same as the sampled scaled trajectory isn't true anymore + while (scaled_sample_time + controller_period <= end_time) + { + traj_controller_->update(sample_time, controller_period); + for (size_t i = 0; i < joint_state_pos_.size(); ++i) + { + joint_state_pos_[i] += (joint_pos_[i] - joint_state_pos_[i]) * scaling_factor; + } + trajectory_msgs::msg::JointTrajectoryPoint sampled_point; + joint_trajectory_controller::TrajectoryPointConstIter start_segment_itr, end_segment_itr; + trajectory.sample( + scaled_sample_time, joint_trajectory_controller::interpolation_methods::DEFAULT_INTERPOLATION, + sampled_point, start_segment_itr, end_segment_itr); + + auto state_reference = traj_controller_->get_state_reference(); + + EXPECT_EQ(sampled_point.positions, state_reference.positions); + + sample_time += controller_period; + scaled_sample_time += controller_period * scaling_factor; + } +} + +INSTANTIATE_TEST_SUITE_P( + ScaledJTCTests, TestTrajectoryActionsTestScalingFactor, ::testing::Values(0.25, 0.87, 1.0, 2.0)); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 21d0277541..69899b55fd 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1686,7 +1686,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro rclcpp::executors::SingleThreadedExecutor executor; // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", false); - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + rclcpp::Parameter update_rate_param("update_rate", 100); + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters, update_rate_param}, true); if (traj_controller_->has_position_command_interface() == false) { @@ -2207,3 +2209,196 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); } + +TEST_F(TrajectoryControllerTest, setting_scaling_factor_works_correctly) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = {}; + SetUpAndActivateTrajectoryController(executor, params); + auto speed_scaling_pub = node_->create_publisher( + controller_name_ + "/speed_scaling_input", rclcpp::SystemDefaultsQoS()); + subscribeToState(executor); + + control_msgs::msg::SpeedScalingFactor msg; + msg.factor = 0.765; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + + updateController(); + + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); + EXPECT_EQ(state->speed_scaling_factor, 0.765); + + // 0.0 should work as an edge case + msg.factor = 0.0; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + updateController(); + executor.spin_some(); + state = getState(); + EXPECT_EQ(state->speed_scaling_factor, 0.0); + + // Sending a negative value will be ignored + msg.factor = 0.45; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + msg.factor = -0.12; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + updateController(); + executor.spin_some(); + state = getState(); + EXPECT_EQ(state->speed_scaling_factor, 0.45); +} + +TEST_F(TrajectoryControllerTest, scaling_factor_from_param) +{ + double initial_factor = 0.123; + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = { + rclcpp::Parameter("scaling_factor_initial_default", initial_factor), + }; + SetUpAndActivateTrajectoryController(executor, params); + subscribeToState(executor); + updateController(); + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); + EXPECT_EQ(state->speed_scaling_factor, initial_factor); +} + +TEST_F( + TrajectoryControllerTest, wrong_scaling_state_interface_parameter_controller_fails_activation) +{ + double initial_factor = 0.123; + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = { + rclcpp::Parameter("scaling_factor_initial_default", initial_factor), + rclcpp::Parameter("speed_scaling_state_interface_name", "idontexist"), + }; + SetUpTrajectoryController(executor, params); + + auto state = traj_controller_->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + state = ActivateTrajectoryController(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F( + TrajectoryControllerTest, wrong_scaling_command_interface_parameter_controller_fails_activation) +{ + double initial_factor = 0.123; + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = { + rclcpp::Parameter("scaling_factor_initial_default", initial_factor), + rclcpp::Parameter("speed_scaling_command_interface_name", "idontexist"), + }; + SetUpTrajectoryController(executor, params); + + auto state = traj_controller_->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + state = ActivateTrajectoryController(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_F(TrajectoryControllerTest, scaling_state_interface_sets_value) +{ + double initial_factor = 0.123; + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = { + rclcpp::Parameter("scaling_factor_initial_default", initial_factor), + rclcpp::Parameter("speed_scaling_state_interface_name", "speed_scaling/speed_scaling_factor"), + }; + SetUpAndActivateTrajectoryController(executor, params); + + auto speed_scaling_pub = node_->create_publisher( + controller_name_ + "/speed_scaling_input", rclcpp::SystemDefaultsQoS()); + subscribeToState(executor); + updateController(); + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); + EXPECT_EQ(state->speed_scaling_factor, speed_scaling_factor_); + + control_msgs::msg::SpeedScalingFactor msg; + msg.factor = 0.765; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + + updateController(); + + // Spin to receive latest state + executor.spin_some(); + state = getState(); + // Since we have a speed scaling state interface active, the value set via topic will be + // overwritten from the state interface. + EXPECT_EQ(state->speed_scaling_factor, speed_scaling_factor_); +} + +TEST_F(TrajectoryControllerTest, scaling_command_interface_sets_value) +{ + double initial_factor = 0.123; + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = { + rclcpp::Parameter("scaling_factor_initial_default", initial_factor), + rclcpp::Parameter("speed_scaling_state_interface_name", "speed_scaling/speed_scaling_factor"), + rclcpp::Parameter( + "speed_scaling_command_interface_name", "speed_scaling/target_speed_fraction_cmd"), + }; + SetUpAndActivateTrajectoryController(executor, params); + + auto speed_scaling_pub = node_->create_publisher( + controller_name_ + "/speed_scaling_input", rclcpp::SystemDefaultsQoS()); + subscribeToState(executor); + updateController(); + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); + // The initial value should be written to the hardware + EXPECT_EQ(state->speed_scaling_factor, initial_factor); + + control_msgs::msg::SpeedScalingFactor msg; + msg.factor = 0.765; + speed_scaling_pub->publish(msg); + traj_controller_->wait_for_trajectory(executor); + + // Value will be set during the first update and read in the second update + updateController(); + updateController(); + + // Spin to receive latest state + executor.spin_some(); + state = getState(); + EXPECT_EQ(state->speed_scaling_factor, 0.765); +} + +TEST_F(TrajectoryControllerTest, activate_with_scaling_interfaces) +{ + rclcpp::executors::MultiThreadedExecutor executor; + std::vector params = { + rclcpp::Parameter("speed_scaling_state_interface_name", "speed_scaling/speed_scaling_factor"), + rclcpp::Parameter( + "speed_scaling_command_interface_name", "speed_scaling/target_speed_fraction_cmd"), + }; + SetUpTrajectoryController(executor, params); + + auto state = traj_controller_->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + auto cmd_if_conf = traj_controller_->command_interface_configuration(); + ASSERT_EQ(cmd_if_conf.names.size(), joint_names_.size() * command_interface_types_.size() + 1); + EXPECT_EQ(cmd_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + auto state_if_conf = traj_controller_->state_interface_configuration(); + ASSERT_EQ(state_if_conf.names.size(), joint_names_.size() * state_interface_types_.size() + 1); + EXPECT_EQ(state_if_conf.type, controller_interface::interface_configuration_type::INDIVIDUAL); + + state = ActivateTrajectoryController(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_ACTIVE); + + executor.cancel(); +} diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b0e66394d1..3bb560886d 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -388,6 +388,17 @@ class TrajectoryControllerTest : public ::testing::Test state_interfaces.emplace_back(acc_state_interfaces_.back()); } + speed_scaling_factor_ = 1.0; + target_speed_scaling_factor_ = 1.0; + gpio_state_interfaces.emplace_back(hardware_interface::StateInterface( + "speed_scaling", "speed_scaling_factor", + separate_cmd_and_state_values ? &speed_scaling_factor_ : &target_speed_scaling_factor_)); + state_interfaces.emplace_back(gpio_state_interfaces.back()); + + gpio_command_interfaces_.emplace_back(hardware_interface::CommandInterface( + "speed_scaling", "target_speed_fraction_cmd", &target_speed_scaling_factor_)); + cmd_interfaces.emplace_back(gpio_command_interfaces_.back()); + traj_controller_->assign_interfaces(std::move(cmd_interfaces), std::move(state_interfaces)); return traj_controller_->get_node()->activate(); } @@ -771,13 +782,17 @@ class TrajectoryControllerTest : public ::testing::Test std::vector joint_state_pos_; std::vector joint_state_vel_; std::vector joint_state_acc_; + double speed_scaling_factor_; + double target_speed_scaling_factor_; std::vector pos_cmd_interfaces_; std::vector vel_cmd_interfaces_; std::vector acc_cmd_interfaces_; std::vector eff_cmd_interfaces_; + std::vector gpio_command_interfaces_; std::vector pos_state_interfaces_; std::vector vel_state_interfaces_; std::vector acc_state_interfaces_; + std::vector gpio_state_interfaces; }; // From the tutorial: https://www.sandordargo.com/blog/2019/04/24/parameterized-testing-with-gtest