diff --git a/joint_trajectory_controller/doc/parameters.rst b/joint_trajectory_controller/doc/parameters.rst index 8be135ff4b..f62320870c 100644 --- a/joint_trajectory_controller/doc/parameters.rst +++ b/joint_trajectory_controller/doc/parameters.rst @@ -100,7 +100,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) @@ -128,10 +128,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 d85d23b37b..cff2918b75 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -722,12 +722,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 a754929ef7..bb0b45bb80 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml +++ b/joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml @@ -109,7 +109,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: {