Package fr3_impedance_controller_real in nix env

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

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

Closes #7 (closed)

common/src/controller/ImpedanceController.cpp

 #include "ImpedanceController.hpp"
 
+#include "orientation_utilities.hpp"
+
 #include <iostream>
 
+namespace ori = orient_utils;
+
 using namespace std;
 
 template <typename T>
-ImpedanceController<T>::ImpedanceController(const Mat6<T> & K_task, const Mat6<T> & B_task,
-                                            const Mat7<T> & K_null, const Mat7<T> & B_null)
-  : K_task_(K_task), B_task_(B_task), K_null_(K_null), B_null_(B_null)
+ImpedanceController<T>::ImpedanceController()
 {
   x_0e_err_.setZero();
   x_0e_err_prev_.setZero();
@@ -17,9 +19,25 @@ ImpedanceController<T>::ImpedanceController(const Mat6<T> & K_task, const Mat6<T
   q_null_err_.setZero();
   q_null_err_prev_.setZero();
   tau_null_.setZero();
+
+  K_task_.setIdentity();
+  D_task_.setIdentity();
+  K_null_.setIdentity();
+  D_null_.setIdentity();
+}
+
+//* ----- SETTERS ----------------------------------------------------------------------------------
+template <typename T>
+void ImpedanceController<T>::set_parameters(const Mat6<T> & K_task, const Mat6<T> & D_task,
+                                            const Mat7<T> & K_null, const Mat7<T> & D_null)
+{
+  K_task_ = K_task;
+  D_task_ = D_task;
+  K_null_ = K_null;
+  D_null_ = D_null;
 }
 
-//* ----- METHODS ------------------------------------------------------------------------------ *//
+//* ----- METHODS ----------------------------------------------------------------------------------
 template <typename T>
 void ImpedanceController<T>::compute_null_error(const Vec7<T> & q_null_des,
                                                 const Vec7<T> & q_null_mes)
@@ -27,7 +45,7 @@ void ImpedanceController<T>::compute_null_error(const Vec7<T> & q_null_des,
   q_null_err_ = q_null_des - q_null_mes;
 }
 
-//* ----- GETTERS ------------------------------------------------------------------------------ *//
+//* ----- GETTERS ----------------------------------------------------------------------------------
 template <typename T>
 Vec6<T> ImpedanceController<T>::get_pose_error(const Vec3<T> & p_0e_des, const Vec3<T> & p_0e_mes,
                                                const Quat<T> & q_0e_des, const Quat<T> & q_0e_mes)
@@ -44,8 +62,10 @@ Vec6<T> ImpedanceController<T>::get_pose_error(const Vec3<T> & p_0e_des, const V
     q_new.coeffs() << q_0e_mes.coeffs();
   }
   Quat<T> q_0e_err = q_new.inverse() * q_0e_des;
+  RotMat<T> R = ori::quaternionToRotationMatrix(q_new);
   x_0e_err_.tail(3) << q_0e_err.x(), q_0e_err.y(), q_0e_err.z();
-  x_0e_err_.tail(3) << q_new * x_0e_err_.tail(3);
+  // x_0e_err_.tail(3) << q_new * x_0e_err_.tail(3);
+  x_0e_err_.tail(3) << R * x_0e_err_.tail(3);
 
   return x_0e_err_;
 }
@@ -62,16 +82,16 @@ Vec6<T> ImpedanceController<T>::get_previous_pose_error()
 template <typename T>
 Vec6<T> ImpedanceController<T>::get_task_impedance_force(const Vec6<T> & dx_0e_err)
 {
-  return K_task_ * x_0e_err_ + B_task_ * dx_0e_err;
+  return K_task_ * x_0e_err_ + D_task_ * dx_0e_err;
 }
 
 template <typename T>
 Vec7<T> ImpedanceController<T>::get_null_impedance_torque(const Vec7<T> & dq_null_err,
-                                                          const Eigen::MatrixXd & J,
-                                                          const Eigen::MatrixXd & Jt_pinv)
+                                                          const MatX<T> & J_t,
+                                                          const MatX<T> & J_t_pinv)
 {
-  return (Eigen::MatrixXd::Identity(7, 7) - J.transpose() * Jt_pinv) *
-         (K_null_ * q_null_err_ - B_null_ * dq_null_err);
+  Mat7<T> I = Mat7<T>::Identity();
+  return (I - J_t * J_t_pinv) * (K_null_ * q_null_err_ - D_null_ * dq_null_err);
 }
 
 //* ----- PRINTER ------------------------------------------------------------------------------ *//