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..1adf5d6cbd 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 @@ -139,11 +139,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa bool has_position_command_interface_ = false; bool has_velocity_command_interface_ = false; bool has_acceleration_command_interface_ = false; + bool has_effort_command_interface_ = false; /// If true, a velocity feedforward term plus corrective PID term is used - // TODO(anyone): This flag is not used for now - // 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; // TODO(karsten1987): eventually activate and deactivate subscriber directly when its supported diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index c5b75eb60d..ee693a2710 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -191,23 +191,52 @@ controller_interface::return_type JointTrajectoryController::update( const bool before_last_point = end_segment_itr != (*traj_point_active_ptr_)->end(); // set values for next hardware write() + if (use_closed_loop_pid_adapter) + { + // 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()); + } + } if (has_position_command_interface_) { assign_interface_from_point(joint_command_interface_[0], state_desired.positions); } if (has_velocity_command_interface_) { +<<<<<<< HEAD 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], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[1], state_desired.velocities); + } +>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225)) } if (has_acceleration_command_interface_) { assign_interface_from_point(joint_command_interface_[2], state_desired.accelerations); } - // TODO(anyone): Add here "if using_closed_loop_hw_interface_adapter" (see ROS1) - #171 - // if (check_if_interface_type_exist( - // command_interface_types_, hardware_interface::HW_IF_EFFORT)) { - // assign_interface_from_point(joint_command_interface_[3], state_desired.effort); - // } + if (has_effort_command_interface_) + { + if (use_closed_loop_pid_adapter) + { + assign_interface_from_point(joint_command_interface_[3], tmp_command_); + } + else + { + assign_interface_from_point(joint_command_interface_[3], state_desired.effort); + } + } for (size_t index = 0; index < joint_num; ++index) { @@ -463,6 +492,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S // 2. velocity // 2. position [velocity, [acceleration]] +<<<<<<< HEAD // effort can't be combined with other interfaces if (contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT)) { @@ -495,6 +525,16 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S { 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); + has_effort_command_interface_ = + contains_interface_type(command_interface_types_, hardware_interface::HW_IF_EFFORT); +>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225)) if (has_velocity_command_interface_) { @@ -526,6 +566,44 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S return CallbackReturn::FAILURE; } +<<<<<<< HEAD +======= + // effort can't be combined with other interfaces + if (has_effort_command_interface_) + { + if (command_interface_types_.size() == 1) + { + use_closed_loop_pid_adapter = true; + } + else + { + RCLCPP_ERROR(logger, "'effort' command interface has to be used alone."); + return CallbackReturn::FAILURE; + } + } + + if (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); + } + } + +>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225)) // 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. @@ -569,7 +647,7 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S if (has_velocity_state_interface_) { - if (!contains_interface_type(state_interface_types_, hardware_interface::HW_IF_POSITION)) + if (!has_position_state_interface_) { RCLCPP_ERROR( logger, @@ -578,13 +656,33 @@ CallbackReturn JointTrajectoryController::on_configure(const rclcpp_lifecycle::S return CallbackReturn::FAILURE; } } - else if (has_acceleration_state_interface_) + else { - RCLCPP_ERROR( - logger, - "'acceleration' state interface cannot be used if 'position' and 'velocity' " - "interfaces are not present."); - return CallbackReturn::FAILURE; + if (has_acceleration_state_interface_) + { + RCLCPP_ERROR( + logger, + "'acceleration' state interface cannot be used if 'position' and 'velocity' " + "interfaces are not present."); + return CallbackReturn::FAILURE; + } + if (has_velocity_command_interface_ && command_interface_types_.size() == 1) + { + RCLCPP_ERROR( + logger, + "'velocity' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; + } + // effort is always used alone so no need for size check + if (has_effort_command_interface_) + { + RCLCPP_ERROR( + logger, + "'effort' command interface can only be used alone if 'velocity' and " + "'position' state interfaces are present"); + return CallbackReturn::FAILURE; + } } auto get_interface_list = [](const std::vector & interface_types) { @@ -774,8 +872,26 @@ 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) { +<<<<<<< HEAD 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); + } + + if (has_effort_command_interface_) + { + joint_command_interface_[3][index].get().set_value(0.0); + } +>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225)) } for (size_t index = 0; index < allowed_interface_types_.size(); ++index) @@ -813,6 +929,11 @@ bool JointTrajectoryController::reset() subscriber_is_active_ = false; joint_command_subscriber_.reset(); + for (const auto & pid : pids_) + { + pid->reset(); + } + // iterator has no default value // prev_traj_point_ptr_; traj_point_active_ptr_ = nullptr; diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index a16ff96d13..af9be65981 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -309,6 +309,13 @@ TEST_P(TrajectoryControllerTestParameterized, correct_initialization_using_param EXPECT_NEAR(4.4, joint_pos_[1], allowed_delta); EXPECT_NEAR(5.5, joint_pos_[2], allowed_delta); + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_LE(0.0, joint_eff_[0]); + EXPECT_LE(0.0, joint_eff_[1]); + EXPECT_LE(0.0, joint_eff_[2]); + } + // cleanup state = traj_controller_->cleanup(); @@ -447,6 +454,25 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) traj_msg.points[0].positions[1] = 3.0; traj_msg.points[0].positions[2] = 1.0; +<<<<<<< HEAD +======= + 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; + } + + if (traj_controller_->has_effort_command_interface()) + { + traj_msg.points[0].effort.resize(3); + traj_msg.points[0].effort[0] = -0.1; + traj_msg.points[0].effort[1] = -0.1; + traj_msg.points[0].effort[2] = -0.1; + } + +>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225)) trajectory_publisher_->publish(traj_msg); } @@ -456,9 +482,32 @@ TEST_P(TrajectoryControllerTestParameterized, test_jumbled_joint_order) // Currently COMMON_THRESHOLD is adjusted. updateController(rclcpp::Duration::from_seconds(0.25)); +<<<<<<< HEAD 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]); + } + + if (traj_controller_->has_effort_command_interface()) + { + EXPECT_GT(0.0, joint_eff_[0]); + EXPECT_GT(0.0, joint_eff_[1]); + EXPECT_GT(0.0, joint_eff_[2]); + } +>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225)) } /** @@ -511,6 +560,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_partial_joint_list) EXPECT_NEAR(0.0, joint_vel_[2], threshold) << "Joint 3 velocity should be 0.0 since it's not in the goal"; } + + if ( + std::find(command_interface_types_.begin(), command_interface_types_.end(), "effort") != + command_interface_types_.end()) + { + // estimate the sign of the velocity + // joint rotates forward + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[0] - initial_joint2_cmd, joint_eff_[0])); + EXPECT_TRUE(is_same_sign(traj_msg.points[0].positions[1] - initial_joint1_cmd, joint_eff_[1])); + EXPECT_NEAR(0.0, joint_eff_[2], threshold) + << "Joint 3 effort should be 0.0 since it's not in the goal"; + } // TODO(anyone): add here ckecks for acceleration commands executor.cancel(); @@ -1104,6 +1165,29 @@ INSTANTIATE_TEST_SUITE_P( std::vector({"position", "velocity", "acceleration"}), std::vector({"position", "velocity", "acceleration"})))); +<<<<<<< HEAD +======= +// only velocity controller +INSTANTIATE_TEST_SUITE_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"})))); + +// only effort controller +INSTANTIATE_TEST_CASE_P( + OnlyEffortTrajectoryControllers, TrajectoryControllerTestParameterized, + ::testing::Values( + std::make_tuple( + std::vector({"effort"}), std::vector({"position", "velocity"})), + std::make_tuple( + std::vector({"effort"}), + std::vector({"position", "velocity", "acceleration"})))); + +>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225)) TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parameters) { auto set_parameter_and_check_result = [&]() { @@ -1123,10 +1207,6 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame command_interface_types_ = {"bad_name"}; set_parameter_and_check_result(); - // command interfaces: effort not yet implemented - command_interface_types_ = {"effort"}; - set_parameter_and_check_result(); - // command interfaces: effort has to be only command_interface_types_ = {"effort", "position"}; set_parameter_and_check_result(); @@ -1164,4 +1244,18 @@ TEST_F(TrajectoryControllerTest, incorrect_initialization_using_interface_parame // state interfaces: acceleration without position and velocity state_interface_types_ = {"acceleration"}; set_parameter_and_check_result(); + + // velocity-only command interface: position - velocity not present + command_interface_types_ = {"velocity"}; + state_interface_types_ = {"position"}; + set_parameter_and_check_result(); + state_interface_types_ = {"velocity"}; + set_parameter_and_check_result(); + + // effort-only command interface: position - velocity not present + command_interface_types_ = {"effort"}; + state_interface_types_ = {"position"}; + set_parameter_and_check_result(); + state_interface_types_ = {"velocity"}; + 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..41160b067b 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -36,6 +36,7 @@ const std::vector INITIAL_POS_JOINTS = { INITIAL_POS_JOINT1, INITIAL_POS_JOINT2, INITIAL_POS_JOINT3}; const std::vector INITIAL_VEL_JOINTS = {0.0, 0.0, 0.0}; const std::vector INITIAL_ACC_JOINTS = {0.0, 0.0, 0.0}; +const std::vector INITIAL_EFF_JOINTS = {0.0, 0.0, 0.0}; } // namespace namespace test_trajectory_controllers @@ -94,6 +95,15 @@ class TestableJointTrajectoryController return last_commanded_state_; } +<<<<<<< HEAD +======= + bool has_position_command_interface() { return has_position_command_interface_; } + + bool has_velocity_command_interface() { return has_velocity_command_interface_; } + + bool has_effort_command_interface() { return has_effort_command_interface_; } + +>>>>>>> 97c9431 ([JTC] Implement effort-only command interface (#225)) rclcpp::WaitSet joint_cmd_sub_wait_set_; }; @@ -113,6 +123,7 @@ class TrajectoryControllerTest : public ::testing::Test joint_state_vel_.resize(joint_names_.size(), 0.0); joint_acc_.resize(joint_names_.size(), 0.0); joint_state_acc_.resize(joint_names_.size(), 0.0); + joint_eff_.resize(joint_names_.size(), 0.0); // Default interface values - they will be overwritten by parameterized tests command_interface_types_ = {"position"}; state_interface_types_ = {"position", "velocity"}; @@ -178,6 +189,7 @@ class TrajectoryControllerTest : public ::testing::Test pos_cmd_interfaces_.reserve(joint_names_.size()); vel_cmd_interfaces_.reserve(joint_names_.size()); acc_cmd_interfaces_.reserve(joint_names_.size()); + eff_cmd_interfaces_.reserve(joint_names_.size()); pos_state_interfaces_.reserve(joint_names_.size()); vel_state_interfaces_.reserve(joint_names_.size()); acc_state_interfaces_.reserve(joint_names_.size()); @@ -189,6 +201,8 @@ class TrajectoryControllerTest : public ::testing::Test joint_names_[i], hardware_interface::HW_IF_VELOCITY, &joint_vel_[i])); acc_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( joint_names_[i], hardware_interface::HW_IF_ACCELERATION, &joint_acc_[i])); + eff_cmd_interfaces_.emplace_back(hardware_interface::CommandInterface( + joint_names_[i], hardware_interface::HW_IF_EFFORT, &joint_eff_[i])); pos_state_interfaces_.emplace_back(hardware_interface::StateInterface( joint_names_[i], hardware_interface::HW_IF_POSITION, @@ -207,6 +221,8 @@ class TrajectoryControllerTest : public ::testing::Test cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]); cmd_interfaces.emplace_back(acc_cmd_interfaces_.back()); cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]); + cmd_interfaces.emplace_back(eff_cmd_interfaces_.back()); + cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]); joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; @@ -363,12 +379,14 @@ class TrajectoryControllerTest : public ::testing::Test std::vector joint_pos_; std::vector joint_vel_; std::vector joint_acc_; + std::vector joint_eff_; std::vector joint_state_pos_; std::vector joint_state_vel_; std::vector joint_state_acc_; std::vector pos_cmd_interfaces_; std::vector vel_cmd_interfaces_; std::vector acc_cmd_interfaces_; + std::vector eff_cmd_interfaces_; std::vector pos_state_interfaces_; std::vector vel_state_interfaces_; std::vector acc_state_interfaces_;