Vel desired damping

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

include/crisp_controllers/cartesian_controller.hpp

@@ -13,6 +13,7 @@
 
 #include <controller_interface/controller_interface.hpp>
 #include <geometry_msgs/msg/pose_stamped.hpp>
+#include <geometry_msgs/msg/twist_stamped.hpp>
 #include <geometry_msgs/msg/wrench_stamped.hpp>
 #include <pinocchio/algorithm/kinematics.hpp>
 #include <pinocchio/multibody/fwd.hpp>
@@ -29,6 +30,7 @@
 #include <sensor_msgs/msg/joint_state.hpp>
 #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<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+  /** @brief Subscription for target twist messages */
+  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr twist_sub_;
   /** @brief Subscription for target joint state messages */
   rclcpp::Subscription<sensor_msgs::msg::JointState>::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<std::shared_ptr<geometry_msgs::msg::PoseStamped>>
     target_pose_buffer_;
+
+  realtime_tools::RealtimeBuffer<std::shared_ptr<geometry_msgs::msg::TwistStamped>>
+    target_twist_buffer_;
 
   realtime_tools::RealtimeBuffer<std::shared_ptr<sensor_msgs::msg::JointState>>
     target_joint_buffer_;
@@ -150,14 +163,21 @@ 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 */
   std::shared_ptr<cartesian_controller::ParamListener> params_listener_;

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 <typename T>
-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;
 }
 
 /**

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();
@@ -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;
@@ -83,10 +87,53 @@ 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_ =
-    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_);*/
@@ -141,9 +188,9 @@ 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 * (J * dq));
+    tau_task << J.transpose() * (stiffness * error + damping * ( desired_twist_ - J * dq));
   }
 
   if (model_.nq != model_.nv) {
@@ -183,10 +230,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) {
@@ -316,11 +364,12 @@ 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;
 
   multiple_publishers_detected_ = false;
-  max_allowed_publishers_ = 1;
+  max_allowed_publishers_ = 2;
 
   auto target_pose_callback =
     [this](const std::shared_ptr<geometry_msgs::msg::PoseStamped> msg) -> void {
@@ -336,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<geometry_msgs::msg::TwistStamped> 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<sensor_msgs::msg::JointState> msg) -> void {
     if (!check_topic_publisher_count("target_joint")) {
@@ -367,6 +430,9 @@ CartesianController::on_configure(const rclcpp_lifecycle::State & /*previous_sta
   pose_sub_ = get_node()->create_subscription<geometry_msgs::msg::PoseStamped>(
     "target_pose", rclcpp::QoS(1), target_pose_callback);
 
+  twist_sub_ = get_node()->create_subscription<geometry_msgs::msg::TwistStamped>(
+    "target_twist", rclcpp::QoS(1), target_twist_callback);
+
   joint_sub_ = get_node()->create_subscription<sensor_msgs::msg::JointState>(
     "target_joint", rclcpp::QoS(1), target_joint_callback);
 
@@ -386,9 +452,11 @@ 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();
+  desired_twist_ = Eigen::VectorXd::Zero(6);
   desired_orientation_ = Eigen::Quaterniond::Identity();
 
   // Initialize error vector
@@ -537,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()) {

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