More general initialization of state from command

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)
 {
@@ -595,27 +607,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
@@ -648,101 +639,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(
@@ -767,17 +698,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_)
   {
@@ -1276,28 +1196,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)
+    // Thue 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_)
   {
@@ -2161,6 +2073,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/test/test_trajectory_controller.cpp

@@ -2390,57 +2390,33 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_co
 
   for (size_t i = 0; i < 3; ++i)
   {
-    // check position
     if (traj_controller_->has_position_command_interface())
     {
-      // check velocity
-      if (traj_controller_->has_velocity_state_interface())
-      {
-        if (traj_controller_->has_velocity_command_interface())
-        {
-          // check acceleration
-          if (traj_controller_->has_acceleration_state_interface())
-          {
-            if (traj_controller_->has_acceleration_command_interface())
-            {
-              // should have set it to last position + velocity + acceleration command
-              EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]);
-              EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]);
-              EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]);
-            }
-            else
-            {
-              // should have set it to the state interface instead
-              EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
-              EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]);
-              EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]);
-            }
-          }
-          else
-          {
-            // should have set it to last position + velocity command
-            EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]);
-            EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]);
-          }
-        }
-        else
-        {
-          // should have set it to the state interface instead
-          EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
-          EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]);
-        }
-      }
-      else
-      {
-        // should have set it to last position command
-        EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]);
-      }
+      EXPECT_EQ(current_state_when_offset.positions[i], initial_pos_cmd[i]);
     }
     else
     {
       // should have set it to the state interface instead
       EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
     }
+
+    if (traj_controller_->has_velocity_command_interface())
+    {
+      EXPECT_EQ(current_state_when_offset.velocities[i], initial_vel_cmd[i]);
+    }
+    else if (traj_controller_->has_velocity_state_interface())
+    {
+      EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]);
+    }
+
+    if (traj_controller_->has_acceleration_command_interface())
+    {
+      EXPECT_EQ(current_state_when_offset.accelerations[i], initial_acc_cmd[i]);
+    }
+    else if (traj_controller_->has_acceleration_state_interface())
+    {
+      EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]);
+    }
   }
 
   executor.cancel();