diff --git a/joint_trajectory_controller/CMakeLists.txt b/joint_trajectory_controller/CMakeLists.txt index bcbb95d9a6..b11fdcc657 100644 --- a/joint_trajectory_controller/CMakeLists.txt +++ b/joint_trajectory_controller/CMakeLists.txt @@ -14,6 +14,7 @@ find_package(ament_cmake REQUIRED) find_package(angles REQUIRED) find_package(controller_interface REQUIRED) find_package(control_msgs REQUIRED) +find_package(control_toolbox REQUIRED) find_package(hardware_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) @@ -31,6 +32,7 @@ ament_target_dependencies(joint_trajectory_controller builtin_interfaces controller_interface control_msgs + control_toolbox hardware_interface pluginlib rclcpp @@ -65,7 +67,7 @@ if(BUILD_TESTING) ament_add_gtest(test_trajectory_controller test/test_trajectory_controller.cpp ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml) - set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 180) + set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 220) target_include_directories(test_trajectory_controller PRIVATE include) target_link_libraries(test_trajectory_controller joint_trajectory_controller 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 786ef42b0b..f0e74da0c6 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 @@ -24,6 +24,7 @@ #include "control_msgs/action/follow_joint_trajectory.hpp" #include "control_msgs/msg/joint_trajectory_controller_state.hpp" +#include "control_toolbox/pid.hpp" #include "controller_interface/controller_interface.hpp" #include "hardware_interface/types/hardware_interface_type_values.hpp" #include "joint_trajectory_controller/tolerances.hpp" @@ -134,6 +135,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa InterfaceReferences joint_command_interface_; InterfaceReferences joint_state_interface_; + bool has_position_state_interface_ = false; bool has_velocity_state_interface_ = false; bool has_acceleration_state_interface_ = false; bool has_position_command_interface_ = false; @@ -145,6 +147,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa // There should be PID-approach used as in ROS1: // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283 bool use_closed_loop_pid_adapter = false; + using PidPtr = std::shared_ptr; + std::vector pids_; + /// Feed-forward velocity weight factor when calculating closed loop pid adapter's command + std::vector ff_velocity_scale_; + /// reserved storage for result of the command when closed loop pid adapter is used + std::vector tmp_command_; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported bool subscriber_is_active_ = false; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0dd678c354..e2d261f315 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -106,7 +106,7 @@ JointTrajectoryController::state_interface_configuration() const } controller_interface::return_type JointTrajectoryController::update( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) + const rclcpp::Time & /*time*/, const rclcpp::Duration & period) { if (get_state().id() == lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE) { @@ -196,7 +196,23 @@ controller_interface::return_type JointTrajectoryController::update( } if (has_velocity_command_interface_) { - assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); + if (!use_closed_loop_pid_adapter) + { + assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); + } + else + { + // Update PIDs + for (auto i = 0ul; i < joint_num; ++i) + { + tmp_command_[i] = (state_desired.velocities[i] * ff_velocity_scale_[i]) + + pids_[i]->computeCommand( + state_desired.positions[i] - state_current.positions[i], + state_desired.velocities[i] - state_current.velocities[i], + (uint64_t)period.nanoseconds()); + } + assign_interface_from_point(joint_command_interface_[1], tmp_command_); + } } if (has_acceleration_command_interface_) { @@ -482,18 +498,12 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S } } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION)) - { - has_position_command_interface_ = true; - } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY)) - { - has_velocity_command_interface_ = true; - } - if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION)) - { - has_acceleration_command_interface_ = true; - } + has_position_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_POSITION); + has_velocity_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_VELOCITY); + has_acceleration_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_ACCELERATION); if (has_velocity_command_interface_) { @@ -501,10 +511,6 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S if (command_interface_types_.size() == 1) { use_closed_loop_pid_adapter = true; - // TODO(anyone): remove this when implemented - RCLCPP_ERROR(logger, "using 'velocity' command interface alone is not yet implemented yet."); - return CallbackReturn::FAILURE; - // if velocity interface can be used without position if multiple defined } else if (!has_position_command_interface_) { @@ -525,6 +531,28 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S return CallbackReturn::FAILURE; } + // TODO(livanov93): change when other option is implemented + if (has_velocity_command_interface_ && use_closed_loop_pid_adapter) + { + size_t num_joints = joint_names_.size(); + pids_.resize(num_joints); + ff_velocity_scale_.resize(num_joints); + tmp_command_.resize(num_joints, 0.0); + + // Init PID gains from ROS parameter server + for (size_t i = 0; i < pids_.size(); ++i) + { + const std::string prefix = "gains." + joint_names_[i]; + const auto k_p = auto_declare(prefix + ".p", 0.0); + const auto k_i = auto_declare(prefix + ".i", 0.0); + const auto k_d = auto_declare(prefix + ".d", 0.0); + const auto i_clamp = auto_declare(prefix + ".i_clamp", 0.0); + ff_velocity_scale_[i] = auto_declare("ff_velocity_scale/" + joint_names_[i], 0.0); + // Initialize PID + pids_[i] = std::make_shared(k_p, k_i, k_d, i_clamp, -i_clamp); + } + } + // Read always state interfaces from the parameter because they can be used // independently from the controller's type. // Specialized, child controllers should set its default value. @@ -557,14 +585,12 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S } } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY)) - { - has_velocity_state_interface_ = true; - } - if (contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION)) - { - has_acceleration_state_interface_ = true; - } + has_position_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION); + has_velocity_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_VELOCITY); + has_acceleration_state_interface_ = + contains_interface_type(state_interface_types_, hardware_interface::HW_IF_ACCELERATION); if (has_velocity_state_interface_) { @@ -795,8 +821,16 @@ CallbackReturn JointTrajectoryController::on_deactivate(const rclcpp_lifecycle:: // TODO(anyone): How to halt when using effort commands? for (size_t index = 0; index < joint_names_.size(); ++index) { - joint_command_interface_[0][index].get().set_value( - joint_command_interface_[0][index].get().get_value()); + if (has_position_command_interface_) + { + joint_command_interface_[0][index].get().set_value( + joint_command_interface_[0][index].get().get_value()); + } + + if (has_velocity_command_interface_) + { + joint_command_interface_[1][index].get().set_value(0.0); + } } for (size_t index = 0; index < allowed_interface_types_.size(); ++index) @@ -841,6 +875,12 @@ bool JointTrajectoryController::reset() traj_home_point_ptr_.reset(); traj_msg_home_ptr_.reset(); + // reset pids + for (const auto & pid : pids_) + { + pid->reset(); + } + return true; } @@ -986,7 +1026,19 @@ void JointTrajectoryController::fill_partial_goal( for (auto & it : trajectory_msg->points) { // Assume hold position with 0 velocity and acceleration for missing joints - it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + if (!it.positions.empty()) + { + if (has_position_command_interface_) + { + // copy last command if cmd interface exists + it.positions.push_back(joint_command_interface_[0][index].get().get_value()); + } + else if (has_position_state_interface_) + { + // copy current state if state interface exists + it.positions.push_back(joint_state_interface_[0][index].get().get_value()); + } + } if (!it.velocities.empty()) { it.velocities.push_back(0.0); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index a16ff96d13..48c4b41e3e 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -58,6 +58,8 @@ using test_trajectory_controllers::TestableJointTrajectoryController; using test_trajectory_controllers::TrajectoryControllerTest; using test_trajectory_controllers::TrajectoryControllerTestParameterized; +bool is_same_sign(double val1, double val2) { return val1 * val2 >= 0.0; } + void spin(rclcpp::executors::MultiThreadedExecutor * exe) { exe->spin(); } TEST_P(TrajectoryControllerTestParameterized, configure) @@ -267,6 +269,7 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // This call is replacing the way parameters are set via launch SetParameters(); + SetPidParameters(); traj_controller_->configure(); auto state = traj_controller_->get_state(); ASSERT_EQ(State::PRIMARY_STATE_INACTIVE, state.id()); @@ -287,8 +290,10 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // *INDENT-OFF* std::vector> points{ {{3.3, 4.4, 5.5}}, {{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); + publish(time_from_start, points, rclcpp::Time(), {}, points_velocities); traj_controller_->wait_for_trajectory(executor); // first update @@ -304,10 +309,19 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param // TODO(denis): on my laptop I get delta of approx 0.1083. Is this me or is it something wrong? // Come the flackiness here? const auto allowed_delta = 0.11; // 0.05; + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); + EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); + EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + } - EXPECT_NEAR(3.3, joint_pos_[0], allowed_delta); - EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); - EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_LE(0.0, joint_vel_[0]); + EXPECT_LE(0.0, joint_vel_[1]); + EXPECT_LE(0.0, joint_vel_[2]); + } // cleanup state = traj_controller_->cleanup(); @@ -447,6 +461,14 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.points[0].positions[1] = 3.0; traj_msg.points[0].positions[2] = 1.0; + if (traj_controller_->has_velocity_command_interface()) + { + traj_msg.points[0].velocities.resize(3); + traj_msg.points[0].velocities[0] = -0.1; + traj_msg.points[0].velocities[1] = -0.1; + traj_msg.points[0].velocities[2] = -0.1; + } + trajectory_publisher_->publish(traj_msg); } @@ -456,9 +478,19 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) // Currently COMMON_THRESHOLD is adjusted. updateController(rclcpp::Duration::from_seconds(0.25)); - EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); - EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(1.0, joint_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(2.0, joint_pos_[1], COMMON_THRESHOLD); + EXPECT_NEAR(3.0, joint_pos_[2], COMMON_THRESHOLD); + } + + if (traj_controller_->has_velocity_command_interface()) + { + EXPECT_GT(0.0, joint_vel_[0]); + EXPECT_GT(0.0, joint_vel_[1]); + EXPECT_GT(0.0, joint_vel_[2]); + } } /** @@ -471,6 +503,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) rclcpp::executors::SingleThreadedExecutor executor; SetUpAndActivateTrajectoryController(true, {partial_joints_parameters}, &executor); + const double initial_joint1_cmd = joint_pos_[0]; + const double initial_joint2_cmd = joint_pos_[1]; const double initial_joint3_cmd = joint_pos_[2]; trajectory_msgs::msg::JointTrajectory traj_msg; @@ -485,8 +519,10 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) traj_msg.points[0].positions[0] = 2.0; traj_msg.points[0].positions[1] = 1.0; traj_msg.points[0].velocities.resize(2); - traj_msg.points[0].velocities[0] = 2.0; - traj_msg.points[0].velocities[1] = 1.0; + traj_msg.points[0].velocities[0] = + copysign(2.0, traj_msg.points[0].positions[0] - initial_joint2_cmd); + traj_msg.points[0].velocities[1] = + copysign(1.0, traj_msg.points[0].positions[1] - initial_joint1_cmd); trajectory_publisher_->publish(traj_msg); } @@ -496,18 +532,23 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) updateController(rclcpp::Duration::from_seconds(0.25)); double threshold = 0.001; - EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); - EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); - EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) - << "Joint 3 command should be current position"; + + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(traj_msg.points[0].positions[1], joint_pos_[0], threshold); + EXPECT_NEAR(traj_msg.points[0].positions[0], joint_pos_[1], threshold); + EXPECT_NEAR(initial_joint3_cmd, joint_pos_[2], threshold) + << "Joint 3 command should be current position"; + } if ( std::find(command_interface_types_.begin(), command_interface_types_.end(), "velocity") != command_interface_types_.end()) { - // TODO(anyone): need help here - we should at least estimate the sign of the velocity - // EXPECT_NEAR(traj_msg.points[0].velocities[1], joint_vel_[0], threshold); - // EXPECT_NEAR(traj_msg.points[0].velocities[0], joint_vel_[1], threshold); + // estimate the sign of the velocity + // joint rotates forward + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_vel_[0])); + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_vel_[1])); EXPECT_NEAR(0.0, joint_vel_[2], threshold) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } @@ -664,24 +705,33 @@ TEST_P(TrajectoryControllerTestParameterized, test_trajectory_replace) subscribeToState(); std::vector> points_old{{{2., 3., 4.}}}; + std::vector> points_old_velocities{{{0.2, 0.3, 0.4}}}; std::vector> points_partial_new{{1.5}}; + std::vector> points_partial_new_velocities{{0.15}}; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration time_from_start{rclcpp::Duration(delay)}; - publish(time_from_start, points_old); + publish(time_from_start, points_old, rclcpp::Time(), {}, points_old_velocities); trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; expected_actual.positions = {points_old[0].begin(), points_old[0].end()}; expected_desired.positions = {points_old[0].begin(), points_old[0].end()}; + expected_actual.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; + expected_desired.velocities = {points_old_velocities[0].begin(), points_old_velocities[0].end()}; // Check that we reached end of points_old trajectory // Denis: delta was 0.1 with 0.2 works for me waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.2); RCLCPP_INFO(traj_node_->get_logger(), "Sending new trajectory"); - publish(time_from_start, points_partial_new); + points_partial_new_velocities[0][0] = + copysign(0.15, points_partial_new[0][0] - joint_state_pos_[0]); + publish(time_from_start, points_partial_new, rclcpp::Time(), {}, points_partial_new_velocities); // Replaced trajectory is a mix of previous and current goal expected_desired.positions[0] = points_partial_new[0][0]; expected_desired.positions[1] = points_old[0][1]; expected_desired.positions[2] = points_old[0][2]; + expected_desired.velocities[0] = points_partial_new_velocities[0][0]; + expected_desired.velocities[1] = 0.0; + expected_desired.velocities[2] = 0.0; expected_actual = expected_desired; waitAndCompareState(expected_actual, expected_desired, executor, rclcpp::Duration(delay), 0.1); } @@ -764,16 +814,23 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur traj_controller_->activate(); std::vector> full_traj{{{2., 3., 4.}, {4., 6., 8.}}}; + std::vector> full_traj_velocities{{{0.2, 0.3, 0.4}, {0.4, 0.6, 0.8}}}; std::vector> partial_traj{ {{-1., -2.}, { -2., -4, }}}; + std::vector> partial_traj_velocities{ + {{-0.1, -0.2}, + { + -0.2, + -0.4, + }}}; const auto delay = std::chrono::milliseconds(500); builtin_interfaces::msg::Duration points_delay{rclcpp::Duration(delay)}; // Send full trajectory - publish(points_delay, full_traj); + publish(points_delay, full_traj, rclcpp::Time(), {}, full_traj_velocities); // Sleep until first waypoint of full trajectory trajectory_msgs::msg::JointTrajectoryPoint expected_actual, expected_desired; @@ -784,7 +841,8 @@ TEST_P(TrajectoryControllerTestParameterized, test_execute_partial_traj_in_futur // Send partial trajectory starting after full trajecotry is complete RCLCPP_INFO(traj_node->get_logger(), "Sending new trajectory in the future"); - publish(points_delay, partial_traj, rclcpp::Clock().now() + delay * 2); + publish( + points_delay, partial_traj, rclcpp::Clock().now() + delay * 2, {}, partial_traj_velocities); // Wait until the end start and end of partial traj expected_actual.positions = {partial_traj.back()[0], partial_traj.back()[1], full_traj.back()[2]}; @@ -803,7 +861,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro // goal setup std::vector first_goal = {3.3, 4.4, 5.5}; + std::vector> first_goal_velocities = {{0.33, 0.44, 0.55}}; std::vector second_goal = {6.6, 8.8, 11.0}; + std::vector> second_goal_velocities = {{0.66, 0.88, 1.1}}; double state_from_command_offset = 0.3; // send msg @@ -811,69 +871,72 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro time_from_start.sec = 1; time_from_start.nanosec = 0; std::vector> points{{first_goal}}; - publish(time_from_start, points); + publish(time_from_start, points, rclcpp::Time(), {}, first_goal_velocities); traj_controller_->wait_for_trajectory(executor); updateController(rclcpp::Duration::from_seconds(1.1)); - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points); - traj_controller_->wait_for_trajectory(executor); + if (traj_controller_->has_position_command_interface()) + { + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points, rclcpp::Time(), {}, second_goal_velocities); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there should be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_LT(joint_pos_[0], first_goal[0]); - // One the first update(s) there should be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_LT(joint_pos_[0], first_goal[0]); + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there should be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // Expect backward commands at first - EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); + // One the first update(s) there should be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // Expect backward commands at first + EXPECT_NEAR(joint_state_pos_[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_LT(joint_pos_[0], joint_state_pos_[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + } executor.cancel(); } @@ -899,65 +962,68 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e traj_controller_->wait_for_trajectory(executor); updateController(rclcpp::Duration::from_seconds(1.1)); - // JTC is executing trajectory in open-loop therefore: - // - internal state does not have to be updated (in this test-case it shouldn't) - // - internal command is updated - EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = first_goal[0] - state_from_command_offset; - - // Move joint further in the same direction as before (to the second goal) - points = {{second_goal}}; - publish(time_from_start, points); - traj_controller_->wait_for_trajectory(executor); - - // One the first update(s) there **should not** be a "jump" in opposite direction from command - // (towards the state value) - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // There should not be backward commands - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); + if (traj_controller_->has_position_command_interface()) + { + // JTC is executing trajectory in open-loop therefore: + // - internal state does not have to be updated (in this test-case it shouldn't) + // - internal command is updated + EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = first_goal[0] - state_from_command_offset; + + // Move joint further in the same direction as before (to the second goal) + points = {{second_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); + + // One the first update(s) there **should not** be a "jump" in opposite direction from command + // (towards the state value) + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // There should not be backward commands + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); - // Finally the second goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + // Finally the second goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - // State interface should have offset from the command before starting a new trajectory - joint_state_pos_[0] = second_goal[0] - state_from_command_offset; + // State interface should have offset from the command before starting a new trajectory + joint_state_pos_[0] = second_goal[0] - state_from_command_offset; - // Move joint back to the first goal - points = {{first_goal}}; - publish(time_from_start, points); - traj_controller_->wait_for_trajectory(executor); + // Move joint back to the first goal + points = {{first_goal}}; + publish(time_from_start, points); + traj_controller_->wait_for_trajectory(executor); - // One the first update(s) there **should not** be a "jump" in the goal direction from command - // (towards the state value) - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - // There should not be a jump toward commands - EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); - EXPECT_LT(joint_pos_[0], second_goal[0]); - EXPECT_GT(joint_pos_[0], first_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); - traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); - EXPECT_GT(joint_pos_[0], first_goal[0]); - EXPECT_LT(joint_pos_[0], second_goal[0]); + // One the first update(s) there **should not** be a "jump" in the goal direction from command + // (towards the state value) + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + // There should not be a jump toward commands + EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD); + EXPECT_LT(joint_pos_[0], second_goal[0]); + EXPECT_GT(joint_pos_[0], first_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); + traj_controller_->update(rclcpp::Time(0), rclcpp::Duration::from_seconds(0.01)); + EXPECT_GT(joint_pos_[0], first_goal[0]); + EXPECT_LT(joint_pos_[0], second_goal[0]); - // Finally the first goal will be commanded/reached - updateController(rclcpp::Duration::from_seconds(1.1)); - EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + // Finally the first goal will be commanded/reached + updateController(rclcpp::Duration::from_seconds(1.1)); + EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD); + } executor.cancel(); } @@ -1104,6 +1170,16 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"}), std::vector({"position", "velocity", "acceleration"})))); +// only velocity controller +INSTANTIATE_TEST_CASE_P( + OnlyVelocityTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"velocity"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"velocity"}), + std::vector({"position", "velocity", "acceleration"})))); + TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { auto set_parameter_and_check_result = [&]() { @@ -1131,10 +1207,6 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"effort", "position"}; set_parameter_and_check_result(); - // command interfaces: velocity alone not yet implemented - command_interface_types_ = {"velocity"}; - set_parameter_and_check_result(); - // command interfaces: velocity - position not present command_interface_types_ = {"velocity", "acceleration"}; set_parameter_and_check_result(); diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 8ece5c393a..167f82aaad 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -94,6 +94,10 @@ class TestableJointTrajectoryController return last_commanded_state_; } + bool has_position_command_interface() { return has_position_command_interface_; } + + bool has_velocity_command_interface() { return has_velocity_command_interface_; } + rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -146,13 +150,27 @@ class TrajectoryControllerTest : public ::testing::Test const rclcpp::Parameter state_interfaces_params("state_interfaces", state_interface_types_); node->set_parameters({joint_names_param, cmd_interfaces_params, state_interfaces_params}); } + void SetPidParameters() + { + auto node = traj_controller_->get_node(); + + for (size_t i = 0; i < joint_names_.size(); ++i) + { + const std::string prefix = "gains." + joint_names_[i]; + const rclcpp::Parameter k_p(prefix + ".p", 0.0); + const rclcpp::Parameter k_i(prefix + ".i", 0.0); + const rclcpp::Parameter k_d(prefix + ".d", 0.0); + const rclcpp::Parameter i_clamp(prefix + ".i_clamp", 0.0); + const rclcpp::Parameter ff_velocity_scale("ff_velocity_scale/" + joint_names_[i], 1.0); + node->set_parameters({k_p, k_i, k_d, i_clamp, ff_velocity_scale}); + } + } void SetUpAndActivateTrajectoryController( bool use_local_parameters = true, const std::vector & parameters = {}, rclcpp::Executor * executor = nullptr, bool separate_cmd_and_state_values = false) { SetUpTrajectoryController(use_local_parameters); - traj_node_ = traj_controller_->get_node(); for (const auto & param : parameters) { @@ -167,6 +185,9 @@ class TrajectoryControllerTest : public ::testing::Test rclcpp::Parameter stopped_velocity_parameters("constraints.stopped_velocity_tolerance", 0.0); traj_node_->set_parameter(stopped_velocity_parameters); + // set pid parameters before configure + SetPidParameters(); + traj_controller_->configure(); ActivateTrajectoryController(separate_cmd_and_state_values); } @@ -249,7 +270,8 @@ class TrajectoryControllerTest : public ::testing::Test void publish( const builtin_interfaces::msg::Duration & delay_btwn_points, const std::vector> & points, rclcpp::Time start_time = rclcpp::Time(), - const std::vector & joint_names = {}) + const std::vector & joint_names = {}, + const std::vector> & points_velocities = {}) { int wait_count = 0; const auto topic = trajectory_publisher_->get_topic_name(); @@ -294,6 +316,15 @@ class TrajectoryControllerTest : public ::testing::Test duration_total = duration_total + duration; } + for (size_t index = 0; index < points_velocities.size(); ++index) + { + traj_msg.points[index].velocities.resize(points_velocities[index].size()); + for (size_t j = 0; j < points_velocities[index].size(); ++j) + { + traj_msg.points[index].velocities[j] = points_velocities[index][j]; + } + } + trajectory_publisher_->publish(traj_msg); } @@ -326,14 +357,20 @@ class TrajectoryControllerTest : public ::testing::Test { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocties and accelerations - EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(expected_actual.positions[i], state_msg->actual.positions[i], allowed_delta); + } } for (size_t i = 0; i < expected_desired.positions.size(); ++i) { SCOPED_TRACE("Joint " + std::to_string(i)); // TODO(anyone): add checking for velocties and accelerations - EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + if (traj_controller_->has_position_command_interface()) + { + EXPECT_NEAR(expected_desired.positions[i], state_msg->desired.positions[i], allowed_delta); + } } }