Added the implementation for the simulation of the XL320 servo.

src/dynamixel_robot_gazebo.cpp

@@ -166,9 +166,9 @@ namespace dynamixel_robot_gazebo
     try{ 
       dynamixel_nh.getParam("protocol_version", protocol);
       if(protocol==1)
-        this->dyn_slave=new CDynamixelSlaveSerial(root_nh.getNamespace()+"_slave",device_name,dyn_version1);
+        this->dyn_slave=new CDynamixelSlaveSerial(name+"_slave",device_name,dyn_version1);
       else if(protocol==2)
-        this->dyn_slave=new CDynamixelSlaveSerial(root_nh.getNamespace()+"_slave",device_name,dyn_version2);
+        this->dyn_slave=new CDynamixelSlaveSerial(name+"_slave",device_name,dyn_version2);
       else
       {
         ROS_ERROR_STREAM("Dynamixel gazebo plugin: Invalid protocol version");

src/dynamixel_servo.cpp

@@ -114,6 +114,11 @@ namespace dynamixel_robot_gazebo
     }
     else if(model=="XL320")
     {
+      this->encoder_res=1023;
+      this->max_angle=300.0;
+      this->gear_ratio=238.0;
+      this->max_torque=0.39;
+      this->max_speed=684.0;
       this->init_pid(joint_name,controller_nh);
     }
     else if(model=="XL430")
@@ -166,6 +171,12 @@ namespace dynamixel_robot_gazebo
         *((unsigned short int *)&this->memory[82])=gains.d_gain_;
         *((unsigned short int *)&this->memory[84])=gains.d_gain_;
       }
+      else if(this->model=="XL320")
+      {
+        this->memory[27]=gains.d_gain_;
+        this->memory[28]=gains.i_gain_;
+        this->memory[29]=gains.p_gain_;
+      }
     }
   }
 
@@ -326,6 +337,33 @@ namespace dynamixel_robot_gazebo
         this->memory[35]=3;
       }
     }
+    else if(model=="XL320")
+    {
+      this->memory[0]=94;
+      this->memory[1]=1;
+      this->memory[3]=this->id;
+      this->memory[4]=1;
+      this->memory[5]=250;
+      this->memory[8]=255;
+      this->memory[9]=3;
+      this->memory[11]=2;
+      this->memory[12]=65;
+      this->memory[13]=60;
+      this->memory[13]=90;
+      this->memory[15]=255;
+      this->memory[16]=3;
+      this->memory[17]=2;
+      this->memory[18]=7;
+      // initialize RAM
+      this->memory[27]=0;
+      this->memory[28]=0;
+      this->memory[29]=32;
+      this->memory[30]=255;
+      this->memory[31]=1;
+      this->memory[35]=255;
+      this->memory[36]=3;
+      this->memory[48]=32;
+    }
     else
     {
       this->memory[7]=this->id;
@@ -419,6 +457,11 @@ namespace dynamixel_robot_gazebo
       this->memory[36]=value&0x000000FF;
       this->memory[37]=(value>>8)&0x000000FF;
     }
+    else if(this->model=="XL320")
+    {
+      this->memory[37]=value&0x000000FF;
+      this->memory[38]=(value>>8)&0x000000FF;
+    }
     else
     {
       this->memory[132]=value&0x000000FF;
@@ -439,7 +482,7 @@ namespace dynamixel_robot_gazebo
       this->memory[38]=value&0x000000FF;
       this->memory[39]=(value>>8)&0x000000FF;
     }
-    else if (model=="MX12W")
+    else if(model=="MX12W")
     {
       value/=0.916;
       this->memory[38]=value&0x000000FF;
@@ -451,6 +494,12 @@ namespace dynamixel_robot_gazebo
       this->memory[38]=value&0x000000FF;
       this->memory[39]=(value>>8)&0x000000FF;
     }
+    else if(this->model=="XL320")
+    {
+      value/=0.111;
+      this->memory[39]=value&0x000000FF;
+      this->memory[40]=(value>>8)&0x000000FF;
+    }
     else
     {
       value/=0.229;
@@ -474,6 +523,11 @@ namespace dynamixel_robot_gazebo
       this->memory[40]=value&0x000000FF;
       this->memory[41]=(value>>8)&0x000000FF;
     }
+    else if(this->model=="XL320")
+    {
+      this->memory[41]=value&0x000000FF;
+      this->memory[42]=(value>>8)&0x000000FF;
+    }
     else
     {
       this->memory[124]=value&0x000000FF;
@@ -486,7 +540,7 @@ namespace dynamixel_robot_gazebo
     unsigned int value=0;
     double angle;
 
-    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
+    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1" || model=="XL320")
       value=this->memory[30]+(this->memory[31]<<8);
     else
       value=this->memory[116]+(this->memory[133]<<8)+(this->memory[134]<<16)+(this->memory[135]<<24);
@@ -500,19 +554,19 @@ namespace dynamixel_robot_gazebo
     unsigned int value=0;
     double speed;
 
-    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
+    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1" || model=="XL320")
     {
-      this->memory[32]=value&0x000000FF;
-      this->memory[33]=(value>>8)&0x000000FF;
+      value=this->memory[32];
+      value+=this->memory[33]<<8;
     }
     else
     {
-      this->memory[104]=value&0x000000FF;
-      this->memory[105]=(value>>8)&0x000000FF;
-      this->memory[106]=(value>>16)&0x000000FF;
-      this->memory[107]=(value>>24)&0x000000FF;
+      value=this->memory[104];
+      value+=this->memory[105]<<8;
+      value+=this->memory[106]<<16;
+      value+=this->memory[107]<<24;
     }
-    if(model=="AX12W" || model=="AX12A" || model=="AX18A")
+    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="XL320")
       value*=0.111;
     else if (model=="MX12W")
       value*=0.916;
@@ -527,10 +581,12 @@ namespace dynamixel_robot_gazebo
 
   void CDynServo::on_ping(void)
   {
+    std::cout << "device " << (int)this->id << " pinged" << std::endl;
   }
 
   unsigned char CDynServo::on_read(unsigned short int address, unsigned short int length, unsigned char *data)
   {
+    std::cout << "device " << (int)this->id << " read operation" << std::endl;
     for(unsigned int i=0;i<length;i++)
       data[i]=this->memory[address+i];
 
@@ -539,6 +595,7 @@ namespace dynamixel_robot_gazebo
 
   unsigned char CDynServo::on_write(unsigned short int address, unsigned short int length, unsigned char *data)
   {
+    std::cout << "device " << (int)this->id << " write operation" << std::endl;
     for(unsigned int i=0;i<length;i++)
       this->memory[address+i]=data[i];
     return 0;