[JTC] Fix state offset tests

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -261,8 +261,14 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
     const rclcpp::Time & time, const JointTrajectoryPoint & desired_state,
     const JointTrajectoryPoint & current_state, const JointTrajectoryPoint & state_error);
 
-  void read_state_from_hardware(JointTrajectoryPoint & state);
+  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.
+   * @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);
   bool read_commands_from_command_interfaces(JointTrajectoryPoint & commands);
 

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -175,7 +175,7 @@ controller_interface::return_type JointTrajectoryController::update(
 
   // current state update
   state_current_.time_from_start.set__sec(0);
-  read_state_from_hardware(state_current_);
+  read_state_from_state_interfaces(state_current_);
 
   // currently carrying out a trajectory
   if (has_active_trajectory())
@@ -397,7 +397,7 @@ controller_interface::return_type JointTrajectoryController::update(
   return controller_interface::return_type::OK;
 }
 
-void JointTrajectoryController::read_state_from_hardware(JointTrajectoryPoint & state)
+void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectoryPoint & state)
 {
   auto assign_point_from_interface =
     [&](std::vector<double> & trajectory_point_interface, const auto & joint_interface)
@@ -466,6 +466,7 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
     state.positions.clear();
     has_values = false;
   }
+
   // velocity and acceleration states are optional
   if (has_velocity_state_interface_)
   {
@@ -951,20 +952,21 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
 
   subscriber_is_active_ = true;
 
-  // Initialize current state storage if hardware state has tracking offset
-  read_state_from_hardware(state_current_);
-  read_state_from_hardware(state_desired_);
-  read_state_from_hardware(last_commanded_state_);
-  // Handle restart of controller by reading from commands if
-  // those are not nan
+  // 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_);
   if (read_state_from_command_interfaces(state))
   {
     state_current_ = state;
-    state_desired_ = state;
     last_commanded_state_ = state;
   }
+  else
+  {
+    // Initialize current state storage from hardware
+    read_state_from_state_interfaces(state_current_);
+    read_state_from_state_interfaces(last_commanded_state_);
+  }
 
   // Should the controller start by holding position at the beginning of active state?
   if (params_.start_with_holding)

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -1668,21 +1668,23 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
 #endif
 
 // Testing that values are read from state interfaces when hardware is started for the first
-// time and hardware state has offset --> this is indicated by NaN values in state interfaces
+// time and hardware state has offset --> this is indicated by NaN values in command interfaces
 TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_controller_start)
 {
   rclcpp::executors::SingleThreadedExecutor executor;
-  // default if false so it will not be actually set parameter
   rclcpp::Parameter is_open_loop_parameters("open_loop_control", true);
 
   // set command values to NaN
-  for (size_t i = 0; i < 3; ++i)
-  {
-    joint_pos_[i] = std::numeric_limits<double>::quiet_NaN();
-    joint_vel_[i] = std::numeric_limits<double>::quiet_NaN();
-    joint_acc_[i] = std::numeric_limits<double>::quiet_NaN();
-  }
-  SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);
+  std::vector<double> initial_pos_cmd{3, std::numeric_limits<double>::quiet_NaN()};
+  std::vector<double> initial_vel_cmd{3, std::numeric_limits<double>::quiet_NaN()};
+  std::vector<double> initial_acc_cmd{3, std::numeric_limits<double>::quiet_NaN()};
+
+  SetUpAndActivateTrajectoryController(
+    executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd,
+    initial_acc_cmd);
+
+  // no call of update method, so the values should be read from state interfaces
+  // (command interface are NaN)
 
   auto current_state_when_offset = traj_controller_->get_current_state_when_offset();
 
@@ -1691,70 +1693,96 @@ TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_first_co
     EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
 
     // check velocity
-    if (
-      std::find(
-        state_interface_types_.begin(), state_interface_types_.end(),
-        hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() &&
-      traj_controller_->has_velocity_command_interface())
+    if (traj_controller_->has_velocity_state_interface())
     {
-      EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
+      EXPECT_EQ(current_state_when_offset.velocities[i], joint_state_vel_[i]);
     }
 
     // check acceleration
-    if (
-      std::find(
-        state_interface_types_.begin(), state_interface_types_.end(),
-        hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() &&
-      traj_controller_->has_acceleration_command_interface())
+    if (traj_controller_->has_acceleration_state_interface())
     {
-      EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
+      EXPECT_EQ(current_state_when_offset.accelerations[i], joint_state_acc_[i]);
     }
   }
 
   executor.cancel();
 }
 
-// Testing that values are read from state interfaces when hardware is started after some values
+// Testing that values are read from command interfaces when hardware is started after some values
 // are set on the hardware commands
 TEST_P(TrajectoryControllerTestParameterized, test_hw_states_has_offset_later_controller_start)
 {
   rclcpp::executors::SingleThreadedExecutor executor;
-  // default if false so it will not be actually set parameter
   rclcpp::Parameter is_open_loop_parameters("open_loop_control", true);
 
-  // set command values to NaN
+  // set command values to arbitrary values
+  std::vector<double> initial_pos_cmd, initial_vel_cmd, initial_acc_cmd;
   for (size_t i = 0; i < 3; ++i)
   {
-    joint_pos_[i] = 3.1 + static_cast<double>(i);
-    joint_vel_[i] = 0.25 + static_cast<double>(i);
-    joint_acc_[i] = 0.02 + static_cast<double>(i) / 10.0;
+    initial_pos_cmd.push_back(3.1 + static_cast<double>(i));
+    initial_vel_cmd.push_back(0.25 + static_cast<double>(i));
+    initial_acc_cmd.push_back(0.02 + static_cast<double>(i) / 10.0);
   }
-  SetUpAndActivateTrajectoryController(executor, {is_open_loop_parameters}, true);
+  SetUpAndActivateTrajectoryController(
+    executor, {is_open_loop_parameters}, true, 0., 1., false, initial_pos_cmd, initial_vel_cmd,
+    initial_acc_cmd);
+
+  // no call of update method, so the values should be read from command interfaces
 
   auto current_state_when_offset = traj_controller_->get_current_state_when_offset();
 
   for (size_t i = 0; i < 3; ++i)
   {
-    EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]);
-
-    // check velocity
-    if (
-      std::find(
-        state_interface_types_.begin(), state_interface_types_.end(),
-        hardware_interface::HW_IF_VELOCITY) != state_interface_types_.end() &&
-      traj_controller_->has_velocity_command_interface())
+    // check position
+    if (traj_controller_->has_position_command_interface())
     {
-      EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]);
+      // 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]);
+      }
     }
-
-    // check acceleration
-    if (
-      std::find(
-        state_interface_types_.begin(), state_interface_types_.end(),
-        hardware_interface::HW_IF_ACCELERATION) != state_interface_types_.end() &&
-      traj_controller_->has_acceleration_command_interface())
+    else
     {
-      EXPECT_EQ(current_state_when_offset.positions[i], joint_pos_[i]);
+      // should have set it to the state interface instead
+      EXPECT_EQ(current_state_when_offset.positions[i], joint_state_pos_[i]);
     }
   }
 

joint_trajectory_controller/test/test_trajectory_controller_utils.hpp

@@ -220,7 +220,11 @@ class TrajectoryControllerTest : public ::testing::Test
   void SetUpAndActivateTrajectoryController(
     rclcpp::Executor & executor, const std::vector<rclcpp::Parameter> & parameters = {},
     bool separate_cmd_and_state_values = false, double k_p = 0.0, double ff = 1.0,
-    bool angle_wraparound = false)
+    bool angle_wraparound = false,
+    const std::vector<double> initial_pos_joints = INITIAL_POS_JOINTS,
+    const std::vector<double> initial_vel_joints = INITIAL_VEL_JOINTS,
+    const std::vector<double> initial_acc_joints = INITIAL_ACC_JOINTS,
+    const std::vector<double> initial_eff_joints = INITIAL_EFF_JOINTS)
   {
     SetUpTrajectoryController(executor);
 
@@ -239,12 +243,17 @@ class TrajectoryControllerTest : public ::testing::Test
 
     traj_controller_->get_node()->configure();
 
-    ActivateTrajectoryController(separate_cmd_and_state_values);
+    ActivateTrajectoryController(
+      separate_cmd_and_state_values, initial_pos_joints, initial_vel_joints, initial_acc_joints,
+      initial_eff_joints);
   }
 
   void ActivateTrajectoryController(
     bool separate_cmd_and_state_values = false,
-    const std::vector<double> initial_pos_joints = INITIAL_POS_JOINTS)
+    const std::vector<double> initial_pos_joints = INITIAL_POS_JOINTS,
+    const std::vector<double> initial_vel_joints = INITIAL_VEL_JOINTS,
+    const std::vector<double> initial_acc_joints = INITIAL_ACC_JOINTS,
+    const std::vector<double> initial_eff_joints = INITIAL_EFF_JOINTS)
   {
     std::vector<hardware_interface::LoanedCommandInterface> cmd_interfaces;
     std::vector<hardware_interface::LoanedStateInterface> state_interfaces;
@@ -280,14 +289,17 @@ class TrajectoryControllerTest : public ::testing::Test
       cmd_interfaces.emplace_back(pos_cmd_interfaces_.back());
       cmd_interfaces.back().set_value(initial_pos_joints[i]);
       cmd_interfaces.emplace_back(vel_cmd_interfaces_.back());
-      cmd_interfaces.back().set_value(INITIAL_VEL_JOINTS[i]);
+      cmd_interfaces.back().set_value(initial_vel_joints[i]);
       cmd_interfaces.emplace_back(acc_cmd_interfaces_.back());
-      cmd_interfaces.back().set_value(INITIAL_ACC_JOINTS[i]);
+      cmd_interfaces.back().set_value(initial_acc_joints[i]);
       cmd_interfaces.emplace_back(eff_cmd_interfaces_.back());
-      cmd_interfaces.back().set_value(INITIAL_EFF_JOINTS[i]);
-      joint_state_pos_[i] = initial_pos_joints[i];
-      joint_state_vel_[i] = INITIAL_VEL_JOINTS[i];
-      joint_state_acc_[i] = INITIAL_ACC_JOINTS[i];
+      cmd_interfaces.back().set_value(initial_eff_joints[i]);
+      if (separate_cmd_and_state_values)
+      {
+        joint_state_pos_[i] = INITIAL_POS_JOINTS[i];
+        joint_state_vel_[i] = INITIAL_VEL_JOINTS[i];
+        joint_state_acc_[i] = INITIAL_ACC_JOINTS[i];
+      }
       state_interfaces.emplace_back(pos_state_interfaces_.back());
       state_interfaces.emplace_back(vel_state_interfaces_.back());
       state_interfaces.emplace_back(acc_state_interfaces_.back());
@@ -489,27 +501,33 @@ class TrajectoryControllerTest : public ::testing::Test
       // --> set kp > 0.0 in test
       if (traj_controller_->has_velocity_command_interface())
       {
-        EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_vel_[0]))
-          << "current error: " << point.at(0) - joint_state_pos_[0] << ", velocity command is "
-          << joint_vel_[0];
-        EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_vel_[1]))
-          << "current error: " << point.at(1) - joint_state_pos_[1] << ", velocity command is "
-          << joint_vel_[1];
-        EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_vel_[2]))
-          << "current error: " << point.at(2) - joint_state_pos_[2] << ", velocity command is "
-          << joint_vel_[2];
+        EXPECT_TRUE(
+          is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_vel_[0]))
+          << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value()
+          << ", velocity command is " << joint_vel_[0];
+        EXPECT_TRUE(
+          is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_vel_[1]))
+          << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value()
+          << ", velocity command is " << joint_vel_[1];
+        EXPECT_TRUE(
+          is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_vel_[2]))
+          << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value()
+          << ", velocity command is " << joint_vel_[2];
       }
       if (traj_controller_->has_effort_command_interface())
       {
-        EXPECT_TRUE(is_same_sign_or_zero(point.at(0) - joint_state_pos_[0], joint_eff_[0]))
-          << "current error: " << point.at(0) - joint_state_pos_[0] << ", effort command is "
-          << joint_eff_[0];
-        EXPECT_TRUE(is_same_sign_or_zero(point.at(1) - joint_state_pos_[1], joint_eff_[1]))
-          << "current error: " << point.at(1) - joint_state_pos_[1] << ", effort command is "
-          << joint_eff_[1];
-        EXPECT_TRUE(is_same_sign_or_zero(point.at(2) - joint_state_pos_[2], joint_eff_[2]))
-          << "current error: " << point.at(2) - joint_state_pos_[2] << ", effort command is "
-          << joint_eff_[2];
+        EXPECT_TRUE(
+          is_same_sign_or_zero(point.at(0) - pos_state_interfaces_[0].get_value(), joint_eff_[0]))
+          << "current error: " << point.at(0) - pos_state_interfaces_[0].get_value()
+          << ", effort command is " << joint_eff_[0];
+        EXPECT_TRUE(
+          is_same_sign_or_zero(point.at(1) - pos_state_interfaces_[1].get_value(), joint_eff_[1]))
+          << "current error: " << point.at(1) - pos_state_interfaces_[1].get_value()
+          << ", effort command is " << joint_eff_[1];
+        EXPECT_TRUE(
+          is_same_sign_or_zero(point.at(2) - pos_state_interfaces_[2].get_value(), joint_eff_[2]))
+          << "current error: " << point.at(2) - pos_state_interfaces_[2].get_value()
+          << ", effort command is " << joint_eff_[2];
       }
     }
   }