Skip to content

Commit d5372dc

Browse files
evan-palmermergify[bot]
evan-palmer
authored and
mergify[bot]
committed
Implement TPIK controller and AITSMC (#51)
* Updated ISMC to use twist stamped state message and refactored realtime buffers to copy message instead of pointer * starting port of tpik control * Continued work on TPIK controller * Started implementing task priority IK solver * Implemented base tpik solver * Finished initial implementation of constraints and task priority solver * Cleanup * Cleanup and starting to integrate pinocchio * missing change * Continuing to integrate pinocchio for jacobians * bedtime * actually bedtime now * Refactored solver * Finished initial version - starting build * gym time baby * solver compile working * Started ik controller * Wrote initial version of ik controller * gym time baby * ok actually gym time now * ik controller compiles * Remove state subscriber * Added odom sensorg * Added topic sensor readme * Updated to latest version of ros2_control api * Update cmakelists files * updated dockerfile * added structure for aitsmc * Started aitsmc implementation * Implemented bare bones version of aitsmc * Initial version of AITSMC compiles * cleanup * Cleanup * Update issue templates * Started updating readmes * name spell error * fixed ismc * Switch to alternative version of jacobian for pose constraint * 3 hours of my life spent on this freaking bug * use updated hydrodynamics api * updates * Fix topic sensor * debugging sensor * bleh * Fixed segfault in odom sensor * quick hack * quick hack * Start updated ismc and integrated updated hydrodynamics api * finish integrating updated hydrodynamics api * fix transient local * Fix bugs in model parsing * update description topic * remove initial adaptive gain value * fix qos in aitsmc * Fixed disturbance rejection torque * added angular velocity thruster controller * remove deadband from force model * fix controller registration * Fix nan thruster bug * fix bug in controller state publishers * reintroduce cleaned up external state interface for velocity controllers * Added deadband to rate controller * rename parameter * Added a gazebo passthrough thruster controller * test full urdf and start debugging tpik controller * adding docs to thruster controllers * testing math * updated thruster controller docs * testing math mode * update controller docs * update controller docs * update controller docs * Added controller state publisher to aitsmc * update controller docs * potential fix for controller manager * Fix linking bug * debugging wbc * cleanup and debugging wbc * Fixes to wbc * Fix solver export * split ik solvers into new package * bleh * pain * testing * revert * debugging ik solvers * adding tests for tpik controller * move urdf file * remove solver testing package * debug tpik solver * started moving ik solver examples to demos dir * add print to check aitsmc adaptation * testing * bleh * testing * bleh * bleh * bleh * bleh * bug fix * bug fix * bug fix * bug fix * bug fix * bleh * aitsmc debugged * fix parameters * pluginlib export not working again ahhhhh * fix pluginlib export * fix command interfaces * cleanup + cleanup params files * missing quotation mark * fix segfault * cleanup * cleanup * fix bug in state values save * revert change * Fix parameter namespacing * pain * Attempting frame error fix * Fixed q order in update state values * potential fix * dumb * Refactoring ik controller * refactored ik controller * fix resize bug * silly bug * more bug fixes * fixed ik controller * cleanup * Removed testing script * debugging * Fixed state update bug * WORKING STATE * testing ik_solver lib in whole_body_controllers * Revert and cleanup * getting ready for PR * docs * grammar * Add support to auto transform messages using the odom sensor * forgot to update build * fix broken wbc build * fix pinv bug * fix bug in aitsmc * done * precommit * address pr comments * cleanup * change no solution to debug log * remove unused dependency * try fixing ci pipeline * try c++ 20 * fix implicit cast to int * clang tidy fixes * format * Added changelogs and bumped version numbers * spelling (cherry picked from commit b9ada28) # Conflicts: # README.md
1 parent c9c58a1 commit d5372dc

File tree

92 files changed

+4536
-789
lines changed

Some content is hidden

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

92 files changed

+4536
-789
lines changed

‎.devcontainer/Dockerfile

+1-2
Original file line numberDiff line numberDiff line change
@@ -71,8 +71,7 @@ WORKDIR $USER_WORKSPACE
7171

7272
# Install debugging/linting Python packages
7373
RUN pip install \
74-
pre-commit \
75-
mypy
74+
pre-commit
7675

7776
# Install debugging/linting C++ packages
7877
RUN sudo apt-get -q update \

‎.dockerignore

+7-4
Original file line numberDiff line numberDiff line change
@@ -3,11 +3,14 @@
33
*
44

55
# Except the following
6-
6+
!auv_control_demos
7+
!auv_control_msgs
78
!auv_controllers
8-
!velocity_controllers
9+
!controller_common
10+
!ik_solvers
911
!thruster_allocation_matrix_controller
1012
!thruster_controllers
11-
!auv_control_msgs
12-
!auv_control_demos
13+
!topic_sensors
14+
!velocity_controllers
15+
!whole_body_controllers
1316
!ros2.repos

‎.github/ISSUE_TEMPLATE/bug-report.yml

+3-2
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
name: Bug Report
22
description: Report a bug.
3-
title: "[BUG]: <Please write a descriptive title after the '[BUG]: ' prefix>"
4-
labels: [bug, needs triage]
3+
title: "<Please write a descriptive title>"
4+
labels: [needs triage]
5+
type: Bug
56

67
body:
78
- type: markdown

‎.github/ISSUE_TEMPLATE/documentation.yml

+3-2
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
name: Documentation Improvement
22
description: Report an issue related to the project documentation.
3-
title: "[DOC]: <Please write a descriptive title after the '[DOC]: ' prefix>"
4-
labels: [documentation, needs triage]
3+
title: "<Please write a descriptive title>"
4+
labels: [needs triage]
5+
type: Documentation
56

67
body:
78
- type: markdown

‎.github/ISSUE_TEMPLATE/feature-request.yml

+3-2
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
name: Feature Request
22
description: Suggest a new idea for the project.
3-
title: "[FEATURE]: <Please write a descriptive title after the '[FEATURE]: ' prefix>"
4-
labels: [enhancement, needs triage]
3+
title: "<Please write a descriptive title>"
4+
labels: [needs triage]
5+
type: Feature
56

67
body:
78
- type: markdown

‎.github/workflows/ci.yml

+1-1
Original file line numberDiff line numberDiff line change
@@ -28,7 +28,7 @@ jobs:
2828
uses: ros-industrial/industrial_ci@master
2929
env:
3030
ROS_DISTRO: ${{ matrix.env.ROS_DISTRO }}
31-
CXXFLAGS: -Wall -Wextra -Wpedantic
31+
CXXFLAGS: -Wall -Wextra -Wpedantic -std=c++20
3232
CLANG_TIDY: true
3333
UPSTREAM_WORKSPACE: ros2.repos
3434
AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src

‎.pre-commit-config.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -22,7 +22,7 @@ repos:
2222
- id: gersemi
2323

2424
- repo: https://github.com/pre-commit/pre-commit-hooks
25-
rev: v4.4.0
25+
rev: v5.0.0
2626
hooks:
2727
- id: check-added-large-files
2828
- id: check-case-conflict

‎.ruff.toml

+1-1
Original file line numberDiff line numberDiff line change
@@ -1 +1 @@
1-
target-version = "py310"
1+
target-version = "py312"

‎.vscode/c_cpp_properties.json

+1-1
Original file line numberDiff line numberDiff line change
@@ -11,7 +11,7 @@
1111
"defines": [],
1212
"compilerPath": "/usr/bin/clang",
1313
"cStandard": "c99",
14-
"cppStandard": "c++20",
14+
"cppStandard": "c++23",
1515
"intelliSenseMode": "linux-clang-x64"
1616
}
1717
],

‎.vscode/settings.json

+3-2
Original file line numberDiff line numberDiff line change
@@ -3,10 +3,11 @@
33
"*.repos": "yaml",
44
"*.world": "xml",
55
"*.xacro": "xml",
6-
"*.srdf": "xml",
76
"*.rviz": "yaml",
87
"*.config": "xml",
9-
"*.sdf": "xml"
8+
"*.sdf": "xml",
9+
"limits": "cpp",
10+
"format": "cpp"
1011
},
1112
"terminal.integrated.defaultProfile.linux": "bash",
1213
"terminal.integrated.profiles.linux": {

‎README.md

+7-6
Original file line numberDiff line numberDiff line change
@@ -1,19 +1,20 @@
11
# auv_controllers
22

3-
auv_controllers is a collection of chainable controllers for autonomous
4-
underwater vehicles (AUVs) implemented using ros2_control. The controllers have
3+
auv_controllers is a collection of controllers for autonomous underwater
4+
vehicles (AUVs) implemented using ros2_control. The controllers have
55
been designed to support the complete AUV control hierarchy and to enable
66
benchmarking against other commonly-used control algorithms.
77

8-
> [!NOTE]
9-
> If you are interested in adding your own controller to this project, please
10-
> consider submitting a [pull request](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/pulls)!
11-
128
## Installation
139

1410
auv_controllers is currently supported on Linux and is available for the ROS 2
11+
<<<<<<< HEAD
1512
Rolling, Jazzy, Iron and Humble distributions. To install auv_controllers,
1613
first clone this project to the `src` directory of your ROS workspace:
14+
=======
15+
Rolling and Jazzy distributions. To install auv_controllers, first clone this
16+
project to the `src` directory of your ROS workspace:
17+
>>>>>>> b9ada28 (Implement TPIK controller and AITSMC (#51))
1718
1819
```bash
1920
git clone [email protected]:Robotic-Decision-Making-Lab/auv_controllers.git

‎auv_control_demos/CHANGELOG.md

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# Changelog for package auv_control_demos
2+
3+
## 0.1.0 (2025-04-27)
4+
5+
- Updates the individual_controller and chained_controllers demos to use the
6+
correct topic names

‎auv_control_demos/CMakeLists.txt

+4-4
Original file line numberDiff line numberDiff line change
@@ -1,12 +1,12 @@
1-
cmake_minimum_required(VERSION 3.8)
1+
cmake_minimum_required(VERSION 3.23)
22
project(auv_control_demos)
33

44
set(THIS_PACKAGE_INCLUDE_DEPENDS rclpy ament_cmake)
55

66
find_package(ament_cmake REQUIRED)
7-
foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
8-
find_package(${Dependency} REQUIRED)
9-
endforeach()
7+
find_package(rclpy REQUIRED)
8+
9+
ament_export_dependencies(rclpy)
1010

1111
install(
1212
DIRECTORY

‎auv_control_demos/chained_controllers/README.md

+1-1
Original file line numberDiff line numberDiff line change
@@ -88,7 +88,7 @@ applies a PWM command to the hardware interface.
8888
following command:
8989

9090
```bash
91-
ros2 topic pub /integral_sliding_mode_controller/system_state geometry_msgs/msg/Twist
91+
ros2 topic pub /integral_sliding_mode_controller/system_state nav_msgs/msg/Odometry
9292
```
9393

9494
5. The ISMC accepts reference commands sent over a topic or a reference

‎auv_control_demos/individual_controller/README.md

+3-4
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,6 @@
11
# Example 1: Individual Controller
22

3-
This example uses the [integral sliding mode controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) to demonstrate how
4-
to launch a single controller.
3+
This example uses the [integral sliding mode controller](https://github.com/Robotic-Decision-Making-Lab/auv_controllers/tree/main/velocity_controllers) to demonstrate how to launch a single controller.
54

65
## Tutorial Steps
76

@@ -62,7 +61,7 @@ to launch a single controller.
6261
topic-based interface:
6362

6463
```bash
65-
ros2 topic pub /integral_sliding_mode_controller/system_state geometry_msgs/msg/Twist
64+
ros2 topic pub /integral_sliding_mode_controller/system_state nav_msgs/msg/Odometry
6665
```
6766

6867
5. The ISMC accepts reference commands sent via a topic or the controller's
@@ -80,7 +79,7 @@ to launch a single controller.
8079
ros2 topic echo /integral_sliding_mode_controller/status
8180
```
8281

83-
Should yield an output similar to the following:
82+
should yield an output similar to the following:
8483

8584
```bash
8685
header:

‎auv_control_demos/package.xml

+3-3
Original file line numberDiff line numberDiff line change
@@ -1,10 +1,10 @@
11
<?xml version="1.0"?>
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
4+
45
<name>auv_control_demos</name>
5-
<version>0.0.1</version>
6-
<description>Example package that includes demos for using auv_controllers in individual and
7-
chained modes</description>
6+
<version>0.1.0</version>
7+
<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>
1010
<maintainer email="[email protected]">Everardo Gonzalez</maintainer>

‎auv_control_msgs/CHANGELOG.md

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
# Changelog for package auv_control_msgs
2+
3+
## 0.1.0 (2025-04-27)
4+
5+
- Implements the IKControllerStateStamped message to support the new IK
6+
controller

‎auv_control_msgs/CMakeLists.txt

+5-2
Original file line numberDiff line numberDiff line change
@@ -1,14 +1,17 @@
1-
cmake_minimum_required(VERSION 3.8)
1+
cmake_minimum_required(VERSION 3.23)
22
project(auv_control_msgs)
33

44
find_package(ament_cmake REQUIRED)
55
find_package(std_msgs REQUIRED)
6+
find_package(geometry_msgs REQUIRED)
67
find_package(builtin_interfaces REQUIRED)
78
find_package(rosidl_default_generators REQUIRED)
9+
find_package(trajectory_msgs REQUIRED)
810

911
rosidl_generate_interfaces(auv_control_msgs
1012
"msg/MultiActuatorStateStamped.msg"
11-
DEPENDENCIES builtin_interfaces std_msgs
13+
"msg/IKControllerStateStamped.msg"
14+
DEPENDENCIES builtin_interfaces std_msgs geometry_msgs trajectory_msgs
1215
)
1316

1417
ament_package()

‎auv_control_msgs/msg/IKControllerStateStamped.msg

+20
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,20 @@
1+
std_msgs/Header header
2+
3+
# The name of the IK solver used by the controller
4+
string solver_name
5+
6+
# Position DoF names, e.g., joint or axis names.
7+
string[] position_joint_names
8+
9+
# Velocity DoF names.
10+
string[] velocity_joint_names
11+
12+
# The reference end effector pose.
13+
geometry_msgs/Pose reference
14+
15+
# Time between two consecutive updates/execution of the control law.
16+
# This is often used for integration of the solution to determine the desired positions.
17+
float64 time_step
18+
19+
# The IK solver solution.
20+
trajectory_msgs/JointTrajectoryPoint solution

‎auv_control_msgs/package.xml

+3-4
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.0.1</version>
5+
<version>0.1.0</version>
66
<description>Custom messages for AUV controllers</description>
77

88
<maintainer email="[email protected]">Rakesh Vivekanandan</maintainer>
@@ -18,10 +18,9 @@
1818
<buildtool_depend>ament_cmake</buildtool_depend>
1919
<buildtool_depend>rosidl_default_generators</buildtool_depend>
2020

21-
<test_depend>ament_lint_auto</test_depend>
22-
<test_depend>ament_lint_common</test_depend>
23-
2421
<depend>std_msgs</depend>
22+
<depend>geometry_msgs</depend>
23+
<depend>trajectory_msgs</depend>
2524

2625
<exec_depend>rosidl_default_runtime</exec_depend>
2726
<member_of_group>rosidl_interface_packages</member_of_group>

‎auv_controllers/CHANGELOG.md

+11
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,11 @@
1+
# Changelog for package auv_controllers
2+
3+
## 0.1.0 (2025-04-27)
4+
5+
- Adds the adaptive integral terminal sliding mode controller
6+
- Adds the task priority IK solver
7+
- Adds the IK whole-body controller
8+
- Adds the odom topic sensor
9+
- Adds the controller_common package
10+
- Adds the Gazebo passthrough thruster controller
11+
- Adds the thruster rotation rate controller

‎auv_controllers/CMakeLists.txt

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
1-
cmake_minimum_required(VERSION 3.8)
1+
cmake_minimum_required(VERSION 3.23)
22
project(auv_controllers)
33

44
find_package(ament_cmake REQUIRED)

‎auv_controllers/package.xml

+6-2
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.0.1</version>
6+
<version>0.1.0</version>
77
<description>Meta package for auv_controllers</description>
88

99
<maintainer email="[email protected]">Evan Palmer</maintainer>
@@ -16,10 +16,14 @@
1616

1717
<buildtool_depend>ament_cmake</buildtool_depend>
1818

19+
<exec_depend>auv_control_msgs</exec_depend>
20+
<exec_depend>controller_common</exec_depend>
21+
<exec_depend>ik_solvers</exec_depend>
1922
<exec_depend>thruster_allocation_matrix_controller</exec_depend>
2023
<exec_depend>thruster_controllers</exec_depend>
24+
<exec_depend>topic_sensors</exec_depend>
2125
<exec_depend>velocity_controllers</exec_depend>
22-
<exec_depend>auv_control_msgs</exec_depend>
26+
<exec_depend>whole_body_controllers</exec_depend>
2327

2428
<export>
2529
<build_type>ament_cmake</build_type>

‎controller_common/CHANGELOG.md

+5
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
# Changelog for package controller_common
2+
3+
## 0.1.0 (2025-04-27)
4+
5+
- Ports reset message functions and error calculation to a common API

‎controller_common/CMakeLists.txt

+42
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
cmake_minimum_required(VERSION 3.23)
2+
project(controller_common)
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(rclcpp REQUIRED)
12+
find_package(geometry_msgs REQUIRED)
13+
find_package(nav_msgs REQUIRED)
14+
15+
add_library(controller_common SHARED)
16+
target_sources(
17+
controller_common
18+
PRIVATE src/common.cpp
19+
PUBLIC
20+
FILE_SET HEADERS
21+
BASE_DIRS ${CMAKE_CURRENT_SOURCE_DIR}/include
22+
FILES ${CMAKE_CURRENT_SOURCE_DIR}/include/controller_common/common.hpp
23+
)
24+
target_compile_features(controller_common PUBLIC cxx_std_23)
25+
target_link_libraries(
26+
controller_common
27+
PUBLIC rclcpp::rclcpp ${geometry_msgs_TARGETS} ${nav_msgs_TARGETS}
28+
)
29+
30+
install(
31+
TARGETS controller_common
32+
EXPORT export_controller_common
33+
RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
34+
LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
35+
ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR}
36+
FILE_SET HEADERS
37+
)
38+
39+
ament_export_targets(export_controller_common HAS_LIBRARY_TARGET)
40+
ament_export_dependencies("rclcpp" "geometry_msgs" "nav_msgs")
41+
42+
ament_package()

‎controller_common/LICENSE

+17
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.

0 commit comments

Comments
 (0)