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

Author: evan-palmer

.devcontainer/Dockerfile

@@ -71,8 +71,7 @@ WORKDIR $USER_WORKSPACE
 
 # Install debugging/linting Python packages
 RUN pip install \
-    pre-commit \
-    mypy
+    pre-commit
 
 # Install debugging/linting C++ packages
 RUN sudo apt-get -q update \

.dockerignore

@@ -3,11 +3,14 @@
 *
 
 # Except the following
-
+!auv_control_demos
+!auv_control_msgs
 !auv_controllers
-!velocity_controllers
+!controller_common
+!ik_solvers
 !thruster_allocation_matrix_controller
 !thruster_controllers
-!auv_control_msgs
-!auv_control_demos
+!topic_sensors
+!velocity_controllers
+!whole_body_controllers
 !ros2.repos

.github/ISSUE_TEMPLATE/bug-report.yml

@@ -1,7 +1,8 @@
 name: Bug Report
 description: Report a bug.
-title: "[BUG]: <Please write a descriptive title after the '[BUG]: ' prefix>"
-labels: [bug, needs triage]
+title: "<Please write a descriptive title>"
+labels: [needs triage]
+type: Bug
 
 body:
   - type: markdown

.github/ISSUE_TEMPLATE/documentation.yml

@@ -1,7 +1,8 @@
 name: Documentation Improvement
 description: Report an issue related to the project documentation.
-title: "[DOC]: <Please write a descriptive title after the '[DOC]: ' prefix>"
-labels: [documentation, needs triage]
+title: "<Please write a descriptive title>"
+labels: [needs triage]
+type: Documentation
 
 body:
   - type: markdown

.github/ISSUE_TEMPLATE/feature-request.yml

@@ -1,7 +1,8 @@
 name: Feature Request
 description: Suggest a new idea for the project.
-title: "[FEATURE]: <Please write a descriptive title after the '[FEATURE]: ' prefix>"
-labels: [enhancement, needs triage]
+title: "<Please write a descriptive title>"
+labels: [needs triage]
+type: Feature
 
 body:
   - type: markdown

.github/workflows/ci.yml

@@ -28,7 +28,7 @@ jobs:
         uses: ros-industrial/industrial_ci@master
         env:
           ROS_DISTRO: ${{ matrix.env.ROS_DISTRO }}
-          CXXFLAGS: -Wall -Wextra -Wpedantic
+          CXXFLAGS: -Wall -Wextra -Wpedantic -std=c++20
           CLANG_TIDY: true
           UPSTREAM_WORKSPACE: ros2.repos
           AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src

.pre-commit-config.yaml

@@ -22,7 +22,7 @@ repos:
     - id: gersemi
 
   - repo: https://github.com/pre-commit/pre-commit-hooks
-    rev: v4.4.0
+    rev: v5.0.0
     hooks:
       - id: check-added-large-files
       - id: check-case-conflict

.ruff.toml

@@ -1 +1 @@
-target-version = "py310"
+target-version = "py312"

.vscode/c_cpp_properties.json

@@ -11,7 +11,7 @@
       "defines": [],
       "compilerPath": "/usr/bin/clang",
       "cStandard": "c99",
-      "cppStandard": "c++20",
+      "cppStandard": "c++23",
       "intelliSenseMode": "linux-clang-x64"
     }
   ],

.vscode/settings.json

@@ -3,10 +3,11 @@
     "*.repos": "yaml",
     "*.world": "xml",
     "*.xacro": "xml",
-    "*.srdf": "xml",
     "*.rviz": "yaml",
     "*.config": "xml",
-    "*.sdf": "xml"
+    "*.sdf": "xml",
+    "limits": "cpp",
+    "format": "cpp"
   },
   "terminal.integrated.defaultProfile.linux": "bash",
   "terminal.integrated.profiles.linux": {