Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

[JTC] Use time of the last command for set_point_before_trajectory_msg in open-loop mode #780

Merged
merged 8 commits into from
Feb 14, 2025
Merged

[JTC] Use time of the last command for set_point_before_trajectory_msg in open-loop mode #780

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
8 commits
Select commit Hold shift + click to select a range
9f82548
Reset initial value of last commanded time to 0.
destogl Sep 26, 2023
8fc35dc
Fix stair step in JTC when new trajectory message received
bijoua29 Sep 25, 2023
0a5a98d
Merge branch 'master' into jtc-smoothen-interplation-in-open-loop
christophfroehlich Nov 2, 2024
1fe9430
Merge branch 'master' into jtc-smoothen-interplation-in-open-loop
christophfroehlich Feb 12, 2025
073d4d9
Use correct time in the update call
christophfroehlich Feb 12, 2025
9fbe571
Merge branch 'master' into jtc-smoothen-interplation-in-open-loop
christophfroehlich Feb 13, 2025
427e729
Merge branch 'master' into jtc-smoothen-interplation-in-open-loop
christophfroehlich Feb 13, 2025
d2ca6aa
Merge branch 'master' into jtc-smoothen-interplation-in-open-loop
christophfroehlich Feb 14, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
1 change: 1 addition & 0 deletions ...trajectory_controller/include/joint_trajectory_controller/joint_trajectory_controller.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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};
Expand Down
8 changes: 7 additions & 1 deletion joint_trajectory_controller/src/joint_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
{
Expand Down Expand Up @@ -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)
Expand Down Expand Up @@ -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());
Expand Down
14 changes: 7 additions & 7 deletions joint_trajectory_controller/test/test_trajectory_controller.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand All @@ -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
Expand All @@ -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
Expand Down Expand Up @@ -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)
Expand All @@ -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
Expand All @@ -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
Expand Down