Forward trajectory controller #944

URJala · 2024-03-11T09:59:43Z

The controller should be able to forward a cartesian tracjectory to the robot for interpolation.
Creates errors on compilation due to multiple definition of functions, when inheriting from the joint_trajectory_controller. But looking at the passthrough controllers from ROS1 that may not be the right controller to use anyway?
All inputs are welcome.

closes #839

RobertWilbrandt · 2024-03-11T15:17:56Z

I cannot reproduce your build errors, are you up-to-date with the packages listed in Universal_Robots_ROS2_Driver.rolling.repos?

As for the general idea, i don't think the forwarding controller should be derived from the JTC at all as it doesn't do any spline interpolation itself. You can just derive directly from controller_interface::ControllerInterface.

The main problem we will have is encoding the trajectory in a way that it reaches the hardware interface. In ROS 1, we could just define TrajectoryInterface and use it in the hardware and controller. This is not really possible in ROS 2, as you access LoanedStateInterfaces from the controller that only give you access to one double value each. I know that some people are working on allowing more complex interface types, but am also not sure if this would help us (as we ideally want to use data that does not have a statically defined size). @fmauch do you know about the current state of these efforts?

URJala · 2024-03-14T07:23:58Z

I am up-to-date on everything, as far as i can tell, but I have started working while deriving from ControllerInterface and that works fine, so i will just continue on that.
Mads and i talked about making an action_server in the controller that will send over one point at a time to the robot, that way we can have a variable input, while only using the space required for one point in the state interface. This sounds like a good way of doing it to me.

URJala · 2024-05-02T14:57:47Z

The controller is now functional, and you should be able to test it out yourself. A user can give the controller a trajectory to forward via the action server running on the controller. It is possible to define trajectories with just positions, or with both positions and velocities and/or accelerations. The trajectory is transferred to the hardware interface via the command interfaces, before the hardware interface then transfers it to the robot.
Only one trajectory will be accepted at a time, if one is executing others will be denied.
The controller monitors the execution time of the trajectory, and gives a warning if it takes too long, although it isn't counting correctly currently (It warns too early). This has no effect on the execution of the trajectory.
When the robot driver is launched this controller is not running, and needs to be loaded/activated manually, while the scaled joint trajectory controller is unloaded, as they cannot run at the same time. The example load_switch_controller_example.py does this. The robot needs to be running while this is done, so the controller can be loaded correctly. (This should be fixed, so it doesn't need to run, but i don't know how currently.)
The examples example_movej.py and example_spline.py send a trajectory with only positions and one with positions, velocities and accelerations, respectively.
Let me know what you think of the construction of the controller and the communication between the controller and the hardware interface.

fmauch

This is looking very promising but I think we have to iterate this a couple of times, since this is a rather big change :-)

examples/passthrough_example.py

examples/scaled_joint_example.py

examples/robot_class.py

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

ur_controllers/src/passthrough_trajectory_controller.cpp

Creates errors on compilation due to multiple definition of funtions.

…nal.

…m to the robot.

…upports passing through accelerations and velocities.

The number of joints in the controller is variable and received from the hardware interface. (It is not variable there though). The internal action server uses the action type FollowJointTrajectory. Both goal time tolerance and goal position tolerance are checked upon completion of trajectory, and feedback is sent to the user. The integration test has also been updated to test the controller.

Did some cleanup of comments and print statements. Improved the examples, so they are easier to use.

Moved functionality of passthrough controller from separate execute loop in to controller's update method. Also changed how the controller checks validity of a received trajectory, and how it communicates that to the hardware interface.

Command and state interfaces are now only interacted with within the update method. The update method still triggers some action server callbacks, so it is not threadsafe yet, but i dont know how to trigger the action server without having them in the update method.

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

ur_controllers/src/passthrough_trajectory_controller.cpp

This should making it drop in easier

This gets started by the initial joint_controller

…g periods

fmauch · 2024-10-02T15:11:14Z

Universal_Robots_ROS2_Driver.jazzy.repos

+    url: https://github.com/pal-robotics-forks/ros2_control.git
+    version: fix/controller_update_period


Suggested change

url: https://github.com/pal-robotics-forks/ros2_control.git

version: fix/controller_update_period

url: https://github.com/ros-controls/ros2_control.git

version: master

fmauch · 2024-10-02T15:11:33Z

Universal_Robots_ROS2_Driver.rolling.repos

+    url: https://github.com/pal-robotics-forks/ros2_control.git
+    version: fix/controller_update_period


Suggested change

url: https://github.com/pal-robotics-forks/ros2_control.git

version: fix/controller_update_period

url: https://github.com/ros-controls/ros2_control.git

version: master

Make passthrough command interfaces part of the joints and fix claiming conflicts

If we want to use other types for forwarding such as Cartesian setpoints or motion primitives it doesn't really make sense to have this as a part of the joints.

This reverts commit e224dfa.

This should be thread-safe in contrast to calling it from the AsyncIO thread.

VinDp

Works nicely and looks good, I just have some minor suggestions.

VinDp · 2024-10-22T12:24:00Z

ur_controllers/doc/index.rst

+
+.. _passthrough_trajectory_controller:
+
+ur_controlers/PassthroughTrajectoryController


Suggested change

ur_controlers/PassthroughTrajectoryController

ur_controllers/PassthroughTrajectoryController

VinDp · 2024-10-22T12:59:53Z

ur_controllers/doc/index.rst

+     <command_interface name="setpoint_velicities_0"/>
+     <command_interface name="setpoint_velicities_1"/>
+     <command_interface name="setpoint_velicities_2"/>
+     <command_interface name="setpoint_velicities_3"/>
+     <command_interface name="setpoint_velicities_4"/>
+     <command_interface name="setpoint_velicities_5"/>


Suggested change

<command_interface name="setpoint_velicities_0"/>

<command_interface name="setpoint_velicities_1"/>

<command_interface name="setpoint_velicities_2"/>

<command_interface name="setpoint_velicities_3"/>

<command_interface name="setpoint_velicities_4"/>

<command_interface name="setpoint_velicities_5"/>

<command_interface name="setpoint_velocities_0"/>

<command_interface name="setpoint_velocities_1"/>

<command_interface name="setpoint_velocities_2"/>

<command_interface name="setpoint_velocities_3"/>

<command_interface name="setpoint_velocities_4"/>

<command_interface name="setpoint_velocities_5"/>

VinDp · 2024-10-22T13:01:20Z

ur_controllers/doc/index.rst

+"""""""""""""""""""""""""""""""""
+
+* A trajectory passed to the controller will be sent to the hardware component one by one.
+* The controller will send one setpooint and then wait for the hardware to acknowledge that it can


Suggested change

* The controller will send one setpooint and then wait for the hardware to acknowledge that it can

* The controller will send one setpoint and then wait for the hardware to acknowledge that it can

VinDp · 2024-10-22T13:23:02Z

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp

+
+/*
+ * 0.0: No trajectory to forward, the controller is idling and ready to receive a new trajectory.
+ * 1.0: The controller has received and accepted a new trajecotry. When the state is 1.0, the controller will write a


Suggested change

* 1.0: The controller has received and accepted a new trajecotry. When the state is 1.0, the controller will write a

* 1.0: The controller has received and accepted a new trajectory. When the state is 1.0, the controller will write a

VinDp · 2024-10-22T17:55:09Z

ur_controllers/src/passthrough_trajectory_controller.cpp

+      } else if (interface_it->get_interface_name() == "velocity") {
+        joint_velocity_state_interface_.emplace_back(*interface_it);
+      }
+    } else if (interface_it->get_interface_name() == "acceleration") {
+      joint_acceleration_state_interface_.emplace_back(*interface_it);
+    }


Is there one unwanted extra parenthesis before the acceleration case or is it intended?

Suggested change

} else if (interface_it->get_interface_name() == "velocity") {

joint_velocity_state_interface_.emplace_back(*interface_it);

}

} else if (interface_it->get_interface_name() == "acceleration") {

joint_acceleration_state_interface_.emplace_back(*interface_it);

}

} else if (interface_it->get_interface_name() == "velocity") {

joint_velocity_state_interface_.emplace_back(*interface_it);

} else if (interface_it->get_interface_name() == "acceleration") {

joint_acceleration_state_interface_.emplace_back(*interface_it);

}

VinDp · 2024-10-22T17:57:25Z

ur_controllers/doc/index.rst

+
+This controller uses a ``control_msgs/FollowJointTrajectory`` action but instead of interpolating
+the trajectory on the ROS pc it forwards the complete trajectory to the robot controller for
+interpolation and execution. This way, the realtime requirements for the control PC.


"This way, the realtime requirements for the control PC." → isn't the sentence missing something?

VinDp · 2024-10-23T09:38:23Z

ur_controllers/src/passthrough_trajectory_controller.cpp

+
+    if (current_index_ == 0 && current_transfer_state == TRANSFER_STATE_IDLE) {
+      active_trajectory_elapsed_time_ = rclcpp::Duration(0, 0);
+      rclcpp::Duration::from_seconds(duration_to_double(active_joint_traj_.points.back().time_from_start));


Is this line useful? Because it is not assigned to anything and then it's repeated immediately after but assigned to max_trajectory_time

URJala requested a review from RobertWilbrandt May 2, 2024 14:58

fmauch self-requested a review May 3, 2024 10:00

URJala force-pushed the forward_controller branch from c7eea3c to 983bc62 Compare June 14, 2024 14:00

URJala marked this pull request as ready for review June 14, 2024 14:05

fmauch assigned URJala Jun 17, 2024

URJala force-pushed the forward_controller branch 2 times, most recently from 20fe4a6 to a8b83ff Compare July 16, 2024 07:29

fmauch requested changes Aug 5, 2024

View reviewed changes

URJala force-pushed the forward_controller branch from a8b83ff to 8e8f88f Compare August 23, 2024 01:44

URJala added 9 commits September 3, 2024 00:13

Started work on forward position controller

76bee81

Creates errors on compilation due to multiple definition of funtions.

Controller is now able to run and read from speed_scale

686ddbb

Communication between robot and passthrough controller is now functio…

b8ef0a7

…nal.

Controller will now send over all received points, before sending the…

ad13110

…m to the robot.

Created more examples for the use of the controller. Controller now s…

95db48b

…upports passing through accelerations and velocities.

Added passthrough controller to "initially loaded controller" options.

7042157

Did some cleanup of comments and print statements. Improved the examples, so they are easier to use.

Fix test_common

95173af

Move functionality to update method

4c9d9b6

Moved functionality of passthrough controller from separate execute loop in to controller's update method. Also changed how the controller checks validity of a received trajectory, and how it communicates that to the hardware interface.

URJala force-pushed the forward_controller branch from 8e8f88f to 4c9d9b6 Compare September 3, 2024 03:45

URJala added 2 commits September 18, 2024 00:20

Removing examples from this PR

67e5c0d

URJala requested a review from fmauch September 18, 2024 02:28

fmauch reviewed Sep 19, 2024

View reviewed changes

ur_controllers/include/ur_controllers/passthrough_trajectory_controller.hpp Outdated Show resolved Hide resolved

ur_controllers/src/passthrough_trajectory_controller.cpp Outdated Show resolved Hide resolved

fmauch added 4 commits September 25, 2024 18:39

Merge remote-tracking branch 'origin/main' into forward_controller

a19ac31

Updated to latest ControllerInterface API

a61bd79

Rename controller's action topic to match the JTC

caa8941

This should making it drop in easier

Remove duplicate controller entry

3d461ae

This gets started by the initial joint_controller

fmauch added 8 commits October 1, 2024 01:00

Remove unused interfaces

a5957fc

Deactivate goal time check for now since ros2_control can report wron…

5b1f7f3

…g periods

Simplify joint state configuration

5078c60

Make controller robust against joint ordering

51fd002

Make default config reflect the actual parameters

bd9652e

Fix tolerance handling

1389c3c

Merge remote-tracking branch 'origin/main' into forward_controller

e042de9

REVERT_ME: use fixed ros2_control until merged

bc76a61

fmauch requested changes Oct 2, 2024

View reviewed changes

fmauch added 2 commits October 2, 2024 18:07

Make sure the abort state is reset when sending new trajectories

934ccc5

Restructure command interface exporting

0e9b37d

Make passthrough command interfaces part of the joints and fix claiming conflicts

fmauch mentioned this pull request Oct 3, 2024

Add passthrough command interfaces for joints UniversalRobots/Universal_Robots_ROS2_Description#204

Merged

fmauch added 7 commits October 3, 2024 11:23

REVERT_ME: Use passthrough branch of description

e224dfa

Add documentation for the passthrough controller

6eb6def

Store command interface instead of using indices

3a0f562

Correctly handle preemptions

df7c9ab

Correctly abort and act on deactivate

197552a

Update documentation

63bce67

Improve result representation in example_move script

d6df554

fmauch requested a review from VinDp October 3, 2024 12:18

Merge upstream main

0cf3249

fmauch mentioned this pull request Oct 7, 2024

Add passthrough interfaces for joints #1121

Merged

fmauch added 5 commits October 14, 2024 11:37

Re-Restructure command interface exporting

2121cf8

If we want to use other types for forwarding such as Cartesian setpoints or motion primitives it doesn't really make sense to have this as a part of the joints.

Revert "REVERT_ME: Use passthrough branch of description"

17ab6d0

This reverts commit e224dfa.

Merge remote-tracking branch 'origin/main' into forward_controller

fdfcfab

Fix command interface names

ca7aff9

Check passthrough trajectory controller from write function

f887c09

This should be thread-safe in contrast to calling it from the AsyncIO thread.

fmauch mentioned this pull request Oct 18, 2024

Added service to enable and disable tool contact #940

Open

Merge remote-tracking branch 'origin/main' into forward_controller

ab62e4f

VinDp approved these changes Oct 23, 2024

View reviewed changes

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Forward trajectory controller #944

Forward trajectory controller #944

URJala commented Mar 11, 2024 •

edited by fmauch

Loading

RobertWilbrandt commented Mar 11, 2024

URJala commented Mar 14, 2024 •

edited

Loading

URJala commented May 2, 2024

fmauch left a comment

fmauch Oct 2, 2024

fmauch Oct 2, 2024

VinDp left a comment

VinDp Oct 22, 2024

VinDp Oct 22, 2024

VinDp Oct 22, 2024

VinDp Oct 22, 2024

VinDp Oct 22, 2024

VinDp Oct 22, 2024

VinDp Oct 23, 2024

		url: https://github.com/pal-robotics-forks/ros2_control.git
		version: fix/controller_update_period


		.. _passthrough_trajectory_controller:

		ur_controlers/PassthroughTrajectoryController

	ur_controlers/PassthroughTrajectoryController
	ur_controllers/PassthroughTrajectoryController

	* The controller will send one setpooint and then wait for the hardware to acknowledge that it can
	* The controller will send one setpoint and then wait for the hardware to acknowledge that it can

	* 1.0: The controller has received and accepted a new trajecotry. When the state is 1.0, the controller will write a
	* 1.0: The controller has received and accepted a new trajectory. When the state is 1.0, the controller will write a

Forward trajectory controller #944

Are you sure you want to change the base?

Forward trajectory controller #944

Conversation

URJala commented Mar 11, 2024 • edited by fmauch Loading

RobertWilbrandt commented Mar 11, 2024

URJala commented Mar 14, 2024 • edited Loading

URJala commented May 2, 2024

fmauch left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

VinDp 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

URJala commented Mar 11, 2024 •

edited by fmauch

Loading

URJala commented Mar 14, 2024 •

edited

Loading