From 6cee5c72413fbd0253b0d02e96423decb126bd9e Mon Sep 17 00:00:00 2001 From: Philipp Ruppel Date: Mon, 20 May 2024 12:07:04 +0200 Subject: [PATCH] fix control of discontinuous joints --- .../joint_trajectory_controller_impl.h | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h index 58be9e348..bbccef4da 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h +++ b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller_impl.h @@ -779,7 +779,11 @@ updateStates(const ros::Time& sample_time, const Trajectory* const traj) desired_state_.velocity[joint_index] = desired_joint_state_.velocity[0]; desired_state_.acceleration[joint_index] = desired_joint_state_.acceleration[0]; - state_error_.position[joint_index] = angles::shortest_angular_distance(current_state_.position[joint_index],desired_joint_state_.position[0]); + if(angle_wraparound_[joint_index]) + state_error_.position[joint_index] = angles::shortest_angular_distance(current_state_.position[joint_index], desired_joint_state_.position[0]); + else + state_error_.position[joint_index] = desired_joint_state_.position[0] - current_state_.position[joint_index]; + state_error_.velocity[joint_index] = desired_joint_state_.velocity[0] - current_state_.velocity[joint_index]; state_error_.acceleration[joint_index] = 0.0;