Use current time step for checking tolerances

ros-controls · Nov 11, 2024 · 0d00727 · 0d00727
1 parent 6945360
commit 0d00727
Show file tree

Hide file tree

Showing 3 changed files with 36 additions and 24 deletions.
diff --git a/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
@@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   // Preallocate variables used in the realtime update() function
   trajectory_msgs::msg::JointTrajectoryPoint state_current_;
   trajectory_msgs::msg::JointTrajectoryPoint command_current_;
+  trajectory_msgs::msg::JointTrajectoryPoint command_next_;
   trajectory_msgs::msg::JointTrajectoryPoint state_desired_;
   trajectory_msgs::msg::JointTrajectoryPoint state_error_;
 

diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp
@@ -201,9 +201,13 @@ controller_interface::return_type JointTrajectoryController::update(
         time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
     }
 
+    // Sample expected state from the trajectory
+    traj_external_point_ptr_->sample(
+      time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
+
     // Sample setpoint for next control cycle
     const bool valid_point = traj_external_point_ptr_->sample(
-      time + update_period_, interpolation_method_, state_desired_, start_segment_itr,
+      time + update_period_, interpolation_method_, command_next_, start_segment_itr,
       end_segment_itr);
 
     if (valid_point)
@@ -281,7 +285,7 @@ controller_interface::return_type JointTrajectoryController::update(
           // Update PIDs
           for (auto i = 0ul; i < dof_; ++i)
           {
-            tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
+            tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) +
                               pids_[i]->computeCommand(
                                 state_error_.positions[i], state_error_.velocities[i],
                                 (uint64_t)period.nanoseconds());
@@ -291,7 +295,7 @@ controller_interface::return_type JointTrajectoryController::update(
         // set values for next hardware write()
         if (has_position_command_interface_)
         {
-          assign_interface_from_point(joint_command_interface_[0], state_desired_.positions);
+          assign_interface_from_point(joint_command_interface_[0], command_next_.positions);
         }
         if (has_velocity_command_interface_)
         {
@@ -301,20 +305,20 @@ controller_interface::return_type JointTrajectoryController::update(
           }
           else
           {
-            assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities);
+            assign_interface_from_point(joint_command_interface_[1], command_next_.velocities);
           }
         }
         if (has_acceleration_command_interface_)
         {
-          assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations);
+          assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations);
         }
         if (has_effort_command_interface_)
         {
           assign_interface_from_point(joint_command_interface_[3], tmp_command_);
         }
 
         // store the previous command. Used in open-loop control mode
-        last_commanded_state_ = state_desired_;
+        last_commanded_state_ = command_next_;
       }
 
       if (active_goal)

diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp
@@ -659,9 +659,20 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
   const rclcpp::Duration controller_period =
     rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate());
 
-  updateControllerAsync(
+  auto end_time = updateControllerAsync(
     rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME),
     controller_period);
+  if (traj_controller_->has_position_command_interface())
+  {
+    // check command interface
+    // One step before the first point, the target should hit the setpoint
+    EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD);
+    EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD);
+    EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD);
+  }
+
+  // Propagate to actual setpoint time
+  traj_controller_->update(end_time + controller_period, controller_period);
 
   // get states from class variables
   auto state_feedback = traj_controller_->get_state_feedback();
@@ -686,14 +697,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_not_angle_wraparoun
   EXPECT_NEAR(state_error.positions[1], state_reference.positions[1] - INITIAL_POS_JOINTS[1], EPS);
   EXPECT_NEAR(state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2], EPS);
 
-  if (traj_controller_->has_position_command_interface())
-  {
-    // check command interface
-    EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD);
-    EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD);
-    EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD);
-  }
-
   if (traj_controller_->has_velocity_command_interface())
   {
     // use_closed_loop_pid_adapter_
@@ -766,10 +769,22 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
 
   const rclcpp::Duration controller_period =
     rclcpp::Duration::from_seconds(1.0 / traj_controller_->get_update_rate());
-  updateControllerAsync(
+  auto end_time = updateControllerAsync(
     rclcpp::Duration(FIRST_POINT_TIME) - controller_period, rclcpp::Time(0, 0, RCL_STEADY_TIME),
     controller_period);
 
+  if (traj_controller_->has_position_command_interface())
+  {
+    // check command interface
+    // One step before the first point, the target should hit the setpoint
+    EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD);
+    EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD);
+    EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD);
+  }
+
+  // Propagate to actual setpoint time
+  traj_controller_->update(end_time + controller_period, controller_period);
+
   // get states from class variables
   auto state_feedback = traj_controller_->get_state_feedback();
   auto state_reference = traj_controller_->get_state_reference();
@@ -794,14 +809,6 @@ TEST_P(TrajectoryControllerTestParameterized, position_error_angle_wraparound)
   EXPECT_NEAR(
     state_error.positions[2], state_reference.positions[2] - INITIAL_POS_JOINTS[2] - 2 * M_PI, EPS);
 
-  if (traj_controller_->has_position_command_interface())
-  {
-    // check command interface
-    EXPECT_NEAR(points[0][0], joint_pos_[0], COMMON_THRESHOLD);
-    EXPECT_NEAR(points[0][1], joint_pos_[1], COMMON_THRESHOLD);
-    EXPECT_NEAR(points[0][2], joint_pos_[2], COMMON_THRESHOLD);
-  }
-
   if (traj_controller_->has_velocity_command_interface())
   {
     // use_closed_loop_pid_adapter_