From 0a337841deb589c71b650bb49cf93d967ecdedb6 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 12 Sep 2023 11:54:55 +0200 Subject: [PATCH] [JTC] Fix hold position mode with goal_time>0 (backport #758) --- .../joint_trajectory_controller.hpp | 4 + .../src/joint_trajectory_controller.cpp | 64 +++++++++------ .../test/test_trajectory_actions.cpp | 24 +++++- .../test/test_trajectory_controller.cpp | 12 ++- .../test/test_trajectory_controller_utils.hpp | 81 ++++++++++++++----- 5 files changed, 132 insertions(+), 53 deletions(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index b13c12cef8..5a2236f437 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,6 +173,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; + realtime_tools::RealtimeBuffer rt_is_holding_; ///< are we holding position? // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; rclcpp::Subscription::SharedPtr joint_command_subscriber_ = @@ -186,6 +187,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa realtime_tools::RealtimeBuffer> traj_msg_external_point_ptr_; + std::shared_ptr hold_position_msg_ptr_ = nullptr; + using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState; using StatePublisher = realtime_tools::RealtimePublisher; using StatePublisherPtr = std::unique_ptr; @@ -274,6 +277,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool contains_interface_type( const std::vector & interface_type_list, const std::string & interface_type); + void init_hold_position_msg(); void resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); void resize_joint_trajectory_point_command( diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 13a99664f9..e935d27a66 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -230,7 +230,8 @@ controller_interface::return_type JointTrajectoryController::update( if ( (before_last_point || first_sample) && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.state_tolerance[index], false)) + state_error_, index, default_tolerances_.state_tolerance[index], false) && + *(rt_is_holding_.readFromRT()) == false) { tolerance_violated_while_moving = true; } @@ -238,7 +239,8 @@ controller_interface::return_type JointTrajectoryController::update( if ( !before_last_point && !check_state_tolerance_per_joint( - state_error_, index, default_tolerances_.goal_state_tolerance[index], false)) + state_error_, index, default_tolerances_.goal_state_tolerance[index], false) && + *(rt_is_holding_.readFromRT()) == false) { outside_goal_tolerance = true; @@ -831,6 +833,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( logger, "Using '%s' interpolation method.", interpolation_methods::InterpolationMethodMap.at(interpolation_method_).c_str()); + // prepare hold_position_msg + init_hold_position_msg(); + + // create subscriber and publishers joint_command_subscriber_ = get_node()->create_subscription( "~/joint_trajectory", rclcpp::SystemDefaultsQoS(), @@ -1018,6 +1024,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( { add_new_trajectory_msg(set_hold_position()); } + rt_is_holding_.writeFromNonRT(true); return CallbackReturn::SUCCESS; } @@ -1203,6 +1210,7 @@ void JointTrajectoryController::topic_callback( if (subscriber_is_active_) { add_new_trajectory_msg(msg); + rt_is_holding_.writeFromNonRT(false); } }; @@ -1265,6 +1273,7 @@ void JointTrajectoryController::goal_accepted_callback( std::make_shared(goal_handle->get_goal()->trajectory); add_new_trajectory_msg(traj_msg); + rt_is_holding_.writeFromNonRT(false); } // Update the active goal @@ -1559,31 +1568,12 @@ std::shared_ptr JointTrajectoryController::set_hold_position() { // Command to stay at current position - trajectory_msgs::msg::JointTrajectory current_pose_msg; - current_pose_msg.header.stamp = - rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately - current_pose_msg.joint_names = params_.joints; - current_pose_msg.points.push_back(state_current_); - current_pose_msg.points[0].velocities.clear(); - current_pose_msg.points[0].accelerations.clear(); - current_pose_msg.points[0].effort.clear(); - if (has_velocity_command_interface_) - { - // ensure no velocity (PID will fix this) - current_pose_msg.points[0].velocities.resize(dof_, 0.0); - } - if (has_acceleration_command_interface_) - { - // ensure no acceleration - current_pose_msg.points[0].accelerations.resize(dof_, 0.0); - } - if (has_effort_command_interface_) - { - // ensure no explicit effort (PID will fix this) - current_pose_msg.points[0].effort.resize(dof_, 0.0); - } + hold_position_msg_ptr_->points[0].positions = state_current_.positions; - return std::make_shared(current_pose_msg); + // set flag, otherwise tolerances will be checked with holding position too + rt_is_holding_.writeFromNonRT(true); + + return hold_position_msg_ptr_; } bool JointTrajectoryController::contains_interface_type( @@ -1633,6 +1623,28 @@ bool JointTrajectoryController::has_active_trajectory() return traj_external_point_ptr_ != nullptr && traj_external_point_ptr_->has_trajectory_msg(); } +void JointTrajectoryController::init_hold_position_msg() +{ + hold_position_msg_ptr_ = std::make_shared(); + hold_position_msg_ptr_->header.stamp = + rclcpp::Time(0.0, 0.0, get_node()->get_clock()->get_clock_type()); // start immediately + hold_position_msg_ptr_->joint_names = params_.joints; + hold_position_msg_ptr_->points.resize(1); // a trivial msg only + hold_position_msg_ptr_->points[0].velocities.clear(); + hold_position_msg_ptr_->points[0].accelerations.clear(); + hold_position_msg_ptr_->points[0].effort.clear(); + if (has_velocity_command_interface_ || has_acceleration_command_interface_) + { + // add velocity, so that trajectory sampling returns velocity points in any case + hold_position_msg_ptr_->points[0].velocities.resize(dof_, 0.0); + } + if (has_acceleration_command_interface_) + { + // add velocity, so that trajectory sampling returns acceleration points in any case + hold_position_msg_ptr_->points[0].accelerations.resize(dof_, 0.0); + } +} + } // namespace joint_trajectory_controller #include "pluginlib/class_list_macros.hpp" diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index fc24c2c029..371be914e2 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -68,11 +68,11 @@ class TestTrajectoryActions : public TrajectoryControllerTest void SetUpExecutor( const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false) + bool separate_cmd_and_state_values = false, double kp = 0.0) { setup_executor_ = true; - SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values); + SetUpAndActivateTrajectoryController(executor_, parameters, separate_cmd_and_state_values, kp); SetUpActionClient(); @@ -247,7 +247,15 @@ 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 - expectHoldingPoint(point_positions); + if (traj_controller_->has_position_command_interface()) + { + expectHoldingPoint(point_positions); + } + else + { + // no integration to position state interface from velocity/acceleration + expectHoldingPoint(INITIAL_POS_JOINTS); + } } TEST_P(TestTrajectoryActionsTestParameterized, test_success_multi_point_sendgoal) @@ -294,7 +302,15 @@ 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) - expectHoldingPoint(points_positions.at(1)); + if (traj_controller_->has_position_command_interface()) + { + expectHoldingPoint(points_positions.at(1)); + } + else + { + // no integration to position state interface from velocity/acceleration + expectHoldingPoint(INITIAL_POS_JOINTS); + } } /** diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 4d8962e272..69d14efa5f 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1673,7 +1673,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_state_tolerances_fail) rclcpp::Parameter("constraints.joint3.trajectory", state_tol)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, params, true); + double kp = 1.0; // activate feedback control for testing velocity/effort PID + SetUpAndActivateTrajectoryController(executor, params, true, kp); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); @@ -1705,7 +1706,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) rclcpp::Parameter("constraints.goal_time", goal_time)}; rclcpp::executors::MultiThreadedExecutor executor; - SetUpAndActivateTrajectoryController(executor, params, true); + SetUpAndActivateTrajectoryController(executor, params, true, 1.0); // send msg constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(100); @@ -1722,6 +1723,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // it should have aborted and be holding now expectHoldingPoint(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; + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + // it should be still holding the old point + expectHoldingPoint(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 33693cc4c6..d222e90e9c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -38,6 +38,12 @@ const std::vector INITIAL_POS_JOINTS = { const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; + +bool is_same_sign_or_zero(double val1, double val2) +{ + return val1 * val2 > 0.0 || (val1 == 0.0 && val2 == 0.0); +} + } // namespace namespace test_trajectory_controllers @@ -465,32 +471,65 @@ class TrajectoryControllerTest : public ::testing::Test // i.e., active but trivial trajectory (one point only) EXPECT_TRUE(traj_controller_->has_trivial_traj()); - if (traj_controller_->has_position_command_interface()) + if (traj_controller_->use_closed_loop_pid_adapter() == false) { - 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); - } + 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); + } - 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]); - } + 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]); + } - if (traj_controller_->has_acceleration_command_interface()) - { - EXPECT_EQ(0.0, joint_acc_[0]); - EXPECT_EQ(0.0, joint_acc_[1]); - EXPECT_EQ(0.0, joint_acc_[2]); - } + if (traj_controller_->has_acceleration_command_interface()) + { + EXPECT_EQ(0.0, joint_acc_[0]); + EXPECT_EQ(0.0, joint_acc_[1]); + EXPECT_EQ(0.0, joint_acc_[2]); + } - if (traj_controller_->has_effort_command_interface()) + 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]); + } + } + else { - EXPECT_EQ(0.0, joint_eff_[0]); - EXPECT_EQ(0.0, joint_eff_[1]); - EXPECT_EQ(0.0, joint_eff_[2]); + // 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) - joint_state_pos_[0], joint_vel_[0])) + << "current error: " << point.at(0) - joint_state_pos_[0] << ", velocity command is " + << joint_vel_[0]; + EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_vel_[1])) + << "current error: " << point.at(1) - joint_state_pos_[1] << ", velocity command is " + << joint_vel_[1]; + EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_vel_[2])) + << "current error: " << point.at(2) - joint_state_pos_[2] << ", velocity command is " + << joint_vel_[2]; + } + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_eff_[0])) + << "current error: " << point.at(0) - joint_state_pos_[0] << ", effort command is " + << joint_eff_[0]; + EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_eff_[1])) + << "current error: " << point.at(1) - joint_state_pos_[1] << ", effort command is " + << joint_eff_[1]; + EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_eff_[2])) + << "current error: " << point.at(2) - joint_state_pos_[2] << ", effort command is " + << joint_eff_[2]; + } } }