diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 199f1b30dc..bb16f5b250 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -102,7 +102,7 @@ gains (structure) u = k_{ff} v_d + k_p e + k_i \sum e dt + k_d (v_d - v) - with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below), + with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see ``angle_wraparound`` below), the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively. gains..p (double) @@ -130,10 +130,12 @@ gains..ff_velocity_scale (double) Default: 0.0 -gains..normalize_error (bool) +gains..angle_wraparound (bool) + For joints that wrap around (without end stop, ie. are continuous), + where the shortest rotation to the target position is the desired motion. If true, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`. Otherwise :math:`e = s_d - s` is used, with the desired position :math:`s_d` and the measured - position :math:`s` from the state interface. Use this for revolute joints without end stop, - where the shortest rotation to the target position is the desired motion. + position :math:`s` from the state interface. + Default: false diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 229456c104..ad40fc2da8 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -726,12 +726,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure( } } - // Configure joint position error normalization from ROS parameters + // Configure joint position error normalization from ROS parameters (angle_wraparound) normalize_joint_error_.resize(dof_); for (size_t i = 0; i < dof_; ++i) { const auto & gains = params_.gains.joints_map.at(params_.joints[i]); - normalize_joint_error_[i] = gains.normalize_error; + if (gains.normalize_error) + { + // TODO(anyone): Remove deprecation warning in the end of 2023 + RCLCPP_INFO(logger, "`normalize_error` is deprecated, use `angle_wraparound` instead!"); + normalize_joint_error_[i] = gains.normalize_error; + } + else + { + normalize_joint_error_[i] = gains.angle_wraparound; + } } if (params_.state_interfaces.empty()) diff --git a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml index d299944976..5a1d985a7a 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -113,7 +113,13 @@ joint_trajectory_controller: normalize_error: { type: bool, default_value: false, - description: "Use position error normalization to -pi to pi." + description: "(Deprecated) Use position error normalization to -pi to pi." + } + angle_wraparound: { + type: bool, + default_value: false, + description: "For joints that wrap around (ie. are continuous). + Normalizes position-error to -pi to pi." } constraints: stopped_velocity_tolerance: {