From c94d8680d51e56ab94a0c42d267e7d37ae6c9b03 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Fri, 1 Dec 2023 15:51:45 +0000 Subject: [PATCH] [JTC] Continue with last trajectory-point on success (backport #842) --- joint_trajectory_controller/doc/userdoc.rst | 4 +- .../joint_trajectory_controller.hpp | 12 +- .../src/joint_trajectory_controller.cpp | 16 ++- .../test/test_trajectory_actions.cpp | 136 +++++++++++++----- .../test/test_trajectory_controller.cpp | 14 +- .../test/test_trajectory_controller_utils.hpp | 56 +++----- 6 files changed, 162 insertions(+), 76 deletions(-) diff --git a/joint_trajectory_controller/doc/userdoc.rst b/joint_trajectory_controller/doc/userdoc.rst index b6c4c1ecf3..79919c7426 100644 --- a/joint_trajectory_controller/doc/userdoc.rst +++ b/joint_trajectory_controller/doc/userdoc.rst @@ -151,12 +151,14 @@ Actions [#f1]_ /follow_joint_trajectory [control_msgs::action::FollowJointTrajectory] Action server for commanding the controller - The primary way to send trajectories is through the action interface, and should be favored when execution monitoring is desired. + Action goals allow to specify not only the trajectory to execute, but also (optionally) path and goal tolerances. When no tolerances are specified, the defaults given in the parameter interface are used (see :ref:`parameters`). If tolerances are violated during trajectory execution, the action goal is aborted, the client is notified, and the current position is held. +The action server returns success to the client and continues with the last commanded point after the target is reached within the specified tolerances. + .. _Subscriber: Subscriber [#f1]_ 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 b563450d7a..034e4f0cb8 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 @@ -173,7 +173,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Timeout to consider commands old double cmd_timeout_; - // Are we holding position? + // True if holding position or repeating last trajectory point in case of success realtime_tools::RealtimeBuffer rt_is_holding_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; @@ -248,9 +248,19 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa JOINT_TRAJECTORY_CONTROLLER_PUBLIC void preempt_active_goal(); + + /** @brief set the current position with zero velocity and acceleration as new command + */ JOINT_TRAJECTORY_CONTROLLER_PUBLIC std::shared_ptr set_hold_position(); + /** @brief set last trajectory point to be repeated at success + * + * no matter if it has nonzero velocity or acceleration + */ + JOINT_TRAJECTORY_CONTROLLER_PUBLIC + std::shared_ptr set_success_trajectory_point(); + JOINT_TRAJECTORY_CONTROLLER_PUBLIC bool reset(); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e97caabcd3..1d11684b12 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -374,7 +374,7 @@ controller_interface::return_type JointTrajectoryController::update( RCLCPP_INFO(get_node()->get_logger(), "Goal reached, success!"); traj_msg_external_point_ptr_.reset(); - traj_msg_external_point_ptr_.initRT(set_hold_position()); + traj_msg_external_point_ptr_.initRT(set_success_trajectory_point()); } else if (!within_goal_time) { @@ -1564,6 +1564,20 @@ JointTrajectoryController::set_hold_position() return hold_position_msg_ptr_; } +std::shared_ptr +JointTrajectoryController::set_success_trajectory_point() +{ + // set last command to be repeated at success, no matter if it has nonzero velocity or + // acceleration + hold_position_msg_ptr_->points[0] = traj_external_point_ptr_->get_trajectory_msg()->points.back(); + hold_position_msg_ptr_->points[0].time_from_start = rclcpp::Duration(0, 0); + + // set flag, otherwise tolerances will be checked with success_trajectory_point too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; +} + bool JointTrajectoryController::contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type) { diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index ed13a24485..87d557df1b 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -68,11 +68,12 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUpExecutor( const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false, double kp = 0.0) + bool separate_cmd_and_state_values = false, double kp = 0.0, double ff = 1.0) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp); + SetUpAndActivateTrajectoryController( + executor_, parameters, separate_cmd_and_state_values, kp, ff); SetUpActionClient(); @@ -218,7 +219,10 @@ class TestTrajectoryActionsTestParameterized TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor(params, false, 1.0, 0.0); SetUpControllerHardware(); std::shared_future gh_future; @@ -228,8 +232,6 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa std::vector points; JointTrajectoryPoint point; point.time_from_start = rclcpp::Duration::from_seconds(0.5); - point.positions.resize(joint_names_.size()); - point.positions = point_positions; points.push_back(point); @@ -247,20 +249,53 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_sendgoa // 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 - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(point_positions); - } - else + expectCommandPoint(point_positions); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_single_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + std::shared_future gh_future; + // send goal + std::vector point_positions{1.0, 2.0, 3.0}; + std::vector point_velocities{1.0, 1.0, 1.0}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point; + point.time_from_start = rclcpp::Duration::from_seconds(0.5); + point.positions = point_positions; + point.velocities = point_velocities; + + points.push_back(point); + + gh_future = sendActionGoal(points, 1.0, 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(point_positions, point_velocities); } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) { - SetUpExecutor(); + // deactivate velocity tolerance + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0)}; + SetUpExecutor({params}, false, 1.0, 0.0); SetUpControllerHardware(); // add feedback @@ -277,15 +312,11 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal std::vector points; JointTrajectoryPoint point1; point1.time_from_start = rclcpp::Duration::from_seconds(0.2); - 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.3); - point2.positions.resize(joint_names_.size()); - point2.positions = points_positions.at(1); points.push_back(point2); @@ -302,15 +333,57 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - if (traj_controller_->has_position_command_interface()) - { - expectHoldingPoint(points_positions.at(1)); - } - else + expectCommandPoint(points_positions.at(1)); +} + +TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_with_velocity_sendgoal) +{ + // deactivate velocity tolerance and allow velocity at trajectory end + std::vector params = { + rclcpp::Parameter("constraints.stopped_velocity_tolerance", 0.0), + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true)}; + SetUpExecutor(params, false, 1.0, 0.0); + SetUpControllerHardware(); + + // add feedback + bool feedback_recv = false; + goal_options_.feedback_callback = + [&]( + rclcpp_action::ClientGoalHandle::SharedPtr, + const std::shared_ptr) { feedback_recv = true; }; + + std::shared_future gh_future; + // send goal with multiple points + std::vector> points_positions{{{4.0, 5.0, 6.0}}, {{7.0, 8.0, 9.0}}}; + std::vector> points_velocities{{{1.0, 1.0, 1.0}}, {{2.0, 2.0, 2.0}}}; { - // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + std::vector points; + JointTrajectoryPoint point1; + point1.time_from_start = rclcpp::Duration::from_seconds(0.2); + point1.positions = points_positions.at(0); + point1.velocities = points_velocities.at(0); + points.push_back(point1); + + JointTrajectoryPoint point2; + point2.time_from_start = rclcpp::Duration::from_seconds(0.3); + point2.positions = points_positions.at(1); + point2.velocities = points_velocities.at(1); + points.push_back(point2); + + gh_future = sendActionGoal(points, 1.0, goal_options_); } + controller_hw_thread_.join(); + + EXPECT_TRUE(feedback_recv); + 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) + expectCommandPoint(points_positions.at(1), points_velocities.at(1)); } /** @@ -354,7 +427,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_single_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(0)); + expectCommandPoint(points_positions.at(0)); } /** @@ -413,7 +486,7 @@ TEST_F(TestTrajectoryActions, test_goal_tolerances_multi_point_success) // it should be holding the last position goal // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(points_positions.at(1)); + expectCommandPoint(points_positions.at(1)); } TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) @@ -464,8 +537,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_state_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) @@ -513,8 +585,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_goal_tolerances_fail) // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tolerance_fail) @@ -559,8 +630,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_no_time_from_start_state_tol // it should be holding the position (being the initial one) // i.e., active but trivial trajectory (one point only) - std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(INITIAL_POS_JOINTS); } TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) @@ -607,7 +677,7 @@ TEST_P(TestTrajectoryActionsTestParameterized, test_cancel_hold_position) // it should be holding the last position, // i.e., active but trivial trajectory (one point only) - expectHoldingPoint(cancelled_position); + expectCommandPoint(cancelled_position); } TEST_P(TestTrajectoryActionsTestParameterized, test_allow_nonzero_velocity_at_trajectory_end_true) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 47dd81c8bc..6a3b52fc4f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -248,7 +248,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // it should still be holding the position at time of deactivation // i.e., active but trivial trajectory (one point only) traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.1)); - expectHoldingPoint(deactivated_positions); + expectCommandPoint(deactivated_positions); executor.cancel(); } @@ -511,7 +511,7 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) ASSERT_TRUE(traj_controller_->has_active_traj()); // one point, being the position at startup std::vector initial_positions{INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; - expectHoldingPoint(initial_positions); + expectCommandPoint(initial_positions); executor.cancel(); } @@ -871,12 +871,12 @@ TEST_P(TrajectoryControllerTestParameterized, timeout) // should hold last position with zero velocity if (traj_controller_->has_position_command_interface()) { - expectHoldingPoint(points.at(2)); + expectCommandPoint(points.at(2)); } else { // no integration to position state interface from velocity/acceleration - expectHoldingPoint(INITIAL_POS_JOINTS); + expectCommandPoint(INITIAL_POS_JOINTS); } executor.cancel(); @@ -1894,7 +1894,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); } TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) @@ -1926,14 +1926,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) auto end_time = updateControllerAsync(rclcpp::Duration(4 * FIRST_POINT_TIME)); // it should have aborted and be holding now - expectHoldingPoint(joint_state_pos_); + expectCommandPoint(joint_state_pos_); // what happens if we wait longer but it harms the tolerance again? auto hold_position = joint_state_pos_; joint_state_pos_.at(0) = -3.3; updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME), end_time); // it should be still holding the old point - expectHoldingPoint(hold_position); + expectCommandPoint(hold_position); } // position controllers diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index b40c17e4b2..20a598f398 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -551,7 +551,8 @@ class TrajectoryControllerTest : public ::testing::Test } void test_state_publish_rate_target(int target_msg_count); - void expectHoldingPoint(std::vector point) + void expectCommandPoint( + std::vector position, std::vector velocity = {0.0, 0.0, 0.0}) { // it should be holding the given point // i.e., active but trivial trajectory (one point only) @@ -561,16 +562,16 @@ class TrajectoryControllerTest : public ::testing::Test { if (traj_controller_->has_position_command_interface()) { - EXPECT_NEAR(point.at(0), joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(1), joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(point.at(2), joint_pos_[2], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(0), joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(1), joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(position.at(2), joint_pos_[2], COMMON_THRESHOLD); } if (traj_controller_->has_velocity_command_interface()) { - EXPECT_EQ(0.0, joint_vel_[0]); - EXPECT_EQ(0.0, joint_vel_[1]); - EXPECT_EQ(0.0, joint_vel_[2]); + EXPECT_EQ(velocity.at(0), joint_vel_[0]); + EXPECT_EQ(velocity.at(1), joint_vel_[1]); + EXPECT_EQ(velocity.at(2), joint_vel_[2]); } if (traj_controller_->has_acceleration_command_interface()) @@ -587,40 +588,29 @@ class TrajectoryControllerTest : public ::testing::Test EXPECT_EQ(0.0, joint_eff_[2]); } } - else + else // traj_controller_->use_closed_loop_pid_adapter() == true { // velocity or effort PID? - // velocity setpoint is always zero -> feedforward term does not have an effect // --> set kp > 0.0 in test if (traj_controller_->has_velocity_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_vel_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", velocity command is " << joint_vel_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_vel_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", velocity command is " << joint_vel_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_vel_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", velocity command is " << joint_vel_[2]; + 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_vel_[i])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", velocity command is " << joint_vel_[i]; + } } if (traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE( - is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_eff_[0])) - << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() - << ", effort command is " << joint_eff_[0]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_eff_[1])) - << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() - << ", effort command is " << joint_eff_[1]; - EXPECT_TRUE( - is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_eff_[2])) - << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() - << ", effort command is " << joint_eff_[2]; + 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])) + << "test position point " << position.at(i) << ", position state is " + << pos_state_interfaces_[i].get_value() << ", effort command is " << joint_eff_[i]; + } } } }