diff --git a/darwin_control/config/darwin_cpp.yaml b/darwin_control/config/darwin_cpp.yaml
index 5fa9c84ac877f8028fcb2f91ad850d4a2b7df8b8..1f61725bb7be5b68d51a721333f0e99d1c26a267 100644
--- a/darwin_control/config/darwin_cpp.yaml
+++ b/darwin_control/config/darwin_cpp.yaml
@@ -5,9 +5,8 @@ darwin:
     publish_rate: 50  
 
   darwin_controller_cpp:
-    left_sensor_foot_present: true
-    right_sensor_foot_present: true
-    serial_device: '/dev/pts/19'
+    dyn_serial_device: '/dev/pts/19'
+    imu_serial_device: '/dev/pts/22'
     type: effort_controllers/DarwinControllerCPP
     joints:
       - j_shoulder_pitch_r
diff --git a/darwin_controller_cpp/CMakeLists.txt b/darwin_controller_cpp/CMakeLists.txt
index 477d709bcf3afc7066e974c399971387bea7d79b..0dd0b4000dd89bedd1d7ed158260bba30a33d93b 100644
--- a/darwin_controller_cpp/CMakeLists.txt
+++ b/darwin_controller_cpp/CMakeLists.txt
@@ -11,7 +11,7 @@ find_package(catkin REQUIRED roscpp urdf controller_interface hardware_interface
 FIND_PACKAGE(iriutils REQUIRED)
 FIND_PACKAGE(comm REQUIRED)
 FIND_PACKAGE(dynamixel REQUIRED)
-find_package(gazebo REQUIRED)
+FIND_PACKAGE(bno055_imu_sim REQUIRED)
 
 ## Uncomment this if the package has a setup.py. This macro ensures
 ## modules and global scripts declared therein get installed
@@ -110,7 +110,7 @@ include_directories(${STM32_HAL_PATH}/f1/include/core)
 include_directories(${iriutils_INCLUDE_DIR})
 include_directories(${comm_INCLUDE_DIR})
 include_directories(${dynamixel_INCLUDE_DIR})
-include_directories(${GAZEBO_INCLUDE_DIRS})
+include_directories(${bno055_imu_sim_INCLUDE_DIR})
 
 add_definitions(-DSTM32F103xE)
 add_definitions(-DUSE_HAL_DRIVER)
@@ -164,7 +164,7 @@ target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${comm_LIBRARY})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${dynamixel_LIBRARY})
-target_link_libraries(${PROJECT_NAME} ${GAZEBO_LIBRARIES})
+TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${bno055_imu_sim_LIBRARY})
 
 #############
 ## Install ##
diff --git a/darwin_controller_cpp/include/darwin_controller_cpp.h b/darwin_controller_cpp/include/darwin_controller_cpp.h
index 737448d44fd9b577a032bd36f458e73cc576fd8d..808e89e5c6f4dac55cfee9bb316788ebad515133 100644
--- a/darwin_controller_cpp/include/darwin_controller_cpp.h
+++ b/darwin_controller_cpp/include/darwin_controller_cpp.h
@@ -52,14 +52,11 @@
 #include <hardware_interface/joint_command_interface.h>
 #include <control_toolbox/pid.h>
 #include <sensor_msgs/Imu.h>
-#include <sensor_msgs/Range.h>
-
-#include <gazebo/transport/transport.hh>
-#include <gazebo/msgs/msgs.hh>
-#include <gazebo/gazebo.hh>
+#include <tf/transform_datatypes.h>
 
 #include "mutex.h"
 #include "dynamixel_slave_serial.h"
+#include "bno055_imu_sim.h"
 
 namespace darwin_controller_cpp
 {
@@ -84,6 +81,8 @@ namespace darwin_controller_cpp
       typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
 
       CDynamixelSlaveSerial *dyn_slave;
+ 
+      CBNO055IMUSim *imu_sim;
 
       std::string name_;               ///< Controller name.
       std::vector<JointHandle> joints_;             ///< Handles to controlled joints.
@@ -100,32 +99,10 @@ namespace darwin_controller_cpp
       ros::Subscriber accel_sub;
       void accel_callback(const sensor_msgs::Imu::ConstPtr& msg);
 
-      ros::Subscriber range_sub;
-      void range_callback(const sensor_msgs::Range::ConstPtr& msg);
+      ros::Subscriber imu_sub;
+      void imu_callback(const sensor_msgs::Imu::ConstPtr& msg);
 
       CMutex walk_access;
- 
-      // feet sensors 
-      bool left_sensor_foot_present;
-      bool right_sensor_foot_present;
-
-      // smart charger attributes
-      // internal attributes
-      bool use_smart_charger;
-      double initial_charge;
-      double current_charge;
-      double discharge_rate;
-      double charge_current;
-      unsigned short int average_time_to_full;
-      unsigned short int average_time_to_empty;
-      std::string charge_station_link;
-      std::string charge_robot_link;
-      bool charger_connected;
-      bool charger_enabled;
-      // gazebo interface for contacts
-      gazebo::transport::NodePtr gazebo_node;
-      gazebo::transport::SubscriberPtr contacts_subs;
-      void OnContacts(ConstContactsPtr &msg);
   };
 
 } // namespace
diff --git a/darwin_controller_cpp/include/darwin_controller_cpp_impl.h b/darwin_controller_cpp/include/darwin_controller_cpp_impl.h
index 7ce433bc2d502cbcb3a630ead3ac690d33feb888..e10277b25955849a46006cbf448c51d5e3ee4b9b 100644
--- a/darwin_controller_cpp/include/darwin_controller_cpp_impl.h
+++ b/darwin_controller_cpp/include/darwin_controller_cpp_impl.h
@@ -157,7 +157,8 @@ namespace darwin_controller_cpp
   template <class HardwareInterface>
     DarwinControllerCPP<HardwareInterface>::DarwinControllerCPP()
     {
-      this->dyn_slave=NULL;
+      this->dyn_slave=NULL; 
+      this->imu_sim=NULL;
     }
 
   template <class HardwareInterface>
@@ -169,53 +170,10 @@ namespace darwin_controller_cpp
       // Cache controller node handle
       controller_nh_ = controller_nh;
 
-      // smart charger interface
-      this->use_smart_charger=DEFAULT_ENABLE_SMART_CHARGER;
-      controller_nh_.getParam("use_smart_charger", this->use_smart_charger);
-      ROS_DEBUG_STREAM_NAMED(name_, "Smart charger is: " << this->use_smart_charger);
-      if(this->use_smart_charger)
-      {
-        // initialize gazebo interfaces
-        this->gazebo_node=gazebo::transport::NodePtr(new gazebo::transport::Node());
-        this->gazebo_node->Init();
-        this->contacts_subs=this->gazebo_node->Subscribe("/gazebo/default/physics/contacts",&DarwinControllerCPP<HardwareInterface>::OnContacts,this);
-        // read parameters
-        this->initial_charge=DEFAULT_INITIAL_CHARGE;
-        controller_nh_.getParam("initial_charge", this->initial_charge);
-        ROS_DEBUG_STREAM_NAMED(name_, "Initial charge set to: " << this->initial_charge);
-        this->current_charge=this->initial_charge;
-        this->discharge_rate=DEFAULT_DISCHARGE_RATE;
-        controller_nh_.getParam("discharge_rate", this->discharge_rate);
-        ROS_DEBUG_STREAM_NAMED(name_, "Discharge rate set to: " << this->discharge_rate);
-        this->charge_current=DEFAULT_CHARGE_CURRENT;
-        controller_nh_.getParam("charge_current", this->charge_current);
-        ROS_DEBUG_STREAM_NAMED(name_, "Charge current set to: " << this->charge_current);
-        this->charge_station_link=DEFAULT_CHARGE_STATION_LINK;
-        controller_nh_.getParam("charge_station_link", this->charge_station_link);
-        ROS_DEBUG_STREAM_NAMED(name_, "Charger station link set to: " << this->charge_station_link);
-        this->charge_robot_link=DEFAULT_CHARGE_ROBOT_LINK;
-        controller_nh_.getParam("charge_robot_link", this->charge_robot_link);
-        ROS_DEBUG_STREAM_NAMED(name_, "Charger robot link set to: " << this->charge_robot_link);
-        this->average_time_to_full=-1;
-        this->average_time_to_empty=-1;
-        
-        this->charger_connected=false;
-        this->charger_enabled=false;
-      }
       // initialize topics
       this->gyro_sub=root_nh.subscribe("sensors/raw_gyro", 1, &DarwinControllerCPP<HardwareInterface>::gyro_callback,this);
       this->accel_sub=root_nh.subscribe("sensors/raw_accel", 1, &DarwinControllerCPP<HardwareInterface>::accel_callback,this);
-
-      this->left_sensor_foot_present=false;
-      controller_nh_.getParam("left_sensor_foot_present", this->left_sensor_foot_present);
-      ROS_DEBUG_STREAM_NAMED(name_, "left sensor foot is: " << this->left_sensor_foot_present);
-
-      this->right_sensor_foot_present=false;
-      controller_nh_.getParam("right_sensor_foot_present", this->right_sensor_foot_present);
-      ROS_DEBUG_STREAM_NAMED(name_, "right sensor foot is: " << this->right_sensor_foot_present);
-      
-      if(this->right_sensor_foot_present || this->right_sensor_foot_present)
-        this->range_sub=root_nh.subscribe("sensors/range", 1, &DarwinControllerCPP<HardwareInterface>::range_callback,this);
+      this->imu_sub=root_nh.subscribe("sensors/raw_imu", 1, &DarwinControllerCPP<HardwareInterface>::imu_callback,this);
 
       // Controller name
       name_=getLeafNamespace(controller_nh_);
@@ -278,10 +236,10 @@ namespace darwin_controller_cpp
       /* initialize dynamixel slave */
       darwin_dyn_slave_init();
       // initialize dynamixel slave server
-      ROS_INFO("Load slave parameters");
+      ROS_INFO("Load dynamixel slave parameters");
       serial_device="/dev/ttyUSB0";
-      controller_nh_.getParam("serial_device", serial_device);
-      ROS_INFO_STREAM("serial_device is: " << serial_device);
+      controller_nh_.getParam("dyn_serial_device", serial_device);
+      ROS_INFO_STREAM("dyn_serial_device is: " << serial_device);
       try{ 
         this->dyn_slave=new CDynamixelSlaveSerial("dyn_slave",serial_device,dyn_version2);
         this->dyn_slave->set_baudrate(2000000/(ram_data[DARWIN_BAUD_RATE]+1));
@@ -296,6 +254,22 @@ namespace darwin_controller_cpp
         }
         ROS_ERROR_STREAM(e.what());
       }
+      // initialize IMU simulator
+      ROS_INFO("Load IMU simulator");
+      serial_device="/dev/ttyUSB0";
+      controller_nh_.getParam("imu_serial_device", serial_device);
+      ROS_INFO_STREAM("dyn_serial_device is: " << serial_device);
+      try{
+        this->imu_sim=new CBNO055IMUSim("darwin_imu",serial_device);
+        this->imu_sim->set_calibrated();
+      }catch(CException &e){
+        if(this->imu_sim!=NULL)
+        {
+          delete this->imu_sim;
+          this->imu_sim=NULL;
+        }
+        ROS_ERROR_STREAM(e.what());
+      }
       /* initialize motion modules */
       manager_init(7800);// motion manager period in us
       ROS_INFO("CDarwinSim : motion manager initialized");
@@ -337,60 +311,6 @@ namespace darwin_controller_cpp
         double command = pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
         this->joints_[i].setCommand(command);
       }
-      if(this->use_smart_charger)
-      {
-        if(this->charger_enabled)
-        {
-          if(this->charger_connected)
-          {
-            this->current_charge+=this->charge_current*period.toSec()/3600.0;
-            if(this->current_charge>this->initial_charge)
-              this->current_charge=this->initial_charge;
-            this->average_time_to_full=(uint16_t)(((this->initial_charge-this->current_charge)/this->charge_current)*60.0);
-          }
-          else
-          {
-            this->current_charge-=this->discharge_rate*period.toSec()/3600.0;
-            if(this->current_charge<0.0)
-            {
-              this->current_charge=0.0;
-              ROS_ERROR("Battery died out before it could be recharged");
-            }
-            this->average_time_to_empty=(uint16_t)((this->current_charge/this->discharge_rate)*60.0);
-            this->average_time_to_full=(uint16_t)-1;
-          }
-        }
-      }
-    }
-
-  template <class HardwareInterface>
-    void DarwinControllerCPP<HardwareInterface>::OnContacts(ConstContactsPtr &msg)
-    {
-      for(unsigned int i=0;i<msg->contact_size();i++)
-      {
-        if(msg->contact(i).collision1().find(this->charge_station_link)!=std::string::npos)
-        {
-          if(msg->contact(i).collision2().find(this->charge_robot_link)!=std::string::npos)
-          {
-            this->charger_connected=true;
-            break;
-          }
-          else
-            this->charger_connected=false;
-        }
-        else if(msg->contact(i).collision1().find(this->charge_robot_link)!=std::string::npos)
-        {
-          if(msg->contact(i).collision2().find(this->charge_station_link)!=std::string::npos)
-          {
-            this->charger_connected=true;
-            break;
-          }
-          else
-            this->charger_connected=false;
-        }
-        else
-          this->charger_connected=false;
-      }
     }
 
 /****************************************** imu *************************************************/
@@ -421,139 +341,25 @@ namespace darwin_controller_cpp
       accel_data[1]=(int32_t)msg->linear_acceleration.y*1000.0;
       accel_data[2]=(int32_t)msg->linear_acceleration.z*1000.0;
     }
-/****************************************** imu *************************************************/
 
   template <class HardwareInterface>
-    void DarwinControllerCPP<HardwareInterface>::range_callback(const sensor_msgs::Range::ConstPtr& msg)
+    void DarwinControllerCPP<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
     {
-      std::string frame_name;
-      unsigned char threshold;
-      static unsigned int remaining_left_sensors=10,remaining_right_sensors=10; 
-
-      if(msg->header.frame_id.find("/")==std::string::npos)
-        frame_name=msg->header.frame_id;
-      else
-        frame_name=msg->header.frame_id.substr(msg->header.frame_id.find("/")+1);
-
-      /* handle the feet sensors */
-      if(msg->range>(msg->max_range/2.0))
-        threshold=0x01;
-      else
-        threshold=0x00;
-      if(this->left_sensor_foot_present)
+      if(this->imu_sim!=NULL)
       {
-        if(frame_name=="left_down_right_rear_ir_link")
-        {
-//          this->left_foot_sensor_data_msg_.voltages[4]=msg->range;
-//          this->left_foot_sensor_data_msg_.status[4]=threshold;
-          remaining_left_sensors--;
-        }
-        else if(frame_name=="left_down_right_middle_ir_link")
-        {
-//          this->left_foot_sensor_data_msg_.voltages[5]=msg->range;
-//          this->left_foot_sensor_data_msg_.status[5]=threshold;
-          remaining_left_sensors--;
-        }
-        else if(frame_name=="left_down_right_front_ir_link")
-        {
-//          this->left_foot_sensor_data_msg_.voltages[6]=msg->range;
-//          this->left_foot_sensor_data_msg_.status[6]=threshold;
-          remaining_left_sensors--;
-        }
-        else if(frame_name=="left_down_left_rear_ir_link")
-        {
-//          this->left_foot_sensor_data_msg_.voltages[1]=msg->range;
-//          this->left_foot_sensor_data_msg_.status[1]=threshold;
-          remaining_left_sensors--;
-        }
-        else if(frame_name=="left_down_left_middle_ir_link")
-        {
-//          this->left_foot_sensor_data_msg_.voltages[0]=msg->range;
-//          this->left_foot_sensor_data_msg_.status[0]=threshold;
-          remaining_left_sensors--;
-        }
-        else if(frame_name=="left_down_left_front_ir_link")
-        {
-//          this->left_foot_sensor_data_msg_.voltages[3]=msg->range;
-//          this->left_foot_sensor_data_msg_.status[3]=threshold;
-          remaining_left_sensors--;
-        }
-        else if(frame_name=="left_front_right_ir_link")
-        {
-//          this->left_foot_sensor_data_msg_.voltages[8]=msg->range;
-//          this->left_foot_sensor_data_msg_.status[8]=threshold;
-          remaining_left_sensors--;
-        }
-        else if(frame_name=="left_front_left_ir_link")
-        {
-//          this->left_foot_sensor_data_msg_.voltages[7]=msg->range;
-//          this->left_foot_sensor_data_msg_.status[7]=threshold;
-          remaining_left_sensors--;
-        }
-        if(remaining_left_sensors==0)
-        {
-          remaining_left_sensors=10;
-//          this->left_foot_sensor_data_publisher_.publish(this->left_foot_sensor_data_msg_); 
-        }
-      }
-      if(this->right_sensor_foot_present)
-      {
-        if(frame_name=="right_down_right_rear_ir_link")
-        {
-//          this->right_foot_sensor_data_msg_.voltages[4]=msg->range;
-//          this->right_foot_sensor_data_msg_.status[4]=threshold;
-          remaining_right_sensors--;
-        }
-        else if(frame_name=="right_down_right_middle_ir_link")
-        {
-//          this->right_foot_sensor_data_msg_.voltages[5]=msg->range;
-//          this->right_foot_sensor_data_msg_.status[5]=threshold;
-          remaining_right_sensors--;
-        }
-        else if(frame_name=="right_down_right_front_ir_link")
-        {
-//          this->right_foot_sensor_data_msg_.voltages[6]=msg->range;
-//          this->right_foot_sensor_data_msg_.status[6]=threshold;
-          remaining_right_sensors--;
-        }
-        else if(frame_name=="right_down_left_rear_ir_link")
-        {
-//          this->right_foot_sensor_data_msg_.voltages[1]=msg->range;
-//          this->right_foot_sensor_data_msg_.status[1]=threshold;
-          remaining_right_sensors--;
-        }
-        else if(frame_name=="right_down_left_middle_ir_link")
-        {
-//          this->right_foot_sensor_data_msg_.voltages[0]=msg->range;
-//          this->right_foot_sensor_data_msg_.status[0]=threshold;
-          remaining_right_sensors--;
-        }
-        else if(frame_name=="right_down_left_front_ir_link")
-        {
-//          this->right_foot_sensor_data_msg_.voltages[3]=msg->range;
-//          this->right_foot_sensor_data_msg_.status[3]=threshold;
-          remaining_right_sensors--;
-        }
-        else if(frame_name=="right_front_right_ir_link")
-        {
-//          this->right_foot_sensor_data_msg_.voltages[8]=msg->range;
-//          this->right_foot_sensor_data_msg_.status[8]=threshold;
-          remaining_right_sensors--;
-        }
-        else if(frame_name=="right_front_left_ir_link")
-        {
-//          this->right_foot_sensor_data_msg_.voltages[7]=msg->range;
-//          this->right_foot_sensor_data_msg_.status[7]=threshold;
-          remaining_right_sensors--;
-        }
-        if(remaining_right_sensors==0)
-        {
-          remaining_right_sensors=10;
-//          this->right_foot_sensor_data_publisher_.publish(this->right_foot_sensor_data_msg_); 
-        }
+        this->imu_sim->set_quaternion_data(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
+        tf::Quaternion q(msg->orientation.x,msg->orientation.y,msg->orientation.z,msg->orientation.w);
+        tf::Matrix3x3 m(q);
+        double roll, pitch, yaw;
+        m.getRPY(roll, pitch, yaw);
+        this->imu_sim->set_euler_angles_data(yaw,roll,pitch);
+        this->imu_sim->set_linear_accel_data(msg->linear_acceleration.x,msg->linear_acceleration.y,msg->linear_acceleration.z);
+        this->imu_sim->set_gyro_data(msg->angular_velocity.x,msg->angular_velocity.y,msg->angular_velocity.z);
       }
     }
 
+/****************************************** imu *************************************************/
+
   template <class HardwareInterface>
     DarwinControllerCPP<HardwareInterface>::~DarwinControllerCPP()
     {
@@ -562,6 +368,11 @@ namespace darwin_controller_cpp
         delete this->dyn_slave;
         this->dyn_slave=NULL;
       }
+      if(this->imu_sim!=NULL)
+      {
+        delete this->imu_sim;
+        this->imu_sim=NULL;
+      }
     }
 
 } // namespace