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/include/franka_example_controllers/cartesian_impedance_example_controller.h

@@ -15,9 +15,10 @@
 #include <ros/node_handle.h>
 #include <ros/time.h>
 
-#include <geometry_msgs/PoseStamped.h>  // for interactive marker
-#include <geometry_msgs/Twist.h>        // for spacenave teleop
-#include <sensor_msgs/Joy.h>            // for joystick command
+#include <geometry_msgs/PoseStamped.h>        // interactive marker
+#include <geometry_msgs/Twist.h>              // spacenave teleop
+#include <sensor_msgs/Joy.h>                  // joystick
+#include "fr3_custom_msgs/ControllerState.h"  // custom message
 
 #include <franka_example_controllers/compliance_paramConfig.h>
 #include <franka_hw/franka_model_interface.h>
@@ -85,6 +86,7 @@ private:
   //* ----- Publisher & Subscriber -----------------------------------------------------------------
   /* Controller state publisher */
   ros::Publisher pub_ctrl_state_;
+  fr3_custom_msgs::ControllerState msg_ctrl_state_;
 
   /* Target EE Twist & Gripper command from Spacenav (or PS3) Joystick */
   ros::Subscriber sub_target_twist_;
@@ -93,13 +95,11 @@ private:
   Vec6<double> ee_vel_world_target_;
 
   /* Target EE Pose command from Interactive Marker */
-  ros::Subscriber sub_equilibrium_pose_;
-  std::mutex position_and_orientation_d_target_mutex_;
-  Vec3<double> ee_pos_world_target_;
-  Quat<double> ee_quat_world_target_;
-  // bool b_is_marker_mode_ = false;
-  // ros::Subscriber sub_interactive_marker_;
+  // ros::Subscriber sub_ee_pose_;
   // std::mutex target_ee_pose_mutex_;
+  // Vec3<double> ee_pos_world_target_;
+  // Quat<double> ee_quat_world_target_;
+  // bool b_is_marker_mode_ = false;
 
   //* ----- Control Parameters ---------------------------------------------------------------------
   /* Dynamic Reconfigure Server */
@@ -150,36 +150,40 @@ private:
   /* Controllers */
   std::unique_ptr<ImpedanceController<double>> imp_;
   std::unique_ptr<DisturbanceObserver<double>> dob_;
+  Vec7<double> tau_d_hat_dob_;
   Vec6<double> Fd_hat_dob_;
 
   std::unique_ptr<SlidingModeObserver<double>> sosml_;
   Vec7<double> ddq_hat_sosml_;
+  Vec7<double> tau_d_hat_sosml_;
+  Vec6<double> ee_acc_world_hat_sosml_;
+  Vec6<double> Fd_hat_sosml_;
 
   std::unique_ptr<MomentumObserver<double>> mob_;
   Vec7<double> h_mes_, h_mes_prev_;  // General Momentum
   Vec7<double> ddq_hat_mob_;
   Vec7<double> tau_d_hat_mob_;
+  Vec6<double> Fd_hat_mob_;
 
 public:
   //* ----- SUBSCRIBE CALLBACK ---------------------------------------------------------------------
   void subscribe_target_twist(const geometry_msgs::TwistConstPtr & msg);
   void subscribe_joy_command(const sensor_msgs::JoyConstPtr & msg);
 
-  void equilibriumPoseCallback(const geometry_msgs::PoseStampedConstPtr & msg);
-
   //* ----- PARAMETER SERVER CALLBACK --------------------------------------------------------------
   void complianceParamCallback(franka_example_controllers::compliance_paramConfig & config,
                                uint32_t level);
 
   //* ----- METHODS --------------------------------------------------------------------------------
+  void init_states();
+  void update_previous_states();
+  void publish_controller_state();
+
   /* joint torque 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)
 
-  void init_states();
-  void update_previous_states();
-
   //* ----- PRINTER --------------------------------------------------------------------------------
   void print_end_effector_states();
   void print_dynamics();