From 38d27980b1cf54444fa3d0ba367ab912aa063af8 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:12:37 +0000 Subject: [PATCH 01/11] Simplify initialization of states --- .../src/joint_trajectory_controller.cpp | 15 +++++++++------ 1 file changed, 9 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 88dd75b08a..1b7ff33788 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -951,12 +951,8 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( subscriber_is_active_ = true; - // Initialize current state storage if hardware state has tracking offset - read_state_from_hardware(state_current_); - read_state_from_hardware(state_desired_); - read_state_from_hardware(last_commanded_state_); - // Handle restart of controller by reading from commands if - // those are not nan + // Handle restart of controller by reading from commands if those are not nan (a controller was + // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); if (read_state_from_command_interfaces(state)) @@ -965,6 +961,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( state_desired_ = state; last_commanded_state_ = state; } + else + { + // Initialize current state storage if hardware state has tracking offset + read_state_from_hardware(state_current_); + read_state_from_hardware(state_desired_); + read_state_from_hardware(last_commanded_state_); + } // Should the controller start by holding position at the beginning of active state? if (params_.start_with_holding) From 751237dd7761949ba85477be92cd631f2e6cb5cd Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:12:37 +0000 Subject: [PATCH 02/11] Update *states_has_offset* tests' criteria --- .../test/test_trajectory_controller.cpp | 18 ++++++++++-------- 1 file changed, 10 insertions(+), 8 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index b2efc44bff..54fdb7744a 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1672,7 +1672,6 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) { rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); // set command values to NaN @@ -1697,7 +1696,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && traj_controller_->has_velocity_command_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); } // check acceleration @@ -1707,7 +1706,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && traj_controller_->has_acceleration_command_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); } } @@ -1719,15 +1718,18 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) { rclcpp::executors::SingleThreadedExecutor executor; - // default if false so it will not be actually set parameter rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); - // set command values to NaN + // set command values to arbitrary values + std::vector initial_pos_cmd, initial_vel_cmd, initial_acc_cmd; for (size_t i = 0; i < 3; ++i) { joint_pos_[i] = 3.1 + static_cast(i); + initial_pos_cmd.push_back(joint_pos_[i]); joint_vel_[i] = 0.25 + static_cast(i); + initial_vel_cmd.push_back(joint_vel_[i]); joint_acc_[i] = 0.02 + static_cast(i) / 10.0; + initial_acc_cmd.push_back(joint_acc_[i]); } SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); @@ -1735,7 +1737,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co for (size_t i = 0; i < 3; ++i) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); // check velocity if ( @@ -1744,7 +1746,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && traj_controller_->has_velocity_command_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); } // check acceleration @@ -1754,7 +1756,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && traj_controller_->has_acceleration_command_interface()) { - EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]); } } From c73a7afa9f7e86b2f7f0e3890257e1468b8315cf Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:12:37 +0000 Subject: [PATCH 03/11] Fix comment --- joint_trajectory_controller/test/test_trajectory_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 54fdb7744a..f8eec52e6c 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1713,7 +1713,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co executor.cancel(); } -// Testing that values are read from state interfaces when hardware is started after some values +// Testing that values are read from command interfaces when hardware is started after some values // are set on the hardware commands TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start) { From ef6decaa34b826e4fb1fbd9262fa86f5c6a8acd0 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:12:37 +0000 Subject: [PATCH 04/11] Update comments and debug output --- .../joint_trajectory_controller.hpp | 6 ++++ .../src/joint_trajectory_controller.cpp | 29 +++++++++++++++++-- 2 files changed, 33 insertions(+), 2 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 bf75e3f99b..0dbd42121d 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 @@ -263,6 +263,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa void read_state_from_hardware(JointTrajectoryPoint & state); + /** Assign values from the command interfaces as state. + * This is only possible if command AND state interfaces exist for the same type, + * therefore needs check for both. + * @param[out] state to be filled with values from command interfaces. + * @return true if all interfaces exists and contain non-NaN values, false otherwise. + */ bool read_state_from_command_interfaces(JointTrajectoryPoint & state); bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1b7ff33788..a83894dd7b 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -463,9 +463,18 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto } else { + if (has_position_command_interface_ == false) + { + RCLCPP_DEBUG(get_node()->get_logger(), "No position command interface found."); + } + if (interface_has_values(joint_command_interface_[0]) == false) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Interface position doesn't have values."); + } state.positions.clear(); has_values = false; } + // velocity and acceleration states are optional if (has_velocity_state_interface_) { @@ -475,6 +484,14 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto } else { + if (has_velocity_command_interface_ == false) + { + RCLCPP_DEBUG(get_node()->get_logger(), "No velocity command interface found."); + } + if (interface_has_values(joint_command_interface_[1]) == false) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Interface velocity doesn't have values."); + } state.velocities.clear(); has_values = false; } @@ -492,6 +509,14 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto } else { + if (has_acceleration_command_interface_ == false) + { + RCLCPP_DEBUG(get_node()->get_logger(), "No acceleration command interface found."); + } + if (interface_has_values(joint_command_interface_[2]) == false) + { + RCLCPP_DEBUG(get_node()->get_logger(), "Interface acceleration doesn't have values."); + } state.accelerations.clear(); has_values = false; } @@ -951,7 +976,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( subscriber_is_active_ = true; - // Handle restart of controller by reading from commands if those are not nan (a controller was + // Handle restart of controller by reading from commands if those are not NaN (a controller was // running already) trajectory_msgs::msg::JointTrajectoryPoint state; resize_joint_trajectory_point(state, dof_); @@ -963,7 +988,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( } else { - // Initialize current state storage if hardware state has tracking offset + // Initialize current state storage from hardware read_state_from_hardware(state_current_); read_state_from_hardware(state_desired_); read_state_from_hardware(last_commanded_state_); From eb34ead7d146b988d6444ef91dd7232d5a98c211 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:13:29 +0000 Subject: [PATCH 05/11] Fix test criteria and initialization of state + command interfaces --- .../test/test_trajectory_controller.cpp | 101 +++++++++++------- .../test/test_trajectory_controller_utils.hpp | 72 ++++++++----- 2 files changed, 110 insertions(+), 63 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index f8eec52e6c..6a81c3f590 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1668,20 +1668,23 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e #endif // Testing that values are read from state interfaces when hardware is started for the first -// time and hardware state has offset --> this is indicated by NaN values in state interfaces +// time and hardware state has offset --> this is indicated by NaN values in command interfaces TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start) { rclcpp::executors::SingleThreadedExecutor executor; rclcpp::Parameter is_open_loop_parameters("open_loop_control", true); // set command values to NaN - for (size_t i = 0; i < 3; ++i) - { - joint_pos_[i] = std::numeric_limits::quiet_NaN(); - joint_vel_[i] = std::numeric_limits::quiet_NaN(); - joint_acc_[i] = std::numeric_limits::quiet_NaN(); - } - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + std::vector initial_pos_cmd{3, std::numeric_limits::quiet_NaN()}; + std::vector initial_vel_cmd{3, std::numeric_limits::quiet_NaN()}; + std::vector initial_acc_cmd{3, std::numeric_limits::quiet_NaN()}; + + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from state interfaces + // (command interface are NaN) auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); @@ -1690,21 +1693,13 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); // check velocity - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && - traj_controller_->has_velocity_command_interface()) + if (traj_controller_->has_velocity_state_interface()) { EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); } // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - traj_controller_->has_acceleration_command_interface()) + if (traj_controller_->has_acceleration_state_interface()) { EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); } @@ -1731,32 +1726,66 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co joint_acc_[i] = 0.02 + static_cast(i) / 10.0; initial_acc_cmd.push_back(joint_acc_[i]); } - SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true); + SetUpAndActivateTrajectoryController( + executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, + initial_acc_cmd); + + // no call of update method, so the values should be read from command interfaces auto current_state_when_offset = traj_controller_->get_current_state_when_offset(); for (size_t i = 0; i < 3; ++i) { - EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); - - // check velocity - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() && - traj_controller_->has_velocity_command_interface()) + // check position + if (traj_controller_->has_position_command_interface()) { - EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + // check velocity + if (traj_controller_->has_velocity_state_interface()) + { + if (traj_controller_->has_velocity_command_interface()) + { + // check acceleration + if (traj_controller_->has_acceleration_state_interface()) + { + if (traj_controller_->has_acceleration_command_interface()) + { + // should have set it to last position + velocity + acceleration command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]); + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]); + } + } + else + { + // should have set it to last position + velocity command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]); + } + } + else + { + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); + EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]); + } + } + else + { + // should have set it to last position command + EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]); + } } - - // check acceleration - if ( - std::find( - state_interface_types_.begin(), state_interface_types_.end(), - hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() && - traj_controller_->has_acceleration_command_interface()) + else { - EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]); + // should have set it to the state interface instead + EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]); } } diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 69f41d2cd9..9913bfe961 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -220,7 +220,11 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, - bool angle_wraparound = false) + bool angle_wraparound = false, + const std::vector initial_pos_joints = INITIAL_POS_JOINTS, + const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, + const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { SetUpTrajectoryController(executor); @@ -239,12 +243,17 @@ class TrajectoryControllerTest : public ::testing::Test traj_controller_->get_node()->configure(); - ActivateTrajectoryController(separate_cmd_and_state_values); + ActivateTrajectoryController( + separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints, + initial_eff_joints); } void ActivateTrajectoryController( bool separate_cmd_and_state_values = false, - const std::vector initial_pos_joints = INITIAL_POS_JOINTS) + const std::vector initial_pos_joints = INITIAL_POS_JOINTS, + const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, + const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, + const std::vector initial_eff_joints = INITIAL_EFF_JOINTS) { std::vector cmd_interfaces; std::vector state_interfaces; @@ -280,14 +289,17 @@ class TrajectoryControllerTest : public ::testing::Test cmd_interfaces.emplace_back(pos_cmd_interfaces_.back()); cmd_interfaces.back().set_value(initial_pos_joints[i]); cmd_interfaces.emplace_back(vel_cmd_interfaces_.back()); - cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]); + 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.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]; + cmd_interfaces.back().set_value(initial_eff_joints[i]); + if (separate_cmd_and_state_values) + { + joint_state_pos_[i] = INITIAL_POS_JOINTS[i]; + joint_state_vel_[i] = INITIAL_VEL_JOINTS[i]; + joint_state_acc_[i] = INITIAL_ACC_JOINTS[i]; + } state_interfaces.emplace_back(pos_state_interfaces_.back()); state_interfaces.emplace_back(vel_state_interfaces_.back()); state_interfaces.emplace_back(acc_state_interfaces_.back()); @@ -489,27 +501,33 @@ class TrajectoryControllerTest : public ::testing::Test // --> set kp > 0.0 in test if (traj_controller_->has_velocity_command_interface()) { - EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_vel_[0])) - << "current error: " << point.at(0) - joint_state_pos_[0] << ", velocity command is " - << joint_vel_[0]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_vel_[1])) - << "current error: " << point.at(1) - joint_state_pos_[1] << ", velocity command is " - << joint_vel_[1]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_vel_[2])) - << "current error: " << point.at(2) - joint_state_pos_[2] << ", velocity command is " - << joint_vel_[2]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_vel_[0])) + << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() + << ", velocity command is " << joint_vel_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_vel_[1])) + << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() + << ", velocity command is " << joint_vel_[1]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_vel_[2])) + << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() + << ", velocity command is " << joint_vel_[2]; } if (traj_controller_->has_effort_command_interface()) { - EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_eff_[0])) - << "current error: " << point.at(0) - joint_state_pos_[0] << ", effort command is " - << joint_eff_[0]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_eff_[1])) - << "current error: " << point.at(1) - joint_state_pos_[1] << ", effort command is " - << joint_eff_[1]; - EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_eff_[2])) - << "current error: " << point.at(2) - joint_state_pos_[2] << ", effort command is " - << joint_eff_[2]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_eff_[0])) + << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value() + << ", effort command is " << joint_eff_[0]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_eff_[1])) + << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value() + << ", effort command is " << joint_eff_[1]; + EXPECT_TRUE( + is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_eff_[2])) + << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value() + << ", effort command is " << joint_eff_[2]; } } } From 55a2844ef103c33729cc256ff61267d5b4f9622d Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:13:29 +0000 Subject: [PATCH 06/11] Cleanup debug code --- .../src/joint_trajectory_controller.cpp | 24 ------------------- 1 file changed, 24 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index a83894dd7b..d37a0444f9 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -463,14 +463,6 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto } else { - if (has_position_command_interface_ == false) - { - RCLCPP_DEBUG(get_node()->get_logger(), "No position command interface found."); - } - if (interface_has_values(joint_command_interface_[0]) == false) - { - RCLCPP_DEBUG(get_node()->get_logger(), "Interface position doesn't have values."); - } state.positions.clear(); has_values = false; } @@ -484,14 +476,6 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto } else { - if (has_velocity_command_interface_ == false) - { - RCLCPP_DEBUG(get_node()->get_logger(), "No velocity command interface found."); - } - if (interface_has_values(joint_command_interface_[1]) == false) - { - RCLCPP_DEBUG(get_node()->get_logger(), "Interface velocity doesn't have values."); - } state.velocities.clear(); has_values = false; } @@ -509,14 +493,6 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto } else { - if (has_acceleration_command_interface_ == false) - { - RCLCPP_DEBUG(get_node()->get_logger(), "No acceleration command interface found."); - } - if (interface_has_values(joint_command_interface_[2]) == false) - { - RCLCPP_DEBUG(get_node()->get_logger(), "Interface acceleration doesn't have values."); - } state.accelerations.clear(); has_values = false; } From f1eaa4928c495fe0e1d50dc1cf457c91cd402991 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:13:29 +0000 Subject: [PATCH 07/11] Cleanup one test --- .../test/test_trajectory_controller.cpp | 9 +++------ 1 file changed, 3 insertions(+), 6 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 6a81c3f590..117575be37 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -1719,12 +1719,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co std::vector initial_pos_cmd, initial_vel_cmd, initial_acc_cmd; for (size_t i = 0; i < 3; ++i) { - joint_pos_[i] = 3.1 + static_cast(i); - initial_pos_cmd.push_back(joint_pos_[i]); - joint_vel_[i] = 0.25 + static_cast(i); - initial_vel_cmd.push_back(joint_vel_[i]); - joint_acc_[i] = 0.02 + static_cast(i) / 10.0; - initial_acc_cmd.push_back(joint_acc_[i]); + initial_pos_cmd.push_back(3.1 + static_cast(i)); + initial_vel_cmd.push_back(0.25 + static_cast(i)); + initial_acc_cmd.push_back(0.02 + static_cast(i) / 10.0); } SetUpAndActivateTrajectoryController( executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd, From 34ec02ea67827d651c524e0a66c8dcd8ebaa7cbc Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:13:29 +0000 Subject: [PATCH 08/11] Rename read_state_from_hardware method Co-authored-by: Dr. Denis --- .../joint_trajectory_controller.hpp | 2 +- .../src/joint_trajectory_controller.cpp | 10 +++++----- 2 files changed, 6 insertions(+), 6 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 0dbd42121d..eaf821aa26 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 @@ -261,7 +261,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa const rclcpp::Time & time, const JointTrajectoryPoint & desired_state, const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error); - void read_state_from_hardware(JointTrajectoryPoint & state); + void read_state_from_state_interfaces(JointTrajectoryPoint & state); /** Assign values from the command interfaces as state. * This is only possible if command AND state interfaces exist for the same type, diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index d37a0444f9..1c2c31f4d1 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -175,7 +175,7 @@ controller_interface::return_type JointTrajectoryController::update( // current state update state_current_.time_from_start.set__sec(0); - read_state_from_hardware(state_current_); + read_state_from_state_interfaces(state_current_); // currently carrying out a trajectory if (has_active_trajectory()) @@ -397,7 +397,7 @@ controller_interface::return_type JointTrajectoryController::update( return controller_interface::return_type::OK; } -void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state) +void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state) { auto assign_point_from_interface = [&](std::vector & trajectory_point_interface, const auto & joint_interface) @@ -965,9 +965,9 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( else { // Initialize current state storage from hardware - read_state_from_hardware(state_current_); - read_state_from_hardware(state_desired_); - read_state_from_hardware(last_commanded_state_); + read_state_from_state_interfaces(state_current_); + read_state_from_state_interfaces(state_desired_); + read_state_from_state_interfaces(last_commanded_state_); } // Should the controller start by holding position at the beginning of active state? From 5a111e35b719242ebd5b73fecc5da9c611a542d7 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:13:29 +0000 Subject: [PATCH 09/11] Don't set state_desired in on_activate --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1c2c31f4d1..1ba027d5c2 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -959,14 +959,12 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate( if (read_state_from_command_interfaces(state)) { state_current_ = state; - state_desired_ = state; last_commanded_state_ = state; } else { // Initialize current state storage from hardware read_state_from_state_interfaces(state_current_); - read_state_from_state_interfaces(state_desired_); read_state_from_state_interfaces(last_commanded_state_); } From 387afc7c723d148e39f8149f9c54bb5111078a98 Mon Sep 17 00:00:00 2001 From: Christoph Froehlich Date: Wed, 15 Nov 2023 17:28:35 +0000 Subject: [PATCH 10/11] Fix whitespace --- .../test/test_trajectory_controller_utils.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp index 9913bfe961..7118882da1 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp +++ b/joint_trajectory_controller/test/test_trajectory_controller_utils.hpp @@ -220,7 +220,7 @@ class TrajectoryControllerTest : public ::testing::Test void SetUpAndActivateTrajectoryController( rclcpp::Executor & executor, const std::vector & parameters = {}, bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0, - bool angle_wraparound = false, + bool angle_wraparound = false, const std::vector initial_pos_joints = INITIAL_POS_JOINTS, const std::vector initial_vel_joints = INITIAL_VEL_JOINTS, const std::vector initial_acc_joints = INITIAL_ACC_JOINTS, From efcf0d1799bffeeded304fd4f9c9471f4150689a Mon Sep 17 00:00:00 2001 From: "Dr. Denis" Date: Wed, 15 Nov 2023 19:12:35 +0100 Subject: [PATCH 11/11] Remove added empty line. --- joint_trajectory_controller/src/joint_trajectory_controller.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 1ba027d5c2..cf35156541 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -466,7 +466,6 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto state.positions.clear(); has_values = false; } - // velocity and acceleration states are optional if (has_velocity_state_interface_) {