From 561315b6c349398cbd27bb0a15154f49794f6ff8 Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Sun, 21 Dec 2025 22:34:51 -0500 Subject: [PATCH 01/10] Updated how time between pose/twist publish is computed --- CMakeLists.txt | 6 +++ .../crisp_controllers/pose_broadcaster.hpp | 4 +- .../crisp_controllers/twist_broadcaster.hpp | 4 +- src/pose_broadcaster.cpp | 49 ++++++++++--------- src/twist_broadcaster.cpp | 46 +++++++++-------- 5 files changed, 63 insertions(+), 46 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 2176f74..af6c486 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,6 +18,12 @@ endif() # Enable precompiled headers option(USE_PRECOMPILED_HEADERS "Use precompiled headers" ON) +# Enable optimizations +add_compile_options(-O3 -march=native -DNDEBUG) +if(NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release) +endif() + if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") add_compile_options(-Wall -Wextra -Wpedantic) endif() diff --git a/include/crisp_controllers/pose_broadcaster.hpp b/include/crisp_controllers/pose_broadcaster.hpp index 324541d..fe04c2b 100644 --- a/include/crisp_controllers/pose_broadcaster.hpp +++ b/include/crisp_controllers/pose_broadcaster.hpp @@ -64,7 +64,9 @@ class PoseBroadcaster {"JointModelRUBX", "JointModelRUBY", "JointModelRUBZ"}; Eigen::VectorXd q; - rclcpp::Time last_publish_time_; + + rclcpp::Duration publish_elapsed_{0,0}; + rclcpp::Duration publish_interval_{0,0}; }; } // namespace crisp_controllers diff --git a/include/crisp_controllers/twist_broadcaster.hpp b/include/crisp_controllers/twist_broadcaster.hpp index 8838a6c..c6f9c89 100644 --- a/include/crisp_controllers/twist_broadcaster.hpp +++ b/include/crisp_controllers/twist_broadcaster.hpp @@ -65,7 +65,9 @@ class TwistBroadcaster Eigen::VectorXd q; Eigen::VectorXd q_dot; - rclcpp::Time last_publish_time_; + + rclcpp::Duration publish_elapsed_{0,0}; + rclcpp::Duration publish_interval_{0,0}; }; } // namespace crisp_controllers diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp index 7de3bfb..c78c27b 100644 --- a/src/pose_broadcaster.cpp +++ b/src/pose_broadcaster.cpp @@ -35,7 +35,7 @@ PoseBroadcaster::state_interface_configuration() const { controller_interface::return_type PoseBroadcaster::update(const rclcpp::Time &time, - const rclcpp::Duration & /*period*/) { + const rclcpp::Duration &period) { size_t num_joints = params_.joints.size(); Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq); @@ -63,28 +63,26 @@ PoseBroadcaster::update(const rclcpp::Time &time, Eigen::Quaterniond(current_pose.rotation()); // Decide whether to publish the pose or not - bool should_publish = true; - if (params_.publish_frequency > 0.0) { - auto time_since_last = time - last_publish_time_; - auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); - should_publish = time_since_last >= min_interval; - } - - if (should_publish && realtime_pose_publisher_ && realtime_pose_publisher_->trylock()) + publish_elapsed_ = publish_elapsed_ + period; + bool should_publish = (publish_elapsed_ >= publish_interval_) || + (publish_interval_.nanoseconds() == 0); + if (should_publish && realtime_pose_publisher_) { - auto & pose_msg = realtime_pose_publisher_->msg_; - - pose_msg.header.stamp = time; - pose_msg.header.frame_id = params_.base_frame; - pose_msg.pose.position.x = current_pose.translation()[0]; - pose_msg.pose.position.y = current_pose.translation()[1]; - pose_msg.pose.position.z = current_pose.translation()[2]; - pose_msg.pose.orientation.x = current_quaternion.x(); - pose_msg.pose.orientation.y = current_quaternion.y(); - pose_msg.pose.orientation.z = current_quaternion.z(); - pose_msg.pose.orientation.w = current_quaternion.w(); - realtime_pose_publisher_->unlockAndPublish(); - last_publish_time_ = time; + if (realtime_pose_publisher_->trylock()) { + auto & pose_msg = realtime_pose_publisher_->msg_; + pose_msg.header.stamp = time; + pose_msg.header.frame_id = params_.base_frame; + pose_msg.pose.position.x = current_pose.translation()[0]; + pose_msg.pose.position.y = current_pose.translation()[1]; + pose_msg.pose.position.z = current_pose.translation()[2]; + pose_msg.pose.orientation.x = current_quaternion.x(); + pose_msg.pose.orientation.y = current_quaternion.y(); + 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_; + } } return controller_interface::return_type::OK; @@ -168,7 +166,12 @@ CallbackReturn PoseBroadcaster::on_configure( std::make_shared>( pose_publisher_); - last_publish_time_ = this->get_node()->now(); + 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; } diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp index 1724db3..3b3b891 100644 --- a/src/twist_broadcaster.cpp +++ b/src/twist_broadcaster.cpp @@ -36,7 +36,7 @@ TwistBroadcaster::state_interface_configuration() const { controller_interface::return_type TwistBroadcaster::update(const rclcpp::Time &time, - const rclcpp::Duration & /*period*/) { + const rclcpp::Duration &period) { size_t num_joints = params_.joints.size(); Eigen::VectorXd q_pin = Eigen::VectorXd::Zero(model_.nq); @@ -67,27 +67,26 @@ TwistBroadcaster::update(const rclcpp::Time &time, auto current_velocity = pinocchio::getFrameVelocity(model_, data_, end_effector_frame_id); // Decide whether to publish the twist or not - bool should_publish = true; - if (params_.publish_frequency > 0.0) { - auto time_since_last = time - last_publish_time_; - auto min_interval = rclcpp::Duration::from_seconds(1.0 / params_.publish_frequency); - should_publish = time_since_last >= min_interval; - } + publish_elapsed_ = publish_elapsed_ + period; + bool should_publish = (publish_elapsed_ >= publish_interval_) || + (publish_interval_.nanoseconds() == 0); - if (should_publish && realtime_twist_publisher_ && realtime_twist_publisher_->trylock()) + if (should_publish && realtime_twist_publisher_) { - 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(); - last_publish_time_ = time; + 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_; + } } return controller_interface::return_type::OK; @@ -172,7 +171,12 @@ CallbackReturn TwistBroadcaster::on_configure( std::make_shared>( twist_publisher_); - last_publish_time_ = this->get_node()->now(); + 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; } From 877481d994c1a7d0dca2d9012c8f912324774d6d Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Mon, 22 Dec 2025 18:05:27 -0500 Subject: [PATCH 02/10] cleaned up cmake. clamped publish rate accumulation. reset time accumulator in on_activate --- CMakeLists.txt | 41 +++++++++++++++++++++++++++++++-------- src/pose_broadcaster.cpp | 4 ++++ src/twist_broadcaster.cpp | 4 ++++ 3 files changed, 41 insertions(+), 8 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index af6c486..cf05cec 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -18,17 +18,37 @@ endif() # Enable precompiled headers option(USE_PRECOMPILED_HEADERS "Use precompiled headers" ON) -# Enable optimizations -add_compile_options(-O3 -march=native -DNDEBUG) -if(NOT CMAKE_BUILD_TYPE) - set(CMAKE_BUILD_TYPE Release) -endif() +option(CHECK_TIDY "Adds clang-tidy tests" OFF) + +option(ENABLE_NATIVE_OPTIMIZATION + "Enable CPU-specific optimizations (-march=native)" + OFF +) -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) +# Default to release build +if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) + set(CMAKE_BUILD_TYPE Release CACHE STRING "Build type" FORCE) endif() -option(CHECK_TIDY "Adds clang-tidy tests" OFF) +# Compiler flags by build type +add_library(project_compile_options INTERFACE) +target_compile_options(project_compile_options INTERFACE + # Warnings (build only) + $<$:-Wall -Wextra -Wpedantic> + + # Debug + $<$,$>:-O0 -g> + + # Release + $<$,$>:-O3 -DNDEBUG> +) + +# Optional CPU-specific optimizations (non-portable) +if(ENABLE_NATIVE_OPTIMIZATION) + target_compile_options(project_compile_options INTERFACE + $<$,$>:-march=native> +) +endif() # find dependencies find_package(ament_cmake REQUIRED) @@ -71,6 +91,11 @@ add_library( src/twist_broadcaster.cpp ) +target_link_libraries(${PROJECT_NAME} + PRIVATE + project_compile_options +) + target_include_directories(${PROJECT_NAME} PUBLIC $ diff --git a/src/pose_broadcaster.cpp b/src/pose_broadcaster.cpp index c78c27b..a79b332 100644 --- a/src/pose_broadcaster.cpp +++ b/src/pose_broadcaster.cpp @@ -82,6 +82,8 @@ PoseBroadcaster::update(const rclcpp::Time &time, 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_); } } @@ -177,6 +179,8 @@ CallbackReturn PoseBroadcaster::on_configure( CallbackReturn PoseBroadcaster::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { + // reset publish time accumulation + publish_elapsed_ = rclcpp::Duration(0, 0); return CallbackReturn::SUCCESS; } diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp index 3b3b891..51e8fa7 100644 --- a/src/twist_broadcaster.cpp +++ b/src/twist_broadcaster.cpp @@ -86,6 +86,8 @@ TwistBroadcaster::update(const rclcpp::Time &time, 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_); } } @@ -182,6 +184,8 @@ CallbackReturn TwistBroadcaster::on_configure( CallbackReturn TwistBroadcaster::on_activate( const rclcpp_lifecycle::State & /*previous_state*/) { + // reset publish time accumulation + publish_elapsed_ = rclcpp::Duration(0, 0); return CallbackReturn::SUCCESS; } From ab87547200452d8d50086cab61644052dcfb4c57 Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Mon, 22 Dec 2025 18:23:05 -0500 Subject: [PATCH 03/10] default cmake enable_native_optimization=on --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index cf05cec..7c7db07 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,7 +22,7 @@ option(CHECK_TIDY "Adds clang-tidy tests" OFF) option(ENABLE_NATIVE_OPTIMIZATION "Enable CPU-specific optimizations (-march=native)" - OFF + ON ) # Default to release build From 9a93581c71092aed4dd567a1d9244584122ead99 Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Sun, 1 Mar 2026 02:02:00 -0500 Subject: [PATCH 04/10] allowing two concurrent pose publishers --- src/cartesian_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 38c076f..459062e 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -331,7 +331,7 @@ CallbackReturn CartesianController::on_configure( new_target_wrench_ = false; multiple_publishers_detected_ = false; - max_allowed_publishers_ = 1; + max_allowed_publishers_ = 2; auto target_pose_callback = [this](const std::shared_ptr msg) -> void From 23266043f1ccbb59db02151660e40143b5f2595c Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Wed, 4 Mar 2026 05:04:49 -0500 Subject: [PATCH 05/10] refactored EMA so it aligned with matlab func params. Also noticed EMA applied inconsistently and fixed that. Potential bug? --- include/crisp_controllers/utils/fiters.hpp | 6 +++--- src/cartesian_controller.cpp | 5 +++-- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/include/crisp_controllers/utils/fiters.hpp b/include/crisp_controllers/utils/fiters.hpp index 1a59200..3a90842 100644 --- a/include/crisp_controllers/utils/fiters.hpp +++ b/include/crisp_controllers/utils/fiters.hpp @@ -10,13 +10,13 @@ * @brief Compute the exponential moving average of a value * @param output Previous output value * @param current Current value measured to be filtered - * @param alpha Smoothing factor (0 < alpha < 1). The closer to 1, the more it + * @param alpha Smoothing factor (0 < alpha < 1). The closer to 0, the more it * smooths the value * @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 prev, const T current, const double alpha) { + return (1.0 - alpha) * prev + alpha * current; } /** diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index d8d0cc1..99a9978 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -86,7 +86,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & desired_position_ = exponential_moving_average(desired_position_, target_position_, params_.filter.target_pose); desired_orientation_ = - target_orientation_.slerp(params_.filter.target_pose, desired_orientation_); + desired_orientation_.slerp(params_.filter.target_pose, target_orientation_); /*target_pose_ = pinocchio::SE3(target_orientation_.toRotationMatrix(), * target_position_);*/ @@ -183,10 +183,11 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & tau_d << tau_task + tau_nullspace + tau_friction + tau_coriolis + tau_gravity + tau_joint_limits + tau_wrench; + tau_d = exponential_moving_average(tau_previous, tau_d, params_.filter.output_torque); + if (params_.limit_torques) { tau_d = saturateTorqueRate(tau_d, tau_previous, params_.max_delta_tau); } - tau_d = exponential_moving_average(tau_d, tau_previous, params_.filter.output_torque); if (!params_.stop_commands) { for (size_t i = 0; i < params_.joints.size(); ++i) { From 10e14c8b68e61aeb958de13e20d2566298e0221e Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Wed, 4 Mar 2026 05:06:08 -0500 Subject: [PATCH 06/10] merged from upstream --- src/twist_broadcaster.cpp | 5 ----- 1 file changed, 5 deletions(-) diff --git a/src/twist_broadcaster.cpp b/src/twist_broadcaster.cpp index f5d1d3a..37c46ce 100644 --- a/src/twist_broadcaster.cpp +++ b/src/twist_broadcaster.cpp @@ -37,15 +37,10 @@ TwistBroadcaster::state_interface_configuration() const { } controller_interface::return_type -<<<<<<< HEAD -TwistBroadcaster::update(const rclcpp::Time &time, - const rclcpp::Duration &period) { -======= 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); ->>>>>>> upstream/main for (size_t i = 0; i < num_joints; i++) { auto joint_name = params_.joints[i]; From 16c58b9727ef67a9cd7db7088944c294c1bed2a0 Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Thu, 5 Mar 2026 00:20:19 -0500 Subject: [PATCH 07/10] Revert "allowing two concurrent pose publishers" This reverts commit 9a93581c71092aed4dd567a1d9244584122ead99. --- src/cartesian_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 99a9978..cb5de9d 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -321,7 +321,7 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta new_target_wrench_ = false; multiple_publishers_detected_ = false; - max_allowed_publishers_ = 2; + max_allowed_publishers_ = 1; auto target_pose_callback = [this](const std::shared_ptr msg) -> void { From 64a4e72c4d1b66bdfc1ed0c1ba68685ffa78959a Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Thu, 5 Mar 2026 01:24:23 -0500 Subject: [PATCH 08/10] two publishers --- src/cartesian_controller.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index cb5de9d..99a9978 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -321,7 +321,7 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta new_target_wrench_ = false; multiple_publishers_detected_ = false; - max_allowed_publishers_ = 1; + max_allowed_publishers_ = 2; auto target_pose_callback = [this](const std::shared_ptr msg) -> void { From 20cca1382046639bc04539a0d54e2d9faa3803cc Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Mon, 6 Apr 2026 20:21:22 -0400 Subject: [PATCH 09/10] updated damping to account for traj vel --- .../cartesian_controller.hpp | 2 + src/cartesian_controller.cpp | 48 ++++++++++++++++++- 2 files changed, 48 insertions(+), 2 deletions(-) diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index ab3fbbf..ce4b1a4 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -159,6 +159,8 @@ class CartesianController : public controller_interface::ControllerInterface { /** @brief Desired target orientation as quaternion after applying filtering */ Eigen::Quaterniond desired_orientation_; + Eigen::VectorXd desired_twist_; + /** @brief Parameter listener for dynamic parameter updates */ std::shared_ptr params_listener_; /** @brief Current parameter values */ diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 99a9978..089299b 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -61,7 +61,7 @@ CartesianController::state_interface_configuration() const { } controller_interface::return_type -CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & /*period*/) { +CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & period) { // Update current state information with EMA filtered values updateCurrentState(); @@ -83,6 +83,49 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & pinocchio::forwardKinematics(model_, data_, q_pin, dq); pinocchio::updateFramePlacements(model_, data_); + // + // Compute twist + // + + double dt = period.seconds(); + + Eigen::Vector3d linear_velocity = ((target_position_ - desired_position_)/dt).eval(); + + Eigen::Vector3d angular_velocity; + + Eigen::Quaterniond q_delta = target_orientation_ * desired_orientation_.inverse(); + // Ensure shortest path + if (q_delta.w() < 0.0) { + q_delta.coeffs() *= -1.0; + } + + Eigen::AngleAxisd aa(q_delta); + if (aa.angle() < 1e-6) { + angular_velocity = Eigen::VectorXd::Zero(3); + } else { + angular_velocity = aa.axis() * aa.angle() / dt; + } + +// angular_velocity = +// pinocchio::log3( +// target_orientation_.toRotationMatrix() * +// desired_orientation_.toRotationMatrix().transpose() +// ) / dt; + + Eigen::Matrix3d R_transpose = end_effector_pose.rotation().transpose(); + Eigen::VectorXd target_twist = Eigen::VectorXd::Zero(6); + if (params_.use_local_jacobian){ + target_twist.head<3>() = R_transpose * linear_velocity; + target_twist.tail<3>() = R_transpose * angular_velocity; + } + + desired_twist_ = exponential_moving_average(desired_twist_, target_twist, params_.filter.target_pose); + + // RCLCPP_INFO_STREAM( + // get_node()->get_logger(), + // "twist: " << desired_twist_); + + desired_position_ = exponential_moving_average(desired_position_, target_position_, params_.filter.target_pose); desired_orientation_ = @@ -143,7 +186,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & if (params_.use_operational_space) { tau_task << J.transpose() * Mx * (stiffness * error - damping * (J * dq)); } else { - tau_task << J.transpose() * (stiffness * error - damping * (J * dq)); + tau_task << J.transpose() * (stiffness * error + damping * ( desired_twist_ - J * dq)); } if (model_.nq != model_.nv) { @@ -390,6 +433,7 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta target_orientation_ = Eigen::Quaterniond::Identity(); target_wrench_ = Eigen::VectorXd::Zero(6); desired_position_ = Eigen::Vector3d::Zero(); + desired_twist_ = Eigen::VectorXd::Zero(6); desired_orientation_ = Eigen::Quaterniond::Identity(); // Initialize error vector From 6e8356d2963d6c83cf2b0f7c35bc5f8b3139f99d Mon Sep 17 00:00:00 2001 From: Vidullan Surendran Date: Wed, 8 Apr 2026 08:58:08 -0400 Subject: [PATCH 10/10] takes in velocity reference in a seperate topic --- .../cartesian_controller.hpp | 20 +++- src/cartesian_controller.cpp | 91 ++++++++++++------- src/cartesian_controller.yaml | 10 +- 3 files changed, 88 insertions(+), 33 deletions(-) diff --git a/include/crisp_controllers/cartesian_controller.hpp b/include/crisp_controllers/cartesian_controller.hpp index ce4b1a4..bdfbf97 100644 --- a/include/crisp_controllers/cartesian_controller.hpp +++ b/include/crisp_controllers/cartesian_controller.hpp @@ -13,6 +13,7 @@ #include #include +#include #include #include #include @@ -29,6 +30,7 @@ #include #include "realtime_tools/realtime_buffer.hpp" + using CallbackReturn = rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn; namespace crisp_controllers { @@ -98,6 +100,8 @@ class CartesianController : public controller_interface::ControllerInterface { private: /** @brief Subscription for target pose messages */ rclcpp::Subscription::SharedPtr pose_sub_; + /** @brief Subscription for target twist messages */ + rclcpp::Subscription::SharedPtr twist_sub_; /** @brief Subscription for target joint state messages */ rclcpp::Subscription::SharedPtr joint_sub_; /** @brief Subscription for target wrench messages */ @@ -125,6 +129,11 @@ class CartesianController : public controller_interface::ControllerInterface { */ void parse_target_pose_(); + /** + * @brief Reads the target twist in realtime loop from the buffer and parses it to be used in the controller. + */ + void parse_target_twist_(); + /** * @brief Reads the target joint in realtime loop from the buffer and parses it to be used in the controller. */ @@ -136,11 +145,15 @@ class CartesianController : public controller_interface::ControllerInterface { void parse_target_wrench_(); bool new_target_pose_; + bool new_target_twist_; bool new_target_joint_; bool new_target_wrench_; realtime_tools::RealtimeBuffer> target_pose_buffer_; + + realtime_tools::RealtimeBuffer> + target_twist_buffer_; realtime_tools::RealtimeBuffer> target_joint_buffer_; @@ -150,15 +163,20 @@ class CartesianController : public controller_interface::ControllerInterface { /** @brief Target position in Cartesian space */ Eigen::Vector3d target_position_; + + /** @brief Target twist */ + Eigen::VectorXd target_twist_; + /** @brief Target orientation as quaternion */ Eigen::Quaterniond target_orientation_; /** @brief Target wrench in task space */ Eigen::VectorXd target_wrench_; + /** @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 Desired target 6D twist after applying filtering */ Eigen::VectorXd desired_twist_; /** @brief Parameter listener for dynamic parameter updates */ diff --git a/src/cartesian_controller.cpp b/src/cartesian_controller.cpp index 089299b..4b54e7f 100644 --- a/src/cartesian_controller.cpp +++ b/src/cartesian_controller.cpp @@ -71,6 +71,10 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & parse_target_pose_(); new_target_pose_ = false; } + if (new_target_twist_) { + parse_target_twist_(); + new_target_twist_ = false; + } if (new_target_joint_) { parse_target_joint_(); new_target_joint_ = false; @@ -87,39 +91,39 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & // Compute twist // - double dt = period.seconds(); +// double dt = period.seconds(); - Eigen::Vector3d linear_velocity = ((target_position_ - desired_position_)/dt).eval(); +// Eigen::Vector3d linear_velocity = ((target_position_ - desired_position_)/dt).eval(); - Eigen::Vector3d angular_velocity; +// Eigen::Vector3d angular_velocity; - Eigen::Quaterniond q_delta = target_orientation_ * desired_orientation_.inverse(); - // Ensure shortest path - if (q_delta.w() < 0.0) { - q_delta.coeffs() *= -1.0; - } - - Eigen::AngleAxisd aa(q_delta); - if (aa.angle() < 1e-6) { - angular_velocity = Eigen::VectorXd::Zero(3); - } else { - angular_velocity = aa.axis() * aa.angle() / dt; - } - -// angular_velocity = -// pinocchio::log3( -// target_orientation_.toRotationMatrix() * -// desired_orientation_.toRotationMatrix().transpose() -// ) / dt; - - Eigen::Matrix3d R_transpose = end_effector_pose.rotation().transpose(); - Eigen::VectorXd target_twist = Eigen::VectorXd::Zero(6); - if (params_.use_local_jacobian){ - target_twist.head<3>() = R_transpose * linear_velocity; - target_twist.tail<3>() = R_transpose * angular_velocity; - } - - desired_twist_ = exponential_moving_average(desired_twist_, target_twist, params_.filter.target_pose); +// Eigen::Quaterniond q_delta = target_orientation_ * desired_orientation_.inverse(); +// // Ensure shortest path +// if (q_delta.w() < 0.0) { +// q_delta.coeffs() *= -1.0; +// } + +// Eigen::AngleAxisd aa(q_delta); +// if (aa.angle() < 1e-6) { +// angular_velocity = Eigen::VectorXd::Zero(3); +// } else { +// angular_velocity = aa.axis() * aa.angle() / dt; +// } + +// // angular_velocity = +// // pinocchio::log3( +// // target_orientation_.toRotationMatrix() * +// // desired_orientation_.toRotationMatrix().transpose() +// // ) / dt; + +// Eigen::Matrix3d R_transpose = end_effector_pose.rotation().transpose(); +// Eigen::VectorXd target_twist = Eigen::VectorXd::Zero(6); +// if (params_.use_local_jacobian){ +// target_twist.head<3>() = R_transpose * linear_velocity; +// target_twist.tail<3>() = R_transpose * angular_velocity; +// } + + desired_twist_ = exponential_moving_average(desired_twist_, target_twist_, params_.filter.target_pose); // RCLCPP_INFO_STREAM( // get_node()->get_logger(), @@ -184,7 +188,7 @@ CartesianController::update(const rclcpp::Time & time, const rclcpp::Duration & } if (params_.use_operational_space) { - tau_task << J.transpose() * Mx * (stiffness * error - damping * (J * dq)); + tau_task << J.transpose() * Mx * (stiffness * error + damping * (desired_twist_ - J * dq)); } else { tau_task << J.transpose() * (stiffness * error + damping * ( desired_twist_ - J * dq)); } @@ -360,6 +364,7 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta setStiffnessAndDamping(); new_target_pose_ = false; + new_target_twist_ = false; new_target_joint_ = false; new_target_wrench_ = false; @@ -380,6 +385,20 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta new_target_pose_ = true; }; + auto target_twist_callback = + [this](const std::shared_ptr msg) -> void { + if (!check_topic_publisher_count("target_twist")) { + RCLCPP_WARN_THROTTLE( + get_node()->get_logger(), + *get_node()->get_clock(), + 1000, + "Ignoring target_pose message due to multiple publishers detected!"); + return; + } + target_twist_buffer_.writeFromNonRT(msg); + new_target_twist_ = true; + }; + auto target_joint_callback = [this](const std::shared_ptr msg) -> void { if (!check_topic_publisher_count("target_joint")) { @@ -411,6 +430,9 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta pose_sub_ = get_node()->create_subscription( "target_pose", rclcpp::QoS(1), target_pose_callback); + twist_sub_ = get_node()->create_subscription( + "target_twist", rclcpp::QoS(1), target_twist_callback); + joint_sub_ = get_node()->create_subscription( "target_joint", rclcpp::QoS(1), target_joint_callback); @@ -430,6 +452,7 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta // Initialize target state vectors target_position_ = Eigen::Vector3d::Zero(); + target_twist_ = Eigen::VectorXd::Zero(6); target_orientation_ = Eigen::Quaterniond::Identity(); target_wrench_ = Eigen::VectorXd::Zero(6); desired_position_ = Eigen::Vector3d::Zero(); @@ -582,6 +605,12 @@ void CartesianController::parse_target_pose_() { msg->pose.orientation.z); } +void CartesianController::parse_target_twist_() { + auto msg = *target_twist_buffer_.readFromRT(); + target_twist_ << msg->twist.linear.x, msg->twist.linear.y, msg->twist.linear.z, + msg->twist.angular.x, msg->twist.angular.y, msg->twist.angular.z; +} + void CartesianController::parse_target_joint_() { auto msg = *target_joint_buffer_.readFromRT(); if (msg->position.size()) { diff --git a/src/cartesian_controller.yaml b/src/cartesian_controller.yaml index b59eb26..b43fe9e 100644 --- a/src/cartesian_controller.yaml +++ b/src/cartesian_controller.yaml @@ -231,10 +231,18 @@ cartesian_controller: filter: target_pose: type: double - default_value: 0.1 + default_value: 1.0 description: "Amount of smoothing for the target pose when using the EMA. The EMA is applied at each step" validation: bounds<>: [0.1, 1.0] + + target_twist: + type: double + default_value: 0.5 + description: "Amount of smoothing for the target twist when using the EMA. The EMA is applied at each step" + validation: + bounds<>: [0.1, 1.0] + q: type: double default_value: 0.5