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 14 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 @@ -112,6 +112,7 @@ class JointTrajectoryController : public controller_interface::ControllerInterfa
// Preallocate variables used in the realtime update() function
trajectory_msgs::msg::JointTrajectoryPoint state_current_;
trajectory_msgs::msg::JointTrajectoryPoint command_current_;
trajectory_msgs::msg::JointTrajectoryPoint command_next_;
trajectory_msgs::msg::JointTrajectoryPoint state_desired_;
trajectory_msgs::msg::JointTrajectoryPoint state_error_;

Expand All @@ -124,6 +125,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
Original file line number Diff line number Diff line change
Expand Up @@ -69,8 +69,10 @@ class Trajectory
* acceleration respectively. Deduction assumes that the provided velocity or acceleration have to
* be reached at the time defined in the segment.
*
* This function assumes that sampling is only done at monotonically increasing \p sample_time
* for any trajectory.
* This function by default assumes that sampling is only done at monotonically increasing \p
* sample_time for any trajectory. That means, it will only search for a point matching the sample
* time after the point it has been called before. If this isn't desired, set \p
* progress_search_start_index to false.
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
*
* Specific case returns for start_segment_itr and end_segment_itr:
* - Sampling before the trajectory start:
Expand All @@ -94,13 +96,16 @@ class Trajectory
* description above.
* \param[out] end_segment_itr Iterator to the end segment for given \p sample_time. See
* description above.
* \param[in] progress_search_start_index If set to true, the next sample call will start
* searching in the trajectory at the index of this call's result.
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
*/
JOINT_TRAJECTORY_CONTROLLER_PUBLIC
bool sample(
const rclcpp::Time & sample_time,
const interpolation_methods::InterpolationMethod interpolation_method,
trajectory_msgs::msg::JointTrajectoryPoint & output_state,
TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr);
TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr,
const bool progress_search_start_index = true);
bmagyar marked this conversation as resolved.
Show resolved Hide resolved

/**
* Do interpolation between 2 states given a time in between their respective timestamps
Expand Down
32 changes: 24 additions & 8 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,13 +196,20 @@ controller_interface::return_type JointTrajectoryController::update(
traj_external_point_ptr_->set_point_before_trajectory_msg(
time, state_current_, joints_angle_wraparound_);
}
// Sample once at the beginning to establish the start time if none was given
traj_external_point_ptr_->sample(
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;
const bool valid_point = traj_external_point_ptr_->sample(
// Sample expected state from the trajectory
traj_external_point_ptr_->sample(
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,
end_segment_itr, false);

if (valid_point)
{
const rclcpp::Time traj_start = traj_external_point_ptr_->time_from_start();
Expand Down Expand Up @@ -276,7 +285,7 @@ controller_interface::return_type JointTrajectoryController::update(
// Update PIDs
for (auto i = 0ul; i < dof_; ++i)
{
tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
tmp_command_[i] = (command_next_.velocities[i] * ff_velocity_scale_[i]) +
pids_[i]->computeCommand(
state_error_.positions[i], state_error_.velocities[i],
(uint64_t)period.nanoseconds());
Expand All @@ -286,7 +295,7 @@ controller_interface::return_type JointTrajectoryController::update(
// set values for next hardware write()
if (has_position_command_interface_)
{
assign_interface_from_point(joint_command_interface_[0], state_desired_.positions);
assign_interface_from_point(joint_command_interface_[0], command_next_.positions);
}
if (has_velocity_command_interface_)
{
Expand All @@ -296,20 +305,20 @@ controller_interface::return_type JointTrajectoryController::update(
}
else
{
assign_interface_from_point(joint_command_interface_[1], state_desired_.velocities);
assign_interface_from_point(joint_command_interface_[1], command_next_.velocities);
}
}
if (has_acceleration_command_interface_)
{
assign_interface_from_point(joint_command_interface_[2], state_desired_.accelerations);
assign_interface_from_point(joint_command_interface_[2], command_next_.accelerations);
}
if (has_effort_command_interface_)
{
assign_interface_from_point(joint_command_interface_[3], tmp_command_);
}

// store the previous command. Used in open-loop control mode
last_commanded_state_ = state_desired_;
last_commanded_state_ = command_next_;
}

if (active_goal)
Expand Down Expand Up @@ -867,6 +876,13 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
std::string(get_node()->get_name()) + "/query_state",
std::bind(&JointTrajectoryController::query_state_service, this, _1, _2));

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())));

return CallbackReturn::SUCCESS;
}

Expand Down
8 changes: 6 additions & 2 deletions joint_trajectory_controller/src/trajectory.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -91,7 +91,8 @@ bool Trajectory::sample(
const rclcpp::Time & sample_time,
const interpolation_methods::InterpolationMethod interpolation_method,
trajectory_msgs::msg::JointTrajectoryPoint & output_state,
TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr)
TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr,
const bool progress_search_start_index)
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
{
THROW_ON_NULLPTR(trajectory_msg_)

Expand Down Expand Up @@ -176,7 +177,10 @@ bool Trajectory::sample(
}
start_segment_itr = begin() + i;
end_segment_itr = begin() + (i + 1);
last_sample_idx_ = i;
if (progress_search_start_index)
bmagyar marked this conversation as resolved.
Show resolved Hide resolved
{
last_sample_idx_ = i;
}
return true;
}
}
Expand Down
Loading
Loading