From 1ff225dbeab4fc6e6cb38d19fe9f24ac9f6e17fb Mon Sep 17 00:00:00 2001
From: jeo65286 <jeo65286@ipk-extern.fraunhofer.de>
Date: Mon, 13 Jan 2025 15:10:38 +0100
Subject: [PATCH 1/9] First commit for real robot impedance controller in
 nix-env

---
 .gitignore                                    |   1 +
 .../CMakeLists.txt                            |   0
 .../README.md                                 |   0
 .../package.xml                               |   0
 .../dob_impedance_controller.hpp              | 227 -------
 src/dob_impedance_controller.cpp              | 614 ------------------
 src/dob_impedance_controller_node.cpp         |  20 -
 7 files changed, 1 insertion(+), 861 deletions(-)
 rename CMakeLists.txt => fr3_impedance_controller_real/CMakeLists.txt (100%)
 rename README.md => fr3_impedance_controller_real/README.md (100%)
 rename package.xml => fr3_impedance_controller_real/package.xml (100%)
 delete mode 100644 include/dob_impedance_controller/dob_impedance_controller.hpp
 delete mode 100644 src/dob_impedance_controller.cpp
 delete mode 100644 src/dob_impedance_controller_node.cpp

diff --git a/.gitignore b/.gitignore
index df058f6..c106cd8 100644
--- a/.gitignore
+++ b/.gitignore
@@ -1,5 +1,6 @@
 .vscode/
 build/
 assets/data/*.asv
+fr3_impedance_controller_real/build/
 # assets/data/*.csv
 # assets/data/*.bag
diff --git a/CMakeLists.txt b/fr3_impedance_controller_real/CMakeLists.txt
similarity index 100%
rename from CMakeLists.txt
rename to fr3_impedance_controller_real/CMakeLists.txt
diff --git a/README.md b/fr3_impedance_controller_real/README.md
similarity index 100%
rename from README.md
rename to fr3_impedance_controller_real/README.md
diff --git a/package.xml b/fr3_impedance_controller_real/package.xml
similarity index 100%
rename from package.xml
rename to fr3_impedance_controller_real/package.xml
diff --git a/include/dob_impedance_controller/dob_impedance_controller.hpp b/include/dob_impedance_controller/dob_impedance_controller.hpp
deleted file mode 100644
index 6ed0c68..0000000
--- a/include/dob_impedance_controller/dob_impedance_controller.hpp
+++ /dev/null
@@ -1,227 +0,0 @@
-/** @file dob_impedance_controller.hpp
- *  @brief
- */
-
-#ifndef DOB_IMPEDANCE_CONTROLLER_HPP_
-#define DOB_IMPEDANCE_CONTROLLER_HPP_
-
-/* ROS1 Packages */
-#include "ros/ros.h"
-#include "ros/time.h"
-
-#include "franka_msgs/FrankaState.h"
-#include "geometry_msgs/Accel.h"     // end-effector acceleration
-#include "geometry_msgs/Pose.h"      // end-effector position/quaternion
-#include "geometry_msgs/Twist.h"     // end-effector twist
-#include "geometry_msgs/Wrench.h"    // end-effector force/torque
-#include "sensor_msgs/JointState.h"  // joint pos/vel/torque
-
-/* C++ STL */
-#include <memory>
-#include <string>
-
-/* Custom Headers */
-#include "MathUtilities.hpp"
-#include "cppTypes.hpp"
-#include "save_data.hpp"
-#include "symbolic_autodiff.hpp"
-
-#include "DisturbanceObserver.hpp"
-#include "SlidingModeObserver.hpp"
-
-class DobImpedanceController
-{
-private:
-  enum OperationMode
-  {
-    SIMULATION_MODE = 0,
-    REAL_ROBOT_MODE,
-    DIGIT_TWIN_MODE,
-  };
-  int op_mode_ = 0;
-
-  const int queue_size_ = 20;  // queue size for ROS communication
-  const double Hz_;            // loop frequency
-  double period_ = 0.0;        // loop period
-  int tick_ = 0;               // loop iteration
-
-  ros::NodeHandle nh_;
-  ros::Rate loop_Hz_;
-  ros::Time start_time_;
-  bool b_count_loop_time_ = false;
-
-  /* Joystick EE twist command subscriber */
-  ros::Subscriber joy_sub_;
-  bool b_is_teleop_mode_ = false;
-
-  /* Franka state subscriber */
-  ros::Subscriber fr_state_sub_;
-
-  /* Simulation data subscriber */
-  ros::Subscriber mj_joint_state_sub_;
-  ros::Subscriber mj_ee_pose_sub_;
-  ros::Subscriber mj_ee_twist_sub_;
-  ros::Subscriber mj_ee_accel_sub_;
-  ros::Subscriber mj_ee_wrench_sub_;
-  bool b_is_mj_data_received_ = false;
-
-  /* Control command publisher */
-  ros::Publisher joint_cmd_pub_;
-  sensor_msgs::JointState joint_cmd_msg;
-
-  ros::Publisher ee_perturb_pub_;
-  geometry_msgs::Wrench ee_perturb_msg;
-
-  //* Franka states *//
-  Vec7<double> fr_q_mes_;   // measured joint position
-  Vec7<double> fr_dq_mes_;  // measured joint velocity
-
-  Vec7<double> fr_tau_des_;  // desired link-side joint torque sensor signals (w/o gravity)
-  Vec7<double> fr_tau_mes_;  // measured link-side joint torque sensor signals
-
-  Eigen::Affine3d fr_T_des_;     // desired EE transformation matrix
-  Vec3<double> fr_ee_pos_des_;   // desired EE translational position in {W}
-  Quat<double> fr_ee_quat_des_;  // desired EE rotational quaternion in {W}
-
-  Eigen::Affine3d fr_T_mes_;     // measured EE transformation matrix
-  Vec3<double> fr_ee_pos_mes_;   // measured EE translational position in {W}
-  Quat<double> fr_ee_quat_mes_;  // measured EE rotational quaternion in {W}
-
-  Vec6<double> fr_ee_twist_des_;  // desired EE twist in {W}
-
-  Vec7<double> fr_tau_ext_hat_;  // LPF torques by estimated external forces on the joints
-  Vec6<double> fr_F_ext_hat_;    // estimated external wrench acting on {K} w.r.t {W}
-  // Vec6<double> F_ext_hat_filtered_;  // LPF torques by estimated external forces on the EE
-
-  //* MuJoCo sensor data *//
-  Vec7<double> mj_q_;
-  Vec7<double> mj_dq_;
-  Vec7<double> mj_ddq_;
-
-  Vec3<double> mj_ee_pos_world_;
-  Quat<double> mj_ee_quat_world_;
-  Vec6<double> mj_ee_twist_world_;
-  Vec6<double> mj_ee_accel_world_;
-  Vec6<double> mj_ee_wrench_local_;  // external wrench applied to hand's CoM
-  Vec6<double> mj_ee_wrench_world_;
-
-  //* Controller states *//
-  /* joint states */
-  Vec7<double> q_mes_;
-  Vec7<double> dq_mes_;
-  Vec7<double> dq_mes_prev_;  // previous measured joint velocity
-  Vec7<double> ddq_eul_;      // time-derivative of joint velocity using backward Euler method
-  Vec7<double> ddq_eul_prev_;
-
-  /* task states */
-  int ee_frame_id_;
-  Vec3<double> ee_pos_world_des_;
-  Vec3<double> ee_pos_world_mes_;
-  Vec3<double> ee_rpy_world_des_, ee_rpy_world_des_prev_;
-  Vec3<double> ee_rpy_world_mes_, ee_rpy_world_mes_prev_;
-  Quat<double> ee_quat_world_des_;
-  Quat<double> ee_quat_world_mes_;
-  RotMat<double> ee_R_world_mes_;
-
-  // const Vec3<double> p_fe_{ 0.0, 0.0, 0.1034 };  // position from flange to EE
-  // const Quat<double> q_fe_{ 0.0, 0.0, -0.3826834, 0.9238795 };
-
-  Vec6<double> ee_twist_world_des_;  // desired EE twist w.r.t "body" frame from joystick command
-  Vec6<double> ee_twist_world_cal_;  // calculated current EE twist in {W} using v = J * dq
-  Vec6<double> ee_twist_world_cal_prev_;  // calculated current EE twist in {W} using v = J * dq
-  Vec6<double> ee_accel_world_des_;
-  Vec6<double> ee_accel_world_eul_;  // derivative of ee_twist_world_cal_ using Euler method
-  Vec6<double> ee_accel_world_eul_prev_;
-
-  /* Impedance control */
-  bool b_impedance_control_on_ = true;
-  const double k_amp_ = 1.0;         // PD gain ratio to real HW gain
-  Mat6<double> K_task_;              // task-space stiffness
-  Mat6<double> B_task_;              // task-space damping
-  Vec6<double> ee_pose_error_;       // task-space EE pose error
-  Vec6<double> ee_pose_error_prev_;  // previos EE pose error
-  Vec6<double> F_task_;              // computed wrench from impedance controller
-  Vec7<double> tau_task_;            // computed torque from impedance controller
-
-  bool b_null_control_on_ = true;
-  Mat7<double> K_null_;      // null-space stiffness
-  Mat7<double> B_null_;      // null-space damping
-  Vec7<double> q_null_des_;  // desired null-space configuration
-  Vec7<double> tau_null_;    // nullspace control torque
-
-  Vec6<double> F_dist_;    // wrench by external disturbance
-  Vec7<double> tau_dist_;  // torque by external disturbance
-
-  Vec7<double> tau_des_;  // desired torque
-  Vec7<double> tau_cmd_;  // commanded torque
-
-  /* Observers */
-  bool b_is_tfdob_applied_ = false;
-  std::unique_ptr<DisturbanceObserver<double>> tfdob_;
-  Vec6<double> F_dist_hat_tfdob_;  // estimated force disturbance
-  Vec6<double> fq_;                // TFDOB's Q-filter cutoff requency [Hz]
-
-  bool b_is_sosml_applied_ = false;
-  std::unique_ptr<SlidingModeObserver<double>> sosml_;
-  Vec7<double> tau_dist_hat_sosml_;  // estimated force disturbance
-  Vec6<double> F_dist_hat_sosml_;    // estimated force disturbance
-  Vec7<double> ddq_hat_;             // estimated joint acceleration
-  Vec6<double> ee_accel_world_hat_;  // estimated task-space EE acceleration
-  Vec7<double> s1_, s2_;             // SOSML's parameter
-  Vec7<double> t1_, t2_;
-
-  //* Kinematics & Dynamics *//
-  bool b_coriolis_comp_on_ = true;
-  bool b_gravity_comp_on_ = true;
-  Mat67<double> Je_;      // EE Jacobian w.r.t base {W}
-  Mat67<double> dJe_;     // derivative of EE Jacobian w.r.t base {W}
-  Mat7<double> Mq_;       // (upper-triangular) joint ineria matrix
-  Mat7<double> Mq_full_;  // (full) joint inertia matrix
-  Mat7<double> dMq_inv_;  // time-derivative of inverse inertia matrix
-  Mat6<double> Me_;       // (full) operational ineria matrix
-  Mat7<double> Cq_;       // Coriolis & Centrifugal matrix
-  Vec7<double> tau_c_;    // Coriolis & Centrifugal torque vector
-  Vec7<double> tau_g_;    // Gravity torque vector
-
-  /* Pinocchio & CasADi */
-  std::unique_ptr<SymbolicAutoDiff> autodiff_;
-  const std::string repo_path = "/home/jwhong/nix-project/dob_impedance_controller/";
-  std::string urdf_filename = std::string(repo_path + "assets/model/fr3_hand.urdf");
-
-  pinocchio::Model pin_model_;
-  pinocchio::Data pin_data_;
-
-  /* Data Logging */
-  bool b_save_data_ = true;
-  std::string log_filename = std::string(repo_path + "assets/data/data.csv");
-  std::unique_ptr<SaveData<double>> logger_;
-
-public:
-  DobImpedanceController(ros::NodeHandle & nh, const double & Hz);
-
-  //* ----- SUBSCRIBE CALLBACK ----------------------------------------------------------------- *//
-  void subscribe_end_effector_twist_cmd(const geometry_msgs::Twist & msg);
-  void subscribe_franka_states(const franka_msgs::FrankaState & msg);
-  void subscribe_mj_joint_state(const sensor_msgs::JointState & msg);
-  void subscribe_mj_end_effector_pose(const geometry_msgs::Pose & msg);
-  void subscribe_mj_end_effector_twist(const geometry_msgs::Twist & msg);
-  void subscribe_mj_end_effector_accel(const geometry_msgs::Accel & msg);
-  void subscribe_mj_end_effector_wrench(const geometry_msgs::Wrench & msg);
-
-  //* ----- METHODS ---------------------------------------------------------------------------- *//
-  void init();
-  void run_loop();
-  void publish_topics();
-  void update_previous_values();
-
-  //* ----- PRINTER ---------------------------------------------------------------------------- *//
-  void print_jacobian();
-  void print_ee_position();
-  void print_ee_quaternion();
-  void print_ee_twist();
-  void print_ee_accel();  // TODO: check whether acceleration is correct
-  void print_impedance_control();
-  void print_torque_command();
-};
-
-#endif  // DOB_IMPEDANCE_CONTROLLER_HPP_
\ No newline at end of file
diff --git a/src/dob_impedance_controller.cpp b/src/dob_impedance_controller.cpp
deleted file mode 100644
index 184662b..0000000
--- a/src/dob_impedance_controller.cpp
+++ /dev/null
@@ -1,614 +0,0 @@
-#include "dob_impedance_controller.hpp"
-
-#include <chrono>
-#include <cmath>
-#include <iomanip>  // std::setprecision()
-#include <iostream>
-#include <vector>
-
-#include <Eigen/QR>
-
-#include "pinocchio/algorithm/frames.hpp"
-#include "pinocchio/multibody/fwd.hpp"
-#include "pinocchio/spatial/skew.hpp"
-
-#include "OrientationUtilities.hpp"
-#include "pseudo_inversion.h"
-#include "robotics_func.hpp"  // ! CAUTION for header location
-
-using namespace robotics_functions;
-using namespace pinocchio;
-using namespace std;
-
-DobImpedanceController::DobImpedanceController(ros::NodeHandle & nh, const double & Hz)
-  : nh_(nh), loop_Hz_(Hz), Hz_(Hz)
-{
-  period_ = 1.0 / Hz_;
-
-  /* Initialize subscriber */
-  mj_joint_state_sub_ = nh_.subscribe("mj_joint_state", queue_size_,
-                                      &DobImpedanceController::subscribe_mj_joint_state, this);
-  mj_ee_pose_sub_ = nh_.subscribe("mj_ee_pose", queue_size_,
-                                  &DobImpedanceController::subscribe_mj_end_effector_pose, this);
-  mj_ee_twist_sub_ = nh_.subscribe("mj_ee_twist", queue_size_,
-                                   &DobImpedanceController::subscribe_mj_end_effector_twist, this);
-  mj_ee_accel_sub_ = nh_.subscribe("mj_ee_accel", queue_size_,
-                                   &DobImpedanceController::subscribe_mj_end_effector_accel, this);
-  mj_ee_wrench_sub_ = nh_.subscribe(
-    "mj_ee_wrench", queue_size_, &DobImpedanceController::subscribe_mj_end_effector_wrench, this);
-
-  fr_state_sub_ = nh_.subscribe("/franka_state_controller/franka_states", queue_size_,
-                                &DobImpedanceController::subscribe_franka_states, this);
-
-  joy_sub_ = nh_.subscribe("ee_twist_cmd", queue_size_,
-                           &DobImpedanceController::subscribe_end_effector_twist_cmd, this);
-  /* Initialize publisher */
-  joint_cmd_pub_ = nh_.advertise<sensor_msgs::JointState>("mj_joint_cmd", queue_size_);
-  ee_perturb_pub_ = nh_.advertise<geometry_msgs::Wrench>("mj_ee_perturb", queue_size_);
-
-  /* Initialize states */
-  this->init();
-}
-
-//* ----- SUBSCRIBE CALLBACK ------------------------------------------------------------------- *//
-void DobImpedanceController::subscribe_end_effector_twist_cmd(const geometry_msgs::Twist & msg)
-{
-  ee_twist_world_des_[0] = msg.linear.x;
-  ee_twist_world_des_[1] = msg.linear.y;
-  ee_twist_world_des_[2] = msg.linear.z;
-  ee_twist_world_des_[3] = msg.angular.x;
-  ee_twist_world_des_[4] = msg.angular.y;
-  ee_twist_world_des_[5] = msg.angular.z;
-}
-
-void DobImpedanceController::subscribe_franka_states(const franka_msgs::FrankaState & msg)
-{
-  /* joint states */
-  fr_q_mes_ = Eigen::Map<const Vec7<double>>(msg.q.data());
-  fr_dq_mes_ = Eigen::Map<const Vec7<double>>(msg.dq.data());
-
-  fr_tau_des_ = Eigen::Map<const Vec7<double>>(msg.tau_J_d.data());
-  fr_tau_mes_ = Eigen::Map<const Vec7<double>>(msg.tau_J.data());
-
-  /* Cartesian states */
-  fr_T_des_ = Eigen::Affine3d(Eigen::Matrix4d::Map(msg.O_T_EE_d.data()));
-  fr_ee_pos_des_ = fr_T_des_.translation();
-  fr_ee_quat_des_ = fr_T_des_.rotation();
-
-  fr_T_mes_ = Eigen::Affine3d(Eigen::Matrix4d::Map(msg.O_T_EE.data()));
-  fr_ee_pos_mes_ = fr_T_mes_.translation();
-  fr_ee_quat_mes_ = fr_T_mes_.rotation();
-
-  fr_ee_twist_des_ = Eigen::Map<const Vec6<double>>(msg.O_dP_EE_d.data());
-
-  /* External torque/wrench estimation */
-  fr_tau_ext_hat_ = Eigen::Map<const Vec7<double>>(msg.tau_ext_hat_filtered.data());
-  fr_F_ext_hat_ = Eigen::Map<const Vec6<double>>(msg.O_F_ext_hat_K.data());
-}
-
-void DobImpedanceController::subscribe_mj_joint_state(const sensor_msgs::JointState & msg)
-{
-  for (size_t i = 0; i < 7; ++i)
-  {
-    mj_q_[i] = msg.position[i];
-    mj_dq_[i] = msg.velocity[i];
-    mj_ddq_[i] = msg.effort[i];  // ! this is joint accel, not a torque measurement
-  }
-  b_is_mj_data_received_ = true;
-}
-
-void DobImpedanceController::subscribe_mj_end_effector_pose(const geometry_msgs::Pose & msg)
-{
-  mj_ee_pos_world_[0] = msg.position.x;
-  mj_ee_pos_world_[1] = msg.position.y;
-  mj_ee_pos_world_[2] = msg.position.z;
-  mj_ee_quat_world_.coeffs() << msg.orientation.x, msg.orientation.y, msg.orientation.z,
-    msg.orientation.w;
-}
-
-void DobImpedanceController::subscribe_mj_end_effector_twist(const geometry_msgs::Twist & msg)
-{
-  mj_ee_twist_world_[0] = msg.linear.x;
-  mj_ee_twist_world_[1] = msg.linear.y;
-  mj_ee_twist_world_[2] = msg.linear.z;
-  mj_ee_twist_world_[3] = msg.angular.x;
-  mj_ee_twist_world_[4] = msg.angular.y;
-  mj_ee_twist_world_[5] = msg.angular.z;
-}
-
-void DobImpedanceController::subscribe_mj_end_effector_accel(const geometry_msgs::Accel & msg)
-{
-  mj_ee_accel_world_[0] = msg.linear.x;
-  mj_ee_accel_world_[1] = msg.linear.y;
-  mj_ee_accel_world_[2] = msg.linear.z - 9.81;  // ! gravity accel
-  mj_ee_accel_world_[3] = msg.angular.x;
-  mj_ee_accel_world_[4] = msg.angular.y;
-  mj_ee_accel_world_[5] = msg.angular.z;
-}
-
-void DobImpedanceController::subscribe_mj_end_effector_wrench(const geometry_msgs::Wrench & msg)
-{
-  mj_ee_wrench_local_[0] = msg.force.x;
-  mj_ee_wrench_local_[1] = msg.force.y;
-  mj_ee_wrench_local_[2] = msg.force.z;
-  mj_ee_wrench_local_[3] = msg.torque.x;
-  mj_ee_wrench_local_[4] = msg.torque.y;
-  mj_ee_wrench_local_[5] = msg.torque.z;
-}
-
-//* ----- METHODS ------------------------------------------------------------------------------ *//
-void DobImpedanceController::init()
-{
-  /* Franka states */
-  fr_q_mes_.setZero();
-  fr_dq_mes_.setZero();
-  fr_tau_des_.setZero();
-  fr_tau_mes_.setZero();
-  fr_ee_pos_des_.setZero();
-  fr_ee_pos_mes_.setZero();
-  fr_ee_quat_des_.coeffs() << 0.0, 0.0, 0.0, 1.0;
-  fr_ee_quat_mes_.coeffs() << 0.0, 0.0, 0.0, 1.0;
-  fr_ee_twist_des_.setZero();
-  fr_tau_ext_hat_.setZero();
-  fr_F_ext_hat_.setZero();
-
-  /* MuJoCo states */
-  mj_q_ << 0.0130147, -0.633825, -0.131726, -2.50088, -0.105203, 1.89616, -0.893329;  // ! adjust
-  mj_dq_.setZero();
-  mj_ddq_.setZero();
-  mj_ee_pos_world_.setZero();
-  mj_ee_quat_world_.coeffs() << 0.0, 0.0, 0.0, 1.0;
-  mj_ee_twist_world_.setZero();
-  mj_ee_accel_world_.setZero();
-  mj_ee_wrench_local_.setZero();
-  mj_ee_wrench_world_.setZero();
-
-  /* Controller states */
-  q_mes_.setZero();
-  dq_mes_.setZero();
-  dq_mes_prev_.setZero();
-  ddq_eul_.setZero();
-  ddq_eul_prev_.setZero();
-
-  // ee_pos_world_des_ << 0.359081, -0.0541908, 0.496675; // flange
-  // ee_quat_world_des_.coeffs() << 0.914706, 0.403755, 0.00625574, 0.0159898;
-  ee_pos_world_des_ << 0.361599, -0.0566931, 0.393336;  // tcp
-  ee_pos_world_mes_.setZero();
-  ee_rpy_world_des_.setZero();
-  ee_rpy_world_des_prev_.setZero();
-  ee_rpy_world_mes_.setZero();
-  ee_rpy_world_mes_prev_.setZero();
-  ee_quat_world_des_.coeffs() << 0.690568, 0.723064, -0.000339485, 0.0171666;
-  ee_quat_world_mes_.coeffs() << 0.0, -0.7071, 0.7071, 0.0;
-  ee_R_world_mes_ = orient_tools::quaternionToRotationMatrix(ee_quat_world_mes_);
-  ee_twist_world_des_.setZero();
-  ee_twist_world_cal_.setZero();
-  ee_twist_world_cal_prev_.setZero();
-  ee_accel_world_des_.setZero();
-  ee_accel_world_eul_.setZero();
-  ee_accel_world_eul_prev_.setZero();
-
-  /* Impedance controller */
-  K_task_.setIdentity();
-  K_task_.diagonal() << 200, 200, 200, 10, 10, 10;  // real HW value
-  K_task_.diagonal() *= k_amp_;
-  B_task_.setIdentity();
-  for (size_t i = 0; i < 6; ++i)
-  {
-    double k = K_task_.diagonal()(i);
-    B_task_(i, i) = 2.0 * sqrt(k);
-  }
-  ee_pose_error_.setZero();
-  ee_pose_error_prev_.setZero();
-  F_task_.setZero();
-  tau_task_.setZero();
-
-  K_null_.setIdentity();
-  K_null_.diagonal() << 0.5, 0.5, 0.5, 0.5, 0.5, 0.5, 0.5;  //  real HW value
-  K_null_.diagonal() *= k_amp_;
-  B_null_.setIdentity();
-  for (size_t i = 0; i < 7; ++i)
-  {
-    double k = K_null_.diagonal()(i);
-    B_null_(i, i) = 2.0 * sqrt(k);
-  }
-  // q_null_des_ << 0.0, 0.0, 0.0, -1.57079, 0.0, 1.57079, -0.7853;  // ! adjust
-  q_null_des_ << 0.0130147, -0.633825, -0.131726, -2.50088, -0.105203, 1.89616, -0.893329;
-  tau_null_.setZero();
-
-  F_dist_.setZero();
-  tau_dist_.setZero();
-  tau_des_.setZero();
-  tau_cmd_.setZero();
-
-  /* Observers */
-  tfdob_ = std::make_unique<DisturbanceObserver<double>>();
-  F_dist_hat_tfdob_.setZero();
-  fq_ << 15, 15, 15, 15, 15, 15;  // ! tune
-
-  sosml_ = std::make_unique<SlidingModeObserver<double>>();
-  ddq_hat_.setZero();
-  tau_dist_hat_sosml_.setZero();
-  F_dist_hat_sosml_.setZero();
-  s1_ << 10.0, 10.0, 10.0, 10.0, 2.0, 2.0, 2.0;
-  s2_ << 100.0, 100.0, 100.0, 100.0, 10.0, 10.0, 10.0;
-  for (size_t i = 0; i < 7; ++i)
-  {
-    t1_[i] = 2.0 * sqrt(s1_[i]);
-    t2_[i] = 2.0 * sqrt(s2_[i]);
-  }
-  sosml_->set_control_param(s1_, s2_, t1_, t2_);
-
-  /* Kinematics & Dynamics */
-  Je_.setZero();
-  dJe_.setZero();
-  Mq_.setZero();
-  Mq_full_.setZero();
-  dMq_inv_.setZero();
-  Me_.setZero();
-  Cq_.setZero();
-  tau_c_.setZero();
-  tau_g_.setZero();
-
-  /* Load FR3 URDF model and data */
-  if (urdf_filename.empty())
-  {
-    throw std::runtime_error("URDF filename is invalid");
-  }
-  pinocchio::urdf::buildModel(urdf_filename, pin_model_);
-  pin_data_ = Data(pin_model_);
-  // ee_frame_id_ = pin_model_.getFrameId("fr3_link8", (pinocchio::FrameType::BODY));  // flange
-  ee_frame_id_ = pin_model_.getFrameId("fr3_hand_tcp", (pinocchio::FrameType::BODY));  // tcp
-
-  /* Auto-Diff Symbolic representation for computing Minv_dot */
-  // ! this should be initialized after Pinocchio's initialization
-  autodiff_ = std::make_unique<SymbolicAutoDiff>(pin_model_, pin_data_);
-  autodiff_->create_func_dMinv();
-  autodiff_->create_func_dJ();
-
-  /* data logger */
-  logger_ = std::make_unique<SaveData<double>>(log_filename);
-}
-
-void DobImpedanceController::run_loop()
-{
-  while (ros::ok())
-  {
-    /* count loop's start time */
-    auto start_exec_time = std::chrono::high_resolution_clock::now();
-    double elapsed_time;
-    if (b_count_loop_time_ == true)
-    {
-      if (start_time_.isZero())
-      {
-        start_time_ = ros::Time::now();
-      }
-      ros::Time current_time = ros::Time::now();
-      elapsed_time = (current_time - start_time_).toSec();
-    }
-
-    double loop_time = tick_ * period_;
-
-    /* execute subscribe callback functions */
-    ros::spinOnce();
-
-    /* select operation mode */
-    switch (op_mode_)
-    {
-    case OperationMode::SIMULATION_MODE:
-      q_mes_ = mj_q_;
-      dq_mes_ = mj_dq_;
-      break;
-
-    case OperationMode::REAL_ROBOT_MODE:
-      q_mes_ = fr_q_mes_;
-      dq_mes_ = fr_dq_mes_;
-
-    default:
-      q_mes_ = fr_q_mes_;
-      dq_mes_ = fr_dq_mes_;
-      break;
-    }
-
-    //* ----- get Current States  -------------------------------------------------------------- *//
-    // ddq_eul_ = getEulerDerivative(dq_mes_, dq_mes_prev_, period_);
-    ddq_eul_ = getFilteredTustinDerivative(dq_mes_, dq_mes_prev_, ddq_eul_prev_, period_, 5);
-
-    ee_pos_world_mes_ = pin_data_.oMf[ee_frame_id_].translation();
-    ee_R_world_mes_ = pin_data_.oMf[ee_frame_id_].rotation();
-    ee_quat_world_mes_ = Eigen::Quaterniond(ee_R_world_mes_);
-    ee_rpy_world_mes_ = orient_tools::quaternionToRPY(ee_quat_world_mes_, ee_rpy_world_mes_prev_);
-
-    //* ----- set Desired EE Pose -------------------------------------------------------------- *//
-    if (b_is_teleop_mode_ == false)
-    {
-      double dt = 4.0;
-      double t_pause = 1.0;
-      double t[6];
-      for (size_t i = 0; i < 6; ++i)
-      {
-        t[i] = (2 * i + 1) * dt + i * t_pause;
-      }
-
-      double amp[6] = { 0.1, 0.1, 0.1, 0.5, 0.5, 0.5 };
-
-      for (size_t i = 0; i < 6; ++i)
-      {
-        if (loop_time >= t[i] && loop_time < t[i] + 2 * dt)
-        {
-          ee_twist_world_des_[i] = amp[i] * std::sin(2 * M_PI * (1 / dt) * (loop_time - t[i]));
-          ee_accel_world_des_[i] =
-            2 * M_PI * (1 / dt) * amp[i] * std::cos(2 * M_PI * (1 / dt) * (loop_time - t[i]));
-        }
-        else
-        {
-          ee_twist_world_des_[i] = 0.0;
-          ee_accel_world_des_[i] = 0.0;
-        }
-      }
-    }
-
-    Vec3<double> ee_linvel_des = ee_twist_world_des_.head(3);
-    Vec3<double> ee_angvel_des = ee_twist_world_des_.tail(3);
-
-    ee_pos_world_des_ += ee_linvel_des * period_;
-    ee_quat_world_des_ = orient_tools::getNewQuat(ee_quat_world_des_, ee_angvel_des, period_);
-    ee_rpy_world_des_ = orient_tools::quaternionToRPY(ee_quat_world_des_, ee_rpy_world_des_prev_);
-    // this->print_ee_position();
-    // this->print_ee_quaternion();
-
-    //* ----- compute Kinematics & Dynamics ---------------------------------------------------- *//
-    /* Jacobian */
-    pinocchio::getFrameJacobian(pin_model_, pin_data_, ee_frame_id_,
-                                pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, Je_);
-    pinocchio::getFrameJacobianTimeVariation(pin_model_, pin_data_, ee_frame_id_,
-                                             pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED, dJe_);
-
-    // TODO: Check which pseudo-inverse is correct or more efficient
-    Eigen::MatrixXd Je_pinv;
-    Eigen::MatrixXd Je_transpose_pinv;
-    Je_pinv = Je_.completeOrthogonalDecomposition().pseudoInverse();
-    Je_transpose_pinv = Je_.transpose().completeOrthogonalDecomposition().pseudoInverse();
-    // pseudoInverse(Je_, Je_pinv);
-    // pseudoInverse(Je_.transpose(), Je_transpose_pinv);
-
-    /* compute current EE Twist & Acceleration using backward Euler method */
-    ee_twist_world_cal_ = Je_ * dq_mes_;
-    // ee_accel_world_eul_ =
-    //   getEulerDerivative(ee_twist_world_cal_, ee_twist_world_cal_prev_, period_);
-    ee_accel_world_eul_ = getFilteredTustinDerivative(ee_twist_world_cal_, ee_twist_world_cal_prev_,
-                                                      ee_accel_world_eul_prev_, period_, 5);
-    // this->print_ee_twist();
-
-    /* Inertia matrix */
-    pinocchio::crba(pin_model_, pin_data_, q_mes_);
-    Mq_ << pin_data_.M;                        // upper triangular matrix in joint-space
-    Mq_full_ = computeFullInertiaMatrix(Mq_);  // full matrix in joint-space
-    Me_ = computeTaskInertiaMatrix(Mq_full_, Je_pinv, Je_transpose_pinv);  // task-space
-
-    /* Coriolis/Centrifugal & Gravity torque */
-    pinocchio::computeCoriolisMatrix(pin_model_, pin_data_, q_mes_, dq_mes_);
-    Cq_ << pin_data_.C;
-    tau_c_ = Cq_ * dq_mes_;
-
-    pinocchio::computeGeneralizedGravity(pin_model_, pin_data_, q_mes_);
-    tau_g_ << pin_data_.g;
-
-    /* time-derivative of Inverse Inertia matrix : get numerical results from AD */
-    dMq_inv_ = autodiff_->get_dMinv(q_mes_, dq_mes_);
-
-    //* ----- CONTROL LAW ---------------------------------------------------------------------- *//
-    /* Cartesian Impedance Control */
-    ee_pose_error_.head(3) << ee_pos_world_des_ - ee_pos_world_mes_;
-
-    if (ee_quat_world_des_.coeffs().dot(ee_quat_world_mes_.coeffs()) < 0.0)
-    {
-      ee_quat_world_mes_.coeffs() << -ee_quat_world_mes_.coeffs();
-    }
-    Quat<double> ee_quat_error = ee_quat_world_mes_.inverse() * ee_quat_world_des_;
-    ee_pose_error_.tail(3) << ee_quat_error.x(), ee_quat_error.y(), ee_quat_error.z();
-    ee_pose_error_.tail(3) << ee_quat_world_mes_ * ee_pose_error_.tail(3);
-
-    if (b_is_mj_data_received_ == true)
-    {
-      // F_task_ = K_task_ * ee_pose_error_ + B_task_ * (-ee_twist_world_cal_);
-      // F_task_ = K_task_ * ee_pose_error_ + B_task_ * (ee_twist_world_des_ - ee_twist_world_cal_);
-      Vec6<double> dpose_error = getEulerDerivative(ee_pose_error_, ee_pose_error_prev_, period_);
-      F_task_ = K_task_ * ee_pose_error_ + B_task_ * dpose_error;
-
-      /* Task-Disturbance compensation */
-      if (b_is_tfdob_applied_ == true)
-      {
-        tau_task_ = Je_.transpose() * (F_task_ - F_dist_hat_tfdob_);
-      }
-      else
-      {
-        tau_task_ = Je_.transpose() * F_task_;
-      }
-    }
-    else
-    {
-      F_task_.setZero();
-    }
-
-    /* Null-space control */
-    tau_null_ = (Eigen::MatrixXd::Identity(7, 7) - Je_.transpose() * Je_transpose_pinv) *
-                (K_null_ * (q_null_des_ - q_mes_) - B_null_ * (dq_mes_));
-
-    /* compute desired joint torque command */
-    tau_des_.setZero();
-    if (b_impedance_control_on_ == true)
-    {
-      tau_des_ += tau_task_;
-    }
-    if (b_gravity_comp_on_ == true)
-    {
-      tau_des_ += tau_g_;
-    }
-    if (b_coriolis_comp_on_ == true)
-    {
-      tau_des_ += tau_c_;
-    }
-    if (b_null_control_on_ == true)
-    {
-      tau_des_ += tau_null_;
-    }
-    // tau_des_ = tau_g_ + tau_c_ + tau_task_ + tau_null_;
-    // tau_des_ = saturatreTorqueRate(tau_des_, fr_tau_des_);
-    // F_task_ = Je_transpose_pinv * (tau_des_ - tau_null_ - tau_g_ - tau_c_);
-
-    /* 2nd-order Sliding mode Observer (SOSML) */
-    sosml_->estimate_generalized_momentum(Mq_full_, Cq_, tau_g_, dq_mes_, tau_des_, period_);
-    ddq_hat_ = sosml_->get_estimated_joint_acceleration(Mq_full_, dMq_inv_);
-    ee_accel_world_hat_ = getTaskAcceleration(Je_, dJe_, dq_mes_, ddq_hat_);
-    // this->print_ee_accel();
-
-    /* Task-space Force Disturbance Observer (TFDOB) */
-    if (b_is_sosml_applied_ == true)
-    {
-      F_dist_hat_tfdob_ =
-        tfdob_->get_force_disturbance(F_task_, Me_, ee_accel_world_hat_, fq_, period_);
-    }
-    else
-    {
-      F_dist_hat_tfdob_ =
-        tfdob_->get_force_disturbance(F_task_, Me_, ee_accel_world_eul_, fq_, period_);
-    }
-    // Vec6<double> fr_F_task_ = Je_transpose_pinv * fr_tau_des_;
-    // F_dist_hat_tfdob_ = tfdob_->get_force_disturbance(fr_F_task_, Me_, ee_accel_world_eul_,
-    // fq_);
-
-    /* publish command */
-    this->publish_topics();
-
-    /* update states */
-    this->update_previous_values();
-
-    /* save data */
-    if (b_save_data_ == true)
-    {
-      if (tick_ % 5 == 0)  // 100 Hz
-      {
-        /* represent mjcf force/torque in world frame */
-        mj_ee_wrench_world_.head(3) << ee_R_world_mes_ * (-mj_ee_wrench_local_.head(3));
-        mj_ee_wrench_world_.tail(3) << ee_R_world_mes_ * (-mj_ee_wrench_local_.tail(3));
-
-        logger_->save_value(loop_time);
-        logger_->save_vector(ee_pos_world_des_);
-        logger_->save_vector(ee_pos_world_mes_);
-        logger_->save_vector(ee_rpy_world_des_);
-        logger_->save_vector(ee_rpy_world_mes_);
-        logger_->save_vector(ee_twist_world_des_);
-        logger_->save_vector(ee_twist_world_cal_);
-        logger_->save_vector(mj_ee_twist_world_);
-        logger_->save_vector(ddq_eul_);
-        logger_->save_vector(ddq_hat_);
-        logger_->save_vector(mj_ddq_);
-        logger_->save_vector(ee_accel_world_des_);
-        logger_->save_vector(ee_accel_world_eul_);
-        logger_->save_vector(ee_accel_world_hat_);
-        logger_->save_vector(mj_ee_accel_world_);
-        logger_->save_vector(F_dist_hat_tfdob_);
-        logger_->save_vector(mj_ee_wrench_world_, true);
-      }
-    }
-
-    /* check execution time and elpased time */
-    auto end_exec_time = std::chrono::high_resolution_clock::now();
-    if (b_count_loop_time_ == true)
-    {
-      std::chrono::duration<double, std::milli> execution_time = end_exec_time - start_exec_time;
-      ROS_INFO("Callback execution time: %f ms, Elapsed time: %f sec", execution_time.count(),
-               elapsed_time);
-    }
-
-    tick_++;
-    loop_Hz_.sleep();
-  }
-}
-
-void DobImpedanceController::publish_topics()
-{
-  /* publish joint pos, vel, torque command */
-  joint_cmd_msg.position.clear();
-  joint_cmd_msg.velocity.clear();
-  joint_cmd_msg.effort.clear();
-  for (size_t i = 0; i < 7; ++i)
-  {
-    joint_cmd_msg.position.push_back(0.0);
-    joint_cmd_msg.velocity.push_back(0.0);
-    joint_cmd_msg.effort.push_back(tau_des_[i]);
-  }
-  joint_cmd_pub_.publish(joint_cmd_msg);
-
-  // ! Check index of real HW's coordinates
-  ee_perturb_msg.force.x = fr_F_ext_hat_[1];
-  ee_perturb_msg.force.y = fr_F_ext_hat_[0];
-  ee_perturb_msg.force.z = fr_F_ext_hat_[2];
-  ee_perturb_msg.torque.x = fr_F_ext_hat_[3];
-  ee_perturb_msg.torque.y = fr_F_ext_hat_[4];
-  ee_perturb_msg.torque.z = fr_F_ext_hat_[5];
-  ee_perturb_pub_.publish(ee_perturb_msg);
-}
-
-void DobImpedanceController::update_previous_values()
-{
-  dq_mes_prev_ = dq_mes_;
-  ddq_eul_prev_ = ddq_eul_;
-  ee_pose_error_prev_ = ee_pose_error_;
-  ee_rpy_world_des_prev_ = ee_rpy_world_des_;
-  ee_rpy_world_mes_prev_ = ee_rpy_world_mes_;
-  ee_twist_world_cal_prev_ = ee_twist_world_cal_;
-  ee_accel_world_eul_prev_ = ee_accel_world_eul_;
-}
-
-//* ----- PRINTER ------------------------------------------------------------------------------ *//
-void DobImpedanceController::print_jacobian()
-{
-  cout << "Jacobian:\n" << Je_ << endl;
-  cout << "Jacobian derivative:\n" << dJe_ << endl;
-}
-
-void DobImpedanceController::print_ee_position()
-{
-  cout << "Desired EE position:\t" << ee_pos_world_des_.transpose() << endl;
-  cout << "Pinocchio EE position:\t" << ee_pos_world_mes_.transpose() << endl;
-  cout << "MJCF EE position:\t" << mj_ee_pos_world_.transpose() << endl << endl;
-}
-
-void DobImpedanceController::print_ee_quaternion()
-{
-  cout << "Desired EE quaternion:\t" << ee_quat_world_des_ << endl;
-  cout << "Pinocchio EE quaternion:\t" << ee_quat_world_mes_ << endl;
-  cout << "MJCF EE quaternion:\t" << mj_ee_quat_world_ << endl << endl;
-}
-
-void DobImpedanceController::print_ee_twist()
-{
-  cout << "Desired EE Twist:\t" << ee_twist_world_des_.transpose() << endl;
-  cout << "Computed EE Twist:\t" << ee_twist_world_cal_.transpose() << endl;
-  cout << "MJCF vel:\t" << mj_ee_twist_world_.transpose() << endl << endl;
-}
-
-void DobImpedanceController::print_ee_accel()
-{
-  cout << "Computed EE accel:\t" << ee_accel_world_eul_.transpose() << endl;
-  cout << "Estimated EE accel:\t" << ee_accel_world_hat_.transpose() << endl;
-  cout << "MJCF sensor data:\t" << mj_ee_accel_world_.transpose() << endl << endl;
-}
-
-void DobImpedanceController::print_impedance_control()
-{
-  cout << "Impedance Force:\t" << F_task_.transpose() << endl;
-  cout << "Computed torque:\t" << tau_task_.transpose() << endl;
-}
-
-void DobImpedanceController::print_torque_command()
-{
-  cout << "Task torque:\t" << tau_task_.transpose() << endl;
-  cout << "Nullspace torque:\t" << tau_null_.transpose() << endl;
-  cout << "Coriolis torque:\t" << tau_c_.transpose() << endl;
-  cout << "Computed torque:\t" << tau_des_.transpose() << endl;
-  cout << "Desired torque:\t" << fr_tau_des_.transpose() << endl << endl;
-  cout << "Measured torque:\t" << fr_tau_mes_.transpose() << endl;
-}
\ No newline at end of file
diff --git a/src/dob_impedance_controller_node.cpp b/src/dob_impedance_controller_node.cpp
deleted file mode 100644
index a70ea14..0000000
--- a/src/dob_impedance_controller_node.cpp
+++ /dev/null
@@ -1,20 +0,0 @@
-#include "dob_impedance_controller.hpp"
-
-#include <iostream>
-#include <memory>
-
-using namespace std;
-
-int main(int argc, char ** argv)
-{
-  /* initialize and set up ROS node */
-  std::string node_name = "dob_impedance_controller_node";
-  ros::init(argc, argv, node_name);
-  ros::NodeHandle nh;
-
-  double Hz = 500;
-  auto node = std::make_shared<DobImpedanceController>(nh, Hz);
-  node->run_loop();
-
-  return 0;
-}
\ No newline at end of file
-- 
GitLab


From 4b2b8e2bcb233bdfdf86137bfdba19ae832bd50a Mon Sep 17 00:00:00 2001
From: jeo65286 <jeo65286@ipk-extern.fraunhofer.de>
Date: Mon, 13 Jan 2025 15:23:15 +0100
Subject: [PATCH 2/9] Add header and source file of franka_example_controllers

---
 .../fr3_impedance_controller_real_plugin.xml  |   0
 .../fr3_impedance_controller_real.hpp         |  77 +++++
 .../src/fr3_impedance_controller_real.cpp     | 265 ++++++++++++++++++
 3 files changed, 342 insertions(+)
 create mode 100644 fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml
 create mode 100644 fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
 create mode 100644 fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp

diff --git a/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml b/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml
new file mode 100644
index 0000000..e69de29
diff --git a/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp b/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
new file mode 100644
index 0000000..5815efe
--- /dev/null
+++ b/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
@@ -0,0 +1,77 @@
+/**
+ * @file fr3_impedance_controller_real.hpp
+ * This file is impedance controller integrated with observers that operates for real hardware
+ * Franka Research 3 (FR3) robot. Compared to local `franka_ros` package that can be installed
+ * at https://github.com/frankaemika/franka_ros, this is a ROS1 package that is set up in Nix
+ * environment.
+ */
+
+#ifndef IPK_FR3_IMPEDANCE_CONTROLLER_REAL_HPP_
+#define IPK_FR3_IMPEDANCE_CONTROLLER_REAL_HPP_
+
+#include <memory>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include <controller_interface/multi_interface_controller.h>
+#include <dynamic_reconfigure/server.h>
+#include <geometry_msgs/PoseStamped.h>
+#include <hardware_interface/joint_command_interface.h>
+#include <hardware_interface/robot_hw.h>
+#include <ros/node_handle.h>
+#include <ros/time.h>
+#include <Eigen/Dense>
+
+#include <franka_example_controllers/compliance_paramConfig.h>
+#include <franka_hw/franka_model_interface.h>
+#include <franka_hw/franka_state_interface.h>
+
+class FR3ImpedanceControllerReal :
+  public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface,
+                                                        hardware_interface::EffortJointInterface,
+                                                        franka_hw::FrankaStateInterface>
+{
+private:
+  // Saturation
+  Eigen::Matrix<double, 7, 1> saturateTorqueRate(
+    const Eigen::Matrix<double, 7, 1> & tau_d_calculated,
+    const Eigen::Matrix<double, 7, 1> & tau_J_d);  // NOLINT (readability-identifier-naming)
+
+  std::unique_ptr<franka_hw::FrankaStateHandle> state_handle_;
+  std::unique_ptr<franka_hw::FrankaModelHandle> model_handle_;
+  std::vector<hardware_interface::JointHandle> joint_handles_;
+
+  double filter_params_{ 0.005 };
+  double nullspace_stiffness_{ 20.0 };
+  double nullspace_stiffness_target_{ 20.0 };
+  const double delta_tau_max_{ 1.0 };
+  Eigen::Matrix<double, 6, 6> cartesian_stiffness_;
+  Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_;
+  Eigen::Matrix<double, 6, 6> cartesian_damping_;
+  Eigen::Matrix<double, 6, 6> cartesian_damping_target_;
+  Eigen::Matrix<double, 7, 1> q_d_nullspace_;
+  Eigen::Vector3d position_d_;
+  Eigen::Quaterniond orientation_d_;
+  std::mutex position_and_orientation_d_target_mutex_;
+  Eigen::Vector3d position_d_target_;
+  Eigen::Quaterniond orientation_d_target_;
+
+  // Dynamic reconfigure
+  std::unique_ptr<dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>
+    dynamic_server_compliance_param_;
+  ros::NodeHandle dynamic_reconfigure_compliance_param_node_;
+  void complianceParamCallback(franka_example_controllers::compliance_paramConfig & config,
+                               uint32_t level);
+
+  // Equilibrium pose subscriber
+  ros::Subscriber sub_equilibrium_pose_;
+  void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr & msg);
+
+public:
+  bool init(hardware_interface::RobotHW * robot_hw, ros::NodeHandle & node_handle) override;
+  void starting(const ros::Time &) override;
+  void update(const ros::Time &, const ros::Duration & period) override;
+};
+
+#endif  // IPK_FR3_IMPEDANCE_CONTROLLER_REAL_HPP_
\ No newline at end of file
diff --git a/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp b/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp
new file mode 100644
index 0000000..68b9e6b
--- /dev/null
+++ b/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp
@@ -0,0 +1,265 @@
+#include "fr3_impedance_controller_real.hpp"
+
+#include <cmath>
+#include <memory>
+
+#include <controller_interface/controller_base.h>
+#include <franka/robot_state.h>
+#include <pluginlib/class_list_macros.h>
+#include <ros/ros.h>
+
+bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW * robot_hw,
+                                               ros::NodeHandle & node_handle)
+{
+  std::vector<double> cartesian_stiffness_vector;
+  std::vector<double> cartesian_damping_vector;
+
+  sub_equilibrium_pose_ = node_handle.subscribe(
+    "equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
+    ros::TransportHints().reliable().tcpNoDelay());
+
+  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;
+    }
+  }
+
+  dynamic_reconfigure_compliance_param_node_ =
+    ros::NodeHandle(node_handle.getNamespace() + "/dynamic_reconfigure_compliance_param_node");
+
+  dynamic_server_compliance_param_ = std::make_unique<
+    dynamic_reconfigure::Server<franka_example_controllers::compliance_paramConfig>>(
+
+    dynamic_reconfigure_compliance_param_node_);
+  dynamic_server_compliance_param_->setCallback(
+    boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
+
+  position_d_.setZero();
+  orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+  position_d_target_.setZero();
+  orientation_d_target_.coeffs() << 0.0, 0.0, 0.0, 1.0;
+
+  cartesian_stiffness_.setZero();
+  cartesian_damping_.setZero();
+
+  return true;
+}
+
+void CartesianImpedanceExampleController::starting(const ros::Time & /*time*/)
+{
+  // compute initial velocity with jacobian and set x_attractor and q_d_nullspace
+  // to initial configuration
+  franka::RobotState initial_state = state_handle_->getRobotState();
+  // get jacobian
+  std::array<double, 42> jacobian_array =
+    model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+  // convert to eigen
+  Eigen::Map<Eigen::Matrix<double, 7, 1>> q_initial(initial_state.q.data());
+  Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(initial_state.O_T_EE.data()));
+
+  // set equilibrium point to current state
+  position_d_ = initial_transform.translation();
+  orientation_d_ = Eigen::Quaterniond(initial_transform.rotation());
+  position_d_target_ = initial_transform.translation();
+  orientation_d_target_ = Eigen::Quaterniond(initial_transform.rotation());
+
+  // set nullspace equilibrium configuration to initial q
+  q_d_nullspace_ = q_initial;
+}
+
+void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
+                                                 const ros::Duration & /*period*/)
+{
+  // get state variables
+  franka::RobotState robot_state = state_handle_->getRobotState();
+  std::array<double, 7> coriolis_array = model_handle_->getCoriolis();
+  std::array<double, 42> jacobian_array =
+    model_handle_->getZeroJacobian(franka::Frame::kEndEffector);
+
+  // convert to Eigen
+  Eigen::Map<Eigen::Matrix<double, 7, 1>> coriolis(coriolis_array.data());
+  Eigen::Map<Eigen::Matrix<double, 6, 7>> jacobian(jacobian_array.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1>> q(robot_state.q.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1>> dq(robot_state.dq.data());
+  Eigen::Map<Eigen::Matrix<double, 7, 1>> tau_J_d(  // NOLINT (readability-identifier-naming)
+    robot_state.tau_J_d.data());
+  Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
+  Eigen::Vector3d position(transform.translation());
+  Eigen::Quaterniond orientation(transform.rotation());
+
+  // compute error to desired pose
+  // position error
+  Eigen::Matrix<double, 6, 1> error;
+  error.head(3) << position - position_d_;
+
+  // orientation error
+  if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0)
+  {
+    orientation.coeffs() << -orientation.coeffs();
+  }
+  // "difference" quaternion
+  Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d_);
+  error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
+  // Transform to base frame
+  error.tail(3) << -transform.rotation() * error.tail(3);
+
+  // compute control
+  // allocate variables
+  Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7);
+
+  // pseudoinverse for nullspace handling
+  // kinematic pseuoinverse
+  Eigen::MatrixXd jacobian_transpose_pinv;
+  pseudoInverse(jacobian.transpose(), jacobian_transpose_pinv);
+
+  // Cartesian PD control with damping ratio = 1
+  tau_task << jacobian.transpose() *
+                (-cartesian_stiffness_ * error - cartesian_damping_ * (jacobian * dq));
+  // nullspace PD control with damping ratio = 1
+  tau_nullspace << (Eigen::MatrixXd::Identity(7, 7) -
+                    jacobian.transpose() * jacobian_transpose_pinv) *
+                     (nullspace_stiffness_ * (q_d_nullspace_ - q) -
+                      (2.0 * sqrt(nullspace_stiffness_)) * dq);
+  // Desired torque
+  tau_d << tau_task + tau_nullspace + coriolis;
+  // Saturate torque rate to avoid discontinuities
+  tau_d << saturateTorqueRate(tau_d, tau_J_d);
+  for (size_t i = 0; i < 7; ++i)
+  {
+    joint_handles_[i].setCommand(tau_d(i));
+  }
+
+  // update parameters changed online either through dynamic reconfigure or through the interactive
+  // target by filtering
+  cartesian_stiffness_ =
+    filter_params_ * cartesian_stiffness_target_ + (1.0 - filter_params_) * cartesian_stiffness_;
+  cartesian_damping_ =
+    filter_params_ * cartesian_damping_target_ + (1.0 - filter_params_) * cartesian_damping_;
+  nullspace_stiffness_ =
+    filter_params_ * nullspace_stiffness_target_ + (1.0 - filter_params_) * nullspace_stiffness_;
+  std::lock_guard<std::mutex> position_d_target_mutex_lock(
+    position_and_orientation_d_target_mutex_);
+  position_d_ = filter_params_ * position_d_target_ + (1.0 - filter_params_) * position_d_;
+  orientation_d_ = orientation_d_.slerp(filter_params_, orientation_d_target_);
+}
+
+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;
+}
+
+void CartesianImpedanceExampleController::complianceParamCallback(
+  franka_example_controllers::compliance_paramConfig & config, uint32_t /*level*/)
+{
+  cartesian_stiffness_target_.setIdentity();
+  cartesian_stiffness_target_.topLeftCorner(3, 3)
+    << config.translational_stiffness * Eigen::Matrix3d::Identity();
+  cartesian_stiffness_target_.bottomRightCorner(3, 3)
+    << config.rotational_stiffness * Eigen::Matrix3d::Identity();
+  cartesian_damping_target_.setIdentity();
+  // Damping ratio = 1
+  cartesian_damping_target_.topLeftCorner(3, 3)
+    << 2.0 * sqrt(config.translational_stiffness) * Eigen::Matrix3d::Identity();
+  cartesian_damping_target_.bottomRightCorner(3, 3)
+    << 2.0 * sqrt(config.rotational_stiffness) * Eigen::Matrix3d::Identity();
+  nullspace_stiffness_target_ = config.nullspace_stiffness;
+}
+
+void CartesianImpedanceExampleController::equilibriumPoseCallback(
+  const geometry_msgs::PoseStampedConstPtr & msg)
+{
+  std::lock_guard<std::mutex> position_d_target_mutex_lock(
+    position_and_orientation_d_target_mutex_);
+
+  position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
+
+  Eigen::Quaterniond last_orientation_d_target(orientation_d_target_);
+  orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y,
+    msg->pose.orientation.z, msg->pose.orientation.w;
+
+  if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0)
+  {
+    orientation_d_target_.coeffs() << -orientation_d_target_.coeffs();
+  }
+}
+
+PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController,
+                       controller_interface::ControllerBase)
\ No newline at end of file
-- 
GitLab


From f3ae134258767a1d8fc42d9e16148443092672c5 Mon Sep 17 00:00:00 2001
From: jeo65286 <jeo65286@ipk-extern.fraunhofer.de>
Date: Mon, 13 Jan 2025 15:34:27 +0100
Subject: [PATCH 3/9] Add variables for interactive marker

---
 .../fr3_impedance_controller_real.hpp                      | 7 +++++++
 1 file changed, 7 insertions(+)

diff --git a/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp b/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
index 5815efe..6af5cd4 100644
--- a/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
+++ b/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
@@ -68,6 +68,13 @@ private:
   ros::Subscriber sub_equilibrium_pose_;
   void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr & msg);
 
+  /* Target EE Pose command */
+  // bool b_is_marker_mode_ = false;
+  // ros::Subscriber sub_interactive_marker_;
+  // std::mutex target_ee_pose_mutex_;
+  // Vec3<double> ee_pos_world_des_target_;
+  // Quat<double> ee_quat_world_des_target_;
+
 public:
   bool init(hardware_interface::RobotHW * robot_hw, ros::NodeHandle & node_handle) override;
   void starting(const ros::Time &) override;
-- 
GitLab


From 28132214469bfeebdd3e29c029bb258e7f993c7d Mon Sep 17 00:00:00 2001
From: jeo65286 <jeo65286@ipk-extern.fraunhofer.de>
Date: Mon, 20 Jan 2025 14:42:50 +0100
Subject: [PATCH 4/9] Edit header and source codes, Add cfg file

---
 .../cfg/compliance_param.cfg                  |  16 +++
 .../fr3_impedance_controller_real.hpp         |  64 ++++++---
 .../src/fr3_impedance_controller_real.cpp     | 132 +++++++++++++++---
 3 files changed, 179 insertions(+), 33 deletions(-)
 create mode 100644 fr3_impedance_controller_real/cfg/compliance_param.cfg

diff --git a/fr3_impedance_controller_real/cfg/compliance_param.cfg b/fr3_impedance_controller_real/cfg/compliance_param.cfg
new file mode 100644
index 0000000..6d5bd84
--- /dev/null
+++ b/fr3_impedance_controller_real/cfg/compliance_param.cfg
@@ -0,0 +1,16 @@
+#!/usr/bin/env python
+PACKAGE = "franka_example_controllers"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+gen.add("translational_stiffness", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("translational_stiffness_x", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("translational_stiffness_y", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("translational_stiffness_z", double_t, 0, "Cartesian translational stiffness", 200, 0, 400)
+gen.add("rotational_stiffness", double_t, 0, "Cartesian rotational stiffness", 10, 0, 30)
+gen.add("nullspace_stiffness", double_t, 0, "Stiffness of the joint space nullspace controller (the desired configuration is the one at startup)", 0.5, 0, 100)
+
+exit(gen.generate(PACKAGE, "dynamic_compliance", "compliance_param"))
+
diff --git a/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp b/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
index 6af5cd4..bfe738d 100644
--- a/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
+++ b/fr3_impedance_controller_real/include/fr3_impedance_controller_real/fr3_impedance_controller_real.hpp
@@ -1,13 +1,6 @@
-/**
- * @file fr3_impedance_controller_real.hpp
- * This file is impedance controller integrated with observers that operates for real hardware
- * Franka Research 3 (FR3) robot. Compared to local `franka_ros` package that can be installed
- * at https://github.com/frankaemika/franka_ros, this is a ROS1 package that is set up in Nix
- * environment.
- */
-
-#ifndef IPK_FR3_IMPEDANCE_CONTROLLER_REAL_HPP_
-#define IPK_FR3_IMPEDANCE_CONTROLLER_REAL_HPP_
+// Copyright (c) 2017 Franka Emika GmbH
+// Use of this source code is governed by the Apache-2.0 license, see LICENSE
+#pragma once
 
 #include <memory>
 #include <mutex>
@@ -23,15 +16,28 @@
 #include <ros/time.h>
 #include <Eigen/Dense>
 
+#include <geometry_msgs/Twist.h>      // for spacenave teleop
+#include <sensor_msgs/Joy.h>          // for joystick command
+#include "eigen_types.hpp"            // for custom Eigen types
+#include "orientation_utilities.hpp"  // for orientation computation
+
 #include <franka_example_controllers/compliance_paramConfig.h>
 #include <franka_hw/franka_model_interface.h>
 #include <franka_hw/franka_state_interface.h>
 
-class FR3ImpedanceControllerReal :
+namespace franka_example_controllers
+{
+class CartesianImpedanceExampleController :
   public controller_interface::MultiInterfaceController<franka_hw::FrankaModelInterface,
                                                         hardware_interface::EffortJointInterface,
                                                         franka_hw::FrankaStateInterface>
 {
+public:
+  bool init(hardware_interface::RobotHW * robot_hw, ros::NodeHandle & node_handle) override;
+  void starting(const ros::Time &) override;
+  //   void stopping(const ros::Time&) override;
+  void update(const ros::Time &, const ros::Duration & period) override;
+
 private:
   // Saturation
   Eigen::Matrix<double, 7, 1> saturateTorqueRate(
@@ -46,6 +52,17 @@ private:
   double nullspace_stiffness_{ 20.0 };
   double nullspace_stiffness_target_{ 20.0 };
   const double delta_tau_max_{ 1.0 };
+
+  const std::vector<double> fric_phi1{
+    0.54615, 0.87224, 0.64068, 1.2794, 0.83904, 0.30301, 0.56489
+  };
+  const std::vector<double> fric_phi2{ 5.1181, 9.0657, 10.136, 5.590, 8.3469, 17.133, 10.336 };
+  // const std::vector<double> fric_phi3{0.039533, 0.025882, -0.04607, 0.036194, 0.026226,
+  // -0.021047, 0.0035526};
+  const std::vector<double> fric_phi3{
+    0., 0., 0., 0., 0., 0., 0.,
+  };
+
   Eigen::Matrix<double, 6, 6> cartesian_stiffness_;
   Eigen::Matrix<double, 6, 6> cartesian_stiffness_target_;
   Eigen::Matrix<double, 6, 6> cartesian_damping_;
@@ -75,10 +92,25 @@ private:
   // Vec3<double> ee_pos_world_des_target_;
   // Quat<double> ee_quat_world_des_target_;
 
-public:
-  bool init(hardware_interface::RobotHW * robot_hw, ros::NodeHandle & node_handle) override;
-  void starting(const ros::Time &) override;
-  void update(const ros::Time &, const ros::Duration & period) override;
+  /* spacenav target twist subscriber */
+  ros::Subscriber sub_target_twist_;
+  void subscribe_target_twist(const geometry_msgs::TwistConstPtr & msg);
+  //   Vec6<double> ee_vel_world_target_;
+  Vec6<double> ee_vel_scale_;
+  Vec6<double> ee_vel_world_des_, ee_vel_world_des_prev_;
+  Vec6<double> ee_acc_world_des_, ee_acc_world_des_prev_;
+  Vec3<double> ee_pos_world_des_;
+  Quat<double> ee_quat_world_des_;
+  Vec3<double> ee_rpy_world_des_, ee_rpy_world_des_prev_;
+  Vec3<double> ee_rpy_world_mes_, ee_rpy_world_mes_prev_;
+
+  Vec6<double> fr3_vel_limit_;
+  Vec6<double> fr3_acc_limit_;
+  Vec6<double> fr3_jrk_limit_;
+
+  ros::Subscriber sub_joy_;
+  void subscribe_joy_command(const sensor_msgs::JoyConstPtr & msg);
+  std::array<int, 2> btn_cmd_;
 };
 
-#endif  // IPK_FR3_IMPEDANCE_CONTROLLER_REAL_HPP_
\ No newline at end of file
+}  // namespace franka_example_controllers
diff --git a/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp b/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp
index 68b9e6b..e7798ab 100644
--- a/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp
+++ b/fr3_impedance_controller_real/src/fr3_impedance_controller_real.cpp
@@ -1,6 +1,9 @@
-#include "fr3_impedance_controller_real.hpp"
+// 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>
@@ -8,16 +11,32 @@
 #include <pluginlib/class_list_macros.h>
 #include <ros/ros.h>
 
+#include <franka_example_controllers/pseudo_inversion.h>
+
+namespace franka_example_controllers
+{
 bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW * robot_hw,
                                                ros::NodeHandle & node_handle)
 {
   std::vector<double> cartesian_stiffness_vector;
   std::vector<double> cartesian_damping_vector;
 
+  /* Subscriber declaration */
   sub_equilibrium_pose_ = node_handle.subscribe(
     "equilibrium_pose", 20, &CartesianImpedanceExampleController::equilibriumPoseCallback, this,
     ros::TransportHints().reliable().tcpNoDelay());
 
+  sub_target_twist_ = node_handle.subscribe<geometry_msgs::Twist>(
+    "/spacenav/twist", 1, &CartesianImpedanceExampleController::subscribe_target_twist, this);
+
+  sub_joy_ = node_handle.subscribe<sensor_msgs::Joy>(
+    "/spacenave/joy", 1, &CartesianImpedanceExampleController::subscribe_joy_command, this);
+
+  /* Initialize limits */
+  fr3_vel_limit_ << 3.0, 3.0, 3.0, 2.5, 2.5, 2.5;                    // [m/s, rad/s]
+  fr3_acc_limit_ << 9.0, 9.0, 9.0, 17.0, 17.0, 17.0;                 // [m/s^2, rad/s^2]
+  fr3_jrk_limit_ << 4500.0, 4500.0, 4500.0, 8500.0, 8500.0, 8500.0;  // [m/s^3, rad/s^3]
+
   std::string arm_id;
   if (!node_handle.getParam("arm_id", arm_id))
   {
@@ -104,6 +123,19 @@ bool CartesianImpedanceExampleController::init(hardware_interface::RobotHW * rob
   dynamic_server_compliance_param_->setCallback(
     boost::bind(&CartesianImpedanceExampleController::complianceParamCallback, this, _1, _2));
 
+  /* Initialize states */
+  ee_vel_scale_ << 0.1, 0.1, 0.1, 0.1, 0.1, 0.1;
+  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_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_rpy_world_mes_.setZero();
+  ee_rpy_world_mes_prev_.setZero();
+
   position_d_.setZero();
   orientation_d_.coeffs() << 0.0, 0.0, 0.0, 1.0;
   position_d_target_.setZero();
@@ -133,13 +165,31 @@ void CartesianImpedanceExampleController::starting(const ros::Time & /*time*/)
   position_d_target_ = initial_transform.translation();
   orientation_d_target_ = Eigen::Quaterniond(initial_transform.rotation());
 
+  ee_pos_world_des_ = initial_transform.translation();
+  ee_quat_world_des_ = Eigen::Quaterniond(initial_transform.rotation());
+
   // set nullspace equilibrium configuration to initial q
   q_d_nullspace_ = q_initial;
 }
 
 void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
-                                                 const ros::Duration & /*period*/)
+                                                 const ros::Duration & period)
 {
+  /* get target end-effector's twist */
+  // std::cout << std::fixed;
+  // std::cout << std::setprecision(5);
+  // std::cout << "Desired EE twist:\t" << ee_vel_world_des_.transpose() << std::endl;
+
+  // 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 * period.toSec();
+  ee_quat_world_des_ = orient_utils::getNewQuat(ee_quat_world_des_, w_des, period.toSec());
+  ee_rpy_world_des_ = orient_utils::quaternionToRPY(ee_quat_world_des_, ee_rpy_world_des_prev_);
+
+  // TODO: Add conversion of Quat2RPY to make debugging easier
+
   // get state variables
   franka::RobotState robot_state = state_handle_->getRobotState();
   std::array<double, 7> coriolis_array = model_handle_->getCoriolis();
@@ -156,26 +206,34 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
   Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data()));
   Eigen::Vector3d position(transform.translation());
   Eigen::Quaterniond orientation(transform.rotation());
+  ee_rpy_world_mes_ = orient_utils::quaternionToRPY(orientation, ee_rpy_world_mes_prev_);
+  // std::cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << std::endl;
+  // std::cout << "Measured EE pos [m]:\t" << position.transpose() << std::endl << std::endl;
+  // std::cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << std::endl;
+  // std::cout << "Measured EE RPY [rad]:\t" << ee_rpy_world_mes_.transpose() << std::endl <<
+  // std::endl;
 
   // compute error to desired pose
   // position error
   Eigen::Matrix<double, 6, 1> error;
-  error.head(3) << position - position_d_;
+  error.head(3) << position - ee_pos_world_des_;
 
   // orientation error
-  if (orientation_d_.coeffs().dot(orientation.coeffs()) < 0.0)
+  if (ee_quat_world_des_.coeffs().dot(orientation.coeffs()) < 0.0)
   {
     orientation.coeffs() << -orientation.coeffs();
   }
   // "difference" quaternion
-  Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d_);
+  Eigen::Quaterniond error_quaternion(orientation.inverse() * ee_quat_world_des_);
   error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z();
   // Transform to base frame
   error.tail(3) << -transform.rotation() * error.tail(3);
 
   // compute control
   // allocate variables
-  Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7);
+  Eigen::VectorXd tau_task(7), tau_nullspace(7), tau_d(7), tau_fr(7);
+
+  tau_fr.setZero();
 
   // pseudoinverse for nullspace handling
   // kinematic pseuoinverse
@@ -190,15 +248,31 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
                     jacobian.transpose() * jacobian_transpose_pinv) *
                      (nullspace_stiffness_ * (q_d_nullspace_ - q) -
                       (2.0 * sqrt(nullspace_stiffness_)) * dq);
+
+  // calculate friction torque
+  for (size_t i = 0; i < 7; i++)
+  {
+    tau_fr(i) = fric_phi1[i] / (1 + exp(-fric_phi2[i] * (dq(i) + fric_phi3[i]))) -
+                fric_phi1[i] / (1 + exp(-fric_phi2[i] * fric_phi3[i]));
+  }
+
   // Desired torque
-  tau_d << tau_task + tau_nullspace + coriolis;
+  tau_d << tau_task + tau_nullspace + coriolis + tau_fr;
+
   // Saturate torque rate to avoid discontinuities
   tau_d << saturateTorqueRate(tau_d, tau_J_d);
+  // std::cout << "Desired torque:\t" << tau_d.transpose() << std::endl;
   for (size_t i = 0; i < 7; ++i)
   {
     joint_handles_[i].setCommand(tau_d(i));
   }
 
+  /* update previous states */
+  ee_vel_world_des_prev_ = ee_vel_world_des_;
+  ee_acc_world_des_prev_ = ee_acc_world_des_;
+  ee_rpy_world_des_prev_ = ee_rpy_world_des_;
+  ee_rpy_world_mes_prev_ = ee_rpy_world_mes_;
+
   // update parameters changed online either through dynamic reconfigure or through the interactive
   // target by filtering
   cartesian_stiffness_ =
@@ -230,16 +304,22 @@ void CartesianImpedanceExampleController::complianceParamCallback(
   franka_example_controllers::compliance_paramConfig & config, uint32_t /*level*/)
 {
   cartesian_stiffness_target_.setIdentity();
-  cartesian_stiffness_target_.topLeftCorner(3, 3)
-    << config.translational_stiffness * Eigen::Matrix3d::Identity();
+
+  // 26.4.23 @Kevin, change so can set stiffness independent per DOF
+  // cartesian_stiffness_target_.topLeftCorner(3, 3)
+  //    << config.translational_stiffness * Eigen::Matrix3d::Identity();
+  cartesian_stiffness_target_(0, 0) = config.translational_stiffness_x;
+  cartesian_stiffness_target_(1, 1) = config.translational_stiffness_y;
+  cartesian_stiffness_target_(2, 2) = config.translational_stiffness_z;
+
   cartesian_stiffness_target_.bottomRightCorner(3, 3)
     << config.rotational_stiffness * Eigen::Matrix3d::Identity();
   cartesian_damping_target_.setIdentity();
   // Damping ratio = 1
-  cartesian_damping_target_.topLeftCorner(3, 3)
-    << 2.0 * sqrt(config.translational_stiffness) * Eigen::Matrix3d::Identity();
-  cartesian_damping_target_.bottomRightCorner(3, 3)
-    << 2.0 * sqrt(config.rotational_stiffness) * Eigen::Matrix3d::Identity();
+  for (size_t i = 0; i < 6; i++)
+  {
+    cartesian_damping_target_(i, i) = 2.0 * sqrt(cartesian_stiffness_target_(i, i));
+  }
   nullspace_stiffness_target_ = config.nullspace_stiffness;
 }
 
@@ -248,18 +328,36 @@ void CartesianImpedanceExampleController::equilibriumPoseCallback(
 {
   std::lock_guard<std::mutex> position_d_target_mutex_lock(
     position_and_orientation_d_target_mutex_);
-
   position_d_target_ << msg->pose.position.x, msg->pose.position.y, msg->pose.position.z;
-
   Eigen::Quaterniond last_orientation_d_target(orientation_d_target_);
   orientation_d_target_.coeffs() << msg->pose.orientation.x, msg->pose.orientation.y,
     msg->pose.orientation.z, msg->pose.orientation.w;
-
   if (last_orientation_d_target.coeffs().dot(orientation_d_target_.coeffs()) < 0.0)
   {
     orientation_d_target_.coeffs() << -orientation_d_target_.coeffs();
   }
 }
 
+void CartesianImpedanceExampleController::subscribe_target_twist(
+  const geometry_msgs::TwistConstPtr & msg)
+{
+  // std::cout << msg->linear.x <<  ", " << msg->linear.y << ", " << msg->linear.z << std::endl;
+  ee_vel_world_des_[0] = ee_vel_scale_[0] * msg->linear.x;
+  ee_vel_world_des_[1] = ee_vel_scale_[1] * msg->linear.y;
+  ee_vel_world_des_[2] = ee_vel_scale_[2] * msg->linear.z;
+  ee_vel_world_des_[3] = ee_vel_scale_[3] * msg->angular.x;
+  ee_vel_world_des_[4] = ee_vel_scale_[4] * msg->angular.y;
+  ee_vel_world_des_[5] = ee_vel_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];
+}
+
+}  // namespace franka_example_controllers
+
 PLUGINLIB_EXPORT_CLASS(franka_example_controllers::CartesianImpedanceExampleController,
-                       controller_interface::ControllerBase)
\ No newline at end of file
+                       controller_interface::ControllerBase)
-- 
GitLab


From fe2a18b65a91158b3b8118ab79ef2f8300acae1d Mon Sep 17 00:00:00 2001
From: jeo65286 <jeo65286@ipk-extern.fraunhofer.de>
Date: Mon, 20 Jan 2025 14:43:58 +0100
Subject: [PATCH 5/9] Add franka_example_controllers YAML file in config

---
 .../config/franka_example_controllers.yaml    | 179 ++++++++++++++++++
 1 file changed, 179 insertions(+)
 create mode 100644 fr3_impedance_controller_real/config/franka_example_controllers.yaml

diff --git a/fr3_impedance_controller_real/config/franka_example_controllers.yaml b/fr3_impedance_controller_real/config/franka_example_controllers.yaml
new file mode 100644
index 0000000..4cb8fa5
--- /dev/null
+++ b/fr3_impedance_controller_real/config/franka_example_controllers.yaml
@@ -0,0 +1,179 @@
+joint_velocity_example_controller:
+    type: franka_example_controllers/JointVelocityExampleController
+    arm_id: $(arg arm_id)
+    joint_names:
+        - $(arg arm_id)_joint1
+        - $(arg arm_id)_joint2
+        - $(arg arm_id)_joint3
+        - $(arg arm_id)_joint4
+        - $(arg arm_id)_joint5
+        - $(arg arm_id)_joint6
+        - $(arg arm_id)_joint7
+
+joint_position_example_controller:
+    type: franka_example_controllers/JointPositionExampleController
+    joint_names:
+        - $(arg arm_id)_joint1
+        - $(arg arm_id)_joint2
+        - $(arg arm_id)_joint3
+        - $(arg arm_id)_joint4
+        - $(arg arm_id)_joint5
+        - $(arg arm_id)_joint6
+        - $(arg arm_id)_joint7
+
+cartesian_sinpose_controller:
+    type: franka_example_controllers/CartesianSinPoseExampleController
+    arm_id: $(arg arm_id)
+
+cartesian_pose_example_controller:
+    type: franka_example_controllers/CartesianPoseExampleController
+    arm_id: $(arg arm_id)
+
+elbow_example_controller:
+    type: franka_example_controllers/ElbowExampleController
+    arm_id: $(arg arm_id)
+
+cartesian_velocity_example_controller:
+    type: franka_example_controllers/CartesianVelocityExampleController
+    arm_id: $(arg arm_id)
+
+model_example_controller:
+    type: franka_example_controllers/ModelExampleController
+    arm_id: $(arg arm_id)
+
+force_example_controller:
+    type: franka_example_controllers/ForceExampleController
+    arm_id: $(arg arm_id)
+    joint_names:
+        - $(arg arm_id)_joint1
+        - $(arg arm_id)_joint2
+        - $(arg arm_id)_joint3
+        - $(arg arm_id)_joint4
+        - $(arg arm_id)_joint5
+        - $(arg arm_id)_joint6
+        - $(arg arm_id)_joint7
+
+joint_impedance_example_controller:
+    type: franka_example_controllers/JointImpedanceExampleController
+    arm_id: $(arg arm_id)
+    joint_names:
+        - $(arg arm_id)_joint1
+        - $(arg arm_id)_joint2
+        - $(arg arm_id)_joint3
+        - $(arg arm_id)_joint4
+        - $(arg arm_id)_joint5
+        - $(arg arm_id)_joint6
+        - $(arg arm_id)_joint7
+    k_gains:
+        - 600.0
+        - 600.0
+        - 600.0
+        - 600.0
+        - 250.0
+        - 150.0
+        - 50.0
+    d_gains:
+        - 50.0
+        - 50.0
+        - 50.0
+        - 20.0
+        - 20.0
+        - 20.0
+        - 10.0
+    radius: 0.1
+    acceleration_time: 2.0
+    vel_max: 0.15
+    publish_rate: 10.0
+    coriolis_factor: 1.0
+
+cartesian_impedance_example_controller:
+    type: franka_example_controllers/CartesianImpedanceExampleController
+    arm_id: $(arg arm_id)
+    joint_names:
+        - $(arg arm_id)_joint1
+        - $(arg arm_id)_joint2
+        - $(arg arm_id)_joint3
+        - $(arg arm_id)_joint4
+        - $(arg arm_id)_joint5
+        - $(arg arm_id)_joint6
+        - $(arg arm_id)_joint7
+
+dual_arm_cartesian_impedance_example_controller:
+    type: franka_example_controllers/DualArmCartesianImpedanceExampleController
+    right:
+        arm_id: panda_1
+        joint_names:
+            - panda_1_joint1
+            - panda_1_joint2
+            - panda_1_joint3
+            - panda_1_joint4
+            - panda_1_joint5
+            - panda_1_joint6
+            - panda_1_joint7
+    left:
+        arm_id: panda_2
+        joint_names:
+            - panda_2_joint1
+            - panda_2_joint2
+            - panda_2_joint3
+            - panda_2_joint4
+            - panda_2_joint5
+            - panda_2_joint6
+            - panda_2_joint7
+
+teleop_joint_pd_example_controller:
+   type: franka_example_controllers/TeleopJointPDExampleController
+   leader:
+       arm_id: leader
+       joint_names:
+            - leader_joint1
+            - leader_joint2
+            - leader_joint3
+            - leader_joint4
+            - leader_joint5
+            - leader_joint6
+            - leader_joint7
+       # WARNING: Varying the following parameters may result in instability!
+       d_gains_lower: [1.0, 1.0, 1.0, 1.0, 0.33, 0.33, 0.17]
+       d_gains_upper: [35.0, 35.0, 35.0, 35.0, 10.0, 10.0, 8.0]
+       dq_max_lower: [1.3, 1.3, 1.3, 1.3, 1.4, 1.4, 1.4] # [rad/s]
+       dq_max_upper: [1.9, 1.9, 1.9, 1.9, 2.2, 2.2, 2.2] # [rad/s]
+       contact_force_threshold: 4.0 # [N]
+
+   follower:
+       arm_id: follower
+       joint_names:
+            - follower_joint1
+            - follower_joint2
+            - follower_joint3
+            - follower_joint4
+            - follower_joint5
+            - follower_joint6
+            - follower_joint7
+       # WARNING: Varying the following parameters may result in instability!
+       p_gains: [900.0, 900.0, 900.0, 900.0, 375.0, 225.0, 100.0]
+       d_gains: [45.0, 45.0, 45.0, 45.0, 15.0, 15.0, 10.0]
+       drift_comp_gains: [4.3, 4.3, 4.3, 4.3, 4.3, 4.3, 4.3]
+       dq_max_lower: [0.8, 0.8, 0.8, 0.8, 2.5, 2.5, 2.5] # [rad/s]
+       dq_max_upper: [2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5] # [rad/s]
+       ddq_max_lower: [0.8, 0.8, 0.8, 0.8, 10.0, 10.0, 10.0] # [rad/s^2]
+       ddq_max_upper: [6.0, 6.0, 6.0, 6.0, 15.0, 20.0, 20.0] # [rad/s^2]
+       contact_force_threshold: 5.0 # [N]
+
+spacenav_teleop_twist_controller:
+    type: franka_example_controllers/SpacenavTeleopTwistController
+    arm_id: $(arg arm_id)
+    linear_gain: $(arg linear_gain)
+    angular_gain: $(arg angular_gain)
+
+cartesian_pose_controller:
+    type: franka_example_controllers/CartesianPoseController
+    arm_id: $(arg arm_id)
+
+cartesian_traj_controller:
+    type: franka_example_controllers/CartesianTrajController
+    arm_id: $(arg arm_id)
+
+cartesian_traj_twist_correct_controller:
+    type: franka_example_controllers/CartesianTrajTwistCorrectController
+    arm_id: $(arg arm_id)
-- 
GitLab


From e75c8037c1d2b06bbe2bd6e47302921625abf292 Mon Sep 17 00:00:00 2001
From: jeo65286 <jeo65286@ipk-extern.fraunhofer.de>
Date: Mon, 20 Jan 2025 14:45:52 +0100
Subject: [PATCH 6/9] Add launch file to connect with spacenav

---
 ...tesian_impedance_example_controller.launch | 20 +++++++++++++++++++
 1 file changed, 20 insertions(+)
 create mode 100644 fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch

diff --git a/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch b/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch
new file mode 100644
index 0000000..682f04c
--- /dev/null
+++ b/fr3_impedance_controller_real/launch/cartesian_impedance_example_controller.launch
@@ -0,0 +1,20 @@
+<?xml version="1.0" ?>
+<launch>
+  <arg name="robot" default="panda" doc="choose your robot. Possible values: [panda, fr3]"/>
+  <arg name="arm_id" default="$(arg robot)" />
+
+  <arg name="linear_gain" default="1.0" />
+  <arg name="angular_gain" default="1.0" />
+
+  <include file="$(find franka_control)/launch/franka_control.launch" pass_all_args="true"/>
+  <include file="$(find spacenav_node)/launch/classic.launch" pass_all_args="true"/>
+  
+  <rosparam command="load" file="$(find franka_example_controllers)/config/franka_example_controllers.yaml" subst_value="true" />
+  <node name="controller_spawner" pkg="controller_manager" type="spawner" respawn="false" output="screen" args="cartesian_impedance_example_controller"/>
+  <node pkg="rviz" type="rviz" output="screen" name="rviz" args="-d $(find franka_example_controllers)/launch/rviz/franka_description_with_marker.rviz"/>
+  <node name="interactive_marker" pkg="franka_example_controllers" type="interactive_marker.py" required="true" output="screen">
+    <param name="link_name" value="$(arg arm_id)_link0" />
+    <remap from="equilibrium_pose" to="/cartesian_impedance_example_controller/equilibrium_pose" />
+  </node>
+  <node name="rqt_reconfigure" pkg="rqt_reconfigure" type="rqt_reconfigure" required="false" />
+</launch>
-- 
GitLab


From 3013711a14ccd61657506e021714751eebc3dc23 Mon Sep 17 00:00:00 2001
From: jeo65286 <jeo65286@ipk-extern.fraunhofer.de>
Date: Mon, 20 Jan 2025 14:46:45 +0100
Subject: [PATCH 7/9] Add rviz launch file

---
 .../rviz/franka_description_with_marker.rviz  | 279 ++++++++++++++++++
 1 file changed, 279 insertions(+)
 create mode 100644 fr3_impedance_controller_real/launch/rviz/franka_description_with_marker.rviz

diff --git a/fr3_impedance_controller_real/launch/rviz/franka_description_with_marker.rviz b/fr3_impedance_controller_real/launch/rviz/franka_description_with_marker.rviz
new file mode 100644
index 0000000..931fd1c
--- /dev/null
+++ b/fr3_impedance_controller_real/launch/rviz/franka_description_with_marker.rviz
@@ -0,0 +1,279 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Marker1
+        - /Marker1/Status1
+        - /Pose1
+      Splitter Ratio: 0.5
+    Tree Height: 862
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        panda_hand:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_hand_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_hand_tcp:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_leftfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link0_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link1_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link2_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link3_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link4_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link5_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link6_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link7_sc:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        panda_link8:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        panda_rightfinger:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Class: rviz/InteractiveMarkers
+      Enable Transparency: true
+      Enabled: false
+      Name: InteractiveMarkers
+      Show Axes: false
+      Show Descriptions: true
+      Show Visual Aids: false
+      Update Topic: /equilibrium_pose_marker/update
+      Value: false
+    - Class: rviz/Marker
+      Enabled: true
+      Marker Topic: /mpc_trajectory
+      Name: Marker
+      Namespaces:
+        "": true
+      Queue Size: 100
+      Value: true
+    - Alpha: 1
+      Axes Length: 0.10000000149011612
+      Axes Radius: 0.05000000074505806
+      Class: rviz/Pose
+      Color: 255; 25; 0
+      Enabled: true
+      Head Length: 0.30000001192092896
+      Head Radius: 0.10000000149011612
+      Name: Pose
+      Queue Size: 10
+      Shaft Length: 1
+      Shaft Radius: 0.05000000074505806
+      Shape: Axes
+      Topic: /cartesian_impedance_example_controller/equilibrium_pose
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: panda_link0
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 1.8979061841964722
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0
+        Y: 0
+        Z: 0
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.17539797723293304
+      Target Frame: <Fixed Frame>
+      Yaw: 0.0903986394405365
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1163
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000016a000003e9fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003e9000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000003e9fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003d000003e9000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000042fc0100000002fb0000000800540069006d0065010000000000000780000003bc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004fb000003e900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1920
+  X: 2560
+  Y: 0
-- 
GitLab


From 0f55641acdf0937444e99ecca6b00203f65eb5cc Mon Sep 17 00:00:00 2001
From: jeo65286 <jeo65286@ipk-extern.fraunhofer.de>
Date: Mon, 20 Jan 2025 14:48:51 +0100
Subject: [PATCH 8/9] Add CMakeLists, xml file for package and plugin

---
 fr3_impedance_controller_real/CMakeLists.txt  | 206 ++++++++++++++----
 .../fr3_impedance_controller_real_plugin.xml  |  84 +++++++
 fr3_impedance_controller_real/package.xml     |  48 +++-
 3 files changed, 287 insertions(+), 51 deletions(-)

diff --git a/fr3_impedance_controller_real/CMakeLists.txt b/fr3_impedance_controller_real/CMakeLists.txt
index 840e340..2ad3b57 100644
--- a/fr3_impedance_controller_real/CMakeLists.txt
+++ b/fr3_impedance_controller_real/CMakeLists.txt
@@ -1,54 +1,182 @@
-################################################################################
-## Set minimum required version of cmake, project name and compile options
-################################################################################
-cmake_minimum_required(VERSION 3.10)
-project(dob_impedance_controller)
+cmake_minimum_required(VERSION 3.4)
+project(franka_example_controllers)
 
-# Set C++ standard
+set(CMAKE_BUILD_TYPE Release)
+# set(CMAKE_CXX_STANDARD 14)
 set(CMAKE_CXX_STANDARD 17)
-set(CMAKE_CXX_STANDARD_REQUIRED True)
+set(CMAKE_CXX_STANDARD_REQUIRED ON)
 
-################################################################################
-## Find required CMake packages : pinocchio, casadi, catkin, ROS-related ...
-################################################################################
-find_package(pinocchio REQUIRED)
-find_package(casadi REQUIRED)
 find_package(catkin REQUIRED COMPONENTS
+  controller_interface
+  dynamic_reconfigure
+  eigen_conversions
+  franka_hw
+  franka_gripper
+  geometry_msgs
+  hardware_interface
+  joint_limits_interface
+  tf
+  tf_conversions
+  message_generation
+  pluginlib
+  realtime_tools
   roscpp
-  rosbag
-  rostime
+  rospy
   urdf
-  franka_msgs
-  geometry_msgs
+  visualization_msgs
 )
 
-################################################################################
-## Find source and header files to create executable binary file
-################################################################################
-file(GLOB_RECURSE SOURCES
-  ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp
-  ${CMAKE_CURRENT_SOURCE_DIR}/common/controller/src/*.cpp
-  ${CMAKE_CURRENT_SOURCE_DIR}/common/utilities/src/*.cpp
-  ${CMAKE_CURRENT_SOURCE_DIR}/common/robot/src/*.cpp
-  ${CMAKE_CURRENT_SOURCE_DIR}/common/math/src/*.cpp
+find_package(Eigen3 REQUIRED)
+find_package(Franka 0.9.0 QUIET)
+if(NOT Franka_FOUND)
+  find_package(Franka 0.8.0 REQUIRED)
+endif()
+
+add_message_files(FILES
+  JointTorqueComparison.msg
 )
 
-include_directories(
-  ${CMAKE_CURRENT_SOURCE_DIR}/include/dob_impedance_controller # ! check this out
-  ${CMAKE_CURRENT_SOURCE_DIR}/common/controller/include
-  ${CMAKE_CURRENT_SOURCE_DIR}/common/utilities/include
-  ${CMAKE_CURRENT_SOURCE_DIR}/common/robot/include
-  ${CMAKE_CURRENT_SOURCE_DIR}/common/math/include
-  ${catkin_INCLUDE_DIRS}
+generate_messages()
+
+generate_dynamic_reconfigure_options(
+  cfg/compliance_param.cfg
+  cfg/desired_mass_param.cfg
+  cfg/dual_arm_compliance_param.cfg
+  cfg/teleop_param.cfg
+  cfg/teleop_gripper_param.cfg
+)
+
+catkin_package(
+  INCLUDE_DIRS include
+  LIBRARIES franka_example_controllers
+  CATKIN_DEPENDS
+    controller_interface
+    dynamic_reconfigure
+    eigen_conversions
+    franka_hw
+    franka_gripper
+    geometry_msgs
+    hardware_interface
+    joint_limits_interface
+    tf
+    tf_conversions
+    message_runtime
+    pluginlib
+    realtime_tools
+    roscpp
+    urdf
+    visualization_msgs
+  DEPENDS Franka
+)
+
+add_library(franka_example_controllers
+  src/elbow_example_controller.cpp
+  src/cartesian_sinpose_controller.cpp
+  src/cartesian_pose_example_controller.cpp
+  src/cartesian_pose_controller.cpp
+  src/cartesian_traj_controller.cpp
+  src/cartesian_traj_twist_correct_controller.cpp
+  src/cartesian_velocity_example_controller.cpp
+  src/joint_position_example_controller.cpp
+  src/joint_velocity_example_controller.cpp
+  src/model_example_controller.cpp
+  src/joint_impedance_example_controller.cpp
+  src/cartesian_impedance_example_controller.cpp
+  src/force_example_controller.cpp
+  src/dual_arm_cartesian_impedance_example_controller.cpp
+  src/teleop_joint_pd_example_controller.cpp
+  src/joint_wall.cpp
+  src/spacenav_teleop_twist_controller.cpp
+)
+
+add_dependencies(franka_example_controllers
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  ${PROJECT_NAME}_generate_messages_cpp
+  ${PROJECT_NAME}_gencpp
+  ${PROJECT_NAME}_gencfg
+)
+
+target_link_libraries(franka_example_controllers PUBLIC
+  ${Franka_LIBRARIES}
+  ${catkin_LIBRARIES}
 )
 
-################################################################################
-## Define executable, Add source files & Link libraries
-################################################################################
-add_executable(${PROJECT_NAME}_node ${SOURCES})
+target_include_directories(franka_example_controllers SYSTEM PUBLIC
+  ${Franka_INCLUDE_DIRS}
+  ${EIGEN3_INCLUDE_DIRS}
+  ${catkin_INCLUDE_DIRS}
+)
+target_include_directories(franka_example_controllers PUBLIC
+  include
+  ${CMAKE_CURRENT_SOURCE_DIR}/../common/utilities/include
+  ${CMAKE_CURRENT_SOURCE_DIR}/../common/math/include
+)
 
-target_link_libraries(${PROJECT_NAME}_node
+add_executable(teleop_gripper_node
+  src/teleop_gripper_node.cpp
+)
+target_include_directories(teleop_gripper_node PUBLIC
+  ${catkin_INCLUDE_DIRS}
+)
+target_link_libraries(teleop_gripper_node PUBLIC
+  ${Franka_LIBRARIES}
   ${catkin_LIBRARIES}
-  pinocchio::pinocchio
-  casadi
 )
+add_dependencies(teleop_gripper_node
+  ${${PROJECT_NAME}_EXPORTED_TARGETS}
+  ${catkin_EXPORTED_TARGETS}
+  ${PROJECT_NAME}_generate_messages_cpp
+  ${PROJECT_NAME}_gencpp
+  ${PROJECT_NAME}_gencfg
+)
+
+## Installation
+install(TARGETS franka_example_controllers
+  ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+  RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+install(DIRECTORY include/${PROJECT_NAME}/
+  DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+)
+install(DIRECTORY launch
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+install(DIRECTORY config
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+install(FILES franka_example_controllers_plugin.xml
+  DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+)
+catkin_install_python(
+  PROGRAMS scripts/interactive_marker.py scripts/move_to_start.py scripts/dual_arm_interactive_marker.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## Tools
+include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
+  RESULT_VARIABLE CLANG_TOOLS
+)
+if(CLANG_TOOLS)
+  file(GLOB_RECURSE SOURCES ${CMAKE_CURRENT_SOURCE_DIR}/src/*.cpp)
+  file(GLOB_RECURSE HEADERS
+    ${CMAKE_CURRENT_SOURCE_DIR}/include/*.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/src/*.h
+    ${CMAKE_CURRENT_SOURCE_DIR}/../common/utilities/include/*.hpp
+    ${CMAKE_CURRENT_SOURCE_DIR}/../common/math/include/*.hpp
+  )
+  add_format_target(franka_example_controllers FILES ${SOURCES} ${HEADERS})
+  add_tidy_target(franka_example_controllers
+    FILES ${SOURCES}
+    DEPENDS franka_example_controllers
+  )
+endif()
+
+include(${CMAKE_CURRENT_LIST_DIR}/../cmake/PepTools.cmake OPTIONAL
+  RESULT_VARIABLE PEP_TOOLS
+)
+if(PEP_TOOLS)
+  file(GLOB_RECURSE PYSOURCES ${CMAKE_CURRENT_SOURCE_DIR}/scripts/*.py)
+  add_pyformat_target(franka_control FILES ${PYSOURCES})
+endif()
diff --git a/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml b/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml
index e69de29..1bec1f5 100644
--- a/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml
+++ b/fr3_impedance_controller_real/fr3_impedance_controller_real_plugin.xml
@@ -0,0 +1,84 @@
+<library path="lib/libfranka_example_controllers">
+  <class name="franka_example_controllers/JointVelocityExampleController" type="franka_example_controllers::JointVelocityExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that executes a short motion based on joint velocities to demonstrate correct usage
+    </description>
+  </class>
+  <class name="franka_example_controllers/JointPositionExampleController" type="franka_example_controllers::JointPositionExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that executes a short motion based on joint positions to demonstrate correct usage
+    </description>
+  </class>
+  <class name="franka_example_controllers/CartesianPoseExampleController" type="franka_example_controllers::CartesianPoseExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that executes a short motion based on cartesian poses to demonstrate correct usage
+    </description>
+  </class>
+  <!-- ADD here -->
+  <class name="franka_example_controllers/CartesianSinPoseExampleController" type="franka_example_controllers::CartesianSinPoseExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that executes a short motion based on cartesian poses to demonstrate correct usage
+    </description>
+  </class>
+  <!--  -->
+  <class name="franka_example_controllers/CartesianVelocityExampleController" type="franka_example_controllers::CartesianVelocityExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that executes a short motion based on cartesian velocities to demonstrate correct usage
+    </description>
+  </class>
+  <class name="franka_example_controllers/ElbowExampleController" type="franka_example_controllers::ElbowExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that executes a short motion based on cartesian poses and elbow to demonstrate correct usage
+    </description>
+  </class>
+  <class name="franka_example_controllers/ModelExampleController" type="franka_example_controllers::ModelExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that evaluates and prints the dynamic model of Franka
+    </description>
+  </class>
+  <class name="franka_example_controllers/JointImpedanceExampleController" type="franka_example_controllers::JointImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that tracks a cartesian path with a joint impedance controller that compensates coriolis torques. The torque commands are compared to measured torques in Console outputs.
+    </description>
+  </class>
+  <class name="franka_example_controllers/CartesianImpedanceExampleController" type="franka_example_controllers::CartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A controller that renders a spring damper system in cartesian space. Compliance parameters and the equilibrium pose can be modified online with dynamic reconfigure and an interactive marker respectively.
+    </description>
+  </class>
+  <class name="franka_example_controllers/ForceExampleController" type="franka_example_controllers::ForceExampleController" base_class_type="controller_interface::ControllerBase">
+    <description>
+      A PI controller that applies a force corresponding to a user-provided desired mass in the z axis. The desired mass value can be modified online with dynamic reconfigure.
+    </description>
+  </class>
+  <class name="franka_example_controllers/DualArmCartesianImpedanceExampleController" type="franka_example_controllers::DualArmCartesianImpedanceExampleController" base_class_type="controller_interface::ControllerBase">
+      <description>
+          A controller that renders Cartesian impedance based tracking of separate target poses for 2 panda arms.
+      </description>
+  </class>
+  <class name="franka_example_controllers/TeleopJointPDExampleController" type="franka_example_controllers::TeleopJointPDExampleController" base_class_type="controller_interface::ControllerBase">
+      <description>
+          A PD controller that tracks position and velocity of the leader arm and applies it to the follower arm. It also applies force-feedback to the leader arm.
+      </description>
+  </class>
+  <class name="franka_example_controllers/SpacenavTeleopTwistController" type="franka_example_controllers::SpacenavTeleopTwistController" base_class_type="controller_interface::ControllerBase">
+      <description>
+          A Controller that takes Twist Input from the 6DoF spacenav mouse and sends these to the robot.
+      </description>
+  </class>
+  <class name="franka_example_controllers/CartesianPoseController" type="franka_example_controllers::CartesianPoseController" base_class_type="controller_interface::ControllerBase">
+      <description>
+          A Controller that takes single cartesian pose (currently only position, no orientation).
+      </description>
+  </class>
+  <class name="franka_example_controllers/CartesianTrajController" type="franka_example_controllers::CartesianTrajController" base_class_type="controller_interface::ControllerBase">
+      <description>
+          A Controller that takes PoseArray (pos traj).
+      </description>
+  </class>
+  <class name="franka_example_controllers/CartesianTrajTwistCorrectController" type="franka_example_controllers::CartesianTrajTwistCorrectController" base_class_type="controller_interface::ControllerBase">
+      <description>
+          A Controller that takes PoseArray (pos traj) and can correct the unsmoothness of the traj with the spacenav twist commands.
+      </description>
+  </class>
+</library>
diff --git a/fr3_impedance_controller_real/package.xml b/fr3_impedance_controller_real/package.xml
index d2f0784..8530cf3 100644
--- a/fr3_impedance_controller_real/package.xml
+++ b/fr3_impedance_controller_real/package.xml
@@ -1,22 +1,46 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>dob_impedance_controller</name>
-  <version>0.0.0</version>
-  <description>
-    This is a package for integrated DOB + Impedance controller.
-  </description>
-  <maintainer email="jeo65286@ipk-extern.fraunhofer.de">jeo65286</maintainer>
-  <author>Jeongwoo Hong</author>
-  <license>TODO</license>
+  <name>franka_example_controllers</name>
+  <version>0.10.1</version>
+  <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros_control</description>
+  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
+  <license>Apache 2.0</license>
+
+  <url type="website">http://wiki.ros.org/franka_example_controllers</url>
+  <url type="repository">https://github.com/frankaemika/franka_ros</url>
+  <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
+  <author>Franka Emika GmbH</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 
+  <build_depend>message_generation</build_depend>
   <build_depend>eigen</build_depend>
 
+  <build_export_depend>message_runtime</build_export_depend>
+
+  <depend>controller_interface</depend>
+  <depend>dynamic_reconfigure</depend>
+  <depend>eigen_conversions</depend>
+  <depend>franka_hw</depend>
+  <depend>franka_gripper</depend>
+  <depend>geometry_msgs</depend>
+  <depend>hardware_interface</depend>
+  <depend>joint_limits_interface</depend>
+  <depend>tf</depend>
+  <depend>tf_conversions</depend>
+  <depend>libfranka</depend>
+  <depend>pluginlib</depend>
+  <depend>realtime_tools</depend>
   <depend>roscpp</depend>
-  <depend>rosbag</depend>
-  <depend>rostime</depend>
   <depend>urdf</depend>
-  <depend>franka_msgs</depend>
-  <depend>geometry_msgs</depend>
+  <depend>visualization_msgs</depend>
+
+  <exec_depend>franka_control</exec_depend>
+  <exec_depend>franka_description</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+  <export>
+    <controller_interface plugin="${prefix}/franka_example_controllers_plugin.xml" />
+  </export>
 </package>
-- 
GitLab


From 202ae324c7856e15742fa97f32571d24b96c0e45 Mon Sep 17 00:00:00 2001
From: Kevin Haninger <khaninger@gmail.com>
Date: Wed, 22 Jan 2025 21:33:54 +0100
Subject: [PATCH 9/9] add nix packaging, but hitting lots of build errors when
 building fr3_impedance_controller_real.cpp

---
 common/CMakeLists.txt                        | 85 ++++++++++++++++++++
 common/default.nix                           | 31 +++++++
 flake.nix                                    | 17 +++-
 fr3_impedance_controller_real/CMakeLists.txt | 70 ++++------------
 fr3_impedance_controller_real/default.nix    | 72 +++++++++++++++++
 fr3_impedance_controller_real/package.xml    | 16 ++--
 6 files changed, 225 insertions(+), 66 deletions(-)
 create mode 100644 common/CMakeLists.txt
 create mode 100644 common/default.nix
 create mode 100644 fr3_impedance_controller_real/default.nix

diff --git a/common/CMakeLists.txt b/common/CMakeLists.txt
new file mode 100644
index 0000000..9d4025d
--- /dev/null
+++ b/common/CMakeLists.txt
@@ -0,0 +1,85 @@
+# @Kevin 01.2025
+# This builds common as a shared lib so it can be built as Nix package and
+# linked to the other binaries.
+
+################################################################################
+## Set minimum required version of cmake, project name and compile options
+################################################################################
+cmake_minimum_required(VERSION 3.10)
+project(common)
+
+# Set C++ standard
+set(CMAKE_CXX_STANDARD 17)
+set(CMAKE_CXX_STANDARD_REQUIRED True)
+
+################################################################################
+## Find required CMake packages : pinocchio, casadi, catkin, ROS-related ...
+################################################################################
+find_package(pinocchio REQUIRED)
+find_package(casadi REQUIRED)
+find_package(Eigen3 REQUIRED NO_MODULE)
+
+################################################################################
+## Find source and header files to create executable binary file
+################################################################################
+file(GLOB_RECURSE SOURCES
+     controller/src/*.cpp
+     math/src/*.cpp
+     motion/src/*.cpp
+     robot/src/*.cpp
+     utilities/src/*.cpp
+)
+
+include_directories(
+     controller/include
+     math/include
+     motion/include
+     robot/include
+     utilities/include
+)
+
+################################################################################
+## Define executable, Add source files & Link libraries
+################################################################################
+add_library(common SHARED ${SOURCES})
+
+target_link_libraries(common
+  Eigen3::Eigen
+  pinocchio::pinocchio
+  casadi
+)
+
+target_include_directories(common
+        PUBLIC
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/controller/include>
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/math/include>
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/motion/include>
+        $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/robot/include> 
+       $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/utilities/include>
+        )
+
+install(TARGETS common
+        EXPORT commonTargets
+        LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR}
+        RUNTIME DESTINATION ${CMAKE_INSTALL_LIBDIR})
+
+install(EXPORT commonTargets
+        FILE commonConfig.cmake
+        NAMESPACE common::
+        DESTINATION lib/cmake/common)
+
+install(DIRECTORY controller/include/
+        DESTINATION include)
+
+install(DIRECTORY math/include/
+        DESTINATION include)
+
+install(DIRECTORY motion/include/
+        DESTINATION include)
+
+install(DIRECTORY robot/include/
+        DESTINATION include)
+
+install(DIRECTORY utilities/include/
+        DESTINATION include)
+
diff --git a/common/default.nix b/common/default.nix
new file mode 100644
index 0000000..3c6701c
--- /dev/null
+++ b/common/default.nix
@@ -0,0 +1,31 @@
+# This defines a function which returns a packge, intended to be evaluated by callPackage
+{ lib
+, stdenv
+, cmake
+, eigen
+, pinocchio
+, casadi
+}:
+
+stdenv.mkDerivation {
+  pname = "common";
+  version = "0.0.0"; 
+  src = ./.;
+  
+  # These are the build-time dependencies
+  nativeBuildInputs = [
+    cmake
+  ];
+  
+  # These dependencies are propagated, any package (or shell) which
+  # includes these will also include these deps
+  propagatedBuildInputs = [
+    eigen
+    casadi
+    pinocchio
+  ];
+
+  meta = {
+    description = "Common files for the observer and impedance controller";
+  };
+}
diff --git a/flake.nix b/flake.nix
index c8f8cc1..09bb168 100644
--- a/flake.nix
+++ b/flake.nix
@@ -4,7 +4,6 @@
   inputs = {
     nixpkgs_2411.url =  "github:nixos/nixpkgs/nixos-24.11";
     nixpkgs.follows = "noetic/nixpkgs";
-    #pincas.url = "git+https://gitlab.cc-asp.fraunhofer.de/ipk_aut/bestpractices/nix.git?dir=pincas";
     nix-ros-overlay.follows = "noetic/nix-ros-overlay";
     noetic.url = "git+https://gitlab.cc-asp.fraunhofer.de/ipk_aut/bestpractices/nix.git?dir=ros1/noetic";
   };
@@ -15,7 +14,6 @@
             
       overlays = [
         inputs.nix-ros-overlay.overlays.default
-        #inputs.pincas.overlays.default
       ];
       
       pkgs = import inputs.nixpkgs {
@@ -50,13 +48,26 @@
         franka-msgs
         urdf
       ];
+
+      
+      common = pkgs_2411.callPackage ./common { };
+      fr3-impedance-controller = rospkgs.callPackage ./fr3_impedance_controller_real {
+        inherit common;
+      }; 
+      
+      projectPackages = [
+        common
+        fr3-impedance-controller
+      ];
+      
+
     in
       {
         devShells.${system} = {        
           default = pkgs.mkShell {
             packages = [
               (rospkgs.buildEnv {
-                paths = rosPackages ++ [
+                paths = rosPackages ++ projectPackages ++ [
                   #pkgs.python3Packages.pincas
                   pkgs_2411.casadi
                   pkgs_2411.pinocchio
diff --git a/fr3_impedance_controller_real/CMakeLists.txt b/fr3_impedance_controller_real/CMakeLists.txt
index 2ad3b57..34e464a 100644
--- a/fr3_impedance_controller_real/CMakeLists.txt
+++ b/fr3_impedance_controller_real/CMakeLists.txt
@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.4)
-project(franka_example_controllers)
+project(fr3_impedance_controller_real)
 
 set(CMAKE_BUILD_TYPE Release)
 # set(CMAKE_CXX_STANDARD 14)
@@ -12,6 +12,7 @@ find_package(catkin REQUIRED COMPONENTS
   eigen_conversions
   franka_hw
   franka_gripper
+  franka_example_controllers
   geometry_msgs
   hardware_interface
   joint_limits_interface
@@ -24,6 +25,7 @@ find_package(catkin REQUIRED COMPONENTS
   rospy
   urdf
   visualization_msgs
+  sensor_msgs
 )
 
 find_package(Eigen3 REQUIRED)
@@ -32,18 +34,14 @@ if(NOT Franka_FOUND)
   find_package(Franka 0.8.0 REQUIRED)
 endif()
 
-add_message_files(FILES
-  JointTorqueComparison.msg
-)
+#add_message_files(FILES
+#  JointTorqueComparison.msg
+#)
 
 generate_messages()
 
 generate_dynamic_reconfigure_options(
   cfg/compliance_param.cfg
-  cfg/desired_mass_param.cfg
-  cfg/dual_arm_compliance_param.cfg
-  cfg/teleop_param.cfg
-  cfg/teleop_gripper_param.cfg
 )
 
 catkin_package(
@@ -55,6 +53,7 @@ catkin_package(
     eigen_conversions
     franka_hw
     franka_gripper
+    franka_example_controllers
     geometry_msgs
     hardware_interface
     joint_limits_interface
@@ -66,30 +65,15 @@ catkin_package(
     roscpp
     urdf
     visualization_msgs
+    sensor_msgs
   DEPENDS Franka
 )
 
-add_library(franka_example_controllers
-  src/elbow_example_controller.cpp
-  src/cartesian_sinpose_controller.cpp
-  src/cartesian_pose_example_controller.cpp
-  src/cartesian_pose_controller.cpp
-  src/cartesian_traj_controller.cpp
-  src/cartesian_traj_twist_correct_controller.cpp
-  src/cartesian_velocity_example_controller.cpp
-  src/joint_position_example_controller.cpp
-  src/joint_velocity_example_controller.cpp
-  src/model_example_controller.cpp
-  src/joint_impedance_example_controller.cpp
-  src/cartesian_impedance_example_controller.cpp
-  src/force_example_controller.cpp
-  src/dual_arm_cartesian_impedance_example_controller.cpp
-  src/teleop_joint_pd_example_controller.cpp
-  src/joint_wall.cpp
-  src/spacenav_teleop_twist_controller.cpp
+add_library(${PROJECT_NAME}
+  src/fr3_impedance_controller_real.cpp
 )
 
-add_dependencies(franka_example_controllers
+add_dependencies(${PROJECT_NAME}
   ${${PROJECT_NAME}_EXPORTED_TARGETS}
   ${catkin_EXPORTED_TARGETS}
   ${PROJECT_NAME}_generate_messages_cpp
@@ -97,42 +81,24 @@ add_dependencies(franka_example_controllers
   ${PROJECT_NAME}_gencfg
 )
 
-target_link_libraries(franka_example_controllers PUBLIC
+target_link_libraries(${PROJECT_NAME} PUBLIC
   ${Franka_LIBRARIES}
   ${catkin_LIBRARIES}
 )
 
-target_include_directories(franka_example_controllers SYSTEM PUBLIC
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
   ${Franka_INCLUDE_DIRS}
   ${EIGEN3_INCLUDE_DIRS}
   ${catkin_INCLUDE_DIRS}
 )
-target_include_directories(franka_example_controllers PUBLIC
+target_include_directories(${PROJECT_NAME} PUBLIC
   include
   ${CMAKE_CURRENT_SOURCE_DIR}/../common/utilities/include
   ${CMAKE_CURRENT_SOURCE_DIR}/../common/math/include
 )
 
-add_executable(teleop_gripper_node
-  src/teleop_gripper_node.cpp
-)
-target_include_directories(teleop_gripper_node PUBLIC
-  ${catkin_INCLUDE_DIRS}
-)
-target_link_libraries(teleop_gripper_node PUBLIC
-  ${Franka_LIBRARIES}
-  ${catkin_LIBRARIES}
-)
-add_dependencies(teleop_gripper_node
-  ${${PROJECT_NAME}_EXPORTED_TARGETS}
-  ${catkin_EXPORTED_TARGETS}
-  ${PROJECT_NAME}_generate_messages_cpp
-  ${PROJECT_NAME}_gencpp
-  ${PROJECT_NAME}_gencfg
-)
-
 ## Installation
-install(TARGETS franka_example_controllers
+install(TARGETS ${PROJECT_NAME}
   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
@@ -146,13 +112,9 @@ install(DIRECTORY launch
 install(DIRECTORY config
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
-install(FILES franka_example_controllers_plugin.xml
+install(FILES fr3_impedance_controller_real_plugin.xml
   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
 )
-catkin_install_python(
-  PROGRAMS scripts/interactive_marker.py scripts/move_to_start.py scripts/dual_arm_interactive_marker.py
-  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
-)
 
 ## Tools
 include(${CMAKE_CURRENT_LIST_DIR}/../cmake/ClangTools.cmake OPTIONAL
diff --git a/fr3_impedance_controller_real/default.nix b/fr3_impedance_controller_real/default.nix
new file mode 100644
index 0000000..8c7ab94
--- /dev/null
+++ b/fr3_impedance_controller_real/default.nix
@@ -0,0 +1,72 @@
+# This defines a function which returns a packge, intended to be evaluated by callPackage
+{ lib
+, buildRosPackage
+, catkin
+, controller-interface
+, dynamic-reconfigure
+, pluginlib
+, realtime-tools
+, eigen
+, eigen-conversions
+, franka-control
+, franka-description
+, franka-gripper
+, franka-hw
+, franka-example-controllers
+, libfranka
+, hardware-interface
+, joint-limits-interface
+, message-generation
+, message-runtime
+, geometry-msgs
+, visualization-msgs
+, roscpp
+, rospy
+, tf
+, tf-conversions
+, urdf
+
+, common # shared library from the project
+}:
+
+buildRosPackage {
+  pname = "fr3-impedance-controller";
+  version = "0.0.0"; 
+  src = ./.;
+  
+  buildType = "catkin";
+
+  buildInputs = [ catkin eigen message-generation ];
+  propagatedBuildInputs = [
+    controller-interface
+    dynamic-reconfigure
+    eigen-conversions
+    franka-control
+    franka-description
+    franka-gripper
+    franka-hw
+    libfranka
+    franka-example-controllers
+    geometry-msgs
+    hardware-interface
+    joint-limits-interface
+
+    message-runtime
+    pluginlib
+    realtime-tools
+    roscpp rospy
+    tf
+    tf-conversions
+    urdf
+    visualization-msgs
+    common
+  ];
+  
+  nativeBuildInputs = [ catkin ];
+
+
+  meta = {
+    description = "Impedance controller to run on the Franka Emika FR3";
+  };
+}
+
diff --git a/fr3_impedance_controller_real/package.xml b/fr3_impedance_controller_real/package.xml
index 8530cf3..297f200 100644
--- a/fr3_impedance_controller_real/package.xml
+++ b/fr3_impedance_controller_real/package.xml
@@ -1,15 +1,11 @@
 <?xml version="1.0"?>
 <package format="2">
-  <name>franka_example_controllers</name>
-  <version>0.10.1</version>
-  <description>franka_example_controllers provides example code for controlling Franka Emika research robots with ros_control</description>
-  <maintainer email="support@franka.de">Franka Emika GmbH</maintainer>
+  <name>fr3_impedance_controller_real</name>
+  <version>0.0.1</version>
+  <description>Impedance controller for the Franka Emika FR3</description>
+  <maintainer email="todo@todo.com"></maintainer>
   <license>Apache 2.0</license>
 
-  <url type="website">http://wiki.ros.org/franka_example_controllers</url>
-  <url type="repository">https://github.com/frankaemika/franka_ros</url>
-  <url type="bugtracker">https://github.com/frankaemika/franka_ros/issues</url>
-  <author>Franka Emika GmbH</author>
 
   <buildtool_depend>catkin</buildtool_depend>
 
@@ -23,6 +19,7 @@
   <depend>eigen_conversions</depend>
   <depend>franka_hw</depend>
   <depend>franka_gripper</depend>
+  <depend>franka_example_controllers</depend>
   <depend>geometry_msgs</depend>
   <depend>hardware_interface</depend>
   <depend>joint_limits_interface</depend>
@@ -33,6 +30,7 @@
   <depend>realtime_tools</depend>
   <depend>roscpp</depend>
   <depend>urdf</depend>
+  <depend>sensor_msgs</depend>
   <depend>visualization_msgs</depend>
 
   <exec_depend>franka_control</exec_depend>
@@ -41,6 +39,6 @@
   <exec_depend>rospy</exec_depend>
 
   <export>
-    <controller_interface plugin="${prefix}/franka_example_controllers_plugin.xml" />
+    <controller_interface plugin="${prefix}/fr3_impedance_controller_real_plugin.xml" />
   </export>
 </package>
-- 
GitLab