Skip to content

Commit 5ed4fa5

Browse files
mathias31415bmagyarfmauchchristophfroehlich
authored
Add motion_primitives_forward_controller for interfacing motion primitive messages with hardware interfaces (#1636)
Co-authored-by: Bence Magyar <[email protected]> Co-authored-by: Felix Exner (fexner) <[email protected]> Co-authored-by: Christoph Froehlich <[email protected]>
1 parent 83fceac commit 5ed4fa5

23 files changed

+2057
-1
lines changed

‎.pre-commit-config.yaml‎

Lines changed: 5 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -113,7 +113,11 @@ repos:
113113
rev: v2.0.0
114114
hooks:
115115
- id: doc8
116-
args: ['--max-line-length=100', '--ignore=D001']
116+
args: [
117+
'--max-line-length=100',
118+
'--ignore=D001',
119+
'--ignore-path=motion_primitives_forward_controller/userdoc.rst' # D000 fails with myst_parser
120+
]
117121
exclude: CHANGELOG\.rst$
118122

119123
- repo: https://github.com/pre-commit/pygrep-hooks

‎doc/controllers_index.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@ The controllers are using `common hardware interface definitions`_, and may use
5858
Position Controllers <../position_controllers/doc/userdoc.rst>
5959
Velocity Controllers <../velocity_controllers/doc/userdoc.rst>
6060
Gpio Command Controller <../gpio_controllers/doc/userdoc.rst>
61+
Motion Primitive Controller <../motion_primitives_forward_controller/userdoc.rst>
6162

6263

6364
Broadcasters

‎doc/release_notes.rst‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -31,3 +31,7 @@ pid_controller
3131
* The legacy ``antiwindup`` boolean and integral clamp parameters ``i_clamp_max``/``i_clamp_min`` have been deprecated in favor of the new ``antiwindup_strategy`` parameter. A ``tracking_time_constant`` parameter has also been introduced to configure the back-calculation strategy.
3232
* A new ``error_deadband`` parameter stops integration when the error is within a specified range.
3333
* PID state publisher can be turned off or on by using ``activate_state_publisher`` parameter. (`#1823 <https://github.com/ros-controls/ros2_controllers/pull/1823>`_).
34+
35+
motion_primitives_forward_controller
36+
*******************************************
37+
* 🚀 The motion_primitives_forward_controller was added 🎉 (`#1636 <https://github.com/ros-controls/ros2_controllers/pull/1636>`_).

‎motion_primitives_forward_controller/CMakeLists.txt‎

Lines changed: 110 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,110 @@
1+
cmake_minimum_required(VERSION 3.10)
2+
project(motion_primitives_forward_controller)
3+
4+
find_package(ros2_control_cmake REQUIRED)
5+
set_compiler_options()
6+
export_windows_symbols()
7+
8+
set(THIS_PACKAGE_INCLUDE_DEPENDS
9+
control_msgs
10+
controller_interface
11+
hardware_interface
12+
pluginlib
13+
rclcpp
14+
rclcpp_lifecycle
15+
realtime_tools
16+
std_srvs
17+
)
18+
19+
find_package(ament_cmake REQUIRED)
20+
find_package(generate_parameter_library REQUIRED)
21+
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
22+
find_package(${Dependency} REQUIRED)
23+
endforeach()
24+
25+
generate_parameter_library(motion_primitives_forward_controller_parameters
26+
src/motion_primitives_forward_controller_parameter.yaml
27+
)
28+
add_library(
29+
motion_primitives_forward_controller
30+
SHARED
31+
src/motion_primitives_forward_controller.cpp
32+
)
33+
target_include_directories(motion_primitives_forward_controller PUBLIC
34+
$<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
35+
$<INSTALL_INTERFACE:include/${PROJECT_NAME}>
36+
)
37+
target_link_libraries(motion_primitives_forward_controller PUBLIC
38+
motion_primitives_forward_controller_parameters
39+
controller_interface::controller_interface
40+
hardware_interface::hardware_interface
41+
pluginlib::pluginlib
42+
rclcpp::rclcpp
43+
rclcpp_lifecycle::rclcpp_lifecycle
44+
realtime_tools::realtime_tools
45+
${control_msgs_TARGETS}
46+
${std_srvs_TARGETS}
47+
)
48+
49+
target_compile_definitions(motion_primitives_forward_controller PRIVATE "MOTION_PRIMITIVES_FORWARD_CONTROLLER_BUILDING_DLL")
50+
51+
pluginlib_export_plugin_description_file(
52+
controller_interface motion_primitives_forward_controller.xml)
53+
54+
install(
55+
TARGETS
56+
motion_primitives_forward_controller
57+
motion_primitives_forward_controller_parameters
58+
EXPORT motion_primitives_forward_controller_targets
59+
RUNTIME DESTINATION bin
60+
ARCHIVE DESTINATION lib
61+
LIBRARY DESTINATION lib
62+
)
63+
64+
65+
install(
66+
DIRECTORY include/
67+
DESTINATION include/${PROJECT_NAME}
68+
)
69+
ament_export_targets(motion_primitives_forward_controller_targets HAS_LIBRARY_TARGET)
70+
71+
ament_export_include_directories(
72+
include
73+
)
74+
ament_export_dependencies(
75+
${THIS_PACKAGE_INCLUDE_DEPENDS}
76+
)
77+
ament_export_libraries(
78+
motion_primitives_forward_controller
79+
)
80+
81+
ament_package()
82+
83+
if(BUILD_TESTING)
84+
find_package(ament_cmake_gmock REQUIRED)
85+
find_package(controller_manager REQUIRED)
86+
find_package(hardware_interface REQUIRED)
87+
find_package(ros2_control_test_assets REQUIRED)
88+
89+
ament_add_gmock(
90+
test_load_motion_primitives_forward_controller
91+
test/test_load_motion_primitives_forward_controller.cpp
92+
)
93+
target_link_libraries(test_load_motion_primitives_forward_controller
94+
motion_primitives_forward_controller
95+
controller_manager::controller_manager
96+
ros2_control_test_assets::ros2_control_test_assets
97+
)
98+
99+
add_rostest_with_parameters_gmock(
100+
test_motion_primitives_forward_controller
101+
test/test_motion_primitives_forward_controller.cpp
102+
${CMAKE_CURRENT_SOURCE_DIR}/test/motion_primitives_forward_controller_params.yaml
103+
)
104+
target_link_libraries(test_motion_primitives_forward_controller
105+
motion_primitives_forward_controller
106+
controller_interface::controller_interface
107+
hardware_interface::hardware_interface
108+
ros2_control_test_assets::ros2_control_test_assets
109+
)
110+
endif()

‎motion_primitives_forward_controller/README.md‎

Lines changed: 68 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
motion_primitive_controllers
2+
==========================================
3+
4+
Package to control robots using motion primitives like LINEAR_JOINT (PTP/ MOVEJ), LINEAR_CARTESIAN (LIN/ MOVEL) and CIRCULAR_CARTESIAN (CIRC/ MOVEC)
5+
6+
# Description
7+
This project provides an interface for sending motion primitives to an industrial robot controller using the `ExecuteMotionPrimitiveSequence.action` action from [control_msgs](https://github.com/ros-controls/control_msgs/blob/motion_primitives/control_msgs/action/ExecuteMotionPrimitiveSequence.action). The controller receives the primitives via the action interface and forwards them through command interfaces to the robot-specific hardware interface. Currently, hardware interfaces for [Universal Robots](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver) and [KUKA Robots](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver) are implemented.
8+
9+
- Supported motion primitives:
10+
- `LINEAR_JOINT`
11+
- `LINEAR_CARTESIAN`
12+
- `CIRCULAR_CARTESIAN`
13+
14+
If multiple motion primitives are passed to the controller via the action, the controller forwards them to the hardware interface as a sequence. To do this, it first sends `MOTION_SEQUENCE_START`, followed by each individual primitive, and finally `MOTION_SEQUENCE_END`. All primitives between these two markers will be executed as a single, continuous sequence. This allows seamless transitions (blending) between primitives.
15+
16+
The action interface also allows stopping the current execution of motion primitives. When a stop request is received, the controller sends `STOP_MOTION` to the hardware interface, which then halts the robot's movement. Once the controller receives confirmation that the robot has stopped, it sends `RESET_STOP` to the hardware interface. After that, new commands can be sent.
17+
18+
![Action Image](doc/Moprim_Controller_ExecuteMotion_Action.drawio.png)
19+
20+
This can be done, for example, via a Python script as demonstrated in the [`example python script`](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver/blob/main/ur_robot_driver/examples/send_dummy_motion_primitives_ur10e.py) in the `Universal_Robots_ROS2_Driver` package.
21+
22+
## Command and State Interfaces
23+
To transmit the motion primitives, the following command and state interfaces are required. All interfaces use the naming scheme `tf_prefix_ + "motion_primitive/<interface name>"` where the `tf_prefix` is provided to the controller as a parameter.
24+
25+
### Command Interfaces
26+
These interfaces are used to send motion primitive data to the hardware interface:
27+
- `motion_type`: Type of motion primitive (LINEAR_JOINT, LINEAR_CARTESIAN, CIRCULAR_CARTESIAN)
28+
- `q1` – `q6`: Target joint positions for joint-based motion
29+
- `pos_x`, `pos_y`, `pos_z`: Target Cartesian position
30+
- `pos_qx`, `pos_qy`, `pos_qz`, `pos_qw`: Orientation quaternion of the target pose
31+
- `pos_via_x`, `pos_via_y`, `pos_via_z`: Intermediate via-point position for circular motion
32+
- `pos_via_qx`, `pos_via_qy`, `pos_via_qz`, `pos_via_qw`: Orientation quaternion of via-point
33+
- `blend_radius`: Blending radius for smooth transitions
34+
- `velocity`: Desired motion velocity
35+
- `acceleration`: Desired motion acceleration
36+
- `move_time`: Optional duration for time-based execution (For LINEAR_JOINT and LINEAR_CARTESIAN. If move_time > 0, velocity and acceleration are ignored)
37+
38+
### State Interfaces
39+
These interfaces are used to communicate the internal status of the hardware interface back to the `motion_primitives_forward_controller`.
40+
- `execution_status`: Indicates the current execution state of the primitive. Possible values are:
41+
- `IDLE`: No motion in progress
42+
- `EXECUTING`: Currently executing a primitive
43+
- `SUCCESS`: Last command finished successfully
44+
- `ERROR`: An error occurred during execution
45+
- `STOPPING`: The hardware interface has received the `STOP_MOTION` command, but the robot has not yet come to a stop.
46+
- `STOPPED`: The robot was stopped using the `STOP_MOTION` command and must be reset with the `RESET_STOP` command before executing new commands.
47+
- `ready_for_new_primitive`: Boolean flag indicating whether the interface is ready to receive a new motion primitive
48+
49+
50+
## Parameters
51+
This controller uses the [`generate_parameter_library`](https://github.com/PickNikRobotics/generate_parameter_library) to handle its parameters. The parameter [definition file located in the src folder](https://github.com/ros-controls/ros2_controllers/blob/master/motion_primitives_forward_controller/src/motion_primitives_forward_controller_parameter.yaml) contains descriptions for all the parameters used by the controller.
52+
An example parameter file for this controller can be found in [the test directory](https://github.com/ros-controls/ros2_controllers/blob/master/motion_primitives_forward_controller/test/motion_primitives_forward_controller_params.yaml).
53+
54+
55+
# Architecture Overview
56+
Architecture for a UR robot with [`Universal_Robots_ROS2_Driver` in motion primitives mode](https://github.com/UniversalRobots/Universal_Robots_ROS2_Driver).
57+
58+
![UR Robot Architecture](doc/ros2_control_motion_primitives_ur.drawio.png)
59+
60+
Architecture for a KUKA robot with [`kuka_eki_motion_primitives_hw_interface`](https://github.com/b-robotized-forks/kuka_experimental/tree/motion_primitive_kuka_driver/kuka_eki_motion_primitives_hw_interface).
61+
62+
![KUKA Robot Architecture](doc/ros2_control_motion_primitives_kuka.drawio.png)
63+
64+
# Demo-Video with UR10e
65+
[![UR demo video](doc/moprim_forward_controller_ur_demo_thumbnail.png)](https://youtu.be/SKz6LFvJmhQ)
66+
67+
# Demo-Video with KR3
68+
[![KUKA demo video](doc/moprim_forward_controller_kuka_demo_thumbnail.png)](https://youtu.be/_BWCO36j9bg)

‎motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio‎

Lines changed: 99 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,99 @@
1+
<mxfile host="embed.diagrams.net" agent="Mozilla/5.0 (X11; Ubuntu; Linux x86_64; rv:139.0) Gecko/20100101 Firefox/139.0" version="27.1.4">
2+
<diagram name="Page-1" id="9u_5SAsCUr1ULjLXrb9y">
3+
<mxGraphModel grid="1" page="1" gridSize="10" guides="1" tooltips="1" connect="1" arrows="1" fold="1" pageScale="1" pageWidth="850" pageHeight="1100" math="0" shadow="0" adaptiveColors="none">
4+
<root>
5+
<mxCell id="0" />
6+
<mxCell id="1" parent="0" />
7+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-1" value="Python Script" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" vertex="1" parent="1">
8+
<mxGeometry x="40" y="280" width="120" height="280" as="geometry" />
9+
</mxCell>
10+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-2" value="Motion Primitives Forward Controller" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=top;" vertex="1" parent="1">
11+
<mxGeometry x="440" y="280" width="120" height="280" as="geometry" />
12+
</mxCell>
13+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-4" value="&lt;div&gt;Action&lt;/div&gt;&lt;div&gt;Server&lt;/div&gt;" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=middle;fillColor=#C3C3C3;" vertex="1" parent="1">
14+
<mxGeometry x="450" y="320" width="100" height="230" as="geometry" />
15+
</mxCell>
16+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-5" value="&lt;div&gt;Action&lt;/div&gt;&lt;div&gt;Client&lt;/div&gt;" style="rounded=0;whiteSpace=wrap;html=1;verticalAlign=middle;fillColor=#C3C3C3;" vertex="1" parent="1">
17+
<mxGeometry x="50" y="320" width="100" height="230" as="geometry" />
18+
</mxCell>
19+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-6" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.009;entryY=0.086;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#0000FF;" edge="1" parent="1" target="sG7xvZkgZCj6ZZEBmGvM-4">
20+
<mxGeometry width="50" height="50" relative="1" as="geometry">
21+
<mxPoint x="150" y="340" as="sourcePoint" />
22+
<mxPoint x="340" y="430" as="targetPoint" />
23+
</mxGeometry>
24+
</mxCell>
25+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-7" value="Send Goal (Motion Primitives Sequence)" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-6">
26+
<mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
27+
<mxPoint x="17" as="offset" />
28+
</mxGeometry>
29+
</mxCell>
30+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-8" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.976;entryY=0.178;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#0000FF;" edge="1" parent="1">
31+
<mxGeometry width="50" height="50" relative="1" as="geometry">
32+
<mxPoint x="450" y="356" as="sourcePoint" />
33+
<mxPoint x="147.5999999999999" y="356.93999999999994" as="targetPoint" />
34+
</mxGeometry>
35+
</mxCell>
36+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-9" value="Goal accepted or rejected" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-8">
37+
<mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
38+
<mxPoint x="-16" y="-3" as="offset" />
39+
</mxGeometry>
40+
</mxCell>
41+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-10" value="" style="endArrow=classic;html=1;rounded=0;entryX=0.009;entryY=0.086;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#CC0000;" edge="1" parent="1">
42+
<mxGeometry width="50" height="50" relative="1" as="geometry">
43+
<mxPoint x="150" y="410" as="sourcePoint" />
44+
<mxPoint x="450" y="409.5" as="targetPoint" />
45+
</mxGeometry>
46+
</mxCell>
47+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-11" value="Cancle Goal request" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-10">
48+
<mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
49+
<mxPoint x="17" as="offset" />
50+
</mxGeometry>
51+
</mxCell>
52+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-12" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.976;entryY=0.178;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#CC0000;" edge="1" parent="1">
53+
<mxGeometry width="50" height="50" relative="1" as="geometry">
54+
<mxPoint x="449" y="425.5" as="sourcePoint" />
55+
<mxPoint x="147" y="426.5" as="targetPoint" />
56+
</mxGeometry>
57+
</mxCell>
58+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-13" value="Response code" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-12">
59+
<mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
60+
<mxPoint x="-16" y="-2" as="offset" />
61+
</mxGeometry>
62+
</mxCell>
63+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-14" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.009;entryY=0.086;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#009900;" edge="1" parent="1">
64+
<mxGeometry width="50" height="50" relative="1" as="geometry">
65+
<mxPoint x="150" y="520" as="sourcePoint" />
66+
<mxPoint x="452" y="520" as="targetPoint" />
67+
</mxGeometry>
68+
</mxCell>
69+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-15" value="Request result" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-14">
70+
<mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
71+
<mxPoint x="17" as="offset" />
72+
</mxGeometry>
73+
</mxCell>
74+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-16" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.976;entryY=0.178;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#009900;" edge="1" parent="1">
75+
<mxGeometry width="50" height="50" relative="1" as="geometry">
76+
<mxPoint x="451" y="536" as="sourcePoint" />
77+
<mxPoint x="149" y="537" as="targetPoint" />
78+
</mxGeometry>
79+
</mxCell>
80+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-17" value="Result" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-16">
81+
<mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
82+
<mxPoint x="-16" y="-3" as="offset" />
83+
</mxGeometry>
84+
</mxCell>
85+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-18" value="" style="endArrow=classic;html=1;rounded=0;exitX=0.987;exitY=0.088;exitDx=0;exitDy=0;exitPerimeter=0;entryX=0.976;entryY=0.178;entryDx=0;entryDy=0;entryPerimeter=0;strokeWidth=2;strokeColor=#7F7F7F;" edge="1" parent="1">
86+
<mxGeometry width="50" height="50" relative="1" as="geometry">
87+
<mxPoint x="450" y="474" as="sourcePoint" />
88+
<mxPoint x="148" y="475" as="targetPoint" />
89+
</mxGeometry>
90+
</mxCell>
91+
<mxCell id="sG7xvZkgZCj6ZZEBmGvM-19" value="Feedback" style="edgeLabel;html=1;align=center;verticalAlign=middle;resizable=0;points=[];fontSize=10;fontColor=#7F7F7F;" connectable="0" vertex="1" parent="sG7xvZkgZCj6ZZEBmGvM-18">
92+
<mxGeometry x="-0.1147" y="1" relative="1" as="geometry">
93+
<mxPoint x="-16" y="-3" as="offset" />
94+
</mxGeometry>
95+
</mxCell>
96+
</root>
97+
</mxGraphModel>
98+
</diagram>
99+
</mxfile>

‎motion_primitives_forward_controller/doc/Moprim_Controller_ExecuteMotion_Action.drawio.png‎

75.9 KB
Loading

‎motion_primitives_forward_controller/doc/moprim_forward_controller_kuka_demo_thumbnail.png‎

780 KB
Loading

‎motion_primitives_forward_controller/doc/moprim_forward_controller_ur_demo_thumbnail.png‎

896 KB
Loading

‎motion_primitives_forward_controller/doc/ros2_control_motion_primitives_kuka.drawio‎

Lines changed: 237 additions & 0 deletions
Large diffs are not rendered by default.

0 commit comments

Comments
 (0)