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] Enable feed-forward effort trajectories #1200

Open
wants to merge 5 commits into
base: master
Choose a base branch
from
Open
Show file tree
Hide file tree
Changes from all 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
2 changes: 2 additions & 0 deletions joint_trajectory_controller/doc/trajectory.rst
Original file line number Diff line number Diff line change
Expand Up @@ -49,6 +49,8 @@ The spline interpolator uses the following interpolation strategies depending on

Trajectories with velocity fields only, velocity and acceleration only, or acceleration fields only can be processed and are accepted, if ``allow_integration_in_goal_trajectories`` is true. Position (and velocity) is then integrated from velocity (or acceleration, respectively) by Heun's method.

Effort trajectories are allowed for controllers that claim the ``effort`` command interface and they are treated as feed-forward effort that is added to the position feedback. Effort is handled separately from position, velocity and acceleration. We use linear interpolation for effort when the ``spline`` interpolation method is selected.

Visualized Examples
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
To visualize the difference of the different interpolation methods and their inputs, different trajectories defined at a 0.5s grid and are sampled at a rate of 10ms.
Expand Down
5 changes: 4 additions & 1 deletion joint_trajectory_controller/doc/userdoc.rst
Original file line number Diff line number Diff line change
Expand Up @@ -24,12 +24,15 @@ Currently, joints with hardware interface types ``position``, ``velocity``, ``ac
* ``position``, ``velocity``, ``acceleration``
* ``velocity``
* ``effort``
* ``position``, ``effort``

This means that the joints can have one or more command interfaces, where the following control laws are applied at the same time:

* For command interfaces ``position``, the desired positions are simply forwarded to the joints,
* For command interfaces ``acceleration``, desired accelerations are simply forwarded to the joints.
* For ``velocity`` (``effort``) command interfaces, the position+velocity trajectory following error is mapped to ``velocity`` (``effort``) commands through a PID loop if it is configured (:ref:`parameters`).
* For ``effort`` command interface (without ``position`` command interface), if the trajectory contains effort, this will be added to the PID commands as a feed forward effort.
* For ``position, effort`` command interface, if the trajectory contains effort, this will be passed directly to the ``effort`` interface (PID won't be used) while the positions will be passed to the ``position`` interface.
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Can't the same be used with the combination of velocity, effortand position, velocity, and effort?. Ofcourse, given that there is no closed-loop option use_closed_loop_pid_adapter_ selected.

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I suppose it could work. The logic to switch what is calculated via PIDs and what comes from the desired effort would need a bit of thought. I don't have a use case for this though. Position+effort is very common for manipulation where you need to add that extra force to maintain contact. I've only used velocity control for an arm that had a very low control rate and for wheeled robots, neither of which needed feed forward effort.


This leads to the following allowed combinations of command and state interfaces:

Expand All @@ -38,7 +41,7 @@ This leads to the following allowed combinations of command and state interfaces

* if command interface ``velocity`` is the only one, state interfaces must include ``position, velocity`` .

* With command interface ``effort``, state interfaces must include ``position, velocity``.
* With command interface ``effort`` or ``position, effort``, state interfaces must include ``position, velocity``.

* With command interface ``acceleration``, state interfaces must include ``position, velocity``.

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -32,7 +32,8 @@ tl::expected<void, std::string> command_interface_type_combinations(
// Check if command interfaces combination is valid. Valid combinations are:
// 1. effort
// 2. velocity
// 2. position [velocity, [acceleration]]
// 3. position [velocity, [acceleration]]
// 4. position, effort

if (
rsl::contains<std::vector<std::string>>(interface_types, "velocity") &&
Expand All @@ -56,9 +57,12 @@ tl::expected<void, std::string> command_interface_type_combinations(

if (
rsl::contains<std::vector<std::string>>(interface_types, "effort") &&
interface_types.size() > 1)
!(interface_types.size() == 1 ||
(interface_types.size() == 2 &&
rsl::contains<std::vector<std::string>>(interface_types, "position"))))
Comment on lines +60 to +62
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this is alright but I think we'll have to reimplement this function as it's getting a little chaotic in here :D

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I agree but I'm not volunteering to open this container of invertebrates just now. Happy to file it as an issue.

{
return tl::make_unexpected("'effort' command interface has to be used alone");
return tl::make_unexpected(
"'effort' command interface has to be used alone or with a 'position' interface");
}

return {};
Expand Down
61 changes: 54 additions & 7 deletions joint_trajectory_controller/src/joint_trajectory_controller.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -276,12 +276,26 @@ controller_interface::return_type JointTrajectoryController::update(
// Update PIDs
for (auto i = 0ul; i < dof_; ++i)
{
// If effort interface only, add desired effort as feed forward
// If velocity interface, ignore desired effort
tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
(has_effort_command_interface_ ? state_desired_.effort[i] : 0.0) +
Comment on lines +279 to +282
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

how are we ignoring desired effort with velocity interface here?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Yup! Same doubt

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// If effort interface only, add desired effort as feed forward
// If velocity interface, ignore desired effort
tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
(has_effort_command_interface_ ? state_desired_.effort[i] : 0.0) +
// If effort interface only, add desired effort as feed-forward
// If velocity interface, ignore desired effort
tmp_command_[i] = (state_desired_.velocities[i] * ff_velocity_scale_[i]) +
(has_velocity_command_interface_ ? 0.0 : (has_effort_command_interface_ ? state_desired_.effort[i] : 0.0)) +

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I admit this one is a not very readable.
It works because the whole block runs only if use_closed_loop_pid_adapter_ == true (line 256, just above the displayed preview).
This is set here if has_velocity_command_interface_ || has_effort_command_interface_ as long as there is only one command interface. So checking for only for velocity command interface gives the correct result. It is probably an open door for all sorts of bugs though. I'll go with the suggestion.

pids_[i]->computeCommand(
state_error_.positions[i], state_error_.velocities[i],
(uint64_t)period.nanoseconds());
}
}
else
{
if (has_position_command_interface_ && has_effort_command_interface_)
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
if (has_position_command_interface_ && has_effort_command_interface_)
if (has_effort_command_interface_ && (has_position_command_interface_ || has_velocity_command_interface_))

If we support the combinations as commented as above, this would be sufficient I guess

{
for (auto i = 0ul; i < dof_; ++i)
{
// If position and effort command interfaces, only pass desired effort
tmp_command_[i] = state_desired_.effort[i];
}
}
}

// set values for next hardware write()
if (has_position_command_interface_)
Expand Down Expand Up @@ -442,6 +456,11 @@ void JointTrajectoryController::read_state_from_state_interfaces(JointTrajectory
state.velocities.clear();
state.accelerations.clear();
}
// No state interface for now, use command interface
if (has_effort_command_interface_)
{
assign_point_from_interface(state.effort, joint_command_interface_[3]);
}
Comment on lines +459 to +463
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Do you think it would be interesting to support effort state interfaces in the future? it would help in introspect about tracking and all.

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

isn't th read_state_from_command_interfaces doing the same?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What would be the convention for getting effort from the effort interface?
Some actuators expose torque or current measurements, for others it will be just returning back the last command. I would argue that both should be exposed when available, potentially needing two separate data fields in the message.

}

bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajectoryPoint & state)
Expand Down Expand Up @@ -510,6 +529,20 @@ bool JointTrajectoryController::read_state_from_command_interfaces(JointTrajecto
state.accelerations.clear();
}

// Effort state always comes from last command
if (has_effort_command_interface_)
{
if (interface_has_values(joint_command_interface_[3]))
{
assign_point_from_interface(state.effort, joint_command_interface_[3]);
}
else
{
state.effort.clear();
has_values = false;
}
}

return has_values;
}

Expand Down Expand Up @@ -704,13 +737,14 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
use_closed_loop_pid_adapter_ =
(has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&
!params_.open_loop_control) ||
has_effort_command_interface_;
(has_effort_command_interface_ && params_.command_interfaces.size() == 1);

tmp_command_.resize(dof_, 0.0);

if (use_closed_loop_pid_adapter_)
{
pids_.resize(dof_);
ff_velocity_scale_.resize(dof_);
tmp_command_.resize(dof_, 0.0);

update_pids();
}
Expand Down Expand Up @@ -747,15 +781,17 @@ controller_interface::CallbackReturn JointTrajectoryController::on_configure(
return CallbackReturn::FAILURE;
}

// effort is always used alone so no need for size check
// effort only or effort and position command interfaces require position and velocity state
if (
has_effort_command_interface_ &&
(params_.command_interfaces.size() == 1 ||
(params_.command_interfaces.size() == 2 && has_position_command_interface_)) &&
(!has_velocity_state_interface_ || !has_position_state_interface_))
{
RCLCPP_ERROR(
logger,
"'effort' command interface can only be used alone if 'velocity' and "
"'position' state interfaces are present");
"'effort' command interface can only be used alone or with 'position' command interface "
"if 'velocity' and 'position' state interfaces are present");
return CallbackReturn::FAILURE;
}

Expand Down Expand Up @@ -1214,6 +1250,10 @@ void JointTrajectoryController::compute_error_for_joint(
{
error.accelerations[index] = desired.accelerations[index] - current.accelerations[index];
}
if (has_effort_command_interface_)
{
error.effort[index] = desired.effort[index];
}
}

void JointTrajectoryController::fill_partial_goal(
Expand Down Expand Up @@ -1463,10 +1503,12 @@ bool JointTrajectoryController::validate_trajectory_msg(
return false;
}
// reject effort entries
if (!points[i].effort.empty())
if (!has_effort_command_interface_ && !points[i].effort.empty())
{
RCLCPP_ERROR(
get_node()->get_logger(), "Trajectories with effort fields are currently not supported.");
get_node()->get_logger(),
"Trajectories with effort fields are only supported for "
"controllers using the 'effort' command interface.");
return false;
}
Comment on lines +1506 to 1513
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Should we do something if there is effort_command_interface and the points with empty effort?, should we at least print one time warning? or not needed?

Copy link
Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

I would argue that for backward compatibility we shouldn't print warnings. Previously the effort command interface accepted position-only or position+velocity trajectories without warnings. It would start printing warnings for completely valid trajectories.

}
Expand Down Expand Up @@ -1537,6 +1579,7 @@ void JointTrajectoryController::resize_joint_trajectory_point(
{
point.accelerations.resize(size, 0.0);
}
point.effort.resize(size, 0.0);
}

void JointTrajectoryController::resize_joint_trajectory_point_command(
Expand Down Expand Up @@ -1605,6 +1648,10 @@ void JointTrajectoryController::init_hold_position_msg()
// add velocity, so that trajectory sampling returns acceleration points in any case
hold_position_msg_ptr_->points[0].accelerations.resize(dof_, 0.0);
}
if (has_effort_command_interface_)
{
hold_position_msg_ptr_->points[0].effort.resize(dof_, 0.0);
}
}

} // namespace joint_trajectory_controller
Expand Down
33 changes: 33 additions & 0 deletions joint_trajectory_controller/src/trajectory.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -192,6 +192,10 @@ bool Trajectory::sample(
{
output_state.accelerations.resize(output_state.positions.size(), 0.0);
}
if (output_state.effort.empty())
{
output_state.effort.resize(output_state.positions.size(), 0.0);
}
return true;
}

Expand All @@ -207,6 +211,7 @@ void Trajectory::interpolate_between_points(
output.positions.resize(dim, 0.0);
output.velocities.resize(dim, 0.0);
output.accelerations.resize(dim, 0.0);
output.effort.resize(dim, 0.0);

auto generate_powers = [](int n, double x, double * powers)
{
Expand All @@ -219,6 +224,7 @@ void Trajectory::interpolate_between_points(

bool has_velocity = !state_a.velocities.empty() && !state_b.velocities.empty();
bool has_accel = !state_a.accelerations.empty() && !state_b.accelerations.empty();
bool has_effort = !state_a.effort.empty() && !state_b.effort.empty();
if (duration_so_far.seconds() < 0.0)
{
duration_so_far = rclcpp::Duration::from_seconds(0.0);
Expand All @@ -233,6 +239,25 @@ void Trajectory::interpolate_between_points(
double t[6];
generate_powers(5, duration_so_far.seconds(), t);

if (has_effort)
{
// do linear interpolation
for (size_t i = 0; i < dim; ++i)
{
double start_effort = state_a.effort[i];
double end_effort = state_b.effort[i];

double coefficients[2] = {0.0, 0.0};
coefficients[0] = start_effort;
if (duration_btwn_points.seconds() != 0.0)
{
coefficients[1] = (end_effort - start_effort) / duration_btwn_points.seconds();
}

output.effort[i] = t[0] * coefficients[0] + t[1] * coefficients[1];
}
}

if (!has_velocity && !has_accel)
{
// do linear interpolation
Expand Down Expand Up @@ -331,6 +356,14 @@ void Trajectory::deduce_from_derivatives(
trajectory_msgs::msg::JointTrajectoryPoint & first_state,
trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim, const double delta_t)
{
if (first_state.effort.empty())
{
first_state.effort.assign(dim, 0.0);
}
if (second_state.effort.empty())
{
second_state.effort.assign(dim, 0.0);
}
if (second_state.positions.empty())
{
second_state.positions.resize(dim);
Expand Down
Loading
Loading