Added a feature to set the initial angle of the servos.

include/dynamixel_servo.h

@@ -32,6 +32,7 @@ namespace dynamixel_robot_gazebo
       void init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh);
       void init_memory(void);
       void set_current_angle(double angle);
+      void set_target_angle(double angle);
       void set_current_speed(double speed);
       void set_current_pwm(double pwm);
       double saturate_command(double command);

src/dynamixel_servo.cpp

@@ -5,6 +5,7 @@ namespace dynamixel_robot_gazebo
   CDynServo::CDynServo(const std::string &joint_name,CDynamixelSlaveSerial *dyn_slave,hardware_interface::EffortJointInterface* hw,ros::NodeHandle &controller_nh)
   {
     int id;
+    double initial_angle;
 
     // Joint handle
     try {
@@ -140,6 +141,10 @@ namespace dynamixel_robot_gazebo
     else 
       this->id=id;
     this->init_memory();
+    // read initial angle
+    initial_angle=0.0;
+    servo_nh.getParam("initial_angle",initial_angle);
+    this->set_target_angle(initial_angle);
     dyn_slave->add_slave(id,boost::bind(&CDynServo::on_ping,this),boost::bind(&CDynServo::on_read,this,_1,_2,_3),boost::bind(&CDynServo::on_write,this,_1,_2,_3));
   }
 
@@ -471,6 +476,26 @@ namespace dynamixel_robot_gazebo
     }
   }
 
+  void CDynServo::set_target_angle(double angle)
+  {
+    unsigned int value;
+ 
+    value=((((angle*180.0)/3.14159)+(this->max_angle/2.0))*this->encoder_res)/this->max_angle;
+    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1" || model=="XL320")
+    {
+      this->memory[30]=value&0x000000FF;
+      this->memory[31]=(value>>8)&0x000000FF;
+    }
+    else
+    {
+      this->memory[116]=value&0x000000FF;
+      this->memory[117]=(value>>8)&0x000000FF;
+      this->memory[118]=(value>>16)&0x000000FF;
+      this->memory[119]=(value>>24)&0x000000FF;
+    }
+  }
+
+
   void CDynServo::set_current_speed(double speed)
   {
     unsigned int value;
@@ -553,7 +578,7 @@ namespace dynamixel_robot_gazebo
     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);
+      value=this->memory[116]+(this->memory[117]<<8)+(this->memory[118]<<16)+(this->memory[119]<<24);
     angle=(((((value*this->max_angle)/this->encoder_res))-this->max_angle/2.0)*3.14159)/180.0;
    
     return angle;