From 0aa4a5683bee35a32a4c4ed1f6c3aff22c88b079 Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Sun, 13 Oct 2024 22:26:25 +0200 Subject: [PATCH 1/3] Sample trajectory based on the sum of periods instead of the absolute time --- .../joint_trajectory_controller.hpp | 2 ++ .../src/joint_trajectory_controller.cpp | 11 ++++++++--- 2 files changed, 10 insertions(+), 3 deletions(-) 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 67954b8378..2377b2c0b6 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 @@ -127,6 +127,8 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa Params params_; rclcpp::Duration update_period_{0, 0}; + rclcpp::Time traj_time_; + trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_; /// Specify interpolation method. Default to splines. interpolation_methods::InterpolationMethod interpolation_method_{ diff --git a/joint_trajectory_controller/src/joint_trajectory_controller.cpp b/joint_trajectory_controller/src/joint_trajectory_controller.cpp index 0fc39ecaaf..82d9c94aab 100644 --- a/joint_trajectory_controller/src/joint_trajectory_controller.cpp +++ b/joint_trajectory_controller/src/joint_trajectory_controller.cpp @@ -196,15 +196,20 @@ controller_interface::return_type JointTrajectoryController::update( traj_external_point_ptr_->set_point_before_trajectory_msg( time, state_current_, joints_angle_wraparound_); } + traj_time_ = time; + } + else + { + traj_time_ += period; } // Sample expected state from the trajectory traj_external_point_ptr_->sample( - time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr); + traj_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_, command_next_, start_segment_itr, + traj_time_ + update_period_, interpolation_method_, command_next_, start_segment_itr, end_segment_itr, false); if (valid_point) @@ -217,7 +222,7 @@ controller_interface::return_type JointTrajectoryController::update( // time_difference is // - negative until first point is reached // - counting from zero to time_from_start of next point - double time_difference = time.seconds() - segment_time_from_start.seconds(); + double time_difference = traj_time_.seconds() - segment_time_from_start.seconds(); bool tolerance_violated_while_moving = false; bool outside_goal_tolerance = false; bool within_goal_time = true; From 17adfc369961a116cfb83fcbaaad7468e469201e Mon Sep 17 00:00:00 2001 From: Felix Exner Date: Sun, 13 Oct 2024 22:25:30 +0200 Subject: [PATCH 2/3] Fix period calculation in trajectory_actions test The generated period was more of an uptime --- .../test/test_trajectory_actions.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 164f7d0e11..8f45fb66af 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -83,12 +83,16 @@ class TestTrajectoryActions : public TrajectoryControllerTest { // controller hardware cycle update loop auto clock = rclcpp::Clock(RCL_STEADY_TIME); - auto start_time = clock.now(); + auto now_time = clock.now(); + auto start_time = now_time; + auto last_time = start_time; rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); auto end_time = start_time + wait; while (clock.now() < end_time) { - traj_controller_->update(clock.now(), clock.now() - start_time); + now_time = clock.now(); + traj_controller_->update(now_time, now_time - last_time); + last_time = now_time; } }); From 9e97d9f91911a556b72ec5249df2deb303415973 Mon Sep 17 00:00:00 2001 From: "Felix Exner (fexner)" Date: Thu, 28 Nov 2024 21:50:17 +0100 Subject: [PATCH 3/3] Update joint_trajectory_controller/test/test_trajectory_actions.cpp Co-authored-by: Sai Kishor Kothakota --- joint_trajectory_controller/test/test_trajectory_actions.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/joint_trajectory_controller/test/test_trajectory_actions.cpp b/joint_trajectory_controller/test/test_trajectory_actions.cpp index 8f45fb66af..d4979deef8 100644 --- a/joint_trajectory_controller/test/test_trajectory_actions.cpp +++ b/joint_trajectory_controller/test/test_trajectory_actions.cpp @@ -84,10 +84,9 @@ class TestTrajectoryActions : public TrajectoryControllerTest // controller hardware cycle update loop auto clock = rclcpp::Clock(RCL_STEADY_TIME); auto now_time = clock.now(); - auto start_time = now_time; - auto last_time = start_time; + auto last_time = now_time; rclcpp::Duration wait = rclcpp::Duration::from_seconds(2.0); - auto end_time = start_time + wait; + auto end_time = last_time + wait; while (clock.now() < end_time) { now_time = clock.now();