Merge branch 'feature/message' into main

fr3_custom_msgs/CMakeLists.txt

@@ -9,13 +9,14 @@ find_package(catkin REQUIRED COMPONENTS
   std_srvs
 )
 
-## Generate messages in the 'msg' folder
+# --- Generate messages in the 'msg' folder
 add_message_files(
   FILES
+  MujocoCommand.msg
   MujocoSensor.msg
 )
 
-## Generate added messages and services with any dependencies listed here
+# --- Generate added messages and services with any dependencies listed here
 # !! generate_messages() must be called before catkin_package() !!
 generate_messages(
   DEPENDENCIES

fr3_custom_msgs/msg/MujocoCommand.msg

+std_msgs/Header header
+sensor_msgs/JointState joint_cmd
+geometry_msgs/Wrench ee_perturb
\ No newline at end of file

fr3_custom_msgs/msg/MujocoSensor.msg

 std_msgs/Header header
-sensor_msgs/JointState mj_joint_state
-geometry_msgs/Pose mj_ee_pose
-geometry_msgs/Twist mj_ee_twist
-geometry_msgs/Accel mj_ee_accel
-geometry_msgs/Wrench mj_ee_wrench
+float64 time
+std_msgs/Bool b_mj_node_run
+sensor_msgs/JointState joint_state
+geometry_msgs/Pose ee_pose
+geometry_msgs/Twist ee_twist
\ No newline at end of file
+geometry_msgs/Accel ee_accel
+geometry_msgs/Wrench ee_wrench
\ No newline at end of file

fr3_impedance_controller_sim/include/fr3_impedance_controller_sim/fr3_impedance_controller_sim.hpp

@@ -9,8 +9,10 @@
 #include "ros/ros.h"
 #include "ros/time.h"
 
+#include "fr3_custom_msgs/MujocoCommand.h"
 #include "fr3_custom_msgs/MujocoSensor.h"
 #include "franka_msgs/FrankaState.h"
+#include "std_srvs/SetBool.h"
 
 /* C++ STL */
 #include <memory>
@@ -36,12 +38,17 @@ private:
   double dt_ = 0.0;            // loop timestep
   int tick_ = 0;               // loop iteration
 
+  double loop_time_ = 0.0;
+  double max_exec_time_ = 0.0;
+  double total_exec_time_ = 0.0;
+
   ros::NodeHandle nh_;
   ros::Rate loop_rate_;
   ros::Time start_time_;
 
   //* ----- Booleans to control ---------------------------------------------------------------- *//
   bool b_subscribe_real_data_ = false;  // set "true" to validate w/ real HW data
+  bool b_apply_disturb = true;
   bool b_is_joy_mode_ = true;
 
   bool b_is_impedance_applied_ = true;
@@ -52,11 +59,14 @@ private:
   bool b_save_data_ = true;
   bool b_count_loop_time_ = false;
 
-  //* ----- Publisher -------------------------------------------------------------------------- *//
-  ros::Publisher pub_joint_cmd_;
-  ros::Publisher pub_ee_perturb_;
+  //* ----- Service Server -------------------------------------------------------------------------
+  ros::ServiceServer server_controller_status_;
+  bool b_is_controller_running_ = false;
+
+  //* ----- Publisher & Subscriber -----------------------------------------------------------------
+  ros::Publisher pub_mujoco_cmd_;
+  fr3_custom_msgs::MujocoCommand msg_mujoco_cmd_;
 
-  //* ----- Subscriber ------------------------------------------------------------------------- *//
   /* Franka states */
   ros::Subscriber sub_fr_state_;
   bool b_is_fr_state_received_ = false;
@@ -84,8 +94,9 @@ private:
   /* MuJoCo states */
   ros::Subscriber sub_mj_sensor_;
   fr3_custom_msgs::MujocoSensor msg_mj_sensor_;
-  bool b_is_mj_state_received_ = false;
 
+  double mj_time_;
+  bool b_mj_node_run_;
   Vec7<double> mj_q_;
   Vec7<double> mj_dq_;
   Vec7<double> mj_ddq_;
@@ -107,6 +118,7 @@ private:
   Vec7<double> dq_mes_, dq_mes_prev_;
   Vec7<double> ddq_eul_, ddq_eul_prev_;  // joint accel obtained by backward Euler method
   Vec7<double> h_mes_, h_mes_prev_;
+  Vec7<double> tau_des_;
 
   Vec3<double> ee_pos_world_des_;   // desired EE Cartesian position w.r.t {W}
   Vec3<double> ee_pos_world_mes_;   // measured EE Cartesian position w.r.t {W}
@@ -133,6 +145,7 @@ private:
   Vec7<double> s1_, s2_, t1_, t2_;
 
   std::unique_ptr<MomentumObserver<double>> mob_;
+  Vec7<double> tau_dist_hat_mob_;
   Vec7<double> r_th_, km_;
   Vec7<double> ddq_hat_mob_filtered_, ddq_hat_mob_filtered_prev_;
 
@@ -141,18 +154,19 @@ private:
   std::unique_ptr<SymbolicAutoDiff> autodiff_;
   pinocchio::Model model_;
   pinocchio::Data data_;
-  int ee_frame_id_;  // "fr3_link8" (flange) / "fr3_hand_tcp" (end-effector)
+  int ee_id_;  // end-effector frame ID : "fr3_link8" (flange) / "fr3_hand_tcp" (end-effector)
 
   /* Data logger */
   std::string log_filename = std::string("../../assets/data/data.csv");
   std::unique_ptr<SaveData<double>> logger_;
-  bool b_is_first_data_received_ = false;
-  double first_data_time_ = 0.0;
-  double elapsed_time_since_first_data_ = 0.0;
 
 public:
   FR3ImpedanceControllerSim(ros::NodeHandle & nh, const double & Hz);
 
+  //* ----- SERVER CALLBACK ------------------------------------------------------------------------
+  bool handle_controller_status(std_srvs::SetBool::Request & req,
+                                std_srvs::SetBool::Response & res);
+
   //* ----- SUBSCRIBE CALLBACK ----------------------------------------------------------------- *//
   void subscribe_target_twist(const geometry_msgs::Twist & msg);
   void subscribe_mujoco_sensor(const fr3_custom_msgs::MujocoSensor & msg);
@@ -163,7 +177,10 @@ public:
   void update();
   void update_previous_states();
 
+  Vec6<double> apply_disturb();
+
   //* ----- PRINTER ---------------------------------------------------------------------------- *//
+  void print_execution_statistics();
   void print_ee_position();
   void print_ee_quaternion();
   void print_ee_twist();

fr3_ros1_mujoco_interface/CMakeLists.txt

@@ -22,6 +22,7 @@ find_package(catkin REQUIRED COMPONENTS
   rostime
   geometry_msgs
   sensor_msgs
+  std_srvs
 )
 
 # --- Find source files and Include header files to create executable file of node

fr3_ros1_mujoco_interface/include/fr3_ros1_mujoco_interface/fr3_ros1_mujoco_interface.hpp

@@ -5,7 +5,9 @@
 #include "ros/ros.h"
 #include "ros/time.h"
 
+#include "fr3_custom_msgs/MujocoCommand.h"
 #include "fr3_custom_msgs/MujocoSensor.h"
+#include "std_srvs/SetBool.h"
 
 /* MuJoCo Headers */
 #include "array_safety.h"
@@ -36,8 +38,9 @@ public:
     std::vector<double> pos_;
     std::vector<double> vel_;
     std::vector<double> tau_;
+    std::vector<double> ee_perturb_;
   };
-  std::shared_ptr<MuJoCoControlCommand> joint_cmd_ptr_;
+  std::shared_ptr<MuJoCoControlCommand> mujoco_cmd_ptr_;
 
   struct MuJoCoGuiCommand
   {
@@ -54,28 +57,25 @@ private:
   ros::NodeHandle nh_;
   ros::Timer timer_;
 
-  /* Control command subscriber */
-  ros::Subscriber joint_cmd_sub_;
-  ros::Subscriber ee_perturb_sub_;
+  //* ----- Service Client -------------------------------------------------------------------------
+  ros::Timer timer_check_controller_status_;
+  ros::ServiceClient client_controller_status_;
+
+  //* ----- Publisher / Subscriber -----------------------------------------------------------------
+  ros::Subscriber sub_mujoco_cmd_;  // control command subscriber
 
-  /* Simulation data publisher */
-  ros::Publisher pub_mj_sensor_;
+  ros::Publisher pub_mj_sensor_;  // simulation sensor data publisher
   fr3_custom_msgs::MujocoSensor msg_mj_sensor_;
 
 public:
-  void subscribe_joint_command(const sensor_msgs::JointState & msg);
-  void subscribe_end_effector_perturb(const geometry_msgs::Wrench & msg);
+  void subscribe_mujoco_command(const fr3_custom_msgs::MujocoCommand & msg);
   void publish_mujoco_sensor();
+  void check_controller_node_status(const ros::TimerEvent &);
 
   //* **************************************** GETTERS ***************************************** *//
-  std::shared_ptr<MuJoCoControlCommand> get_joint_cmd_ptr()
-  {
-    return joint_cmd_ptr_;
-  }
-
-  std::shared_ptr<MuJoCoGuiCommand> get_gui_cmd_ptr()
+  std::shared_ptr<MuJoCoControlCommand> get_mujoco_cmd_ptr()
   {
-    return gui_cmd_ptr_;
+    return mujoco_cmd_ptr_;
   }
 
   ros::NodeHandle & get_node_handle()

fr3_ros1_mujoco_interface/package.xml

@@ -16,6 +16,7 @@
   <depend>roscpp</depend>
   <depend>rosbag</depend>
   <depend>rostime</depend>
+  <depend>std_srvs</depend>
   <depend>sensor_msgs</depend>
   <depend>geometry_msgs</depend>
 </package>

fr3_ros1_mujoco_interface/src/fr3_ros1_mujoco_interface/fr3_ros1_mujoco_interface.cpp

@@ -8,58 +8,86 @@ using std::placeholders::_1;
 FR3MuJoCoRosOneInterface::FR3MuJoCoRosOneInterface(mj::Simulate * sim, ros::NodeHandle & nh)
   : sim_(sim), nh_(nh)
 {
-  /* Initialize subscriber */
-  joint_cmd_sub_ = nh_.subscribe("mj_joint_cmd", queue_size_,
-                                 &FR3MuJoCoRosOneInterface::subscribe_joint_command, this);
-  joint_cmd_ptr_ = std::make_shared<MuJoCoControlCommand>();
+  /* Initialize client */
+  client_controller_status_ = nh_.serviceClient<std_srvs::SetBool>("fr3_controller_status");
+  timer_check_controller_status_ = nh_.createTimer(
+    ros::Duration(2.0), &FR3MuJoCoRosOneInterface::check_controller_node_status, this);
 
-  ee_perturb_sub_ = nh_.subscribe("mj_ee_perturb", queue_size_,
-                                  &FR3MuJoCoRosOneInterface::subscribe_end_effector_perturb, this);
-  gui_cmd_ptr_ = std::make_shared<MuJoCoGuiCommand>();
+  /* Initialize subscriber */
+  sub_mujoco_cmd_ = nh_.subscribe("mujoco_command", queue_size_,
+                                  &FR3MuJoCoRosOneInterface::subscribe_mujoco_command, this);
+  mujoco_cmd_ptr_ = std::make_shared<MuJoCoControlCommand>();
 
   /* Initialize publishers */
-  pub_mj_sensor_ = nh_.advertise<fr3_custom_msgs::MujocoSensor>("/mujoco_sensor", queue_size_);
-  msg_mj_sensor_.mj_joint_state.position.resize(7);
-  msg_mj_sensor_.mj_joint_state.velocity.resize(7);
-  msg_mj_sensor_.mj_joint_state.effort.resize(7);
+  pub_mj_sensor_ = nh_.advertise<fr3_custom_msgs::MujocoSensor>("mujoco_sensor", queue_size_);
 
   timer_ = nh_.createTimer(ros::Duration(0.002),
                            std::bind(&FR3MuJoCoRosOneInterface::publish_mujoco_sensor, this));
 }
 
-void FR3MuJoCoRosOneInterface::subscribe_joint_command(const sensor_msgs::JointState & msg)
+void FR3MuJoCoRosOneInterface::check_controller_node_status(const ros::TimerEvent &)
 {
-  if (sim_->d_ != nullptr)
+  auto srv = std_srvs::SetBool();
+  srv.request.data = true;
+
+  if (client_controller_status_.call(srv))
   {
-    // ! Watch out for the size.
-    joint_cmd_ptr_->time_ = ros::Time::now().toSec();
-    joint_cmd_ptr_->name_.resize(msg.name.size());
-    joint_cmd_ptr_->pos_.resize(msg.position.size());
-    joint_cmd_ptr_->vel_.resize(msg.velocity.size());
-    joint_cmd_ptr_->tau_.resize(msg.effort.size());
-
-    for (size_t i = 0; i < joint_cmd_ptr_->tau_.size(); ++i)
+    if (srv.response.success)
     {
-      joint_cmd_ptr_->name_[i] = msg.name[i];
-      joint_cmd_ptr_->pos_[i] = msg.position[i];
-      joint_cmd_ptr_->vel_[i] = msg.velocity[i];
-      joint_cmd_ptr_->tau_[i] = msg.effort[i];
+      ROS_INFO("Controller is running. Simulation will starts in 3 sec.");
+      sim_->pending_.load_key = true;
+      // sim_->cam.azimuth = -180.0;
+      // sim_->cam.distance = 6.75;
+      // sim_->cam.elevation = -6.75;
+      // sim_->cam.lookat[0] = -0.65;
+      // sim_->cam.lookat[1] = 0.25;
+      // sim_->cam.lookat[2] = 0.4;
+
+      ros::Time start_time = ros::Time::now();
+      ros::Duration wait_time(3.0);  // wait for 3 sec
+
+      while (ros::Time::now() - start_time < wait_time)
+      {
+        ros::spinOnce();
+        ros::Duration(0.01).sleep();
+      }
+      sim_->run = true;
+
+      timer_check_controller_status_.stop();
     }
   }
+  else
+  {
+    ROS_WARN("Failed to call service: controller_status");
+  }
 }
 
-void FR3MuJoCoRosOneInterface::subscribe_end_effector_perturb(const geometry_msgs::Wrench & msg)
+void FR3MuJoCoRosOneInterface::subscribe_mujoco_command(const fr3_custom_msgs::MujocoCommand & msg)
 {
+  // const std::lock_guard<mujoco::SimulateMutex> lock(sim_->mtx);
   if (sim_->d_ != nullptr)
   {
-    gui_cmd_ptr_->ee_perturb_.resize(6);
-
-    gui_cmd_ptr_->ee_perturb_[0] = msg.force.x;
-    gui_cmd_ptr_->ee_perturb_[1] = msg.force.y;
-    gui_cmd_ptr_->ee_perturb_[2] = msg.force.z;
-    gui_cmd_ptr_->ee_perturb_[3] = msg.torque.x;
-    gui_cmd_ptr_->ee_perturb_[4] = msg.torque.y;
-    gui_cmd_ptr_->ee_perturb_[5] = msg.torque.z;
+    mujoco_cmd_ptr_->time_ = ros::Time::now().toSec();
+    mujoco_cmd_ptr_->name_.resize(msg.joint_cmd.name.size());
+    mujoco_cmd_ptr_->pos_.resize(msg.joint_cmd.position.size());
+    mujoco_cmd_ptr_->vel_.resize(msg.joint_cmd.velocity.size());
+    mujoco_cmd_ptr_->tau_.resize(msg.joint_cmd.effort.size());
+    mujoco_cmd_ptr_->ee_perturb_.resize(6);
+
+    for (size_t i = 0; i < mujoco_cmd_ptr_->tau_.size(); ++i)
+    {
+      mujoco_cmd_ptr_->name_[i] = msg.joint_cmd.name[i];
+      mujoco_cmd_ptr_->pos_[i] = msg.joint_cmd.position[i];
+      mujoco_cmd_ptr_->vel_[i] = msg.joint_cmd.velocity[i];
+      mujoco_cmd_ptr_->tau_[i] = msg.joint_cmd.effort[i];
+    }
+
+    mujoco_cmd_ptr_->ee_perturb_[0] = msg.ee_perturb.force.x;
+    mujoco_cmd_ptr_->ee_perturb_[1] = msg.ee_perturb.force.y;
+    mujoco_cmd_ptr_->ee_perturb_[2] = msg.ee_perturb.force.z;
+    mujoco_cmd_ptr_->ee_perturb_[3] = msg.ee_perturb.torque.x;
+    mujoco_cmd_ptr_->ee_perturb_[4] = msg.ee_perturb.torque.y;
+    mujoco_cmd_ptr_->ee_perturb_[5] = msg.ee_perturb.torque.z;
   }
 }
 
@@ -68,51 +96,52 @@ void FR3MuJoCoRosOneInterface::publish_mujoco_sensor()
 {
   if (sim_->d_ != nullptr)
   {
-    const std::lock_guard<mujoco::SimulateMutex> lock(sim_->mtx);
-
+    msg_mj_sensor_.joint_state.position.resize(sim_->m_->njnt - 2);
+    msg_mj_sensor_.joint_state.velocity.resize(sim_->m_->njnt - 2);
+    msg_mj_sensor_.joint_state.effort.resize(sim_->m_->njnt - 2);
     // cout << "njnt:\t" << sim_->m_->njnt << endl;  // 9
     // cout << "nv:\t" << sim_->m_->nv << endl;  // 9
+
+    const std::lock_guard<mujoco::SimulateMutex> lock(sim_->mtx);
+
+    msg_mj_sensor_.time = sim_->d_->time;
+    msg_mj_sensor_.b_mj_node_run.data = static_cast<bool>(sim_->run);
+
     for (size_t i = 0; i < sim_->m_->njnt - 2; ++i)
     {
-      msg_mj_sensor_.mj_joint_state.position[i] = sim_->d_->sensordata[i];
-      msg_mj_sensor_.mj_joint_state.velocity[i] = sim_->d_->sensordata[i + 7];
-      msg_mj_sensor_.mj_joint_state.effort[i] = sim_->d_->qacc[i];
+      msg_mj_sensor_.joint_state.position[i] = sim_->d_->sensordata[i];
+      msg_mj_sensor_.joint_state.velocity[i] = sim_->d_->sensordata[i + 7];
+      msg_mj_sensor_.joint_state.effort[i] = sim_->d_->qacc[i];
     }
 
-    msg_mj_sensor_.mj_ee_pose.position.x = sim_->d_->sensordata[14];
-    msg_mj_sensor_.mj_ee_pose.position.y = sim_->d_->sensordata[15];
-    msg_mj_sensor_.mj_ee_pose.position.z = sim_->d_->sensordata[16];
-    msg_mj_sensor_.mj_ee_pose.orientation.w = sim_->d_->sensordata[17];
-    msg_mj_sensor_.mj_ee_pose.orientation.x = sim_->d_->sensordata[18];
-    msg_mj_sensor_.mj_ee_pose.orientation.y = sim_->d_->sensordata[19];
-    msg_mj_sensor_.mj_ee_pose.orientation.z = sim_->d_->sensordata[20];
-
-    msg_mj_sensor_.mj_ee_twist.linear.x = sim_->d_->sensordata[21];
-    msg_mj_sensor_.mj_ee_twist.linear.y = sim_->d_->sensordata[22];
-    msg_mj_sensor_.mj_ee_twist.linear.z = sim_->d_->sensordata[23];
-    msg_mj_sensor_.mj_ee_twist.angular.x = sim_->d_->sensordata[24];
-    msg_mj_sensor_.mj_ee_twist.angular.y = sim_->d_->sensordata[25];
-    msg_mj_sensor_.mj_ee_twist.angular.z = sim_->d_->sensordata[26];
-
-    msg_mj_sensor_.mj_ee_accel.linear.x = sim_->d_->sensordata[27];
-    msg_mj_sensor_.mj_ee_accel.linear.y = sim_->d_->sensordata[28];
-    msg_mj_sensor_.mj_ee_accel.linear.z = sim_->d_->sensordata[29];
-    msg_mj_sensor_.mj_ee_accel.angular.x = sim_->d_->sensordata[30];
-    msg_mj_sensor_.mj_ee_accel.angular.y = sim_->d_->sensordata[31];
-    msg_mj_sensor_.mj_ee_accel.angular.z = sim_->d_->sensordata[32];
-
-    msg_mj_sensor_.mj_ee_wrench.force.x = sim_->d_->sensordata[33];
-    msg_mj_sensor_.mj_ee_wrench.force.y = sim_->d_->sensordata[34];
-    msg_mj_sensor_.mj_ee_wrench.force.z = sim_->d_->sensordata[35];
-    msg_mj_sensor_.mj_ee_wrench.torque.x = sim_->d_->sensordata[36];
-    msg_mj_sensor_.mj_ee_wrench.torque.y = sim_->d_->sensordata[37];
-    msg_mj_sensor_.mj_ee_wrench.torque.z = sim_->d_->sensordata[38];
+    msg_mj_sensor_.ee_pose.position.x = sim_->d_->sensordata[14];
+    msg_mj_sensor_.ee_pose.position.y = sim_->d_->sensordata[15];
+    msg_mj_sensor_.ee_pose.position.z = sim_->d_->sensordata[16];
+    msg_mj_sensor_.ee_pose.orientation.w = sim_->d_->sensordata[17];
+    msg_mj_sensor_.ee_pose.orientation.x = sim_->d_->sensordata[18];
+    msg_mj_sensor_.ee_pose.orientation.y = sim_->d_->sensordata[19];
+    msg_mj_sensor_.ee_pose.orientation.z = sim_->d_->sensordata[20];
+
+    msg_mj_sensor_.ee_twist.linear.x = sim_->d_->sensordata[21];
+    msg_mj_sensor_.ee_twist.linear.y = sim_->d_->sensordata[22];
+    msg_mj_sensor_.ee_twist.linear.z = sim_->d_->sensordata[23];
+    msg_mj_sensor_.ee_twist.angular.x = sim_->d_->sensordata[24];
+    msg_mj_sensor_.ee_twist.angular.y = sim_->d_->sensordata[25];
+    msg_mj_sensor_.ee_twist.angular.z = sim_->d_->sensordata[26];
+
+    msg_mj_sensor_.ee_accel.linear.x = sim_->d_->sensordata[27];
+    msg_mj_sensor_.ee_accel.linear.y = sim_->d_->sensordata[28];
+    msg_mj_sensor_.ee_accel.linear.z = sim_->d_->sensordata[29];
+    msg_mj_sensor_.ee_accel.angular.x = sim_->d_->sensordata[30];
+    msg_mj_sensor_.ee_accel.angular.y = sim_->d_->sensordata[31];
+    msg_mj_sensor_.ee_accel.angular.z = sim_->d_->sensordata[32];
+
+    msg_mj_sensor_.ee_wrench.force.x = sim_->d_->sensordata[33];
+    msg_mj_sensor_.ee_wrench.force.y = sim_->d_->sensordata[34];
+    msg_mj_sensor_.ee_wrench.force.z = sim_->d_->sensordata[35];
+    msg_mj_sensor_.ee_wrench.torque.x = sim_->d_->sensordata[36];
+    msg_mj_sensor_.ee_wrench.torque.y = sim_->d_->sensordata[37];
+    msg_mj_sensor_.ee_wrench.torque.z = sim_->d_->sensordata[38];
     pub_mj_sensor_.publish(msg_mj_sensor_);
   }
 }
-
-// std::shared_ptr<FR3MuJoCoRosOneInterface::MuJoCoControlCommand>
-// FR3MuJoCoRosOneInterface::get_joint_cmd_ptr()
-// {
-//   return joint_cmd_ptr_;
-// }

fr3_ros1_mujoco_interface/src/main.cc

@@ -35,8 +35,7 @@
 
 // + ROS node interface header & joint command pointer & global variables
 #include "fr3_ros1_mujoco_interface.hpp"
-std::shared_ptr<FR3MuJoCoRosOneInterface::MuJoCoControlCommand> joint_cmd_ptr;
-std::shared_ptr<FR3MuJoCoRosOneInterface::MuJoCoGuiCommand> gui_cmd_ptr;
+std::shared_ptr<FR3MuJoCoRosOneInterface::MuJoCoControlCommand> mujoco_cmd_ptr;
 bool b_is_pertrub_on = false;
 bool b_is_subscribe_real_data = false;
 
@@ -299,73 +298,69 @@ mjModel * LoadModel(const char * file, mj::Simulate & sim)
 // + ----- CUSTOM FUNCTIONS ------------------------------------------------------------------------
 void apply_control(mjModel * m, mjData * d)
 {
-  if (joint_cmd_ptr->tau_.empty())
+  if (mujoco_cmd_ptr == nullptr)
   {
-    std::cerr << "Error: tau_ is empty!" << std::endl;
+    std::cerr << "Error: mujoco_cmd_ptr is null!" << std::endl;
     return;
   }
 
-  if (joint_cmd_ptr->tau_.empty())
+  if (mujoco_cmd_ptr->tau_.empty())
   {
     std::cerr << "Error: tau_ is empty!" << std::endl;
     return;
   }
 
+  /* joint command */
   for (size_t i = 0; i < m->njnt - 2; ++i)
   {
-    d->ctrl[i] = joint_cmd_ptr->tau_[i];
-  }
-}
-
-/**
- * @brief Apply external disturbance
- * !!!!!! It is seemed that applied disturbance is aligend w/ EE's "Global" frame
- * !!!!!! while the sensor data are represented in "Global" frame -> this should be checked
- * TODO: Check force/torque sensor data's reference frame
- */
-void apply_disturb(mjModel * m, mjData * d)
-{
-  int dof = 6;
-  double dt = 2.0;
-  double t[7];
-  for (size_t i = 0; i < dof + 1; ++i)
-  {
-    t[i] = 2 * dt + 4 * i;
+    d->ctrl[i] = mujoco_cmd_ptr->tau_[i];
   }
 
-  double fd(10.0), md(10.0);
-
+  /* end-effector perturbation */
   int hand_id = mj_name2id(m, mjOBJ_BODY, "hand");
-
   if (hand_id >= 0)
   {
-    if (b_is_subscribe_real_data == true)
-    {
-      for (size_t i = 0; i < dof; ++i)
-      {
-        d->xfrc_applied[hand_id * 6 + i] = gui_cmd_ptr->ee_perturb_[i];
-      }
-    }
-    else
+    for (size_t i = 0; i < 6; ++i)
     {
-      for (size_t i = 0; i < 3; ++i)
-      {
-        if (d->time >= t[i] && d->time < t[i + 1])
-        {
-          // d->xfrc_applied[hand_id * 6 + i] = (d->time < t[i] + dt ? fd : -fd);
-          d->xfrc_applied[hand_id * 6 + i] =
-            0.5 * fd * (1 - std::cos(2 * M_PI * (1 / dt) * (d->time - t[i])));
-        }
-        if (d->time >= t[i + 3] && d->time < t[i + 4])
-        {
-          // d->xfrc_applied[hand_id * 6 + i + 3] = (d->time < t[i + 3] + dt ? md : -md);
-          d->xfrc_applied[hand_id * 6 + i + 3] =
-            0.5 * md * (1 - std::cos(2 * M_PI * (1 / dt) * (d->time - t[i + 3])));
-        }
-      }
+      d->xfrc_applied[hand_id * 6 + i] = mujoco_cmd_ptr->ee_perturb_[i];
     }
   }
 }
+
+/**
+ * @brief Apply external disturbance
+ * !!!!!! It is seemed that applied disturbance is aligend w/ EE's "Global" frame
+ * !!!!!! while the sensor data are represented in "Global" frame -> this should be checked
+ * TODO: Check force/torque sensor data's reference frame
+ */
+// void apply_disturb(mjModel * m, mjData * d)
+// {
+//   int dof = 6;
+//   double dt = 2.0;
+//   double t[7];
+//   for (size_t i = 0; i < dof + 1; ++i)
+//   {
+//     t[i] = 2 * dt + 4 * i;
+//   }
+
+//   double fd(10.0), md(10.0);
+
+//   for (size_t i = 0; i < 3; ++i)
+//   {
+//     if (d->time >= t[i] && d->time < t[i + 1])
+//     {
+//       // d->xfrc_applied[hand_id * 6 + i] = (d->time < t[i] + dt ? fd : -fd);
+//       d->xfrc_applied[hand_id * 6 + i] =
+//         0.5 * fd * (1 - std::cos(2 * M_PI * (1 / dt) * (d->time - t[i])));
+//     }
+//     if (d->time >= t[i + 3] && d->time < t[i + 4])
+//     {
+//       // d->xfrc_applied[hand_id * 6 + i + 3] = (d->time < t[i + 3] + dt ? md : -md);
+//       d->xfrc_applied[hand_id * 6 + i + 3] =
+//         0.5 * md * (1 - std::cos(2 * M_PI * (1 / dt) * (d->time - t[i + 3])));
+//     }
+//   }
+// }
 // + -----------------------------------------------------------------------------------------------
 
 // simulate in background thread (while rendering in main thread)
@@ -510,11 +505,6 @@ void PhysicsLoop(mj::Simulate & sim)
 
             // + ----- Call custom function --------------------------------------------------------
             apply_control(m, d);
-            if (b_is_pertrub_on == true)
-            {
-              apply_disturb(m, d);
-            }
-            // + -----------------------------------------------------------------------------------
 
             // run single step, let next iteration deal with timing
             mj_step(m, d);
@@ -543,11 +533,6 @@ void PhysicsLoop(mj::Simulate & sim)
 
               // + ----- Call custom function ------------------------------------------------------
               apply_control(m, d);
-              if (b_is_pertrub_on == true)
-              {
-                apply_disturb(m, d);
-              }
-              // + ---------------------------------------------------------------------------------
 
               // call mj_step
               mj_step(m, d);
@@ -684,18 +669,17 @@ int main(int argc, char ** argv)
   // }
 
   // + UI Settings
-  sim->ui0_enable = false;        // left UI is disabled (TAB)
-  sim->ui1_enable = false;        // right UI <is disabled (Shift + TAB)
-  sim->pending_.load_key = true;  // load key frame
-  // sim->run = false;
+  sim->ui0_enable = false;         // left UI is disabled (TAB)
+  sim->ui1_enable = false;         // right UI <is disabled (Shift + TAB)
+  sim->pending_.load_key = false;  // load key frame
+  sim->run = false;
 
   // start physics thread
   std::thread physicsthreadhandle(&PhysicsThread, sim.get(), filename);
 
   // + create ROS node
   auto node = std::make_shared<FR3MuJoCoRosOneInterface>(sim.get(), nh);
-  joint_cmd_ptr = node->get_joint_cmd_ptr();
-  gui_cmd_ptr = node->get_gui_cmd_ptr();
+  mujoco_cmd_ptr = node->get_mujoco_cmd_ptr();
 
   auto spin_func = [](ros::NodeHandle nh) {
     ros::spin();