[JointTrajectoryController] Enable position, velocity and acceleratio…

…n interfaces (ros-controls#140)

* Test position only version of JTC for KUKA RSI robots.
* All changes in JTC added
* Don't freak out when wait set can't be established
* Test should expect that before a run, controller is unconfigured as well as after an unsuccessful configure call
* joint_trajectory_controller should not go into FINALIZED state when fails to configure, remain in UNCONFIGURED


Co-authored-by: Bence Magyar <bence.magyar.robotics@gmail.com>

doc/controllers_index.rst

@@ -1,5 +1,38 @@
 .. _controllers:
 
+========================
+ros2_controllers
+========================
+
+`GitHub Repository <https://github.com/ros-controls/ros2_controllers>`_
+
+Nomenclature
+-------------
+The ros2_control framework uses namespaces to sort controller according to the type of command interface they are using.
+The controllers are using `common hardware interface definitions`_.
+The controllers' namespaces are commanding the following command interface types:
+
+  - ``position_controllers``: ``hardware_interface::HW_IF_POSITION``
+  - ``velocity_controller``: ``hardware_interface::HW_IF_VELOCITY``
+  - ``effort_controllers``: ``hardware_interface::HW_IF_EFFORT``
+  - ...
+
+
+Controllers
+--------------
+
+The following standard controllers are implemented:
+
+  - `Joint Trajectory Controller <joint_trajectory_controller/docs/index.rst>`_ - provided a list of waypoints or target point defined with position, velocity and acceleration, the controller interpolates joint trajectories through it.
+  - ... <the list is not complete> ...
+
+
+
+
+.. _common hardware interface definitions: https://github.com/ros-controls/ros2_control/blob/master/hardware_interface/include/hardware_interface/types/hardware_interface_type_values.hpp
+
+
+
 Controllers user documentation
 ==============================
 

forward_command_controller/src/forward_command_controller.cpp

@@ -65,7 +65,7 @@ CallbackReturn ForwardCommandController::on_configure(
     return CallbackReturn::ERROR;
   }
 
-  // Specialized, child controllers set interfaces before calling init function.
+  // Specialized, child controllers set interfaces before calling configure function.
   if (interface_name_.empty()) {
     interface_name_ = node_->get_parameter("interface_name").as_string();
   }

joint_trajectory_controller/CMakeLists.txt

@@ -72,6 +72,7 @@ if(BUILD_TESTING)
   ament_add_gtest(test_trajectory_controller
     test/test_trajectory_controller.cpp
     ENV config_file=${CMAKE_CURRENT_SOURCE_DIR}/test/config/test_joint_trajectory_controller.yaml)
+  set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 120)
   target_include_directories(test_trajectory_controller PRIVATE include)
   target_link_libraries(test_trajectory_controller
     joint_trajectory_controller

joint_trajectory_controller/README.md

@@ -0,0 +1,5 @@
+# joint_trajectory_controllers package
+
+The package implements controllers to interpolate joint's trajectory.
+
+For detailed documentation check the `docs` folder or [ros2_control documentation](https://ros-controls.github.io/control.ros.org/).

joint_trajectory_controller/doc/userdoc.rst

@@ -30,4 +30,80 @@ Other features
 
     Proper handling of wrapping (continuous) joints.
 
-    Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments.
+    Robust to system clock changes: Discontinuous system clock changes do not cause discontinuities in the execution of already queued trajectory segments.
+
+
+Using Joint Trajectory Controller(s)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The controller expects at least position feedback from the hardware.
+Joint velocities and accelerations are optional.
+Currently the controller does not internally integrate velocity from acceleration and position from velocity.
+Therefore if the hardware provides only acceleration or velocity states they have to be integrated in the hardware-interface implementation of velocity and position to use these controllers.
+
+The generic version of Joint Trajectory controller is implemented in this package.
+A yaml file for using it could be:
+
+   .. code-block:: yaml
+
+      controller_manager:
+        ros__parameters:
+          joint_trajectory_controller:
+          type: "joint_trajectory_controller/JointTrajectoryController"
+
+      joint_trajectory_controller:
+        ros__parameters:
+          joints:
+            - joint0
+            - joint1
+            - joint2
+            - joint3
+            - joint4
+            - joint5
+
+          command_interfaces:
+            - position
+
+          state_interfaces:
+            - position
+            - velocity
+
+          state_publish_rate: 50.0 # Defaults to 50
+          action_monitor_rate: 20.0 # Defaults to 20
+
+          allow_partial_joints_goal: false # Defaults to false
+          hardware_state_has_offset: true
+          deduce_states_from_derivatives: true
+          constraints:
+            stopped_velocity_tolerance: 0.01 # Defaults to 0.01
+            goal_time: 0.0 # Defaults to 0.0 (start immediately)
+
+
+Specialized versions of JointTrajectoryController (TBD in ...)
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+The controller types are placed into namespaces according to their command types for the hardware (see `general introduction into controllers <../../index.rst>`_).
+
+The following version of the Joint Trajectory Controller are available mapping the following interfaces:
+
+  - position_controllers::JointTrajectoryController
+    - Input: position, [velocity, [acceleration]]
+    - Output: position
+  - position_velocity_controllers::JointTrajectoryController
+    - Input: position, [velocity, [acceleration]]
+    - Output: position and velocity
+  - position_velocity_acceleration_controllers::JointTrajectoryController
+    - Input: position, [velocity, [acceleration]]
+    - Output: position, velocity and acceleration
+..   - velocity_controllers::JointTrajectoryController
+..     - Input: position, [velocity, [acceleration]]
+..     - Output: velocity
+.. TODO(anyone): would it be possible to output velocty and acceleration?
+..               (to have an vel_acc_controllers)
+..   - effort_controllers::JointTrajectoryController
+..     - Input: position, [velocity, [acceleration]]
+..     - Output: effort
+
+The controller uses `common hardware interface definitions`_.
+
+(*Not implemented yet*) When using pure ``velocity`` or ``effort`` controllers a command is generated using the desired state and state error using a velocity feedforward term plus a corrective PID term. (#171)

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -1,4 +1,4 @@
-// Copyright 2017 Open Source Robotics Foundation, Inc.
+// Copyright (c) 2021 ros2_control Development Team
 //
 // Licensed under the Apache License, Version 2.0 (the "License");
 // you may not use this file except in compliance with the License.
@@ -24,6 +24,7 @@
 #include "control_msgs/msg/joint_trajectory_controller_state.hpp"
 #include "control_msgs/action/follow_joint_trajectory.hpp"
 #include "controller_interface/controller_interface.hpp"
+#include "hardware_interface/types/hardware_interface_type_values.hpp"
 #include "joint_trajectory_controller/tolerances.hpp"
 #include "joint_trajectory_controller/visibility_control.h"
 #include "rclcpp/duration.hpp"
@@ -41,10 +42,6 @@
 #include "trajectory_msgs/msg/joint_trajectory.hpp"
 #include "trajectory_msgs/msg/joint_trajectory_point.hpp"
 
-namespace hardware_interface
-{
-class RobotHardware;
-}  // namespace hardware_interface
 namespace rclcpp_action
 {
 template<typename ActionT>
@@ -74,14 +71,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
    * interfaces for the controlled joints
    */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  controller_interface::InterfaceConfiguration command_interface_configuration() const override;
+  controller_interface::InterfaceConfiguration
+  command_interface_configuration() const override;
 
   /**
    * @brief command_interface_configuration This controller requires the position and velocity
    * state interfaces for the controlled joints
    */
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
-  controller_interface::InterfaceConfiguration state_interface_configuration() const override;
+  controller_interface::InterfaceConfiguration
+  state_interface_configuration() const override;
 
   JOINT_TRAJECTORY_CONTROLLER_PUBLIC
   controller_interface::return_type
@@ -113,15 +112,35 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
 
 protected:
   std::vector<std::string> joint_names_;
-
-  // For convenience, we have ordered the interfaces so i-th position matches i-th index
-  // in joint_names_
-  std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>>
-  joint_position_command_interface_;
-  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
-  joint_position_state_interface_;
-  std::vector<std::reference_wrapper<hardware_interface::LoanedStateInterface>>
-  joint_velocity_state_interface_;
+  std::vector<std::string> command_interface_types_;
+  std::vector<std::string> state_interface_types_;
+
+  // To reduce number of variables and to make the code shorter the interfaces are ordered in types
+  // as the following constants
+  const std::vector<std::string> allowed_interface_types_ = {
+    hardware_interface::HW_IF_POSITION,
+    hardware_interface::HW_IF_VELOCITY,
+    hardware_interface::HW_IF_ACCELERATION,
+    hardware_interface::HW_IF_EFFORT,
+  };
+
+  // The interfaces are defined as the types in 'allowed_interface_types_' member.
+  // For convenience, for each type the interfaces are ordered so that i-th position
+  // matches i-th index in joint_names_
+  template<typename T>
+  using InterfaceReferences = std::vector<std::vector<std::reference_wrapper<T>>>;
+
+  InterfaceReferences<hardware_interface::LoanedCommandInterface> joint_command_interface_;
+  InterfaceReferences<hardware_interface::LoanedStateInterface> joint_state_interface_;
+
+  bool has_velocity_state_interface_ = false;
+  bool has_acceleration_state_interface_ = false;
+  bool has_position_command_interface_ = false;
+  bool has_velocity_command_interface_ = false;
+  bool has_acceleration_command_interface_ = false;
+
+  /// If true, a velocity feedforward term plus corrective PID term is used
+  bool use_closed_loop_pid_adapter = false;
 
   // TODO(karsten1987): eventually activate and deactive subscriber directly when its supported
   bool subscriber_is_active_ = false;
@@ -135,7 +154,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   realtime_tools::RealtimeBuffer<std::shared_ptr<trajectory_msgs::msg::JointTrajectory>>
   traj_msg_external_point_ptr_;
 
-  bool is_halted = false;
+  // The controller should be in halted state after creation otherwise memory corruption
+  // TODO(anyone): Is the variable relevant, since we are using lifecycle?
+  bool is_halted_ = true;
 
   using ControllerStateMsg = control_msgs::msg::JointTrajectoryControllerState;
   using StatePublisher = realtime_tools::RealtimePublisher<ControllerStateMsg>;
@@ -188,13 +209,20 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void set_hold_position();
 
   bool reset();
-  void halt();
 
   using JointTrajectoryPoint = trajectory_msgs::msg::JointTrajectoryPoint;
   void publish_state(
     const JointTrajectoryPoint & desired_state,
     const JointTrajectoryPoint & current_state,
     const JointTrajectoryPoint & state_error);
+
+private:
+  bool contains_interface_type(
+    const std::vector<std::string> & interface_type_list, const std::string & interface_type)
+  {
+    return std::find(interface_type_list.begin(), interface_type_list.end(), interface_type) !=
+           interface_type_list.end();
+  }
 };
 
 }  // namespace joint_trajectory_controller

joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp

@@ -152,8 +152,10 @@ inline bool check_state_tolerance_per_joint(
 {
   using std::abs;
   const double error_position = state_error.positions[joint_idx];
-  const double error_velocity = state_error.velocities[joint_idx];
-  const double error_acceleration = state_error.accelerations[joint_idx];
+  const double error_velocity = state_error.velocities.empty() ?
+    0.0 : state_error.velocities[joint_idx];
+  const double error_acceleration = state_error.accelerations.empty() ?
+    0.0 : state_error.accelerations[joint_idx];
 
   const bool is_valid =
     !(state_tolerance.position > 0.0 && abs(error_position) > state_tolerance.position) &&