From ac8d3cb20f32a53dcad08bfba59a3ca3d47d4e87 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Wed, 10 Jul 2024 16:00:15 +0000 Subject: [PATCH 1/6] Added effort and position control --- .../validate_jtc_parameters.hpp | 12 +- .../src/joint_trajectory_controller.cpp | 62 +++++- .../src/trajectory.cpp | 33 ++++ .../test/test_trajectory_controller.cpp | 178 +++++++++++++----- .../test/test_trajectory_controller_utils.hpp | 22 ++- 5 files changed, 247 insertions(+), 60 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index 0a6bf99bb7..bbd7e3dd75 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -32,7 +32,8 @@ tl::expected command_interface_type_combinations( // Check if command interfaces combination is valid. Valid combinations are: // 1. effort // 2. velocity - // 2. position [velocity, [acceleration]] + // 3. position [velocity, [acceleration]] + // 4. position, effort if ( rsl::contains>(interface_types, "velocity") && @@ -55,10 +56,13 @@ tl::expected command_interface_type_combinations( } if ( - rsl::contains>(interface_types, "effort") && - interface_types.size() > 1) + rsl::contains>(interface_types, "effort") && + !(interface_types.size() == 1 || (interface_types.size() == 2 && + rsl::contains>(interface_types, "position"))) + ) { - return tl::make_unexpected("'effort' command interface has to be used alone"); + return tl::make_unexpected( + "'effort' command interface has to be used alone or with a 'position' interface"); } return {}; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 31598bb813..16f0f0e159 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -261,12 +261,26 @@ controller_interface::return_type JointTrajectoryController::update( // Update PIDs for (auto i = 0ul; i < dof_; ++i) { + // If effort interface only, add desired effort as feed forward + // If velocity interface, ignore desired effort tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + + (has_effort_command_interface_ ? state_desired_.effort[i] : 0.0) + pids_[i]->computeCommand( state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); } } + else + { + if (has_position_command_interface_ && has_effort_command_interface_) + { + for (auto i = 0ul; i < dof_; ++i) + { + // If position and effort command interfaces, only pass desired effort + tmp_command_[i] = state_desired_.effort[i]; + } + } + } // set values for next hardware write() if (has_position_command_interface_) @@ -427,6 +441,11 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory state.velocities.clear(); state.accelerations.clear(); } + // No state interface for now, use command interface + if (has_effort_command_interface_) + { + assign_point_from_interface(state.effort, joint_command_interface_[3]); + } } bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state) @@ -495,6 +514,20 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto state.accelerations.clear(); } + // Effort state always comes from last command + if (has_effort_command_interface_) + { + if (interface_has_values(joint_command_interface_[3])) + { + assign_point_from_interface(state.effort, joint_command_interface_[3]); + } + else + { + state.effort.clear(); + has_values = false; + } + } + return has_values; } @@ -689,13 +722,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( use_closed_loop_pid_adapter_ = (has_velocity_command_interface_ && params_.command_interfaces.size() == 1 && !params_.open_loop_control) || - has_effort_command_interface_; + (has_effort_command_interface_ && params_.command_interfaces.size() == 1); + + tmp_command_.resize(dof_, 0.0); if (use_closed_loop_pid_adapter_) { pids_.resize(dof_); ff_velocity_scale_.resize(dof_); - tmp_command_.resize(dof_, 0.0); update_pids(); } @@ -750,15 +784,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( return CallbackReturn::FAILURE; } - // effort is always used alone so no need for size check + // effort only or effort and position command interfaces require position and velocity state if ( - has_effort_command_interface_ && + has_effort_command_interface_ && + (params_.command_interfaces.size() == 1 || + (params_.command_interfaces.size() == 2 && has_position_command_interface_)) && (!has_velocity_state_interface_ || !has_position_state_interface_)) { RCLCPP_ERROR( logger, - "'effort' command interface can only be used alone if 'velocity' and " - "'position' state interfaces are present"); + "'effort' command interface can only be used alone or with 'position' command interface " + "if 'velocity' and 'position' state interfaces are present"); return CallbackReturn::FAILURE; } @@ -1217,6 +1253,10 @@ void JointTrajectoryController::compute_error_for_joint( { error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; } + if (has_effort_command_interface_) + { + error.effort[index] = desired.effort[index]; + } } void JointTrajectoryController::fill_partial_goal( @@ -1466,10 +1506,11 @@ bool JointTrajectoryController::validate_trajectory_msg( return false; } // reject effort entries - if (!points[i].effort.empty()) + if (!has_effort_command_interface_ && !points[i].effort.empty()) { RCLCPP_ERROR( - get_node()->get_logger(), "Trajectories with effort fields are currently not supported."); + get_node()->get_logger(), "Trajectories with effort fields are only supported for " + "controllers using the 'effort' command interface."); return false; } } @@ -1540,6 +1581,7 @@ void JointTrajectoryController::resize_joint_trajectory_point( { point.accelerations.resize(size, 0.0); } + point.effort.resize(size, 0.0); } void JointTrajectoryController::resize_joint_trajectory_point_command( @@ -1608,6 +1650,10 @@ void JointTrajectoryController::init_hold_position_msg() // add velocity, so that trajectory sampling returns acceleration points in any case hold_position_msg_ptr_->points[0].accelerations.resize(dof_, 0.0); } + if (has_effort_command_interface_) + { + hold_position_msg_ptr_->points[0].effort.resize(dof_, 0.0); + } } } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/trajectory.cpp b/joint_trajectory_controller/src/trajectory.cpp index 0ed7f2ff13..b9cef17dcb 100644 --- a/joint_trajectory_controller/src/trajectory.cpp +++ b/joint_trajectory_controller/src/trajectory.cpp @@ -193,6 +193,10 @@ bool Trajectory::sample( { output_state.accelerations.resize(output_state.positions.size(), 0.0); } + if (output_state.effort.empty()) + { + output_state.effort.resize(output_state.positions.size(), 0.0); + } return true; } @@ -208,6 +212,7 @@ void Trajectory::interpolate_between_points( output.positions.resize(dim, 0.0); output.velocities.resize(dim, 0.0); output.accelerations.resize(dim, 0.0); + output.effort.resize(dim, 0.0); auto generate_powers = [](int n, double x, double * powers) { @@ -220,6 +225,7 @@ void Trajectory::interpolate_between_points( bool has_velocity = !state_a.velocities.empty() && !state_b.velocities.empty(); bool has_accel = !state_a.accelerations.empty() && !state_b.accelerations.empty(); + bool has_effort = !state_a.effort.empty() && !state_b.effort.empty(); if (duration_so_far.seconds() < 0.0) { duration_so_far = rclcpp::Duration::from_seconds(0.0); @@ -234,6 +240,25 @@ void Trajectory::interpolate_between_points( double t[6]; generate_powers(5, duration_so_far.seconds(), t); + if (has_effort) + { + // do linear interpolation + for (size_t i = 0; i < dim; ++i) + { + double start_effort = state_a.effort[i]; + double end_effort = state_b.effort[i]; + + double coefficients[2] = {0.0, 0.0}; + coefficients[0] = start_effort; + if (duration_btwn_points.seconds() != 0.0) + { + coefficients[1] = (end_effort - start_effort) / duration_btwn_points.seconds(); + } + + output.effort[i] = t[0] * coefficients[0] + t[1] * coefficients[1]; + } + } + if (!has_velocity && !has_accel) { // do linear interpolation @@ -332,6 +357,14 @@ void Trajectory::deduce_from_derivatives( trajectory_msgs::msg::JointTrajectoryPoint & first_state, trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim, const double delta_t) { + if (first_state.effort.empty()) + { + first_state.effort.assign(dim, 0.0); + } + if (second_state.effort.empty()) + { + second_state.effort.assign(dim, 0.0); + } if (second_state.positions.empty()) { second_state.positions.resize(dim); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 21c5705505..a6073b11cf 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -654,8 +654,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun // *INDENT-OFF* std::vector> points{ {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; + std::vector> effort{ + {{0.3, 0.4, 0.6}}, {{1.7, 1.8, 1.9}}, {{2.10, 2.11, 2.12}}}; + std::vector> empty_effort; // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time()); + bool has_effort_command_interface = std::find(command_interface_types_.begin(), + command_interface_types_.end(), "effort") != command_interface_types_.end(); + publish(time_from_start, points, rclcpp::Time(), {}, {}, + has_effort_command_interface ? effort : empty_effort); traj_controller_->wait_for_trajectory(executor); updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); @@ -719,17 +725,28 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun if (traj_controller_->has_effort_command_interface()) { - // with effort command interface, use_closed_loop_pid_adapter is always true - // we expect u = k_p * (s_d-s) for positions - EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * COMMON_THRESHOLD); - EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * COMMON_THRESHOLD); - EXPECT_NEAR( - k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_eff_[2], - k_p * COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + // with position and effort command interface, we expect u = ff for feed forward effort, + // positions are passed directly to the position command interface (no PID is done) + EXPECT_NEAR(state_reference.effort[0], joint_eff_[0], COMMON_THRESHOLD); + EXPECT_NEAR(state_reference.effort[1], joint_eff_[1], COMMON_THRESHOLD); + EXPECT_NEAR(state_reference.effort[2], joint_eff_[2], COMMON_THRESHOLD); + } + else + { + // with effort command interface, use_closed_loop_pid_adapter is always true + // we expect u = k_p * (s_d-s) + ff for positions and feed forward effort + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]) + state_reference.effort[0], + joint_eff_[0], k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]) + state_reference.effort[1], + joint_eff_[1], k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]) + state_reference.effort[2], + joint_eff_[2], k_p * COMMON_THRESHOLD); + } } executor.cancel(); @@ -825,19 +842,30 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) if (traj_controller_->has_effort_command_interface()) { - // with effort command interface, use_closed_loop_pid_adapter is always true - // we expect u = k_p * (s_d-s) for joint0 and joint1 - EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_eff_[0], - k_p * COMMON_THRESHOLD); - EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_eff_[1], - k_p * COMMON_THRESHOLD); - // is error of positions[2] wrapped around? - EXPECT_GT(0.0, joint_eff_[2]); - EXPECT_NEAR( - k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_eff_[2], - k_p * COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + // with position and effort command interface, we expect u = ff for feed forward effort, + // positions are passed directly to the position command interface (no PID is done) + EXPECT_NEAR(state_reference.effort[0], joint_eff_[0], COMMON_THRESHOLD); + EXPECT_NEAR(state_reference.effort[1], joint_eff_[1], COMMON_THRESHOLD); + EXPECT_NEAR(state_reference.effort[2], joint_eff_[2], COMMON_THRESHOLD); + } + else + { + // with effort command interface, use_closed_loop_pid_adapter is always true + // we expect u = k_p * (s_d-s) + ff for positions and feed forward effort + EXPECT_NEAR( + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]) + state_reference.effort[0], + joint_eff_[0], k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]) + state_reference.effort[1], + joint_eff_[1], k_p * COMMON_THRESHOLD); + // is error of positions[2] wrapped around? + EXPECT_GT(0.0, joint_eff_[2]); + EXPECT_NEAR( + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI) + + state_reference.effort[2], joint_eff_[2], k_p * COMMON_THRESHOLD); + } } executor.cancel(); @@ -993,7 +1021,8 @@ TEST_P(TrajectoryControllerTestParameterized, use_closed_loop_pid) !traj_controller_->has_effort_command_interface() && !traj_controller_->has_acceleration_command_interface() && !traj_controller_->is_open_loop()) || - traj_controller_->has_effort_command_interface()) + (traj_controller_->has_effort_command_interface() && + !traj_controller_->has_position_command_interface())) { EXPECT_TRUE(traj_controller_->use_closed_loop_pid_adapter()); } @@ -1075,6 +1104,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(executor); std::vector points_positions = {1.0, 2.0, 3.0}; + std::vector points_effort = {4.0, 5.0, 6.0}; std::vector jumble_map = {1, 2, 0}; double dt = 0.25; { @@ -1093,6 +1123,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.points[0].positions[2] = points_positions.at(jumble_map[2]); traj_msg.points[0].velocities.resize(3); traj_msg.points[0].accelerations.resize(3); + if (traj_controller_->has_effort_command_interface()) + { + traj_msg.points[0].effort.resize(3); + traj_msg.points[0].effort[0] = points_effort.at(jumble_map[0]); + traj_msg.points[0].effort[1] = points_effort.at(jumble_map[1]); + traj_msg.points[0].effort[2] = points_effort.at(jumble_map[2]); + } for (size_t dof = 0; dof < 3; dof++) { @@ -1134,10 +1171,19 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) if (traj_controller_->has_effort_command_interface()) { - // effort should be nonzero, because we use PID with feedforward term - EXPECT_GT(0.0, joint_eff_[0]); - EXPECT_GT(0.0, joint_eff_[1]); - EXPECT_GT(0.0, joint_eff_[2]); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(points_effort.at(0), joint_eff_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points_effort.at(1), joint_eff_[1], COMMON_THRESHOLD); + EXPECT_NEAR(points_effort.at(2), joint_eff_[2], COMMON_THRESHOLD); + } + else + { + // effort should be nonzero, because we use PID with feedforward term + EXPECT_GT(points_effort.at(0), joint_eff_[0]); + EXPECT_GT(points_effort.at(1), joint_eff_[1]); + EXPECT_GT(points_effort.at(2), joint_eff_[2]); + } } } @@ -1224,14 +1270,24 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) if (traj_controller_->has_effort_command_interface()) { - // estimate the sign of the effort - // joint rotates forward - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); - EXPECT_TRUE( - is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); - EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) - << "Joint 3 effort should be 0.0 since it's not in the goal"; + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(0.0, joint_eff_[0], COMMON_THRESHOLD); + EXPECT_NEAR(0.0, joint_eff_[1], COMMON_THRESHOLD); + EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) + << "Joint 3 effort should be 0.0 since it's not in the goal"; + } + else + { + // estimate the sign of the effort + // joint rotates forward + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); + EXPECT_TRUE( + is_same_sign_or_zero(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); + EXPECT_NEAR(0.0, joint_eff_[2], COMMON_THRESHOLD) + << "Joint 3 effort should be 0.0 since it's not in the goal"; + } } executor.cancel(); @@ -1378,7 +1434,16 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) // Effort is not supported in trajectory message traj_msg = good_traj_msg; traj_msg.points[0].effort = {1.0, 2.0, 3.0}; - EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + bool has_effort_command_interface = std::find(command_interface_types_.begin(), + command_interface_types_.end(), "effort") != command_interface_types_.end(); + if (has_effort_command_interface) + { + EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); + } + else + { + EXPECT_FALSE(traj_controller_->validate_trajectory_msg(traj_msg)); + } // Non-strictly increasing waypoint times traj_msg = good_traj_msg; @@ -2018,8 +2083,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) {{3.3, 4.4, 5.5}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; std::vector> points_velocities{ {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> points_effort{ + {{0.0, 0.0, 0.0}}, {{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; + std::vector> empty_effort; // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities); + bool has_effort_command_interface = std::find(command_interface_types_.begin(), + command_interface_types_.end(), "effort") != command_interface_types_.end(); + publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities, + has_effort_command_interface ? points_effort : empty_effort); traj_controller_->wait_for_trajectory(executor); auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); @@ -2092,6 +2163,16 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"effort"}), std::vector({"position", "velocity", "acceleration"})))); +// position effort controller +INSTANTIATE_TEST_SUITE_P( + PositionEffortTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"position", "effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"position", "effort"}), + std::vector({"position", "velocity", "acceleration"})))); + /** * @brief see if parameter validation is correct * @@ -2110,8 +2191,12 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"bad_name"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); - // command interfaces: effort has to be only - command_interface_types_ = {"effort", "position"}; + // command interfaces: effort has to be only or with position + command_interface_types_ = {"effort", "velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + command_interface_types_ = {"effort", "acceleration"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + command_interface_types_ = {"effort", "velocity", "acceleration"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); // command interfaces: velocity - position not present @@ -2161,4 +2246,13 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); state_interface_types_ = {"velocity"}; EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); + + // effort-position command interface: position - velocity not present + command_interface_types_ = {"effort", "position"}; + state_interface_types_ = {"position"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::OK); + state = traj_controller_->get_node()->configure(); + EXPECT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); + state_interface_types_ = {"velocity"}; + EXPECT_EQ(SetUpTrajectoryControllerLocal(), controller_interface::return_type::ERROR); } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index a15b2e5bc5..379241e15e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -430,7 +430,8 @@ class TrajectoryControllerTest : public ::testing::Test const builtin_interfaces::msg::Duration & delay_btwn_points, const std::vector> & points_positions, rclcpp::Time start_time, const std::vector & joint_names = {}, - const std::vector> & points_velocities = {}) + const std::vector> & points_velocities = {}, + const std::vector> & points_effort = {}) { int wait_count = 0; const auto topic = trajectory_publisher_->get_topic_name(); @@ -485,6 +486,15 @@ class TrajectoryControllerTest : public ::testing::Test } } + for (size_t index = 0; index < points_effort.size(); ++index) + { + traj_msg.points[index].effort.resize(points_effort[index].size()); + for (size_t j = 0; j < points_effort[index].size(); ++j) + { + traj_msg.points[index].effort[j] = points_effort[index][j]; + } + } + trajectory_publisher_->publish(traj_msg); } @@ -590,7 +600,7 @@ class TrajectoryControllerTest : public ::testing::Test } void expectCommandPoint( - std::vector position, std::vector velocity = {0.0, 0.0, 0.0}) + std::vector position, std::vector velocity = {0.0, 0.0, 0.0}, std::vector effort = {0.0, 0.0, 0.0}) { // it should be holding the given point // i.e., active but trivial trajectory (one point only) @@ -621,9 +631,9 @@ class TrajectoryControllerTest : public ::testing::Test if (traj_controller_->has_effort_command_interface()) { - EXPECT_EQ(0.0, joint_eff_[0]); - EXPECT_EQ(0.0, joint_eff_[1]); - EXPECT_EQ(0.0, joint_eff_[2]); + EXPECT_EQ(effort.at(0), joint_eff_[0]); + EXPECT_EQ(effort.at(1), joint_eff_[1]); + EXPECT_EQ(effort.at(2), joint_eff_[2]); } } else // traj_controller_->use_closed_loop_pid_adapter() == true @@ -645,7 +655,7 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < 3; i++) { EXPECT_TRUE(is_same_sign_or_zero( - position.at(i) - pos_state_interfaces_[i].get_value(), joint_eff_[i])) + position.at(i) - pos_state_interfaces_[i].get_value() + effort.at(i), joint_eff_[i])) << "test position point " << position.at(i) << ", position state is " << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; } From 62afca28e40a55d908fbe87c8ae3108478d2a617 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Thu, 11 Jul 2024 10:01:22 +0000 Subject: [PATCH 2/6] Updated docs --- joint_trajectory_controller/doc/trajectory.rst | 2 ++ joint_trajectory_controller/doc/userdoc.rst | 5 ++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/doc/trajectory.rst b/joint_trajectory_controller/doc/trajectory.rst index 44f385de27..d570d59fab 100644 --- a/joint_trajectory_controller/doc/trajectory.rst +++ b/joint_trajectory_controller/doc/trajectory.rst @@ -49,6 +49,8 @@ The spline interpolator uses the following interpolation strategies depending on Trajectories with velocity fields only, velocity and acceleration only, or acceleration fields only can be processed and are accepted, if ``allow_integration_in_goal_trajectories`` is true. Position (and velocity) is then integrated from velocity (or acceleration, respectively) by Heun's method. +Effort trajectories are allowed for controllers that claim the ``effort`` command interface and they are treated as feed-forward effort that is added to the position feedback. Effort is handled separately from position, velocity and acceleration. We use linear interpolation for effort when the ``spline`` interpolation method is selected. + Visualized Examples ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ To visualize the difference of the different interpolation methods and their inputs, different trajectories defined at a 0.5s grid and are sampled at a rate of 10ms. diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index 4dcb71a064..2967e4e883 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -24,12 +24,15 @@ Currently, joints with hardware interface types ``position``, ``velocity``, ``ac * ``position``, ``velocity``, ``acceleration`` * ``velocity`` * ``effort`` +* ``position``, ``effort`` This means that the joints can have one or more command interfaces, where the following control laws are applied at the same time: * For command interfaces ``position``, the desired positions are simply forwarded to the joints, * For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints. * For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop if it is configured (:ref:`parameters`). +* For ``effort`` command interface (without ``position`` command interface), if the trajectory contains effort, this will be added to the PID commands as a feed forward effort. +* For ``position, effort`` command interface, if the trajectory contains effort, this will be passed directly to the ``effort`` interface (PID won't be used) while the positions will be passed to the ``position`` interface. This leads to the following allowed combinations of command and state interfaces: @@ -38,7 +41,7 @@ This leads to the following allowed combinations of command and state interfaces * if command interface ``velocity`` is the only one, state interfaces must include ``position, velocity`` . -* With command interface ``effort``, state interfaces must include ``position, velocity``. +* With command interface ``effort`` or ``position, effort``, state interfaces must include ``position, velocity``. * With command interface ``acceleration``, state interfaces must include ``position, velocity``. From 7b28c55c42104258c7d282b9fd878cac781cd862 Mon Sep 17 00:00:00 2001 From: Vladimir Ivan Date: Thu, 11 Jul 2024 10:03:16 +0000 Subject: [PATCH 3/6] pre-commit fixes --- .../validate_jtc_parameters.hpp | 8 ++--- .../src/joint_trajectory_controller.cpp | 7 +++-- .../test/test_trajectory_controller.cpp | 29 ++++++++++++------- .../test/test_trajectory_controller_utils.hpp | 3 +- 4 files changed, 28 insertions(+), 19 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp index bbd7e3dd75..e0cb4313d5 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp @@ -56,10 +56,10 @@ tl::expected command_interface_type_combinations( } if ( - rsl::contains>(interface_types, "effort") && - !(interface_types.size() == 1 || (interface_types.size() == 2 && - rsl::contains>(interface_types, "position"))) - ) + rsl::contains>(interface_types, "effort") && + !(interface_types.size() == 1 || + (interface_types.size() == 2 && + rsl::contains>(interface_types, "position")))) { return tl::make_unexpected( "'effort' command interface has to be used alone or with a 'position' interface"); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 16f0f0e159..a14e47b8fd 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -786,7 +786,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( // effort only or effort and position command interfaces require position and velocity state if ( - has_effort_command_interface_ && + has_effort_command_interface_ && (params_.command_interfaces.size() == 1 || (params_.command_interfaces.size() == 2 && has_position_command_interface_)) && (!has_velocity_state_interface_ || !has_position_state_interface_)) @@ -1509,8 +1509,9 @@ bool JointTrajectoryController::validate_trajectory_msg( if (!has_effort_command_interface_ && !points[i].effort.empty()) { RCLCPP_ERROR( - get_node()->get_logger(), "Trajectories with effort fields are only supported for " - "controllers using the 'effort' command interface."); + get_node()->get_logger(), + "Trajectories with effort fields are only supported for " + "controllers using the 'effort' command interface."); return false; } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index a6073b11cf..5765317ee5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -658,9 +658,11 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun {{0.3, 0.4, 0.6}}, {{1.7, 1.8, 1.9}}, {{2.10, 2.11, 2.12}}}; std::vector> empty_effort; // *INDENT-ON* - bool has_effort_command_interface = std::find(command_interface_types_.begin(), - command_interface_types_.end(), "effort") != command_interface_types_.end(); - publish(time_from_start, points, rclcpp::Time(), {}, {}, + bool has_effort_command_interface = + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != + command_interface_types_.end(); + publish( + time_from_start, points, rclcpp::Time(), {}, {}, has_effort_command_interface ? effort : empty_effort); traj_controller_->wait_for_trajectory(executor); @@ -746,7 +748,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun EXPECT_NEAR( k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]) + state_reference.effort[2], joint_eff_[2], k_p * COMMON_THRESHOLD); - } + } } executor.cancel(); @@ -864,7 +866,8 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) EXPECT_GT(0.0, joint_eff_[2]); EXPECT_NEAR( k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI) + - state_reference.effort[2], joint_eff_[2], k_p * COMMON_THRESHOLD); + state_reference.effort[2], + joint_eff_[2], k_p * COMMON_THRESHOLD); } } @@ -1434,8 +1437,9 @@ TEST_P(TrajectoryControllerTestParameterized, invalid_message) // Effort is not supported in trajectory message traj_msg = good_traj_msg; traj_msg.points[0].effort = {1.0, 2.0, 3.0}; - bool has_effort_command_interface = std::find(command_interface_types_.begin(), - command_interface_types_.end(), "effort") != command_interface_types_.end(); + bool has_effort_command_interface = + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != + command_interface_types_.end(); if (has_effort_command_interface) { EXPECT_TRUE(traj_controller_->validate_trajectory_msg(traj_msg)); @@ -2087,9 +2091,11 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) {{0.0, 0.0, 0.0}}, {{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; std::vector> empty_effort; // *INDENT-ON* - bool has_effort_command_interface = std::find(command_interface_types_.begin(), - command_interface_types_.end(), "effort") != command_interface_types_.end(); - publish(time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities, + bool has_effort_command_interface = + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != + command_interface_types_.end(); + publish( + time_from_start, points, rclcpp::Time(0, 0, RCL_STEADY_TIME), {}, points_velocities, has_effort_command_interface ? points_effort : empty_effort); traj_controller_->wait_for_trajectory(executor); auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); @@ -2168,7 +2174,8 @@ INSTANTIATE_TEST_SUITE_P( PositionEffortTrajectoryControllers, TrajectoryControllerTestParameterized, ::testing::Values( std::make_tuple( - std::vector({"position", "effort"}), std::vector({"position", "velocity"})), + std::vector({"position", "effort"}), + std::vector({"position", "velocity"})), std::make_tuple( std::vector({"position", "effort"}), std::vector({"position", "velocity", "acceleration"})))); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 379241e15e..569e594d74 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -600,7 +600,8 @@ class TrajectoryControllerTest : public ::testing::Test } void expectCommandPoint( - std::vector position, std::vector velocity = {0.0, 0.0, 0.0}, std::vector effort = {0.0, 0.0, 0.0}) + std::vector position, std::vector velocity = {0.0, 0.0, 0.0}, + std::vector effort = {0.0, 0.0, 0.0}) { // it should be holding the given point // i.e., active but trivial trajectory (one point only) From d7f05a29fcd48a8bb4874bc11eec8cea670d4b2b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 6 Dec 2024 20:37:42 +0000 Subject: [PATCH 4/6] Fix segfault --- .../src/joint_trajectory_controller.cpp | 4 ---- 1 file changed, 4 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c6d5b519c4..2aa749fc9b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -1270,10 +1270,6 @@ void JointTrajectoryController::compute_error_for_joint( { error.accelerations[index] = desired.accelerations[index] - current.accelerations[index]; } - if (has_effort_command_interface_) - { - error.effort[index] = desired.effort[index]; - } } void JointTrajectoryController::fill_partial_goal( From ef6965b02a60913c8e6589cb8a801425ba33b1eb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 6 Dec 2024 20:41:02 +0000 Subject: [PATCH 5/6] Fix tests with t+dT sampling --- .../test/test_trajectory_controller.cpp | 31 ++++++++++--------- .../test/test_trajectory_controller_utils.hpp | 1 + 2 files changed, 17 insertions(+), 15 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 8f5f2042e7..01afc4cfea 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -642,7 +642,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun SetUpAndActivateTrajectoryController( executor, params, true, k_p, 0.0, INITIAL_POS_JOINTS, INITIAL_VEL_JOINTS, INITIAL_ACC_JOINTS, INITIAL_EFF_JOINTS, test_trajectory_controllers::urdf_rrrbot_revolute); - subscribeToState(executor); size_t n_joints = joint_names_.size(); @@ -656,20 +655,17 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun {{0.3, 0.4, 0.6}}, {{1.7, 1.8, 1.9}}, {{2.10, 2.11, 2.12}}}; std::vector> empty_effort; // *INDENT-ON* - bool has_effort_command_interface = - std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != - command_interface_types_.end(); publish( time_from_start, points, rclcpp::Time(), {}, {}, - has_effort_command_interface ? effort : empty_effort); + traj_controller_->has_effort_command_interface() ? effort : empty_effort); traj_controller_->wait_for_trajectory(executor); const rclcpp::Duration controller_period = rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate()); - auto end_time = updateControllerAsync( rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME), controller_period); + if (traj_controller_->has_position_command_interface()) { // check command interface @@ -685,6 +681,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); auto state_reference = traj_controller_->get_state_reference(); + auto command_next = traj_controller_->get_command_next(); auto state_error = traj_controller_->get_state_error(); // no update of state_interface @@ -746,13 +743,13 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun // with effort command interface, use_closed_loop_pid_adapter is always true // we expect u = k_p * (s_d-s) + ff for positions and feed forward effort EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]) + state_reference.effort[0], + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]) + command_next.effort[0], joint_eff_[0], k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]) + state_reference.effort[1], + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]) + command_next.effort[1], joint_eff_[1], k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]) + state_reference.effort[2], + k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2]) + command_next.effort[2], joint_eff_[2], k_p * COMMON_THRESHOLD); } } @@ -780,10 +777,13 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) // *INDENT-OFF* std::vector> points{ {{3.3, 4.4, 6.6}}, {{7.7, 8.8, 9.9}}, {{10.10, 11.11, 12.12}}}; - std::vector> points_velocities{ - {{0.01, 0.01, 0.01}}, {{0.05, 0.05, 0.05}}, {{0.06, 0.06, 0.06}}}; + std::vector> effort{ + {{0.3, 0.4, 0.6}}, {{1.7, 1.8, 1.9}}, {{2.10, 2.11, 2.12}}}; + std::vector> empty_effort; // *INDENT-ON* - publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + publish( + time_from_start, points, rclcpp::Time(), {}, {}, + traj_controller_->has_effort_command_interface() ? effort : empty_effort); traj_controller_->wait_for_trajectory(executor); const rclcpp::Duration controller_period = @@ -807,6 +807,7 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) // get states from class variables auto state_feedback = traj_controller_->get_state_feedback(); auto state_reference = traj_controller_->get_state_reference(); + auto command_next = traj_controller_->get_command_next(); auto state_error = traj_controller_->get_state_error(); // no update of state_interface @@ -871,16 +872,16 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) // with effort command interface, use_closed_loop_pid_adapter is always true // we expect u = k_p * (s_d-s) + ff for positions and feed forward effort EXPECT_NEAR( - k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]) + state_reference.effort[0], + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]) + command_next.effort[0], joint_eff_[0], k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]) + state_reference.effort[1], + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]) + command_next.effort[1], joint_eff_[1], k_p * COMMON_THRESHOLD); // is error of positions[2] wrapped around? EXPECT_GT(0.0, joint_eff_[2]); EXPECT_NEAR( k_p * (state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI) + - state_reference.effort[2], + command_next.effort[2], joint_eff_[2], k_p * COMMON_THRESHOLD); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 9bd1773934..06e395d4ee 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -199,6 +199,7 @@ class TestableJointTrajectoryController trajectory_msgs::msg::JointTrajectoryPoint get_state_feedback() { return state_current_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_reference() { return state_desired_; } + trajectory_msgs::msg::JointTrajectoryPoint get_command_next() { return command_next_; } trajectory_msgs::msg::JointTrajectoryPoint get_state_error() { return state_error_; } /** From 86cc1ebb0840130dd460baf694af7f13a8b2ee9e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 6 Dec 2024 20:53:44 +0000 Subject: [PATCH 6/6] Update release notes --- doc/release_notes.rst | 1 + 1 file changed, 1 insertion(+) diff --git a/doc/release_notes.rst b/doc/release_notes.rst index 974bca880f..2d399d31d3 100644 --- a/doc/release_notes.rst +++ b/doc/release_notes.rst @@ -49,6 +49,7 @@ joint_trajectory_controller allowed to move without restriction. * Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 `_). +* Feed-forward effort trajectories are supported now (`#1200 `_). mecanum_drive_controller ************************