diff --git a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp index 98fd5a56b1..efcbb6ad83 100644 --- a/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp +++ b/gripper_controllers/include/gripper_controllers/hardware_interface_adapter.hpp @@ -166,7 +166,7 @@ class HardwareInterfaceAdapter const auto period = std::chrono::steady_clock::now() - last_update_time_; // Update PIDs double command = - pid_->computeCommand(error_position, error_velocity, static_cast(period.count())); + pid_->compute_command(error_position, error_velocity, static_cast(period.count())); command = std::min( fabs(max_allowed_effort), std::max(-fabs(max_allowed_effort), command)); joint_handle_->get().set_value(command); diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 365ce993d0..ae00f24388 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -288,9 +288,8 @@ controller_interface::return_type JointTrajectoryController::update( for (auto i = 0ul; i < dof_; ++i) { tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) + - pids_[i]->computeCommand( - state_error_.positions[i], state_error_.velocities[i], - (uint64_t)period.nanoseconds()); + pids_[i]->compute_command( + state_error_.positions[i], state_error_.velocities[i], period); } } @@ -1593,7 +1592,7 @@ void JointTrajectoryController::update_pids() if (pids_[i]) { // update PIDs with gains from ROS parameters - pids_[i]->setGains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); + pids_[i]->set_gains(gains.p, gains.i, gains.d, gains.i_clamp, -gains.i_clamp); } else { diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 21d0277541..8036414c20 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/joint_trajectory_controller/test/test_trajectory_controller.cpp @@ -359,7 +359,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) if (traj_controller_->use_closed_loop_pid_adapter()) { EXPECT_EQ(pids.size(), 3); - auto gain_0 = pids.at(0)->getGains(); + auto gain_0 = pids.at(0)->get_gains(); EXPECT_EQ(gain_0.p_gain_, 0.0); double kp = 1.0; @@ -368,7 +368,7 @@ TEST_P(TrajectoryControllerTestParameterized, update_dynamic_parameters) pids = traj_controller_->get_pids(); EXPECT_EQ(pids.size(), 3); - gain_0 = pids.at(0)->getGains(); + gain_0 = pids.at(0)->get_gains(); EXPECT_EQ(gain_0.p_gain_, kp); } else diff --git a/pid_controller/src/pid_controller.cpp b/pid_controller/src/pid_controller.cpp index f3b2ba33a0..9d387d53ad 100644 --- a/pid_controller/src/pid_controller.cpp +++ b/pid_controller/src/pid_controller.cpp @@ -140,7 +140,7 @@ controller_interface::CallbackReturn PidController::configure_parameters() // prefix should be interpreted as parameters prefix pids_[i] = std::make_shared(get_node(), "gains." + params_.dof_names[i], true); - if (!pids_[i]->initPid()) + if (!pids_[i]->initialize_from_ros_parameters()) { return CallbackReturn::FAILURE; } @@ -536,19 +536,19 @@ controller_interface::return_type PidController::update_and_write_commands( !std::isnan(measured_state_values_[dof_ + i])) { // use calculation with 'error' and 'error_dot' - tmp_command += pids_[i]->computeCommand( + tmp_command += pids_[i]->compute_command( error, reference_interfaces_[dof_ + i] - measured_state_values_[dof_ + i], period); } else { // Fallback to calculation with 'error' only - tmp_command += pids_[i]->computeCommand(error, period); + tmp_command += pids_[i]->compute_command(error, period); } } else { // use calculation with 'error' only - tmp_command += pids_[i]->computeCommand(error, period); + tmp_command += pids_[i]->compute_command(error, period); } // write calculated values