diff --git a/src/bioloid_robot.cpp b/src/bioloid_robot.cpp
index f6daf9ec13d68c076d0cc99d70a1bec5a33ccba3..49fd95cda2c09dca36d3826cd2b8a7d5f01f5821 100644
--- a/src/bioloid_robot.cpp
+++ b/src/bioloid_robot.cpp
@@ -707,6 +707,38 @@ double CBioloidRobot::mm_get_servo_angle(unsigned char servo_id)
     throw CBioloidRobotException(_HERE_,"Invalid robot device");
 }
 
+void CBioloidRobot::mm_set_offset(unsigned char servo_id,double offset)
+{
+  if(offset<-9.0 || offset>9.0)
+    throw CBioloidRobotException(_HERE_,"Desired offset out of range");
+  else
+  { 
+    if(servo_id>MAX_NUM_SERVOS)
+      throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+    else
+    {
+      this->robot_device->write_byte_register(BIOLOID_MM_SERVO0_OFFSET+servo_id,(unsigned char)((int)(offset*16.0)));
+      usleep(10000);
+    }
+  }
+}
+
+double CBioloidRobot::mm_get_offset(unsigned char servo_id)
+{
+  unsigned char value;
+  double offset;
+
+  if(servo_id>MAX_NUM_SERVOS)
+    throw CBioloidRobotException(_HERE_,"Invalid servo identifier");
+  else
+  {
+    this->robot_device->read_byte_register(BIOLOID_MM_SERVO0_OFFSET+servo_id,&value);
+    offset=((double)((signed char)value))/16.0;
+
+    return offset;
+  } 
+}
+
 // motion action interface
 void CBioloidRobot::action_load_page(unsigned char page_id)
 {
diff --git a/src/bioloid_robot.h b/src/bioloid_robot.h
index c678dee34441b29270335072115c38674126a0da..2e997f2c50af6f7ada1efec13548e3e343c51730 100644
--- a/src/bioloid_robot.h
+++ b/src/bioloid_robot.h
@@ -158,6 +158,8 @@ class CBioloidRobot
     mm_mode_t mm_get_module(unsigned char servo_id);
     std::vector<double> mm_get_servo_angles(void);
     double mm_get_servo_angle(unsigned char servo_id);
+    void mm_set_offset(unsigned char servo_id,double offset);
+    double mm_get_offset(unsigned char servo_id);
     // motion action interface
     void action_load_page(unsigned char page_id);
     unsigned char action_get_current_page(void);
diff --git a/src/examples/bioloid_robot_test.cpp b/src/examples/bioloid_robot_test.cpp
index e060430109b4532372755ada3e182dc86e85d7a2..81a73374b5ac557b31d7a55c0aa3eba2b68fdb80 100644
--- a/src/examples/bioloid_robot_test.cpp
+++ b/src/examples/bioloid_robot_test.cpp
@@ -35,6 +35,8 @@ int main(int argc, char *argv[])
   std::vector<double> angles;  
   unsigned int i=0,j=0;
   CCallbacks callbacks;
+  unsigned char key;
+  double offset=0.0;
   
   try{
     CBioloidRobot tina("Tina",robot_device,921600,0x02,GPIO2,25);
@@ -177,7 +179,7 @@ int main(int argc, char *argv[])
     tina.mm_disable_power();
     tina.mm_stop();*/
     //tina.mm_set_period(7.8);
-    std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl;
+/*    std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl;
     std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
     for(i=0;i<=MAX_NUM_SERVOS;i++)
     {
@@ -197,9 +199,6 @@ int main(int argc, char *argv[])
     while(tina.action_is_page_running())
       usleep(100000);    
     sleep(2);
-    tina.mm_disable_power();
-    tina.mm_stop();
-    return 0;
     tina.action_load_page(RT_S_R);
     tina.action_start();
     for(i=0;i<100;i++)
@@ -212,6 +211,54 @@ int main(int argc, char *argv[])
       usleep(100000);    
     sleep(2);
     tina.mm_disable_power();
+    tina.mm_stop();*/
+    tina.mm_set_offset(1,0.0);
+    tina.mm_set_offset(2,0.0);
+    tina.mm_set_offset(8,0.0);
+    tina.mm_set_offset(9,0.0);
+    tina.mm_set_offset(10,0.0);//5.25
+    tina.mm_set_offset(11,0.0);//1.75
+    tina.mm_set_offset(12,0.0);//4.0
+    tina.mm_set_offset(13,0.0);
+    tina.mm_set_offset(14,0.0);
+    tina.mm_set_offset(15,0.0);//1.75
+    tina.mm_set_offset(16,0.0);//-4
+    tina.mm_set_offset(17,0.0);
+    tina.mm_set_offset(18,0.0);
+    for(i=0;i<=MAX_NUM_SERVOS;i++)
+    {
+      std::cout << "Servo " << i << " offset: " << tina.mm_get_offset(i) << std::endl;
+    }
+    std::cout << "Motion manager period: " << tina.mm_get_period() << std::endl;
+    std::cout << "Number of servos: " << (int)tina.mm_get_num_servos() << std::endl;
+    for(i=0;i<=MAX_NUM_SERVOS;i++)
+    {
+      tina.mm_enable_servo(i);
+      tina.mm_assign_module(i,BIOLOID_MM_ACTION);
+      if(tina.mm_is_servo_enabled(i))
+        std::cout << "Servo " << i << " is enabled";
+      else
+        std::cout << "Servo " << i << " is disabled";
+      std::cout << " and assigned to module " << tina.mm_get_module(i) << std::endl; 
+    }
+    tina.mm_start();
+    //tina.mm_enable_balance();
+    tina.mm_enable_power();
+    tina.action_load_page(WALK_READY);
+    tina.action_start();
+    while(tina.action_is_page_running())
+      usleep(100000);    
+    do{
+      std::cin >> key;
+      if(key=='+')
+        offset+=0.5;
+      else if(key=='-')
+        offset-=0.5;
+      std::cout << offset << std::endl;
+      tina.mm_set_offset(1,offset);
+    }while(key!='q');
+    sleep(10);
+    tina.mm_disable_power();
     tina.mm_stop();
   }catch(CException &e){
     std::cout << e.what() << std::endl;