diff --git a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp b/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp index af0f35be18..2176993828 100644 --- a/joint_trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp +++ b/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}; diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 12b39120d8..069434c303 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/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()); diff --git a/joint_trajectory_controller/test/test_trajectory_controller.cpp b/joint_trajectory_controller/test/test_trajectory_controller.cpp index 0969866422..e110e244b7 100644 --- a/joint_trajectory_controller/test/test_trajectory_controller.cpp +++ b/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> 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