feat!: major improvements to contorllers + ros2 versions handled by macros

.clang-format

@@ -0,0 +1,29 @@
+---
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+AllowAllParametersOfDeclarationOnNextLine: true
+# Do NOT force it always
+AlwaysBreakAfterReturnType: None
+AlignOperands: DontAlign
+BraceWrapping:
+  AfterClass: false
+  AfterFunction: false
+  AfterNamespace: false
+  AfterStruct: false
+BreakBeforeBraces: Custom
+BreakBeforeTernaryOperators: true
+BinPackArguments: false
+BinPackParameters: false
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+# Make it "cheap" to put the return type on its own line when needed
+PenaltyReturnTypeOnItsOwnLine: 50   # try 10–50
+ReflowComments: false
+IncludeBlocks: Preserve
+...

.clang-tidy

@@ -0,0 +1,41 @@
+Checks: >
+  # Disable all checks by default to explicitly enable those you want
+  -*,
+
+  # Bug Prone Checks: Focus on potential bugs and common mistakes
+  bugprone-*,
+
+  # Modernize Checks: Suggest modern C++ features and idioms
+  modernize-*,
+
+  # Performance Checks: Identify potential performance pitfalls
+  performance-*,
+
+  # Readability Checks: Improve code readability and maintainability
+  readability-*,
+
+  # Specific exceptions or stricter rules
+  -modernize-use-trailing-return-type, # Often generates noisy suggestions
+
+  # NOTE: These slow checks are disabled for pre-commit speed.
+  # Consider running them in CI:
+  # - clang-analyzer-* (deep dataflow analysis)
+  # - cppcoreguidelines-* (many checks, some overlap)
+  # - misc-*
+
+WarningsAsErrors: >
+  bugprone-*,
+
+HeaderFilterRegex: '^((?!/opt/ros|/usr/include).)*$'
+
+# AnalyzeTemporaryDtors: true
+# Disabled for speed. Enable in CI for thorough analysis.
+
+FormatStyle: file
+# Integrate with .clang-format to ensure consistent formatting options.
+
+CheckOptions:
+  # bugprone-argument-comment.StrictMode: 1
+  # readability-function-size.StatementThreshold: 800
+  # google-readability-braces-around-statements.ShortStatementLines: 1
+  # cert-str34-c.Strip: 'printf,sprintf,log'

.github/workflows/rolling_ros2_ci.yml

@@ -17,8 +17,6 @@ jobs:
       CCACHE_DIR: "${{ github.workspace }}/.ccache" # directory for ccache (and how we enable ccache in industrial_ci)
     steps:
       - uses: actions/checkout@v4 # clone target repository
-        with:
-          ref: rolling
       - uses: actions/cache@v4 # fetch/store the directory used by ccache before/after the ci run
         with:
           path: ${{ env.CCACHE_DIR }}

.gitignore

@@ -7,3 +7,5 @@ src/franka_description
 src/franka_ros2
 src/libfranka
 MUJOCO_LOG.TXT
+src/crisp_controllers.egg-info
+tmp

.pre-commit-config.yaml

@@ -0,0 +1,51 @@
+repos:
+  - repo: https://github.com/pre-commit/pre-commit-hooks
+    rev: v5.0.0
+    hooks:
+      - id: check-yaml
+      - id: check-added-large-files
+        args:
+          - --maxkb=2048
+      - id: check-merge-conflict
+  - repo: https://github.com/gitleaks/gitleaks
+    rev: v8.28.0
+    hooks:
+      - id: gitleaks
+
+  # CPP hooks
+  - repo: https://github.com/pre-commit/mirrors-clang-format
+    rev: v20.1.8
+    hooks:
+      - id: clang-format
+        types_or: [c++, c, cuda]
+
+  - repo: local
+    hooks:
+      - id: clang-tidy
+        name: clang-tidy
+        description: C++ code analysis tool
+        language: system
+        types_or: [c++, c, cuda]
+        always_run: false
+        pass_filenames: true
+        entry: bash -c
+        args:
+          - |
+            # If pre-commit passed no files, do nothing (success).
+            if [ "$#" -eq 0 ]; then
+              exit 0
+            fi
+            # Find compile_commands.json by searching up from current directory
+            d="$PWD"
+            while [ "$d" != "/" ]; do
+              if [ -f "$d/build/compile_commands.json" ]; then
+                # Run clang-tidy and ignore PCH-related errors
+                clang-tidy -p "$d/build" "$@" 2>&1 | grep -v "is not a valid precompiled PCH file" | grep -v "is not a PCH file:" || true
+                exit 0
+              fi
+              d="$(dirname "$d")"
+            done
+            echo "Error: compile_commands.json not found. Build the project first." >&2
+            exit 1
+          - --
+

CMakeLists.txt

@@ -1,4 +1,4 @@
-cmake_minimum_required(VERSION 3.5)
+cmake_minimum_required(VERSION 3.16)
 project(crisp_controllers)
 
 find_program(CCACHE_PROGRAM ccache)
@@ -63,8 +63,8 @@ find_package(realtime_tools REQUIRED)
 find_package(generate_parameter_library REQUIRED)
 
 generate_parameter_library(
-  cartesian_impedance_controller_parameters
-  src/cartesian_impedance_controller.yaml
+  cartesian_controller_parameters
+  src/cartesian_controller.yaml
 )
 
 generate_parameter_library(
@@ -112,7 +112,7 @@ endif()
 # Link parameter libraries to the main library
 target_link_libraries(${PROJECT_NAME}
   PRIVATE
-    cartesian_impedance_controller_parameters
+    cartesian_controller_parameters
     pose_broadcaster_parameters
     torque_feedback_controller_parameters
     twist_broadcaster_parameters

README.md

@@ -5,6 +5,9 @@
 <a href="https://github.com/utiasDSL/crisp_controllers/actions/workflows/kilted_ros2_ci.yml"><img src="https://github.com/utiasDSL/crisp_controllers/actions/workflows/kilted_ros2_ci.yml/badge.svg"/></a>
 <a href="https://github.com/utiasDSL/crisp_controllers/actions/workflows/rolling_ros2_ci.yml"><img src="https://github.com/utiasDSL/crisp_controllers/actions/workflows/rolling_ros2_ci.yml/badge.svg"/></a>
 <a href="https://danielsanjosepro.github.io/crisp_controllers/"><img alt="Static Badge" src="https://img.shields.io/badge/docs-passing-blue?style=flat&link=https%3A%2F%2Fdanielsanjosepro.github.io%2Fcrisp_controllers%2F"></a>
+
+> [!NOTE]
+> To reduce maintenance overhead, all ROS 2 distributions are supported from a single `main` branch. The code uses compile-time macros to handle version-specific differences.
 <a href="https://utiasDSL.github.io/crisp_controllers#citing"><img alt="Static Badge" src="https://img.shields.io/badge/arxiv-cite-b31b1b?style=flat"></a>
 
 CRISP is a collection of real-time, C++ controllers for compliant torque-based control for manipulators compatible with `ros2_control`, including **Cartesian Impedance Control** and **Operational Space Control**. Developed for deploying high-level learning-based policies (VLA, Diffusion, ...) and teleoperation on your manipulator. It is robot-agnostic and compatible with any manipulator offering and effort interface. Check the [project website](https://utiasdsl.github.io/crisp_controllers/) for guides, getting started, demos and more! 
@@ -24,10 +27,9 @@ ______
 
 ##### Updating the website
 
-We use [mkdocs](https://www.mkdocs.org/) to generate the website from markdown. You can modify it within `docs/` in particular the `index.md`.
-Then you can serve it locally or update the github pages with:
+We use [zensical](https://www.zensical.org/) to generate the website from markdown. You can modify it within `docs/` in particular the `index.md`.
+You can run the website locally with:
 ```bash
-uv run mkdocs serve
-uv run mkdocs gh-deploy
+pixi run zensical serve
 ```
-
+The website is automatically generated and deployed on GitHub pages with the CI. You can check the [project website](https://utiasdsl.github.io/crisp_controllers/) for guides, getting started, demos and more!

docs/index.md

@@ -14,11 +14,16 @@ hide:
 *Authors: [Daniel San Jose Pro](https://danielsanjosepro.github.io)[^1], [Oliver Hausdörfer](https://oliver.hausdoerfer.de/)[^1], [Ralf Römer](https://ralfroemer99.github.io)[^1], Maximilian Dösch[^1], [Martin Schuck](https://amacati.github.io/) [^1] and Angela Schoellig [^1]*.
 [^1]: The authors are with Technical University of Munich, Germany; TUM School of Computation, Information and Technology, Department of Computer Engineering, Learning Systems and Robotics Lab; Munich Institute of Robotics and Machine Intelligence.
 
-> A collection of real-time, C++ controllers for compliant torque-based control for manipulators compatible with `ros2_control`. Developed for deploying high-level learning-based policies (VLA, Diffusion, ...) and teleoperation on your manipulator. It is compatible with any manipulator offering an effort interface.
+You want to deploy your learning-based policy to your manipulator, or collect teleoperation data? `CRISP` provides the tools for that.
+
+- `CRISP` provides the `ros2_control` low-level controllers (compliant, real-time, C++, torque-based) and simple python interfaces `CRISP_PY` to interface with them. It is compatible with any manipulator offering an joint-level torque effort interface.
+
+- `CRISP` provides also a Gymnasium environment `CRISP_GYM` to deploy learning-based `LeRobot` policies and collect data in `LeRobotDataset` format.
 
 _If you use this work, please cite it using the [bibtex](#citing) below._
 
-Check the [controllers (CRISP controllers) :simple-github:](https://github.com/utiasDSL/crisp_controllers) , robot [demos (CRISP controllers demos) :simple-github:](https://github.com/utiasDSL/crisp_controllers_demos), a simple [python interface (CRISP_PY) :simple-github:](https://github.com/utiasDSL/crisp_py), and a [Gymnasium wrapper (CRISP_GYM) :simple-github:](https://github.com/utiasDSL/crisp_gym) for real-world experiments.
+Check the [controllers (CRISP controllers) :simple-github:](https://github.com/utiasDSL/crisp_controllers), the simple [python interface (CRISP_PY) :simple-github:](https://github.com/utiasDSL/crisp_py), and a [Gymnasium wrapper (CRISP_GYM) :simple-github:](https://github.com/utiasDSL/crisp_gym) for real-world experiments.
+
 
 !!! info "Aloha gripper for Manipulators"
     Check out [aloha4franka](https://tum-lsy.github.io/aloha4franka/) for the gripper used in the videos.
@@ -98,7 +103,7 @@ While `ROS2` frameworks like `MoveIt` offer comprehensive motion planning capabi
 We present a set of lightweight, torque-based Cartesian and joint-space controllers implemented in C++ for `ros2_control`, compatible with any robot exposing an effort interface—a common standard among modern manipulators.
 Our controllers incorporate friction compensation, joint limit avoidance, and error clipping, and have been validated on the Franka Robotics FR3 on hardware, and on various platforms in simulation.
 
-Designed for fast integration and real-time control, our implementation lowers the barrier to deploying learning-based algorithms on `ROS2`-compatible platforms.
+We provide tooling to collect data in `LeRobotDataset` format using teleoperation and deploy learning-based policies with minimal effort using `CRISP_PY` and `CRISP_GYM`.
 
 **Why the name "CRISP"**? "CRISP" reflects our design philosophy behind the package: a concise, to-the-point implementation for easy deployment and integration in other software stacks.
 

include/crisp_controllers/cartesian_controller.hpp

@@ -3,28 +3,33 @@
 /**
  * @file cartesian_controller.hpp
  * @brief Cartesian controller implementation for robot manipulation (supports impedance and OSC)
- * @author Your Organization
  */
 
-#include <Eigen/Dense>
+#include <memory>
+#include <string>
+#include <unordered_set>
+
+#include <Eigen/Dense>  // NOLINT(build/include_order)
 
-#include <Eigen/src/Core/Matrix.h>
-#include <Eigen/src/Geometry/Transform.h>
 #include <controller_interface/controller_interface.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/multibody/fwd.hpp>
 #include <rclcpp/rclcpp.hpp>
 
-#include "realtime_tools/realtime_buffer.hpp"
-#include <cartesian_impedance_controller_parameters.hpp>
+#include <crisp_controllers/utils/ros2_version.hpp>
+
+#if ROS2_VERSION_ABOVE_HUMBLE
+#include <crisp_controllers/cartesian_controller_parameters.hpp>
+#else
+#include <cartesian_controller_parameters.hpp>
+#endif
+
 #include <sensor_msgs/msg/joint_state.hpp>
-#include <string>
-#include <unordered_set>
+#include "realtime_tools/realtime_buffer.hpp"
 
-using CallbackReturn =
-    rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
+using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn;
 
 namespace crisp_controllers {
 
@@ -36,8 +41,7 @@ namespace crisp_controllers {
  * allowing for compliant interaction with the environment while maintaining
  * desired position and orientation targets.
  */
-class CartesianController
-    : public controller_interface::ControllerInterface {
+class CartesianController : public controller_interface::ControllerInterface {
 public:
   /**
    * @brief Get the command interface configuration
@@ -60,7 +64,7 @@ class CartesianController
    * @return Success/failure of update
    */
   controller_interface::return_type
-  update(const rclcpp::Time &time, const rclcpp::Duration &period) override;
+  update(const rclcpp::Time & time, const rclcpp::Duration & period) override;
 
   /**
    * @brief Initialize the controller
@@ -73,24 +77,21 @@ class CartesianController
    * @param previous_state Previous lifecycle state
    * @return Success/failure of configuration
    */
-  CallbackReturn
-  on_configure(const rclcpp_lifecycle::State &previous_state) override;
+  CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override;
 
   /**
    * @brief Activate the controller
    * @param previous_state Previous lifecycle state
    * @return Success/failure of activation
    */
-  CallbackReturn
-  on_activate(const rclcpp_lifecycle::State &previous_state) override;
+  CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override;
 
   /**
    * @brief Deactivate the controller
    * @param previous_state Previous lifecycle state
    * @return Success/failure of deactivation
    */
-  CallbackReturn
-  on_deactivate(const rclcpp_lifecycle::State &previous_state) override;
+  CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override;
 
   /*CartesianImpedanceController();*/
 
@@ -147,14 +148,15 @@ class CartesianController
   Eigen::Quaterniond target_orientation_;
   /** @brief Target wrench in task space */
   Eigen::VectorXd target_wrench_;
-  /** @brief Combined target pose as SE3 transformation */
-  pinocchio::SE3 target_pose_;
+  /** @brief Desired target position in Cartesian space after applying filtering */
+  Eigen::Vector3d desired_position_;
+  /** @brief Desired target orientation as quaternion after applying filtering */
+  Eigen::Quaterniond desired_orientation_;
 
   /** @brief Parameter listener for dynamic parameter updates */
-  std::shared_ptr<cartesian_impedance_controller::ParamListener>
-      params_listener_;
+  std::shared_ptr<cartesian_controller::ParamListener> params_listener_;
   /** @brief Current parameter values */
-  cartesian_impedance_controller::Params params_;
+  cartesian_controller::Params params_;
 
   /** @brief Frame ID of the end effector in the robot model */
   int end_effector_frame_id;
@@ -187,6 +189,8 @@ class CartesianController
   Eigen::VectorXd q_ref;
   /** @brief Reference joint velocities */
   Eigen::VectorXd dq_ref;
+  /** @brief Target joint positions for posture task */
+  Eigen::VectorXd q_target;
 
   /** @brief Previously computed torque */
   Eigen::VectorXd tau_previous;
@@ -203,7 +207,6 @@ class CartesianController
   /** @brief Friction parameters 3 of size nv */
   Eigen::VectorXd fp3;
 
-
   /** @brief Allowed type of joints **/
   const std::unordered_set<std::basic_string<char>> allowed_joint_types = {
     "JointModelRX",
@@ -215,8 +218,8 @@ class CartesianController
     "JointModelRUBZ",
   };
   /** @brief Continous joint types that should be considered separetly. **/
-  const std::unordered_set<std::basic_string<char>> continous_joint_types =
-    {"JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"};
+  const std::unordered_set<std::basic_string<char>> continous_joint_types = {
+    "JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"};
 
   /** @brief Maximum allowed delta values for error clipping */
   Eigen::VectorXd max_delta_ = Eigen::VectorXd::Zero(6);
@@ -246,18 +249,23 @@ class CartesianController
   /** @brief Final desired torque command */
   Eigen::VectorXd tau_d;
 
+  /** @brief Inverse of the manipulator joint mass projected in Cartesian space (6x6) */
+  Eigen::Matrix<double, 6, 6> Mx_inv = Eigen::Matrix<double, 6, 6>::Zero();
+  /** @brief the manipulator joint mass projected in Cartesian space (6x6) */
+  Eigen::Matrix<double, 6, 6> Mx = Eigen::Matrix<double, 6, 6>::Zero();
+
   /**
    * @brief Log debug information based on parameter settings
    * @param time Current time for throttling logs
    */
-  void log_debug_info(const rclcpp::Time& time);
+  void log_debug_info(const rclcpp::Time & time);
 
   /**
    * @brief Check publisher count for a specific topic
    * @param topic_name Name of the topic to check
    * @return true if publisher count is safe (<=1), false otherwise
    */
-  bool check_topic_publisher_count(const std::string& topic_name);
+  bool check_topic_publisher_count(const std::string & topic_name);
 };
 
-} // namespace crisp_controllers
+}  // namespace crisp_controllers