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 ae370df0a6..d51c592d44 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 @@ -120,6 +120,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // Degrees of freedom size_t dof_; + size_t num_cmd_joints_; + std::vector map_cmd_to_joints_; // Storing command joint names for interfaces std::vector command_joint_names_; @@ -295,9 +297,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 value = 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 value = 0.0); urdf::Model model_; @@ -313,9 +315,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void assign_interface_from_point( const T & 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]]); } } }; diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp index b00d79481c..b2c429c07b 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp +++ b/joint_trajectory_controller/include/joint_trajectory_controller/trajectory.hpp @@ -165,7 +165,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) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 5e3d9d1c7d..ef3917d291 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -17,7 +17,7 @@ #include #include #include - +#include #include #include @@ -40,7 +40,7 @@ namespace joint_trajectory_controller { JointTrajectoryController::JointTrajectoryController() -: controller_interface::ControllerInterface(), dof_(0) +: controller_interface::ControllerInterface(), dof_(0), num_cmd_joints_(0) { } @@ -83,16 +83,7 @@ JointTrajectoryController::command_interface_configuration() const { controller_interface::InterfaceConfiguration conf; conf.type = controller_interface::interface_configuration_type::INDIVIDUAL; - if (dof_ == 0) - { - fprintf( - stderr, - "During ros2_control interface configuration, degrees of freedom is not valid;" - " it should be positive. Actual DOF is %zu\n", - dof_); - 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) @@ -258,12 +249,14 @@ 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_cmd_joint = map_cmd_to_joints_[i]; + tmp_command_[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()); } } @@ -435,9 +428,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(); } }; @@ -506,9 +500,10 @@ 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(); + trajectory_point_interface[map_cmd_to_joints_[index]] = + joint_interface[index].get().get_value(); } }; @@ -647,8 +642,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; @@ -659,12 +654,40 @@ 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_) + { + // 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, + "'command_joints' parameter must be a subset of 'joints' parameter, if their size is not " + "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()); + } + } + else + { + // 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()) { @@ -694,9 +717,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(); } @@ -871,10 +894,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", @@ -905,7 +930,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; } @@ -935,7 +960,11 @@ 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)) + // 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; last_commanded_state_ = state; @@ -990,7 +1019,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_deactivate( rt_active_goal_.writeFromNonRT(RealtimeGoalHandlePtr()); } - for (size_t index = 0; index < dof_; ++index) + for (size_t index = 0; index < num_cmd_joints_; ++index) { if (has_position_command_interface_) { @@ -1536,37 +1565,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, 0.0); + point.positions.resize(size, value); if (has_velocity_state_interface_) { - point.velocities.resize(size, 0.0); + point.velocities.resize(size, value); } if (has_acceleration_state_interface_) { - point.accelerations.resize(size, 0.0); + 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); } } @@ -1577,9 +1606,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(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_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 f5e9cb7260..84c0287d79 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -44,6 +44,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_assets.hpp" #include "test_trajectory_controller_utils.hpp" @@ -53,6 +54,9 @@ using test_trajectory_controllers::TrajectoryControllerTestParameterized; void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } +// Floating-point value comparison threshold +const double EPS = 1e-6; + TEST_P(TrajectoryControllerTestParameterized, configure_state_ignores_commands) { rclcpp::executors::MultiThreadedExecutor executor; @@ -112,6 +116,52 @@ 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) +{ + rclcpp::executors::MultiThreadedExecutor executor; + // 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}); + + const auto state = traj_controller_->get_node()->configure(); + ASSERT_EQ(state.id(), State::PRIMARY_STATE_INACTIVE); + + compare_joints(joint_names_, command_joint_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); +} + TEST_P(TrajectoryControllerTestParameterized, activate) { rclcpp::executors::MultiThreadedExecutor executor; @@ -249,7 +299,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 */ @@ -350,6 +400,108 @@ 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; + 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()); + } +} + /** * @brief check if dynamic parameters are updated */ @@ -459,9 +611,6 @@ TEST_P(TrajectoryControllerTestParameterized, hold_on_startup) executor.cancel(); } -// Floating-point value comparison threshold -const double EPS = 1e-6; - /** * @brief check if calculated trajectory error is correct (angle wraparound) for continuous joints */ @@ -847,6 +996,191 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound) 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) +{ + 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 = {command_joint_names_param}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0); + + 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); + + // trylock() has to succeed at least once to have current_command set + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // 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_feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + 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_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_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], 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()) + { + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + 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_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + } + } + + 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_TRUE(std::isnan(current_command.effort[2])); + } + + executor.cancel(); +} + +/** + * @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) +{ + 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 = {command_joint_names_param}; + SetUpAndActivateTrajectoryController(executor, params, true, k_p, 0.0); + + 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); + + // trylock() has to succeed at least once to have current_command set + updateControllerAsync(rclcpp::Duration(FIRST_POINT_TIME)); + + // 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_feedback.positions, INITIAL_POS_JOINTS); + + // has the msg the correct vector sizes? + 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_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_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], 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()) + { + // check command interface + EXPECT_LT(0.0, joint_vel_[0]); + EXPECT_LT(0.0, joint_vel_[1]); + 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_reference.positions[0] - INITIAL_POS_JOINTS[0]), joint_vel_[0], + k_p * COMMON_THRESHOLD); + EXPECT_NEAR( + k_p * (state_reference.positions[1] - INITIAL_POS_JOINTS[1]), joint_vel_[1], + k_p * COMMON_THRESHOLD); + } + } + + 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_TRUE(std::isnan(current_command.effort[2])); + } + + executor.cancel(); +} + /** * @brief cmd_timeout must be greater than constraints.goal_time */ diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index baa4111239..77151c6fac 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" @@ -164,6 +165,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_; + } /** * a copy of the private member function diff --git a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py index 72c11625e4..2f5ed7147a 100644 --- a/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py +++ b/rqt_joint_trajectory_controller/rqt_joint_trajectory_controller/joint_trajectory_controller.py @@ -457,12 +457,9 @@ def _joint_widgets(self): # TODO: Cache instead of compute every time? def _jtc_joint_names(jtc_info): - # NOTE: We assume that there is at least one hardware interface that - # claims resources (there should be), and the resource list is fetched - # from the first available interface joint_names = [] - for interface in jtc_info.claimed_interfaces: + for interface in jtc_info.required_state_interfaces: name = interface.split("/")[-2] if name not in joint_names: joint_names.append(name)