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

Conversation

VladimirIvan
Copy link

  • Adds support for using the effort field in the trajectory message for JTCs that claim the effort command interface (warning message is no longer displayed).
  • For effort only controllers, the desired effort is added to the PID effort as a feed forward term.
  • Adds support for claiming a combination of position, effort command interfaces. The desired effort will be passed directly to the effort interface in this case (no PID is calculated). Positions are passed to directly to the position interface. This is useful when the harware runs a PID with a feed-forward term at a higher rate on the motor driver.
  • Adds tests for above features and changes testing logic.
  • Tested on hardware.
  • Passed local tests and pre-commit.
  • Updated docs.

Comment on lines +60 to +62
!(interface_types.size() == 1 ||
(interface_types.size() == 2 &&
rsl::contains<std::vector<std::string>>(interface_types, "position"))))
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.

Comment on lines +261 to +264
// 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) +
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.

Copy link
Member

@bmagyar bmagyar left a comment

Choose a reason for hiding this comment

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

Just the one question

Copy link
Member

@saikishor saikishor left a comment

Choose a reason for hiding this comment

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

Thanks for the nice add-on to JTC. I've left some comments from my side


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.

Comment on lines +261 to +264
// 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) +
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

Comment on lines +441 to +445
// No state interface for now, use command interface
if (has_effort_command_interface_)
{
assign_point_from_interface(state.effort, joint_command_interface_[3]);
}
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.

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

Comment on lines +261 to +264
// 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) +
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)) +

Comment on lines +1506 to 1513
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;
}
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.

Copy link
Contributor

@christophfroehlich christophfroehlich left a comment

Choose a reason for hiding this comment

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

Thanks for this contribution, including docs and tests.

However, the tests seem to fail deterministically with a segfault

2024-09-25T20:19:05.9980161Z     [ RUN      ] OnlyEffortTrajectoryControllers/TrajectoryControllerTestParameterized.compute_error_angle_wraparound_true/0
2024-09-25T20:19:05.9980772Z     [INFO] [1727295318.864311285] [test_joint_trajectory_controller]: No specific joint names are used for command interfaces. Using 'joints' parameter.
2024-09-25T20:19:05.9981269Z     [INFO] [1727295318.864359095] [test_joint_trajectory_controller]: Command interfaces are [effort] and state interfaces are [position velocity].
2024-09-25T20:19:05.9981683Z     [INFO] [1727295318.864391335] [test_joint_trajectory_controller]: Using 'splines' interpolation method.
2024-09-25T20:19:05.9982066Z     [INFO] [1727295318.864946880] [test_joint_trajectory_controller]: Action status changes will be monitored at 20.00 Hz.
2024-09-25T20:19:05.9982218Z     -- run_test.py: return code -11

Can you please address this?

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

Successfully merging this pull request may close these issues.

5 participants