diff --git a/ur_controllers/src/scaled_joint_trajectory_controller.cpp b/ur_controllers/src/scaled_joint_trajectory_controller.cpp index e7cee27de..d1a19b70d 100644 --- a/ur_controllers/src/scaled_joint_trajectory_controller.cpp +++ b/ur_controllers/src/scaled_joint_trajectory_controller.cpp @@ -90,7 +90,7 @@ controller_interface::return_type ScaledJointTrajectoryController::update(const auto compute_error_for_joint = [&](JointTrajectoryPoint& error, size_t index, const JointTrajectoryPoint& current, const JointTrajectoryPoint& desired) { // error defined as the difference between current and desired - if (normalize_joint_error_[index]) { + if (joints_angle_wraparound_[index]) { // if desired, the shortest_angular_distance is calculated, i.e., the error is // normalized between -pi