From 41713e188c95229ab1e96f9c2f4f7f7d62bfe522 Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Thu, 7 May 2015 13:34:12 +0000
Subject: [PATCH] Created a new file for the dynamixel servos registers.
 Removed the information from the other modules. Added static functions to
 read the configuration files without modifying the class attributes. Removed
 the punch parameter from the compliance control. Now it can be independently
 modified and consulted. Added a new configuration parameter to choose the
 desired dynamixel version. Added delays in the EEPROM write functions to
 allow time for the write to take effect (necessary for the XL-320 servos).

---
 src/CMakeLists.txt                         |   4 +-
 src/dynamixel_motor.cpp                    | 244 ++++++++++++++------
 src/dynamixel_motor.h                      |  70 ++----
 src/dynamixel_motor_group.cpp              | 256 +++++++++++++++------
 src/dynamixel_motor_group.h                |  27 ++-
 src/dynamixel_registers.cpp                | 125 ++++++++++
 src/dynamixel_registers.h                  | 138 +++++++++++
 src/examples/CMakeLists.txt                |   6 +
 src/examples/motor_testbench.cpp           | 111 +++++++++
 src/examples/test_dynamixel_motor.cpp      |   2 +-
 src/xml/dynamixel_motor_group_cfg_file.xsd |   2 +
 11 files changed, 797 insertions(+), 188 deletions(-)
 create mode 100644 src/dynamixel_registers.cpp
 create mode 100644 src/dynamixel_registers.h
 create mode 100755 src/examples/motor_testbench.cpp

diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c626213..1e6bb53 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 591e7ec..015703c 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 3d31520..ba52bca 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 8cd9adf..c0904f5 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 633f620..0d9e5ad 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 0000000..58904ec
--- /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 0000000..2bfbe13
--- /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 d9cd621..fcfb0cc 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 0000000..7f238c3
--- /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 aaab65f..4396ee5 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 6b416ce..c2664d6 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>
-- 
GitLab