chore: pre-commit options update

Author: danielsanjosepro

.pre-commit-config.yaml

@@ -49,31 +49,3 @@ repos:
             exit 1
           - --
 
-  - repo: local
-    hooks:
-      - id: ament_cppcheck
-        name: ament_cppcheck
-        description: Static code analysis of C/C++ files.
-        entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck
-        language: system
-        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
-
-  - repo: local
-    hooks:
-      - id: ament_cpplint
-        name: ament_cpplint
-        description: Static code analysis of C/C++ files.
-        entry: ament_cpplint
-        language: system
-        files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$
-        args: ["--linelength=100", "--filter=-whitespace/newline,-legal/copyright"]
-
-  # Cmake hooks
-  - repo: local
-    hooks:
-      - id: ament_lint_cmake
-        name: ament_lint_cmake
-        description: Check format of CMakeLists.txt files.
-        entry: ament_lint_cmake
-        language: system
-        files: CMakeLists\.txt$

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

include/crisp_controllers/cartesian_controller.hpp

@@ -18,7 +18,14 @@
 #include <pinocchio/multibody/fwd.hpp>
 #include <rclcpp/rclcpp.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 "realtime_tools/realtime_buffer.hpp"
 
@@ -147,9 +154,9 @@ class CartesianController : public controller_interface::ControllerInterface {
   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;

include/crisp_controllers/pose_broadcaster.hpp

@@ -7,7 +7,14 @@
 #include <Eigen/Dense>  // NOLINT(build/include_order)
 
 #include <controller_interface/controller_interface.hpp>
+#include <crisp_controllers/utils/ros2_version.hpp>
+
+#if ROS2_VERSION_ABOVE_HUMBLE
 #include <crisp_controllers/pose_broadcaster_parameters.hpp>
+#else
+#include <pose_broadcaster_parameters.hpp>
+#endif
+
 #include <geometry_msgs/msg/pose_stamped.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/multibody/fwd.hpp>

include/crisp_controllers/torque_feedback_controller.hpp

@@ -7,7 +7,13 @@
 #include <Eigen/Dense>  // NOLINT(build/include_order)
 
 #include <controller_interface/controller_interface.hpp>
+#include <crisp_controllers/utils/ros2_version.hpp>
+
+#if ROS2_VERSION_ABOVE_HUMBLE
 #include <crisp_controllers/torque_feedback_controller_parameters.hpp>
+#else
+#include <torque_feedback_controller_parameters.hpp>
+#endif
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <pinocchio/multibody/data.hpp>
 #include <pinocchio/multibody/model.hpp>

include/crisp_controllers/twist_broadcaster.hpp

@@ -7,7 +7,14 @@
 #include <Eigen/Dense>  // NOLINT(build/include_order)
 
 #include <controller_interface/controller_interface.hpp>
+#include <crisp_controllers/utils/ros2_version.hpp>
+
+#if ROS2_VERSION_ABOVE_HUMBLE
 #include <crisp_controllers/twist_broadcaster_parameters.hpp>
+#else
+#include <twist_broadcaster_parameters.hpp>
+#endif
+
 #include <geometry_msgs/msg/twist_stamped.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/multibody/fwd.hpp>

pixi.lock

@@ -17,9 +17,14 @@ cmake = "*"
 pkg-config = "*"
 make = "*"
 ninja = "*"
+pinocchio = ">=3.7.0,<4"
 # pinocchio = ">=3.7.0,<4"
 
 [tasks]
+clean = "rm -rf build install log"
+build = "colcon build --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON"
+clean-build = [{ task = "clean" }, { task = "build" }]
+source = "source install/setup.sh"
 
 [target.linux.dependencies]
 libgl-devel = "*"
@@ -63,3 +68,4 @@ fmt = ">=11.2.0,<12"
 
 [pypi-dependencies]
 crisp-controllers = { path = ".", editable = false }
+pre-commit = ">=4.5.1, <5"

pixi.toml

@@ -242,7 +242,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration &
 }
 
 CallbackReturn CartesianController::on_init() {
-  params_listener_ = std::make_shared<cartesian_impedance_controller::ParamListener>(get_node());
+  params_listener_ = std::make_shared<cartesian_controller::ParamListener>(get_node());
   params_listener_->refresh_dynamic_parameters();
   params_ = params_listener_->get_params();
 
@@ -492,11 +492,11 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat
     auto joint_id = model_.getJointId(joint_name);  // pinocchio joind id might be different
     auto joint = model_.joints[joint_id];
 
-    #if ROS2_VERSION_ABOVE_HUMBLE
+#if ROS2_VERSION_ABOVE_HUMBLE
     q[i] = state_interfaces_[i].get_optional().value_or(q[i]);
-    #else
+#else
     q[i] = state_interfaces_[i].get_value();
-    #endif
+#endif
     if (joint.shortname() == "JointModelRZ") {  // simple revolute joint case
       q_pin[joint.idx_q()] = q[i];
     } else if (continous_joint_types.count(
@@ -509,13 +509,13 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat
     q_ref[i] = q[i];
     q_target[i] = q[i];
 
-    #if ROS2_VERSION_ABOVE_HUMBLE
+#if ROS2_VERSION_ABOVE_HUMBLE
     dq[i] = state_interfaces_[num_joints + i].get_optional().value_or(dq[i]);
     dq_ref[i] = dq[i];
-    #else
+#else
     dq[i] = state_interfaces_[num_joints + i].get_value();
     dq_ref[i] = state_interfaces_[num_joints + i].get_value();
-    #endif
+#endif
   }
 
   pinocchio::forwardKinematics(model_, data_, q_pin, dq);

src/cartesian_controller.cpp

@@ -1,4 +1,4 @@
-cartesian_impedance_controller:
+cartesian_controller:
   joints:
     type: string_array
     description: "Names of the joints"
@@ -17,6 +17,13 @@ cartesian_impedance_controller:
     default_value: False
     description: "Whether we use operational space control or cartesian impedance control"
 
+  operational_space_regularization:
+    type: double
+    default_value: 0.1
+    description: "Regularization (damping) for Mx pseudo-inverse. Higher values improve stability near singularities but reduce accuracy. Valid range is 0.001-1.0; typical useful values are in the 0.01-0.5 range."
+    validation:
+      bounds<>: [0.001, 1.0]
+
   task:
     k_pos_x:
       type: double
@@ -243,7 +250,7 @@ cartesian_impedance_controller:
     q_ref:
       type: double
       default_value: 1.0
-      description: "Amount of smoothing for the target joint when using the EMA."
+      description: "Amount of smoothing for the target pose when using the EMA."
       validation:
         bounds<>: [0.0, 1.0]
 
@@ -264,6 +271,24 @@ cartesian_impedance_controller:
     default_value: true
     description: "Limit torques"
 
+  joint_limit_repulsion:
+    enabled:
+      type: bool
+      default_value: true
+      description: "Enable joint limit repulsion torques that push joints away from their limits"
+    safe_range:
+      type: double
+      default_value: 0.1
+      description: "Distance from joint limit where repulsion starts [rad]"
+      validation:
+        bounds<>: [0.01, 1.0]
+    max_torque:
+      type: double
+      default_value: 5.0
+      description: "Maximum repulsion torque at joint limit [Nm]"
+      validation:
+        bounds<>: [0.0, 50.0]
+
   max_delta_tau:
     type: double
     default_value: 0.5