More general initialization of state from command

doc/release_notes.rst

@@ -40,6 +40,8 @@ omni_wheel_drive_controller
 
 joint_trajectory_controller
 ***************************
+* When using ``set_last_command_interface_value_as_state_on_activation``, it is no longer required to have state and command for the same interface type (e.g. velocity). With this param set, the JTC state and command will be initialized using a command interface value, if available, and will otherwise fallback to the value read from the state interface. This allows you to have position command and position+velocity state, for example, which previously would have been disallowed (with this param set).  (`#2294
+  <https://github.com/ros-controls/ros2_controllers/pull/2294>`_)
 * Fill in 0 velocities and accelerations into point before trajectories if the state interfaces
   don't contain velocity / acceleration information, but the trajectory does. This way, the segment
   up to the first waypoint will use the same interpolation as the rest of the trajectory. (`#2043

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -273,12 +273,11 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   void read_state_from_state_interfaces(JointTrajectoryPoint & state);
 
   /** Assign values from the command interfaces as state.
-   * This is only possible if command AND state interfaces exist for the same type,
-   *  therefore needs check for both.
+   * state values (e.g. velocity, acceleration, or effort) which do not have command interfaces will
+   * NOT be updated.
    * @param[out] state to be filled with values from command interfaces.
-   * @return true if all interfaces exists and contain non-NaN values, false otherwise.
    */
-  bool read_state_from_command_interfaces(JointTrajectoryPoint & state);
+  void update_state_from_command_interfaces(JointTrajectoryPoint & state);
   bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands);
 
   void query_state_service(
@@ -296,6 +295,10 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0);
   void resize_joint_trajectory_point_command(
     trajectory_msgs::msg::JointTrajectoryPoint & point, size_t size, double value = 0.0);
+  void assign_point_from_command_interface(
+    std::vector<double> & trajectory_point_interface,
+    const std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> &
+      joint_interface);
 
   /**
    * @brief Set scaling factor used for speed scaling trajectory execution

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -43,6 +43,18 @@
 
 namespace joint_trajectory_controller
 {
+
+auto interface_has_values = [](const auto & joint_interface)
+{
+  return std::find_if(
+           joint_interface.begin(), joint_interface.end(),
+           [](const auto & interface)
+           {
+             auto interface_op = interface.get().get_optional();
+             return !interface_op.has_value() || std::isnan(interface_op.value());
+           }) == joint_interface.end();
+};
+
 JointTrajectoryController::JointTrajectoryController()
 : controller_interface::ControllerInterface(), dof_(0), num_cmd_joints_(0)
 {
@@ -598,27 +610,6 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory
       }
     }
   };
-  auto assign_point_from_command_interface =
-    [&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
-  {
-    std::fill(
-      trajectory_point_interface.begin(), trajectory_point_interface.end(),
-      std::numeric_limits<double>::quiet_NaN());
-    for (size_t index = 0; index < num_cmd_joints_; ++index)
-    {
-      const auto joint_command_interface_value_op = joint_interface[index].get().get_optional();
-      if (!joint_command_interface_value_op.has_value())
-      {
-        RCLCPP_DEBUG(
-          logger, "Unable to retrieve joint command interface value for joint at index %zu", index);
-      }
-      else
-      {
-        trajectory_point_interface[map_cmd_to_joints_[index]] =
-          joint_command_interface_value_op.value();
-      }
-    }
-  };
 
   // Assign values from the hardware
   // Position states always exist
@@ -651,101 +642,41 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory
   }
 }
 
-bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state)
+void JointTrajectoryController::update_state_from_command_interfaces(JointTrajectoryPoint & state)
 {
-  bool has_values = true;
-
-  auto assign_point_from_interface =
-    [&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
-  {
-    for (size_t index = 0; index < num_cmd_joints_; ++index)
-    {
-      const auto joint_interface_value_op = joint_interface[index].get().get_optional();
-      if (!joint_interface_value_op.has_value())
-      {
-        RCLCPP_DEBUG(
-          get_node()->get_logger(),
-          "Unable to retrieve value of joint interface for joint at index %zu", index);
-      }
-      else
-      {
-        trajectory_point_interface[map_cmd_to_joints_[index]] = joint_interface_value_op.value();
-      }
-    }
-  };
-
-  auto interface_has_values = [](const auto & joint_interface)
-  {
-    return std::find_if(
-             joint_interface.begin(), joint_interface.end(),
-             [](const auto & interface)
-             {
-               auto interface_op = interface.get().get_optional();
-               return !interface_op.has_value() || std::isnan(interface_op.value());
-             }) == joint_interface.end();
-  };
-
-  // Assign values from the command interfaces as state. Therefore needs check for both.
-  // Position state interface has to exist always
+  // Assign values from the command interfaces as state
+  // Position state interface has to exist always, so no need to check for it
   if (has_position_command_interface_ && interface_has_values(joint_command_interface_[0]))
   {
-    assign_point_from_interface(state.positions, joint_command_interface_[0]);
-  }
-  else
-  {
-    state.positions.clear();
-    has_values = false;
+    assign_point_from_command_interface(state.positions, joint_command_interface_[0]);
   }
+
   // velocity and acceleration states are optional
-  if (has_velocity_state_interface_)
+  if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1]))
   {
-    if (has_velocity_command_interface_ && interface_has_values(joint_command_interface_[1]))
+    // If no state interface exists, then our velocities vector will be empty, and we must resize
+    // before assigning.
+    if (!has_velocity_state_interface_)
     {
-      assign_point_from_interface(state.velocities, joint_command_interface_[1]);
+      state.velocities.resize(dof_, std::numeric_limits<double>::quiet_NaN());
     }
-    else
-    {
-      state.velocities.clear();
-      has_values = false;
-    }
-  }
-  else
-  {
-    state.velocities.clear();
+    assign_point_from_command_interface(state.velocities, joint_command_interface_[1]);
   }
-  // Acceleration is used only in combination with velocity
-  if (has_acceleration_state_interface_)
+
+  if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2]))
   {
-    if (has_acceleration_command_interface_ && interface_has_values(joint_command_interface_[2]))
-    {
-      assign_point_from_interface(state.accelerations, joint_command_interface_[2]);
-    }
-    else
+    if (!has_acceleration_state_interface_)
     {
-      state.accelerations.clear();
-      has_values = false;
+      state.accelerations.resize(dof_, std::numeric_limits<double>::quiet_NaN());
     }
-  }
-  else
-  {
-    state.accelerations.clear();
+    assign_point_from_command_interface(state.accelerations, joint_command_interface_[2]);
   }
 
-  // Effort state always comes from last command
-  if (has_effort_command_interface_)
+  if (has_effort_command_interface_ && interface_has_values(joint_command_interface_[3]))
   {
-    if (interface_has_values(joint_command_interface_[3]))
-    {
-      assign_point_from_interface(state.effort, joint_command_interface_[3]);
-    }
-    else
-    {
-      state.effort.clear();
-      has_values = false;
-    }
+    state.effort.resize(dof_, std::numeric_limits<double>::quiet_NaN());
+    assign_point_from_command_interface(state.effort, joint_command_interface_[3]);
   }
-
-  return has_values;
 }
 
 bool JointTrajectoryController::read_commands_from_command_interfaces(
@@ -770,17 +701,6 @@ bool JointTrajectoryController::read_commands_from_command_interfaces(
     }
   };
 
-  auto interface_has_values = [](const auto & joint_interface)
-  {
-    return std::find_if(
-             joint_interface.begin(), joint_interface.end(),
-             [](const auto & interface)
-             {
-               auto interface_op = interface.get().get_optional();
-               return !interface_op.has_value() || std::isnan(interface_op.value());
-             }) == joint_interface.end();
-  };
-
   // Assign values from the command interfaces as command.
   if (has_position_command_interface_)
   {
@@ -1279,28 +1199,20 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
 
   subscriber_is_active_ = true;
 
-  // Handle restart of controller by reading from commands if those are not NaN (a controller was
-  // running already)
-  trajectory_msgs::msg::JointTrajectoryPoint state;
-  resize_joint_trajectory_point(state, dof_);
-  // read from cmd joints only if all joints have command interface
-  // otherwise it leaves the entries of joints without command interface NaN.
-  // if no interpolate_from_desired_state, state_current_ is then used for
-  // `set_point_before_trajectory_msg` and future trajectory sampling will always give NaN for these
-  // joints
-  if (
-    params_.set_last_command_interface_value_as_state_on_activation && dof_ == num_cmd_joints_ &&
-    read_state_from_command_interfaces(state))
-  {
-    state_current_ = state;
-    last_commanded_state_ = state;
-  }
-  else
+  // Initialize current state storage from hardware state interfaces
+  read_state_from_state_interfaces(state_current_);
+  read_state_from_state_interfaces(last_commanded_state_);
+
+  if (params_.set_last_command_interface_value_as_state_on_activation && dof_ == num_cmd_joints_)
   {
-    // Initialize current state storage from hardware
-    read_state_from_state_interfaces(state_current_);
-    read_state_from_state_interfaces(last_commanded_state_);
+    // Handle restart of controller by reading from commands if those are not NaN (a controller was
+    // running already)
+    // The function checks if all joints have values on the command interfaces.
+    // otherwise it will not update them, leaving them based on the state interfaces.
+    update_state_from_command_interfaces(state_current_);
+    update_state_from_command_interfaces(last_commanded_state_);
   }
+
   // reset/zero out all of the PID's (The integral term is not retained and reset to zero)
   for (auto & pid : pids_)
   {
@@ -2164,6 +2076,31 @@ void JointTrajectoryController::init_hold_position_msg()
   }
 }
 
+void JointTrajectoryController::assign_point_from_command_interface(
+  std::vector<double> & trajectory_point_interface,
+  const std::vector<std::reference_wrapper<hardware_interface::LoanedCommandInterface>> &
+    joint_interface)
+{
+  std::fill(
+    trajectory_point_interface.begin(), trajectory_point_interface.end(),
+    std::numeric_limits<double>::quiet_NaN());
+  for (size_t index = 0; index < num_cmd_joints_; ++index)
+  {
+    const auto joint_command_interface_value_op = joint_interface[index].get().get_optional();
+    if (!joint_command_interface_value_op.has_value())
+    {
+      RCLCPP_DEBUG(
+        get_node()->get_logger(),
+        "Unable to retrieve joint command interface value for joint at index %zu", index);
+    }
+    else
+    {
+      trajectory_point_interface.at(map_cmd_to_joints_[index]) =
+        joint_command_interface_value_op.value();
+    }
+  }
+}
+
 }  // namespace joint_trajectory_controller
 
 #include "pluginlib/class_list_macros.hpp"

joint_trajectory_controller/src/joint_trajectory_controller_parameters.yaml

@@ -100,7 +100,7 @@ joint_trajectory_controller:
   set_last_command_interface_value_as_state_on_activation: {
     type: bool,
     default_value: true,
-    description: "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.",
+    description: "When set to true, the last command interface value is used as both the current state and the last commanded state upon activation. If an interface (e.g. velocity) is available only as state and not command, the state is used. Use this to prevent sagging/jumps on activation when using soft or compliant hardware. When set to false, the current state is used for both.",
   }
   action_monitor_rate: {
     type: double,