Postition, Velocity, and Effort Joint Trajectory Controller

[zacharyyamaoka]

Hi ROS2 Control team,

I am looking into adding support for a combined position, velocity and effort command interface for the JTC.

This would be useful when using a BLDC driver like: https://mjbots.com/ that internally runs its own PID control. You want to be able to you want to send a reference position, reference velocity, and feedforward effort command calculated by inverse dynamics.

It seems that the basics are in place: https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/src/joint_trajectory_controller.cpp

Is there a reason this is not support right now?

Thanks!

Zach

[christophfroehlich]

We recently merged effort feed-forward trajectories with #1200
I think it is not a big change now to also support feedforward position-velocity-effort trajectories.

And don't forget to test the new combination by adding this here
https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/test/test_trajectory_controller.cpp#L2559
https://github.com/ros-controls/ros2_controllers/blob/master/joint_trajectory_controller/test/test_trajectory_controller.cpp#L2642

[zacharyyamaoka]

Ok amazing, thanks for this background info. I will give it a go! :)

[zacharyyamaoka]

@christophfroehlich Thoughts on restructuring the PID controller from the JTC?

• I think the core purpose of the JTC is to receive any combination of position, velocity, acceleration, effort. Interpolate/integrate/derive as needed (which it does beautifully) and then pass them to the hardware interface.
• Right now this "main" use case seems to be obscured by the need to consider different combinations of pos, vel, accel, effort, and then the PID controller.

For example it doesn't make sense to me that an effort command would be then be converted into velocity:

            // If effort interface only, add desired effort as feed forward
            // If velocity interface, ignore desired effort
            size_t index_cmd_joint = map_cmd_to_joints_[i];
            tmp_command_[index_cmd_joint] =
              (command_next_.velocities[index_cmd_joint] * ff_velocity_scale_[i]) +
              (has_effort_command_interface_ ? command_next_.effort[index_cmd_joint] : 0.0) +
              pids_[i]->compute_command(
                state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint],
                period);
          }
        }

        // set values for next hardware write()
        if (has_position_command_interface_)
        {
          assign_interface_from_point(joint_command_interface_[0], command_next_.positions);
        }
        if (has_velocity_command_interface_)
        {
          if (use_closed_loop_pid_adapter_)
          {
            assign_interface_from_point(joint_command_interface_[1], tmp_command_);
          }
          else
          {
            assign_interface_from_point(joint_command_interface_[1], command_next_.velocities);
          }
        }

Which I guess doesn't happen right now as velocity + effort command interfaces are not supported... but that is what I am now looking to add (position, velocity + effort)!!

What makes sense to me is as follows:

• The user can define "any" combination of command interfaces (maybe just not position & acceleration, as its unclear how to get velocity)
• Depending on what command interfaces are defined, position, velocity, acceleration will be derived/integrated as needed to the best of the JTC's ability (no changes needed from what is already being done)
• User params can be available to configure the the interpolation behaviour if needed
• The user can define any combination of state interfaces
• Whenever the user defines both a state and command interface for a certain value, say position, then they have the option to run feedback control on that value.
• The gains are by default always set to 0 but can then be increased as desired by the user
• The output of the PID controller is always forwarded to the effort command interface. The "units" of this effort command is up to the user to define. If they are running only velocity control, then the units could be interpeted as m/s and they could add the corrective values to the nominal velocity command in their hardware interface (and achieve the same function that the JTC currently provides). If they want to do position, velocity, effort control, then the units of the effort interface would likely be interperted in N/m.
• This promotes better "separation of concerns" as the effort command interface is always responsible for the corrective term. Instead, right now the effect of corrective term changes depending on what command/state interfaces are used, which is hard to follow/understand
• Effort commands can always be sent, regardless of the combination of command/state interfaces, and will always be interpolated and then added as a feedforward term to the PID controller, whose output is always sent to an effort command interface.

Thanks for your consideration!

• Zach

[qleonardolp]

For example it doesn't make sense to me that an effort command would be then be converted into velocity:

To me it does some sense for simulations only. For the actual hardware it would only make sense by means of interpret this conversion as an admittance control on joint level, converting effort into velocity.

• The output of the PID controller is always forwarded to the effort command interface. The "units" of this effort command is up to the user to define. If they are running only velocity control, then the units could be interpeted as m/s and they could add the corrective values to the nominal velocity command in their hardware interface (and achieve the same function that the JTC currently provides). If they want to do position, velocity, effort control, then the units of the effort interface would likely be interperted in N/m.
• This promotes better "separation of concerns" as the effort command interface is always responsible for the corrective term. Instead, right now the effect of corrective term changes depending on what command/state interfaces are used, which is hard to follow/understand

It is interesting to consider such a "separation of concerns" here, but in this way I think it is not as helpful as it appears. The corrective term may depend of how the controller access the hardware/motors. For exemple, If my robot is velocity-controlled, using low-level PIDs to set the motor current according to the velocity error, it is not necessary to set an effort-based corrective term to compensate the gravitational effects.

In another words, I think the "separation of concerns" is more hardware interface (pos, vel, effort) oriented than application oriented. Thus, in this is why, as you said, the effect of corrective term changes depending on what command interfaces are used.

[christophfroehlich]

For example it doesn't make sense to me that an effort command would be then be converted into velocity:

To me it does some sense for simulations only. For the actual hardware it would only make sense by means of interpret this conversion as an admittance control on joint level, converting effort into velocity.

Fyi: The effort feedforward is not sent to the velocity command. It is sent via PID only updated if effort is the only command interface.

ros2_controllers/joint_trajectory_controller/src/joint_trajectory_controller.cpp

Lines 783 to 788 in d1844e6

	// if there is only velocity or if there is effort command interface
	// then use also PID adapter
	use_closed_loop_pid_adapter_ =
	(has_velocity_command_interface_ && params_.command_interfaces.size() == 1 &&
	!params_.open_loop_control) ||
	(has_effort_command_interface_ && params_.command_interfaces.size() == 1);

In principle, I support the idea of "separation of concerns", I have proposed to outsource the actual control law to plugins in #885
However, the structure of nested ifs with the tmp_command remained the same. And I'm not very convinced of your proposal: Why shouldn't there be a position-controlled PID feedback loop to a velocity command interfaces? Why to send this to an effort command? (which is per convention typically a force for linear joints, and torque for revolute joints?).

Btw I realized now that the documentation is not up-to-date: #1617

[qleonardolp]

Fyi: The effort feedforward is not sent to the velocity command. It is sent via PID only updated if effort is the only command interface.

Interesting! Makes sense to me now.

[zacharyyamaoka]

Hi @christophfroehlich @qleonardolp

Thank you for your feedback :)

I have made some changes, I will be pulling over shortly (https://github.com/zacharyyamaoka/ros2_controllers/tree/jtc_update_v2)

Thoughts on changing the non-pid effort to use the command_next_.effort instead of the state_desired_.effort? I am not sure why state_desired_.effort is used right now...

            assign_interface_from_point(joint_command_interface_[3], state_desired_.effort);

           assign_interface_from_point(joint_command_interface_[3], command_next_.effort);

best,

Zach

[zacharyyamaoka]

When only an 'effort' command interface is defined, why is the PID still calculated?

I would think the PID doesn't really have any meaning if there is no position/velocity setpoint?

        if (use_closed_loop_pid_adapter_)
        {
          // Update PIDs
          for (auto i = 0ul; i < num_cmd_joints_; ++i)
          {
            // If effort interface only, add desired effort as feed forward
            // If velocity interface, ignore desired effort
            size_t index_cmd_joint = map_cmd_to_joints_[i];
            tmp_command_[index_cmd_joint] =
              (command_next_.velocities[index_cmd_joint] * ff_velocity_scale_[i]) +
              (has_effort_command_interface_ ? command_next_.effort[index_cmd_joint] : 0.0) +
              pids_[i]->compute_command(
                state_error_.positions[index_cmd_joint], state_error_.velocities[index_cmd_joint],
                period);
          }
        }

best,

Zach

[zacharyyamaoka]

Hey @christophfroehlich @qleonardolp

Running the tests takes a while, like 5 mins on my computer,

colcon test --ctest-args tests --packages-select joint_trajectory_controller 

is there a way to speed up? It would help immensly with my dev cycle

Thank you!

Zach

[qleonardolp]

Running the tests takes a while, like 5 mins on my computer,

colcon test --ctest-args tests --packages-select joint_trajectory_controller 

is there a way to speed up? It would help immensly with my dev cycle

Thank you!

Zach

Try the following

colcon test --ctest-args -j $(nproc) --packages-select joint_trajectory_controller 

[christophfroehlich]

colcon test --ctest-args tests --packages-select joint_trajectory_controller 

What do you want to achieve with --ctest-args tests?

is there a way to speed up? It would help immensly with my dev cycle

Not directly. What I do in this case is to deactivate (uncomment or delete) other tests in the CMakeLists file, for example test_trajectory_actions in

ros2_controllers/joint_trajectory_controller/CMakeLists.txt

Lines 90 to 97 in d0d6890

	ament_add_gmock(test_trajectory_actions
	test/test_trajectory_actions.cpp
	)
	set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300)
	target_link_libraries(test_trajectory_actions
	joint_trajectory_controller
	)
	ament_target_dependencies(test_trajectory_actions ros2_control_test_assets)

[zacharyyamaoka]

I appreciate the fast reply guys!!

Try the following

colcon test --ctest-args -j $(nproc) --packages-select joint_trajectory_controller

amazing thanks will give it a go

What do you want to achieve with --ctest-args tests?

Lol not sure XD I think mabye chat gpt put that there lol

ament_add_gmock(test_trajectory_actions
test/test_trajectory_actions.cpp
)
set_tests_properties(test_trajectory_actions PROPERTIES TIMEOUT 300)
target_link_libraries(test_trajectory_actions
joint_trajectory_controller
)
ament_target_dependencies(test_trajectory_actions ros2_control_test_assets)

Kk I will play around with that :)