diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c626213960026842e233717dde865f1d5ce084ad..1e6bb535f58970ecc144663df0615254c97f4455 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,9 +1,9 @@
 ADD_SUBDIRECTORY(xml)
 
 # edit the following line to add all the source code files of the library
-SET(sources dynamixel_motor.cpp dynamixel_motor_group.cpp dynamixel_motor_exceptions.cpp)
+SET(sources dynamixel_motor.cpp dynamixel_motor_group.cpp dynamixel_registers.cpp dynamixel_motor_exceptions.cpp)
 # edit the following line to add all the header files of the library
-SET(headers dynamixel_motor.h dynamixel_motor_group.h dynamixel_motor_exceptions.h)
+SET(headers dynamixel_motor.h dynamixel_motor_group.h dynamixel_registers.h dynamixel_motor_exceptions.h)
 
 FIND_PACKAGE(iriutils REQUIRED)
 
diff --git a/src/dynamixel_motor.cpp b/src/dynamixel_motor.cpp
index 591e7ecdc158005012d1f74b5d5f388180f4ee21..015703c47489ac6809c56dffc305ce7d9d5d4058 100644
--- a/src/dynamixel_motor.cpp
+++ b/src/dynamixel_motor.cpp
@@ -1,4 +1,5 @@
 #include "dynamixel_motor_exceptions.h"
+#include "dynamixel_registers.h"
 #include "dynamixelexceptions.h"
 #include "dynamixelserver.h"
 #include "eventexceptions.h"
@@ -15,7 +16,7 @@
 #include "xml/dynamixel_motor_cfg_file.hxx"
 #endif
 
-CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id)
+CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id,dyn_version_t version)
 {
   this->info.model="";
   this->info.firmware_ver=(unsigned char)-1;
@@ -41,13 +42,15 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b
   this->config.max_voltage=0.0;
   this->config.min_voltage=0.0;
   this->config.max_torque=0.0;
+  this->config.punch=0x20;
+  this->registers=NULL;
   this->dyn_server=CDynamixelServer::instance();
   this->dynamixel_dev=NULL;
   try{
     this->dyn_server->config_bus(bus_id,baudrate);
     this->info.baudrate=baudrate;
     this->info.bus_id=this->dyn_server->get_bus_serial();
-    this->dynamixel_dev=this->dyn_server->get_device(dev_id);
+    this->dynamixel_dev=this->dyn_server->get_device(dev_id,version);
     this->info.id=dev_id;
     this->get_model();
     this->get_position_range(&this->config.min_angle,&this->config.max_angle);
@@ -70,7 +73,7 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int b
   }
 }
 
-CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id)
+CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id,dyn_version_t version)
 {
   this->info.model="";
   this->info.firmware_ver=(unsigned char)-1;
@@ -96,13 +99,15 @@ CDynamixelMotor::CDynamixelMotor(std::string& cont_id,std::string &bus_id,int ba
   this->config.max_voltage=0.0;
   this->config.min_voltage=0.0;
   this->config.max_torque=0.0;
+  this->config.punch=0x20;
+  this->registers=NULL;
   this->dyn_server=CDynamixelServer::instance();
   this->dynamixel_dev=NULL;
   try{
     this->dyn_server->config_bus(bus_id,baudrate);
     this->info.baudrate=baudrate;
     this->info.bus_id=bus_id;
-    this->dynamixel_dev=this->dyn_server->get_device(dev_id);
+    this->dynamixel_dev=this->dyn_server->get_device(dev_id,version);
     this->info.id=dev_id;
     this->get_model();
     this->get_position_range(&this->config.min_angle,&this->config.max_angle);
@@ -169,27 +174,27 @@ void CDynamixelMotor::reset_motor(void)
   unsigned short int maximum_torque;
 
   try{
-    this->dynamixel_dev->read_word_register(current_pos,&current_position);
+    this->dynamixel_dev->read_word_register(this->registers[current_pos],&current_position);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev->write_word_register(goal_pos,current_position);
+    this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev->read_word_register(max_torque,&maximum_torque);
+    this->dynamixel_dev->read_word_register(this->registers[max_torque],&maximum_torque);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev->write_word_register(torque_limit,maximum_torque);
+    this->dynamixel_dev->write_word_register(this->registers[torque_limit],maximum_torque);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev->write_byte_register(led,0x00);
+    this->dynamixel_dev->write_byte_register(this->registers[led],0x00);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
@@ -207,8 +212,8 @@ void CDynamixelMotor::get_model(void)
   else
   {
     try{
-      this->dynamixel_dev->read_byte_register(firmware_version,&this->info.firmware_ver);
-      this->dynamixel_dev->read_word_register(model_number,&model);
+      this->dynamixel_dev->read_byte_register(std_firmware_version,&this->info.firmware_ver);
+      this->dynamixel_dev->read_word_register(std_model_number,&model);
       switch(model)
       {
 	case 0x000C: this->info.model="AX-12A";
@@ -217,6 +222,7 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=354;
 		     this->info.encoder_resolution=1023;
 		     this->info.gear_ratio=254;
+                     this->registers=std_compl_reg;
 		     this->info.pid_control=false;
 		     break;
 	case 0x012C: this->info.model="AX-12W";
@@ -225,6 +231,7 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=2830;
 		     this->info.encoder_resolution=1023;
 		     this->info.gear_ratio=32;
+                     this->registers=std_compl_reg;
 		     this->info.pid_control=false;
 		     break;
 	case 0x0012: this->info.model="AX-18A";
@@ -233,6 +240,7 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=582;
 		     this->info.encoder_resolution=1023;
 		     this->info.gear_ratio=254;
+                     this->registers=std_compl_reg;
 		     this->info.pid_control=false;
 		     break;
 	case 0x001D: this->info.model="MX-28";
@@ -241,6 +249,7 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=330;
 		     this->info.encoder_resolution=4095;
 		     this->info.gear_ratio=193;
+                     this->registers=std_pid_reg;
 		     this->info.pid_control=true;
 		     break;
 	case 0x0018: this->info.model="RX-24F";
@@ -249,6 +258,7 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=756;
 		     this->info.encoder_resolution=1023;
 		     this->info.gear_ratio=193;
+                     this->registers=std_compl_reg;
 		     this->info.pid_control=false;
 		     break;
 	case 0x001C: this->info.model="RX-28";
@@ -257,6 +267,7 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=402;
 		     this->info.encoder_resolution=1023;
 		     this->info.gear_ratio=193;
+                     this->registers=std_compl_reg;
 		     this->info.pid_control=false;
 		     break;
 	case 0x0136: this->info.model="MX-64";
@@ -265,6 +276,7 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=378;
 		     this->info.encoder_resolution=4095;
 		     this->info.gear_ratio=200;
+                     this->registers=std_pid_reg;
 		     this->info.pid_control=true;
 		     break;
 	case 0x0040: this->info.model="Rx-64";
@@ -273,6 +285,7 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=294;
 		     this->info.encoder_resolution=1023;
 		     this->info.gear_ratio=200;
+                     this->registers=std_compl_reg;
 		     this->info.pid_control=false;
 		     break;
 	case 0x0140: this->info.model="MX-106";
@@ -281,6 +294,7 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=270;
 		     this->info.encoder_resolution=4095;
 		     this->info.gear_ratio=225;
+                     this->registers=std_pid_reg;
 		     this->info.pid_control=true;
 		     break;
 	case 0x006B: this->info.model="Ex-106+";
@@ -289,8 +303,18 @@ void CDynamixelMotor::get_model(void)
 		     this->info.max_speed=414;
 		     this->info.encoder_resolution=4095;
 		     this->info.gear_ratio=184;
+                     this->registers=std_compl_reg;
 		     this->info.pid_control=false;
 		     break;
+	case 0x015E: this->info.model="XL_320";
+		     this->info.max_angle=300.0;
+		     this->info.center_angle=150;
+		     this->info.max_speed=684;
+		     this->info.encoder_resolution=1023;
+		     this->info.gear_ratio=238;
+                     this->registers=xl_reg;
+		     this->info.pid_control=true;
+		     break;
 	default: this->info.model="unknown";
 		 break;
       }
@@ -313,6 +337,43 @@ void CDynamixelMotor::load_config(TDynamixel_config &config)
 }
 
 #ifdef _HAVE_XSD
+void CDynamixelMotor::read_config(std::string &filename,TDynamixel_config &config, TDynamixel_compliance &comp, TDynamixel_pid &pid)
+{
+  struct stat buffer;
+
+  if(stat(filename.c_str(),&buffer)==0)
+  {
+    // try to open the specified file
+    try{
+      std::auto_ptr<dynamixel_motor_config_t> cfg(dynamixel_motor_config(filename.c_str(), xml_schema::flags::dont_validate));
+      // configure the parameters of the controller
+      config.max_angle=cfg->max_angle();
+      config.min_angle=cfg->min_angle();
+      config.max_temperature=cfg->temp_limit();
+      config.max_voltage=cfg->max_voltage();
+      config.min_voltage=cfg->min_voltage();
+      config.max_torque=cfg->max_torque();
+      config.punch=cfg->punch();
+      /* pid control */
+      pid.p=cfg->kp();
+      pid.i=cfg->ki();
+      pid.d=cfg->kd();
+      /* compliance control */
+      comp.cw_compliance_margin=cfg->cw_comp_margin();
+      comp.ccw_compliance_margin=cfg->ccw_comp_margin();
+      comp.cw_compliance_slope=cfg->cw_comp_slope();
+      comp.ccw_compliance_slope=cfg->ccw_comp_slope();
+    }catch (const xml_schema::exception& e){
+      std::ostringstream os;
+      os << e;
+      /* handle exceptions */
+      throw CDynamixelMotorException(_HERE_,os.str());
+    }
+  }
+  else
+    throw CDynamixelMotorException(_HERE_,"The configuration file does not exist");
+}
+
 void CDynamixelMotor::load_config(std::string &filename)
 {
   TDynamixel_compliance compliance;
@@ -342,9 +403,9 @@ void CDynamixelMotor::load_config(std::string &filename)
         compliance.ccw_compliance_margin=cfg->ccw_comp_margin();
         compliance.cw_compliance_slope=cfg->cw_comp_slope();
         compliance.ccw_compliance_slope=cfg->ccw_comp_slope();
-        compliance.punch=cfg->punch();
         this->set_compliance_control(compliance);
       }
+      this->set_punch(cfg->punch());
       this->set_max_torque(cfg->max_torque());
       this->set_limit_torque(cfg->max_torque());
     }catch (const xml_schema::exception& e){
@@ -384,7 +445,7 @@ void CDynamixelMotor::save_config(std::string &filename)
 	compliance.ccw_compliance_margin,
 	compliance.cw_compliance_slope,
 	compliance.ccw_compliance_slope,
-	compliance.punch,
+	this->get_punch(),
 	pid.p,
 	pid.i,
 	pid.d);
@@ -438,8 +499,10 @@ void CDynamixelMotor::set_position_range(double min, double max)
       try{
 	this->config.max_angle=max;
 	this->config.min_angle=min;
-	this->dynamixel_dev->write_word_register(ccw_angle_limit,max_pos);
-	this->dynamixel_dev->write_word_register(cw_angle_limit,min_pos);
+	this->dynamixel_dev->write_word_register(this->registers[ccw_angle_limit],max_pos);
+        usleep(10000);
+	this->dynamixel_dev->write_word_register(this->registers[cw_angle_limit],min_pos);
+        usleep(10000);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms)
@@ -462,9 +525,9 @@ void CDynamixelMotor::get_position_range(double *min, double *max)
   else
   {
     try{
-      this->dynamixel_dev->read_word_register(ccw_angle_limit,&angle_limit);
+      this->dynamixel_dev->read_word_register(this->registers[ccw_angle_limit],&angle_limit);
       (*max)=this->to_angles(angle_limit);
-      this->dynamixel_dev->read_word_register(cw_angle_limit,&angle_limit);
+      this->dynamixel_dev->read_word_register(this->registers[cw_angle_limit],&angle_limit);
       (*min)=this->to_angles(angle_limit);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
@@ -487,7 +550,7 @@ int CDynamixelMotor::get_temperature_limit(void)
   else
   {
     try{
-      this->dynamixel_dev->read_byte_register(temp_limit,&limit);
+      this->dynamixel_dev->read_byte_register(this->registers[temp_limit],&limit);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -517,7 +580,8 @@ void CDynamixelMotor::set_temperature_limit(int limit)
     {
       try{
 	this->config.max_temperature=limit;
-	this->dynamixel_dev->write_byte_register(temp_limit,(unsigned char)limit);
+	this->dynamixel_dev->write_byte_register(this->registers[temp_limit],(unsigned char)limit);
+        usleep(10000);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms)
@@ -540,8 +604,8 @@ void CDynamixelMotor::get_voltage_limits(double *min, double *max)
   else
   {
     try{
-      this->dynamixel_dev->read_byte_register(low_voltage_limit,&min_voltage);
-      this->dynamixel_dev->read_byte_register(high_voltage_limit,&max_voltage);
+      this->dynamixel_dev->read_byte_register(this->registers[low_voltage_limit],&min_voltage);
+      this->dynamixel_dev->read_byte_register(this->registers[high_voltage_limit],&max_voltage);
       *min=((double)min_voltage/10.0);
       *max=((double)max_voltage/10.0);
     }catch(CDynamixelAlarmException &e){
@@ -583,8 +647,10 @@ void CDynamixelMotor::set_voltage_limits(double min, double max)
 	  this->config.max_voltage=max;
 	  min_voltage=min*10;
 	  max_voltage=max*10;
-	  this->dynamixel_dev->write_byte_register(low_voltage_limit,min_voltage);
-	  this->dynamixel_dev->write_byte_register(high_voltage_limit,max_voltage);
+	  this->dynamixel_dev->write_byte_register(this->registers[low_voltage_limit],min_voltage);
+          usleep(10000);
+	  this->dynamixel_dev->write_byte_register(this->registers[high_voltage_limit],max_voltage);
+          usleep(10000);
 	}catch(CDynamixelAlarmException &e){
 	  /* handle dynamixel exception */
 	  if(e.get_alarm()&this->alarms)
@@ -608,7 +674,7 @@ unsigned char CDynamixelMotor::get_led_alarms(void)
   else
   {
     try{
-      this->dynamixel_dev->read_byte_register(alarm_led,&led_alarms);
+      this->dynamixel_dev->read_byte_register(this->registers[alarm_led],&led_alarms);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -637,7 +703,7 @@ void CDynamixelMotor::set_led_alarms(unsigned char alarms)
     else
     {
       try{
-	this->dynamixel_dev->write_byte_register(alarm_led,(unsigned char)alarms);
+	this->dynamixel_dev->write_byte_register(this->registers[alarm_led],(unsigned char)alarms);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms)
@@ -660,7 +726,7 @@ unsigned char CDynamixelMotor::get_turn_off_alarms(void)
   else
   {
     try{
-      this->dynamixel_dev->read_byte_register(alarm_shtdwn, &shutdown_alarms);
+      this->dynamixel_dev->read_byte_register(this->registers[alarm_shtdwn], &shutdown_alarms);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -690,7 +756,8 @@ void CDynamixelMotor::set_turn_off_alarms(unsigned char alarms)
     {
       try{
 	this->alarms=alarms;
-	this->dynamixel_dev->write_byte_register(alarm_shtdwn,(unsigned char)alarms);
+	this->dynamixel_dev->write_byte_register(this->registers[alarm_shtdwn],(unsigned char)alarms);
+        usleep(10000);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms)
@@ -714,7 +781,7 @@ double CDynamixelMotor::get_max_torque(void)
   else
   {
     try{
-      this->dynamixel_dev->read_word_register(max_torque,&load);
+      this->dynamixel_dev->read_word_register(this->registers[max_torque],&load);
       torque=(load&0x3FF)*100.0/1023;
       if(load>0x3FF)
 	torque=-1*torque;
@@ -742,7 +809,7 @@ double CDynamixelMotor::get_limit_torque(void)
   else
   {
     try{
-      this->dynamixel_dev->read_word_register(torque_limit,&load);
+      this->dynamixel_dev->read_word_register(this->registers[torque_limit],&load);
       torque=(load&0x3FF)*100.0/1023;
       if(load>0x3FF)
 	torque=-1*torque;
@@ -777,7 +844,8 @@ void CDynamixelMotor::set_max_torque(double torque_ratio)
       load=torque_ratio*1023/100.0;
       try{
 	this->config.max_torque=torque_ratio;
-	this->dynamixel_dev->write_word_register(max_torque,load);
+	this->dynamixel_dev->write_word_register(this->registers[max_torque],load);
+        usleep(10000);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms)
@@ -807,7 +875,7 @@ void CDynamixelMotor::set_limit_torque(double torque_ratio)
     {
       load=torque_ratio*1023/100.0;
       try{
-	this->dynamixel_dev->write_word_register(torque_limit,load);
+	this->dynamixel_dev->write_word_register(this->registers[torque_limit],load);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms)
@@ -830,11 +898,10 @@ void CDynamixelMotor::get_compliance_control(TDynamixel_compliance &config)
     if(!this->info.pid_control)
     {
       try{
-	this->dynamixel_dev->read_byte_register(cw_comp_margin,&config.cw_compliance_margin);
-	this->dynamixel_dev->read_byte_register(ccw_comp_margin,&config.ccw_compliance_margin);
-	this->dynamixel_dev->read_byte_register(cw_comp_slope,&config.cw_compliance_slope);
-	this->dynamixel_dev->read_byte_register(ccw_comp_slope,&config.ccw_compliance_slope);
-	this->dynamixel_dev->read_byte_register(punch,&config.punch);
+	this->dynamixel_dev->read_byte_register(this->registers[cw_comp_margin],&config.cw_compliance_margin);
+	this->dynamixel_dev->read_byte_register(this->registers[ccw_comp_margin],&config.ccw_compliance_margin);
+	this->dynamixel_dev->read_byte_register(this->registers[cw_comp_slope],&config.cw_compliance_slope);
+	this->dynamixel_dev->read_byte_register(this->registers[ccw_comp_slope],&config.ccw_compliance_slope);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms)
@@ -890,21 +957,15 @@ void CDynamixelMotor::set_compliance_control(TDynamixel_compliance &config)
 	/* handle exceptions */
 	throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
       }
-      if(config.punch<0 || config.punch>1023)
-      {
-	/* handle exceptions */
-	throw CDynamixelMotorException(_HERE_,"Invalid punch value.");
-      }
       try{
 	this->compliance.cw_compliance_margin=config.cw_compliance_margin;
 	this->compliance.ccw_compliance_margin=config.ccw_compliance_margin;
 	this->compliance.cw_compliance_slope=config.cw_compliance_slope;
 	this->compliance.ccw_compliance_slope=config.ccw_compliance_slope;
-	this->dynamixel_dev->write_byte_register(cw_comp_margin,config.cw_compliance_margin);
-	this->dynamixel_dev->write_byte_register(ccw_comp_margin,config.ccw_compliance_margin);
-	this->dynamixel_dev->write_byte_register(cw_comp_slope,config.cw_compliance_slope);
-	this->dynamixel_dev->write_byte_register(ccw_comp_slope,config.ccw_compliance_slope);
-	this->dynamixel_dev->write_word_register(punch,config.punch);
+	this->dynamixel_dev->write_byte_register(this->registers[cw_comp_margin],config.cw_compliance_margin);
+	this->dynamixel_dev->write_byte_register(this->registers[ccw_comp_margin],config.ccw_compliance_margin);
+	this->dynamixel_dev->write_byte_register(this->registers[cw_comp_slope],config.cw_compliance_slope);
+	this->dynamixel_dev->write_byte_register(this->registers[ccw_comp_slope],config.ccw_compliance_slope);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms)
@@ -927,9 +988,9 @@ void CDynamixelMotor::get_pid_control(TDynamixel_pid &config)
     if(this->info.pid_control)
     {
       try{
-	this->dynamixel_dev->read_byte_register(pid_p,&config.p);
-	this->dynamixel_dev->read_byte_register(pid_i,&config.i);
-	this->dynamixel_dev->read_byte_register(pid_d,&config.d);
+	this->dynamixel_dev->read_byte_register(this->registers[pid_p],&config.p);
+	this->dynamixel_dev->read_byte_register(this->registers[pid_i],&config.i);
+	this->dynamixel_dev->read_byte_register(this->registers[pid_d],&config.d);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms)
@@ -954,9 +1015,9 @@ void CDynamixelMotor::set_pid_control(TDynamixel_pid &config)
       this->pid.p=config.p;
       this->pid.i=config.i;
       this->pid.d=config.d;
-      this->dynamixel_dev->write_byte_register(pid_p,config.p);
-      this->dynamixel_dev->write_byte_register(pid_i,config.i);
-      this->dynamixel_dev->write_byte_register(pid_d,config.d);
+      this->dynamixel_dev->write_byte_register(this->registers[pid_p],config.p);
+      this->dynamixel_dev->write_byte_register(this->registers[pid_i],config.i);
+      this->dynamixel_dev->write_byte_register(this->registers[pid_d],config.d);
     }
   }
 }
@@ -971,7 +1032,7 @@ void CDynamixelMotor::turn_led_on(void)
   else
   {
     try{
-      this->dynamixel_dev->write_byte_register(led,1);
+      this->dynamixel_dev->write_byte_register(this->registers[led],1);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -991,7 +1052,7 @@ void CDynamixelMotor::turn_led_off(void)
   else
   {
     try{
-      this->dynamixel_dev->write_byte_register(led,0);
+      this->dynamixel_dev->write_byte_register(this->registers[led],0);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -1021,7 +1082,7 @@ void CDynamixelMotor::enable(void)
   else
   {
     try{
-      this->dynamixel_dev->write_byte_register(torque_en,1);
+      this->dynamixel_dev->write_byte_register(this->registers[torque_en],1);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -1041,7 +1102,7 @@ void CDynamixelMotor::disable(void)
   else
   {
     try{
-      this->dynamixel_dev->write_byte_register(torque_en,0);
+      this->dynamixel_dev->write_byte_register(this->registers[torque_en],0);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -1070,7 +1131,7 @@ void CDynamixelMotor::move_absolute_angle(double angle,double speed)
       }
       cmd[0]=this->from_angles(angle);
       cmd[1]=this->from_speeds(speed);
-      this->dynamixel_dev->write_registers(goal_pos,(unsigned char *)cmd,4);
+      this->dynamixel_dev->write_registers(this->registers[goal_pos],(unsigned char *)cmd,4);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -1097,10 +1158,10 @@ void CDynamixelMotor::move_relative_angle(double angle,double speed)
 	this->set_position_range(-this->info.center_angle,this->info.max_angle-this->info.center_angle);
 	this->mode=angle_ctrl;
       }
-      this->dynamixel_dev->read_word_register(current_pos,&pos);
+      this->dynamixel_dev->read_word_register(this->registers[current_pos],&pos);
       cmd[0]=this->from_angles(angle+this->to_angles(pos));
       cmd[1]=this->from_speeds(speed);
-      this->dynamixel_dev->write_registers(goal_pos,(unsigned char *)cmd,4);
+      this->dynamixel_dev->write_registers(this->registers[goal_pos],(unsigned char *)cmd,4);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -1130,7 +1191,7 @@ void CDynamixelMotor::move_torque(double torque_ratio)
       if(torque_ratio<0.0)
 	torque+=0x0400;
       torque+=((unsigned short int)(fabs(torque_ratio)*1023.0/100.0))&0x03FF;
-      this->dynamixel_dev->write_word_register(goal_speed,torque);
+      this->dynamixel_dev->write_word_register(this->registers[goal_speed],torque);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -1154,11 +1215,11 @@ void CDynamixelMotor::stop(void)
     try{
       if(this->mode==angle_ctrl)
       {
-	this->dynamixel_dev->read_word_register(current_pos,&current_position);
-	this->dynamixel_dev->write_word_register(goal_pos,current_position);
+	this->dynamixel_dev->read_word_register(this->registers[current_pos],&current_position);
+	this->dynamixel_dev->write_word_register(this->registers[goal_pos],current_position);
       }
       else
-	this->dynamixel_dev->write_word_register(goal_speed,0);
+	this->dynamixel_dev->write_word_register(this->registers[goal_speed],0);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms)
@@ -1181,7 +1242,7 @@ double CDynamixelMotor::get_current_angle(void)
   else
   {
     try{
-      this->dynamixel_dev->read_word_register(current_pos,&data);
+      this->dynamixel_dev->read_word_register(this->registers[current_pos],&data);
       current_position = this->to_angles(data);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
@@ -1207,7 +1268,7 @@ double CDynamixelMotor::get_current_speed(void)
   else
   {
     try{
-      this->dynamixel_dev->read_word_register(current_speed,&data);
+      this->dynamixel_dev->read_word_register(this->registers[current_speed],&data);
       c_speed = this->to_speeds(data);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
@@ -1233,7 +1294,7 @@ double CDynamixelMotor::get_current_temperature(void)
   else
   {
     try{
-      this->dynamixel_dev->read_byte_register(current_temp,&data);
+      this->dynamixel_dev->read_byte_register(this->registers[current_temp],&data);
       c_temp = (double)data;
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
@@ -1259,7 +1320,7 @@ double CDynamixelMotor::get_current_voltage(void)
   else
   {
     try{
-      this->dynamixel_dev->read_byte_register(current_voltage,&data);
+      this->dynamixel_dev->read_byte_register(this->registers[current_voltage],&data);
       c_voltage = (double)data/10;
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
@@ -1285,7 +1346,7 @@ double CDynamixelMotor::get_current_effort(void)
   else
   {
     try{
-      this->dynamixel_dev->read_byte_register(current_load,&data);
+      this->dynamixel_dev->read_byte_register(this->registers[current_load],&data);
       c_effort = (double)((data&0x3FF)*100.0/1023);
       if (this->get_current_speed() < 0)
       {
@@ -1302,6 +1363,53 @@ double CDynamixelMotor::get_current_effort(void)
   return c_effort;
 }
 
+unsigned short int CDynamixelMotor::get_punch(void)
+{  
+  unsigned short int value;
+
+  if(this->dynamixel_dev==NULL)
+  {
+    /* handle exceptions */
+    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+  }
+  else
+  {
+    try{
+      this->dynamixel_dev->read_word_register(this->registers[punch],&value);
+    }catch(CDynamixelAlarmException &e){
+      /* handle dynamixel exception */
+      if(e.get_alarm()&this->alarms)
+	this->reset_motor();
+      throw;
+    }
+  }
+
+  return value;
+}
+
+void CDynamixelMotor::set_punch(unsigned short int punch_value)
+{
+  if(this->dynamixel_dev==NULL)
+  {
+    /* handle exceptions */
+    throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+  }
+  else
+  {
+    try{
+      if(punch<0x0020 || punch>0x03FF) 
+        throw CDynamixelMotorException(_HERE_,"Invalid Punch value");
+      this->config.punch=punch;
+      this->dynamixel_dev->write_word_register(this->registers[punch],punch);
+    }catch(CDynamixelAlarmException &e){
+      /* handle dynamixel exception */
+      if(e.get_alarm()&this->alarms)
+	this->reset_motor();
+      throw;
+    }
+  }
+}
+
 CDynamixelMotor::~CDynamixelMotor()
 {
   /* stop the motor */
diff --git a/src/dynamixel_motor.h b/src/dynamixel_motor.h
index 3d315201ce00e4af5b476f5c409d1824ba11d721..ba52bca56d8a3439c974aa4ece4a048ec52d6006 100644
--- a/src/dynamixel_motor.h
+++ b/src/dynamixel_motor.h
@@ -12,49 +12,6 @@
 #include <vector>
 #include <memory>
 
-typedef enum {
-  //                           [Access] [Description]
-  // EEPROM Area 
-  model_number=0x00,        //   (R)     Model Number
-  firmware_version=0x02,    //   (R)     Version of the Firmware
-  id=0x03,                  //   (RW)    ID of Dynamixel
-  baudrate=0x04,            //   (RW)    Baud Rate of Dynamixel
-  return_delay_time=0x05,   //   (RW)    Return Delay Time 
-  cw_angle_limit=0x06,      //   (RW)    Clockwise Angle Limit
-  ccw_angle_limit=0x08,     //   (RW)    Counterclockwise Angle Limit 
-  temp_limit=0x0B,          //   (RW)    Internal Temperature Limit
-  low_voltage_limit=0x0C,   //   (RW)    Lowest Voltage Limit
-  high_voltage_limit=0x0D,  //   (RW)    Highest Voltage Limit
-  max_torque=0x0E,          //   (RW)    Maximum Torque 
-  return_level=0x10,        //   (RW)    Status Return Level
-  alarm_led=0x11,           //   (RW)    LED for Alarm 
-  alarm_shtdwn=0x12,        //   (RW)    Shutdown for Alarm 
-  down_cal=0x14,            //   (RW)     
-  up_cal=0x16,              //   (RW)    
-  // RAM Area 
-  torque_en=0x18,           //   (RW)    Torque On/Off 
-  led=0x19,                 //   (RW)    LED On/Off 
-  pid_p=0x1A,               //   (RW)     
-  pid_i=0x1B,               //   (RW)     
-  pid_d=0x1C,               //   (RW)     
-  cw_comp_margin=0x1A,      //   (RW)    CW Compliance Margin
-  ccw_comp_margin=0x1B,     //   (RW)    CCW Compliance Margin
-  cw_comp_slope=0x1C,       //   (RW)    CW Compliance Slope
-  ccw_comp_slope=0x1D,      //   (RW)    CCW Compliance Slope 
-  goal_pos=0x1E,            //   (RW)    Goal Position 
-  goal_speed=0x20,          //   (RW)    Moving Speed 
-  torque_limit=0x22,        //   (RW)    Torque Limit 
-  current_pos=0x24,         //   (R)     Current Position 
-  current_speed=0x26,       //   (R)     Current Speed 
-  current_load=0x28,        //   (R)     Current Load 
-  current_voltage=0x2A,     //   (R)     Current Voltage 
-  current_temp=0x2B,        //   (RW)    Current Temperature 
-  reg_inst=0x2C,            //   (RW)    Means if Instruction is registered 
-  moving=0x2E,              //   (R)     Means if there is any movement 
-  lock=0x2F,                //   (RW)    Locking EEPROM 
-  punch=0x30                //   (RW)    Punch 
-} reg_id;
-
 typedef enum
 {
   input_voltage_error=0x01,
@@ -87,7 +44,6 @@ typedef struct
   unsigned char ccw_compliance_margin;
   unsigned char cw_compliance_slope;
   unsigned char ccw_compliance_slope;
-  unsigned char punch;
 }TDynamixel_compliance;
 
 typedef struct
@@ -105,6 +61,7 @@ typedef struct
   double max_voltage;
   double min_voltage;
   double max_torque;
+  unsigned short int punch;
 }TDynamixel_config;
 
 typedef enum{angle_ctrl=0,torque_ctrl=1} control_mode;
@@ -152,7 +109,11 @@ class CDynamixelMotor
      *
      */
     control_mode mode;
-
+    /**
+     * \brief
+     *
+     */
+    unsigned short int *registers;
   protected:
     /**
      * \brief
@@ -190,12 +151,12 @@ class CDynamixelMotor
      * \brief
      *
      */ 
-    CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id);
+    CDynamixelMotor(std::string& cont_id,unsigned char bus_id,int baudrate,unsigned char dev_id,dyn_version_t version=dyn_version1);
     /**
      * \brief
      *
      */ 
-    CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id);
+    CDynamixelMotor(std::string& cont_id,std::string &bus_id,int baudrate,unsigned char dev_id,dyn_version_t version=dyn_version1);
     /**
      * \brief
      *
@@ -208,6 +169,11 @@ class CDynamixelMotor
      */
     void load_config(TDynamixel_config &config);
 #ifdef _HAVE_XSD
+    /**
+     * \brief 
+     *
+     */
+    static void read_config(std::string &filename,TDynamixel_config &config, TDynamixel_compliance &comp, TDynamixel_pid &pid);
     /**
      * \brief 
      *
@@ -385,6 +351,16 @@ class CDynamixelMotor
      *  
      */ 
     double get_current_temperature(void);
+    /**
+     * \brief 
+     *  
+     */ 
+    unsigned short int get_punch(void);
+    /**
+     * \brief 
+     *  
+     */ 
+    void set_punch(unsigned short int punch_value);
     /**
      * \brief 
      *  
diff --git a/src/dynamixel_motor_group.cpp b/src/dynamixel_motor_group.cpp
index 8cd9adf0d12a289f4495323cf8bd2ea82ce09fae..c0904f58b228c0d80a75c2b2dba5d0c593909d99 100644
--- a/src/dynamixel_motor_group.cpp
+++ b/src/dynamixel_motor_group.cpp
@@ -1,4 +1,5 @@
 #include "dynamixel_motor_exceptions.h"
+#include "dynamixel_registers.h"
 #include "dynamixelexceptions.h"
 #include "dynamixelserver.h"
 #include "eventexceptions.h"
@@ -39,7 +40,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id)
   }
 }
 
-CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids)
+CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids,dyn_version_t version)
 {
   unsigned int i=0;
 
@@ -66,7 +67,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
     try{
       this->dyn_server->config_bus(bus_id,baudrate);
       for(i=0;i<ids.size();i++)
-	this->init_motor(ids[i]);
+	this->init_motor(ids[i],version);
       if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
       {
 	this->mode=torque_ctrl;
@@ -89,7 +90,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,unsigned bus_id
   }
 }
 
-CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids)
+CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids,dyn_version_t version)
 {
   unsigned int i=0;
 
@@ -111,7 +112,7 @@ CDynamixelMotorGroup::CDynamixelMotorGroup(std::string &group_id,std::string &bu
     try{
       this->dyn_server->config_bus(bus_id,baudrate);
       for(i=0;i<ids.size();i++)
-	this->init_motor(ids[i]);
+	this->init_motor(ids[i],version);
       if(this->config[0].min_angle==-this->info[0].center_angle && this->config[0].max_angle==-this->info[0].center_angle)
       {
 	this->mode=torque_ctrl;
@@ -178,27 +179,27 @@ void CDynamixelMotorGroup::reset_motor(unsigned int index)
   unsigned short int maximum_torque;
 
   try{
-    this->dynamixel_dev[index]->read_word_register(current_pos,&current_position);
+    this->dynamixel_dev[index]->read_word_register(this->registers[index][current_pos],&current_position);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev[index]->write_word_register(goal_pos,current_position);
+    this->dynamixel_dev[index]->write_word_register(this->registers[index][goal_pos],current_position);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev[index]->read_word_register(max_torque,&maximum_torque);
+    this->dynamixel_dev[index]->read_word_register(this->registers[index][max_torque],&maximum_torque);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev[index]->write_word_register(torque_limit,maximum_torque);
+    this->dynamixel_dev[index]->write_word_register(this->registers[index][torque_limit],maximum_torque);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
   try{
-    this->dynamixel_dev[index]->write_byte_register(led,0x00);
+    this->dynamixel_dev[index]->write_byte_register(this->registers[index][led],0x00);
   }catch(CDynamixelAlarmException &e){
     /* do nothing - expected exception */
   }
@@ -216,8 +217,8 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
   else
   {
     try{
-      this->dynamixel_dev[index]->read_byte_register(firmware_version,&this->info[index].firmware_ver);
-      this->dynamixel_dev[index]->read_word_register(model_number,&model);
+      this->dynamixel_dev[index]->read_byte_register(std_firmware_version,&this->info[index].firmware_ver);
+      this->dynamixel_dev[index]->read_word_register(std_model_number,&model);
       switch(model)
       {
 	case 0x000C: this->info[index].model="AX-12A";
@@ -226,6 +227,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=354.0;
 		     this->info[index].encoder_resolution=1023;
 		     this->info[index].gear_ratio=254;
+                     this->registers[index]=std_compl_reg;
 		     this->info[index].pid_control=false;
 		     break;
 	case 0x012C: this->info[index].model="AX-12W";
@@ -234,6 +236,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=2830;
 		     this->info[index].encoder_resolution=1023;
 		     this->info[index].gear_ratio=32;
+                     this->registers[index]=std_compl_reg;
 		     this->info[index].pid_control=false;
 		     break;
 	case 0x0012: this->info[index].model="AX-18A";
@@ -242,6 +245,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=582;
 		     this->info[index].encoder_resolution=1023;
 		     this->info[index].gear_ratio=254;
+                     this->registers[index]=std_compl_reg;
 		     this->info[index].pid_control=false;
 		     break;
 	case 0x001D: this->info[index].model="MX-28";
@@ -250,6 +254,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=330;
 		     this->info[index].encoder_resolution=4095;
 		     this->info[index].gear_ratio=193;
+                     this->registers[index]=std_pid_reg;
 		     this->info[index].pid_control=true;
 		     break;
 	case 0x0018: this->info[index].model="RX-24F";
@@ -258,6 +263,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=756;
 		     this->info[index].encoder_resolution=1023;
 		     this->info[index].gear_ratio=193;
+                     this->registers[index]=std_compl_reg;
 		     this->info[index].pid_control=false;
 		     break;
 	case 0x001C: this->info[index].model="RX-28";
@@ -266,6 +272,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=402;
 		     this->info[index].encoder_resolution=1023;
 		     this->info[index].gear_ratio=193;
+                     this->registers[index]=std_compl_reg;
 		     this->info[index].pid_control=false;
 		     break;
 	case 0x0136: this->info[index].model="MX-64";
@@ -274,6 +281,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=378;
 		     this->info[index].encoder_resolution=4095;
 		     this->info[index].gear_ratio=200;
+                     this->registers[index]=std_pid_reg;
 		     this->info[index].pid_control=true;
 		     break;
 	case 0x0040: this->info[index].model="Rx-64";
@@ -282,6 +290,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=294;
 		     this->info[index].encoder_resolution=1023;
 		     this->info[index].gear_ratio=200;
+                     this->registers[index]=std_compl_reg;
 		     this->info[index].pid_control=false;
 		     break;
 	case 0x0140: this->info[index].model="MX-106";
@@ -290,6 +299,7 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=270;
 		     this->info[index].encoder_resolution=4095;
 		     this->info[index].gear_ratio=225;
+                     this->registers[index]=std_pid_reg;
 		     this->info[index].pid_control=true;
 		     break;
 	case 0x006B: this->info[index].model="Ex-106+";
@@ -298,7 +308,17 @@ void CDynamixelMotorGroup::get_model(unsigned int index)
 		     this->info[index].max_speed=414;
 		     this->info[index].encoder_resolution=4095;
 		     this->info[index].gear_ratio=184;
+                     this->registers[index]=std_compl_reg;
 		     this->info[index].pid_control=false;
+                     break;
+        case 0x015E: this->info[index].model="XL_320";
+                     this->info[index].max_angle=300.0;
+                     this->info[index].center_angle=150;
+                     this->info[index].max_speed=684;
+                     this->info[index].encoder_resolution=1023;
+                     this->info[index].gear_ratio=238;
+                     this->registers[index]=xl_reg;
+                     this->info[index].pid_control=true;
 		     break;
 	default: this->info[index].model="unknown";
 		 break;
@@ -335,8 +355,10 @@ void CDynamixelMotorGroup::set_position_range(unsigned int index,double min, dou
       try{
 	this->config[index].max_angle=max;
 	this->config[index].min_angle=min;
-	this->dynamixel_dev[index]->write_word_register(ccw_angle_limit,max_pos);
-	this->dynamixel_dev[index]->write_word_register(cw_angle_limit,min_pos);
+	this->dynamixel_dev[index]->write_word_register(this->registers[index][ccw_angle_limit],max_pos);
+        usleep(10000);
+	this->dynamixel_dev[index]->write_word_register(this->registers[index][cw_angle_limit],min_pos);
+        usleep(10000);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms[index])
@@ -359,9 +381,9 @@ void CDynamixelMotorGroup::get_position_range(unsigned int index,double *min, do
   else
   {
     try{
-      this->dynamixel_dev[index]->read_word_register(ccw_angle_limit,&angle_limit);
+      this->dynamixel_dev[index]->read_word_register(this->registers[index][ccw_angle_limit],&angle_limit);
       (*max)=this->to_angles(index,angle_limit);
-      this->dynamixel_dev[index]->read_word_register(cw_angle_limit,&angle_limit);
+      this->dynamixel_dev[index]->read_word_register(this->registers[index][cw_angle_limit],&angle_limit);
       (*min)=this->to_angles(index,angle_limit);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
@@ -390,7 +412,8 @@ void CDynamixelMotorGroup::set_temperature_limit(unsigned int index,int limit)
     {
       try{
 	this->config[index].max_temperature=limit;
-	this->dynamixel_dev[index]->write_byte_register(temp_limit,(unsigned char)limit);
+	this->dynamixel_dev[index]->write_byte_register(this->registers[index][temp_limit],(unsigned char)limit);
+        usleep(10000);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms[index])
@@ -413,7 +436,7 @@ int CDynamixelMotorGroup::get_temperature_limit(unsigned int index)
   else
   {
     try{
-      this->dynamixel_dev[index]->read_byte_register(temp_limit,&limit);
+      this->dynamixel_dev[index]->read_byte_register(this->registers[index][temp_limit],&limit);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms[index])
@@ -455,8 +478,10 @@ void CDynamixelMotorGroup::set_voltage_limits(unsigned int index,double min, dou
 	  this->config[index].max_voltage=max;
 	  min_voltage=min*10;
 	  max_voltage=max*10;
-	  this->dynamixel_dev[index]->write_byte_register(low_voltage_limit,min_voltage);
-	  this->dynamixel_dev[index]->write_byte_register(high_voltage_limit,max_voltage);
+	  this->dynamixel_dev[index]->write_byte_register(this->registers[index][low_voltage_limit],min_voltage);
+          usleep(10000);
+	  this->dynamixel_dev[index]->write_byte_register(this->registers[index][high_voltage_limit],max_voltage);
+          usleep(10000);
 	}catch(CDynamixelAlarmException &e){
 	  /* handle dynamixel exception */
 	  if(e.get_alarm()&this->alarms[index])
@@ -480,8 +505,8 @@ void CDynamixelMotorGroup::get_voltage_limits(unsigned int index,double *min, do
   else
   {
     try{
-      this->dynamixel_dev[index]->read_byte_register(low_voltage_limit,&min_voltage);
-      this->dynamixel_dev[index]->read_byte_register(high_voltage_limit,&max_voltage);
+      this->dynamixel_dev[index]->read_byte_register(this->registers[index][low_voltage_limit],&min_voltage);
+      this->dynamixel_dev[index]->read_byte_register(this->registers[index][high_voltage_limit],&max_voltage);
       *min=((double)min_voltage/10.0);
       *max=((double)max_voltage/10.0);
     }catch(CDynamixelAlarmException &e){
@@ -505,7 +530,7 @@ unsigned char CDynamixelMotorGroup::get_turn_off_alarms(unsigned int index)
   else
   {
     try{
-      this->dynamixel_dev[index]->read_byte_register(alarm_shtdwn,&shutdown_alarms);
+      this->dynamixel_dev[index]->read_byte_register(this->registers[index][alarm_shtdwn],&shutdown_alarms);
     }catch(CDynamixelAlarmException &e){
       /* handle dynamixel exception */
       if(e.get_alarm()&this->alarms[index])
@@ -537,7 +562,8 @@ void CDynamixelMotorGroup::set_max_torque(unsigned int index,double torque_ratio
       load=torque_ratio*1023/100.0;
       try{
 	this->config[index].max_torque=torque_ratio;
-	this->dynamixel_dev[index]->write_word_register(max_torque,load);
+	this->dynamixel_dev[index]->write_word_register(this->registers[index][max_torque],load);
+        usleep(10000);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms[index])
@@ -567,7 +593,7 @@ void CDynamixelMotorGroup::set_limit_torque(unsigned int index,double torque_rat
     {
       load=torque_ratio*1023/100.0;
       try{
-	this->dynamixel_dev[index]->write_word_register(torque_limit,load);
+	this->dynamixel_dev[index]->write_word_register(this->registers[index][torque_limit],load);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms[index])
@@ -591,7 +617,7 @@ double CDynamixelMotorGroup::get_max_torque(unsigned int index)
   else
   {
     try{
-      this->dynamixel_dev[index]->read_word_register(max_torque,&load);
+      this->dynamixel_dev[index]->read_word_register(this->registers[index][max_torque],&load);
       torque=(load&0x3FF)*100.0/1023;
       if(load>0x3FF)
 	torque=-1*torque;
@@ -618,11 +644,10 @@ void CDynamixelMotorGroup::get_compliance_control(unsigned int index,TDynamixel_
     if(!this->info[index].pid_control)
     {
       try{
-	this->dynamixel_dev[index]->read_byte_register(cw_comp_margin,&config.cw_compliance_margin);
-	this->dynamixel_dev[index]->read_byte_register(ccw_comp_margin,&config.ccw_compliance_margin);
-	this->dynamixel_dev[index]->read_byte_register(cw_comp_slope,&config.cw_compliance_slope);
-	this->dynamixel_dev[index]->read_byte_register(ccw_comp_slope,&config.ccw_compliance_slope);
-	this->dynamixel_dev[index]->read_byte_register(punch,&config.punch);
+	this->dynamixel_dev[index]->read_byte_register(this->registers[index][cw_comp_margin],&config.cw_compliance_margin);
+	this->dynamixel_dev[index]->read_byte_register(this->registers[index][ccw_comp_margin],&config.ccw_compliance_margin);
+	this->dynamixel_dev[index]->read_byte_register(this->registers[index][cw_comp_slope],&config.cw_compliance_slope);
+	this->dynamixel_dev[index]->read_byte_register(this->registers[index][ccw_comp_slope],&config.ccw_compliance_slope);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms[index])
@@ -645,9 +670,9 @@ void CDynamixelMotorGroup::get_pid_control(unsigned int index,TDynamixel_pid &co
     if(this->info[index].pid_control)
     {
       try{
-	this->dynamixel_dev[index]->read_byte_register(pid_p,&config.p);
-	this->dynamixel_dev[index]->read_byte_register(pid_i,&config.i);
-	this->dynamixel_dev[index]->read_byte_register(pid_d,&config.d);
+	this->dynamixel_dev[index]->read_byte_register(this->registers[index][pid_p],&config.p);
+	this->dynamixel_dev[index]->read_byte_register(this->registers[index][pid_i],&config.i);
+	this->dynamixel_dev[index]->read_byte_register(this->registers[index][pid_d],&config.d);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms[index])
@@ -744,7 +769,7 @@ void *CDynamixelMotorGroup::sequence_thread(void *param)
   pthread_exit(NULL);
 }
 
-void CDynamixelMotorGroup::init_motor(unsigned int id)
+void CDynamixelMotorGroup::init_motor(unsigned int id,dyn_version_t version)
 {
   TDynamixel_info info;
   TDynamixel_config config;
@@ -754,10 +779,11 @@ void CDynamixelMotorGroup::init_motor(unsigned int id)
 
   info.baudrate=this->dyn_server->get_baudrate();
   info.bus_id=this->dyn_server->get_bus_serial();
-  dynamixel_dev=this->dyn_server->get_device(id);
+  dynamixel_dev=this->dyn_server->get_device(id,version);
   this->dynamixel_dev.push_back(dynamixel_dev);
   info.id=id;
   this->info.push_back(info);
+  this->registers.push_back(NULL);
   this->get_model(this->servo_id.size());
   this->get_position_range(this->servo_id.size(),&config.min_angle,&config.max_angle);
   config.max_temperature=this->get_temperature_limit(this->servo_id.size());
@@ -817,6 +843,7 @@ void CDynamixelMotorGroup::clear(void)
   this->dynamixel_dev.clear();
   this->compliance.clear();
   this->pid.clear();
+  this->registers.clear();
   this->config.clear();
   this->info.clear();
   this->alarms.clear();
@@ -876,6 +903,7 @@ int CDynamixelMotorGroup::load_motion(unsigned int step_id)
 void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
 {
   std::vector<TDynamixel_compliance> compliance;
+  std::vector<unsigned short int> punch;
   std::vector<TDynamixel_pid> pid;
   unsigned int i=0;
 
@@ -883,10 +911,11 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
   try{
     this->dyn_server->config_bus(config.bus_id,config.baudrate);
     pid.resize(config.id.size());
+    punch.resize(config.id.size());
     compliance.resize(config.id.size());
     for(i=0;i<config.id.size();i++)
     {
-      this->init_motor(config.id[i]);
+      this->init_motor(config.id[i],config.dyn_version);
       // load the configuration file
       this->set_position_range(i,config.dyn_config[i].min_angle,config.dyn_config[i].max_angle);
       this->set_temperature_limit(i,config.dyn_config[i].max_temperature);
@@ -903,8 +932,8 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
 	compliance[i].ccw_compliance_margin=config.compliance_control[i].ccw_compliance_margin;
 	compliance[i].cw_compliance_slope=config.compliance_control[i].cw_compliance_slope;
 	compliance[i].ccw_compliance_slope=config.compliance_control[i].ccw_compliance_slope;
-	compliance[i].punch=config.compliance_control[i].punch;
       }
+      punch[i]=config.dyn_config[i].punch;
       this->set_max_torque(i,config.dyn_config[i].max_torque);
       this->set_limit_torque(i,config.dyn_config[i].max_torque);
     }
@@ -920,6 +949,7 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
       for(i=1;i<this->servo_id.size();i++)
 	this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
     }
+    this->set_punch(punch);
     this->set_pid_control(pid);
     this->set_compliance_control(compliance);
   }catch(CException &e){
@@ -929,10 +959,51 @@ void CDynamixelMotorGroup::load_config(TDynamixelGroup_config &config)
 }
 
 #ifdef _HAVE_XSD
+TDynamixelGroup_config CDynamixelMotorGroup::read_config(std::string &filename)
+{
+  dyn_motor_group_config_t::dyn_motor_config_iterator iterator;
+  TDynamixelGroup_config config;
+  std::string full_path,path;
+  struct stat buffer;
+  size_t found;
+  unsigned int i=0;
+
+  if(stat(filename.c_str(),&buffer)==0)
+  {
+    // try to open the specified file
+    try{
+      std::auto_ptr<dyn_motor_group_config_t> cfg(dyn_motor_group_config(filename.c_str(), xml_schema::flags::dont_validate));
+      config.bus_id=cfg->bus_id();
+      config.baudrate=cfg->baudrate();
+      config.dyn_config.resize(cfg->dyn_motor_config().size());
+      config.pid_control.resize(cfg->dyn_motor_config().size());
+      config.compliance_control.resize(cfg->dyn_motor_config().size());
+      found=filename.find_last_of("/");
+      path=filename.substr(0,found+1);
+      for(iterator=cfg->dyn_motor_config().begin(),i=0;iterator!=cfg->dyn_motor_config().end();iterator++,i++)
+      {
+        config.bus_id[i]=iterator->id();
+        full_path=path+iterator->config_file();
+        CDynamixelMotor::read_config(full_path,config.dyn_config[i],config.compliance_control[i],config.pid_control[i]);
+      }
+    }catch (const xml_schema::exception& e){
+      std::ostringstream os;
+      os << e;
+      /* handle exceptions */
+      throw CDynamixelMotorException(_HERE_,os.str());
+    }
+  }
+  else
+    throw CDynamixelMotorException(_HERE_,"The configuration file does not exist");
+
+  return config;
+}
+
 void CDynamixelMotorGroup::load_config(std::string &filename)
 {
   dyn_motor_group_config_t::dyn_motor_config_iterator iterator;
   std::vector<TDynamixel_compliance> compliance;
+  std::vector<unsigned short int> punch;
   std::vector<TDynamixel_pid> pid;
   std::string full_path,path;
   struct stat buffer;
@@ -948,12 +1019,16 @@ void CDynamixelMotorGroup::load_config(std::string &filename)
       std::auto_ptr<dyn_motor_group_config_t> cfg(dyn_motor_group_config(filename.c_str(), xml_schema::flags::dont_validate));
       this->dyn_server->config_bus(cfg->bus_id(),cfg->baudrate());
       pid.resize(cfg->dyn_motor_config().size());
+      punch.resize(cfg->dyn_motor_config().size());
       compliance.resize(cfg->dyn_motor_config().size());
       found=filename.find_last_of("/");
       path=filename.substr(0,found+1);
       for(iterator=cfg->dyn_motor_config().begin(),i=0;iterator!=cfg->dyn_motor_config().end();iterator++,i++)
       {
-        this->init_motor(iterator->id());
+        if(cfg->dynamixel_version().present())
+          this->init_motor(iterator->id(),(dyn_version_t)cfg->dynamixel_version().get());
+        else
+          this->init_motor(iterator->id());
         // load the configuration file
         full_path=path+iterator->config_file();
         std::auto_ptr<dynamixel_motor_config_t> motor(dynamixel_motor_config(full_path.c_str(),xml_schema::flags::dont_validate));
@@ -972,8 +1047,8 @@ void CDynamixelMotorGroup::load_config(std::string &filename)
           compliance[i].ccw_compliance_margin=motor->ccw_comp_margin();
           compliance[i].cw_compliance_slope=motor->cw_comp_slope();
           compliance[i].ccw_compliance_slope=motor->ccw_comp_slope();
-          compliance[i].punch=motor->punch();
         }
+        punch[i]=motor->punch();
         this->set_max_torque(i,motor->max_torque());
         this->set_limit_torque(i,motor->max_torque());
       }  
@@ -989,6 +1064,7 @@ void CDynamixelMotorGroup::load_config(std::string &filename)
         for(i=1;i<this->servo_id.size();i++)
           this->set_position_range(i,-this->info[i].center_angle,this->info[i].max_angle-this->info[i].center_angle);
       }
+      this->set_punch(punch);
       this->set_pid_control(pid);
       this->set_compliance_control(compliance);
     }catch (const xml_schema::exception& e){
@@ -1161,21 +1237,15 @@ void CDynamixelMotorGroup::set_compliance_control(std::vector<TDynamixel_complia
 	  /* handle exceptions */
 	  throw CDynamixelMotorException(_HERE_,"Invalid counterclockwise compliance slope.");
 	}
-	if(config[i].punch<0 || config[i].punch>1023)
-	{
-	  /* handle exceptions */
-	  throw CDynamixelMotorException(_HERE_,"Invalid punch value.");
-	}
 	try{
 	  this->compliance[i].cw_compliance_margin=config[i].cw_compliance_margin;
 	  this->compliance[i].ccw_compliance_margin=config[i].ccw_compliance_margin;
 	  this->compliance[i].cw_compliance_slope=config[i].cw_compliance_slope;
 	  this->compliance[i].ccw_compliance_slope=config[i].ccw_compliance_slope;
-	  this->dynamixel_dev[i]->write_byte_register(cw_comp_margin,config[i].cw_compliance_margin);
-	  this->dynamixel_dev[i]->write_byte_register(ccw_comp_margin,config[i].ccw_compliance_margin);
-	  this->dynamixel_dev[i]->write_byte_register(cw_comp_slope,config[i].cw_compliance_slope);
-	  this->dynamixel_dev[i]->write_byte_register(ccw_comp_slope,config[i].ccw_compliance_slope);
-	  this->dynamixel_dev[i]->write_word_register(punch,config[i].punch);
+	  this->dynamixel_dev[i]->write_byte_register(this->registers[i][cw_comp_margin],config[i].cw_compliance_margin);
+	  this->dynamixel_dev[i]->write_byte_register(this->registers[i][ccw_comp_margin],config[i].ccw_compliance_margin);
+	  this->dynamixel_dev[i]->write_byte_register(this->registers[i][cw_comp_slope],config[i].cw_compliance_slope);
+	  this->dynamixel_dev[i]->write_byte_register(this->registers[i][ccw_comp_slope],config[i].ccw_compliance_slope);
 	}catch(CDynamixelAlarmException &e){
 	  /* handle dynamixel exception */
 	  if(e.get_alarm()&this->alarms[i])
@@ -1205,9 +1275,9 @@ void CDynamixelMotorGroup::set_pid_control(std::vector<TDynamixel_pid> &config)
 	this->pid[i].p=config[i].p;
 	this->pid[i].i=config[i].i;
 	this->pid[i].d=config[i].d;
-	this->dynamixel_dev[i]->write_byte_register(pid_p,config[i].p);
-	this->dynamixel_dev[i]->write_byte_register(pid_i,config[i].i);
-	this->dynamixel_dev[i]->write_byte_register(pid_d,config[i].d);
+	this->dynamixel_dev[i]->write_byte_register(this->registers[i][pid_p],config[i].p);
+	this->dynamixel_dev[i]->write_byte_register(this->registers[i][pid_i],config[i].i);
+	this->dynamixel_dev[i]->write_byte_register(this->registers[i][pid_d],config[i].d);
       }
     }
   }
@@ -1227,7 +1297,7 @@ void CDynamixelMotorGroup::enable(void)
     else
     {
       try{
-	this->dynamixel_dev[i]->write_byte_register(torque_en,1);
+	this->dynamixel_dev[i]->write_byte_register(this->registers[i][torque_en],1);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms[i])
@@ -1252,7 +1322,7 @@ void CDynamixelMotorGroup::disable(void)
     else
     {
       try{
-	this->dynamixel_dev[i]->write_byte_register(torque_en,0);
+	this->dynamixel_dev[i]->write_byte_register(this->registers[i][torque_en],0);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
 	if(e.get_alarm()&this->alarms[i])
@@ -1287,7 +1357,7 @@ void CDynamixelMotorGroup::move_absolute_angle(std::vector<double> &angles,std::
       data[i][2]=((int)(cmd[1])%256);
       data[i][3]=(int)(cmd[1]/256);
     }
-    this->dyn_server->write_sync(this->servo_id, goal_pos, data);
+    this->dyn_server->write_sync(this->servo_id, this->registers[0][goal_pos], data);
   }catch(CDynamixelAlarmException &e){
     /* handle dynamixel exception */
     if(e.get_alarm()&this->alarms[i])
@@ -1319,7 +1389,7 @@ void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std::
       }
       else
       {
-	this->dynamixel_dev[i]->read_word_register(current_pos,&pos);
+	this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],&pos);
 	data[i].resize(4);
 	cmd[0]=this->from_angles(i,angles[i]+this->to_angles(i,pos));
 	cmd[1]=this->from_speeds(i,speeds[i]);
@@ -1329,7 +1399,7 @@ void CDynamixelMotorGroup::move_relative_angle(std::vector<double> &angles,std::
 	data[i][3]=(int)(cmd[1]/256);
       }
     }
-    this->dyn_server->write_sync(this->servo_id,goal_pos,data);
+    this->dyn_server->write_sync(this->servo_id,this->registers[0][goal_pos],data);
   }catch(CDynamixelAlarmException &e){
     /* handle dynamixel exception */
     if(e.get_alarm()&this->alarms[i])
@@ -1370,7 +1440,7 @@ void CDynamixelMotorGroup::move_torque(std::vector<double> &torque_ratios)
 	data[i][1]=(int)(torque/256);
       }
     }
-    this->dyn_server->write_sync(this->servo_id,goal_speed,data);
+    this->dyn_server->write_sync(this->servo_id,this->registers[0][goal_speed],data);
   }catch(CDynamixelAlarmException &e){
     /* handle dynamixel exception */
     if(e.get_alarm()&this->alarms[i])
@@ -1396,11 +1466,11 @@ void CDynamixelMotorGroup::stop(void)
       {
 	if(this->mode==angle_ctrl)
 	{
-	  this->dynamixel_dev[i]->read_word_register(current_pos,&current_position);
-	  this->dynamixel_dev[i]->write_word_register(goal_pos,current_position);
+	  this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],&current_position);
+	  this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_pos],current_position);
 	}
 	else
-	  this->dynamixel_dev[i]->write_word_register(goal_speed,0);
+	  this->dynamixel_dev[i]->write_word_register(this->registers[i][goal_speed],0);
       }
     }
   }catch(CDynamixelAlarmException &e){
@@ -1427,7 +1497,7 @@ void CDynamixelMotorGroup::get_current_angle(std::vector<double> &angles)
     else
     {
       try{
-	this->dynamixel_dev[i]->read_word_register(current_pos,&data);
+	this->dynamixel_dev[i]->read_word_register(this->registers[i][current_pos],&data);
 	angles[i] = this->to_angles(i, data);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
@@ -1455,7 +1525,7 @@ void CDynamixelMotorGroup::get_current_speed(std::vector<double> &speeds)
     else
     {
       try{
-	this->dynamixel_dev[i]->read_word_register(current_speed,&data);
+	this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&data);
 	speeds[i] = this->to_speeds(i, data);
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
@@ -1484,8 +1554,8 @@ void CDynamixelMotorGroup::get_current_effort(std::vector<double> &efforts)
     else
     {
       try{
-	this->dynamixel_dev[i]->read_word_register(current_load,&c_effort);
-	this->dynamixel_dev[i]->read_word_register(current_speed,&c_speed);
+	this->dynamixel_dev[i]->read_word_register(this->registers[i][current_load],&c_effort);
+	this->dynamixel_dev[i]->read_word_register(this->registers[i][current_speed],&c_speed);
 	efforts[i] = (double)((c_effort&0x3FF)*100.0/1023);
 	if (this->to_speeds(i, c_speed) < 0)
 	{
@@ -1517,7 +1587,7 @@ void CDynamixelMotorGroup::get_current_voltage(std::vector<double> &voltages)
     else
     {
       try{
-	this->dynamixel_dev[i]->read_byte_register(current_voltage,&data);
+	this->dynamixel_dev[i]->read_byte_register(this->registers[i][current_voltage],&data);
 	voltages[i] = (double)data/10;
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
@@ -1545,7 +1615,7 @@ void CDynamixelMotorGroup::get_current_temperature(std::vector<double> &temperat
     else
     {
       try{
-	this->dynamixel_dev[i]->read_byte_register(current_temp,&data);
+	this->dynamixel_dev[i]->read_byte_register(this->registers[i][current_temp],&data);
 	temperatures[i] = (double)data;
       }catch(CDynamixelAlarmException &e){
 	/* handle dynamixel exception */
@@ -1562,6 +1632,58 @@ unsigned int CDynamixelMotorGroup::get_num_motors(void)
   return this->servo_id.size();
 }
 
+void CDynamixelMotorGroup::get_punch(std::vector<unsigned short int> &punches)
+{
+  punches.resize(this->servo_id.size());
+
+  for(unsigned int i=0; i<this->servo_id.size(); i++)
+  {
+    if(this->dynamixel_dev[i]==NULL)
+    {
+      /* handle exceptions */
+      throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+    }
+    else
+    {
+      try{
+	this->dynamixel_dev[i]->read_word_register(this->registers[i][punch],&punches[i]);
+      }catch(CDynamixelAlarmException &e){
+	/* handle dynamixel exception */
+	if(e.get_alarm()&this->alarms[i])
+	  this->reset_motor(i);
+	throw;
+      }
+    }
+  }
+}
+
+void CDynamixelMotorGroup::set_punch(std::vector<unsigned short int> &punches)
+{
+  punches.resize(this->servo_id.size());
+
+  for(unsigned int i=0; i<this->servo_id.size(); i++)
+  {
+    if(this->dynamixel_dev[i]==NULL)
+    {
+      /* handle exceptions */
+      throw CDynamixelMotorException(_HERE_,"The dynamixel device is not properly configured.");
+    }
+    else
+    {
+      try{
+        if(punches[i]<0x0020 || punches[i]>0x03FF)
+          throw CDynamixelMotorException(_HERE_,"Invalid punch value");
+	this->dynamixel_dev[i]->write_word_register(this->registers[i][punch],punches[i]);
+      }catch(CDynamixelAlarmException &e){
+	/* handle dynamixel exception */
+	if(e.get_alarm()&this->alarms[i])
+	  this->reset_motor(i);
+	throw;
+      }
+    }
+  }
+}
+
 TDynamixelMotionStates CDynamixelMotorGroup::get_sequence_current_state(void)
 {
   return this->sequence_state;
diff --git a/src/dynamixel_motor_group.h b/src/dynamixel_motor_group.h
index 633f62082aad0c1f1fa04531e65d0ba8371fed45..0d9e5ad97b18dd9eb36472118eaa6f03cf23574b 100644
--- a/src/dynamixel_motor_group.h
+++ b/src/dynamixel_motor_group.h
@@ -12,6 +12,7 @@ typedef struct
   std::string bus_id;
   unsigned int baudrate;
   std::vector<unsigned char> id;
+  dyn_version_t dyn_version;
   std::vector<TDynamixel_config> dyn_config;
   std::vector<TDynamixel_pid> pid_control;
   std::vector<TDynamixel_compliance> compliance_control;
@@ -125,6 +126,11 @@ class CDynamixelMotorGroup
      *
      */
     std::vector<unsigned char> servo_id;
+    /**
+     * \brief 
+     *
+     */
+    std::vector<unsigned short int *>registers;
   protected:
     /**
      * \brief 
@@ -251,7 +257,7 @@ class CDynamixelMotorGroup
      * \brief 
      *
      */
-    void init_motor(unsigned int id);
+    void init_motor(unsigned int id,dyn_version_t version=dyn_version1);
     /**
      * \brief 
      *
@@ -288,12 +294,12 @@ class CDynamixelMotorGroup
      * \brief
      *
      */
-    CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids);
+    CDynamixelMotorGroup(std::string &group_id,unsigned bus_id,int baudrate,std::vector<unsigned char> &ids,dyn_version_t version=dyn_version1);
     /**
      * \brief
      *
      */
-    CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids);
+    CDynamixelMotorGroup(std::string &group_id,std::string &bus_id,int baudrate,std::vector<unsigned char> &ids,dyn_version_t version=dyn_version1);
     /* configuration functions */
     /**
      * \brief
@@ -301,6 +307,11 @@ class CDynamixelMotorGroup
      */
     void load_config(TDynamixelGroup_config &config);
 #ifdef _HAVE_XSD
+    /**
+     * \brief 
+     *
+     */
+    static TDynamixelGroup_config read_config(std::string &filename);
     /**
      * \brief 
      *
@@ -382,6 +393,16 @@ class CDynamixelMotorGroup
      *
      */
     unsigned int get_num_motors(void);
+    /**
+     * \brief 
+     *
+     */
+    void get_punch(std::vector<unsigned short int> &punches);
+    /**
+     * \brief 
+     *
+     */
+    void set_punch(std::vector<unsigned short int> &punches);
     /* motion sequence public functions */
     /**
      * \brief 
diff --git a/src/dynamixel_registers.cpp b/src/dynamixel_registers.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..58904ec06956226413f8d9611a6dd4df606f1570
--- /dev/null
+++ b/src/dynamixel_registers.cpp
@@ -0,0 +1,125 @@
+#include "dynamixel_registers.h"
+
+unsigned short int std_compl_reg[40]={std_model_number,
+                                     std_firmware_version,
+                                     std_id,
+                                     std_baudrate,
+                                     std_return_delay_time,
+                                     std_cw_angle_limit,
+                                     std_ccw_angle_limit,
+                                     (unsigned short int)-1,
+                                     std_temp_limit,
+                                     std_low_voltage_limit,
+                                     std_high_voltage_limit,
+                                     std_max_torque,
+                                     std_return_level,
+                                     std_alarm_led,
+                                     std_alarm_shtdwn,
+                                     std_down_cal,
+                                     std_up_cal,
+                                     std_torque_en,
+                                     std_led,
+                                     (unsigned short int)-1,
+                                     (unsigned short int)-1,
+                                     (unsigned short int)-1,
+                                     std_cw_comp_margin,
+                                     std_ccw_comp_margin,
+                                     std_cw_comp_slope,
+                                     std_ccw_comp_slope,
+                                     std_goal_pos,
+                                     std_goal_speed,
+                                     std_torque_limit,
+                                     (unsigned short int)-1,
+                                     std_current_pos,
+                                     std_current_speed,
+                                     std_current_load,
+                                     std_current_voltage,
+                                     std_current_temp,
+                                     std_reg_inst,
+                                     std_moving,
+                                     std_lock,
+                                     (unsigned short int)-1,
+                                     std_punch};
+
+unsigned short int std_pid_reg[40]={std_model_number,
+                                    std_firmware_version,
+                                    std_id,
+                                    std_baudrate,
+                                    std_return_delay_time,
+                                    std_cw_angle_limit,
+                                    std_ccw_angle_limit,
+                                    (unsigned short int)-1,
+                                    std_temp_limit,
+                                    std_low_voltage_limit,
+                                    std_high_voltage_limit,
+                                    std_max_torque,
+                                    std_return_level,
+                                    std_alarm_led,
+                                    std_alarm_shtdwn,
+                                    std_down_cal,
+                                    std_up_cal,
+                                    std_torque_en,
+                                    std_led,
+                                    std_pid_p,
+                                    std_pid_i,
+                                    std_pid_d,
+                                    (unsigned short int)-1,
+                                    (unsigned short int)-1,
+                                    (unsigned short int)-1,
+                                    (unsigned short int)-1,
+                                    std_goal_pos,
+                                    std_goal_speed,
+                                    std_torque_limit,
+                                    (unsigned short int)-1,
+                                    std_current_pos,
+                                    std_current_speed,
+                                    std_current_load,
+                                    std_current_voltage,
+                                    std_current_temp,
+                                    std_reg_inst,
+                                    std_moving,
+                                    std_lock,
+                                    (unsigned short int)-1,
+                                    std_punch};
+
+unsigned short int xl_reg[40]={xl_model_number,
+                               xl_firmware_version,
+                               xl_id,
+                               xl_baudrate,
+                               xl_return_delay_time,
+                               xl_cw_angle_limit,
+                               xl_ccw_angle_limit,
+                               xl_control_mode,
+                               xl_temp_limit,
+                               xl_low_voltage_limit,
+                               xl_high_voltage_limit,
+                               xl_max_torque,
+                               xl_return_level,
+                               (unsigned short int)-1,
+                               xl_alarm_shtdwn,
+                               xl_down_cal,
+                               xl_up_cal,
+                               xl_torque_en,
+                               xl_led,
+                               xl_pid_p,
+                               xl_pid_i,
+                               xl_pid_d,
+                               (unsigned short int)-1,
+                               (unsigned short int)-1,
+                               (unsigned short int)-1,
+                               (unsigned short int)-1,
+                               xl_goal_pos,
+                               xl_goal_speed,
+                               (unsigned short int)-1,
+                               xl_goal_torque,
+                               xl_current_pos,
+                               xl_current_speed,
+                               xl_current_load,
+                               xl_current_voltage,
+                               xl_current_temp,
+                               xl_reg_inst,
+                               xl_moving,
+                               (unsigned short int)-1,
+                               xl_hardware_error,
+                               xl_punch};
+
diff --git a/src/dynamixel_registers.h b/src/dynamixel_registers.h
new file mode 100644
index 0000000000000000000000000000000000000000..2bfbe13d4dfd6b2fcbab947ace5e4b8ecb88e0aa
--- /dev/null
+++ b/src/dynamixel_registers.h
@@ -0,0 +1,138 @@
+#ifndef _DYNAMIXEL_REGISTERS_H
+#define _DYNAMIXEL_REGISTERS_H
+
+extern unsigned short int std_compl_reg[40];
+extern unsigned short int std_pid_reg[40];
+extern unsigned short int xl_reg[40];
+
+typedef enum {
+  //                           [Access] [Description]
+  // EEPROM Area 
+  model_number=0,        //   (R)     Model Number
+  firmware_version,    //   (R)     Version of the Firmware
+  id,                  //   (RW)    ID of Dynamixel
+  baudrate,            //   (RW)    Baud Rate of Dynamixel
+  return_delay_time,   //   (RW)    Return Delay Time 
+  cw_angle_limit,      //   (RW)    Clockwise Angle Limit
+  ccw_angle_limit,     //   (RW)    Counterclockwise Angle Limit 
+  dyn_control_mode,    //   (RW)    Joint or wheel mode
+  temp_limit,          //   (RW)    Internal Temperature Limit
+  low_voltage_limit,   //   (RW)    Lowest Voltage Limit
+  high_voltage_limit,  //   (RW)    Highest Voltage Limit
+  max_torque,          //   (RW)    Maximum Torque 
+  return_level,        //   (RW)    Status Return Level
+  alarm_led,           //   (RW)    LED for Alarm 
+  alarm_shtdwn,        //   (RW)    Shutdown for Alarm 
+  down_cal,            //   (RW)     
+  up_cal,              //   (RW)    
+  // RAM Area 
+  torque_en,           //   (RW)    Torque On/Off 
+  led,                 //   (RW)    LED On/Off 
+  pid_p,               //   (RW)     
+  pid_i,               //   (RW)     
+  pid_d,               //   (RW)     
+  cw_comp_margin,      //   (RW)    CW Compliance Margin
+  ccw_comp_margin,     //   (RW)    CCW Compliance Margin
+  cw_comp_slope,       //   (RW)    CW Compliance Slope
+  ccw_comp_slope,      //   (RW)    CCW Compliance Slope 
+  goal_pos,            //   (RW)    Goal Position 
+  goal_speed,          //   (RW)    Moving Speed 
+  torque_limit,        //   (RW)    Torque Limit 
+  goal_torque,         //   (RW)    Torque Limit 
+  current_pos,         //   (R)     Current Position 
+  current_speed,       //   (R)     Current Speed 
+  current_load,        //   (R)     Current Load 
+  current_voltage,     //   (R)     Current Voltage 
+  current_temp,        //   (RW)    Current Temperature 
+  reg_inst,            //   (RW)    Means if Instruction is registered 
+  moving,              //   (R)     Means if there is any movement 
+  lock,                //   (RW)    Locking EEPROM 
+  hardware_error,      //   (R)     Means if there is any movement 
+  punch                //   (RW)    Punch 
+} reg_id;
+
+typedef enum {
+  //                           [Access] [Description]
+  // EEPROM Area 
+  std_model_number=0x00,        //   (R)     Model Number
+  std_firmware_version=0x02,    //   (R)     Version of the Firmware
+  std_id=0x03,                  //   (RW)    ID of Dynamixel
+  std_baudrate=0x04,            //   (RW)    Baud Rate of Dynamixel
+  std_return_delay_time=0x05,   //   (RW)    Return Delay Time 
+  std_cw_angle_limit=0x06,      //   (RW)    Clockwise Angle Limit
+  std_ccw_angle_limit=0x08,     //   (RW)    Counterclockwise Angle Limit 
+  std_temp_limit=0x0B,          //   (RW)    Internal Temperature Limit
+  std_low_voltage_limit=0x0C,   //   (RW)    Lowest Voltage Limit
+  std_high_voltage_limit=0x0D,  //   (RW)    Highest Voltage Limit
+  std_max_torque=0x0E,          //   (RW)    Maximum Torque 
+  std_return_level=0x10,        //   (RW)    Status Return Level
+  std_alarm_led=0x11,           //   (RW)    LED for Alarm 
+  std_alarm_shtdwn=0x12,        //   (RW)    Shutdown for Alarm 
+  std_down_cal=0x14,            //   (RW)     
+  std_up_cal=0x16,              //   (RW)    
+  // RAM Area 
+  std_torque_en=0x18,           //   (RW)    Torque On/Off 
+  std_led=0x19,                 //   (RW)    LED On/Off 
+  std_pid_p=0x1A,               //   (RW)     
+  std_pid_i=0x1B,               //   (RW)     
+  std_pid_d=0x1C,               //   (RW)     
+  std_cw_comp_margin=0x1A,      //   (RW)    CW Compliance Margin
+  std_ccw_comp_margin=0x1B,     //   (RW)    CCW Compliance Margin
+  std_cw_comp_slope=0x1C,       //   (RW)    CW Compliance Slope
+  std_ccw_comp_slope=0x1D,      //   (RW)    CCW Compliance Slope 
+  std_goal_pos=0x1E,            //   (RW)    Goal Position 
+  std_goal_speed=0x20,          //   (RW)    Moving Speed 
+  std_torque_limit=0x22,        //   (RW)    Torque Limit 
+  std_current_pos=0x24,         //   (R)     Current Position 
+  std_current_speed=0x26,       //   (R)     Current Speed 
+  std_current_load=0x28,        //   (R)     Current Load 
+  std_current_voltage=0x2A,     //   (R)     Current Voltage 
+  std_current_temp=0x2B,        //   (RW)    Current Temperature 
+  std_reg_inst=0x2C,            //   (RW)    Means if Instruction is registered 
+  std_moving=0x2E,              //   (R)     Means if there is any movement 
+  std_lock=0x2F,                //   (RW)    Locking EEPROM 
+  std_punch=0x30                //   (RW)    Punch 
+} std_reg_id;
+
+typedef enum {
+  //                           [Access] [Description]
+  // EEPROM Area 
+  xl_model_number=0x00,        //   (R)     Model Number
+  xl_firmware_version=0x02,    //   (R)     Version of the Firmware
+  xl_id=0x03,                  //   (RW)    ID of Dynamixel
+  xl_baudrate=0x04,            //   (RW)    Baud Rate of Dynamixel
+  xl_return_delay_time=0x05,   //   (RW)    Return Delay Time 
+  xl_cw_angle_limit=0x06,      //   (RW)    Clockwise Angle Limit
+  xl_ccw_angle_limit=0x08,     //   (RW)    Counterclockwise Angle Limit 
+  xl_control_mode=0x0B,        //   (RW)    Joint or wheel mode
+  xl_temp_limit=0x0C,          //   (RW)    Internal Temperature Limit
+  xl_low_voltage_limit=0x0D,   //   (RW)    Lowest Voltage Limit
+  xl_high_voltage_limit=0x0E,  //   (RW)    Highest Voltage Limit
+  xl_max_torque=0x0F,          //   (RW)    Maximum Torque 
+  xl_return_level=0x11,        //   (RW)    Status Return Level
+  xl_alarm_shtdwn=0x12,        //   (RW)    Shutdown for Alarm 
+  xl_down_cal=0x14,            //   (RW)     
+  xl_up_cal=0x16,              //   (RW)    
+  // RAM Area 
+  xl_torque_en=0x18,           //   (RW)    Torque On/Off 
+  xl_led=0x19,                 //   (RW)    LED On/Off 
+  xl_pid_d=0x1B,               //   (RW)     
+  xl_pid_i=0x1C,               //   (RW)     
+  xl_pid_p=0x1D,               //   (RW)     
+  xl_goal_pos=0x1E,            //   (RW)    Goal Position 
+  xl_goal_speed=0x20,          //   (RW)    Moving Speed 
+  xl_goal_torque=0x23,         //   (RW)    Torque Limit 
+  xl_current_pos=0x25,         //   (R)     Current Position 
+  xl_current_speed=0x27,       //   (R)     Current Speed 
+  xl_current_load=0x29,        //   (R)     Current Load 
+  xl_current_voltage=0x2D,     //   (R)     Current Voltage 
+  xl_current_temp=0x2E,        //   (RW)    Current Temperature 
+  xl_reg_inst=0x2F,            //   (RW)    Means if Instruction is registered 
+  xl_moving=0x31,              //   (R)     Means if there is any movement 
+  xl_hardware_error=0x32,      //   (R)     Means if there is any movement 
+  xl_punch=0x33                //   (RW)    Punch 
+} xl_reg_id;
+
+
+#endif
+
diff --git a/src/examples/CMakeLists.txt b/src/examples/CMakeLists.txt
index d9cd6210087bde801f80bb1799652b5129eb4830..fcfb0ccaa1376723d9a757983d2ab89c62b2b549 100644
--- a/src/examples/CMakeLists.txt
+++ b/src/examples/CMakeLists.txt
@@ -15,3 +15,9 @@ ADD_EXECUTABLE(test_dynamixel_motion_seq test_dynamixel_motion_seq.cpp)
 
 # edit the following line to add the necessary libraries
 TARGET_LINK_LIBRARIES(test_dynamixel_motion_seq dynamixel_motor_cont ${comm_LIBRARY})
+
+# edit the following line to add the source code for the example and the name of the executable
+ADD_EXECUTABLE(motor_testbench motor_testbench.cpp)
+
+# edit the following line to add the necessary libraries
+TARGET_LINK_LIBRARIES(motor_testbench dynamixel_motor_cont ${comm_LIBRARY})
diff --git a/src/examples/motor_testbench.cpp b/src/examples/motor_testbench.cpp
new file mode 100755
index 0000000000000000000000000000000000000000..7f238c3ab6b4aea2520369eb7e35c3585eecf081
--- /dev/null
+++ b/src/examples/motor_testbench.cpp
@@ -0,0 +1,111 @@
+#include "dynamixelexceptions.h"
+#include "dynamixelserver.h"
+#include "exceptions.h"
+#include "dynamixel_motor.h"
+#include <iostream>
+#include "ctime.h"
+
+std::string cont_name="XL_320";
+std::string config_file="../src/xml/dyn_config.xml";
+
+int main(int argc, char *argv[])
+{
+  CDynamixelServer *dyn_server=CDynamixelServer::instance();
+  CDynamixelMotor *cont;
+  std::string serial="A400gavq";
+  TDynamixel_info info;
+  double angle;
+  double amplitude,freq;
+  CTime time;
+  TDynamixel_pid pid;
+  int i;
+
+  try
+  {
+    if(argc!=3 && argc!=6)
+    {
+      std::cout << "Invalid number of arguments." << std::endl;
+      std::cout << "  motor_testbench <amplitude> <speed>" << std::endl;
+    }  
+    else if(argc==3)
+    {
+      amplitude=atoi(argv[1]);
+      freq=atoi(argv[2]);    
+    } 
+    else
+    {
+      amplitude=atoi(argv[1]);
+      freq=atoi(argv[2]);    
+      pid.p=atoi(argv[3]);
+      pid.i=atoi(argv[4]);
+      pid.d=atoi(argv[5]);
+    }
+    if(dyn_server->get_num_buses()>0)
+    {
+      cont = new CDynamixelMotor(cont_name, serial, 1000000, 23,dyn_version2);
+      if(argc==3)
+        cont->load_config(config_file);
+      else
+        cont->set_pid_control(pid);
+
+/*      cont->get_servo_info(info);
+      std::cout << "[INFO]: Servo model: " << info.model << std::endl;
+      std::cout << "[INFO]: Firmware version: " << (int)info.firmware_ver << std::endl;
+      std::cout << "[INFO]: Gear ratio: " << info.gear_ratio << std::endl;
+      std::cout << "[INFO]: Encoder resolution: " << info.encoder_resolution << " counts" << std::endl;
+      if(info.pid_control)
+      {
+	std::cout << "[INFO]: PID control capable" << std::endl;
+	pid.p=10;
+	pid.i=20;
+	pid.d=30;
+	cont->set_pid_control(pid);
+	cont->get_pid_control(pid);
+	std::cout << "[INFO]: PID control: (" << (int)pid.p << "," << (int)pid.i << "," << (int)pid.d << ")" << std::endl;
+      }
+      else
+      {
+	std::cout << "[INFO]: Compliance control capable" << std::endl;
+	cont->get_compliance_control(compliance);
+	std::cout << "[INFO]: Compliance margin: (" << std::dec << (int)compliance.cw_compliance_margin << "," << (int)compliance.ccw_compliance_margin << ")" << std::endl;
+	std::cout << "[INFO]: Compliance slope: (" << std::dec << (int)compliance.cw_compliance_slope << "," << (int)compliance.ccw_compliance_slope << ")" << std::endl;
+      }
+      std::cout << "[INFO]: Maximum angle: " << info.max_angle << " degrees" << std::endl;
+      std::cout << "[INFO]: Center angle: " << info.center_angle << " degrees" << std::endl;
+      std::cout << "[INFO]: Maximum speed: " << info.max_speed << " degress/s" << std::endl;
+      std::cout << "[INFO]: Bus identifier: " << info.bus_id << std::endl;
+      std::cout << "[INFO]: Bus baudrate: " << info.baudrate << " bps" << std::endl;
+      std::cout << "[INFO]: Servo identifier: " << (int)info.id << std::endl;
+      cont->get_position_range(&min_angle,&max_angle);
+      std::cout << "[INFO]: Angle range: (" << min_angle << "," << max_angle << ")" << std::endl;
+      sd_alarms = cont->get_turn_off_alarms();
+      std::cout << "[INFO]: Shutdown alarm flags: " << std::hex << (int)sd_alarms << std::endl;
+      std::cout << "[INFO]: - Input Voltage Error:\t" << bool(sd_alarms&0x01) << std::endl;
+      std::cout << "[INFO]: - Angle Limit Error:\t"   << bool(sd_alarms&0x02) << std::endl;
+      std::cout << "[INFO]: - OverHeating Error:\t"   << bool(sd_alarms&0x04) << std::endl;
+      std::cout << "[INFO]: - Range Error:\t\t"       << bool(sd_alarms&0x08) << std::endl;
+      std::cout << "[INFO]: - CheckSum Error:\t"    << bool(sd_alarms&0x10) << std::endl;
+      std::cout << "[INFO]: - Overload Error:\t"    << bool(sd_alarms&0x20) << std::endl;
+      std::cout << "[INFO]: - Instruction Error:\t"   << bool(sd_alarms&0x40) << std::endl;
+      led_alarms=cont->get_led_alarms();
+      std::cout << "[INFO]: LED alarm flags: " << std::hex << (int)led_alarms << std::endl;
+      std::cout << "[INFO]: Punch: " << std::dec << cont->get_punch() << std::endl;
+      cont->turn_led_on();
+      sleep(1);
+      cont->turn_led_off();*/
+      for(i=0;i<freq;i++)
+      {
+        angle=amplitude*sin(i*2*3.14159/freq);
+        cont->move_absolute_angle(angle,0);
+        time.set();
+        std::cout << time.getTimeInMicroseconds();
+        std::cout << "," << angle;
+        std::cout << "," << cont->get_current_angle() << std::endl;
+        usleep(7800);
+      }
+      delete cont;
+    }
+  }catch(CException &e){
+    std::cout << "[Genereal exception]: " << e.what() << std::endl; }
+  return 0; 
+}
diff --git a/src/examples/test_dynamixel_motor.cpp b/src/examples/test_dynamixel_motor.cpp
index aaab65f988ce37b46d86714f794d9a9b3a2c3c90..4396ee5b92805c536f991808171dc7f5747c35fc 100755
--- a/src/examples/test_dynamixel_motor.cpp
+++ b/src/examples/test_dynamixel_motor.cpp
@@ -65,7 +65,7 @@ int main(int argc, char *argv[])
       std::cout << "[INFO]: - Instruction Error:\t"   << bool(sd_alarms&0x40) << std::endl;
       led_alarms=cont->get_led_alarms();
       std::cout << "[INFO]: LED alarm flags: " << std::hex << (int)led_alarms << std::endl;
-      std::cout << "[INFO]: Punch: " << std::dec << (int)compliance.punch << std::endl;
+      std::cout << "[INFO]: Punch: " << std::dec << cont->get_punch() << std::endl;
       cont->turn_led_on();
       sleep(1);
       cont->turn_led_off();
diff --git a/src/xml/dynamixel_motor_group_cfg_file.xsd b/src/xml/dynamixel_motor_group_cfg_file.xsd
index 6b416ce9f6eafb7d149af2d534c4ab60652c5c0e..c2664d681dca68d02659ecc4e96b0c9bcd694f03 100755
--- a/src/xml/dynamixel_motor_group_cfg_file.xsd
+++ b/src/xml/dynamixel_motor_group_cfg_file.xsd
@@ -25,6 +25,8 @@ copyright : not copyrighted - public domain
       </xsd:element>
       <xsd:element name="baudrate" type="xsd:int">
       </xsd:element>
+      <xsd:element name="dynamixel_version" type="xsd:int" minOccurs="0">
+      </xsd:element>
       <xsd:element name="dyn_motor_config" type="dyn_motor_config_t" maxOccurs="unbounded">
       </xsd:element>
     </xsd:sequence>