From dbf271542430a2e18c737e22084ed5a424cefff3 Mon Sep 17 00:00:00 2001 From: adi-git-slu Date: Fri, 23 Jan 2026 15:52:16 -0500 Subject: [PATCH 1/6] [#1] Apply Customized Clang Format to the Code Base - Added config file for precommit. - Added formatting and linting. - Applied formatting and linting to all related files. --- .clang-format | 29 + .clang-tidy | 46 ++ .pre-commit-config.yaml | 93 +++ .../cartesian_controller.hpp | 44 +- include/crisp_controllers/pch.hpp | 8 +- .../crisp_controllers/pose_broadcaster.hpp | 45 +- .../torque_feedback_controller.hpp | 66 +-- .../crisp_controllers/twist_broadcaster.hpp | 45 +- include/crisp_controllers/utils/fiters.hpp | 48 +- .../utils/friction_model.hpp | 20 +- .../crisp_controllers/utils/joint_limits.hpp | 34 +- .../utils/pseudo_inverse.hpp | 30 +- .../utils/torque_rate_saturation.hpp | 9 +- src/cartesian_controller.cpp | 561 +++++++++--------- src/pose_broadcaster.cpp | 119 ++-- src/torque_feedback_controller.cpp | 173 +++--- src/twist_broadcaster.cpp | 314 +++++----- tests/test_filters.cpp | 25 +- tests/test_pseudo_inverse.cpp | 140 +++-- tests/test_torque_rate_saturation.cpp | 8 +- 20 files changed, 1042 insertions(+), 815 deletions(-) create mode 100644 .clang-format create mode 100644 .clang-tidy create mode 100644 .pre-commit-config.yaml diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..f4c5783 --- /dev/null +++ b/.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 +... diff --git a/.clang-tidy b/.clang-tidy new file mode 100644 index 0000000..7194de5 --- /dev/null +++ b/.clang-tidy @@ -0,0 +1,46 @@ +Checks: > + # Disable all checks by default to explicitly enable those you want + -*, + + # Bug Prone Checks: Focus on potential bugs and common mistakes + bugprone-*, + + # C++ Core Guidelines Checks: Enforce Modern C++ Guidelines + cppcoreguidelines-*, + + # Clang Static Analyzer Checks: Deeper semantic analysis for bugs + clang-analyzer-*, + + # Modernize Checks: Suggest modern C++ features and idioms + modernize-*, + + # Performance Checks: Identify potential performance pitfalls + performance-*, + + # Readability Checks: Improve code readability and maintainability + readability-*, + + # Misc Checks: Catch various issues + misc-*, + + # Specific exceptions or stricter rules + -modernize-use-trailing-return-type, # Often generates noisy suggestions + -cppcoreguidelines-pro-type-member-init, # Can be overly strict for some styles + +WarningsAsErrors: > + bugprone-*, + clang-analyzer-*, + +HeaderFilterRegex: '^((?!/opt/ros|/usr/include).)*$' + +AnalyzeTemporaryDtors: true +# Enables analysis of temporary destructors, which can catch more bugs but might be slower. + +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' \ No newline at end of file diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..8c01610 --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,93 @@ +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/astral-sh/ruff-pre-commit + # Ruff version. + rev: v0.12.7 + hooks: + # Run the linter. + - id: ruff-check + args: [ --fix, --exit-non-zero-on-fix ] + # Run the formatter. + - id: ruff-format + - repo: https://github.com/python-poetry/poetry + rev: 2.1.3 + hooks: + - id: poetry-check + - id: poetry-lock + - 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 + - -- + + - 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"] + + # 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$ \ No newline at end of file diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 1e55898..5e73cd0 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -1,3 +1,4 @@ +// "Copyright [2026] " #pragma once /** @@ -6,10 +7,12 @@ * @author Your Organization */ -#include +#include +#include +#include + +#include // NOLINT(build/include_order) -#include -#include #include #include #include @@ -17,14 +20,11 @@ #include #include -#include "realtime_tools/realtime_buffer.hpp" #include #include -#include -#include +#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 +36,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 +59,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 +72,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();*/ @@ -151,8 +147,7 @@ class CartesianController pinocchio::SE3 target_pose_; /** @brief Parameter listener for dynamic parameter updates */ - std::shared_ptr - params_listener_; + std::shared_ptr params_listener_; /** @brief Current parameter values */ cartesian_impedance_controller::Params params_; @@ -203,7 +198,6 @@ class CartesianController /** @brief Friction parameters 3 of size nv */ Eigen::VectorXd fp3; - /** @brief Allowed type of joints **/ const std::unordered_set> allowed_joint_types = { "JointModelRX", @@ -215,8 +209,8 @@ class CartesianController "JointModelRUBZ", }; /** @brief Continous joint types that should be considered separetly. **/ - const std::unordered_set> continous_joint_types = - {"JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; + const std::unordered_set> continous_joint_types = { + "JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; /** @brief Maximum allowed delta values for error clipping */ Eigen::VectorXd max_delta_ = Eigen::VectorXd::Zero(6); @@ -250,14 +244,14 @@ class CartesianController * @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 diff --git a/include/crisp_controllers/pch.hpp b/include/crisp_controllers/pch.hpp index 0304aa2..cd80487 100644 --- a/include/crisp_controllers/pch.hpp +++ b/include/crisp_controllers/pch.hpp @@ -1,12 +1,12 @@ +// "Copyright [2026] " #pragma once +#include #include #include #include -#include -#include -#include +#include // NOLINT(build/include_order) #include #include - +#include diff --git a/include/crisp_controllers/pose_broadcaster.hpp b/include/crisp_controllers/pose_broadcaster.hpp index fe04c2b..1cbd136 100644 --- a/include/crisp_controllers/pose_broadcaster.hpp +++ b/include/crisp_controllers/pose_broadcaster.hpp @@ -1,38 +1,38 @@ +// "Copyright [2026] " #pragma once -#include + +#include +#include +#include + +#include // NOLINT(build/include_order) #include -#include #include -#include +#include #include +#include -#include #include +#include #include - -using CallbackReturn = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace crisp_controllers { -class PoseBroadcaster - : public controller_interface::ControllerInterface { +class PoseBroadcaster : public controller_interface::ControllerInterface { public: [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() const override; [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() const override; controller_interface::return_type - update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + update(const rclcpp::Time & time, const rclcpp::Duration & period) override; CallbackReturn on_init() override; - CallbackReturn - on_configure(const rclcpp_lifecycle::State &previous_state) override; - CallbackReturn - on_activate(const rclcpp_lifecycle::State &previous_state) override; - CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; private: std::shared_ptr params_listener_; @@ -42,7 +42,6 @@ class PoseBroadcaster std::shared_ptr> realtime_pose_publisher_; - std::string end_effector_frame_; int end_effector_frame_id; @@ -60,13 +59,13 @@ class PoseBroadcaster "JointModelRUBZ", }; /** @brief Continous joint types that should be considered separetly. **/ - const std::unordered_set> continous_joint_types = - {"JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; + const std::unordered_set> continous_joint_types = { + "JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; Eigen::VectorXd q; - - rclcpp::Duration publish_elapsed_{0,0}; - rclcpp::Duration publish_interval_{0,0}; + + rclcpp::Duration publish_elapsed_{0, 0}; + rclcpp::Duration publish_interval_{0, 0}; }; -} // namespace crisp_controllers +} // namespace crisp_controllers diff --git a/include/crisp_controllers/torque_feedback_controller.hpp b/include/crisp_controllers/torque_feedback_controller.hpp index b6a19bd..8ff4b47 100644 --- a/include/crisp_controllers/torque_feedback_controller.hpp +++ b/include/crisp_controllers/torque_feedback_controller.hpp @@ -1,15 +1,20 @@ +// "Copyright [2026] " #pragma once -#include + +#include +#include +#include + +#include // NOLINT(build/include_order) #include #include -#include #include -#include #include +#include +#include -using CallbackReturn = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace crisp_controllers { @@ -34,8 +39,7 @@ namespace crisp_controllers { * - tau_friction: Friction compensation torques * - tau_nullspace: Nullspace control torques to maintain initial joint positions */ -class TorqueFeedbackController - : public controller_interface::ControllerInterface { +class TorqueFeedbackController : public controller_interface::ControllerInterface { public: /** * @brief Configure command interfaces for the controller @@ -43,14 +47,14 @@ class TorqueFeedbackController */ [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() const override; - + /** * @brief Configure state interfaces for the controller * @return Configuration specifying joint position, velocity, and effort interfaces */ [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() const override; - + /** * @brief Main control update loop * @@ -63,37 +67,34 @@ class TorqueFeedbackController * @return Control execution status */ 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 controller parameters and subscribers * @return Initialization status */ CallbackReturn on_init() override; - + /** * @brief Configure controller (set collision behavior if using real hardware) * @param previous_state Previous lifecycle state * @return Configuration status */ - CallbackReturn - on_configure(const rclcpp_lifecycle::State &previous_state) override; - + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + /** * @brief Activate controller and initialize joint states * @param previous_state Previous lifecycle state * @return Activation status */ - CallbackReturn - on_activate(const rclcpp_lifecycle::State &previous_state) override; - + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + /** * @brief Deactivate controller * @param previous_state Previous lifecycle state * @return Deactivation status */ - CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; private: /// Parameter listener for dynamic parameter updates @@ -103,10 +104,10 @@ class TorqueFeedbackController /// Subscriber for external torque commands rclcpp::Subscription::SharedPtr joint_sub_; - + /// Publisher for commanded wrench rclcpp::Publisher::SharedPtr wrench_pub_; - + /// Timer for 200Hz wrench publishing rclcpp::TimerBase::SharedPtr wrench_timer_; @@ -117,9 +118,8 @@ class TorqueFeedbackController * Only effort values are used from the incoming message. * @param msg Joint state message containing external torques in effort field */ - void - target_joint_callback_(const sensor_msgs::msg::JointState::SharedPtr msg); - + void target_joint_callback_(const sensor_msgs::msg::JointState::SharedPtr msg); + /** * @brief Callback for wrench publishing timer * @@ -142,30 +142,30 @@ class TorqueFeedbackController /// External torques received from subscriber Eigen::VectorXd tau_ext_; - + /// Initial joint positions recorded on activation (nullspace target) Eigen::VectorXd q_init_; - + /// Nullspace weights (computed from parameters) Eigen::VectorXd nullspace_weights_; - + /// Friction parameters as Eigen vectors Eigen::VectorXd friction_fp1_; Eigen::VectorXd friction_fp2_; Eigen::VectorXd friction_fp3_; - + /// Nullspace projection matrix Eigen::MatrixXd nullspace_projection_; - + /// Pinocchio model and data for dynamics computations pinocchio::Model model_; pinocchio::Data data_; - + /// End-effector frame ID for jacobian computation pinocchio::FrameIndex end_effector_frame_id_; - + /// Jacobian matrix for nullspace projection Eigen::MatrixXd J_; }; -} // namespace crisp_controllers +} // namespace crisp_controllers diff --git a/include/crisp_controllers/twist_broadcaster.hpp b/include/crisp_controllers/twist_broadcaster.hpp index c6f9c89..9501e16 100644 --- a/include/crisp_controllers/twist_broadcaster.hpp +++ b/include/crisp_controllers/twist_broadcaster.hpp @@ -1,38 +1,38 @@ +// "Copyright [2026] " #pragma once -#include + +#include +#include +#include + +#include // NOLINT(build/include_order) #include -#include #include -#include +#include #include +#include -#include #include +#include #include - -using CallbackReturn = - rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; +using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace crisp_controllers { -class TwistBroadcaster - : public controller_interface::ControllerInterface { +class TwistBroadcaster : public controller_interface::ControllerInterface { public: [[nodiscard]] controller_interface::InterfaceConfiguration command_interface_configuration() const override; [[nodiscard]] controller_interface::InterfaceConfiguration state_interface_configuration() const override; controller_interface::return_type - update(const rclcpp::Time &time, const rclcpp::Duration &period) override; + update(const rclcpp::Time & time, const rclcpp::Duration & period) override; CallbackReturn on_init() override; - CallbackReturn - on_configure(const rclcpp_lifecycle::State &previous_state) override; - CallbackReturn - on_activate(const rclcpp_lifecycle::State &previous_state) override; - CallbackReturn - on_deactivate(const rclcpp_lifecycle::State &previous_state) override; + CallbackReturn on_configure(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_activate(const rclcpp_lifecycle::State & previous_state) override; + CallbackReturn on_deactivate(const rclcpp_lifecycle::State & previous_state) override; private: std::shared_ptr params_listener_; @@ -42,7 +42,6 @@ class TwistBroadcaster std::shared_ptr> realtime_twist_publisher_; - std::string end_effector_frame_; int end_effector_frame_id; @@ -60,14 +59,14 @@ class TwistBroadcaster "JointModelRUBZ", }; /** @brief Continous joint types that should be considered separetly. **/ - const std::unordered_set> continous_joint_types = - {"JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; + const std::unordered_set> continous_joint_types = { + "JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; Eigen::VectorXd q; Eigen::VectorXd q_dot; - - rclcpp::Duration publish_elapsed_{0,0}; - rclcpp::Duration publish_interval_{0,0}; + + rclcpp::Duration publish_elapsed_{0, 0}; + rclcpp::Duration publish_interval_{0, 0}; }; -} // namespace crisp_controllers +} // namespace crisp_controllers diff --git a/include/crisp_controllers/utils/fiters.hpp b/include/crisp_controllers/utils/fiters.hpp index 2b6fbe6..d8f035c 100644 --- a/include/crisp_controllers/utils/fiters.hpp +++ b/include/crisp_controllers/utils/fiters.hpp @@ -1,6 +1,11 @@ +// "Copyright [2026] " #pragma once -#include +#include +#include +#include + +#include // NOLINT(build/include_order) /** * @brief Compute the exponential moving average of a value @@ -11,11 +16,9 @@ * @return returns the filtered value */ template - inline T exponential_moving_average(const T output, const T current, const double alpha) - { - return (1.0 - alpha) * current + alpha * output; - } - +inline T exponential_moving_average(const T output, const T current, const double alpha) { + return (1.0 - alpha) * current + alpha * output; +} /** * @brief Filter the joint values from a message used by the controller @@ -26,23 +29,22 @@ template */ template void filterJointValues( - const std::vector& msg_names, - const FieldType& msg_values, - const std::vector& desired_joint_names, - Eigen::VectorXd& output) -{ - std::unordered_map name_to_index; - for (size_t i = 0; i < desired_joint_names.size(); ++i) { - name_to_index[desired_joint_names[i]] = i; - } + const std::vector & msg_names, + const FieldType & msg_values, + const std::vector & desired_joint_names, + Eigen::VectorXd & output) { + std::unordered_map name_to_index; + for (size_t i = 0; i < desired_joint_names.size(); ++i) { + name_to_index[desired_joint_names[i]] = i; + } - for (size_t i = 0; i < msg_names.size(); ++i) { - if (msg_names[i].empty() || msg_names[i].size() > 1000) { - continue; - } - auto it = name_to_index.find(msg_names[i]); - if (it != name_to_index.end() && i < msg_values.size()) { - output(it->second) = msg_values[i]; - } + for (size_t i = 0; i < msg_names.size(); ++i) { + if (msg_names[i].empty() || msg_names[i].size() > 1000) { + continue; } + auto it = name_to_index.find(msg_names[i]); + if (it != name_to_index.end() && i < msg_values.size()) { + output(it->second) = msg_values[i]; + } + } } diff --git a/include/crisp_controllers/utils/friction_model.hpp b/include/crisp_controllers/utils/friction_model.hpp index 88742bc..3cb13fe 100644 --- a/include/crisp_controllers/utils/friction_model.hpp +++ b/include/crisp_controllers/utils/friction_model.hpp @@ -1,18 +1,18 @@ +// "Copyright [2026] " #pragma once // Based in // https://github.com/marcocognetti/FrankaEmikaPandaDynModel/tree/master -#include #include #include +#include - -inline Eigen::VectorXd get_friction(const Eigen::VectorXd &dq, - Eigen::VectorXd fp1, - Eigen::VectorXd fp2, - Eigen::VectorXd fp3) { - return ( - fp1.array() / (Eigen::VectorXd::Ones(dq.size()).array() + (-fp2.array() * (dq.array() + fp3.array())).exp()) - - fp1.array() / (Eigen::VectorXd::Ones(dq.size()).array() + (-fp2.array() * fp3.array()).exp()) - ).matrix(); +inline Eigen::VectorXd get_friction( + const Eigen::VectorXd & dq, Eigen::VectorXd fp1, Eigen::VectorXd fp2, Eigen::VectorXd fp3) { + return (fp1.array() / + (Eigen::VectorXd::Ones(dq.size()).array() + + (-fp2.array() * (dq.array() + fp3.array())).exp()) - + fp1.array() / + (Eigen::VectorXd::Ones(dq.size()).array() + (-fp2.array() * fp3.array()).exp())) + .matrix(); } diff --git a/include/crisp_controllers/utils/joint_limits.hpp b/include/crisp_controllers/utils/joint_limits.hpp index 8298292..ff1fa64 100644 --- a/include/crisp_controllers/utils/joint_limits.hpp +++ b/include/crisp_controllers/utils/joint_limits.hpp @@ -1,14 +1,20 @@ -#include +// "Copyright [2026] " +#pragma once -using namespace Eigen; +#include // NOLINT(build/include_order) -inline Eigen::VectorXd get_joint_limit_torque(const VectorXd &joint_positions, - const VectorXd &lower_limits, - const VectorXd &upper_limits, - double safe_range = 0.3, // [rad] - double max_torque = 5.0 // [Nm] -) { +using Eigen::Array; +using Eigen::ArrayXd; +using Eigen::Dynamic; +using Eigen::VectorXd; +inline Eigen::VectorXd get_joint_limit_torque( + const VectorXd & joint_positions, + const VectorXd & lower_limits, + const VectorXd & upper_limits, + double safe_range = 0.3, // [rad] + double max_torque = 5.0 // [Nm] +) { VectorXd torques = VectorXd::Zero(joint_positions.size()); VectorXd dist_to_lower = joint_positions - lower_limits; @@ -17,15 +23,13 @@ inline Eigen::VectorXd get_joint_limit_torque(const VectorXd &joint_positions, Array near_lower = (dist_to_lower.array() < safe_range); Array near_upper = (dist_to_upper.array() < safe_range); - ArrayXd lower_ratios = ((safe_range - dist_to_lower.array()) / safe_range) - .cwiseMax(0.0) - .cwiseMin(1.0); - ArrayXd upper_ratios = ((safe_range - dist_to_upper.array()) / safe_range) - .cwiseMax(0.0) - .cwiseMin(1.0); + ArrayXd lower_ratios = + ((safe_range - dist_to_lower.array()) / safe_range).cwiseMax(0.0).cwiseMin(1.0); + ArrayXd upper_ratios = + ((safe_range - dist_to_upper.array()) / safe_range).cwiseMax(0.0).cwiseMin(1.0); torques = (near_lower.select(max_torque * lower_ratios, 0.0) - near_upper.select(max_torque * upper_ratios, 0.0)) - .matrix(); + .matrix(); return torques; } diff --git a/include/crisp_controllers/utils/pseudo_inverse.hpp b/include/crisp_controllers/utils/pseudo_inverse.hpp index c1836e4..1911652 100644 --- a/include/crisp_controllers/utils/pseudo_inverse.hpp +++ b/include/crisp_controllers/utils/pseudo_inverse.hpp @@ -1,43 +1,39 @@ +// "Copyright [2026] " +#pragma once + #include #include #include namespace crisp_controllers { -inline Eigen::MatrixXd pseudo_inverse(const Eigen::MatrixXd &matrix, - double epsilon = 1e-4) { - Eigen::JacobiSVD svd(matrix, Eigen::ComputeFullU | - Eigen::ComputeFullV); +inline Eigen::MatrixXd pseudo_inverse(const Eigen::MatrixXd & matrix, double epsilon = 1e-4) { + Eigen::JacobiSVD svd(matrix, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::VectorXd singularValues = svd.singularValues(); Eigen::MatrixXd S = Eigen::MatrixXd::Zero(svd.matrixV().cols(), svd.matrixU().rows()); for (int i = 0; i < singularValues.size(); ++i) { - S(i, i) = - singularValues[i] / - (singularValues[i] * singularValues[i] + epsilon * epsilon); + S(i, i) = singularValues[i] / (singularValues[i] * singularValues[i] + epsilon * epsilon); } return svd.matrixV() * S * svd.matrixU().transpose(); } -inline Eigen::MatrixXd pseudo_inverse_moore_penrose(const Eigen::MatrixXd &matrix, - double epsilon = 1e-6) { - Eigen::JacobiSVD svd(matrix, Eigen::ComputeFullU | - Eigen::ComputeFullV); +inline Eigen::MatrixXd +pseudo_inverse_moore_penrose(const Eigen::MatrixXd & matrix, double epsilon = 1e-6) { + Eigen::JacobiSVD svd(matrix, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::VectorXd singularValues = svd.singularValues(); Eigen::MatrixXd S = Eigen::MatrixXd::Zero(svd.matrixV().cols(), svd.matrixU().rows()); for (int i = 0; i < singularValues.size(); ++i) { - S(i,i) = singularValues[i] > epsilon ? 1.0 / singularValues[i] : 0.0; + S(i, i) = singularValues[i] > epsilon ? 1.0 / singularValues[i] : 0.0; } return svd.matrixV() * S * svd.matrixU().transpose(); } -inline bool is_near_singular(const Eigen::MatrixXd &matrix, double epsilon = 1e-6) { - Eigen::JacobiSVD svd(matrix, Eigen::ComputeFullU | - Eigen::ComputeFullV); +inline bool is_near_singular(const Eigen::MatrixXd & matrix, double epsilon = 1e-6) { + Eigen::JacobiSVD svd(matrix, Eigen::ComputeFullU | Eigen::ComputeFullV); Eigen::VectorXd singularValues = svd.singularValues(); return (singularValues.array() < epsilon).any(); } - -} // namespace crisp_controllers +} // namespace crisp_controllers diff --git a/include/crisp_controllers/utils/torque_rate_saturation.hpp b/include/crisp_controllers/utils/torque_rate_saturation.hpp index dc1c7e3..d5fe0e3 100644 --- a/include/crisp_controllers/utils/torque_rate_saturation.hpp +++ b/include/crisp_controllers/utils/torque_rate_saturation.hpp @@ -1,13 +1,14 @@ +// "Copyright [2026] " #pragma once #include template -inline Eigen::VectorXd saturateTorqueRate(const Eigen::VectorXd &tau_d_calculated, - const Eigen::VectorXd &tau_J_d, - const T &delta_tau_max) { +inline Eigen::VectorXd saturateTorqueRate( + const Eigen::VectorXd & tau_d_calculated, + const Eigen::VectorXd & tau_J_d, + const T & delta_tau_max) { Eigen::VectorXd difference = tau_d_calculated - tau_J_d; Eigen::VectorXd clamped_diff = difference.cwiseMin(delta_tau_max).cwiseMax(-delta_tau_max); return tau_J_d + clamped_diff; } - diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 38c076f..6b35705 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -1,18 +1,19 @@ -#include "crisp_controllers/utils/fiters.hpp" -#include "crisp_controllers/utils/torque_rate_saturation.hpp" - -#include +// Copyright [2026] +#include #include +#include + +#include // NOLINT(build/include_order) + #include #include #include #include #include #include +#include "crisp_controllers/utils/fiters.hpp" +#include "crisp_controllers/utils/torque_rate_saturation.hpp" -#include "pinocchio/algorithm/model.hpp" -#include -#include #include #include #include @@ -22,6 +23,7 @@ #include #include #include +#include "pinocchio/algorithm/model.hpp" namespace crisp_controllers { @@ -29,7 +31,7 @@ controller_interface::InterfaceConfiguration CartesianController::command_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : params_.joints) { + for (const auto & joint_name : params_.joints) { config.names.push_back(joint_name + "/effort"); } return config; @@ -40,35 +42,30 @@ CartesianController::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : params_.joints) { + for (const auto & joint_name : params_.joints) { config.names.push_back(joint_name + "/position"); } - for (const auto &joint_name : params_.joints) { + for (const auto & joint_name : params_.joints) { config.names.push_back(joint_name + "/velocity"); } return config; } controller_interface::return_type -CartesianController::update(const rclcpp::Time &time, - const rclcpp::Duration & /*period*/) { - +CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { size_t num_joints = params_.joints.size(); for (size_t i = 0; i < num_joints; i++) { - - // TODO: later it might be better to get this thing prepared in the + // TODO(placeholder): later it might be better to get this thing prepared in the // configuration part (not in the control loop) auto joint_name = params_.joints[i]; - auto joint_id = - model_.getJointId(joint_name); // pinocchio joind id might be different + auto joint_id = model_.getJointId(joint_name); // pinocchio joind id might be different auto joint = model_.joints[joint_id]; /*q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(),*/ /* params_.filter.q);*/ q[i] = state_interfaces_[i].get_value(); - if (continous_joint_types.count( - joint.shortname())) { // Then we are handling a continous - // joint that is SO(2) + if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous + // joint that is SO(2) q_pin[joint.idx_q()] = std::cos(q[i]); q_pin[joint.idx_q() + 1] = std::sin(q[i]); } else { // simple revolute joint case @@ -80,19 +77,27 @@ CartesianController::update(const rclcpp::Time &time, dq[i] = state_interfaces_[num_joints + i].get_value(); } - if (new_target_pose_) {parse_target_pose_(); new_target_pose_ = false;} - if (new_target_joint_) {parse_target_joint_(); new_target_joint_ = false;} - if (new_target_wrench_) {parse_target_wrench_(); new_target_wrench_ = false;} + if (new_target_pose_) { + parse_target_pose_(); + new_target_pose_ = false; + } + if (new_target_joint_) { + parse_target_joint_(); + new_target_joint_ = false; + } + if (new_target_wrench_) { + parse_target_wrench_(); + new_target_wrench_ = false; + } pinocchio::forwardKinematics(model_, data_, q_pin, dq); pinocchio::updateFramePlacements(model_, data_); pinocchio::SE3 new_target_pose = - pinocchio::SE3(target_orientation_.toRotationMatrix(), target_position_); + pinocchio::SE3(target_orientation_.toRotationMatrix(), target_position_); target_pose_ = pinocchio::exp6(exponential_moving_average( - pinocchio::log6(target_pose_), pinocchio::log6(new_target_pose), - params_.filter.target_pose)); + pinocchio::log6(target_pose_), pinocchio::log6(new_target_pose), params_.filter.target_pose)); /*target_pose_ = pinocchio::SE3(target_orientation_.toRotationMatrix(), * target_position_);*/ @@ -101,30 +106,26 @@ CartesianController::update(const rclcpp::Time &time, // We consider translation and rotation separately to avoid unatural screw // motions if (params_.use_local_jacobian) { - error.head(3) = - end_effector_pose.rotation().transpose() * - (target_pose_.translation() - end_effector_pose.translation()); - error.tail(3) = pinocchio::log3(end_effector_pose.rotation().transpose() * - target_pose_.rotation()); + error.head(3) = end_effector_pose.rotation().transpose() * + (target_pose_.translation() - end_effector_pose.translation()); + error.tail(3) = + pinocchio::log3(end_effector_pose.rotation().transpose() * target_pose_.rotation()); } else { - error.head(3) = - target_pose_.translation() - end_effector_pose.translation(); - error.tail(3) = pinocchio::log3(target_pose_.rotation() * - end_effector_pose.rotation().transpose()); + error.head(3) = target_pose_.translation() - end_effector_pose.translation(); + error.tail(3) = + pinocchio::log3(target_pose_.rotation() * end_effector_pose.rotation().transpose()); } if (params_.limit_error) { - max_delta_ << params_.task.error_clip.x, params_.task.error_clip.y, params_.task.error_clip.z, - params_.task.error_clip.rx, params_.task.error_clip.ry, params_.task.error_clip.rz; + max_delta_ << params_.task.error_clip.x, params_.task.error_clip.y, params_.task.error_clip.z, + params_.task.error_clip.rx, params_.task.error_clip.ry, params_.task.error_clip.rz; error = error.cwiseMax(-max_delta_).cwiseMin(max_delta_); } J.setZero(); - auto reference_frame = params_.use_local_jacobian - ? pinocchio::ReferenceFrame::LOCAL - : pinocchio::ReferenceFrame::WORLD; - pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id, - reference_frame, J); + auto reference_frame = params_.use_local_jacobian ? pinocchio::ReferenceFrame::LOCAL + : pinocchio::ReferenceFrame::WORLD; + pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id, reference_frame, J); Eigen::MatrixXd J_pinv(model_.nv, 6); J_pinv = pseudo_inverse(J, params_.nullspace.regularization); @@ -141,14 +142,13 @@ CartesianController::update(const rclcpp::Time &time, } else if (params_.nullspace.projector_type == "none") { nullspace_projection = Eigen::MatrixXd::Identity(model_.nv, model_.nv); } else { - RCLCPP_ERROR_STREAM_ONCE(get_node()->get_logger(), - "Unknown nullspace projector type: " - << params_.nullspace.projector_type); + RCLCPP_ERROR_STREAM_ONCE( + get_node()->get_logger(), + "Unknown nullspace projector type: " << params_.nullspace.projector_type); return controller_interface::return_type::ERROR; } if (params_.use_operational_space) { - pinocchio::computeMinverse(model_, data_, q_pin); auto Mx_inv = J * data_.Minv * J.transpose(); auto Mx = pseudo_inverse(Mx_inv); @@ -159,40 +159,37 @@ CartesianController::update(const rclcpp::Time &time, } if (model_.nq != model_.nv) { - // TODO: Then we have some continouts joints, not being handled for now + // TODO(placeholder): Then we have some continouts joints, not being handled for now tau_joint_limits = Eigen::VectorXd::Zero(model_.nv); } else { - tau_joint_limits = get_joint_limit_torque(q, model_.lowerPositionLimit, - model_.upperPositionLimit); + tau_joint_limits = + get_joint_limit_torque(q, model_.lowerPositionLimit, model_.upperPositionLimit); } - tau_secondary << nullspace_stiffness * (q_ref - q) + - nullspace_damping * (dq_ref - dq); + tau_secondary << nullspace_stiffness * (q_ref - q) + nullspace_damping * (dq_ref - dq); tau_nullspace << nullspace_projection * tau_secondary; - tau_nullspace = tau_nullspace.cwiseMin(params_.nullspace.max_tau) - .cwiseMax(-params_.nullspace.max_tau); - - tau_friction = params_.use_friction ? get_friction(dq, fp1, fp2, fp3) - : Eigen::VectorXd::Zero(model_.nv); + tau_nullspace = + tau_nullspace.cwiseMin(params_.nullspace.max_tau).cwiseMax(-params_.nullspace.max_tau); + tau_friction = + params_.use_friction ? get_friction(dq, fp1, fp2, fp3) : Eigen::VectorXd::Zero(model_.nv); if (params_.use_coriolis_compensation) { pinocchio::computeAllTerms(model_, data_, q_pin, dq); - tau_coriolis = - pinocchio::computeCoriolisMatrix(model_, data_, q_pin, dq) * dq; + tau_coriolis = pinocchio::computeCoriolisMatrix(model_, data_, q_pin, dq) * dq; } else { tau_coriolis = Eigen::VectorXd::Zero(model_.nv); } tau_gravity = params_.use_gravity_compensation - ? pinocchio::computeGeneralizedGravity(model_, data_, q_pin) - : Eigen::VectorXd::Zero(model_.nv); + ? pinocchio::computeGeneralizedGravity(model_, data_, q_pin) + : Eigen::VectorXd::Zero(model_.nv); tau_wrench << J.transpose() * target_wrench_; - tau_d << tau_task + tau_nullspace + tau_friction + tau_coriolis + - tau_gravity + tau_joint_limits + tau_wrench; + tau_d << tau_task + tau_nullspace + tau_friction + tau_coriolis + tau_gravity + tau_joint_limits + + tau_wrench; if (params_.limit_torques) { tau_d = saturateTorqueRate(tau_d, tau_previous, params_.max_delta_tau); @@ -200,7 +197,7 @@ CartesianController::update(const rclcpp::Time &time, /*tau_d = exponential_moving_average(tau_d, tau_previous,*/ /* params_.filter.output_torque);*/ - if (not params_.stop_commands) { + if (!params_.stop_commands) { for (size_t i = 0; i < num_joints; ++i) { command_interfaces_[i].set_value(tau_d[i]); } @@ -217,22 +214,18 @@ CartesianController::update(const rclcpp::Time &time, return controller_interface::return_type::OK; } - CallbackReturn CartesianController::on_init() { - params_listener_ = - std::make_shared( - get_node()); + params_listener_ = std::make_shared(get_node()); params_listener_->refresh_dynamic_parameters(); params_ = params_listener_->get_params(); return CallbackReturn::SUCCESS; } -CallbackReturn CartesianController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { - - auto parameters_client = std::make_shared( - get_node(), "robot_state_publisher"); +CallbackReturn +CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + auto parameters_client = + std::make_shared(get_node(), "robot_state_publisher"); parameters_client->wait_for_service(); auto future = parameters_client->get_parameters({"robot_description"}); @@ -242,8 +235,7 @@ CallbackReturn CartesianController::on_configure( if (!result.empty()) { robot_description_ = result[0].value_to_string(); } else { - RCLCPP_ERROR(get_node()->get_logger(), - "Failed to get robot_description parameter."); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter."); return CallbackReturn::ERROR; } @@ -252,57 +244,55 @@ CallbackReturn CartesianController::on_configure( RCLCPP_INFO(get_node()->get_logger(), "Checking available joints in model:"); for (int joint_id = 0; joint_id < raw_model_.njoints; joint_id++) { - RCLCPP_INFO_STREAM(get_node()->get_logger(), - "Joint " << joint_id << " with name " - << raw_model_.names[joint_id] << " is of type " - << raw_model_.joints[joint_id].shortname()); + RCLCPP_INFO_STREAM( + get_node()->get_logger(), + "Joint " << joint_id << " with name " << raw_model_.names[joint_id] << " is of type " + << raw_model_.joints[joint_id].shortname()); } // First we check that the passed joints exist in the kineatic tree - for (auto &joint : params_.joints) { - if (not raw_model_.existJointName(joint)) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), - "Failed to configure because " - << joint - << " is not part of the kinematic tree but it " - "has been passed in the parameters."); + for (auto & joint : params_.joints) { + if (!raw_model_.existJointName(joint)) { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Failed to configure because " << joint + << " is not part of the kinematic tree but it " + "has been passed in the parameters."); return CallbackReturn::ERROR; } } - RCLCPP_INFO(get_node()->get_logger(), - "All joints passed in the parameters exist in the kinematic tree " - "of the URDF."); - RCLCPP_INFO_STREAM(get_node()->get_logger(), - "Removing the rest of the joints that are not used: "); + RCLCPP_INFO( + get_node()->get_logger(), + "All joints passed in the parameters exist in the kinematic tree " + "of the URDF."); + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Removing the rest of the joints that are not used: "); // Now we fix all joints that are not referenced in the tree std::vector list_of_joints_to_lock_by_id; - for (auto &joint : raw_model_.names) { - if (std::find(params_.joints.begin(), params_.joints.end(), joint) == - params_.joints.end() and - joint != "universe") { + for (auto & joint : raw_model_.names) { + if ( + std::find(params_.joints.begin(), params_.joints.end(), joint) == params_.joints.end() && + joint != "universe") { RCLCPP_INFO_STREAM( - get_node()->get_logger(), - "Joint " << joint << " is not used, removing it from the model."); + get_node()->get_logger(), "Joint " << joint << " is not used, removing it from the model."); list_of_joints_to_lock_by_id.push_back(raw_model_.getJointId(joint)); } } Eigen::VectorXd q_locked = Eigen::VectorXd::Zero(raw_model_.nq); - model_ = pinocchio::buildReducedModel(raw_model_, - list_of_joints_to_lock_by_id, q_locked); + model_ = pinocchio::buildReducedModel(raw_model_, list_of_joints_to_lock_by_id, q_locked); data_ = pinocchio::Data(model_); for (int joint_id = 0; joint_id < model_.njoints; joint_id++) { if (model_.names[joint_id] == "universe") { continue; } - if (not allowed_joint_types.count(model_.joints[joint_id].shortname())) { + if (!allowed_joint_types.count(model_.joints[joint_id].shortname())) { RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "Joint type " - << model_.joints[joint_id].shortname() << " is unsupported (" - << model_.names[joint_id] - << "), only revolute/continous like joints can be used."); + get_node()->get_logger(), + "Joint type " << model_.joints[joint_id].shortname() << " is unsupported (" + << model_.names[joint_id] + << "), only revolute/continous like joints can be used."); return CallbackReturn::ERROR; } } @@ -334,11 +324,13 @@ CallbackReturn CartesianController::on_configure( max_allowed_publishers_ = 1; auto target_pose_callback = - [this](const std::shared_ptr msg) -> void - { + [this](const std::shared_ptr msg) -> void { if (!check_topic_publisher_count("target_pose")) { - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, - "Ignoring target_pose message due to multiple publishers detected!"); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "Ignoring target_pose message due to multiple publishers detected!"); return; } target_pose_buffer_.writeFromNonRT(msg); @@ -346,11 +338,13 @@ CallbackReturn CartesianController::on_configure( }; auto target_joint_callback = - [this](const std::shared_ptr msg) -> void - { + [this](const std::shared_ptr msg) -> void { if (!check_topic_publisher_count("target_joint")) { - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, - "Ignoring target_joint message due to multiple publishers detected!"); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "Ignoring target_joint message due to multiple publishers detected!"); return; } target_joint_buffer_.writeFromNonRT(msg); @@ -358,11 +352,13 @@ CallbackReturn CartesianController::on_configure( }; auto target_wrench_callback = - [this](const std::shared_ptr msg) -> void - { + [this](const std::shared_ptr msg) -> void { if (!check_topic_publisher_count("target_wrench")) { - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 1000, - "Ignoring target_wrench message due to multiple publishers detected!"); + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "Ignoring target_wrench message due to multiple publishers detected!"); return; } target_wrench_buffer_.writeFromNonRT(msg); @@ -370,14 +366,13 @@ CallbackReturn CartesianController::on_configure( }; pose_sub_ = get_node()->create_subscription( - "target_pose", rclcpp::QoS(1),target_pose_callback); + "target_pose", rclcpp::QoS(1), target_pose_callback); joint_sub_ = get_node()->create_subscription( - "target_joint", rclcpp::QoS(1), target_joint_callback); + "target_joint", rclcpp::QoS(1), target_joint_callback); wrench_sub_ = get_node()->create_subscription( - "target_wrench", rclcpp::QoS(1), target_wrench_callback); - + "target_wrench", rclcpp::QoS(1), target_wrench_callback); // Initialize all control vectors with appropriate dimensions tau_task = Eigen::VectorXd::Zero(model_.nv); @@ -409,59 +404,53 @@ CallbackReturn CartesianController::on_configure( } void CartesianController::setStiffnessAndDamping() { - stiffness.setZero(); - stiffness.diagonal() << params_.task.k_pos_x, params_.task.k_pos_y, - params_.task.k_pos_z, params_.task.k_rot_x, params_.task.k_rot_y, - params_.task.k_rot_z; + stiffness.diagonal() << params_.task.k_pos_x, params_.task.k_pos_y, params_.task.k_pos_z, + params_.task.k_rot_x, params_.task.k_rot_y, params_.task.k_rot_z; damping.setZero(); // For each axis, use explicit damping if > 0, otherwise compute from stiffness - damping.diagonal() << - (params_.task.d_pos_x > 0 ? params_.task.d_pos_x : 2.0 * std::sqrt(params_.task.k_pos_x)), - (params_.task.d_pos_y > 0 ? params_.task.d_pos_y : 2.0 * std::sqrt(params_.task.k_pos_y)), - (params_.task.d_pos_z > 0 ? params_.task.d_pos_z : 2.0 * std::sqrt(params_.task.k_pos_z)), - (params_.task.d_rot_x > 0 ? params_.task.d_rot_x : 2.0 * std::sqrt(params_.task.k_rot_x)), - (params_.task.d_rot_y > 0 ? params_.task.d_rot_y : 2.0 * std::sqrt(params_.task.k_rot_y)), - (params_.task.d_rot_z > 0 ? params_.task.d_rot_z : 2.0 * std::sqrt(params_.task.k_rot_z)); + damping.diagonal() + << (params_.task.d_pos_x > 0 ? params_.task.d_pos_x : 2.0 * std::sqrt(params_.task.k_pos_x)), + (params_.task.d_pos_y > 0 ? params_.task.d_pos_y : 2.0 * std::sqrt(params_.task.k_pos_y)), + (params_.task.d_pos_z > 0 ? params_.task.d_pos_z : 2.0 * std::sqrt(params_.task.k_pos_z)), + (params_.task.d_rot_x > 0 ? params_.task.d_rot_x : 2.0 * std::sqrt(params_.task.k_rot_x)), + (params_.task.d_rot_y > 0 ? params_.task.d_rot_y : 2.0 * std::sqrt(params_.task.k_rot_y)), + (params_.task.d_rot_z > 0 ? params_.task.d_rot_z : 2.0 * std::sqrt(params_.task.k_rot_z)); nullspace_stiffness.setZero(); nullspace_damping.setZero(); auto weights = Eigen::VectorXd(model_.nv); for (size_t i = 0; i < params_.joints.size(); ++i) { - weights[i] = - params_.nullspace.weights.joints_map.at(params_.joints.at(i)).value; + weights[i] = params_.nullspace.weights.joints_map.at(params_.joints.at(i)).value; } nullspace_stiffness.diagonal() << params_.nullspace.stiffness * weights; - nullspace_damping.diagonal() - << 2.0 * nullspace_stiffness.diagonal().cwiseSqrt(); + nullspace_damping.diagonal() << 2.0 * nullspace_stiffness.diagonal().cwiseSqrt(); if (params_.nullspace.damping) { - nullspace_damping.diagonal() = params_.nullspace.damping * weights; + nullspace_damping.diagonal() = params_.nullspace.damping * weights; } else { - nullspace_damping.diagonal() = 2.0 * nullspace_stiffness.diagonal().cwiseSqrt(); + nullspace_damping.diagonal() = 2.0 * nullspace_stiffness.diagonal().cwiseSqrt(); } } -CallbackReturn CartesianController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) { +CallbackReturn +CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { auto num_joints = params_.joints.size(); for (size_t i = 0; i < num_joints; i++) { - - // TODO: later it might be better to get this thing prepared in the + // TODO(placeholder): later it might be better to get this thing prepared in the // configuration part (not in the control loop) auto joint_name = params_.joints[i]; - auto joint_id = - model_.getJointId(joint_name); // pinocchio joind id might be different + auto joint_id = model_.getJointId(joint_name); // pinocchio joind id might be different auto joint = model_.joints[joint_id]; q[i] = state_interfaces_[i].get_value(); - if (joint.shortname() == "JointModelRZ") { // simple revolute joint case + if (joint.shortname() == "JointModelRZ") { // simple revolute joint case q_pin[joint.idx_q()] = q[i]; } else if (continous_joint_types.count( - joint.shortname())) { // Then we are handling a continous - // joint that is SO(2) + joint.shortname())) { // Then we are handling a continous + // joint that is SO(2) q_pin[joint.idx_q()] = std::cos(q[i]); q_pin[joint.idx_q() + 1] = std::sin(q[i]); } @@ -479,26 +468,25 @@ CallbackReturn CartesianController::on_activate( target_position_ = end_effector_pose.translation(); target_orientation_ = Eigen::Quaterniond(end_effector_pose.rotation()); - target_pose_ = - pinocchio::SE3(target_orientation_.toRotationMatrix(), target_position_); + target_pose_ = pinocchio::SE3(target_orientation_.toRotationMatrix(), target_position_); RCLCPP_INFO(get_node()->get_logger(), "Controller activated."); return CallbackReturn::SUCCESS; } controller_interface::CallbackReturn -CartesianController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) { +CartesianController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { return CallbackReturn::SUCCESS; } void CartesianController::parse_target_pose_() { auto msg = *target_pose_buffer_.readFromRT(); - target_position_ << msg->pose.position.x, msg->pose.position.y, - msg->pose.position.z; - target_orientation_ = - Eigen::Quaterniond(msg->pose.orientation.w, msg->pose.orientation.x, - msg->pose.orientation.y, msg->pose.orientation.z); + target_position_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z; + target_orientation_ = Eigen::Quaterniond( + msg->pose.orientation.w, + msg->pose.orientation.x, + msg->pose.orientation.y, + msg->pose.orientation.z); } void CartesianController::parse_target_joint_() { @@ -520,155 +508,180 @@ void CartesianController::parse_target_joint_() { void CartesianController::parse_target_wrench_() { auto msg = *target_wrench_buffer_.readFromRT(); target_wrench_ << msg->wrench.force.x, msg->wrench.force.y, msg->wrench.force.z, - msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z; + msg->wrench.torque.x, msg->wrench.torque.y, msg->wrench.torque.z; } -void CartesianController::log_debug_info(const rclcpp::Time &time) { +void CartesianController::log_debug_info(const rclcpp::Time & time) { if (!params_.log.enabled) { return; } if (params_.log.robot_state) { - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "nq: " << model_.nq << ", nv: " << model_.nv); - - RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, - "end_effector_pos" << end_effector_pose.translation()); - /*RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "nq: " << model_.nq << ", nv: " << model_.nv); + + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "end_effector_pos" << end_effector_pose.translation()); + /*RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), * *get_node()->get_clock(),*/ - /* 1000, "end_effector_rot" << + /* 1000, "end_effector_rot" << * end_effector_pose.rotation());*/ - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "q: " << q.transpose()); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "q_pin: " << q_pin.transpose()); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "dq: " << dq.transpose()); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, "J: " << J); - } + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "q: " << q.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "q_pin: " << q_pin.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "dq: " << dq.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "J: " << J); + } - if (params_.log.control_values) { - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "error: " << error.transpose()); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "max_delta: " << max_delta_.transpose()); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "q_ref: " << q_ref.transpose()); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "dq_ref: " << dq_ref.transpose()); - } + if (params_.log.control_values) { + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "error: " << error.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "max_delta: " << max_delta_.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "q_ref: " << q_ref.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "dq_ref: " << dq_ref.transpose()); + } - if (params_.log.controller_parameters) { - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "stiffness: " << stiffness); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "damping: " << damping); - RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, - "nullspace_stiffness: " << nullspace_stiffness); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "nullspace_damping: " << nullspace_damping); - } + if (params_.log.controller_parameters) { + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "stiffness: " << stiffness); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "damping: " << damping); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "nullspace_stiffness: " << nullspace_stiffness); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "nullspace_damping: " << nullspace_damping); + } - if (params_.log.limits) { - RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, - "joint_limits: " << model_.lowerPositionLimit.transpose() << ", " - << model_.upperPositionLimit.transpose()); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "velocity_limits: " << model_.velocityLimit); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "effort_limits: " << model_.effortLimit); - } + if (params_.log.limits) { + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "joint_limits: " << model_.lowerPositionLimit.transpose() << ", " + << model_.upperPositionLimit.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "velocity_limits: " << model_.velocityLimit); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "effort_limits: " << model_.effortLimit); + } - if (params_.log.computed_torques) { - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "tau_task: " << tau_task.transpose()); - RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, - "tau_joint_limits: " << tau_joint_limits.transpose()); - RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, - "tau_nullspace: " << tau_nullspace.transpose()); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "tau_friction: " << tau_friction.transpose()); - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "tau_coriolis: " << tau_coriolis.transpose()); - } + if (params_.log.computed_torques) { + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "tau_task: " << tau_task.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "tau_joint_limits: " << tau_joint_limits.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "tau_nullspace: " << tau_nullspace.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "tau_friction: " << tau_friction.transpose()); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "tau_coriolis: " << tau_coriolis.transpose()); + } - if (params_.log.dynamic_params) { - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "M: " << data_.M); - /*RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), + if (params_.log.dynamic_params) { + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "M: " << data_.M); + /*RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), * *get_node()->get_clock(),*/ - /* 1000, "Mx: " << Mx);*/ - RCLCPP_INFO_STREAM_THROTTLE(get_node()->get_logger(), - *get_node()->get_clock(), 1000, - "Minv: " << data_.Minv); - RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 1000, - "nullspace projector: " << nullspace_projection); - } - - if (params_.log.timing) { + /* 1000, "Mx: " << Mx);*/ + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), *get_node()->get_clock(), 1000, "Minv: " << data_.Minv); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "nullspace projector: " << nullspace_projection); + } - auto t_end = get_node()->get_clock()->now(); - RCLCPP_INFO_STREAM_THROTTLE( - get_node()->get_logger(), *get_node()->get_clock(), 2000, - "Control loop needed: " - << (t_end.nanoseconds() - time.nanoseconds()) * 1e-6 << " ms"); - } + if (params_.log.timing) { + auto t_end = get_node()->get_clock()->now(); + RCLCPP_INFO_STREAM_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 2000, + "Control loop needed: " << (t_end.nanoseconds() - time.nanoseconds()) * 1e-6 << " ms"); + } } -bool CartesianController::check_topic_publisher_count(const std::string& topic_name) { +bool CartesianController::check_topic_publisher_count(const std::string & topic_name) { auto topic_info = get_node()->get_publishers_info_by_topic(topic_name); size_t publisher_count = topic_info.size(); - + if (publisher_count > max_allowed_publishers_) { - RCLCPP_WARN_THROTTLE(get_node()->get_logger(), *get_node()->get_clock(), 2000, - "Topic '%s' has %zu publishers (expected max: %zu). Multiple command sources detected!", - topic_name.c_str(), publisher_count, max_allowed_publishers_); - + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 2000, + "Topic '%s' has %zu publishers (expected max: %zu). Multiple command sources detected!", + topic_name.c_str(), + publisher_count, + max_allowed_publishers_); + if (!multiple_publishers_detected_) { - RCLCPP_ERROR(get_node()->get_logger(), - "SAFETY WARNING: Multiple publishers detected on topic '%s'! " - "Ignoring commands from this topic to prevent conflicting control signals.", - topic_name.c_str()); + RCLCPP_ERROR( + get_node()->get_logger(), + "SAFETY WARNING: Multiple publishers detected on topic '%s'! " + "Ignoring commands from this topic to prevent conflicting control signals.", + topic_name.c_str()); multiple_publishers_detected_ = true; } return false; } - + if (multiple_publishers_detected_ && publisher_count <= max_allowed_publishers_) { - RCLCPP_INFO(get_node()->get_logger(), - "Publisher conflict resolved on topic '%s'. Resuming message processing.", - topic_name.c_str()); + RCLCPP_INFO( + get_node()->get_logger(), + "Publisher conflict resolved on topic '%s'. Resuming message processing.", + topic_name.c_str()); multiple_publishers_detected_ = false; } - + return true; } -} // namespace crisp_controllers +} // namespace crisp_controllers #include "pluginlib/class_list_macros.hpp" // NOLINTNEXTLINE -PLUGINLIB_EXPORT_CLASS(crisp_controllers::CartesianController, - controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + crisp_controllers::CartesianController, controller_interface::ControllerInterface) diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp index a79b332..92b467a 100644 --- a/src/pose_broadcaster.cpp +++ b/src/pose_broadcaster.cpp @@ -1,11 +1,14 @@ -#include -#include +// "Copyright [2026] " -#include #include #include #include #include + +#include // NOLINT(build/include_order) +#include + +#include #include #include #include @@ -27,29 +30,27 @@ controller_interface::InterfaceConfiguration PoseBroadcaster::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : params_.joints) { + for (const auto & joint_name : params_.joints) { config.names.push_back(joint_name + "/position"); } return config; } controller_interface::return_type -PoseBroadcaster::update(const rclcpp::Time &time, - const rclcpp::Duration &period) { - +PoseBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & period) { size_t num_joints = params_.joints.size(); Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq); for (size_t i = 0; i < num_joints; i++) { - auto joint_name = params_.joints[i]; auto joint_id = model_.getJointId(joint_name); auto joint = model_.joints[joint_id]; q[i] = state_interfaces_[i].get_value(); - if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous joint that is SO(2) + if (continous_joint_types.count( + joint.shortname())) { // Then we are handling a continous joint that is SO(2) q_pin[joint.idx_q()] = std::cos(q[i]); - q_pin[joint.idx_q()+1] = std::sin(q[i]); + q_pin[joint.idx_q() + 1] = std::sin(q[i]); } else { q_pin[joint.idx_q()] = q[i]; } @@ -59,15 +60,13 @@ PoseBroadcaster::update(const rclcpp::Time &time, pinocchio::updateFramePlacements(model_, data_); auto current_pose = data_.oMf[end_effector_frame_id]; - auto current_quaternion = - Eigen::Quaterniond(current_pose.rotation()); + auto current_quaternion = Eigen::Quaterniond(current_pose.rotation()); // Decide whether to publish the pose or not publish_elapsed_ = publish_elapsed_ + period; - bool should_publish = (publish_elapsed_ >= publish_interval_) || - (publish_interval_.nanoseconds() == 0); - if (should_publish && realtime_pose_publisher_) - { + bool should_publish = + (publish_elapsed_ >= publish_interval_) || (publish_interval_.nanoseconds() == 0); + if (should_publish && realtime_pose_publisher_) { if (realtime_pose_publisher_->trylock()) { auto & pose_msg = realtime_pose_publisher_->msg_; pose_msg.header.stamp = time; @@ -80,10 +79,10 @@ PoseBroadcaster::update(const rclcpp::Time &time, pose_msg.pose.orientation.z = current_quaternion.z(); pose_msg.pose.orientation.w = current_quaternion.w(); realtime_pose_publisher_->unlockAndPublish(); - + publish_elapsed_ = publish_elapsed_ - publish_interval_; // clamp to publish only 1 time even if missed multiple intervals - publish_elapsed_ = std::min(publish_elapsed_, publish_interval_); + publish_elapsed_ = std::min(publish_elapsed_, publish_interval_); } } @@ -92,19 +91,16 @@ PoseBroadcaster::update(const rclcpp::Time &time, CallbackReturn PoseBroadcaster::on_init() { // Initialize parameters - params_listener_ = - std::make_shared(get_node()); + params_listener_ = std::make_shared(get_node()); params_listener_->refresh_dynamic_parameters(); params_ = params_listener_->get_params(); return CallbackReturn::SUCCESS; } -CallbackReturn PoseBroadcaster::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { - - auto parameters_client = std::make_shared( - get_node(), "robot_state_publisher"); +CallbackReturn PoseBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + auto parameters_client = + std::make_shared(get_node(), "robot_state_publisher"); parameters_client->wait_for_service(); auto future = parameters_client->get_parameters({"robot_description"}); @@ -114,33 +110,45 @@ CallbackReturn PoseBroadcaster::on_configure( if (!result.empty()) { robot_description_ = result[0].value_to_string(); } else { - RCLCPP_ERROR(get_node()->get_logger(), - "Failed to get robot_description parameter."); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter."); return CallbackReturn::ERROR; } pinocchio::Model raw_model_; pinocchio::urdf::buildModelFromXML(robot_description_, raw_model_); - RCLCPP_INFO(get_node()->get_logger(), "Checking available joints in model:"); + RCLCPP_INFO(get_node()->get_logger(), "Checking available joints in model:"); for (int joint_id = 0; joint_id < raw_model_.njoints; joint_id++) { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Joint " << joint_id << " with name " << raw_model_.names[joint_id] << " is of type " << raw_model_.joints[joint_id].shortname()); + RCLCPP_INFO_STREAM( + get_node()->get_logger(), + "Joint " << joint_id << " with name " << raw_model_.names[joint_id] << " is of type " + << raw_model_.joints[joint_id].shortname()); } // First we check that the passed joints exist in the kineatic tree - for (auto& joint : params_.joints) { - if (not raw_model_.existJointName(joint)) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Failed to configure because " << joint << " is not part of the kinematic tree but it has been passed in the parameters."); + for (auto & joint : params_.joints) { + if (!raw_model_.existJointName(joint)) { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Failed to configure because " + << joint + << " is not part of the kinematic tree but it has been passed in the parameters."); return CallbackReturn::ERROR; } } - RCLCPP_INFO(get_node()->get_logger(), "All joints passed in the parameters exist in the kinematic tree of the URDF."); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Removing the rest of the joints that are not used: "); + RCLCPP_INFO( + get_node()->get_logger(), + "All joints passed in the parameters exist in the kinematic tree of the URDF."); + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Removing the rest of the joints that are not used: "); // Now we fix all joints that are not referenced in the tree std::vector list_of_joints_to_lock_by_id; - for (auto& joint : raw_model_.names) { - if (std::find(params_.joints.begin(), params_.joints.end(), joint) == params_.joints.end() and joint != "universe") { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Joint " << joint << " is not used, removing it from the model."); + for (auto & joint : raw_model_.names) { + if ( + std::find(params_.joints.begin(), params_.joints.end(), joint) == params_.joints.end() && + joint != "universe") { + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Joint " << joint << " is not used, removing it from the model."); list_of_joints_to_lock_by_id.push_back(raw_model_.getJointId(joint)); } } @@ -153,44 +161,47 @@ CallbackReturn PoseBroadcaster::on_configure( if (model_.names[joint_id] == "universe") { continue; } - if (not allowed_joint_types.count(model_.joints[joint_id].shortname())) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Joint type " << model_.joints[joint_id].shortname() << " is unsupported (" << model_.names[joint_id] << "), only revolute/continous like joints can be used."); - return CallbackReturn::ERROR; - } + if (!allowed_joint_types.count(model_.joints[joint_id].shortname())) { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Joint type " << model_.joints[joint_id].shortname() << " is unsupported (" + << model_.names[joint_id] + << "), only revolute/continous like joints can be used."); + return CallbackReturn::ERROR; + } } end_effector_frame_id = model_.getFrameId(params_.end_effector_frame); q = Eigen::VectorXd::Zero(model_.nv); pose_publisher_ = get_node()->create_publisher( - "current_pose", rclcpp::SystemDefaultsQoS()); + "current_pose", rclcpp::SystemDefaultsQoS()); realtime_pose_publisher_ = - std::make_shared>( - pose_publisher_); - + std::make_shared>( + pose_publisher_); + if (params_.publish_frequency > 0.0) { publish_interval_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); } else { publish_interval_ = rclcpp::Duration(0, 0); // publish every cycle } - + return CallbackReturn::SUCCESS; } -CallbackReturn PoseBroadcaster::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) { +CallbackReturn PoseBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { // reset publish time accumulation - publish_elapsed_ = rclcpp::Duration(0, 0); + publish_elapsed_ = rclcpp::Duration(0, 0); return CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn PoseBroadcaster::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +PoseBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { return CallbackReturn::SUCCESS; } -} // namespace crisp_controllers +} // namespace crisp_controllers #include "pluginlib/class_list_macros.hpp" // NOLINTNEXTLINE -PLUGINLIB_EXPORT_CLASS(crisp_controllers::PoseBroadcaster, - controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + crisp_controllers::PoseBroadcaster, controller_interface::ControllerInterface) diff --git a/src/torque_feedback_controller.cpp b/src/torque_feedback_controller.cpp index 2535a8f..bd9d443 100644 --- a/src/torque_feedback_controller.cpp +++ b/src/torque_feedback_controller.cpp @@ -1,19 +1,23 @@ -#include -#include -#include +// "Copyright [2026] " #include #include #include -#include -#include + +// NOLINTBEGIN(build/include_order) +#include +#include +#include + +#include +#include #include #include -#include -#include #include #include - +#include +#include +// NOLINTEND(build/include_order) namespace crisp_controllers { @@ -21,7 +25,7 @@ controller_interface::InterfaceConfiguration TorqueFeedbackController::command_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : joint_names_) { + for (const auto & joint_name : joint_names_) { config.names.push_back(joint_name + "/effort"); } return config; @@ -31,21 +35,20 @@ controller_interface::InterfaceConfiguration TorqueFeedbackController::state_interface_configuration() const { controller_interface::InterfaceConfiguration config; config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : joint_names_) { + for (const auto & joint_name : joint_names_) { config.names.push_back(joint_name + "/position"); } - for (const auto &joint_name : joint_names_) { + for (const auto & joint_name : joint_names_) { config.names.push_back(joint_name + "/velocity"); } - for (const auto &joint_name : joint_names_) { + for (const auto & joint_name : joint_names_) { config.names.push_back(joint_name + "/effort"); } return config; } -controller_interface::return_type -TorqueFeedbackController::update(const rclcpp::Time & /*time*/, - const rclcpp::Duration & /*period*/) { +controller_interface::return_type TorqueFeedbackController::update( + const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { // Update joint states for (int i = 0; i < num_joints_; i++) { q_[i] = state_interfaces_[i].get_value(); @@ -57,8 +60,8 @@ TorqueFeedbackController::update(const rclcpp::Time & /*time*/, pinocchio::updateFramePlacements(model_, data_); J_.setZero(); - pinocchio::computeFrameJacobian(model_, data_, q_, end_effector_frame_id_, - pinocchio::ReferenceFrame::LOCAL, J_); + pinocchio::computeFrameJacobian( + model_, data_, q_, end_effector_frame_id_, pinocchio::ReferenceFrame::LOCAL, J_); // Apply threshold to external torques based on vector magnitude Eigen::VectorXd tau_ext_thresholded = Eigen::VectorXd::Zero(num_joints_); @@ -69,10 +72,13 @@ TorqueFeedbackController::update(const rclcpp::Time & /*time*/, // Compute nullspace control to maintain initial joint positions Eigen::VectorXd q_error = q_ - q_init_; - double nullspace_damping = params_.nullspace.damping > 0 ? params_.nullspace.damping : 2.0 * sqrt(params_.nullspace.stiffness); + double nullspace_damping = params_.nullspace.damping > 0 + ? params_.nullspace.damping + : 2.0 * sqrt(params_.nullspace.stiffness); - Eigen::VectorXd tau_secondary = -params_.nullspace.stiffness * nullspace_weights_.cwiseProduct(q_error) - - nullspace_damping * nullspace_weights_.cwiseProduct(dq_); + Eigen::VectorXd tau_secondary = + -params_.nullspace.stiffness * nullspace_weights_.cwiseProduct(q_error) - + nullspace_damping * nullspace_weights_.cwiseProduct(dq_); // Compute nullspace projection based on projector type Eigen::MatrixXd Id_nv = Eigen::MatrixXd::Identity(model_.nv, model_.nv); @@ -88,20 +94,21 @@ TorqueFeedbackController::update(const rclcpp::Time & /*time*/, } else if (params_.nullspace.projector_type == "none") { nullspace_projection_ = Id_nv; } else { - RCLCPP_ERROR_STREAM_ONCE(get_node()->get_logger(), - "Unknown nullspace projector type: " - << params_.nullspace.projector_type); + RCLCPP_ERROR_STREAM_ONCE( + get_node()->get_logger(), + "Unknown nullspace projector type: " << params_.nullspace.projector_type); return controller_interface::return_type::ERROR; } - + // Apply nullspace projection Eigen::VectorXd tau_nullspace = nullspace_projection_ * tau_secondary; - + // Limit nullspace torques for (int i = 0; i < num_joints_; i++) { - tau_nullspace[i] = std::max(-params_.nullspace.max_tau, std::min(params_.nullspace.max_tau, tau_nullspace[i])); + tau_nullspace[i] = + std::max(-params_.nullspace.max_tau, std::min(params_.nullspace.max_tau, tau_nullspace[i])); } - + auto tau_d = -params_.k_fb * tau_ext_thresholded - params_.kd * dq_; auto tau_f = get_friction(dq_, friction_fp1_, friction_fp2_, friction_fp3_); @@ -114,19 +121,18 @@ TorqueFeedbackController::update(const rclcpp::Time & /*time*/, params_listener_->refresh_dynamic_parameters(); params_ = params_listener_->get_params(); - + // Update nullspace weights for (size_t i = 0; i < joint_names_.size(); ++i) { nullspace_weights_[i] = params_.nullspace.weights.joints_map.at(joint_names_[i]).value; } - + return controller_interface::return_type::OK; } CallbackReturn TorqueFeedbackController::on_init() { // Initialize parameters - params_listener_ = - std::make_shared(get_node()); + params_listener_ = std::make_shared(get_node()); params_listener_->refresh_dynamic_parameters(); params_ = params_listener_->get_params(); @@ -147,27 +153,28 @@ CallbackReturn TorqueFeedbackController::on_init() { nullspace_weights_[i] = params_.nullspace.weights.joints_map.at(joint_names_[i]).value; } - friction_fp1_ = Eigen::Map(params_.friction.fp1.data(), params_.friction.fp1.size()); - friction_fp2_ = Eigen::Map(params_.friction.fp2.data(), params_.friction.fp2.size()); - friction_fp3_ = Eigen::Map(params_.friction.fp3.data(), params_.friction.fp3.size()); + friction_fp1_ = + Eigen::Map(params_.friction.fp1.data(), params_.friction.fp1.size()); + friction_fp2_ = + Eigen::Map(params_.friction.fp2.data(), params_.friction.fp2.size()); + friction_fp3_ = + Eigen::Map(params_.friction.fp3.data(), params_.friction.fp3.size()); nullspace_projection_ = Eigen::MatrixXd::Identity(num_joints_, num_joints_); joint_sub_ = get_node()->create_subscription( - params_.joint_source_topic, - rclcpp::QoS(1).best_effort().keep_last(1).durability_volatile(), - std::bind(&TorqueFeedbackController::target_joint_callback_, this, - std::placeholders::_1)); + params_.joint_source_topic, + rclcpp::QoS(1).best_effort().keep_last(1).durability_volatile(), + std::bind(&TorqueFeedbackController::target_joint_callback_, this, std::placeholders::_1)); return CallbackReturn::SUCCESS; } -CallbackReturn TorqueFeedbackController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { - +CallbackReturn +TorqueFeedbackController::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { // Get robot description for pinocchio model - auto parameters_client = std::make_shared( - get_node(), "robot_state_publisher"); + auto parameters_client = + std::make_shared(get_node(), "robot_state_publisher"); parameters_client->wait_for_service(); auto future = parameters_client->get_parameters({"robot_description"}); @@ -177,76 +184,74 @@ CallbackReturn TorqueFeedbackController::on_configure( if (!result.empty()) { robot_description_ = result[0].value_to_string(); } else { - RCLCPP_ERROR(get_node()->get_logger(), - "Failed to get robot_description parameter."); + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter."); return CallbackReturn::ERROR; } pinocchio::Model raw_model_; pinocchio::urdf::buildModelFromXML(robot_description_, raw_model_); - for (auto &joint : joint_names_) { - if (not raw_model_.existJointName(joint)) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), - "Failed to configure because " - << joint - << " is not part of the kinematic tree but it " - "has been passed in the parameters."); + for (auto & joint : joint_names_) { + if (!raw_model_.existJointName(joint)) { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Failed to configure because " << joint + << " is not part of the kinematic tree but it " + "has been passed in the parameters."); return CallbackReturn::ERROR; } } // Remove unused joints from the model std::vector list_of_joints_to_lock_by_id; - for (auto &joint : raw_model_.names) { - if (std::find(joint_names_.begin(), joint_names_.end(), joint) == - joint_names_.end() and - joint != "universe") { + for (auto & joint : raw_model_.names) { + if ( + std::find(joint_names_.begin(), joint_names_.end(), joint) == joint_names_.end() && + joint != "universe") { list_of_joints_to_lock_by_id.push_back(raw_model_.getJointId(joint)); } } Eigen::VectorXd q_locked = Eigen::VectorXd::Zero(raw_model_.nq); - model_ = pinocchio::buildReducedModel(raw_model_, - list_of_joints_to_lock_by_id, q_locked); + model_ = pinocchio::buildReducedModel(raw_model_, list_of_joints_to_lock_by_id, q_locked); data_ = pinocchio::Data(model_); - std::set allowed_joint_types = {"JointModelRZ", "JointModelRevoluteUnaligned", "JointModelRX", "JointModelRY"}; + std::set allowed_joint_types = { + "JointModelRZ", "JointModelRevoluteUnaligned", "JointModelRX", "JointModelRY"}; for (int joint_id = 0; joint_id < model_.njoints; joint_id++) { if (model_.names[joint_id] == "universe") { continue; } - if (not allowed_joint_types.count(model_.joints[joint_id].shortname())) { + if (!allowed_joint_types.count(model_.joints[joint_id].shortname())) { RCLCPP_ERROR_STREAM( - get_node()->get_logger(), - "Joint type " - << model_.joints[joint_id].shortname() << " is unsupported (" - << model_.names[joint_id] - << "). Continuous joints are not implemented yet for torque feedback controller. " - << "Only revolute joints are supported."); + get_node()->get_logger(), + "Joint type " + << model_.joints[joint_id].shortname() << " is unsupported (" << model_.names[joint_id] + << "). Continuous joints are not implemented yet for torque feedback controller. " + << "Only revolute joints are supported."); return CallbackReturn::ERROR; } } // Get end-effector frame ID end_effector_frame_id_ = model_.getFrameId(params_.end_effector_frame); - + // Initialize jacobian matrix J_ = Eigen::MatrixXd::Zero(6, model_.nv); - + wrench_pub_ = get_node()->create_publisher( - "~/commanded_wrench", rclcpp::QoS(10)); - + "~/commanded_wrench", rclcpp::QoS(10)); + wrench_timer_ = get_node()->create_wall_timer( - std::chrono::milliseconds(5), - std::bind(&TorqueFeedbackController::publish_wrench_callback_, this)); + std::chrono::milliseconds(5), + std::bind(&TorqueFeedbackController::publish_wrench_callback_, this)); return CallbackReturn::SUCCESS; } -CallbackReturn TorqueFeedbackController::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) { +CallbackReturn +TorqueFeedbackController::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { for (int i = 0; i < num_joints_; i++) { q_[i] = state_interfaces_[i].get_value(); dq_[i] = state_interfaces_[num_joints_ + i].get_value(); @@ -257,15 +262,15 @@ CallbackReturn TorqueFeedbackController::on_activate( return CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn TorqueFeedbackController::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) { +controller_interface::CallbackReturn +TorqueFeedbackController::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { return CallbackReturn::SUCCESS; } void TorqueFeedbackController::target_joint_callback_( - const sensor_msgs::msg::JointState::SharedPtr msg) { + const sensor_msgs::msg::JointState::SharedPtr msg) { for (size_t i = 0; i < msg->effort.size(); i++) { - if ((int)i < num_joints_) { + if (static_cast(i) < num_joints_) { tau_ext_[i] = msg->effort[i]; } } @@ -275,25 +280,25 @@ void TorqueFeedbackController::publish_wrench_callback_() { // Convert joint torques to Cartesian wrench using Jacobian transpose // F = J^T * tau Eigen::VectorXd wrench_commanded = J_.transpose() * tau_commanded_; - + // Create and publish wrench message auto wrench_msg = geometry_msgs::msg::WrenchStamped(); wrench_msg.header.stamp = get_node()->get_clock()->now(); wrench_msg.header.frame_id = params_.end_effector_frame; - + wrench_msg.wrench.force.x = wrench_commanded[0]; wrench_msg.wrench.force.y = wrench_commanded[1]; wrench_msg.wrench.force.z = wrench_commanded[2]; wrench_msg.wrench.torque.x = wrench_commanded[3]; wrench_msg.wrench.torque.y = wrench_commanded[4]; wrench_msg.wrench.torque.z = wrench_commanded[5]; - + wrench_pub_->publish(wrench_msg); } -} // namespace crisp_controllers +} // namespace crisp_controllers #include "pluginlib/class_list_macros.hpp" // NOLINTNEXTLINE -PLUGINLIB_EXPORT_CLASS(crisp_controllers::TorqueFeedbackController, - controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + crisp_controllers::TorqueFeedbackController, controller_interface::ControllerInterface) diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp index 51e8fa7..99f1caf 100644 --- a/src/twist_broadcaster.cpp +++ b/src/twist_broadcaster.cpp @@ -1,11 +1,14 @@ -#include -#include +// "Copyright [2026] " -#include #include #include #include #include + +#include // NOLINT(build/include_order) +#include + +#include #include #include #include @@ -18,184 +21,193 @@ namespace crisp_controllers { controller_interface::InterfaceConfiguration TwistBroadcaster::command_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::NONE; - return config; + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::NONE; + return config; } controller_interface::InterfaceConfiguration TwistBroadcaster::state_interface_configuration() const { - controller_interface::InterfaceConfiguration config; - config.type = controller_interface::interface_configuration_type::INDIVIDUAL; - for (const auto &joint_name : params_.joints) { - config.names.push_back(joint_name + "/position"); - config.names.push_back(joint_name + "/velocity"); - } - return config; + controller_interface::InterfaceConfiguration config; + config.type = controller_interface::interface_configuration_type::INDIVIDUAL; + for (const auto & joint_name : params_.joints) { + config.names.push_back(joint_name + "/position"); + config.names.push_back(joint_name + "/velocity"); + } + return config; } controller_interface::return_type -TwistBroadcaster::update(const rclcpp::Time &time, - const rclcpp::Duration &period) { - - size_t num_joints = params_.joints.size(); - Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq); - Eigen::VectorXd q_dot_pin = Eigen::VectorXd::Zero(model_.nv); - - for (size_t i = 0; i < num_joints; i++) { - - auto joint_name = params_.joints[i]; - auto joint_id = model_.getJointId(joint_name); - auto joint = model_.joints[joint_id]; - - q[i] = state_interfaces_[i*2].get_value(); - q_dot[i] = state_interfaces_[i*2+1].get_value(); - - if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous joint that is SO(2) - q_pin[joint.idx_q()] = std::cos(q[i]); - q_pin[joint.idx_q()+1] = std::sin(q[i]); - q_dot_pin[joint.idx_v()] = q_dot[i]; - } else { - q_pin[joint.idx_q()] = q[i]; - q_dot_pin[joint.idx_v()] = q_dot[i]; - } +TwistBroadcaster::update(const rclcpp::Time & time, const rclcpp::Duration & period) { + size_t num_joints = params_.joints.size(); + Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq); + Eigen::VectorXd q_dot_pin = Eigen::VectorXd::Zero(model_.nv); + + for (size_t i = 0; i < num_joints; i++) { + auto joint_name = params_.joints[i]; + auto joint_id = model_.getJointId(joint_name); + auto joint = model_.joints[joint_id]; + + q[i] = state_interfaces_[i * 2].get_value(); + q_dot[i] = state_interfaces_[i * 2 + 1].get_value(); + + if (continous_joint_types.count( + joint.shortname())) { // Then we are handling a continous joint that is SO(2) + q_pin[joint.idx_q()] = std::cos(q[i]); + q_pin[joint.idx_q() + 1] = std::sin(q[i]); + q_dot_pin[joint.idx_v()] = q_dot[i]; + } else { + q_pin[joint.idx_q()] = q[i]; + q_dot_pin[joint.idx_v()] = q_dot[i]; } - - pinocchio::forwardKinematics(model_, data_, q_pin, q_dot_pin); - pinocchio::updateFramePlacements(model_, data_); - - auto current_velocity = pinocchio::getFrameVelocity(model_, data_, end_effector_frame_id); - - // Decide whether to publish the twist or not - publish_elapsed_ = publish_elapsed_ + period; - bool should_publish = (publish_elapsed_ >= publish_interval_) || - (publish_interval_.nanoseconds() == 0); - - if (should_publish && realtime_twist_publisher_) - { - if (realtime_twist_publisher_->trylock()) { - auto & twist_msg = realtime_twist_publisher_->msg_; - twist_msg.header.stamp = time; - twist_msg.header.frame_id = params_.end_effector_frame; - twist_msg.twist.linear.x = current_velocity.linear()[0]; - twist_msg.twist.linear.y = current_velocity.linear()[1]; - twist_msg.twist.linear.z = current_velocity.linear()[2]; - twist_msg.twist.angular.x = current_velocity.angular()[0]; - twist_msg.twist.angular.y = current_velocity.angular()[1]; - twist_msg.twist.angular.z = current_velocity.angular()[2]; - realtime_twist_publisher_->unlockAndPublish(); - - publish_elapsed_ = publish_elapsed_ - publish_interval_; - // clamp to publish only 1 time even if missed multiple intervals - publish_elapsed_ = std::min(publish_elapsed_, publish_interval_); - } + } + + pinocchio::forwardKinematics(model_, data_, q_pin, q_dot_pin); + pinocchio::updateFramePlacements(model_, data_); + + auto current_velocity = pinocchio::getFrameVelocity(model_, data_, end_effector_frame_id); + + // Decide whether to publish the twist or not + publish_elapsed_ = publish_elapsed_ + period; + bool should_publish = + (publish_elapsed_ >= publish_interval_) || (publish_interval_.nanoseconds() == 0); + + if (should_publish && realtime_twist_publisher_) { + if (realtime_twist_publisher_->trylock()) { + auto & twist_msg = realtime_twist_publisher_->msg_; + twist_msg.header.stamp = time; + twist_msg.header.frame_id = params_.end_effector_frame; + twist_msg.twist.linear.x = current_velocity.linear()[0]; + twist_msg.twist.linear.y = current_velocity.linear()[1]; + twist_msg.twist.linear.z = current_velocity.linear()[2]; + twist_msg.twist.angular.x = current_velocity.angular()[0]; + twist_msg.twist.angular.y = current_velocity.angular()[1]; + twist_msg.twist.angular.z = current_velocity.angular()[2]; + realtime_twist_publisher_->unlockAndPublish(); + + publish_elapsed_ = publish_elapsed_ - publish_interval_; + // clamp to publish only 1 time even if missed multiple intervals + publish_elapsed_ = std::min(publish_elapsed_, publish_interval_); } + } - return controller_interface::return_type::OK; + return controller_interface::return_type::OK; } CallbackReturn TwistBroadcaster::on_init() { - // Initialize parameters - params_listener_ = - std::make_shared(get_node()); - params_listener_->refresh_dynamic_parameters(); - params_ = params_listener_->get_params(); + // Initialize parameters + params_listener_ = std::make_shared(get_node()); + params_listener_->refresh_dynamic_parameters(); + params_ = params_listener_->get_params(); - return CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } -CallbackReturn TwistBroadcaster::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { - - auto parameters_client = std::make_shared( - get_node(), "robot_state_publisher"); - parameters_client->wait_for_service(); - - auto future = parameters_client->get_parameters({"robot_description"}); - auto result = future.get(); - - std::string robot_description_; - if (!result.empty()) { - robot_description_ = result[0].value_to_string(); - } else { - RCLCPP_ERROR(get_node()->get_logger(), - "Failed to get robot_description parameter."); - return CallbackReturn::ERROR; +CallbackReturn TwistBroadcaster::on_configure(const rclcpp_lifecycle::State & /*previous_state*/) { + auto parameters_client = + std::make_shared(get_node(), "robot_state_publisher"); + parameters_client->wait_for_service(); + + auto future = parameters_client->get_parameters({"robot_description"}); + auto result = future.get(); + + std::string robot_description_; + if (!result.empty()) { + robot_description_ = result[0].value_to_string(); + } else { + RCLCPP_ERROR(get_node()->get_logger(), "Failed to get robot_description parameter."); + return CallbackReturn::ERROR; + } + + pinocchio::Model raw_model_; + pinocchio::urdf::buildModelFromXML(robot_description_, raw_model_); + + RCLCPP_INFO(get_node()->get_logger(), "Checking available joints in model:"); + for (int joint_id = 0; joint_id < raw_model_.njoints; joint_id++) { + RCLCPP_INFO_STREAM( + get_node()->get_logger(), + "Joint " << joint_id << " with name " << raw_model_.names[joint_id] << " is of type " + << raw_model_.joints[joint_id].shortname()); + } + + // First we check that the passed joints exist in the kineatic tree + for (auto & joint : params_.joints) { + if (!raw_model_.existJointName(joint)) { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Failed to configure because " + << joint + << " is not part of the kinematic tree but it has been passed in the parameters."); + return CallbackReturn::ERROR; } - - pinocchio::Model raw_model_; - pinocchio::urdf::buildModelFromXML(robot_description_, raw_model_); - - RCLCPP_INFO(get_node()->get_logger(), "Checking available joints in model:"); - for (int joint_id = 0; joint_id < raw_model_.njoints; joint_id++) { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Joint " << joint_id << " with name " << raw_model_.names[joint_id] << " is of type " << raw_model_.joints[joint_id].shortname()); + } + RCLCPP_INFO( + get_node()->get_logger(), + "All joints passed in the parameters exist in the kinematic tree of the URDF."); + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Removing the rest of the joints that are not used: "); + // Now we fix all joints that are not referenced in the tree + std::vector list_of_joints_to_lock_by_id; + for (auto & joint : raw_model_.names) { + if ( + std::find(params_.joints.begin(), params_.joints.end(), joint) == params_.joints.end() && + joint != "universe") { + RCLCPP_INFO_STREAM( + get_node()->get_logger(), "Joint " << joint << " is not used, removing it from the model."); + list_of_joints_to_lock_by_id.push_back(raw_model_.getJointId(joint)); } + } - // First we check that the passed joints exist in the kineatic tree - for (auto& joint : params_.joints) { - if (not raw_model_.existJointName(joint)) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Failed to configure because " << joint << " is not part of the kinematic tree but it has been passed in the parameters."); - return CallbackReturn::ERROR; - } - } - RCLCPP_INFO(get_node()->get_logger(), "All joints passed in the parameters exist in the kinematic tree of the URDF."); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Removing the rest of the joints that are not used: "); - // Now we fix all joints that are not referenced in the tree - std::vector list_of_joints_to_lock_by_id; - for (auto& joint : raw_model_.names) { - if (std::find(params_.joints.begin(), params_.joints.end(), joint) == params_.joints.end() and joint != "universe") { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "Joint " << joint << " is not used, removing it from the model."); - list_of_joints_to_lock_by_id.push_back(raw_model_.getJointId(joint)); - } - } + Eigen::VectorXd q_locked = Eigen::VectorXd::Zero(raw_model_.nq); + model_ = pinocchio::buildReducedModel(raw_model_, list_of_joints_to_lock_by_id, q_locked); + data_ = pinocchio::Data(model_); - Eigen::VectorXd q_locked = Eigen::VectorXd::Zero(raw_model_.nq); - model_ = pinocchio::buildReducedModel(raw_model_, list_of_joints_to_lock_by_id, q_locked); - data_ = pinocchio::Data(model_); - - for (int joint_id = 0; joint_id < model_.njoints; joint_id++) { - if (model_.names[joint_id] == "universe") { - continue; - } - if (not allowed_joint_types.count(model_.joints[joint_id].shortname())) { - RCLCPP_ERROR_STREAM(get_node()->get_logger(), "Joint type " << model_.joints[joint_id].shortname() << " is unsupported (" << model_.names[joint_id] << "), only revolute/continous like joints can be used."); - return CallbackReturn::ERROR; - } + for (int joint_id = 0; joint_id < model_.njoints; joint_id++) { + if (model_.names[joint_id] == "universe") { + continue; + } + if (!allowed_joint_types.count(model_.joints[joint_id].shortname())) { + RCLCPP_ERROR_STREAM( + get_node()->get_logger(), + "Joint type " << model_.joints[joint_id].shortname() << " is unsupported (" + << model_.names[joint_id] + << "), only revolute/continous like joints can be used."); + return CallbackReturn::ERROR; } + } - end_effector_frame_id = model_.getFrameId(params_.end_effector_frame); - q = Eigen::VectorXd::Zero(model_.nv); - q_dot = Eigen::VectorXd::Zero(model_.nv); + end_effector_frame_id = model_.getFrameId(params_.end_effector_frame); + q = Eigen::VectorXd::Zero(model_.nv); + q_dot = Eigen::VectorXd::Zero(model_.nv); - twist_publisher_ = get_node()->create_publisher( - "current_twist", rclcpp::SystemDefaultsQoS()); - realtime_twist_publisher_ = - std::make_shared>( - twist_publisher_); + twist_publisher_ = get_node()->create_publisher( + "current_twist", rclcpp::SystemDefaultsQoS()); + realtime_twist_publisher_ = + std::make_shared>( + twist_publisher_); - if (params_.publish_frequency > 0.0) { - publish_interval_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); - } else { - publish_interval_ = rclcpp::Duration(0, 0); // publish every cycle - } + if (params_.publish_frequency > 0.0) { + publish_interval_ = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); + } else { + publish_interval_ = rclcpp::Duration(0, 0); // publish every cycle + } - return CallbackReturn::SUCCESS; + return CallbackReturn::SUCCESS; } -CallbackReturn TwistBroadcaster::on_activate( - const rclcpp_lifecycle::State & /*previous_state*/) { - // reset publish time accumulation - publish_elapsed_ = rclcpp::Duration(0, 0); - return CallbackReturn::SUCCESS; +CallbackReturn TwistBroadcaster::on_activate(const rclcpp_lifecycle::State & /*previous_state*/) { + // reset publish time accumulation + publish_elapsed_ = rclcpp::Duration(0, 0); + return CallbackReturn::SUCCESS; } -controller_interface::CallbackReturn TwistBroadcaster::on_deactivate( - const rclcpp_lifecycle::State & /*previous_state*/) { - return CallbackReturn::SUCCESS; +controller_interface::CallbackReturn +TwistBroadcaster::on_deactivate(const rclcpp_lifecycle::State & /*previous_state*/) { + return CallbackReturn::SUCCESS; } -} // namespace crisp_controllers +} // namespace crisp_controllers #include "pluginlib/class_list_macros.hpp" // NOLINTNEXTLINE -PLUGINLIB_EXPORT_CLASS(crisp_controllers::TwistBroadcaster, - controller_interface::ControllerInterface) +PLUGINLIB_EXPORT_CLASS( + crisp_controllers::TwistBroadcaster, controller_interface::ControllerInterface) diff --git a/tests/test_filters.cpp b/tests/test_filters.cpp index 2afbbcd..86fa8b9 100644 --- a/tests/test_filters.cpp +++ b/tests/test_filters.cpp @@ -1,3 +1,5 @@ +// "Copyright [2026] " + #include #include #include "crisp_controllers/utils/fiters.hpp" @@ -6,18 +8,18 @@ TEST(ExponentialMovingAverageTest, BasicFiltering) { double output = 0.0; double current = 1.0; double alpha = 0.5; - + double result = exponential_moving_average(output, current, alpha); - EXPECT_DOUBLE_EQ(result, 0.5); // Should be (1-0.5)*1.0 + 0.5*0.0 = 0.5 + EXPECT_DOUBLE_EQ(result, 0.5); // Should be (1-0.5)*1.0 + 0.5*0.0 = 0.5 } TEST(ExponentialMovingAverageTest, AlphaLimits) { double output = 1.0; double current = 2.0; - + double result1 = exponential_moving_average(output, current, 0.01); EXPECT_NEAR(result1, 1.99, 1e-10); - + double result2 = exponential_moving_average(output, current, 0.99); EXPECT_NEAR(result2, 1.01, 1e-10); } @@ -30,7 +32,7 @@ TEST(ExponentialMovingAverageTest, TypeTest) { float result = exponential_moving_average(output, current, 0.5); EXPECT_FLOAT_EQ(result, 1.5f); } - + { int output = 1; int current = 2; @@ -43,11 +45,11 @@ TEST(ExponentialMovingAverageTest, Convergence) { double output = 0.0; double current = 1.0; double alpha = 0.5; - - for(int i = 0; i < 10; i++) { + + for (int i = 0; i < 10; i++) { output = exponential_moving_average(output, current, alpha); } - + EXPECT_NEAR(output, current, 1e-3); } @@ -55,18 +57,17 @@ TEST(FilterJointValuesTest, BasicFiltering) { std::vector msg_names = {"joint1", "joint2", "joint3"}; Eigen::VectorXd msg_values(3); msg_values << 1.0, 2.0, 3.0; - + std::vector desired_joint_names = {"joint2", "joint3"}; Eigen::VectorXd result = Eigen::VectorXd::Zero(2); filterJointValues(msg_names, msg_values, desired_joint_names, result); - + EXPECT_DOUBLE_EQ(result(0), 2.0); EXPECT_DOUBLE_EQ(result(1), 3.0); } -int main(int argc, char **argv) { +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - diff --git a/tests/test_pseudo_inverse.cpp b/tests/test_pseudo_inverse.cpp index 2983328..003804c 100644 --- a/tests/test_pseudo_inverse.cpp +++ b/tests/test_pseudo_inverse.cpp @@ -1,110 +1,132 @@ +// "Copyright [2026] " #include #include "crisp_controllers/utils/pseudo_inverse.hpp" TEST(PseudoInverseTest, SquareMatrixTest) { Eigen::MatrixXd A(2, 2); - A << 4, 7, - 2, 6; - + A << 4, 7, 2, 6; + Eigen::MatrixXd A_pinv = crisp_controllers::pseudo_inverse(A); - + // Test if A * A_pinv * A ≈ A Eigen::MatrixXd result = A * A_pinv * A; - EXPECT_TRUE(result.isApprox(A, 1e-5)) - << "Square Matrix Test Failed!\n" - << "Original matrix A:\n" << A << "\n" - << "Pseudo-inverse A_pinv:\n" << A_pinv << "\n" - << "Result (A * A_pinv * A):\n" << result << "\n" - << "Expected (A):\n" << A << "\n" - << "Difference:\n" << (result - A) << "\n"; + EXPECT_TRUE(result.isApprox(A, 1e-5)) << "Square Matrix Test Failed!\n" + << "Original matrix A:\n" + << A << "\n" + << "Pseudo-inverse A_pinv:\n" + << A_pinv << "\n" + << "Result (A * A_pinv * A):\n" + << result << "\n" + << "Expected (A):\n" + << A << "\n" + << "Difference:\n" + << (result - A) << "\n"; } TEST(PseudoInverseTest, RectangularMatrixTest) { Eigen::MatrixXd B(3, 2); - B << 1, 2, - 3, 4, - 5, 6; - + B << 1, 2, 3, 4, 5, 6; + Eigen::MatrixXd B_pinv = crisp_controllers::pseudo_inverse(B); - + Eigen::MatrixXd result = B * B_pinv * B; - EXPECT_TRUE(result.isApprox(B, 1e-5)) - << "Rectangular Matrix Test Failed!\n" - << "Original matrix B:\n" << B << "\n" - << "Pseudo-inverse B_pinv:\n" << B_pinv << "\n" - << "Result (B * B_pinv * B):\n" << result << "\n" - << "Expected (B):\n" << B << "\n" - << "Difference:\n" << (result - B) << "\n"; + EXPECT_TRUE(result.isApprox(B, 1e-5)) << "Rectangular Matrix Test Failed!\n" + << "Original matrix B:\n" + << B << "\n" + << "Pseudo-inverse B_pinv:\n" + << B_pinv << "\n" + << "Result (B * B_pinv * B):\n" + << result << "\n" + << "Expected (B):\n" + << B << "\n" + << "Difference:\n" + << (result - B) << "\n"; } TEST(PseudoInverseTest, MoorePenroseTest) { Eigen::MatrixXd C(2, 3); - C << 1, 2, 3, - 4, 5, 6; - + C << 1, 2, 3, 4, 5, 6; + Eigen::MatrixXd C_pinv = crisp_controllers::pseudo_inverse_moore_penrose(C); - + // Test Moore-Penrose conditions // 1. C * C_pinv * C = C Eigen::MatrixXd condition1 = C * C_pinv * C; - EXPECT_TRUE(condition1.isApprox(C, 1e-5)) - << "Moore-Penrose Condition 1 Failed!\n" - << "Original matrix C:\n" << C << "\n" - << "Pseudo-inverse C_pinv:\n" << C_pinv << "\n" - << "Result (C * C_pinv * C):\n" << condition1 << "\n" - << "Expected (C):\n" << C << "\n" - << "Difference:\n" << (condition1 - C) << "\n"; - + EXPECT_TRUE(condition1.isApprox(C, 1e-5)) << "Moore-Penrose Condition 1 Failed!\n" + << "Original matrix C:\n" + << C << "\n" + << "Pseudo-inverse C_pinv:\n" + << C_pinv << "\n" + << "Result (C * C_pinv * C):\n" + << condition1 << "\n" + << "Expected (C):\n" + << C << "\n" + << "Difference:\n" + << (condition1 - C) << "\n"; + // 2. C_pinv * C * C_pinv = C_pinv Eigen::MatrixXd condition2 = C_pinv * C * C_pinv; - EXPECT_TRUE(condition2.isApprox(C_pinv, 1e-5)) - << "Moore-Penrose Condition 2 Failed!\n" - << "Result (C_pinv * C * C_pinv):\n" << condition2 << "\n" - << "Expected (C_pinv):\n" << C_pinv << "\n" - << "Difference:\n" << (condition2 - C_pinv) << "\n"; - + EXPECT_TRUE(condition2.isApprox(C_pinv, 1e-5)) << "Moore-Penrose Condition 2 Failed!\n" + << "Result (C_pinv * C * C_pinv):\n" + << condition2 << "\n" + << "Expected (C_pinv):\n" + << C_pinv << "\n" + << "Difference:\n" + << (condition2 - C_pinv) << "\n"; + // 3. (C * C_pinv)' = C * C_pinv Eigen::MatrixXd condition3 = C * C_pinv; Eigen::MatrixXd condition3_trans = condition3.transpose(); EXPECT_TRUE(condition3_trans.isApprox(condition3, 1e-5)) << "Moore-Penrose Condition 3 Failed!\n" - << "C * C_pinv:\n" << condition3 << "\n" - << "(C * C_pinv)':\n" << condition3_trans << "\n" - << "Difference:\n" << (condition3_trans - condition3) << "\n"; - + << "C * C_pinv:\n" + << condition3 << "\n" + << "(C * C_pinv)':\n" + << condition3_trans << "\n" + << "Difference:\n" + << (condition3_trans - condition3) << "\n"; + // 4. (C_pinv * C)' = C_pinv * C Eigen::MatrixXd condition4 = C_pinv * C; Eigen::MatrixXd condition4_trans = condition4.transpose(); EXPECT_TRUE(condition4_trans.isApprox(condition4, 1e-5)) << "Moore-Penrose Condition 4 Failed!\n" - << "C_pinv * C:\n" << condition4 << "\n" - << "(C_pinv * C)':\n" << condition4_trans << "\n" - << "Difference:\n" << (condition4_trans - condition4) << "\n"; + << "C_pinv * C:\n" + << condition4 << "\n" + << "(C_pinv * C)':\n" + << condition4_trans << "\n" + << "Difference:\n" + << (condition4_trans - condition4) << "\n"; } TEST(PseudoInverseTest, ZeroMatrixTest) { // Test with near-zero matrix Eigen::MatrixXd D = Eigen::MatrixXd::Zero(2, 2); - + Eigen::MatrixXd D_pinv1 = crisp_controllers::pseudo_inverse(D); Eigen::MatrixXd D_pinv2 = crisp_controllers::pseudo_inverse_moore_penrose(D); - + // Both should return zero matrix EXPECT_TRUE(D_pinv1.isApprox(Eigen::MatrixXd::Zero(2, 2), 1e-5)) << "Zero Matrix Test Failed (pseudoInverse)!\n" - << "Result:\n" << D_pinv1 << "\n" - << "Expected zero matrix:\n" << Eigen::MatrixXd::Zero(2, 2) << "\n" - << "Difference:\n" << (D_pinv1 - Eigen::MatrixXd::Zero(2, 2)) << "\n"; - + << "Result:\n" + << D_pinv1 << "\n" + << "Expected zero matrix:\n" + << Eigen::MatrixXd::Zero(2, 2) << "\n" + << "Difference:\n" + << (D_pinv1 - Eigen::MatrixXd::Zero(2, 2)) << "\n"; + EXPECT_TRUE(D_pinv2.isApprox(Eigen::MatrixXd::Zero(2, 2), 1e-10)) << "Zero Matrix Test Failed (pseudoInverseMoorePenrose)!\n" - << "Result:\n" << D_pinv2 << "\n" - << "Expected zero matrix:\n" << Eigen::MatrixXd::Zero(2, 2) << "\n" - << "Difference:\n" << (D_pinv2 - Eigen::MatrixXd::Zero(2, 2)) << "\n"; + << "Result:\n" + << D_pinv2 << "\n" + << "Expected zero matrix:\n" + << Eigen::MatrixXd::Zero(2, 2) << "\n" + << "Difference:\n" + << (D_pinv2 - Eigen::MatrixXd::Zero(2, 2)) << "\n"; } -int main(int argc, char **argv) { +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - diff --git a/tests/test_torque_rate_saturation.cpp b/tests/test_torque_rate_saturation.cpp index 039dad0..2d4af89 100644 --- a/tests/test_torque_rate_saturation.cpp +++ b/tests/test_torque_rate_saturation.cpp @@ -1,6 +1,7 @@ -#include "crisp_controllers/utils/torque_rate_saturation.hpp" -#include +// "Copyright [2026] " +#include +#include "crisp_controllers/utils/torque_rate_saturation.hpp" TEST(SaturateTorqueRateTest, ScalarDeltaTauMax) { Eigen::VectorXd tau_d(3), tau_J(3); @@ -30,8 +31,7 @@ TEST(SaturateTorqueRateTest, VectorDeltaTauMax) { EXPECT_TRUE(result.isApprox(expected)); } -int main(int argc, char **argv) { +int main(int argc, char ** argv) { testing::InitGoogleTest(&argc, argv); return RUN_ALL_TESTS(); } - From 49d3f21215f57eaef76619d100cb284c49ca9aae Mon Sep 17 00:00:00 2001 From: adi-git-slu Date: Tue, 27 Jan 2026 17:06:52 -0500 Subject: [PATCH 2/6] [#7] Fix Interpolation Error from Doing Internal Target Pose Filtering (#8) - Replaced using slerp to interpolate orientation in stead of doing interpolation in twist, since angular velocity vector is surjective to rotation matrix. - Added extra logging section in cartesian_controller.cpp to avoid using log_debug_info due to glitches. - Added ros2 log to csv pythong script and json to help converting ros2 log to csv file for easy data plotting when use plotjuggler. --- .../cartesian_controller.hpp | 6 +- script/config/roslog_to_csv.json | 18 ++ script/roslog_to_csv.py | 159 ++++++++++++++++++ src/cartesian_controller.cpp | 64 +++++-- 4 files changed, 234 insertions(+), 13 deletions(-) create mode 100644 script/config/roslog_to_csv.json create mode 100755 script/roslog_to_csv.py diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 5e73cd0..475d17b 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -143,8 +143,10 @@ class CartesianController : public controller_interface::ControllerInterface { 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 params_listener_; diff --git a/script/config/roslog_to_csv.json b/script/config/roslog_to_csv.json new file mode 100644 index 0000000..1553096 --- /dev/null +++ b/script/config/roslog_to_csv.json @@ -0,0 +1,18 @@ +{ + "keys": { + "current eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"], + "interpolated eef": ["x", "y", "z", "yaw", "pitch", "roll","qx", "qy", "qz", "qw"], + "desired eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"], + "target eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"], + "error": ["x", "y", "z", "omega_x", "omega_y", "omega_z"], + "unnormalized new_angular": ["omega_x", "omega_y", "omega_z"], + "unnormalized filtered_angular": ["omega_x", "omega_y", "omega_z"], + "normalized new_angular": ["omega_x", "omega_y", "omega_z"], + "normalized filtered_angular": ["omega_x", "omega_y", "omega_z"], + "angle_diff": ["angle_diff"], + "inter angular": ["omega_x", "omega_y", "omega_z"], + "control input": ["x", "y", "z", "yaw", "pitch", "roll"], + "task wrench": ["x", "y", "z", "yaw", "pitch", "roll"], + "task tau": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"] + } +} \ No newline at end of file diff --git a/script/roslog_to_csv.py b/script/roslog_to_csv.py new file mode 100755 index 0000000..34830be --- /dev/null +++ b/script/roslog_to_csv.py @@ -0,0 +1,159 @@ +#!/usr/bin/env python3 +# Copyright (c) 2024 Analog Devices Inc. +# Licensed under the Apache License, Version 2.0 + +import argparse +import csv +import json +import os +import re + +LOG_RE = re.compile(r"^\[\w+\]\s+\[(?P\d+\.\d+)\]\s+\[[^\]]+\]:\s+(?P.*)$") + +KEYVAL_RE = re.compile(r"^(?P[^:]+):\s*(?P.*)$") +SPLIT_RE = re.compile(r"[,\s]+") + + +def sanitize(s: str) -> str: + s = s.strip().lower() + s = re.sub(r"\s+", "_", s) + return re.sub(r"[^a-z0-9_]", "", s) + + +def parse_values(s: str): + return [float(v) for v in SPLIT_RE.split(s.strip()) if v] + + +def load_config(config_path: str) -> dict: + """Load and parse configuration file.""" + with open(config_path, encoding="utf-8") as f: + return json.load(f) + + +def prepare_columns(keys: dict) -> tuple[list, dict]: + """Prepare column headers and key-to-column mappings.""" + columns = [] + key_cols = {} + for key, fields in keys.items(): + key_cols[key] = [f"{sanitize(key)}.{sanitize(f)}" for f in fields] + columns.extend(key_cols[key]) + return columns, key_cols + + +def parse_log_file( + input_file: str, keys: dict, key_cols: dict, columns: list +) -> tuple[list, set]: + """Parse log file and extract data rows.""" + rows = [] + current_row = None + found_keys = set() + + def flush(): + nonlocal current_row + if current_row: + rows.append(current_row) + current_row = None + + with open(input_file, encoding="utf-8") as f: + for line in f: + m = LOG_RE.match(line) + if not m: + continue + + body = m.group("body") + km = KEYVAL_RE.match(body) + if not km: + continue + + key = km.group("key").strip() + if key not in keys: + continue + + found_keys.add(key) + values = parse_values(km.group("values")) + fields = keys[key] + + if len(values) < len(fields): + continue + + # Check if this key's columns are already populated - if so, create new row + if current_row and any(current_row[col] for col in key_cols[key]): + flush() + + # Initialize row if needed + if current_row is None: + current_row = {c: "" for c in columns} + + for col, v in zip(key_cols[key], values): + current_row[col] = v + + flush() + return rows, found_keys + + +def remove_missing_columns( + columns: list, key_cols: dict, rows: list, keys: dict, found_keys: set +) -> list: + """Remove columns for keys not found in the log file.""" + missing_keys = set(keys.keys()) - found_keys + if not missing_keys: + return columns + + print("Warning: The following data fields were not found in the log file:") + for key in sorted(missing_keys): + print(f" - {key}") + + columns_to_remove = set() + for missing_key in missing_keys: + columns_to_remove.update(key_cols[missing_key]) + + columns = [col for col in columns if col not in columns_to_remove] + + for row in rows: + for col in columns_to_remove: + row.pop(col, None) + + return columns + + +def format_float_values(rows: list, columns: list) -> None: + """Convert scientific notation to decimal format for better CSV compatibility.""" + for row in rows: + for col in columns: + if col in row and isinstance(row[col], float): + row[col] = f"{row[col]:.10f}".rstrip("0").rstrip(".") + + +def write_csv(output_file: str, columns: list, rows: list) -> None: + """Write data to CSV file.""" + with open(output_file, "w", newline="", encoding="utf-8") as out: + writer = csv.DictWriter(out, fieldnames=columns) + writer.writeheader() + writer.writerows(rows) + + +def main(): + ap = argparse.ArgumentParser() + ap.add_argument("-i", "--input", required=True) + ap.add_argument("-o", "--output", required=False) + ap.add_argument("-c", "--config", default="config/roslog_to_csv.json") + args = ap.parse_args() + + # If no output name is given, use input filename (without extension) + .csv + if args.output is None: + base_name = os.path.basename(args.input) + name_without_ext = os.path.splitext(base_name)[0] + args.output = name_without_ext + ".csv" + + cfg = load_config(args.config) + keys = cfg["keys"] + + columns, key_cols = prepare_columns(keys) + rows, found_keys = parse_log_file(args.input, keys, key_cols, columns) + columns = remove_missing_columns(columns, key_cols, rows, keys, found_keys) + format_float_values(rows, columns) + write_csv(args.output, columns, rows) + + +if __name__ == "__main__": + main() diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 6b35705..ce08919 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -93,11 +93,10 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & pinocchio::forwardKinematics(model_, data_, q_pin, dq); pinocchio::updateFramePlacements(model_, data_); - pinocchio::SE3 new_target_pose = - pinocchio::SE3(target_orientation_.toRotationMatrix(), target_position_); - - target_pose_ = pinocchio::exp6(exponential_moving_average( - pinocchio::log6(target_pose_), pinocchio::log6(new_target_pose), params_.filter.target_pose)); + desired_position_ = + exponential_moving_average(desired_position_, target_position_, params_.filter.target_pose); + desired_orientation_ = + target_orientation_.slerp(params_.filter.target_pose, desired_orientation_); /*target_pose_ = pinocchio::SE3(target_orientation_.toRotationMatrix(), * target_position_);*/ @@ -107,13 +106,13 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & // motions if (params_.use_local_jacobian) { error.head(3) = end_effector_pose.rotation().transpose() * - (target_pose_.translation() - end_effector_pose.translation()); + (desired_position_ - end_effector_pose.translation()); error.tail(3) = - pinocchio::log3(end_effector_pose.rotation().transpose() * target_pose_.rotation()); + pinocchio::log3(end_effector_pose.rotation().transpose() * desired_orientation_); } else { - error.head(3) = target_pose_.translation() - end_effector_pose.translation(); + error.head(3) = desired_position_ - end_effector_pose.translation(); error.tail(3) = - pinocchio::log3(target_pose_.rotation() * end_effector_pose.rotation().transpose()); + pinocchio::log3(desired_orientation_ * end_effector_pose.rotation().transpose()); } if (params_.limit_error) { @@ -209,8 +208,49 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & params_ = params_listener_->get_params(); setStiffnessAndDamping(); + // TODO(placeholder): log_debug_info function has glitches which results in different number of + // data points logged same frequence. For instance, q, dq are logged with same frequency, + // but the number of data points can be different. Turning off the logging by assigning false + // to params_.log.enabled. This can be done in controllers yaml file. log_debug_info(time); + // TODO(placeholder): The following debug output is supposed to be wrapped within + // #ifndef NDEBUG + // ... + // #endif + // So that the debug build can enable the output. However, debug build causes significant + // performance drop in the controller which reults in "communication_constraint_violation" errors + // for Franka robot. + + static int read_counter = 0; + // Change the number that `read_counter` doing mode operation with to control the output frequency + // 1 is for every cycle, 2 is for every other cycle, etc. Increase the number to reduce output + // frequency, to avoid causing "communication_constraint_violation" errors for Franka robot. + if (read_counter % 1 == 0) { + RCLCPP_INFO_STREAM(get_node()->get_logger(), "q: " << q.transpose()); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "dq: " << dq.transpose()); + RCLCPP_INFO_STREAM( + get_node()->get_logger(), + "current eef:" << end_effector_pose.translation().transpose() << " " + << end_effector_pose.rotation().eulerAngles(2, 1, 0).transpose() << " " + << Eigen::Quaterniond(end_effector_pose.rotation()).coeffs().transpose()); + RCLCPP_INFO_STREAM( + get_node()->get_logger(), + "desired eef:" << desired_position_.transpose() << " " + << desired_orientation_.toRotationMatrix().eulerAngles(2, 1, 0).transpose() + << " " << desired_orientation_.coeffs().transpose()); + + RCLCPP_INFO_STREAM( + get_node()->get_logger(), + "target eef:" << target_position_.transpose() << " " + << target_orientation_.toRotationMatrix().eulerAngles(2, 1, 0).transpose() + << " " << target_orientation_.coeffs().transpose()); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "error: " << error.transpose()); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "task tau: " << tau_task.transpose()); + read_counter = 0; // Optional: prevents overflow, but not necessary + } + read_counter++; + return controller_interface::return_type::OK; } @@ -389,7 +429,8 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta target_position_ = Eigen::Vector3d::Zero(); target_orientation_ = Eigen::Quaterniond::Identity(); target_wrench_ = Eigen::VectorXd::Zero(6); - target_pose_ = pinocchio::SE3::Identity(); + desired_position_ = Eigen::Vector3d::Zero(); + desired_orientation_ = Eigen::Quaterniond::Identity(); // Initialize error vector error = Eigen::VectorXd::Zero(6); @@ -468,7 +509,8 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat target_position_ = end_effector_pose.translation(); target_orientation_ = Eigen::Quaterniond(end_effector_pose.rotation()); - target_pose_ = pinocchio::SE3(target_orientation_.toRotationMatrix(), target_position_); + desired_position_ = target_position_; + desired_orientation_ = target_orientation_; RCLCPP_INFO(get_node()->get_logger(), "Controller activated."); return CallbackReturn::SUCCESS; From abb0bbd792ff9c35bd2b99d18f2b20934a2b10fc Mon Sep 17 00:00:00 2001 From: adi-git-slu Date: Wed, 28 Jan 2026 14:23:43 -0500 Subject: [PATCH 3/6] [#9] Correct the Usage of the Inverse of Joint Mass Matrix Calculated from Pinocchio (#11) - Corrected the usage of joint mass matrix by explicitly making it as a symmetric matrix, since the one from Pinocchio is an upper triangle matrix for internal calculation speedup within Pinocchio lib. - Added controller_tuning.xml file to use plotjuggler to plot curves in real time. --- controller_tuning.xml | 208 ++++++++++++++++++ .../cartesian_controller.hpp | 5 + script/config/roslog_to_csv.json | 2 +- src/cartesian_controller.cpp | 19 +- 4 files changed, 223 insertions(+), 11 deletions(-) create mode 100644 controller_tuning.xml diff --git a/controller_tuning.xml b/controller_tuning.xml new file mode 100644 index 0000000..fd96ef5 --- /dev/null +++ b/controller_tuning.xml @@ -0,0 +1,208 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 475d17b..f25f243 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -242,6 +242,11 @@ class CartesianController : public controller_interface::ControllerInterface { /** @brief Final desired torque command */ Eigen::VectorXd tau_d; + /** @brief Inverse of the manipulator joint mass projected in Cartesian space (6x6) */ + Eigen::Matrix Mx_inv = Eigen::Matrix::Zero(); + /** @brief the manipulator joint mass projected in Cartesian space (6x6) */ + Eigen::Matrix Mx = Eigen::Matrix::Zero(); + /** * @brief Log debug information based on parameter settings * @param time Current time for throttling logs diff --git a/script/config/roslog_to_csv.json b/script/config/roslog_to_csv.json index 1553096..17c00d3 100644 --- a/script/config/roslog_to_csv.json +++ b/script/config/roslog_to_csv.json @@ -4,7 +4,7 @@ "interpolated eef": ["x", "y", "z", "yaw", "pitch", "roll","qx", "qy", "qz", "qw"], "desired eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"], "target eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"], - "error": ["x", "y", "z", "omega_x", "omega_y", "omega_z"], + "error": ["x", "y", "z", "yaw", "pitch", "roll"], "unnormalized new_angular": ["omega_x", "omega_y", "omega_z"], "unnormalized filtered_angular": ["omega_x", "omega_y", "omega_z"], "normalized new_angular": ["omega_x", "omega_y", "omega_z"], diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index ce08919..fb0eba0 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -61,9 +61,8 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & auto joint_id = model_.getJointId(joint_name); // pinocchio joind id might be different auto joint = model_.joints[joint_id]; - /*q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(),*/ - /* params_.filter.q);*/ - q[i] = state_interfaces_[i].get_value(); + // Filtering joint position measurement 1 uses previous q, 0 uses new q from measurement. + q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(), params_.filter.q); if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous // joint that is SO(2) q_pin[joint.idx_q()] = std::cos(q[i]); @@ -129,11 +128,15 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & Eigen::MatrixXd J_pinv(model_.nv, 6); J_pinv = pseudo_inverse(J, params_.nullspace.regularization); Eigen::MatrixXd Id_nv = Eigen::MatrixXd::Identity(model_.nv, model_.nv); + if (params_.nullspace.projector_type == "dynamic" || params_.use_operational_space) { + pinocchio::computeMinverse(model_, data_, q_pin); + data_.Minv.triangularView() = + data_.Minv.transpose().triangularView(); + Mx_inv = J * data_.Minv * J.transpose(); + Mx = pseudo_inverse(Mx_inv); + } if (params_.nullspace.projector_type == "dynamic") { - pinocchio::computeMinverse(model_, data_, q_pin); - auto Mx_inv = J * data_.Minv * J.transpose(); - auto Mx = pseudo_inverse(Mx_inv); auto J_bar = data_.Minv * J.transpose() * Mx; nullspace_projection = Id_nv - J.transpose() * J_bar.transpose(); } else if (params_.nullspace.projector_type == "kinematic") { @@ -148,10 +151,6 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & } if (params_.use_operational_space) { - pinocchio::computeMinverse(model_, data_, q_pin); - auto Mx_inv = J * data_.Minv * J.transpose(); - auto Mx = pseudo_inverse(Mx_inv); - tau_task << J.transpose() * Mx * (stiffness * error - damping * (J * dq)); } else { tau_task << J.transpose() * (stiffness * error - damping * (J * dq)); From 82521d2c683b7724a85430260d3c33de1b654fe8 Mon Sep 17 00:00:00 2001 From: adi-git-slu Date: Fri, 6 Feb 2026 22:40:39 -0500 Subject: [PATCH 4/6] [#12] Apply Exponential Moving Average to Target Joint Value (#13) - Applied EMA to target joint value when the value is received from target joint topic. - Applied EMA to the current joint velocity. - Changed the header included from Pinocchio from frames.hxx to frames.hpp. --- .../crisp_controllers/cartesian_controller.hpp | 2 ++ script/config/roslog_to_csv.json | 4 ++++ src/cartesian_controller.cpp | 17 +++++++++++------ src/cartesian_impedance_controller.yaml | 6 ++++++ src/pose_broadcaster.cpp | 2 +- 5 files changed, 24 insertions(+), 7 deletions(-) diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index f25f243..12e7cd7 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -184,6 +184,8 @@ class CartesianController : public controller_interface::ControllerInterface { 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; diff --git a/script/config/roslog_to_csv.json b/script/config/roslog_to_csv.json index 17c00d3..18cce6c 100644 --- a/script/config/roslog_to_csv.json +++ b/script/config/roslog_to_csv.json @@ -1,5 +1,9 @@ { "keys": { + "q": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"], + "q target": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"], + "q ref": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"], + "dq": ["joint_1", "joint_2", "joint_3", "joint_4", "joint_5", "joint_6", "joint_7"], "current eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"], "interpolated eef": ["x", "y", "z", "yaw", "pitch", "roll","qx", "qy", "qz", "qw"], "desired eef": ["x", "y", "z", "yaw", "pitch", "roll", "qx", "qy", "qz", "qw"], diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index fb0eba0..abe029c 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -16,7 +16,7 @@ #include #include -#include +#include #include #include #include @@ -61,6 +61,8 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & auto joint_id = model_.getJointId(joint_name); // pinocchio joind id might be different auto joint = model_.joints[joint_id]; + q_ref[i] = exponential_moving_average(q_ref[i], q_target[i], params_.filter.q_ref); + // Filtering joint position measurement 1 uses previous q, 0 uses new q from measurement. q[i] = exponential_moving_average(q[i], state_interfaces_[i].get_value(), params_.filter.q); if (continous_joint_types.count(joint.shortname())) { // Then we are handling a continous @@ -70,10 +72,8 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & } else { // simple revolute joint case q_pin[joint.idx_q()] = q[i]; } - /*dq[i] = exponential_moving_average(*/ - /* dq[i], state_interfaces_[num_joints + i].get_value(),*/ - /* params_.filter.dq);*/ - dq[i] = state_interfaces_[num_joints + i].get_value(); + dq[i] = exponential_moving_average( + dq[i], state_interfaces_[num_joints + i].get_value(), params_.filter.dq); } if (new_target_pose_) { @@ -227,6 +227,8 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & // frequency, to avoid causing "communication_constraint_violation" errors for Franka robot. if (read_counter % 1 == 0) { RCLCPP_INFO_STREAM(get_node()->get_logger(), "q: " << q.transpose()); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "q target: " << q_target.transpose()); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "q ref: " << q_ref.transpose()); RCLCPP_INFO_STREAM(get_node()->get_logger(), "dq: " << dq.transpose()); RCLCPP_INFO_STREAM( get_node()->get_logger(), @@ -246,6 +248,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & << " " << target_orientation_.coeffs().transpose()); RCLCPP_INFO_STREAM(get_node()->get_logger(), "error: " << error.transpose()); RCLCPP_INFO_STREAM(get_node()->get_logger(), "task tau: " << tau_task.transpose()); + RCLCPP_INFO_STREAM(get_node()->get_logger(), "nullspace tau: " << tau_nullspace.transpose()); read_counter = 0; // Optional: prevents overflow, but not necessary } read_counter++; @@ -341,6 +344,7 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta q_pin = Eigen::VectorXd::Zero(model_.nq); dq = Eigen::VectorXd::Zero(model_.nv); q_ref = Eigen::VectorXd::Zero(model_.nv); + q_target = Eigen::VectorXd::Zero(model_.nv); dq_ref = Eigen::VectorXd::Zero(model_.nv); tau_previous = Eigen::VectorXd::Zero(model_.nv); J = Eigen::MatrixXd::Zero(6, model_.nv); @@ -496,6 +500,7 @@ CartesianController::on_activate(const rclcpp_lifecycle::State & /*previous_stat } q_ref[i] = state_interfaces_[i].get_value(); + q_target[i] = q_ref[i]; dq[i] = state_interfaces_[num_joints + i].get_value(); dq_ref[i] = state_interfaces_[num_joints + i].get_value(); @@ -534,7 +539,7 @@ void CartesianController::parse_target_joint_() { auto msg = *target_joint_buffer_.readFromRT(); if (msg->position.size()) { for (size_t i = 0; i < msg->position.size(); ++i) { - q_ref[i] = msg->position[i]; + q_target[i] = msg->position[i]; } /*filterJointValues(msg->name, msg->position, params_.joints, q_ref);*/ } diff --git a/src/cartesian_impedance_controller.yaml b/src/cartesian_impedance_controller.yaml index eed5ee0..d459420 100644 --- a/src/cartesian_impedance_controller.yaml +++ b/src/cartesian_impedance_controller.yaml @@ -240,6 +240,12 @@ cartesian_impedance_controller: description: "Amount of smoothing for the target pose when using the EMA." validation: bounds<>: [0.0, 1.0] + q_ref: + type: double + default_value: 1.0 + description: "Amount of smoothing for the target joint when using the EMA." + validation: + bounds<>: [0.0, 1.0] output_torque: type: double diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp index 92b467a..7ddb296 100644 --- a/src/pose_broadcaster.cpp +++ b/src/pose_broadcaster.cpp @@ -9,7 +9,7 @@ #include #include -#include +#include #include #include #include From a8d463f637462930cfb1d06922f105027e81ec24 Mon Sep 17 00:00:00 2001 From: adi-git-slu Date: Thu, 12 Feb 2026 14:17:08 -0500 Subject: [PATCH 5/6] [#14] Remove the Copyright from Source File (#15) - Removed previously added copyright. --- .pre-commit-config.yaml | 2 +- include/crisp_controllers/cartesian_controller.hpp | 1 - include/crisp_controllers/pch.hpp | 1 - include/crisp_controllers/pose_broadcaster.hpp | 1 - include/crisp_controllers/torque_feedback_controller.hpp | 1 - include/crisp_controllers/twist_broadcaster.hpp | 1 - include/crisp_controllers/utils/fiters.hpp | 1 - include/crisp_controllers/utils/friction_model.hpp | 1 - include/crisp_controllers/utils/joint_limits.hpp | 1 - include/crisp_controllers/utils/pseudo_inverse.hpp | 1 - include/crisp_controllers/utils/torque_rate_saturation.hpp | 1 - script/roslog_to_csv.py | 2 +- src/cartesian_controller.cpp | 1 - src/pose_broadcaster.cpp | 2 -- src/torque_feedback_controller.cpp | 2 -- src/twist_broadcaster.cpp | 2 -- tests/test_filters.cpp | 2 -- tests/test_pseudo_inverse.cpp | 1 - tests/test_torque_rate_saturation.cpp | 2 -- 19 files changed, 2 insertions(+), 24 deletions(-) diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index 8c01610..ab254ba 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -80,7 +80,7 @@ repos: 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"] + args: ["--linelength=100", "--filter=-whitespace/newline,-legal/copyright"] # Cmake hooks - repo: local diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index 12e7cd7..5859ef2 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once /** diff --git a/include/crisp_controllers/pch.hpp b/include/crisp_controllers/pch.hpp index cd80487..9072216 100644 --- a/include/crisp_controllers/pch.hpp +++ b/include/crisp_controllers/pch.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once #include diff --git a/include/crisp_controllers/pose_broadcaster.hpp b/include/crisp_controllers/pose_broadcaster.hpp index 1cbd136..be1afe2 100644 --- a/include/crisp_controllers/pose_broadcaster.hpp +++ b/include/crisp_controllers/pose_broadcaster.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once #include diff --git a/include/crisp_controllers/torque_feedback_controller.hpp b/include/crisp_controllers/torque_feedback_controller.hpp index 8ff4b47..04cf83e 100644 --- a/include/crisp_controllers/torque_feedback_controller.hpp +++ b/include/crisp_controllers/torque_feedback_controller.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once #include diff --git a/include/crisp_controllers/twist_broadcaster.hpp b/include/crisp_controllers/twist_broadcaster.hpp index 9501e16..4e8ad9f 100644 --- a/include/crisp_controllers/twist_broadcaster.hpp +++ b/include/crisp_controllers/twist_broadcaster.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once #include diff --git a/include/crisp_controllers/utils/fiters.hpp b/include/crisp_controllers/utils/fiters.hpp index d8f035c..1a59200 100644 --- a/include/crisp_controllers/utils/fiters.hpp +++ b/include/crisp_controllers/utils/fiters.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once #include diff --git a/include/crisp_controllers/utils/friction_model.hpp b/include/crisp_controllers/utils/friction_model.hpp index 3cb13fe..20635a8 100644 --- a/include/crisp_controllers/utils/friction_model.hpp +++ b/include/crisp_controllers/utils/friction_model.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once // Based in // https://github.com/marcocognetti/FrankaEmikaPandaDynModel/tree/master diff --git a/include/crisp_controllers/utils/joint_limits.hpp b/include/crisp_controllers/utils/joint_limits.hpp index ff1fa64..5e8b743 100644 --- a/include/crisp_controllers/utils/joint_limits.hpp +++ b/include/crisp_controllers/utils/joint_limits.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once #include // NOLINT(build/include_order) diff --git a/include/crisp_controllers/utils/pseudo_inverse.hpp b/include/crisp_controllers/utils/pseudo_inverse.hpp index 1911652..80a0b55 100644 --- a/include/crisp_controllers/utils/pseudo_inverse.hpp +++ b/include/crisp_controllers/utils/pseudo_inverse.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once #include diff --git a/include/crisp_controllers/utils/torque_rate_saturation.hpp b/include/crisp_controllers/utils/torque_rate_saturation.hpp index d5fe0e3..5211ef2 100644 --- a/include/crisp_controllers/utils/torque_rate_saturation.hpp +++ b/include/crisp_controllers/utils/torque_rate_saturation.hpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #pragma once #include diff --git a/script/roslog_to_csv.py b/script/roslog_to_csv.py index 34830be..e87a750 100755 --- a/script/roslog_to_csv.py +++ b/script/roslog_to_csv.py @@ -1,5 +1,5 @@ #!/usr/bin/env python3 -# Copyright (c) 2024 Analog Devices Inc. +# Copyright (c) 2026 Su Lu. # Licensed under the Apache License, Version 2.0 import argparse diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index abe029c..92d90fd 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -1,4 +1,3 @@ -// Copyright [2026] #include #include #include diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp index 7ddb296..84cacf8 100644 --- a/src/pose_broadcaster.cpp +++ b/src/pose_broadcaster.cpp @@ -1,5 +1,3 @@ -// "Copyright [2026] " - #include #include #include diff --git a/src/torque_feedback_controller.cpp b/src/torque_feedback_controller.cpp index bd9d443..23e9964 100644 --- a/src/torque_feedback_controller.cpp +++ b/src/torque_feedback_controller.cpp @@ -1,5 +1,3 @@ -// "Copyright [2026] " - #include #include #include diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp index 99f1caf..ef5cf51 100644 --- a/src/twist_broadcaster.cpp +++ b/src/twist_broadcaster.cpp @@ -1,5 +1,3 @@ -// "Copyright [2026] " - #include #include #include diff --git a/tests/test_filters.cpp b/tests/test_filters.cpp index 86fa8b9..0a5f86b 100644 --- a/tests/test_filters.cpp +++ b/tests/test_filters.cpp @@ -1,5 +1,3 @@ -// "Copyright [2026] " - #include #include #include "crisp_controllers/utils/fiters.hpp" diff --git a/tests/test_pseudo_inverse.cpp b/tests/test_pseudo_inverse.cpp index 003804c..821fe4a 100644 --- a/tests/test_pseudo_inverse.cpp +++ b/tests/test_pseudo_inverse.cpp @@ -1,4 +1,3 @@ -// "Copyright [2026] " #include #include "crisp_controllers/utils/pseudo_inverse.hpp" diff --git a/tests/test_torque_rate_saturation.cpp b/tests/test_torque_rate_saturation.cpp index 2d4af89..8513722 100644 --- a/tests/test_torque_rate_saturation.cpp +++ b/tests/test_torque_rate_saturation.cpp @@ -1,5 +1,3 @@ -// "Copyright [2026] " - #include #include "crisp_controllers/utils/torque_rate_saturation.hpp" From b19aee846e3ce51e4029ad84533ca3641dbb5384 Mon Sep 17 00:00:00 2001 From: Su Lu Date: Wed, 6 May 2026 20:22:15 +0000 Subject: [PATCH 6/6] [#16] Comment out Debugging Output for Intermediate Calculation Result - Commented out debugging out. --- src/cartesian_controller.cpp | 62 ++++++++++++++++++------------------ 1 file changed, 31 insertions(+), 31 deletions(-) diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 03bd4c9..85d0bd2 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -227,37 +227,37 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & // performance drop in the controller which reults in "communication_constraint_violation" errors // for Franka robot. - static int read_counter = 0; - // Change the number that `read_counter` doing mode operation with to control the output frequency - // 1 is for every cycle, 2 is for every other cycle, etc. Increase the number to reduce output - // frequency, to avoid causing "communication_constraint_violation" errors for Franka robot. - if (read_counter % 1 == 0) { - RCLCPP_INFO_STREAM(get_node()->get_logger(), "q: " << q.transpose()); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "q target: " << q_target.transpose()); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "q ref: " << q_ref.transpose()); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "dq: " << dq.transpose()); - RCLCPP_INFO_STREAM( - get_node()->get_logger(), - "current eef:" << end_effector_pose.translation().transpose() << " " - << end_effector_pose.rotation().eulerAngles(2, 1, 0).transpose() << " " - << Eigen::Quaterniond(end_effector_pose.rotation()).coeffs().transpose()); - RCLCPP_INFO_STREAM( - get_node()->get_logger(), - "desired eef:" << desired_position_.transpose() << " " - << desired_orientation_.toRotationMatrix().eulerAngles(2, 1, 0).transpose() - << " " << desired_orientation_.coeffs().transpose()); - - RCLCPP_INFO_STREAM( - get_node()->get_logger(), - "target eef:" << target_position_.transpose() << " " - << target_orientation_.toRotationMatrix().eulerAngles(2, 1, 0).transpose() - << " " << target_orientation_.coeffs().transpose()); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "error: " << error.transpose()); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "task tau: " << tau_task.transpose()); - RCLCPP_INFO_STREAM(get_node()->get_logger(), "nullspace tau: " << tau_nullspace.transpose()); - read_counter = 0; // Optional: prevents overflow, but not necessary - } - read_counter++; + // static int read_counter = 0; + // // Change the number that `read_counter` doing mode operation with to control the output frequency + // // 1 is for every cycle, 2 is for every other cycle, etc. Increase the number to reduce output + // // frequency, to avoid causing "communication_constraint_violation" errors for Franka robot. + // if (read_counter % 1 == 0) { + // RCLCPP_INFO_STREAM(get_node()->get_logger(), "q: " << q.transpose()); + // RCLCPP_INFO_STREAM(get_node()->get_logger(), "q target: " << q_target.transpose()); + // RCLCPP_INFO_STREAM(get_node()->get_logger(), "q ref: " << q_ref.transpose()); + // RCLCPP_INFO_STREAM(get_node()->get_logger(), "dq: " << dq.transpose()); + // RCLCPP_INFO_STREAM( + // get_node()->get_logger(), + // "current eef:" << end_effector_pose.translation().transpose() << " " + // << end_effector_pose.rotation().eulerAngles(2, 1, 0).transpose() << " " + // << Eigen::Quaterniond(end_effector_pose.rotation()).coeffs().transpose()); + // RCLCPP_INFO_STREAM( + // get_node()->get_logger(), + // "desired eef:" << desired_position_.transpose() << " " + // << desired_orientation_.toRotationMatrix().eulerAngles(2, 1, 0).transpose() + // << " " << desired_orientation_.coeffs().transpose()); + + // RCLCPP_INFO_STREAM( + // get_node()->get_logger(), + // "target eef:" << target_position_.transpose() << " " + // << target_orientation_.toRotationMatrix().eulerAngles(2, 1, 0).transpose() + // << " " << target_orientation_.coeffs().transpose()); + // RCLCPP_INFO_STREAM(get_node()->get_logger(), "error: " << error.transpose()); + // RCLCPP_INFO_STREAM(get_node()->get_logger(), "task tau: " << tau_task.transpose()); + // RCLCPP_INFO_STREAM(get_node()->get_logger(), "nullspace tau: " << tau_nullspace.transpose()); + // read_counter = 0; // Optional: prevents overflow, but not necessary + // } + // read_counter++; return controller_interface::return_type::OK; }