Package fr3_impedance_controller_real in nix env

Including all deps. Tested with nix develop then roslaunch franka_example_controllers ....

Issue with libfranka version, in the end needed to use fetchGithub in Nix to get proper files.

Closes #7 (closed)

fr3_impedance_controller_real/src/cartesian_impedance_example_controller.cpp

+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#include <franka_example_controllers/cartesian_impedance_example_controller.h>
+
+#include <cmath>
+#include <iomanip>
+#include <memory>
+
+#include <controller_interface/controller_base.h>
+#include <franka/robot_state.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+#include <franka_example_controllers/pseudo_inversion.h>
+
+/* Custom libraries */
+#include "orientation_utilities.hpp"
+#include "robotics_utilities.hpp"
+
+namespace pin = pinocchio;
+namespace ori = orient_utils;
+namespace robo = robot_utils;
+
+using namespace std;
+// using pin::ReferenceFrame;
+
+namespace franka_example_controllers
+{
+bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW * robot_hw,
+                                               ros::NodeHandle & node_handle)
+{
+  //* Load URDF model and data *//
+  if (urdf_filename_.empty())
+  {
+    throw std::runtime_error("URDF filename is invalid");
+  }
+  pin::urdf::buildModel(urdf_filename_, model_);
+  data_ = pin::Data(model_);
+  ee_id_ = model_.getFrameId("fr3_hand_tcp", (pin::FrameType::BODY));
+
+  //* Set Publisher *//
+  pub_ctrl_state_ = node_handle.advertise<fr3_custom_msgs::ControllerState>("controller_state", 20);
+
+  //* Set Subscriber *//
+  // NOTE: you need to change topic name as `/spacenav/joy` if you want to command gripper
+  sub_joy_ = node_handle.subscribe<sensor_msgs::Joy>(
+    "/joy", 1, &CartesianImpedanceExampleController::subscribe_joy_command, this);
+
+  // FIXME: spacenav is not operating !!
+  // sub_target_twist_ = node_handle.subscribe<geometry_msgs::Twist>(
+  //   "/spacenav/twist", 1, &CartesianImpedanceExampleController::subscribe_target_twist, this);
+
+  //* Set HW interface *//
+  std::string arm_id;
+  if (!node_handle.getParam("arm_id", arm_id))
+  {
+    ROS_ERROR_STREAM("CartesianImpedanceExampleController: Could not read parameter arm_id");
+    return false;
+  }
+  std::vector<std::string> joint_names;
+  if (!node_handle.getParam("joint_names", joint_names) || joint_names.size() != 7)
+  {
+    ROS_ERROR(
+      "CartesianImpedanceExampleController: Invalid or no joint_names parameters provided, "
+      "aborting controller init!");
+    return false;
+  }
+
+  auto * model_interface = robot_hw->get<franka_hw::FrankaModelInterface>();
+  if (model_interface == nullptr)
+  {
+    ROS_ERROR_STREAM(
+      "CartesianImpedanceExampleController: Error getting model interface from hardware");
+    return false;
+  }
+  try
+  {
+    model_handle_ =
+      std::make_unique<franka_hw::FrankaModelHandle>(model_interface->getHandle(arm_id + "_model"));
+  }
+  catch (hardware_interface::HardwareInterfaceException & ex)
+  {
+    ROS_ERROR_STREAM(
+      "CartesianImpedanceExampleController: Exception getting model handle from interface: "
+      << ex.what());
+    return false;
+  }
+
+  auto * state_interface = robot_hw->get<franka_hw::FrankaStateInterface>();
+  if (state_interface == nullptr)
+  {
+    ROS_ERROR_STREAM(
+      "CartesianImpedanceExampleController: Error getting state interface from hardware");
+    return false;
+  }
+  try
+  {
+    state_handle_ =
+      std::make_unique<franka_hw::FrankaStateHandle>(state_interface->getHandle(arm_id + "_robot"));
+  }
+  catch (hardware_interface::HardwareInterfaceException & ex)
+  {
+    ROS_ERROR_STREAM(
+      "CartesianImpedanceExampleController: Exception getting state handle from interface: "
+      << ex.what());
+    return false;
+  }
+
+  auto * effort_joint_interface = robot_hw->get<hardware_interface::EffortJointInterface>();
+  if (effort_joint_interface == nullptr)
+  {
+    ROS_ERROR_STREAM(
+      "CartesianImpedanceExampleController: Error getting effort joint interface from hardware");
+    return false;
+  }
+  for (size_t i = 0; i < 7; ++i)
+  {
+    try
+    {
+      joint_handles_.push_back(effort_joint_interface->getHandle(joint_names[i]));
+    }
+    catch (const hardware_interface::HardwareInterfaceException & ex)
+    {
+      ROS_ERROR_STREAM(
+        "CartesianImpedanceExampleController: Exception getting joint handles: " << ex.what());
+      return false;
+    }
+  }
+
+  //* Initialize states *//
+  this->init_states();  // NOTE: this should be executed aforehand of parameter callback
+
+  //* Parameter server declaration *//
+  ctrl_param_node_ = ros::NodeHandle(node_handle.getNamespace() + "/controller_param_node");
+
+  ctrl_param_server_ = std::make_unique<
+    dynamic_reconfigure::Server<franka_example_controllers::controller_paramConfig>>(
+    ctrl_param_node_);
+
+  ctrl_param_server_->setCallback(
+    boost::bind(&CartesianImpedanceExampleController::request_controller_parameters, this, _1, _2));
+
+  //* Create controllers *//
+  imp_ = std::make_unique<ImpedanceController<double>>();
+  dob_ = std::make_unique<DisturbanceObserver<double>>(fq_);
+  mob_ = std::make_unique<MomentumObserver<double>>(r_th_, km_);
+  sosml_ = std::make_unique<SlidingModeObserver<double>>(s1_, s2_, t1_, t2_);
+
+  return true;
+}
+
+void CartesianImpedanceExampleController::starting(const ros::Time & /*time*/)
+{
+  // compute initial velocity with jacobian and set x_attractor
+  franka::RobotState initial_state = state_handle_->getRobotState();
+  Eigen::Map<Vec7<double>> q_init(initial_state.q.data());
+  Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
+
+  /* set equilibrium pose to current state */
+  ee_pos_world_des_ = initial_transform.translation();
+  ee_quat_world_des_ = Eigen::Quaterniond(initial_transform.rotation());
+
+  /* set nullspace equilibrium configuration to initial q */
+  q_null_des_ = q_init;
+}
+
+void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
+                                                 const ros::Duration & period)
+{
+  if (b_count_loop_time_ == true)
+  {
+    loop_timer_.start();
+  }
+  // cout << std::fixed;
+  // cout << std::setprecision(5);
+  // TODO: add safety logic for time derivative when dt is very small
+  // double dt = period.toSec();  // timestep
+  double dt = 0.001;
+
+  //* ----- set Desired EE Pose & Twist ------------------------------------------------------------
+  // TODO: Add safety for FR3 end-effector's vel, acc, jerk limits
+  Vec3<double> v_des = ee_vel_world_des_.head(3);
+  Vec3<double> w_des = ee_vel_world_des_.tail(3);
+  ee_pos_world_des_ += v_des * dt;
+  ee_quat_world_des_ = ori::getNewQuat(ee_quat_world_des_, w_des, dt);
+  // ee_rpy_world_des_ = ori::quaternionToRPY(ee_quat_world_des_, ee_rpy_world_des_prev_);
+
+  //* ----- get Current States  --------------------------------------------------------------------
+  franka::RobotState robot_state = state_handle_->getRobotState();
+  Eigen::Map<Vec7<double>> tau_J_d(robot_state.tau_J_d.data());  // NOLINT
+  Eigen::Map<const Vec6<double>> tau_ext_hat_filtered(robot_state.tau_ext_hat_filtered.data());
+  q_mes_ = Eigen::Map<const Vec7<double>>(robot_state.q.data());
+  dq_mes_ = Eigen::Map<const Vec7<double>>(robot_state.dq.data());
+
+  // TODO: check noise level
+  // ddq_eul_ = robo::getEulerDerivative(dq_mes_, dq_mes_prev_, dt);
+  ddq_eul_ = robo::getFilteredEulerDerivative(dq_mes_, dq_mes_prev_, ddq_eul_prev_, dt, 10.0);
+
+  pin::forwardKinematics(model_, data_, q_mes_, dq_mes_);
+  pin::updateFramePlacements(model_, data_);
+
+  ee_pos_world_mes_ = data_.oMf[ee_id_].translation();
+  RotMat<double> R_OE = data_.oMf[ee_id_].rotation();
+  ee_quat_world_mes_ = Quat<double>(R_OE);
+  // ee_rpy_world_mes_ = ori::quaternionToRPY(ee_quat_world_mes_, ee_rpy_world_mes_prev_);
+
+  //* ----- compute Kinematics & Dynamics ----------------------------------------------------------
+  /* Jacobian */
+  Mat67<double> Je, dJe;
+  pin::getFrameJacobian(model_, data_, ee_id_, pin::ReferenceFrame::LOCAL_WORLD_ALIGNED, Je);
+  pin::getFrameJacobianTimeVariation(model_, data_, ee_id_,
+                                     pin::ReferenceFrame::LOCAL_WORLD_ALIGNED, dJe);
+
+  Mat76<double> Je_t = Je.transpose();
+  MatX<double> Je_pinv, Je_t_pinv;
+  pseudoInverse(Je, Je_pinv);
+  pseudoInverse(Je_t, Je_t_pinv);
+  // NOTE: using `pseudoInverse()` might be more safe against singularities
+  // MatX<double> Je_pinv = Je.completeOrthogonalDecomposition().pseudoInverse();
+  // MatX<double> Je_t_pinv = Je.transpose().completeOrthogonalDecomposition().pseudoInverse();
+
+  /* compute current EE Twist & Acceleration using backward Euler method */
+  ee_vel_world_mes_ = Je * dq_mes_;
+  // ee_acc_world_eul_ = robo::getEulerDerivative(ee_vel_world_mes_, ee_vel_world_mes_prev_, dt);
+  ee_acc_world_eul_ = robo::getFilteredEulerDerivative(ee_vel_world_mes_, ee_vel_world_mes_prev_, ee_acc_world_eul_prev_, dt, 10.0);
+
+  /* Inertia matrix */
+  pin::crba(model_, data_, q_mes_);
+  // Mat7<double> M = data_.M;  // upper triangular matrix in joint-space
+  // Vec7<double> k_mass;
+  // k_mass << 1.0, 1.0, 1.0, 1.0, 10.0, 10.0, 50.0;
+  // M.diagonal().array() *= k_mass.array();
+  Mat7<double> M = robo::computeFullInertiaMatrix(data_.M);
+  Mat6<double> Me = robo::computeTaskInertiaMatrix(M, Je_pinv, Je_t_pinv);
+
+  /* Coriolis, Centrifugal & Gravity torque */
+  pin::computeCoriolisMatrix(model_, data_, q_mes_, dq_mes_);
+  Mat7<double> C = data_.C;
+  Vec7<double> tau_c = C * dq_mes_;
+
+  // pin::computeGeneralizedGravity(model_, data_, q_mes_);  // * gravity is compensated internally
+  // Vec7<double> tau_g = data_.g;
+  Vec7<double> tau_g;
+  tau_g.setZero();
+
+  //* ----- CONTROL LAW ----------------------------------------------------------------------------
+  /* Cartesian impedance control */
+  imp_->set_parameters(K_task_, D_task_, K_null_, D_null_);
+  Vec6<double> ee_pose_err = imp_->get_pose_error(ee_pos_world_des_, ee_pos_world_mes_,
+                                                  ee_quat_world_des_, ee_quat_world_mes_);
+  Vec6<double> ee_pose_err_prev = imp_->get_previous_pose_error();
+  Vec6<double> dpose_err = robo::getEulerDerivative(ee_pose_err, ee_pose_err_prev, dt);
+  Vec6<double> F_task = imp_->get_task_impedance_force(dpose_err);
+
+  /* Task-Force Disturbance compensation */
+  Vec7<double> tau_task = Je_t * (F_task - (b_dob_applied_ ? Fd_hat_dob_ : Vec6<double>::Zero()));
+
+  /* Null-space impedance control */
+  imp_->compute_null_error(q_null_des_, q_mes_);
+  Vec7<double> tau_null = imp_->get_null_impedance_torque(dq_mes_, Je_t, Je_t_pinv);
+
+  /* compute desired joint torque command */
+  Vec7<double> tau_des = tau_task + tau_null + tau_c;
+  tau_des << saturateTorqueRate(tau_des, tau_J_d);  // Saturate torque rate to avoid discontinuities
+
+  //* ----- Observers ------------------------------------------------------------------------------
+  Fd_hat_fr_ = Je_t_pinv * tau_ext_hat_filtered;
+
+  /* Momentum Observer (MOB) */
+  h_mes_ = M * dq_mes_;
+  Vec7<double> dh_mes = robo::getEulerDerivative(h_mes_, h_mes_prev_, dt);
+
+  mob_->set_parameters(dt, r_th_, km_); // TODO: set parameter similar level to DOB for comparison
+  mob_->set_dynamics(tau_des, M, C, tau_g);
+  mob_->compute_residual(dh_mes, dq_mes_);
+
+  ddq_hat_mob_ = mob_->get_joint_accel_estimation(dq_mes_);
+  ee_acc_world_hat_mob_ = robo::getTaskAcceleration(Je, dJe, dq_mes_, ddq_hat_mob_);
+  tau_d_hat_mob_ = mob_->get_external_torque_estimation();
+  Fd_hat_mob_ = Je_t_pinv * tau_d_hat_mob_;
+
+  /* 2nd-order Sliding mode Observer (SOSML) */
+  sosml_->set_parameters(dt, s1_, s2_, t1_, t2_);
+  sosml_->set_dynamics(tau_des, M, C, tau_g);
+  sosml_->compute_momentum_estimation(dq_mes_);
+  ddq_hat_sosml_ = sosml_->get_joint_accel_estimation();
+  ee_acc_world_hat_sosml_ = robo::getTaskAcceleration(Je, dJe, dq_mes_, ddq_hat_sosml_);
+  tau_d_hat_sosml_ = sosml_->get_external_torque_estimation();
+  Fd_hat_sosml_ = Je_t_pinv * tau_d_hat_sosml_;
+
+  /* Task-space Force Disturbance Observer (TFDOB) */
+  dob_->set_parameters(dt, fq_);
+  Vec6<double> ddx = (b_sosml_applied_ ? ee_acc_world_hat_sosml_ : ee_acc_world_eul_);
+  Fd_hat_dob_ = dob_->get_force_disturbance(F_task, Me, ddx);
+  tau_d_hat_dob_ = Je_t * Fd_hat_dob_;
+
+  //* send joint torque command *//
+  for (size_t i = 0; i < 7; ++i)
+  {
+    joint_handles_[i].setCommand(tau_des(i));
+  }
+  this->publish_controller_state();  // publish states for rosbag
+
+  //* update previous states *//
+  this->update_previous_states();
+  imp_->update_previous_states();
+  dob_->update_previous_states();
+
+  //* update parameters online either via dynamic reconfigure or target by filtering *//
+  /* update desired twist via joystick */
+  ee_vel_world_des_ = alpha_ * ee_vel_world_target_ + (1.0 - alpha_) * ee_vel_world_des_;
+
+  /* update controller parameters */
+  K_task_ = alpha_ * K_task_target_ + (1.0 - alpha_) * K_task_;
+  D_task_ = alpha_ * D_task_target_ + (1.0 - alpha_) * D_task_;
+  K_null_ = alpha_ * K_null_target_ + (1.0 - alpha_) * K_null_;
+  D_null_ = alpha_ * D_null_target_ + (1.0 - alpha_) * D_null_;
+
+  fq_ = alpha_ * fq_target_ + (1.0 - alpha_) * fq_;
+  s1_ = alpha_ * s1_target_ + (1.0 - alpha_) * s1_;
+  s2_ = alpha_ * s2_target_ + (1.0 - alpha_) * s2_;
+  t1_ = s1_.array().sqrt();
+  t2_ = s2_.array().sqrt();
+  r_th_ = alpha_ * r_th_target_ + (1.0 - alpha_) * r_th_;
+  km_ = alpha_ * km_target_ + (1.0 - alpha_) * km_;
+
+  if(b_count_loop_time_ == true)
+  {
+    if(max_exec_time_ < loop_timer_.getMillisec())
+    {
+      max_exec_time_ = loop_timer_.getMillisec();
+    }
+    cout << "[Execution time] " << loop_timer_.getMillisec() << "(msec) [Max] " << max_exec_time_ << endl;
+  }
+}
+
+//* ----- SUBSCRIBE CALLBACK -----------------------------------------------------------------------
+void CartesianImpedanceExampleController::subscribe_target_twist(
+  const geometry_msgs::TwistConstPtr & msg)
+{
+  // cout << msg->linear.x <<  ", " << msg->linear.y << ", " << msg->linear.z << endl;
+  ee_vel_world_des_[0] = twist_scale_[0] * msg->linear.x;
+  ee_vel_world_des_[1] = twist_scale_[1] * msg->linear.y;
+  ee_vel_world_des_[2] = twist_scale_[2] * msg->linear.z;
+  ee_vel_world_des_[3] = twist_scale_[3] * msg->angular.x;
+  ee_vel_world_des_[4] = twist_scale_[4] * msg->angular.y;
+  ee_vel_world_des_[5] = twist_scale_[5] * msg->angular.z;
+}
+
+void CartesianImpedanceExampleController::subscribe_joy_command(
+  const sensor_msgs::JoyConstPtr & msg)
+{
+  // btn_cmd_[0] = msg->buttons[0];
+  // btn_cmd_[1] = msg->buttons[1];
+
+  ee_vel_world_target_[0] = static_cast<double>(twist_scale_[0] * msg->axes[0]);  // x
+  ee_vel_world_target_[1] = static_cast<double>(twist_scale_[1] * msg->axes[1]);  // y
+  ee_vel_world_target_[2] = static_cast<double>(twist_scale_[2] * msg->axes[5]);  // z
+  ee_vel_world_target_[3] = static_cast<double>(twist_scale_[3] * msg->axes[2]);  // roll
+  ee_vel_world_target_[4] = static_cast<double>(twist_scale_[4] * msg->axes[3]);  // pitch
+  ee_vel_world_target_[5] = static_cast<double>(twist_scale_[5] * msg->axes[4]);  // yaw
+}
+
+//* ----- PARAMETER SERVER CALLBACK ----------------------------------------------------------------
+void CartesianImpedanceExampleController::request_controller_parameters(
+  franka_example_controllers::controller_paramConfig & config, uint32_t /*level*/)
+{
+  //* Impedance Controller (NOTE: Damping ratio = 1) *//
+  /* task-space PD gains */
+  K_task_target_.setIdentity().diagonal() << config.K_task_x, config.K_task_y, config.K_task_z,  //
+    config.K_task_rot, config.K_task_rot, config.K_task_rot;
+  D_task_target_.setIdentity().diagonal() << 2.0 * K_task_target_.diagonal().array().sqrt();
+
+  /* null-space PD gains */
+  K_null_target_.setIdentity().diagonal() << config.K_null, config.K_null, config.K_null,  //
+    config.K_null, config.K_null, config.K_null, config.K_null;
+  D_null_target_.setIdentity().diagonal() << 2.0 * K_null_target_.diagonal().array().sqrt();
+
+  //* Disturbance Observer *//
+  fq_target_ << config.fq, config.fq, config.fq, config.fq, config.fq, config.fq;
+
+  //* Sliding Mode Observer *//
+  s1_target_ << config.s1, config.s1, config.s1, config.s1, config.s1, config.s1, config.s1;
+  s2_target_ << config.s2, config.s2, config.s2, config.s2, config.s2, config.s2, config.s2;
+
+  //* Momentum Observer *//
+  r_th_target_ << config.r_th, config.r_th, config.r_th, config.r_th, config.r_th, config.r_th,
+    config.r_th;
+  km_target_ << config.Km, config.Km, config.Km, config.Km, config.Km, config.Km, config.Km;
+}
+
+//* ----- METHODS ----------------------------------------------------------------------------------
+void CartesianImpedanceExampleController::init_states()
+{
+  //* subscriber *//
+  ee_vel_world_target_.setZero();
+
+  //* control parameters *//
+  /* impedance */
+  K_task_.setIdentity().diagonal() << 200.0, 200.0, 200.0, 10.0, 10.0, 10.0;  // TODO: set default
+  K_task_target_.setIdentity();
+  D_task_.setIdentity().diagonal() << 2.0 * K_task_.diagonal().array().sqrt();
+  D_task_target_.setIdentity();
+  K_null_.setIdentity().diagonal() << 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5;
+  K_null_target_.setIdentity();
+  D_null_.setIdentity().diagonal() << 2.0 * K_null_.diagonal().array().sqrt();
+  D_null_target_.setIdentity();
+
+  /* DOB */
+  fq_ << 15.0, 15.0, 15.0, 15.0, 15.0, 15.0;
+  fq_target_.setZero();
+
+  /* SOSML */
+  s1_ << 10, 10, 10, 10, 10, 10, 10;
+  s1_target_.setZero();
+  s2_ << 200, 200, 200, 200, 200, 200, 200;
+  s2_target_.setZero();
+  t1_ = s1_.array().sqrt();
+  t1_target_.setZero();
+  t2_ = s2_.array().sqrt();
+  t2_target_.setZero();
+
+  /* MOB */
+  r_th_ << 0, 0, 0, 0, 0, 0, 0;
+  r_th_target_.setZero();
+  km_ << 2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5;
+  km_target_.setZero();
+
+  //* Control states *//
+  q_mes_.setZero();
+  dq_mes_.setZero();
+  dq_mes_prev_.setZero();
+  ddq_eul_.setZero();
+  ddq_eul_prev_.setZero();
+  q_null_des_.setZero();
+
+  ee_pos_world_des_.setZero();
+  ee_quat_world_des_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+  ee_rpy_world_des_.setZero();
+  ee_rpy_world_des_prev_.setZero();
+  ee_vel_world_des_.setZero();
+  ee_vel_world_des_prev_.setZero();
+  ee_acc_world_des_.setZero();
+  ee_acc_world_des_prev_.setZero();
+
+  ee_pos_world_mes_.setZero();
+  ee_quat_world_mes_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+  ee_rpy_world_mes_.setZero();
+  ee_rpy_world_mes_prev_.setZero();
+  ee_vel_world_mes_.setZero();
+  ee_vel_world_mes_prev_.setZero();
+  ee_acc_world_eul_.setZero();
+  ee_acc_world_eul_prev_.setZero();
+
+  tau_d_hat_dob_.setZero();
+  Fd_hat_dob_.setZero();
+
+  ddq_hat_sosml_.setZero();
+  tau_d_hat_sosml_.setZero();
+  ee_acc_world_hat_sosml_.setZero();
+  Fd_hat_sosml_.setZero();
+
+  h_mes_.setZero();
+  h_mes_prev_.setZero();
+  ddq_hat_mob_.setZero();
+  ee_acc_world_hat_mob_.setZero();
+  tau_d_hat_mob_.setZero();
+  Fd_hat_mob_.setZero();
+
+  Fd_hat_fr_.setZero();
+}
+
+void CartesianImpedanceExampleController::publish_controller_state()
+{
+  for (int i = 0; i < 7; ++i)
+  {
+    msg_ctrl_state_.ddq_eul[i] = ddq_eul_[i];
+    msg_ctrl_state_.ddq_mob[i] = ddq_hat_mob_[i];
+    msg_ctrl_state_.ddq_sosml[i] = ddq_hat_sosml_[i];
+
+    msg_ctrl_state_.tau_dist_dob[i] = tau_d_hat_dob_[i];
+    msg_ctrl_state_.tau_dist_mob[i] = tau_d_hat_mob_[i];
+    msg_ctrl_state_.tau_dist_sosml[i] = tau_d_hat_sosml_[i];
+  }
+
+  msg_ctrl_state_.F_dist_franka.force.x = Fd_hat_fr_[0];
+  msg_ctrl_state_.F_dist_franka.force.y = Fd_hat_fr_[1];
+  msg_ctrl_state_.F_dist_franka.force.z = Fd_hat_fr_[2];
+  msg_ctrl_state_.F_dist_franka.torque.x = Fd_hat_fr_[3];
+  msg_ctrl_state_.F_dist_franka.torque.y = Fd_hat_fr_[4];
+  msg_ctrl_state_.F_dist_franka.torque.z = Fd_hat_fr_[5];
+
+  msg_ctrl_state_.F_dist_dob.force.x = Fd_hat_dob_[0];
+  msg_ctrl_state_.F_dist_dob.force.y = Fd_hat_dob_[1];
+  msg_ctrl_state_.F_dist_dob.force.z = Fd_hat_dob_[2];
+  msg_ctrl_state_.F_dist_dob.torque.x = Fd_hat_dob_[3];
+  msg_ctrl_state_.F_dist_dob.torque.y = Fd_hat_dob_[4];
+  msg_ctrl_state_.F_dist_dob.torque.z = Fd_hat_dob_[5];
+
+  msg_ctrl_state_.F_dist_mob.force.x = Fd_hat_mob_[0];
+  msg_ctrl_state_.F_dist_mob.force.y = Fd_hat_mob_[1];
+  msg_ctrl_state_.F_dist_mob.force.z = Fd_hat_mob_[2];
+  msg_ctrl_state_.F_dist_mob.torque.x = Fd_hat_mob_[3];
+  msg_ctrl_state_.F_dist_mob.torque.y = Fd_hat_mob_[4];
+  msg_ctrl_state_.F_dist_mob.torque.z = Fd_hat_mob_[5];
+
+  /* task-space pose */
+  msg_ctrl_state_.ee_pose_world_des.position.x = ee_pos_world_des_[0];
+  msg_ctrl_state_.ee_pose_world_des.position.y = ee_pos_world_des_[1];
+  msg_ctrl_state_.ee_pose_world_des.position.z = ee_pos_world_des_[2];
+  msg_ctrl_state_.ee_pose_world_des.orientation.x = ee_quat_world_des_.x();
+  msg_ctrl_state_.ee_pose_world_des.orientation.y = ee_quat_world_des_.y();
+  msg_ctrl_state_.ee_pose_world_des.orientation.z = ee_quat_world_des_.z();
+  msg_ctrl_state_.ee_pose_world_des.orientation.w = ee_quat_world_des_.w();
+
+  msg_ctrl_state_.ee_pose_world_mes.position.x = ee_pos_world_mes_[0];
+  msg_ctrl_state_.ee_pose_world_mes.position.y = ee_pos_world_mes_[1];
+  msg_ctrl_state_.ee_pose_world_mes.position.z = ee_pos_world_mes_[2];
+  msg_ctrl_state_.ee_pose_world_mes.orientation.x = ee_quat_world_mes_.x();
+  msg_ctrl_state_.ee_pose_world_mes.orientation.y = ee_quat_world_mes_.y();
+  msg_ctrl_state_.ee_pose_world_mes.orientation.z = ee_quat_world_mes_.z();
+  msg_ctrl_state_.ee_pose_world_mes.orientation.w = ee_quat_world_mes_.w();
+
+  /* task-space twist */
+  msg_ctrl_state_.ee_vel_world_des.linear.x = ee_vel_world_des_[0];
+  msg_ctrl_state_.ee_vel_world_des.linear.y = ee_vel_world_des_[1];
+  msg_ctrl_state_.ee_vel_world_des.linear.z = ee_vel_world_des_[2];
+  msg_ctrl_state_.ee_vel_world_des.angular.x = ee_vel_world_des_[3];
+  msg_ctrl_state_.ee_vel_world_des.angular.y = ee_vel_world_des_[4];
+  msg_ctrl_state_.ee_vel_world_des.angular.z = ee_vel_world_des_[5];
+
+  msg_ctrl_state_.ee_vel_world_mes.linear.x = ee_vel_world_mes_[0];
+  msg_ctrl_state_.ee_vel_world_mes.linear.y = ee_vel_world_mes_[1];
+  msg_ctrl_state_.ee_vel_world_mes.linear.z = ee_vel_world_mes_[2];
+  msg_ctrl_state_.ee_vel_world_mes.angular.x = ee_vel_world_mes_[3];
+  msg_ctrl_state_.ee_vel_world_mes.angular.y = ee_vel_world_mes_[4];
+  msg_ctrl_state_.ee_vel_world_mes.angular.z = ee_vel_world_mes_[5];
+
+  /* task-space acceleration */
+  msg_ctrl_state_.ee_acc_world_eul.linear.x = ee_acc_world_eul_[0];
+  msg_ctrl_state_.ee_acc_world_eul.linear.y = ee_acc_world_eul_[1];
+  msg_ctrl_state_.ee_acc_world_eul.linear.z = ee_acc_world_eul_[2];
+  msg_ctrl_state_.ee_acc_world_eul.angular.x = ee_acc_world_eul_[3];
+  msg_ctrl_state_.ee_acc_world_eul.angular.y = ee_acc_world_eul_[4];
+  msg_ctrl_state_.ee_acc_world_eul.angular.z = ee_acc_world_eul_[5];
+
+  msg_ctrl_state_.ee_acc_world_mob.linear.x = ee_acc_world_hat_mob_[0];
+  msg_ctrl_state_.ee_acc_world_mob.linear.y = ee_acc_world_hat_mob_[1];
+  msg_ctrl_state_.ee_acc_world_mob.linear.z = ee_acc_world_hat_mob_[2];
+  msg_ctrl_state_.ee_acc_world_mob.angular.x = ee_acc_world_hat_mob_[3];
+  msg_ctrl_state_.ee_acc_world_mob.angular.y = ee_acc_world_hat_mob_[4];
+  msg_ctrl_state_.ee_acc_world_mob.angular.z = ee_acc_world_hat_mob_[5];
+
+  msg_ctrl_state_.ee_acc_world_sosml.linear.x = ee_acc_world_hat_sosml_[0];
+  msg_ctrl_state_.ee_acc_world_sosml.linear.y = ee_acc_world_hat_sosml_[1];
+  msg_ctrl_state_.ee_acc_world_sosml.linear.z = ee_acc_world_hat_sosml_[2];
+  msg_ctrl_state_.ee_acc_world_sosml.angular.x = ee_acc_world_hat_sosml_[3];
+  msg_ctrl_state_.ee_acc_world_sosml.angular.y = ee_acc_world_hat_sosml_[4];
+  msg_ctrl_state_.ee_acc_world_sosml.angular.z = ee_acc_world_hat_sosml_[5];
+
+  pub_ctrl_state_.publish(msg_ctrl_state_);
+}
+
+void CartesianImpedanceExampleController::update_previous_states()
+{
+  dq_mes_prev_ = dq_mes_;
+  ddq_eul_prev_ = ddq_eul_;
+  ee_rpy_world_des_prev_ = ee_rpy_world_des_;
+  ee_vel_world_des_prev_ = ee_vel_world_des_;
+  ee_acc_world_des_prev_ = ee_acc_world_des_;
+  ee_rpy_world_mes_prev_ = ee_rpy_world_mes_;
+  ee_vel_world_mes_prev_ = ee_vel_world_mes_;
+  ee_acc_world_eul_prev_ = ee_acc_world_eul_;
+
+  h_mes_prev_ = h_mes_;
+}
+
+Eigen::Matrix<double, 7, 1> CartesianImpedanceExampleController::saturateTorqueRate(
+  const Eigen::Matrix<double, 7, 1> & tau_d_calculated, const Eigen::Matrix<double, 7, 1> & tau_J_d)
+{  // NOLINT (readability-identifier-naming)
+  Eigen::Matrix<double, 7, 1> tau_d_saturated{};
+  for (size_t i = 0; i < 7; i++)
+  {
+    double difference = tau_d_calculated[i] - tau_J_d[i];
+    tau_d_saturated[i] =
+      tau_J_d[i] + std::max(std::min(difference, delta_tau_max_), -delta_tau_max_);
+  }
+  return tau_d_saturated;
+}
+
+//* ----- PRINTER ----------------------------------------------------------------------------------
+void CartesianImpedanceExampleController::print_end_effector_states()
+{
+  // cout << "Target EE twist:\t" << ee_vel_world_target_.transpose() << endl;
+  // cout << "Desired EE twist:\t" << ee_vel_world_des_.transpose() << endl;
+  // std::cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << std::endl;
+  // std::cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << std::endl;
+  cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << endl;
+  cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << endl;
+  cout << "Measured EE RPY [rad]:\t" << ee_rpy_world_mes_.transpose() << endl << endl;
+  // NOTE: results from FR3 state & pinocchio are almost the same.
+  // cout << "Measured Pin EE pos [m]:\t" << ee_pos_world_mes_.transpose() << endl;
+  // cout << "Measured FR3 EE pos [m]:\t" << position.transpose() << endl;
+  // cout << "Measured Pin EE quat:\t" << ee_quat_world_mes_ << endl;
+  // cout << "Measured FR3 EE quat:\t" << orientation << endl;
+}
+
+void CartesianImpedanceExampleController::print_dynamics()
+{
+  // NOTE: dynamics are a little bit different
+  // cout << "FR3:\n" << M_fr << endl;
+  // cout << "Pin:\n" << M << endl;
+  // cout << "Coriolis FR3:\t" << coriolis.transpose() << endl;
+  // cout << "Coriolis Pin:\t" << tau_c.transpose() << endl;
+}
+
+}  // namespace franka_example_controllers
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController,
+                       controller_interface::ControllerBase)