[JTC] Parse URDF for continuous joints (ros-controls#949)

joint_trajectory_controller/CMakeLists.txt

@@ -19,6 +19,7 @@ set(THIS_PACKAGE_INCLUDE_DEPENDS
   rsl
   tl_expected
   trajectory_msgs
+  urdf
 )
 
 find_package(ament_cmake REQUIRED)

joint_trajectory_controller/doc/parameters_context.yaml

@@ -9,5 +9,10 @@ gains: |
 
   .. math:: 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 ``angle_wraparound`` below),
+  with the desired velocity :math:`v_d`, the measured velocity :math:`v`, the position error :math:`e` (definition see below),
   the controller period :math:`dt`, and the ``velocity`` or ``effort`` manipulated variable (control variable) :math:`u`, respectively.
+
+  If the joint is of continuous type, the position error :math:`e = normalize(s_d - s)` is normalized between :math:`-\pi, \pi`,
+  i.e., the shortest rotation to the target position is the desired motion.
+  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.

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -43,6 +43,7 @@
 #include "realtime_tools/realtime_server_goal_handle.h"
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
+#include "urdf/model.h"
 
 // auto-generated by generate_parameter_library
 #include "joint_trajectory_controller_parameters.hpp"
@@ -298,6 +299,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void resize_joint_trajectory_point_command(
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
 
+  urdf::Model model_;
+
   /**
    * @brief Assigns the values from a trajectory point interface to a joint interface.
    *

joint_trajectory_controller/package.xml

@@ -25,6 +25,7 @@
   <depend>rsl</depend>
   <depend>tl_expected</depend>
   <depend>trajectory_msgs</depend>
+  <depend>urdf</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>controller_manager</test_depend>

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -36,6 +36,7 @@
 #include "rclcpp_action/create_server.hpp"
 #include "rclcpp_action/server_goal_handle.hpp"
 #include "rclcpp_lifecycle/state.hpp"
+#include "urdf/model.h"
 
 namespace joint_trajectory_controller
 {
@@ -46,6 +47,23 @@ JointTrajectoryController::JointTrajectoryController()
 
 controller_interface::CallbackReturn JointTrajectoryController::on_init()
 {
+  if (!urdf_.empty())
+  {
+    if (!model_.initString(urdf_))
+    {
+      RCLCPP_ERROR(get_node()->get_logger(), "Failed to parse URDF file");
+    }
+    else
+    {
+      RCLCPP_DEBUG(get_node()->get_logger(), "Successfully parsed URDF file");
+    }
+  }
+  else
+  {
+    // empty URDF is used for some tests
+    RCLCPP_DEBUG(get_node()->get_logger(), "No URDF file given");
+  }
+
   try
   {
     // Create the parameter listener and get the parameters
@@ -684,12 +702,33 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
     update_pids();
   }
 
-  // Configure joint position error normalization from ROS parameters (angle_wraparound)
+  // Configure joint position error normalization (angle_wraparound)
   joints_angle_wraparound_.resize(dof_);
   for (size_t i = 0; i < dof_; ++i)
   {
     const auto & gains = params_.gains.joints_map.at(params_.joints[i]);
-    joints_angle_wraparound_[i] = gains.angle_wraparound;
+    if (gains.angle_wraparound)
+    {
+      // TODO(christophfroehlich): remove this warning in a future release (ROS-J)
+      RCLCPP_WARN(
+        logger,
+        "[Deprecated] Parameter 'gains.<joint>.angle_wraparound' is deprecated. The "
+        "angle_wraparound is now used if a continuous joint is configured in the URDF.");
+      joints_angle_wraparound_[i] = true;
+    }
+
+    if (!urdf_.empty())
+    {
+      auto urdf_joint = model_.getJoint(params_.joints[i]);
+      if (urdf_joint && urdf_joint->type == urdf::Joint::CONTINUOUS)
+      {
+        RCLCPP_DEBUG(
+          logger, "joint '%s' is of type continuous, use angle_wraparound.",
+          params_.joints[i].c_str());
+        joints_angle_wraparound_[i] = true;
+      }
+      // do nothing if joint is not found in the URDF
+    }
   }
 
   if (params_.state_interfaces.empty())

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -125,11 +125,8 @@ joint_trajectory_controller:
       angle_wraparound: {
         type: bool,
         default_value: false,
-        description: '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.'
+        description: "[deprecated] For joints that wrap around (ie. are continuous).
+          Normalizes position-error to -pi to pi."
       }
   constraints:
     stopped_velocity_tolerance: {