From 6919f3826e2bc09bd9d3a8ceb459271dee167880 Mon Sep 17 00:00:00 2001 From: "Ethan K. Gordon" Date: Wed, 14 Jun 2023 16:45:58 -0700 Subject: [PATCH] Command final waypoint identically when traj_point_active_ptr_ is nullptr --- .../src/joint_trajectory_controller.cpp | 102 +++++++++--------- 1 file changed, 51 insertions(+), 51 deletions(-) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 4d994e26bf..5e3e0fd770 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -241,57 +241,6 @@ controller_interface::return_type JointTrajectoryController::update( } } - // set values for next hardware write() if tolerance is met - if (!tolerance_violated_while_moving && within_goal_time) - { - if (use_closed_loop_pid_adapter_) - { - // Update PIDs - for (auto i = 0ul; i < dof_; ++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()); - } - } - - // set values for next hardware write() - if (has_position_command_interface_) - { - assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); - } - if (has_velocity_command_interface_) - { - 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); - } - } - if (has_acceleration_command_interface_) - { - assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); - } - 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); - } - } - - // store the previous command. Used in open-loop control mode - last_commanded_state_ = state_desired_; - } - const auto active_goal = *rt_active_goal_.readFromRT(); if (active_goal) { @@ -359,6 +308,57 @@ controller_interface::return_type JointTrajectoryController::update( set_hold_position(); RCLCPP_ERROR(get_node()->get_logger(), "Holding position due to state tolerance violation"); } + + // set values for next hardware write() if tolerance is met + if (!tolerance_violated_while_moving && within_goal_time) + { + if (use_closed_loop_pid_adapter_) + { + // Update PIDs + for (auto i = 0ul; i < dof_; ++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()); + } + } + + // set values for next hardware write() + if (has_position_command_interface_) + { + assign_interface_from_point(joint_command_interface_[0], state_desired_.positions); + } + if (has_velocity_command_interface_) + { + if (traj_point_active_ptr_ && 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); + } + } + if (has_acceleration_command_interface_) + { + assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations); + } + if (has_effort_command_interface_) + { + if (traj_point_active_ptr_ && 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); + } + } + + // store the previous command. Used in open-loop control mode + last_commanded_state_ = state_desired_; + } } }