Skip to content

Commit d197fc7

Browse files
evan-palmerevan-palmer
authored
Port end effector trajectory controller from Angler (#54)
* testing gif in readme * format * bleh * Added reference to blue docs * Cleanup folders * implemented trajectory * whoops * ee controller mvp done * added error tolerance for execution * add controller state publishing * fix compilation errors * update version and changelogs * Start implementing action server interface * adding action server and improving logic * updates * controller ready for testing * add pluginlib description * fix pluginlib * fix plugin registration * reintegrate default constructor * fix library build * add transform to chained references in ik controller * bug fixes * bug fixes * added readme * fix slerp * fix link to blue docs * add changelogs * fix ci errors and pr comments * cleanup * bad idea :D
1 parent b9ada28 commit d197fc7

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

48 files changed

+1301
-31
lines changed

‎README.md

Lines changed: 11 additions & 4 deletions
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,15 @@
11
# auv_controllers
22

33
auv_controllers is a collection of controllers for autonomous underwater
4-
vehicles (AUVs) implemented using ros2_control. The controllers have
5-
been designed to support the complete AUV control hierarchy and to enable
6-
benchmarking against other commonly-used control algorithms.
4+
vehicles (AUVs) and underwater vehicle manipulator systems (UVMS) implemented
5+
using ros2_control. The controllers have been designed to support the complete
6+
system control hierarchy and to enable benchmarking against other commonly-used
7+
control algorithms.
8+
9+
<p align="center">
10+
<img src="media/uvms.gif" alt="UVMS whole-body control" width="49%" />
11+
<img src="media/teleop.gif" alt="AUV control" width="49%" />
12+
</p>
713

814
## Installation
915

@@ -28,10 +34,11 @@ rosdep update && \
2834
rosdep install -y --from-paths src --ignore-src
2935
```
3036

31-
## Quick start
37+
## Getting started
3238

3339
To learn more about how to use the controllers provided in this project, please
3440
refer to the [examples package](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/auv_control_demos).
41+
You can also find integration tutorials in the [Blue documentation](https://robotic-decision-making-lab.github.io/blue).
3542

3643
## Getting help
3744

‎auv_control_demos/CHANGELOG.md

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,7 @@
11
# Changelog for package auv_control_demos
22

3+
## 0.2.0 (2025-05-03)
4+
35
## 0.1.0 (2025-04-27)
46

57
- Updates the individual_controller and chained_controllers demos to use the

‎auv_control_demos/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<package format="3">
44

55
<name>auv_control_demos</name>
6-
<version>0.1.0</version>
6+
<version>0.2.0</version>
77
<description>Example package that includes demos for using auv_controllers in individual and chained modes</description>
88

99
<maintainer email="[email protected]">Colin Mitchell</maintainer>

‎auv_control_msgs/CHANGELOG.md

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,12 @@
11
# Changelog for package auv_control_msgs
22

3+
## 0.2.0 (2025-05-03)
4+
5+
- Implements the EndEffectorTrajectory message
6+
- Implements the EndEffectorTrajectoryPoint message
7+
- Implements the EndEffectorTrajectoryControllerState message
8+
- Adds the FollowEndEffectorTrajectory action
9+
310
## 0.1.0 (2025-04-27)
411

512
- Implements the IKControllerStateStamped message to support the new IK

‎auv_control_msgs/CMakeLists.txt

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -11,6 +11,10 @@ find_package(trajectory_msgs REQUIRED)
1111
rosidl_generate_interfaces(auv_control_msgs
1212
"msg/MultiActuatorStateStamped.msg"
1313
"msg/IKControllerStateStamped.msg"
14+
"msg/EndEffectorTrajectoryPoint.msg"
15+
"msg/EndEffectorTrajectory.msg"
16+
"msg/EndEffectorTrajectoryControllerState.msg"
17+
"action/FollowEndEffectorTrajectory.action"
1418
DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs
1519
)
1620

‎auv_control_msgs/action/FollowEndEffectorTrajectory.action

Lines changed: 40 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,40 @@
1+
# The end effector trajectory to follow.
2+
#
3+
# The trajectory header stamp is used to set the trajectory start time. This can be
4+
# set to zero to indicate that the controller should start following the trajectory
5+
# now.
6+
auv_control_msgs/EndEffectorTrajectory trajectory
7+
8+
# The maximum error that the controller is allowed when following the trajectory.
9+
# When this is set to 0, the tolerance will not be applied during control.
10+
float64 path_tolerance
11+
12+
# The maximum terminal error that the controller is allowed.
13+
# When this is set to 0, the tolerance will not affect the success of the action.
14+
float64 goal_tolerance
15+
16+
---
17+
int32 error_code
18+
int32 SUCCESSFUL = 0
19+
int32 INVALID_GOAL = -1
20+
int32 OLD_HEADER_TIMESTAMP = -2
21+
int32 PATH_TOLERANCE_VIOLATED = -3
22+
int32 GOAL_TOLERANCE_VIOLATED = -4
23+
24+
# A human-readable error description.
25+
string error_string
26+
27+
---
28+
std_msgs/Header header
29+
30+
# The reference pose for the controller at the current time instance.
31+
# This is distinct from the sample, which is retrieved at the next time
32+
# instance.
33+
geometry_msgs/Pose desired
34+
35+
# The current end effector state.
36+
geometry_msgs/Pose actual
37+
38+
# The squared Euclidean norm of the geodesic distance between the desired
39+
# and actual states.
40+
float64 error

‎auv_control_msgs/msg/EndEffectorTrajectory.msg

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
std_msgs/Header header
2+
3+
# The sequence of end effector points to track.
4+
auv_control_msgs/EndEffectorTrajectoryPoint[] points

‎auv_control_msgs/msg/EndEffectorTrajectoryControllerState.msg

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
std_msgs/Header header
2+
3+
# The reference sample from the trajectory. This is the sample from the current time instance and used
4+
# for error calculation.
5+
geometry_msgs/Pose reference
6+
7+
# The current end effector state.
8+
geometry_msgs/Pose feedback
9+
10+
# The squared Euclidean norm of the geodesic distance between the reference state and end effector state.
11+
float64 error
12+
13+
# The trajectory controller command. This is the trajectory sample from the next time instance.
14+
geometry_msgs/Pose output

‎auv_control_msgs/msg/EndEffectorTrajectoryPoint.msg

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
# The sequence of end effector poses.
2+
geometry_msgs/Pose point
3+
4+
# The time that this point should be reached, measured from the start of the trajectory.
5+
builtin_interfaces/Duration time_from_start

‎auv_control_msgs/msg/IKControllerStateStamped.msg

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
std_msgs/Header header
22

3-
# The name of the IK solver used by the controller
3+
# The name of the IK solver used by the controller.
44
string solver_name
55

66
# Position DoF names, e.g., joint or axis names.

‎auv_control_msgs/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -2,7 +2,7 @@
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
44
<name>auv_control_msgs</name>
5-
<version>0.1.0</version>
5+
<version>0.2.0</version>
66
<description>Custom messages for AUV controllers</description>
77

88
<maintainer email="[email protected]">Rakesh Vivekanandan</maintainer>

‎auv_controllers/CHANGELOG.md

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,9 @@
11
# Changelog for package auv_controllers
22

3+
## 0.2.0 (2025-05-03)
4+
5+
- Adds the end effector trajectory controller
6+
37
## 0.1.0 (2025-04-27)
48

59
- Adds the adaptive integral terminal sliding mode controller

‎auv_controllers/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<package format="3">
44

55
<name>auv_controllers</name>
6-
<version>0.1.0</version>
6+
<version>0.2.0</version>
77
<description>Meta package for auv_controllers</description>
88

99
<maintainer email="[email protected]">Evan Palmer</maintainer>

‎controller_common/CHANGELOG.md

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,9 @@
11
# Changelog for package controller_common
22

3+
## 0.2.0 (2025-05-03)
4+
5+
- Adds the common::math::isclose method for comparing doubles
6+
37
## 0.1.0 (2025-04-27)
48

59
- Ports reset message functions and error calculation to a common API

‎controller_common/include/controller_common/common.hpp

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -64,6 +64,8 @@ auto has_nan(const std::vector<double> & vec) -> bool;
6464

6565
auto all_nan(const std::vector<double> & vec) -> bool;
6666

67+
auto isclose(double a, double b, double rtol = 1e-05, double atol = 1e-08) -> bool;
68+
6769
} // namespace math
6870

6971
} // namespace common

‎controller_common/package.xml

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,7 @@
33
<package format="3">
44

55
<name>controller_common</name>
6-
<version>0.1.0</version>
6+
<version>0.2.0</version>
77
<description>Common interfaces for controllers used in this project</description>
88

99
<maintainer email="[email protected]">Evan Palmer</maintainer>

‎controller_common/src/common.cpp

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -171,6 +171,11 @@ auto all_nan(const std::vector<double> & vec) -> bool
171171
return std::ranges::all_of(vec, [](double x) { return std::isnan(x); });
172172
}
173173

174+
auto isclose(double a, double b, double rtol, double atol) -> bool
175+
{
176+
return std::abs(a - b) <= (atol + rtol * std::abs(b));
177+
}
178+
174179
} // namespace math
175180

176181
} // namespace common

‎end_effector_trajectory_controller/CMakeLists.txt

Lines changed: 96 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,96 @@
1+
cmake_minimum_required(VERSION 3.23)
2+
project(end_effector_trajectory_controller)
3+
4+
if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)")
5+
add_compile_options(-Wall -Wextra -Wpedantic)
6+
endif()
7+
8+
include(GNUInstallDirs)
9+
10+
find_package(ament_cmake REQUIRED)
11+
find_package(geometry_msgs REQUIRED)
12+
find_package(realtime_tools REQUIRED)
13+
find_package(auv_control_msgs REQUIRED)
14+
find_package(controller_common REQUIRED)
15+
find_package(tf2 REQUIRED)
16+
find_package(tf2_ros REQUIRED)
17+
find_package(tf2_eigen REQUIRED)
18+
find_package(hardware_interface REQUIRED)
19+
find_package(rclcpp REQUIRED)
20+
find_package(rclcpp_lifecycle REQUIRED)
21+
find_package(generate_parameter_library REQUIRED)
22+
find_package(controller_interface REQUIRED)
23+
find_package(Eigen3 REQUIRED)
24+
find_package(rclcpp_action REQUIRED)
25+
find_package(lifecycle_msgs REQUIRED)
26+
find_package(pluginlib REQUIRED)
27+
find_package(tf2_geometry_msgs REQUIRED)
28+
29+
generate_parameter_library(end_effector_trajectory_controller_parameters
30+
src/end_effector_trajectory_controller_parameters.yaml
31+
)
32+
33+
add_library(end_effector_trajectory_controller SHARED)
34+
target_sources(
35+
end_effector_trajectory_controller
36+
PRIVATE src/end_effector_trajectory_controller.cpp src/trajectory.cpp
37+
PUBLIC
38+
FILE_SET HEADERS
39+
BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
40+
FILES
41+
${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/end_effector_trajectory_controller.hpp
42+
${CMAKE_CURRENT_SOURCE_DIR}/include/end_effector_trajectory_controller/trajectory.hpp
43+
)
44+
target_compile_features(end_effector_trajectory_controller PUBLIC cxx_std_23)
45+
target_link_libraries(
46+
end_effector_trajectory_controller
47+
PUBLIC
48+
end_effector_trajectory_controller_parameters
49+
realtime_tools::realtime_tools
50+
controller_common::controller_common
51+
hardware_interface::hardware_interface
52+
rclcpp::rclcpp
53+
rclcpp_lifecycle::rclcpp_lifecycle
54+
tf2_ros::tf2_ros
55+
tf2_eigen::tf2_eigen
56+
tf2::tf2
57+
controller_interface::controller_interface
58+
rclcpp_action::rclcpp_action
59+
tf2_geometry_msgs::tf2_geometry_msgs
60+
${geometry_msgs_TARGETS}
61+
${auv_control_msgs_TARGETS}
62+
${lifecycle_msgs_TARGETS}
63+
)
64+
65+
pluginlib_export_plugin_description_file(controller_interface end_effector_trajectory_controller.xml)
66+
67+
install(
68+
TARGETS
69+
end_effector_trajectory_controller
70+
end_effector_trajectory_controller_parameters
71+
EXPORT export_end_effector_trajectory_controller
72+
LIBRARY DESTINATION lib/${PROJECT_NAME}
73+
ARCHIVE DESTINATION lib/${PROJECT_NAME}
74+
RUNTIME DESTINATION bin/${PROJECT_NAME}
75+
FILE_SET HEADERS
76+
)
77+
78+
ament_export_targets(export_end_effector_trajectory_controller HAS_LIBRARY_TARGET)
79+
ament_export_dependencies(
80+
"geometry_msgs"
81+
"realtime_tools"
82+
"auv_control_msgs"
83+
"controller_common"
84+
"tf2"
85+
"tf2_ros"
86+
"tf2_eigen"
87+
"hardware_interface"
88+
"rclcpp"
89+
"rclcpp_lifecycle"
90+
"controller_interface"
91+
"rclcpp_action"
92+
"lifecycle_msgs"
93+
"tf2_geometry_msgs"
94+
)
95+
96+
ament_package()

‎end_effector_trajectory_controller/LICENSE

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
Permission is hereby granted, free of charge, to any person obtaining a copy
2+
of this software and associated documentation files (the "Software"), to deal
3+
in the Software without restriction, including without limitation the rights
4+
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
5+
copies of the Software, and to permit persons to whom the Software is
6+
furnished to do so, subject to the following conditions:
7+
8+
The above copyright notice and this permission notice shall be included in
9+
all copies or substantial portions of the Software.
10+
11+
THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
12+
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
13+
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
14+
THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
15+
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
16+
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
17+
THE SOFTWARE.

‎end_effector_trajectory_controller/README.md

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
# End Effector Trajectory Controller
2+
3+
The end effector trajectory controller interpolates an end effector motion plan
4+
for whole-body inverse kinematic control. The positions are interpolated using
5+
linear interpolation. The orientations are interpolated using spherical linear
6+
interpolation.
7+
8+
## Plugin Library
9+
10+
end_effector_trajectory_controller/EndEffectorTrajectoryController
11+
12+
## References
13+
14+
The input to this controller is a sequence of end effector poses.
15+
16+
## Commands
17+
18+
The output of this controller is a sampled end effector pose.
19+
20+
## Subscribers
21+
22+
- end_effector_trajectory_controller/trajectory [auv_control_msgs::msg::EndEffectorTrajectory]
23+
24+
## Action Servers
25+
26+
- end_effector_trajectory_controller/follow_trajectory [auv_control_msgs::action::FollowEndEffectorTrajectory]
27+
28+
## Publishers
29+
30+
- end_effector_trajectory_controller/status [auv_control_msgs::msg::EndEffectorTrajectoryControllerState]

‎end_effector_trajectory_controller/end_effector_trajectory_controller.xml

Lines changed: 9 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,9 @@
1+
<library path="end_effector_trajectory_controller">
2+
<class name="end_effector_trajectory_controller/EndEffectorTrajectoryController"
3+
type="end_effector_trajectory_controller::EndEffectorTrajectoryController"
4+
base_class_type="controller_interface::ControllerInterface">
5+
<description>
6+
End effector trajectory controller for UVMS whole-body control
7+
</description>
8+
</class>
9+
</library>

0 commit comments

Comments
 (0)