[JTC] Use time of the last command for set_point_before_trajectory_ms…

…g in open-loop mode (#780)

Co-authored-by: bijoua <bijou.abraham@technipfmc.com>
Co-authored-by: Christoph Froehlich <christoph.froehlich@ait.ac.at>

joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp

@@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
   rclcpp::Time traj_time_;
 
   trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
+  rclcpp::Time last_commanded_time_;
   /// Specify interpolation method. Default to splines.
   interpolation_methods::InterpolationMethod interpolation_method_{
     interpolation_methods::DEFAULT_INTERPOLATION};

joint_trajectory_controller/src/joint_trajectory_controller.cpp

@@ -190,8 +190,12 @@ controller_interface::return_type JointTrajectoryController::update(
       first_sample = true;
       if (params_.open_loop_control)
       {
+        if (last_commanded_time_.seconds() == 0.0)
+        {
+          last_commanded_time_ = time;
+        }
         traj_external_point_ptr_->set_point_before_trajectory_msg(
-          time, last_commanded_state_, joints_angle_wraparound_);
+          last_commanded_time_, last_commanded_state_, joints_angle_wraparound_);
       }
       else
       {
@@ -322,6 +326,7 @@ controller_interface::return_type JointTrajectoryController::update(
 
         // store the previous command. Used in open-loop control mode
         last_commanded_state_ = command_next_;
+        last_commanded_time_ = time;
       }
 
       if (active_goal)
@@ -950,6 +955,7 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
     read_state_from_state_interfaces(state_current_);
     read_state_from_state_interfaces(last_commanded_state_);
   }
+  last_commanded_time_ = rclcpp::Time();
 
   // The controller should start by holding position at the beginning of active state
   add_new_trajectory_msg(set_hold_position());

joint_trajectory_controller/test/test_trajectory_controller.cpp

@@ -1679,9 +1679,9 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
   publish(
     time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME), {}, first_goal_velocities);
   traj_controller_->wait_for_trajectory(executor);
-  updateControllerAsync(rclcpp::Duration::from_seconds(1.1));
+  auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(1.1));
 
-  // JTC is executing trajectory in open-loop therefore:
+  // JTC is NOT executing trajectory in open-loop therefore:
   // - internal state does not have to be updated (in this test-case it shouldn't)
   // - internal command is updated
   EXPECT_NEAR(INITIAL_POS_JOINT1, joint_state_pos_[0], COMMON_THRESHOLD);
@@ -1698,7 +1698,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
   // One the first update(s) there should be a "jump" in opposite direction from command
   // (towards the state value)
   EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD);
-  auto end_time = updateControllerAsync(controller_period);
+  end_time = updateControllerAsync(controller_period, end_time);
   // Expect backward commands at first, consider advancement of the trajectory
   // exact value is not directly predictable, because of the spline interpolation -> increase
   // tolerance
@@ -1715,7 +1715,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_jump_when_state_tracking_erro
   EXPECT_LT(joint_pos_[0], first_goal[0]);
 
   // Finally the second goal will be commanded/reached
-  updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time);
+  end_time = updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time);
   EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD);
 
   // State interface should have offset from the command before starting a new trajectory
@@ -1783,7 +1783,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
   std::vector<std::vector<double>> points{{first_goal}};
   publish(time_from_start, points, rclcpp::Time(0.0, 0.0, RCL_STEADY_TIME));
   traj_controller_->wait_for_trajectory(executor);
-  updateControllerAsync(rclcpp::Duration::from_seconds(1.1));
+  auto end_time = updateControllerAsync(rclcpp::Duration::from_seconds(1.1));
 
   // JTC is executing trajectory in open-loop therefore:
   // - internal state does not have to be updated (in this test-case it shouldn't)
@@ -1802,7 +1802,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
   // One the first update(s) there **should not** be a "jump" in opposite direction from
   // command (towards the state value)
   EXPECT_NEAR(first_goal[0], joint_pos_[0], COMMON_THRESHOLD);
-  auto end_time = updateControllerAsync(controller_period);
+  end_time = updateControllerAsync(controller_period, end_time);
   // There should not be backward commands
   // exact value is not directly predictable, because of the spline interpolation -> increase
   // tolerance
@@ -1818,7 +1818,7 @@ TEST_P(TrajectoryControllerTestParameterized, test_no_jump_when_state_tracking_e
   EXPECT_LT(joint_pos_[0], second_goal[0]);
 
   // Finally the second goal will be commanded/reached
-  updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time);
+  end_time = updateControllerAsync(rclcpp::Duration::from_seconds(1.1), end_time);
   EXPECT_NEAR(second_goal[0], joint_pos_[0], COMMON_THRESHOLD);
 
   // State interface should have offset from the command before starting a new trajectory