diff --git a/robotis_controller/include/robotis_controller/robotis_controller.h b/robotis_controller/include/robotis_controller/robotis_controller.h
index 65d7821..4014b29 100644
--- a/robotis_controller/include/robotis_controller/robotis_controller.h
+++ b/robotis_controller/include/robotis_controller/robotis_controller.h
@@ -88,6 +88,7 @@ private:
   void gazeboTimerThread();
   void msgQueueThread();
   void setCtrlModuleThread(std::string ctrl_module);
+  void setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg);
 
   bool isTimerStopped();
   void initializeSyncWrite();
@@ -141,6 +142,7 @@ public:
   void    stopTimer();
   bool    isTimerRunning();
 
+  void    setCtrlModule(std::string module_name);
   void    loadOffset(const std::string path);
 
   /* ROS Topic Callback Functions */
diff --git a/robotis_controller/src/robotis_controller/robotis_controller.cpp b/robotis_controller/src/robotis_controller/robotis_controller.cpp
index 1cb160f..a22c6ae 100644
--- a/robotis_controller/src/robotis_controller/robotis_controller.cpp
+++ b/robotis_controller/src/robotis_controller/robotis_controller.cpp
@@ -1529,75 +1529,18 @@ void RobotisController::setCtrlModuleCallback(const std_msgs::String::ConstPtr &
   set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, _module_name_to_set));
 }
 
+void RobotisController::setCtrlModule(std::string module_name)
+{
+  set_module_thread_ = boost::thread(boost::bind(&RobotisController::setCtrlModuleThread, this, module_name));
+}
 void RobotisController::setJointCtrlModuleCallback(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg)
 {
   if (msg->joint_name.size() != msg->module_name.size())
     return;
 
-  queue_mutex_.lock();
-
-  for (unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
-  {
-    Dynamixel *dxl = NULL;
-    auto dxl_it = robot_->dxls_.find((std::string) (msg->joint_name[idx]));
-    if (dxl_it != robot_->dxls_.end())
-      dxl = dxl_it->second;
-    else
-      continue;
-
-    // none
-    if (msg->module_name[idx] == "" || msg->module_name[idx] == "none")
-    {
-      dxl->ctrl_module_name_ = msg->module_name[idx];
-      continue;
-    }
-
-    // check whether the module is using this joint
-    for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
-    {
-      if ((*m_it)->getModuleName() == msg->module_name[idx])
-      {
-        if ((*m_it)->result_.find(msg->joint_name[idx]) != (*m_it)->result_.end())
-        {
-          dxl->ctrl_module_name_ = msg->module_name[idx];
-          break;
-        }
-      }
-    }
+  set_module_thread_ = boost::thread(boost::bind(&RobotisController::setJointCtrlModuleThread, this, msg));
   }
 
-  for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
-  {
-    // set all modules -> disable
-    (*m_it)->setModuleEnable(false);
-
-    // set all used modules -> enable
-    for (auto& d_it : robot_->dxls_)
-    {
-      if (d_it.second->ctrl_module_name_ == (*m_it)->getModuleName())
-      {
-        (*m_it)->setModuleEnable(true);
-        break;
-      }
-    }
-  }
-
-  // TODO: set indirect address
-  // -> check module's control_mode
-
-  queue_mutex_.unlock();
-
-  robotis_controller_msgs::JointCtrlModule current_module_msg;
-  for (auto& dxl_iter : robot_->dxls_)
-  {
-    current_module_msg.joint_name.push_back(dxl_iter.first);
-    current_module_msg.module_name.push_back(dxl_iter.second->ctrl_module_name_);
-  }
-
-  if (current_module_msg.joint_name.size() == current_module_msg.module_name.size())
-    current_module_pub_.publish(current_module_msg);
-}
-
 bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointModule::Request &req,
     robotis_controller_msgs::GetJointModule::Response &res)
 {
@@ -1617,6 +1560,199 @@ bool RobotisController::getCtrlModuleCallback(robotis_controller_msgs::GetJointM
   return true;
 }
 
+void RobotisController::setJointCtrlModuleThread(const robotis_controller_msgs::JointCtrlModule::ConstPtr &msg)
+{
+  // stop module list
+  std::list<MotionModule *> _stop_modules;
+  std::list<MotionModule *> _enable_modules;
+
+  for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
+  {
+    Dynamixel *_dxl = NULL;
+    std::map<std::string, Dynamixel*>::iterator _dxl_it = robot_->dxls_.find((std::string)(msg->joint_name[idx]));
+    if(_dxl_it != robot_->dxls_.end())
+      _dxl = _dxl_it->second;
+    else
+      continue;
+
+    // enqueue
+    if(_dxl->ctrl_module_name_ != msg->module_name[idx])
+    {
+      for(std::list<MotionModule *>::iterator _stop_m_it = motion_modules_.begin(); _stop_m_it != motion_modules_.end(); _stop_m_it++)
+      {
+        if((*_stop_m_it)->getModuleName() == _dxl->ctrl_module_name_ && (*_stop_m_it)->getModuleEnable() == true)
+          _stop_modules.push_back(*_stop_m_it);
+      }
+    }
+  }
+
+  // stop the module
+  _stop_modules.unique();
+  for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
+  {
+    (*_stop_m_it)->stop();
+  }
+
+  // wait to stop
+  for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
+  {
+    while((*_stop_m_it)->isRunning())
+      usleep(CONTROL_CYCLE_MSEC * 1000);
+  }
+
+  // disable module(s)
+  for(std::list<MotionModule *>::iterator _stop_m_it = _stop_modules.begin(); _stop_m_it != _stop_modules.end(); _stop_m_it++)
+  {
+    (*_stop_m_it)->setModuleEnable(false);
+  }
+
+  // set ctrl module
+  queue_mutex_.lock();
+
+  for(unsigned int idx = 0; idx < msg->joint_name.size(); idx++)
+  {
+    std::string ctrl_module = msg->module_name[idx];
+    std::string joint_name = msg->joint_name[idx];
+
+    Dynamixel *_dxl = NULL;
+    std::map<std::string, Dynamixel*>::iterator _dxl_it = robot_->dxls_.find(joint_name);
+    if(_dxl_it != robot_->dxls_.end())
+      _dxl = _dxl_it->second;
+    else
+      continue;
+
+    // none
+    if(ctrl_module == "" || ctrl_module == "none")
+    {
+      _dxl->ctrl_module_name_ = "none";
+
+      if(gazebo_mode_ == true)
+        continue;
+
+      uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
+      uint8_t _sync_write_data[4];
+      _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
+      _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
+      _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data));
+      _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data));
+
+      if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
+        port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
+
+      if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
+        port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_);
+      if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
+        port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_);
+    }
+    else
+    {
+      // check whether the module exist
+      for(std::list<MotionModule *>::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++)
+      {
+        // if it exist
+        if((*_m_it)->getModuleName() == ctrl_module)
+        {
+          std::map<std::string, DynamixelState*>::iterator _result_it = (*_m_it)->result_.find(joint_name);
+          if(_result_it == (*_m_it)->result_.end())
+            break;
+
+          _dxl->ctrl_module_name_ = ctrl_module;
+
+          // enqueue enable module list
+          _enable_modules.push_back(*_m_it);
+          ControlMode _mode = (*_m_it)->getControlMode();
+
+          if(gazebo_mode_ == true)
+            break;
+
+          if(_mode == PositionControl)
+          {
+            uint32_t _pos_data = _dxl->convertRadian2Value(_dxl->dxl_state_->goal_position_ + _dxl->dxl_state_->position_offset_);
+            uint8_t _sync_write_data[4];
+            _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_pos_data));
+            _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_pos_data));
+            _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_pos_data));
+            _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_pos_data));
+
+            if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
+              port_to_sync_write_position_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
+
+            if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
+              port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_);
+            if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
+              port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_);
+          }
+          else if(_mode == VelocityControl)
+          {
+            uint32_t _vel_data = _dxl->convertVelocity2Value(_dxl->dxl_state_->goal_velocity_);
+            uint8_t _sync_write_data[4];
+            _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_vel_data));
+            _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_vel_data));
+            _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_vel_data));
+            _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_vel_data));
+
+            if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
+              port_to_sync_write_velocity_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
+
+            if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
+              port_to_sync_write_current_[_dxl->port_name_]->removeParam(_dxl->id_);
+            if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
+              port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_);
+          }
+          else if(_mode == TorqueControl)
+          {
+            uint32_t _curr_data = _dxl->convertTorque2Value(_dxl->dxl_state_->goal_torque_);
+            uint8_t _sync_write_data[4];
+            _sync_write_data[0] = DXL_LOBYTE(DXL_LOWORD(_curr_data));
+            _sync_write_data[1] = DXL_HIBYTE(DXL_LOWORD(_curr_data));
+            _sync_write_data[2] = DXL_LOBYTE(DXL_HIWORD(_curr_data));
+            _sync_write_data[3] = DXL_HIBYTE(DXL_HIWORD(_curr_data));
+
+            if(port_to_sync_write_current_[_dxl->port_name_] != NULL)
+              port_to_sync_write_current_[_dxl->port_name_]->addParam(_dxl->id_, _sync_write_data);
+
+            if(port_to_sync_write_velocity_[_dxl->port_name_] != NULL)
+              port_to_sync_write_velocity_[_dxl->port_name_]->removeParam(_dxl->id_);
+            if(port_to_sync_write_position_[_dxl->port_name_] != NULL)
+              port_to_sync_write_position_[_dxl->port_name_]->removeParam(_dxl->id_);
+          }
+          break;
+        }
+      }
+    }
+  }
+
+  // enable module(s)
+  _enable_modules.unique();
+  for(std::list<MotionModule *>::iterator _m_it = _enable_modules.begin(); _m_it != _enable_modules.end(); _m_it++)
+  {
+    (*_m_it)->setModuleEnable(true);
+  }
+
+  // TODO: set indirect address
+  // -> check module's control_mode
+
+  queue_mutex_.unlock();
+
+  // log
+//  std::cout << "Enable Joint Ctrl Module : " << std::endl;
+//  for(std::list<MotionModule *>::iterator _m_it = motion_modules_.begin(); _m_it != motion_modules_.end(); _m_it++)
+//  {
+//    if((*_m_it)->GetModuleEnable() == true)
+//      std::cout << "     - " << (*_m_it)->GetModuleName() << std::endl;
+//  }
+
+  // publish current module
+  robotis_controller_msgs::JointCtrlModule _current_module_msg;
+  for(std::map<std::string, Dynamixel *>::iterator _dxl_iter = robot_->dxls_.begin(); _dxl_iter  != robot_->dxls_.end(); ++_dxl_iter)
+  {
+    _current_module_msg.joint_name.push_back(_dxl_iter->first);
+    _current_module_msg.module_name.push_back(_dxl_iter->second->ctrl_module_name_);
+  }
+
+  if(_current_module_msg.joint_name.size() == _current_module_msg.module_name.size())
+    current_module_pub_.publish(_current_module_msg);
+}
 void RobotisController::setCtrlModuleThread(std::string ctrl_module)
 {
   // stop module
@@ -1679,6 +1815,13 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
       usleep(CONTROL_CYCLE_MSEC * 1000);
   }
 
+  // disable module(s)
+  for(std::list<MotionModule *>::iterator _stop_m_it = stop_modules.begin(); _stop_m_it != stop_modules.end(); _stop_m_it++)
+  {
+    (*_stop_m_it)->setModuleEnable(false);
+  }
+
+
   // set ctrl module
   queue_mutex_.lock();
 
@@ -1688,12 +1831,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
   // none
   if ((ctrl_module == "") || (ctrl_module == "none"))
   {
-    // set all modules -> disable
-    for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
-    {
-      (*m_it)->setModuleEnable(false);
-    }
-
     // set dxl's control module to "none"
     for (auto& d_it : robot_->dxls_)
     {
@@ -1800,9 +1937,6 @@ void RobotisController::setCtrlModuleThread(std::string ctrl_module)
 
   for (auto m_it = motion_modules_.begin(); m_it != motion_modules_.end(); m_it++)
   {
-    // set all modules -> disable
-    (*m_it)->setModuleEnable(false);
-
     // set all used modules -> enable
     for (auto& d_it : robot_->dxls_)
     {