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 a229489422..3eff7f17b1 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 @@ -166,6 +166,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa std::vector pids_; // Feed-forward velocity weight factor when calculating closed loop pid adapter's command std::vector ff_velocity_scale_; + // Configuration for every joint, if position error is normalized + std::vector normalize_joint_error_; // reserved storage for result of the command when closed loop pid adapter is used std::vector tmp_command_; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 114ddc7346..34c67fb4a4 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -125,8 +125,17 @@ controller_interface::return_type JointTrajectoryController::update( const JointTrajectoryPoint & desired) { // error defined as the difference between current and desired - error.positions[index] = - angles::shortest_angular_distance(current.positions[index], desired.positions[index]); + if (normalize_joint_error_[index]) + { + // if desired, the shortest_angular_distance is calculated, i.e., the error is + // normalized between -picomputeCommand( - state_desired_.positions[i] - state_current_.positions[i], - state_desired_.velocities[i] - state_current_.velocities[i], + state_error_.positions[i], state_error_.velocities[i], (uint64_t)period.nanoseconds()); } } @@ -581,6 +589,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } } + // Configure joint position error normalization from ROS parameters + normalize_joint_error_.resize(dof_); + for (size_t i = 0; i < dof_; ++i) + { + const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + normalize_joint_error_[i] = gains.normalize_error; + } + if (params_.state_interfaces.empty()) { RCLCPP_ERROR(logger, "'state_interfaces' parameter is empty."); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index 80aa32585b..f767b15b27 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -101,6 +101,11 @@ joint_trajectory_controller: default_value: 0.0, description: "Feed-forward scaling of velocity." } + normalize_error: { + type: bool, + default_value: false, + description: "Use position error normalization to -pi to pi." + } constraints: stopped_velocity_tolerance: { type: double, diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 1feb094650..ee18eacee8 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -499,6 +499,210 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); } +// Floating-point value comparison threshold +const double EPS = 1e-6; +/** + * @brief check if position error of revolute joints are normalized if not configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_not_normalized) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, false); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *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}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // no normalization of position error + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2], EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LE(0.0, joint_vel_[2]); + + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + // we expect u = k_p * (s_d-s) + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // no normalization of position error + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * allowed_delta); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + +/** + * @brief check if position error of revolute joints are normalized if configured so +*/ +TEST_P(TrajectoryControllerTestParameterized, position_error_normalized) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + SetUpAndActivateTrajectoryController(executor, true, {}, true, k_p, 0.0, true); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg + constexpr auto FIRST_POINT_TIME = std::chrono::milliseconds(250); + builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(FIRST_POINT_TIME)}; + // *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}}}; + // *INDENT-ON* + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); + traj_controller_->wait_for_trajectory(executor); + + // first update + updateController(rclcpp::Duration(FIRST_POINT_TIME)); + + // Spin to receive latest state + executor.spin_some(); + auto state_msg = getState(); + ASSERT_TRUE(state_msg); + + const auto allowed_delta = 0.1; + + // no update of state_interface + EXPECT_EQ(state_msg->actual.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->desired.positions.size()); + EXPECT_EQ(n_joints, state_msg->actual.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct desired positions used? + EXPECT_NEAR(points[0][0], state_msg->desired.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->desired.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->desired.positions[2], 3 * allowed_delta); + + // is error.positions[2] normalized? + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], + state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS); + + if (traj_controller_->has_position_command_interface()) + { + // check command interface + EXPECT_NEAR(points[0][0], joint_pos_[0], allowed_delta); + EXPECT_NEAR(points[0][1], joint_pos_[1], allowed_delta); + EXPECT_NEAR(points[0][2], joint_pos_[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + + // use_closed_loop_pid_adapter_ + if (traj_controller_->use_closed_loop_pid_adapter()) + { + EXPECT_GE(0.0, joint_vel_[2]); + // we expect u = k_p * (s_d-s) for positions[0] and positions[1] + EXPECT_NEAR( + k_p * (state_msg->desired.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->desired.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // is error of positions[2] normalized? + EXPECT_NEAR( + k_p * (state_msg->desired.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI), joint_vel_[2], + k_p * allowed_delta); + } + else + { + // interpolated points_velocities only + EXPECT_LE(0.0, joint_vel_[2]); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + + executor.cancel(); +} + void TrajectoryControllerTest::test_state_publish_rate_target(int target_msg_count) { rclcpp::Parameter state_publish_rate_param( diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 0d523cc88d..ca58b124a5 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -112,6 +112,8 @@ class TestableJointTrajectoryController bool has_effort_command_interface() { return has_effort_command_interface_; } + bool use_closed_loop_pid_adapter() { return use_closed_loop_pid_adapter_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -171,7 +173,8 @@ class TrajectoryControllerTest : public ::testing::Test node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } - void SetPidParameters() + void SetPidParameters( + double p_default = 0.0, double ff_default = 1.0, bool normalize_error_default = false) { traj_controller_->trigger_declare_parameters(); auto node = traj_controller_->get_node(); @@ -179,24 +182,26 @@ class TrajectoryControllerTest : public ::testing::Test for (size_t i = 0; i < joint_names_.size(); ++i) { const std::string prefix = "gains." + joint_names_[i]; - const rclcpp::Parameter k_p(prefix + ".p", 0.0); + const rclcpp::Parameter k_p(prefix + ".p", p_default); const rclcpp::Parameter k_i(prefix + ".i", 0.0); const rclcpp::Parameter k_d(prefix + ".d", 0.0); const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); - const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", 1.0); - node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); + const rclcpp::Parameter ff_velocity_scale(prefix + ".ff_velocity_scale", ff_default); + const rclcpp::Parameter normalize_error(prefix + ".normalize_error", normalize_error_default); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale, normalize_error}); } } void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, bool use_local_parameters = true, const std::vector & parameters = {}, - bool separate_cmd_and_state_values = false) + bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, + bool normalize_error = false) { SetUpTrajectoryController(executor, use_local_parameters); // set pid parameters before configure - SetPidParameters(); + SetPidParameters(k_p, ff, normalize_error); for (const auto & param : parameters) { traj_controller_->get_node()->set_parameter(param);