[JTC] Enable feed-forward effort trajectories #1200

VladimirIvan · 2024-07-11T10:08:54Z

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.

bmagyar · 2024-07-17T09:12:14Z

joint_trajectory_controller/include/joint_trajectory_controller/validate_jtc_parameters.hpp

+    !(interface_types.size() == 1 ||
+      (interface_types.size() == 2 &&
+       rsl::contains<std::vector<std::string>>(interface_types, "position"))))


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

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

bmagyar · 2024-07-17T09:24:09Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+            // 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) +


how are we ignoring desired effort with velocity interface here?

Yup! Same doubt

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)) +

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.

bmagyar

Just the one question

saikishor

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

saikishor · 2024-07-17T13:04:28Z

joint_trajectory_controller/doc/userdoc.rst


 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.


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.

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.

saikishor · 2024-07-17T13:17:34Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+            // 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) +


Yup! Same doubt

saikishor · 2024-07-17T13:22:42Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+  // No state interface for now, use command interface
+  if (has_effort_command_interface_)
+  {
+    assign_point_from_interface(state.effort, joint_command_interface_[3]);
+  }


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

isn't th read_state_from_command_interfaces doing the same?

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.

saikishor · 2024-07-17T13:29:52Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

                              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_)


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

saikishor · 2024-07-18T10:51:35Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+            // 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) +


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)) +

saikishor · 2024-07-18T10:54:04Z

joint_trajectory_controller/src/joint_trajectory_controller.cpp

+    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;
    }


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?

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.

christophfroehlich

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?

github-actions bot requested review from bijoua29, bmagyar, DasRoteSkelett, destogl, fmauch, olivier-stasse and saikishor July 11, 2024 10:09

christophfroehlich self-requested a review July 11, 2024 10:42

vlad-touchlab added 3 commits July 16, 2024 14:47

Added effort and position control

ac8d3cb

Updated docs

62afca2

pre-commit fixes

7b28c55

christophfroehlich force-pushed the effort-trajectory-master branch from e6e3374 to 7b28c55 Compare July 16, 2024 12:47

Merge branch 'master' into effort-trajectory-master

fedd208

bmagyar reviewed Jul 17, 2024

View reviewed changes

bmagyar approved these changes Jul 17, 2024

View reviewed changes

bmagyar reviewed Jul 17, 2024

View reviewed changes

saikishor requested changes Jul 18, 2024

View reviewed changes

Merge branch 'master' into effort-trajectory-master

2f68dc1

christophfroehlich requested changes Sep 26, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[JTC] Enable feed-forward effort trajectories #1200

[JTC] Enable feed-forward effort trajectories #1200

VladimirIvan commented Jul 11, 2024

bmagyar Jul 17, 2024

VladimirIvan Aug 19, 2024

bmagyar Jul 17, 2024

saikishor Jul 17, 2024

saikishor Jul 18, 2024

VladimirIvan Aug 19, 2024

bmagyar left a comment

saikishor left a comment

saikishor Jul 17, 2024

VladimirIvan Aug 19, 2024

saikishor Jul 17, 2024

saikishor Jul 17, 2024

saikishor Jul 18, 2024

VladimirIvan Aug 19, 2024

saikishor Jul 17, 2024

saikishor Jul 18, 2024

saikishor Jul 18, 2024

VladimirIvan Aug 19, 2024

christophfroehlich left a comment

	if (has_position_command_interface_ && has_effort_command_interface_)
	if (has_effort_command_interface_ && (has_position_command_interface_ \|\| has_velocity_command_interface_))

[JTC] Enable feed-forward effort trajectories #1200

Are you sure you want to change the base?

[JTC] Enable feed-forward effort trajectories #1200

Conversation

VladimirIvan commented Jul 11, 2024

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

bmagyar left a comment

Choose a reason for hiding this comment

saikishor left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

christophfroehlich left a comment

Choose a reason for hiding this comment