Debug kinematics, dob and Refactor codes

include/dob_impedance_controller/dob_impedance_controller.hpp

 /** @file dob_impedance_controller.hpp
  *  @brief
- *  @note
- *  - For simplicity, reference frame is omitted in the variable names of end-effector, cause for a
- * fixed-base manipulator, it is common to represent end-effector's states w.r.t base (world) frame
  */
 
 #ifndef DOB_IMPEDANCE_CONTROLLER_HPP_
@@ -51,6 +48,7 @@ private:
   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_;
@@ -94,46 +92,52 @@ private:
   // Vec6<double> F_ext_hat_filtered_;  // LPF torques by estimated external forces on the EE
 
   //* MuJoCo sensor data *//
-  Vec7<double> mj_q_mes_;
-  Vec7<double> mj_dq_mes_;
-  Vec7<double> mj_ddq_mes_;
-  Vec3<double> mj_ee_pos_mes_;
-  Quat<double> mj_ee_quat_mes_;
-  Vec6<double> mj_ee_twist_mes_;
-  Vec6<double> mj_ee_accel_mes_;
-  Vec6<double> mj_ee_wrench_mes_;
+  bool b_is_mj_data_received_ = false;
+  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> dq_filtered_;
-  Vec7<double> ddq_euler_;  // time-derivative of joint velocity
-  Vec7<double> ddq_euler_prev_;
+  Vec7<double> ddq_eul_;      // time-derivative of joint velocity using backward Euler method
+  Vec7<double> ddq_eul_prev_;
 
   /* task states */
-  int frame_id_;  // end-effector frame ID
-
-  Vec3<double> ee_pos_des_;
-  Vec3<double> ee_pos_mes_;
-
-  Quat<double> ee_quat_des_;
-  Quat<double> ee_quat_mes_;
-
-  Vec6<double> ee_twist_des_;       // desired EE twist w.r.t "body" frame from joystick command
-  Vec6<double> ee_twist_cal_;       // calculated current EE twist in {W} using v = J * dq
-  Vec6<double> ee_twist_cal_prev_;  // calculated current EE twist in {W} using v = J * dq
-  Vec6<double> ee_accel_euler_;     // derivative of ee_twist_cal_ using Euler method
-  Vec6<double> ee_accel_euler_prev_;
+  int ee_frame_id_;
+  Vec3<double> ee_pos_world_des_;
+  Vec3<double> ee_pos_world_mes_;
+  Vec3<double> ee_rpy_world_des_;
+  Vec3<double> ee_rpy_world_mes_;
+  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_eul_;       // derivative of ee_twist_world_cal_ using Euler method
+  Vec6<double> ee_accel_world_eul_prev_;
 
   /* Impedance control */
-  const double k_amp_ = 2.25;  // PD gain ratio to real HW gain
-  Mat6<double> K_task_;        // task-space stiffness
-  Mat6<double> B_task_;        // task-space damping
-  Vec6<double> pose_error_;    // task-space EE pose error
-  Vec6<double> F_task_;        // computed wrench from impedance controller
-  Vec7<double> tau_task_;      // computed torque from impedance controller
+  const double k_amp_ = 2.25;   // 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> F_task_;         // computed wrench from impedance controller
+  Vec7<double> tau_task_;       // computed torque from impedance controller
 
   Mat7<double> K_null_;      // null-space stiffness
   Mat7<double> B_null_;      // null-space damping
@@ -147,15 +151,17 @@ private:
   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
+  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_hat_;        // estimated task-space EE acceleration
+  Vec6<double> ee_accel_world_hat_;  // estimated task-space EE acceleration
   Vec7<double> s1_, s2_;             // SOSML's parameter
   Vec7<double> t1_, t2_;
 
@@ -164,22 +170,23 @@ private:
   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 matrixs
+  Mat7<double> dMq_inv_;  // time-derivative of inverse inertia matrix
   Mat6<double> Me_;       // (full) operational ineria matrix
-  Mat7<double> Cq_;       // Coriolis & Centrifugal torque 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.urdf");
+  std::string urdf_filename = std::string(repo_path + "assets/model/fr3_hand.urdf");
 
-  pinocchio::Model pmodel_;
-  pinocchio::Data pdata_;
+  pinocchio::Model pin_model_;
+  pinocchio::Data pin_data_;
 
+  /* Data Logging */
   std::string log_filename = std::string(repo_path + "assets/data/data.csv");
-  std::unique_ptr<SaveData<double>> logger_;  // data logger
+  std::unique_ptr<SaveData<double>> logger_;
 
 public:
   DobImpedanceController(ros::NodeHandle & nh, const double & Hz);
@@ -201,7 +208,10 @@ public:
 
   //* ----- PRINTER ---------------------------------------------------------------------------- *//
   void print_jacobian();
-  void print_twist();
+  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();
 };

src/dob_impedance_controller_node.cpp

@@ -12,7 +12,7 @@ int main(int argc, char ** argv)
   ros::init(argc, argv, node_name);
   ros::NodeHandle nh;
 
-  double Hz = 250;
+  double Hz = 400;
   auto node = std::make_shared<DobImpedanceController>(nh, Hz);
   node->run_loop();