Merge remote-tracking branch 'origin/master' into jtc/controller_plugin

Author: christophfroehlich

diff_drive_controller/src/diff_drive_controller.cpp

@@ -167,9 +167,19 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
     double right_feedback_mean = 0.0;
     for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
     {
-      const double left_feedback = registered_left_wheel_handles_[index].feedback.get().get_value();
-      const double right_feedback =
-        registered_right_wheel_handles_[index].feedback.get().get_value();
+      const auto left_feedback_op =
+        registered_left_wheel_handles_[index].feedback.get().get_optional();
+      const auto right_feedback_op =
+        registered_right_wheel_handles_[index].feedback.get().get_optional();
+
+      if (!left_feedback_op.has_value() || !right_feedback_op.has_value())
+      {
+        RCLCPP_DEBUG(logger, "Unable to retrieve the data from the left or right wheels feedback!");
+        return controller_interface::return_type::OK;
+      }
+
+      const double left_feedback = left_feedback_op.value();
+      const double right_feedback = right_feedback_op.value();
 
       if (std::isnan(left_feedback) || std::isnan(right_feedback))
       {
@@ -278,12 +288,18 @@ controller_interface::return_type DiffDriveController::update_and_write_commands
     (linear_command + angular_command * wheel_separation / 2.0) / right_wheel_radius;
 
   // Set wheels velocities:
+  bool set_command_result = true;
   for (size_t index = 0; index < static_cast<size_t>(wheels_per_side_); ++index)
   {
-    registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left);
-    registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right);
+    set_command_result &=
+      registered_left_wheel_handles_[index].velocity.get().set_value(velocity_left);
+    set_command_result &=
+      registered_right_wheel_handles_[index].velocity.get().set_value(velocity_right);
   }
 
+  RCLCPP_DEBUG_EXPRESSION(
+    logger, !set_command_result, "Unable to set the command to one of the command handles!");
+
   return controller_interface::return_type::OK;
 }
 

diff_drive_controller/test/test_diff_drive_controller.cpp

@@ -20,3 +20,4 @@ joint_trajectory_controller
 * Angle wraparound behavior (continuous joints) was added from the current state to the first segment of the incoming trajectory (`#796 <https://github.com/ros-controls/ros2_controllers/pull/796>`_).
 * The URDF is now parsed for continuous joints and angle wraparound is automatically activated now (`#949 <https://github.com/ros-controls/ros2_controllers/pull/949>`_). Remove the ``angle_wraparound`` parameter from the configuration and set continuous joint type in the URDF of the respective joint.
 * Tolerances sent with the action goal were not used before, but are now processed and used for the upcoming action. (`#716 <https://github.com/ros-controls/ros2_controllers/pull/716>`_). Adaptions to the action goal might be necessary.
+* Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 <https://github.com/ros-controls/ros2_controllers/pull/1525>`_).

doc/migration.rst

@@ -52,6 +52,8 @@ joint_trajectory_controller
             allowed to move without restriction.
 
 * Add the boolean parameter ``set_last_command_interface_value_as_state_on_activation``. When set to ``true``, the last command interface value is used as both the current state and the last commanded state upon activation. When set to ``false``, the current state is used for both (`#1231 <https://github.com/ros-controls/ros2_controllers/pull/1231>`_).
+* Feed-forward effort trajectories are supported now (`#1200 <https://github.com/ros-controls/ros2_controllers/pull/1200>`_).
+* Parameter ``open_loop_control`` is replaced by ``interpolate_from_desired_state`` and setting the feedback gains to zero (`#1525 <https://github.com/ros-controls/ros2_controllers/pull/1525>`_).
 
 mecanum_drive_controller
 ************************

doc/release_notes.rst

@@ -85,12 +85,13 @@ TEST_F(JointGroupEffortControllerTest, ActivateWithWrongJointsNamesFails)
   // activate failed, 'joint4' is not a valid joint name for the hardware
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
   ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
+  ASSERT_EQ(controller_->on_cleanup(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 
   controller_->get_node()->set_parameter({"joints", std::vector<std::string>{"joint1", "joint2"}});
 
-  // activate failed, 'acceleration' is not a registered interface for `joint1`
+  // activate should succeed now
   ASSERT_EQ(controller_->on_configure(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
-  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::ERROR);
+  ASSERT_EQ(controller_->on_activate(rclcpp_lifecycle::State()), CallbackReturn::SUCCESS);
 }
 
 TEST_F(JointGroupEffortControllerTest, CommandSuccessTest)

effort_controllers/test/test_joint_group_effort_controller.cpp

@@ -45,6 +45,7 @@ controller_interface::CallbackReturn ForwardCommandController::read_parameters()
     return controller_interface::CallbackReturn::ERROR;
   }
 
+  command_interface_types_.clear();
   for (const auto & joint : params_.joints)
   {
     command_interface_types_.push_back(joint + "/" + params_.interface_name);

forward_command_controller/src/forward_command_controller.cpp

@@ -74,6 +74,7 @@ if(BUILD_TESTING)
   ament_target_dependencies(test_trajectory_controller ros2_control_test_assets)
   target_compile_definitions(joint_trajectory_controller PRIVATE _USE_MATH_DEFINES)
 
+  add_definitions(-DTEST_FILES_DIRECTORY="${CMAKE_CURRENT_SOURCE_DIR}/test")
   ament_add_gmock(test_load_joint_trajectory_controller
     test/test_load_joint_trajectory_controller.cpp
   )

joint_trajectory_controller/CMakeLists.txt

@@ -2,8 +2,6 @@ constraints:
     Default values for tolerances if no explicit values are set in the ``JointTrajectory`` message.
 
 gains: |
-  Only relevant, if ``open_loop_control`` is not set.
-
   If ``velocity`` is the only command interface for all joints or an ``effort`` command interface is configured, PID controllers are used for every joint.
   This structure contains the controller gains for every joint with the control law
 
@@ -16,3 +14,5 @@ gains: |
   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.
+
+  If you want to turn off the PIDs (open loop control), set the feedback gains to zero.

joint_trajectory_controller/doc/parameters_context.yaml

@@ -49,6 +49,8 @@ The spline interpolator uses the following interpolation strategies depending on
 
 Trajectories with velocity fields only, velocity and acceleration only, or acceleration fields only can be processed and are accepted, if ``allow_integration_in_goal_trajectories`` is true. Position (and velocity) is then integrated from velocity (or acceleration, respectively) by Heun's method.
 
+Effort trajectories are allowed for controllers that claim the ``effort`` command interface and they are treated as feed-forward effort that is added to the position feedback. Effort is handled separately from position, velocity and acceleration. We use linear interpolation for effort when the ``spline`` interpolation method is selected.
+
 Visualized Examples
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 To visualize the difference of the different interpolation methods and their inputs, different trajectories defined at a 0.5s grid and are sampled at a rate of 10ms.

joint_trajectory_controller/doc/trajectory.rst

@@ -24,12 +24,15 @@ Currently, joints with hardware interface types ``position``, ``velocity``, ``ac
 * ``position``, ``velocity``, ``acceleration``
 * ``velocity``
 * ``effort``
+* ``position``, ``effort``
 
 This means that the joints can have one or more command interfaces, where the following control laws are applied at the same time:
 
 * For command interfaces ``position``, the desired positions are simply forwarded to the joints,
 * For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints.
 * For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop if it is configured (:ref:`parameters`).
+* For ``effort`` command interface (without ``position`` command interface), if the trajectory contains effort, this will be added to the PID commands as a feed forward effort.
+* For ``position, effort`` command interface, if the trajectory contains effort, this will be passed directly to the ``effort`` interface (PID won't be used) while the positions will be passed to the ``position`` interface.
 
 This leads to the following allowed combinations of command and state interfaces:
 
@@ -38,7 +41,7 @@ This leads to the following allowed combinations of command and state interfaces
 
   * if command interface ``velocity`` is the only one, state interfaces must include  ``position, velocity`` .
 
-* With command interface ``effort``, state interfaces must include  ``position, velocity``.
+* With command interface ``effort`` or ``position, effort``, state interfaces must include  ``position, velocity``.
 
 * With command interface ``acceleration``, state interfaces must include  ``position, velocity``.
 
@@ -97,7 +100,7 @@ A yaml file for using it could be:
           action_monitor_rate: 20.0
 
           allow_partial_joints_goal: false
-          open_loop_control: true
+          interpolate_from_desired_state: true
           constraints:
             stopped_velocity_tolerance: 0.01
             goal_time: 0.0