Package fr3_impedance_controller_real in nix env

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

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

Closes #7 (closed)

fr3_impedance_controller_real/src/cartesian_impedance_example_controller.cpp

@@ -187,7 +187,7 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
   Vec3<double> w_des = ee_vel_world_des_.tail(3);
   ee_pos_world_des_ += v_des * period.toSec();
   ee_quat_world_des_ = ori::getNewQuat(ee_quat_world_des_, w_des, period.toSec());
-  ee_rpy_world_des_ = ori::quaternionToRPY(ee_quat_world_des_, ee_rpy_world_des_prev_);
+  // ee_rpy_world_des_ = ori::quaternionToRPY(ee_quat_world_des_, ee_rpy_world_des_prev_);
   // std::cout << "Desired EE pos [m]:\t" << ee_pos_world_des_.transpose() << std::endl;
   // std::cout << "Desired EE RPY [rad]:\t" << ee_rpy_world_des_.transpose() << std::endl;
 
@@ -210,9 +210,8 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
 
   ee_pos_world_mes_ = data_.oMf[ee_id_].translation();
   RotMat<double> R_OE = data_.oMf[ee_id_].rotation();
-
   ee_quat_world_mes_ = Eigen::Quaterniond(R_OE);
-  ee_rpy_world_mes_ = ori::quaternionToRPY(orientation, ee_rpy_world_mes_prev_);
+  // ee_rpy_world_mes_ = ori::quaternionToRPY(orientation, ee_rpy_world_mes_prev_);
   // NOTE: results from FR3 state & pinocchio are almost the same.
   // cout << "Measured Pin EE pos [m]:\t" << ee_pos_world_mes_.transpose() << endl;
   // cout << "Measured FR3 EE pos [m]:\t" << position.transpose() << endl;
@@ -288,8 +287,8 @@ void CartesianImpedanceExampleController::update(const ros::Time & /*time*/,
   Vec6<double> ee_pose_err_prev = imp_->get_previous_pose_error();
   Vec6<double> dpose_err = robo::getEulerDerivative(ee_pose_err, ee_pose_err_prev, dt_);
   Vec6<double> F_task = imp_->get_task_impedance_force(dpose_err);
-  // cout << "Imp FR3:\t" << tau_task.transpose() << endl;
-  // cout << "Imp My:\t" << (Je_t * F_task).transpose() << endl;
+  cout << "Imp FR3:\t" << tau_task.transpose() << endl;
+  cout << "Imp Pin:\t" << (Je_t * F_task).transpose() << endl;
 
   /* Task-Force Disturbance compensation */
   // Vec7<double> tau_task = Je_t * (F_task - (b_dob_applied_ ? Fd_hat_dob_ :