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>
@@ -58,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_;
 }