Enable JTC for hardware having offset from state measurements (ros-controls#189)

* Avoid "jumps" with states that have tracking error. All test are passing but separatelly. Is there some kind of timeout?

* Remove allow_integration_flag

* Add reading from command interfaces when restarting controller

* Rename variable according to review

* Use even more clear name

* Remove debug lines.

* Update documentation

* Apply suggestions from code review

Co-authored-by: Bence Magyar <[email protected]>

* Move methods to cpp file as per reviewers comment.

Co-authored-by: Bence Magyar <[email protected]>

Author: destogl

doc/controllers_index.rst

@@ -1,13 +1,15 @@
 .. _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:
@@ -19,7 +21,7 @@ The controllers' namespaces are commanding the following command interface types
 
 
 Controllers
---------------
+***********
 
 The following standard controllers are implemented:
 
@@ -30,7 +32,7 @@ The following standard controllers are implemented:
 
 
 Guidelines and Best Practices
-=============================
+*****************************
 
 .. toctree::
    :titlesonly:

joint_trajectory_controller/CMakeLists.txt

@@ -70,7 +70,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)
+  set_tests_properties(test_trajectory_controller PROPERTIES TIMEOUT 180)
   target_include_directories(test_trajectory_controller PRIVATE include)
   target_link_libraries(test_trajectory_controller
     joint_trajectory_controller

joint_trajectory_controller/doc/userdoc.rst

@@ -6,7 +6,7 @@ joint_trajectory_controller
 Controller for executing joint-space trajectories on a group of joints. Trajectories are specified as a set of waypoints to be reached at specific time instants, which the controller attempts to execute as well as the mechanism allows. Waypoints consist of positions, and optionally velocities and accelerations.
 
 Trajectory representation
-^^^^^^^^^^^^^^^^^^^^^^^^^
+-------------------------
 
 The controller is templated to work with multiple trajectory representations. By default, a spline interpolator is provided, but it's possible to support other representations. The spline interpolator uses the following interpolation strategies depending on the waypoint specification:
 
@@ -17,14 +17,14 @@ The controller is templated to work with multiple trajectory representations. By
     Quintic: Position, velocity and acceleration are specified: Guarantees continuity at the acceleration level.
 
 Hardware interface type
-^^^^^^^^^^^^^^^^^^^^^^^
+-----------------------
 
 The controller is templated to work with multiple hardware interface types. Currently joints with position, velocity and effort interfaces are supported. For position-controlled joints, desired positions are simply forwarded to the joints; while for velocity (effort) joints, the position+velocity trajectory following error is mapped to velocity (effort) commands through a PID loop. Example controller configurations can be found here.
 
 Similarly to the trajectory representation case above, it's possible to support new hardware interfaces, or alternative mappings to an already supported interface (eg. a proxy controller for generating effort commands).
 
 Other features
-^^^^^^^^^^^^^^
+--------------
 
     Realtime-safe implementation.
 
@@ -34,7 +34,7 @@ Other features
 
 
 Using Joint Trajectory Controller(s)
-^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+------------------------------------
 
 The controller expects at least position feedback from the hardware.
 Joint velocities and accelerations are optional.
@@ -54,12 +54,12 @@ A yaml file for using it could be:
       joint_trajectory_controller:
         ros__parameters:
           joints:
-            - joint0
             - joint1
             - joint2
             - joint3
             - joint4
             - joint5
+            - joint6
 
           command_interfaces:
             - position
@@ -68,19 +68,93 @@ A yaml file for using it could be:
             - position
             - velocity
 
-          state_publish_rate: 50.0 # Defaults to 50
-          action_monitor_rate: 20.0 # Defaults to 20
+          state_publish_rate: 50.0
+          action_monitor_rate: 20.0
 
-          allow_partial_joints_goal: false # Defaults to false
-          hardware_state_has_offset: true
-          deduce_states_from_derivatives: true
+          allow_partial_joints_goal: false
+          open_loop_control: true
           constraints:
-            stopped_velocity_tolerance: 0.01 # Defaults to 0.01
-            goal_time: 0.0 # Defaults to 0.0 (start immediately)
+            stopped_velocity_tolerance: 0.01
+            goal_time: 0.0
+            joint1:
+              trajectory: 0.05
+              goal: 0.03
+
+
+Details about parameters
+^^^^^^^^^^^^^^^^^^^^^^^^
+
+joint (list(string)):
+  Joint names to control.
+
+command_interface (list(string)):
+  Command interfaces provided by the hardware interface for all joints.
+
+  Values: [position | velocity | acceleration] (multiple allowed)
+
+state_interfaces (list(string)):
+  State interfaces provided by the hardware for all joints.
+
+  Values: position (mandatory) [velocity, [acceleration]].
+  Acceleration interface can only be used in combination with position and velocity.
+
+  Default: 50.0
+
+state_publish_rate (double):
+  Publish-rate of the controller's "state" topic.
+
+  Default: 20.0
+
+action_monitor_rate (double):
+  Rate to monitor status changes when the controller is executing action (control_msgs::action::FollowJointTrajectory).
+
+allow_partial_joints_goal (boolean):
+  Allow joint goals defining trajectory for only some joints.
+
+open_loop_control (boolean):
+  Use controller in open-loop control mode using ignoring the states provided by hardware interface and using last commands as states in the next control step. This is useful if hardware states are not following commands, i.e., an offset between those (typical for hydraulic manipulators).
+
+  If this flag is set, the controller tries to read the values from the command interfaces on starting. If they have real numeric values, those will be used instead of state interfaces. Therefore it is important set command interfaces to NaN (std::numeric_limits<double>::quiet_NaN()) or state values when the hardware is started.
+
+constraints (structure)
+  Default values for tolerances if no explicit values are states in JointTrajectory message.
+
+constraints.stopped_velocity_tolerance (double)
+  Default value for end velocity deviation.
+
+  Default: 0.01
+
+constraints.goal_time (double)
+  Maximally allowed tolerance for not reaching the end of the trajectory in a predefined time.
+
+  Default: 0.0 (not checked)
+
+constraints.<joint_name>.trajectory
+  Maximally allowed deviation from the target trajectory for a given joint.
+
+  Default: 0.0 (tolerance is not enforced)
+
+constraints.<joint_name>.goal
+  Maximally allowed deviation from the goal (end of the trajectory) for a given joint.
+
+  Default: 0.0 (tolerance is not enforced)
+
+
+ROS2 interface of the controller
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+~/joint_trajectory (input topic) [trajectory_msgs::msg::JointTrajectory]
+  Topic for commanding the controller.
+
+~/state (output topic) [control_msgs::msg::JointTrajectoryControllerState]
+  Topic publishing internal states.
+
+~/follow_joint_trajectory (action server) [control_msgs::action::FollowJointTrajectory]
+  Action server for commanding the controller.
 
 
 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>`_).
 

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -126,6 +126,12 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     hardware_interface::HW_IF_EFFORT,
   };
 
+  // Parameters for some special cases, e.g. hydraulics powered robots
+  /// Run he controller in open-loop, i.e., read hardware states only when starting controller.
+  /// This is useful when robot is not exactly following the commanded trajectory.
+  bool open_loop_control_ = false;
+  trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
+
   // 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_
@@ -142,6 +148,9 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   bool has_acceleration_command_interface_ = false;
 
   /// If true, a velocity feedforward term plus corrective PID term is used
+  // TODO(anyone): This flag is not used for now
+  // There should be PID-approach used as in ROS1:
+  // https://github.com/ros-controls/ros_controllers/blob/noetic-devel/joint_trajectory_controller/include/joint_trajectory_controller/hardware_interface_adapter.h#L283
   bool use_closed_loop_pid_adapter = false;
 
   // TODO(karsten1987): eventually activate and deactive subscriber directly when its supported
@@ -231,13 +240,16 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     const JointTrajectoryPoint & current_state,
     const JointTrajectoryPoint & state_error);
 
+  void read_state_from_hardware(JointTrajectoryPoint & state);
+
+  bool read_state_from_command_interfaces(JointTrajectoryPoint & state);
+
 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();
-  }
+    const std::vector<std::string> & interface_type_list, const std::string & interface_type);
+
+  void resize_joint_trajectory_point(
+    trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size);
 };
 
 }  // namespace joint_trajectory_controller

joint_trajectory_controller/include/joint_trajectory_controller/tolerances.hpp

@@ -77,8 +77,8 @@ struct SegmentTolerances
 /**
  * \brief Populate trajectory segment tolerances using data from the ROS node.
  *
- * It is assumed that the following parameter structure is followed on the provided LifecycleNode. Unspecified parameters
- * will take the defaults shown in the comments:
+ * It is assumed that the following parameter structure is followed on the provided LifecycleNode.
+ * Unspecified parameters will take the defaults shown in the comments:
  *
  * \code
  * constraints: