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.

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] Sample at t + dT instead of the beginning of the control cycle #1306

Merged
merged 17 commits into from
Nov 22, 2024
Merged
Show file tree
Hide file tree
Changes from 5 commits
Commits
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
Original file line number Diff line number Diff line change
Expand Up @@ -124,6 +124,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
// Parameters from ROS for joint_trajectory_controller
std::shared_ptr<ParamListener> param_listener_;
Params params_;
rclcpp::Duration update_period_{0, 0};

trajectory_msgs::msg::JointTrajectoryPoint last_commanded_state_;
/// Specify interpolation method. Default to splines.
Expand Down
22 changes: 18 additions & 4 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -18,6 +18,7 @@
#include <functional>
#include <memory>

#include <stdexcept>
#include <string>
#include <vector>

Expand Down Expand Up @@ -180,6 +181,7 @@ controller_interface::return_type JointTrajectoryController::update(
if (has_active_trajectory())
{
bool first_sample = false;
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
// if sampling the first time, set the point before you sample
if (!traj_external_point_ptr_->is_sampled_already())
{
Expand All @@ -194,12 +196,15 @@ controller_interface::return_type JointTrajectoryController::update(
traj_external_point_ptr_->set_point_before_trajectory_msg(
time, state_current_, joints_angle_wraparound_);
}
traj_external_point_ptr_->sample(
// Sample once at the beginning to establish the start time if none was given
time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
fmauch marked this conversation as resolved.
Show resolved Hide resolved
}

// find segment for current timestamp
TrajectoryPointConstIter start_segment_itr, end_segment_itr;
// Sample setpoint for next control cycle
const bool valid_point = traj_external_point_ptr_->sample(
time, interpolation_method_, state_desired_, start_segment_itr, end_segment_itr);
time + update_period_, interpolation_method_, state_desired_, start_segment_itr,
end_segment_itr);
saikishor marked this conversation as resolved.
Show resolved Hide resolved

if (valid_point)
{
Expand Down Expand Up @@ -279,7 +284,7 @@ controller_interface::return_type JointTrajectoryController::update(
tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
pids_[i]->computeCommand(
state_error_.positions[i], state_error_.velocities[i],
(uint64_t)period.nanoseconds());
(uint64_t)update_period_.nanoseconds());
christophfroehlich marked this conversation as resolved.
Show resolved Hide resolved
}
}

Expand Down Expand Up @@ -961,6 +966,15 @@ controller_interface::CallbackReturn JointTrajectoryController::on_activate(
cmd_timeout_ = 0.0;
}

{
if (get_update_rate() == 0)
{
throw std::runtime_error("Controller's update rate is set to 0. This should not happen!");
}
update_period_ =
rclcpp::Duration(0.0, static_cast<uint32_t>(1.0e9 / static_cast<double>(get_update_rate())));
}

fmauch marked this conversation as resolved.
Show resolved Hide resolved
return CallbackReturn::SUCCESS;
}

Expand Down
Loading
Loading