From 09fd49ab04095789c57d810998de8417635f2fd3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:34:40 +0000 Subject: [PATCH 01/13] First changes to JTC accepting num_cmd_joints map_cmd_to_joints_; // Storing command joint names for interfaces std::vector command_joint_names_; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 3bd4873a31..7164d973ca 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -15,6 +15,7 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ +#include #include #include @@ -189,6 +190,17 @@ inline std::vector mapping(const T & t1, const T & t2) return mapping_vector; } +/** + * \return True if \p B is a subset of \p A, false otherwise. + */ +template +bool is_subset(std::vector A, std::vector B) +{ + std::sort(A.begin(), A.end()); + std::sort(B.begin(), B.end()); + return std::includes(A.begin(), A.end(), B.begin(), B.end()); +} + } // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 59e2c408c5..5216ddcd83 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -46,7 +46,7 @@ namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), dof_(0) +: controller_interface::ControllerInterface(), dof_(0), num_cmd_joints_(0) { } @@ -72,16 +72,16 @@ JointTrajectoryController::command_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - if (dof_ == 0) + if (num_cmd_joints_ == 0) { fprintf( stderr, - "During ros2_control interface configuration, degrees of freedom is not valid;" - " it should be positive. Actual DOF is %zu\n", - dof_); + "During ros2_control interface configuration, number of command interfaces is not valid;" + " it should be positive. Actual number is %zu\n", + num_cmd_joints_); std::exit(EXIT_FAILURE); } - conf.names.reserve(dof_ * params_.command_interfaces.size()); + conf.names.reserve(num_cmd_joints_ * params_.command_interfaces.size()); for (const auto & joint_name : command_joint_names_) { for (const auto & interface_type : params_.command_interfaces) @@ -179,9 +179,9 @@ controller_interface::return_type JointTrajectoryController::update( auto assign_interface_from_point = [&](auto & joint_interface, const std::vector & trajectory_point_interface) { - for (size_t index = 0; index < dof_; ++index) + for (size_t index = 0; index < num_cmd_joints_; ++index) { - joint_interface[index].get().set_value(trajectory_point_interface[index]); + joint_interface[index].get().set_value(trajectory_point_interface[map_cmd_to_joints_[index]]); } }; @@ -280,12 +280,13 @@ controller_interface::return_type JointTrajectoryController::update( if (use_closed_loop_pid_adapter_) { // Update PIDs - for (auto i = 0ul; i < dof_; ++i) + for (auto i = 0ul; i < num_cmd_joints_; ++i) { - tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[i], state_error_.velocities[i], - (uint64_t)period.nanoseconds()); + size_t index = map_cmd_to_joints_[i]; + tmp_command_[index] = (state_desired_.velocities[index] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_error_.positions[index], state_error_.velocities[index], + (uint64_t)period.nanoseconds()); } } @@ -453,9 +454,10 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto auto assign_point_from_interface = [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < dof_; ++index) + for (size_t index = 0; index < num_cmd_joints_; ++index) { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); + trajectory_point_interface[map_cmd_to_joints_[index]] = + joint_interface[index].get().get_value(); } }; @@ -524,7 +526,7 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( auto assign_point_from_interface = [&](std::vector & trajectory_point_interface, const auto & joint_interface) { - for (size_t index = 0; index < dof_; ++index) + for (size_t index = 0; index < num_cmd_joints_; ++index) { trajectory_point_interface[index] = joint_interface[index].get().get_value(); } @@ -665,8 +667,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (params_.joints.empty()) { - // TODO(destogl): is this correct? Can we really move-on if no joint names are not provided? RCLCPP_WARN(logger, "'joints' parameter is empty."); + return CallbackReturn::FAILURE; } command_joint_names_ = params_.command_joints; @@ -677,12 +679,34 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( RCLCPP_INFO( logger, "No specific joint names are used for command interfaces. Using 'joints' parameter."); } - else if (command_joint_names_.size() != params_.joints.size()) + num_cmd_joints_ = command_joint_names_.size(); + + if (num_cmd_joints_ > dof_) { RCLCPP_ERROR( - logger, "'command_joints' parameter has to have the same size as 'joints' parameter."); + logger, "'command_joints' parameter must not have greater size as 'joints' parameter."); return CallbackReturn::FAILURE; } + else if (num_cmd_joints_ < dof_) + { + if (is_subset(params_.joints, command_joint_names_) == false) + { + RCLCPP_ERROR( + logger, + "'command_joints' parameter must be a subset of 'joints' parameter, if their size is not " + "equal."); + return CallbackReturn::FAILURE; + } + } + // create a map for the command joints, trivial if they are the same + map_cmd_to_joints_ = mapping(command_joint_names_, params_.joints); + for (size_t i = 0; i < command_joint_names_.size(); i++) + { + RCLCPP_INFO( + logger, "Command joint %lu: '%s' maps to joint %lu: '%s'.", i, + command_joint_names_[i].c_str(), map_cmd_to_joints_[i], + params_.joints[map_cmd_to_joints_[i]].c_str()); + } if (params_.command_interfaces.empty()) { @@ -712,9 +736,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( if (use_closed_loop_pid_adapter_) { - pids_.resize(dof_); - ff_velocity_scale_.resize(dof_); - tmp_command_.resize(dof_, 0.0); + pids_.resize(num_cmd_joints_); + ff_velocity_scale_.resize(num_cmd_joints_); + tmp_command_.resize(dof_, std::numeric_limits::quiet_NaN()); update_pids(); } @@ -902,7 +926,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( command_interfaces_, command_joint_names_, interface, joint_command_interface_[index])) { RCLCPP_ERROR( - get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", dof_, + get_node()->get_logger(), "Expected %zu '%s' command interfaces, got %zu.", num_cmd_joints_, interface.c_str(), joint_command_interface_[index].size()); return CallbackReturn::ERROR; } @@ -976,7 +1000,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( const rclcpp_lifecycle::State &) { - for (size_t index = 0; index < dof_; ++index) + for (size_t index = 0; index < num_cmd_joints_; ++index) { if (has_position_command_interface_) { @@ -1491,14 +1515,14 @@ bool JointTrajectoryController::contains_interface_type( void JointTrajectoryController::resize_joint_trajectory_point( trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) { - point.positions.resize(size, 0.0); + point.positions.resize(size, std::numeric_limits::quiet_NaN()); if (has_velocity_state_interface_) { - point.velocities.resize(size, 0.0); + point.velocities.resize(size, std::numeric_limits::quiet_NaN()); } if (has_acceleration_state_interface_) { - point.accelerations.resize(size, 0.0); + point.accelerations.resize(size, std::numeric_limits::quiet_NaN()); } } @@ -1530,9 +1554,9 @@ bool JointTrajectoryController::has_active_trajectory() const void JointTrajectoryController::update_pids() { - for (size_t i = 0; i < dof_; ++i) + for (size_t i = 0; i < num_cmd_joints_; ++i) { - const auto & gains = params_.gains.joints_map.at(params_.joints[i]); + const auto & gains = params_.gains.joints_map.at(command_joint_names_[i]); if (pids_[i]) { // update PIDs with gains from ROS parameters diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 05010c562c..4c7aa95684 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -58,6 +58,10 @@ using test_trajectory_controllers::TrajectoryControllerTestParameterized; void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } +// Floating-point value comparison threshold +const double EPS = 1e-6; + +#if 0 TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; @@ -116,7 +120,81 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command compare_joints(joint_names_, command_joint_names_); } +#endif + +TEST_P( + TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_less_than_dof) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // set command_joints parameter + std::vector command_joint_names{joint_names_[0], joint_names_[1]}; + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names); + SetUpTrajectoryController(executor, {command_joint_names_param}); + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + std::vector state_interface_names; + state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); + for (const auto & joint : joint_names_) + { + for (const auto & interface : state_interface_types_) + { + state_interface_names.push_back(joint + "/" + interface); + } + } + auto state_interfaces = traj_controller_->state_interface_configuration(); + EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); + ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); + + std::vector command_interface_names; + command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size()); + for (const auto & joint : command_joint_names) + { + for (const auto & interface : command_interface_types_) + { + command_interface_names.push_back(joint + "/" + interface); + } + } + auto command_interfaces = traj_controller_->command_interface_configuration(); + EXPECT_EQ( + command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); + EXPECT_EQ( + command_interfaces.names.size(), command_joint_names.size() * command_interface_types_.size()); + ASSERT_THAT( + command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); +} + +TEST_P( + TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_greater_than_dof) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // set command_joints parameter + std::vector command_joint_names{ + joint_names_[0], joint_names_[1], joint_names_[2], "joint_4"}; + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names); + SetUpTrajectoryController(executor, {command_joint_names_param}); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); +} + +TEST_P( + TrajectoryControllerTestParameterized, + check_interface_names_with_command_joints_different_than_dof) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // set command_joints parameter + std::vector command_joint_names{joint_names_[0], "joint_4"}; + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names); + SetUpTrajectoryController(executor, {command_joint_names_param}); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); +} + +#if 0 TEST_P(TrajectoryControllerTestParameterized, activate) { rclcpp::executors::MultiThreadedExecutor executor; @@ -354,7 +432,107 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_EQ(n_joints, state->output.effort.size()); } } +#endif + +TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_joints_less_than_dof) +{ + rclcpp::executors::SingleThreadedExecutor executor; + std::vector command_joint_names{joint_names_[0], joint_names_[1]}; + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names); + SetUpAndActivateTrajectoryController(executor, {command_joint_names_param}); + subscribeToState(); + updateController(); + + // Spin to receive latest state + executor.spin_some(); + auto state = getState(); + + size_t n_joints = joint_names_.size(); + + for (unsigned int i = 0; i < n_joints; ++i) + { + EXPECT_EQ(joint_names_[i], state->joint_names[i]); + } + // No trajectory by default, no reference state or error + EXPECT_TRUE( + state->reference.positions.empty() || state->reference.positions == INITIAL_POS_JOINTS); + EXPECT_TRUE( + state->reference.velocities.empty() || state->reference.velocities == INITIAL_VEL_JOINTS); + EXPECT_TRUE( + state->reference.accelerations.empty() || state->reference.accelerations == INITIAL_EFF_JOINTS); + + std::vector zeros(3, 0); + EXPECT_EQ(state->error.positions, zeros); + EXPECT_TRUE(state->error.velocities.empty() || state->error.velocities == zeros); + EXPECT_TRUE(state->error.accelerations.empty() || state->error.accelerations == zeros); + + // expect feedback including all state_interfaces + EXPECT_EQ(n_joints, state->feedback.positions.size()); + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "velocity") == + state_interface_types_.end()) + { + EXPECT_TRUE(state->feedback.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->feedback.velocities.size()); + } + if ( + std::find(state_interface_types_.begin(), state_interface_types_.end(), "acceleration") == + state_interface_types_.end()) + { + EXPECT_TRUE(state->feedback.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->feedback.accelerations.size()); + } + + // expect output including all command_interfaces + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "position") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.positions.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.positions.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.velocities.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.velocities.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "acceleration") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.accelerations.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.accelerations.size()); + } + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") == + command_interface_types_.end()) + { + EXPECT_TRUE(state->output.effort.empty()); + } + else + { + EXPECT_EQ(n_joints, state->output.effort.size()); + } +} +#if 0 /** * @brief check if dynamic parameters are updated */ @@ -464,8 +642,6 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) executor.cancel(); } -// Floating-point value comparison threshold -const double EPS = 1e-6; /** * @brief check if position error of revolute joints are angle_wraparound if not configured so */ @@ -694,7 +870,119 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) executor.cancel(); } +#endif + +/** + * @brief check if position error of revolute joints are normalized if not configured so + */ +TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less_than_dof) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector command_joint_names{joint_names_[0], joint_names_[1]}; + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names); + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg for all joints + 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->feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); + + // no normalization of position error + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], state_msg->reference.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); + EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, joint_vel_[2]); + EXPECT_LT(0.0, state_msg->output.velocities[0]); + EXPECT_LT(0.0, state_msg->output.velocities[1]); + EXPECT_LT(0.0, state_msg->output.velocities[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->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * allowed_delta); + // no normalization of position error + EXPECT_NEAR( + k_p * (state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * allowed_delta); + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, joint_eff_[2]); + EXPECT_LT(0.0, state_msg->output.effort[0]); + EXPECT_LT(0.0, state_msg->output.effort[1]); + EXPECT_LT(0.0, state_msg->output.effort[2]); + } + + executor.cancel(); +} +#if 0 /** * @brief cmd_timeout must be greater than constraints.goal_time */ @@ -1836,7 +2124,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // it should be still holding the old point expectCommandPoint(hold_position); } - +#endif // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, From eb64893a22f7a0ad91fae0744e746f86afa39087 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:34:40 +0000 Subject: [PATCH 02/13] Map command joints to dof --- .../joint_trajectory_controller.hpp | 4 +- .../trajectory.hpp | 13 +- .../src/joint_trajectory_controller.cpp | 54 +++++--- .../test/test_trajectory_controller.cpp | 128 +++++++++++++++--- 4 files changed, 145 insertions(+), 54 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 d4fc3a8ce1..1df3a7eb8e 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 @@ -294,9 +294,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void init_hold_position_msg(); void resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double vale = 0.0); void resize_joint_trajectory_point_command( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size); + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double vale = 0.0); }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index 7164d973ca..e7995efb66 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -161,7 +161,7 @@ class Trajectory /** * \return The map between \p t1 indices (implicitly encoded in return vector indices) to \p t2 * indices. If \p t1 is "{C, B}" and \p t2 is "{A, B, C, D}", the associated - * mapping vector is "{2, 1}". + * mapping vector is "{2, 1}". return empty vector if \p t1 is not a subset of \p t2. */ template inline std::vector mapping(const T & t1, const T & t2) @@ -190,17 +190,6 @@ inline std::vector mapping(const T & t1, const T & t2) return mapping_vector; } -/** - * \return True if \p B is a subset of \p A, false otherwise. - */ -template -bool is_subset(std::vector A, std::vector B) -{ - std::sort(A.begin(), A.end()); - std::sort(B.begin(), B.end()); - return std::includes(A.begin(), A.end(), B.begin(), B.end()); -} - } // namespace joint_trajectory_controller #endif // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5216ddcd83..f076b09931 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include #include @@ -528,7 +529,8 @@ bool JointTrajectoryController::read_commands_from_command_interfaces( { for (size_t index = 0; index < num_cmd_joints_; ++index) { - trajectory_point_interface[index] = joint_interface[index].get().get_value(); + trajectory_point_interface[map_cmd_to_joints_[index]] = + joint_interface[index].get().get_value(); } }; @@ -689,7 +691,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } else if (num_cmd_joints_ < dof_) { - if (is_subset(params_.joints, command_joint_names_) == false) + // create a map for the command joints + map_cmd_to_joints_ = mapping(command_joint_names_, params_.joints); + if (map_cmd_to_joints_.size() != num_cmd_joints_) { RCLCPP_ERROR( logger, @@ -697,15 +701,19 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( "equal."); return CallbackReturn::FAILURE; } + for (size_t i = 0; i < command_joint_names_.size(); i++) + { + RCLCPP_DEBUG( + logger, "Command joint %lu: '%s' maps to joint %lu: '%s'.", i, + command_joint_names_[i].c_str(), map_cmd_to_joints_[i], + params_.joints.at(map_cmd_to_joints_[i]).c_str()); + } } - // create a map for the command joints, trivial if they are the same - map_cmd_to_joints_ = mapping(command_joint_names_, params_.joints); - for (size_t i = 0; i < command_joint_names_.size(); i++) + else { - RCLCPP_INFO( - logger, "Command joint %lu: '%s' maps to joint %lu: '%s'.", i, - command_joint_names_[i].c_str(), map_cmd_to_joints_[i], - params_.joints[map_cmd_to_joints_[i]].c_str()); + // create a map for the command joints, trivial if the size is the same + map_cmd_to_joints_.resize(num_cmd_joints_); + std::iota(map_cmd_to_joints_.begin(), map_cmd_to_joints_.end(), 0); } if (params_.command_interfaces.empty()) @@ -892,10 +900,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( std::bind(&JointTrajectoryController::goal_accepted_callback, this, _1)); resize_joint_trajectory_point(state_current_, dof_); - resize_joint_trajectory_point_command(command_current_, dof_); + resize_joint_trajectory_point_command( + command_current_, dof_, std::numeric_limits::quiet_NaN()); resize_joint_trajectory_point(state_desired_, dof_); resize_joint_trajectory_point(state_error_, dof_); - resize_joint_trajectory_point(last_commanded_state_, dof_); + resize_joint_trajectory_point( + last_commanded_state_, dof_, std::numeric_limits::quiet_NaN()); query_state_srv_ = get_node()->create_service( std::string(get_node()->get_name()) + "/query_state", @@ -956,7 +966,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); - if (read_state_from_command_interfaces(state)) + if (dof_ == num_cmd_joints_ && read_state_from_command_interfaces(state)) { state_current_ = state; last_commanded_state_ = state; @@ -1513,37 +1523,37 @@ bool JointTrajectoryController::contains_interface_type( } void JointTrajectoryController::resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value) { - point.positions.resize(size, std::numeric_limits::quiet_NaN()); + point.positions.resize(size, value); if (has_velocity_state_interface_) { - point.velocities.resize(size, std::numeric_limits::quiet_NaN()); + point.velocities.resize(size, value); } if (has_acceleration_state_interface_) { - point.accelerations.resize(size, std::numeric_limits::quiet_NaN()); + point.accelerations.resize(size, value); } } void JointTrajectoryController::resize_joint_trajectory_point_command( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size) + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value) { if (has_position_command_interface_) { - point.positions.resize(size, 0.0); + point.positions.resize(size, value); } if (has_velocity_command_interface_) { - point.velocities.resize(size, 0.0); + point.velocities.resize(size, value); } if (has_acceleration_command_interface_) { - point.accelerations.resize(size, 0.0); + point.accelerations.resize(size, value); } if (has_effort_command_interface_) { - point.effort.resize(size, 0.0); + point.effort.resize(size, value); } } @@ -1556,7 +1566,7 @@ void JointTrajectoryController::update_pids() { for (size_t i = 0; i < num_cmd_joints_; ++i) { - const auto & gains = params_.gains.joints_map.at(command_joint_names_[i]); + const auto & gains = params_.gains.joints_map.at(params_.joints.at(map_cmd_to_joints_[i])); if (pids_[i]) { // update PIDs with gains from ROS parameters diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 4c7aa95684..839acf894c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -61,7 +61,6 @@ void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } // Floating-point value comparison threshold const double EPS = 1e-6; -#if 0 TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; @@ -120,7 +119,6 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command compare_joints(joint_names_, command_joint_names_); } -#endif TEST_P( TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_less_than_dof) @@ -194,7 +192,6 @@ TEST_P( ASSERT_EQ(state.id(), State::PRIMARY_STATE_UNCONFIGURED); } -#if 0 TEST_P(TrajectoryControllerTestParameterized, activate) { rclcpp::executors::MultiThreadedExecutor executor; @@ -432,7 +429,6 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) EXPECT_EQ(n_joints, state->output.effort.size()); } } -#endif TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_joints_less_than_dof) { @@ -532,7 +528,7 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_jo EXPECT_EQ(n_joints, state->output.effort.size()); } } -#if 0 + /** * @brief check if dynamic parameters are updated */ @@ -870,12 +866,11 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) executor.cancel(); } -#endif /** - * @brief check if position error of revolute joints are normalized if not configured so + * @brief check if trajectory error is calculated correctly in case #command-joints < #dof */ -TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less_than_dof) +TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_less_than_dof) { rclcpp::executors::MultiThreadedExecutor executor; constexpr double k_p = 10.0; @@ -936,10 +931,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less // 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); EXPECT_NEAR(points[0][0], state_msg->output.positions[0], allowed_delta); EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->output.positions[2], allowed_delta); + EXPECT_TRUE(std::isnan(state_msg->output.positions[2])); } if (traj_controller_->has_velocity_command_interface()) @@ -947,10 +941,9 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less // check command interface EXPECT_LT(0.0, joint_vel_[0]); EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, joint_vel_[2]); EXPECT_LT(0.0, state_msg->output.velocities[0]); EXPECT_LT(0.0, state_msg->output.velocities[1]); - EXPECT_LT(0.0, state_msg->output.velocities[2]); + EXPECT_TRUE(std::isnan(state_msg->output.velocities[2])); // use_closed_loop_pid_adapter_ if (traj_controller_->use_closed_loop_pid_adapter()) @@ -962,9 +955,109 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less EXPECT_NEAR( k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); - // no normalization of position error + } + } + + if (traj_controller_->has_effort_command_interface()) + { + // check command interface + EXPECT_LT(0.0, joint_eff_[0]); + EXPECT_LT(0.0, joint_eff_[1]); + EXPECT_LT(0.0, state_msg->output.effort[0]); + EXPECT_LT(0.0, state_msg->output.effort[1]); + EXPECT_TRUE(std::isnan(state_msg->output.effort[2])); + } + + executor.cancel(); +} + +/** + * @brief check if trajectory error is calculated correctly in case #command-joints < #dof + */ +TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_less_than_dof_jumbled) +{ + rclcpp::executors::MultiThreadedExecutor executor; + constexpr double k_p = 10.0; + std::vector command_joint_names{joint_names_[1], joint_names_[0]}; + const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names); + std::vector params = { + rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); + subscribeToState(); + + size_t n_joints = joint_names_.size(); + + // send msg for all joints + 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->feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + EXPECT_EQ(n_joints, state_msg->reference.positions.size()); + EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); + EXPECT_EQ(n_joints, state_msg->error.positions.size()); + + // are the correct reference positions used? + EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); + EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); + + // no normalization of position error + EXPECT_NEAR( + state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR( + state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR( + state_msg->error.positions[2], state_msg->reference.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][0], state_msg->output.positions[0], allowed_delta); + EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); + EXPECT_TRUE(std::isnan(state_msg->output.positions[2])); + } + + if (traj_controller_->has_velocity_command_interface()) + { + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + EXPECT_LT(0.0, state_msg->output.velocities[0]); + EXPECT_LT(0.0, state_msg->output.velocities[1]); + EXPECT_TRUE(std::isnan(state_msg->output.velocities[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->reference.positions[2] - INITIAL_POS_JOINTS[2]), joint_vel_[2], + k_p * (state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * allowed_delta); + EXPECT_NEAR( + k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], k_p * allowed_delta); } } @@ -974,15 +1067,14 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_command_joints_less // check command interface EXPECT_LT(0.0, joint_eff_[0]); EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, joint_eff_[2]); EXPECT_LT(0.0, state_msg->output.effort[0]); EXPECT_LT(0.0, state_msg->output.effort[1]); - EXPECT_LT(0.0, state_msg->output.effort[2]); + EXPECT_TRUE(std::isnan(state_msg->output.effort[2])); } executor.cancel(); } -#if 0 + /** * @brief cmd_timeout must be greater than constraints.goal_time */ @@ -2124,7 +2216,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_goal_tolerances_fail) // it should be still holding the old point expectCommandPoint(hold_position); } -#endif + // position controllers INSTANTIATE_TEST_SUITE_P( PositionTrajectoryControllers, TrajectoryControllerTestParameterized, From d1d55d3820cff1a4d9be0a65f6af6b87850070de Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:34:40 +0000 Subject: [PATCH 03/13] Remove not needed included --- .../include/joint_trajectory_controller/trajectory.hpp | 1 - 1 file changed, 1 deletion(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index e7995efb66..189e2a82fd 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -15,7 +15,6 @@ #ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ #define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_ -#include #include #include From 1327fd9dcbbd40ad7595070b1399a86001548f0e Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:36:25 +0000 Subject: [PATCH 04/13] Add some comments to the tests --- .../test/test_trajectory_controller.cpp | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 839acf894c..2227e03ec0 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -120,6 +120,9 @@ TEST_P(TrajectoryControllerTestParameterized, check_interface_names_with_command compare_joints(joint_names_, command_joint_names_); } +/** + * \brief same as check_interface_names_with_command_joints but with #command-joints < #dof + */ TEST_P( TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_less_than_dof) { @@ -329,7 +332,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param } /** - * @brief test if correct topic is received + * @brief test if correct topic is received, consistent with parameters * * this test doesn't use class variables but subscribes to the state topic */ @@ -430,6 +433,10 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) } } +/** + * @brief same as state_topic_consistency but with #command-joints < #dof + */ + TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_joints_less_than_dof) { rclcpp::executors::SingleThreadedExecutor executor; @@ -972,7 +979,8 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le } /** - * @brief check if trajectory error is calculated correctly in case #command-joints < #dof + * @brief check if trajectory error is calculated correctly in case #command-joints < #dof, but with + * jumbled order of command joints */ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_less_than_dof_jumbled) { From 474b04a7f661776ee045e18d675d36eef811eeb3 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:36:25 +0000 Subject: [PATCH 05/13] Fix typos --- .../joint_trajectory_controller.hpp | 4 ++-- .../src/joint_trajectory_controller.cpp | 11 ++++++----- 2 files changed, 8 insertions(+), 7 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 1df3a7eb8e..8f3fba93f6 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 @@ -294,9 +294,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void init_hold_position_msg(); void resize_joint_trajectory_point( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double vale = 0.0); + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0); void resize_joint_trajectory_point_command( - trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double vale = 0.0); + trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0); }; } // namespace joint_trajectory_controller diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index f076b09931..e701737d84 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -283,11 +283,12 @@ controller_interface::return_type JointTrajectoryController::update( // Update PIDs for (auto i = 0ul; i < num_cmd_joints_; ++i) { - size_t index = map_cmd_to_joints_[i]; - tmp_command_[index] = (state_desired_.velocities[index] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[index], state_error_.velocities[index], - (uint64_t)period.nanoseconds()); + size_t index_cmd_joint = map_cmd_to_joints_[i]; + tmp_command_[index_cmd_joint] = + (state_desired_.velocities[index_cmd_joint] * ff_velocity_scale_[index_cmd_joint]) + + pids_[i]->computeCommand( + state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint], + (uint64_t)period.nanoseconds()); } } From 28bfa80126ffa4f9aa2f19c0972ee6d547602beb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:36:25 +0000 Subject: [PATCH 06/13] Update comments --- .../src/joint_trajectory_controller.cpp | 4 ++++ .../test/test_trajectory_controller.cpp | 1 - 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index e701737d84..a0b63f2a0a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -967,6 +967,10 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); + // read from cmd joints only if all joints have command interface + // otherwise it leaves the entries of joints without command interface NaN. + // if no open_loop control, state_current_ is then used for `set_point_before_trajectory_msg` and + // future trajectory sampling will always give NaN for these joints if (dof_ == num_cmd_joints_ && read_state_from_command_interfaces(state)) { state_current_ = state; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 2227e03ec0..25a748eb67 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -436,7 +436,6 @@ TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency) /** * @brief same as state_topic_consistency but with #command-joints < #dof */ - TEST_P(TrajectoryControllerTestParameterized, state_topic_consistency_command_joints_less_than_dof) { rclcpp::executors::SingleThreadedExecutor executor; From aa67b0bc8b4ef30cf7d9ed9cddffacff87b9f040 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:36:25 +0000 Subject: [PATCH 07/13] Use a function to reuse for tests --- .../test/test_trajectory_controller.cpp | 33 ++----------------- 1 file changed, 2 insertions(+), 31 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 25a748eb67..27d52720ce 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -127,7 +127,7 @@ TEST_P( TrajectoryControllerTestParameterized, check_interface_names_with_command_joints_less_than_dof) { rclcpp::executors::MultiThreadedExecutor executor; - // set command_joints parameter + // set command_joints parameter to a subset of joint_names_ std::vector command_joint_names{joint_names_[0], joint_names_[1]}; const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names); SetUpTrajectoryController(executor, {command_joint_names_param}); @@ -135,36 +135,7 @@ TEST_P( const auto state = traj_controller_->get_node()->configure(); ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); - std::vector state_interface_names; - state_interface_names.reserve(joint_names_.size() * state_interface_types_.size()); - for (const auto & joint : joint_names_) - { - for (const auto & interface : state_interface_types_) - { - state_interface_names.push_back(joint + "/" + interface); - } - } - auto state_interfaces = traj_controller_->state_interface_configuration(); - EXPECT_EQ(state_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ(state_interfaces.names.size(), joint_names_.size() * state_interface_types_.size()); - ASSERT_THAT(state_interfaces.names, testing::UnorderedElementsAreArray(state_interface_names)); - - std::vector command_interface_names; - command_interface_names.reserve(command_joint_names.size() * command_interface_types_.size()); - for (const auto & joint : command_joint_names) - { - for (const auto & interface : command_interface_types_) - { - command_interface_names.push_back(joint + "/" + interface); - } - } - auto command_interfaces = traj_controller_->command_interface_configuration(); - EXPECT_EQ( - command_interfaces.type, controller_interface::interface_configuration_type::INDIVIDUAL); - EXPECT_EQ( - command_interfaces.names.size(), command_joint_names.size() * command_interface_types_.size()); - ASSERT_THAT( - command_interfaces.names, testing::UnorderedElementsAreArray(command_interface_names)); + compare_joints(joint_names_, command_joint_names); } TEST_P( From 7d62e83df883bd0e38c80171a3e10347cebbaacb Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:36:25 +0000 Subject: [PATCH 08/13] Fix includes --- .../test/test_trajectory_actions.cpp | 6 +++--- .../test/test_trajectory_controller.cpp | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 87d557df1b..bc9d9719aa 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -30,9 +30,7 @@ #include "action_msgs/msg/goal_status_array.hpp" #include "control_msgs/action/detail/follow_joint_trajectory__struct.hpp" #include "controller_interface/controller_interface.hpp" -#include "gtest/gtest.h" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/executors/multi_threaded_executor.hpp" @@ -45,10 +43,12 @@ #include "rclcpp_action/client_goal_handle.hpp" #include "rclcpp_action/create_client.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -#include "test_trajectory_controller_utils.hpp" #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" +#include "test_trajectory_controller_utils.hpp" + using std::placeholders::_1; using std::placeholders::_2; using test_trajectory_controllers::TestableJointTrajectoryController; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 27d52720ce..14d130d93a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -30,7 +30,6 @@ #include "builtin_interfaces/msg/time.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/resource_manager.hpp" -#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "lifecycle_msgs/msg/state.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -50,6 +49,7 @@ #include "trajectory_msgs/msg/joint_trajectory.hpp" #include "trajectory_msgs/msg/joint_trajectory_point.hpp" +#include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "test_trajectory_controller_utils.hpp" using lifecycle_msgs::msg::State; From 141f686eeabc36f57e2a633591acc39902272e22 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Christoph=20Fr=C3=B6hlich?= Date: Tue, 28 Nov 2023 08:36:25 +0000 Subject: [PATCH 09/13] Use correct ff_velocity_scale parameter --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a0b63f2a0a..1fca832c82 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -285,7 +285,7 @@ controller_interface::return_type JointTrajectoryController::update( { size_t index_cmd_joint = map_cmd_to_joints_[i]; tmp_command_[index_cmd_joint] = - (state_desired_.velocities[index_cmd_joint] * ff_velocity_scale_[index_cmd_joint]) + + (state_desired_.velocities[index_cmd_joint] * ff_velocity_scale_[i]) + pids_[i]->computeCommand( state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint], (uint64_t)period.nanoseconds()); From 88ddb8f1a004e80e0f61f96e070e69a5a0a7528c Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:37:12 +0000 Subject: [PATCH 10/13] Fix tests due to allow_nonzero.. --- .../test/test_trajectory_controller_utils.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 4a65b4ad51..8eef7f8a8e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -261,6 +261,7 @@ class TrajectoryControllerTest : public ::testing::Test // set pid parameters before configure SetPidParameters(k_p, ff, angle_wraparound); + traj_controller_->get_node()->configure(); ActivateTrajectoryController( From c5c2f0105427bd63ec446036cb81cadc676a4efe Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 08:37:12 +0000 Subject: [PATCH 11/13] Remove unnecessary warning --- .../src/joint_trajectory_controller.cpp | 9 --------- 1 file changed, 9 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1fca832c82..0a037ab9ee 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -73,15 +73,6 @@ JointTrajectoryController::command_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - if (num_cmd_joints_ == 0) - { - fprintf( - stderr, - "During ros2_control interface configuration, number of command interfaces is not valid;" - " it should be positive. Actual number is %zu\n", - num_cmd_joints_); - std::exit(EXIT_FAILURE); - } conf.names.reserve(num_cmd_joints_ * params_.command_interfaces.size()); for (const auto & joint_name : command_joint_names_) { From 23146d84881813783ffc60d02ba948614fc89038 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Tue, 28 Nov 2023 09:43:04 +0000 Subject: [PATCH 12/13] Use new update method for added tests --- .../test/test_trajectory_controller.cpp | 132 +++++++----------- .../test/test_trajectory_controller_utils.hpp | 7 + 2 files changed, 61 insertions(+), 78 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 14d130d93a..c7a219ea4c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -853,10 +853,8 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le constexpr double k_p = 10.0; std::vector command_joint_names{joint_names_[0], joint_names_[1]}; const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names); - std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param}; + std::vector params = {command_joint_names_param}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -872,45 +870,39 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le 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); + // trylock() has to succeed at least once to have current_command set + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - const auto allowed_delta = 0.1; + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + auto current_command = traj_controller_->get_current_command(); // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); // no normalization of position error - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR(state_error.positions[2], state_reference.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][0], state_msg->output.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); - EXPECT_TRUE(std::isnan(state_msg->output.positions[2])); + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_TRUE(std::isnan(current_command.positions[2])); } if (traj_controller_->has_velocity_command_interface()) @@ -918,20 +910,18 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le // check command interface EXPECT_LT(0.0, joint_vel_[0]); EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, state_msg->output.velocities[0]); - EXPECT_LT(0.0, state_msg->output.velocities[1]); - EXPECT_TRUE(std::isnan(state_msg->output.velocities[2])); + EXPECT_TRUE(std::isnan(current_command.velocities[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->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); } } @@ -940,9 +930,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le // check command interface EXPECT_LT(0.0, joint_eff_[0]); EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, state_msg->output.effort[0]); - EXPECT_LT(0.0, state_msg->output.effort[1]); - EXPECT_TRUE(std::isnan(state_msg->output.effort[2])); + EXPECT_TRUE(std::isnan(current_command.effort[2])); } executor.cancel(); @@ -958,10 +946,8 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le constexpr double k_p = 10.0; std::vector command_joint_names{joint_names_[1], joint_names_[0]}; const rclcpp::Parameter command_joint_names_param("command_joints", command_joint_names); - std::vector params = { - rclcpp::Parameter("allow_nonzero_velocity_at_trajectory_end", true), command_joint_names_param}; + std::vector params = {command_joint_names_param}; SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0, false); - subscribeToState(); size_t n_joints = joint_names_.size(); @@ -977,45 +963,39 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le 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); + // trylock() has to succeed at least once to have current_command set + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); - const auto allowed_delta = 0.1; + // get states from class variables + auto state_feedback = traj_controller_->get_state_feedback(); + auto state_reference = traj_controller_->get_state_reference(); + auto state_error = traj_controller_->get_state_error(); + auto current_command = traj_controller_->get_current_command(); // no update of state_interface - EXPECT_EQ(state_msg->feedback.positions, INITIAL_POS_JOINTS); + EXPECT_EQ(state_feedback.positions, INITIAL_POS_JOINTS); // has the msg the correct vector sizes? - EXPECT_EQ(n_joints, state_msg->reference.positions.size()); - EXPECT_EQ(n_joints, state_msg->feedback.positions.size()); - EXPECT_EQ(n_joints, state_msg->error.positions.size()); + EXPECT_EQ(n_joints, state_reference.positions.size()); + EXPECT_EQ(n_joints, state_feedback.positions.size()); + EXPECT_EQ(n_joints, state_error.positions.size()); // are the correct reference positions used? - EXPECT_NEAR(points[0][0], state_msg->reference.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->reference.positions[1], allowed_delta); - EXPECT_NEAR(points[0][2], state_msg->reference.positions[2], 3 * allowed_delta); + EXPECT_NEAR(points[0][0], state_reference.positions[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], state_reference.positions[1], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][2], state_reference.positions[2], COMMON_THRESHOLD); // no normalization of position error - EXPECT_NEAR( - state_msg->error.positions[0], state_msg->reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); - EXPECT_NEAR( - state_msg->error.positions[1], state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); - EXPECT_NEAR( - state_msg->error.positions[2], state_msg->reference.positions[2] - INITIAL_POS_JOINTS[2], EPS); + EXPECT_NEAR(state_error.positions[0], state_reference.positions[0] - INITIAL_POS_JOINTS[0], EPS); + EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS); + EXPECT_NEAR(state_error.positions[2], state_reference.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][0], state_msg->output.positions[0], allowed_delta); - EXPECT_NEAR(points[0][1], state_msg->output.positions[1], allowed_delta); - EXPECT_TRUE(std::isnan(state_msg->output.positions[2])); + EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD); + EXPECT_TRUE(std::isnan(current_command.positions[2])); } if (traj_controller_->has_velocity_command_interface()) @@ -1023,20 +1003,18 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le // check command interface EXPECT_LT(0.0, joint_vel_[0]); EXPECT_LT(0.0, joint_vel_[1]); - EXPECT_LT(0.0, state_msg->output.velocities[0]); - EXPECT_LT(0.0, state_msg->output.velocities[1]); - EXPECT_TRUE(std::isnan(state_msg->output.velocities[2])); + EXPECT_TRUE(std::isnan(current_command.velocities[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->reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], - k_p * allowed_delta); + k_p * (state_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); EXPECT_NEAR( - k_p * (state_msg->reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], - k_p * allowed_delta); + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); } } @@ -1045,9 +1023,7 @@ TEST_P(TrajectoryControllerTestParameterized, trajectory_error_command_joints_le // check command interface EXPECT_LT(0.0, joint_eff_[0]); EXPECT_LT(0.0, joint_eff_[1]); - EXPECT_LT(0.0, state_msg->output.effort[0]); - EXPECT_LT(0.0, state_msg->output.effort[1]); - EXPECT_TRUE(std::isnan(state_msg->output.effort[2])); + EXPECT_TRUE(std::isnan(current_command.effort[2])); } 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 8eef7f8a8e..19c563e284 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -23,6 +23,7 @@ #include "gmock/gmock.h" +#include "control_msgs/msg/joint_trajectory_controller_state.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/joint_trajectory_controller.hpp" #include "joint_trajectory_controller/trajectory.hpp" @@ -154,6 +155,12 @@ 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_state_error() { return state_error_; } + trajectory_msgs::msg::JointTrajectoryPoint get_current_command() { return command_current_; } + + control_msgs::msg::JointTrajectoryControllerState get_state_msg() + { + return state_publisher_->msg_; + } rclcpp::WaitSet joint_cmd_sub_wait_set_; }; From ea50f0f014b4c9f0969144e8ecdff4863347370b Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Sun, 10 Dec 2023 18:13:21 +0000 Subject: [PATCH 13/13] Fix rqt_jtc with num_cmd