dynamixel_servo.cpp

#include "dynamixel_servo.h"

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 {
      this->joint=hw->getHandle(joint_name);
    }catch(...){
      ROS_ERROR_STREAM("Dynamixel servo: Could not find joint '" << joint_name);
      throw;
    }
    // Node handle to PID gains
    this->pid=NULL;
    this->compliance=NULL;
    ros::NodeHandle servo_nh(controller_nh,joint_name);
    if(!servo_nh.getParam("model",this->model))
    {
      ROS_ERROR_STREAM("Dynamixel servo: No servo model specified");
      throw;
    }
    if(model=="AX12W")
    {
      this->encoder_res=1023;
      this->max_angle=300.0;
      this->gear_ratio=32.0;
      this->max_torque=1.5;
      this->max_speed=324.0;
      this->init_compliance(joint_name,controller_nh);
    }
    else if(model=="AX12A")
    {
      this->encoder_res=1023;
      this->max_angle=300.0;
      this->gear_ratio=254.0;
      this->max_torque=1.5;
      this->max_speed=354.0;
      this->init_compliance(joint_name,controller_nh);
    }
    else if(model=="AX18A")
    {
      this->encoder_res=1023;
      this->max_angle=300.0;
      this->gear_ratio=254.0;
      this->max_torque=1.8;
      this->max_speed=582.0;
      this->init_compliance(joint_name,controller_nh);
    }
    else if(model=="MX12W")
    {
      this->encoder_res=4095;
      this->max_angle=360.0;
      this->gear_ratio=32.0;
      this->max_torque=1.5;
      this->max_speed=2820.0;
      this->init_pid(joint_name,controller_nh);
    }
    else if(model=="MX28v1")
    {
      this->encoder_res=4095;
      this->max_angle=360.0;
      this->gear_ratio=193.0;
      this->max_torque=2.5;
      this->max_speed=330.0;
      this->init_pid(joint_name,controller_nh);
    }
    else if(model=="MX64v1")
    {
      this->encoder_res=4095;
      this->max_angle=360.0;
      this->gear_ratio=200.0;
      this->max_torque=6.0;
      this->max_speed=378.0;
      this->init_pid(joint_name,controller_nh);
    }
    else if(model=="MX106v1")
    {
      this->encoder_res=4095;
      this->max_angle=360.0;
      this->gear_ratio=225.0;
      this->max_torque=8.4;
      this->max_speed=270.0;
      this->init_pid(joint_name,controller_nh);
    }
    else if(model=="MX28v2")
    {
      this->encoder_res=4095;
      this->max_angle=360.0;
      this->gear_ratio=193.0;
      this->max_torque=2.5;
      this->max_speed=330.0;
      this->init_pid(joint_name,controller_nh);
    }
    else if(model=="MX64v2")
    {
      this->encoder_res=4095;
      this->max_angle=360.0;
      this->gear_ratio=200.0;
      this->max_torque=6.0;
      this->max_speed=378.0;
      this->init_pid(joint_name,controller_nh);
    }
    else if(model=="MX106v2")
    {
      this->encoder_res=4095;
      this->max_angle=360.0;
      this->gear_ratio=225.0;
      this->max_torque=8.4;
      this->max_speed=270.0;
      this->init_pid(joint_name,controller_nh);
    }
    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")
    {
      this->init_pid(joint_name,controller_nh);
    }
    else
    {
      ROS_ERROR_STREAM("Dynamixel servo: Invalid servo model");
      throw;
    }

    // Init PID gains from ROS parameter server
    if(!servo_nh.getParam("id",id))
    {
      ROS_ERROR_STREAM("Dynamixel servo: No ID for the servo");
      throw;
    }
    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));
  }

  void CDynServo::init_pid(const std::string &joint_name,ros::NodeHandle &controller_nh)
  {
    ros::NodeHandle gain_nh(controller_nh,joint_name+std::string("/gains"));
    control_toolbox::Pid::Gains gains;

    this->pid=new control_toolbox::Pid();
    if(!this->pid->init(gain_nh))
    {
      ROS_WARN_STREAM("Dynamixel gazebo plugin: Failed to initialize PID gains from ROS parameter server.");
      delete this->pid;
      this->pid=NULL;
      throw;
    } 
    else
    {
      gains=this->pid->getGains();
      if(this->model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
      {
        this->memory[26]=gains.d_gain_;
        this->memory[27]=gains.i_gain_;
        this->memory[28]=gains.p_gain_;
      }
      else if(this->model=="MX28v2" || model=="MX64v2" || model=="MX106v2")
      {
        *((unsigned short int *)&this->memory[80])=gains.d_gain_;
        *((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_;
      }
    }
  }

  void CDynServo::init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh)
  {
    ros::NodeHandle params_nh(controller_nh,joint_name+std::string("/params"));
    this->compliance=new CComplianceControl();
    if(!this->compliance->init(params_nh,this->max_torque,this->encoder_res,this->max_angle))
    {
      ROS_WARN_STREAM("Dynamixel gazebo plugin: Failed to initialize Compliance params from ROS parameter server.");
      delete this->compliance;
      this->compliance=NULL;
      throw;
    } 
    else
    {
      this->memory[48]=this->compliance->get_punch();
      this->memory[26]=this->compliance->get_margin();
      this->memory[27]=this->compliance->get_margin();
      this->memory[28]=this->compliance->get_slope();
      this->memory[29]=this->compliance->get_slope();
    }
  }

  void CDynServo::init_memory(void)
  {
    for(unsigned int i=0;i<SERVO_MEM_SIZE;i++)
      this->memory[i]=0;
    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
    {
      this->memory[3]=this->id;
      this->memory[4]=1;
      this->memory[5]=250;
      this->memory[11]=70;
      this->memory[12]=60;
      this->memory[13]=140;
      this->memory[16]=2;
      this->memory[17]=36;
      this->memory[18]=36;
      if(model=="AX12W")
      {
        this->memory[0]=12;
        this->memory[8]=255;
        this->memory[9]=3;
        this->memory[14]=255;
        this->memory[15]=3;
        // initialize RAM
        this->memory[26]=4;
        this->memory[27]=4;
        this->memory[28]=64;
        this->memory[29]=64;
        this->memory[30]=255;
        this->memory[31]=1;
        this->memory[34]=255;
        this->memory[35]=3;
        this->memory[48]=32;
      }
      else if(model=="AX12A")
      {
        this->memory[0]=44;
        this->memory[1]=1;
        this->memory[8]=255;
        this->memory[9]=3;
        this->memory[14]=255;
        this->memory[15]=3;
        // initialize RAM
        this->memory[26]=1;
        this->memory[27]=1;
        this->memory[28]=32;
        this->memory[29]=32;
        this->memory[30]=255;
        this->memory[31]=1;
        this->memory[34]=255;
        this->memory[35]=3;
        this->memory[48]=32;
      }
      else if(model=="AX18A")
      {
        this->memory[0]=18;
        this->memory[8]=255;
        this->memory[9]=3;
        this->memory[14]=255;
        this->memory[15]=3;
        // initialize RAM
        this->memory[26]=1;
        this->memory[27]=1;
        this->memory[28]=32;
        this->memory[29]=32;
        this->memory[30]=255;
        this->memory[31]=1;
        this->memory[34]=255;
        this->memory[35]=3;
        this->memory[48]=32;
      }
      else if(model=="MX12W")
      {
        this->memory[0]=105;
        this->memory[1]=1;
        this->memory[8]=255;
        this->memory[9]=15;
        this->memory[14]=255;
        this->memory[15]=3;
        this->memory[22]=1;
        // initialize RAM
        this->memory[26]=8;
        this->memory[28]=8;
        this->memory[30]=255;
        this->memory[31]=7;
        this->memory[34]=255;
        this->memory[35]=3;
        this->memory[48]=32;
      }
      else if(model=="MX28v1")
      {
        this->memory[0]=29;
        this->memory[8]=255;
        this->memory[9]=15;
        this->memory[14]=255;
        this->memory[15]=3;
        this->memory[22]=1;
        // initialize RAM
        this->memory[28]=32;
        this->memory[30]=255;
        this->memory[31]=7;
        this->memory[34]=255;
        this->memory[35]=3;
      }
      else if(model=="MX64v1")
      {
        this->memory[0]=54;
        this->memory[1]=1;
        this->memory[8]=255;
        this->memory[9]=15;
        this->memory[14]=255;
        this->memory[15]=3;
        this->memory[22]=1;
        // initialize RAM
        this->memory[28]=32;
        this->memory[30]=255;
        this->memory[31]=7;
        this->memory[34]=255;
        this->memory[35]=3;
      }
      else if(model=="MX106v1")
      {
        this->memory[0]=64;
        this->memory[1]=1;
        this->memory[8]=255;
        this->memory[9]=15;
        this->memory[14]=255;
        this->memory[15]=3;
        this->memory[22]=1;
        // initialize RAM
        this->memory[28]=32;
        this->memory[30]=255;
        this->memory[31]=7;
        this->memory[34]=255;
        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;
      this->memory[8]=1;
      this->memory[9]=250;
      this->memory[11]=3;
      this->memory[12]=255;
      this->memory[13]=2;
      this->memory[24]=10;
      this->memory[31]=80;
      this->memory[32]=160;
      this->memory[34]=95;
      this->memory[63]=52;
      if(model=="MX28v2")
      {
        this->memory[0]=30;
        this->memory[36]=117;
        this->memory[37]=3;
        this->memory[40]=127;
        this->memory[41]=255;
        this->memory[44]=230;
        this->memory[48]=255;
        this->memory[49]=15;
        // initialize RAM
        this->memory[68]=2;
        this->memory[76]=80;
        this->memory[77]=7;
        this->memory[78]=100;
        this->memory[84]=52;
        this->memory[85]=3;
        this->memory[116]=255;
        this->memory[117]=7;
      }
      else if(model=="MX64v2")
      {
        this->memory[0]=55;
        this->memory[1]=1;
        this->memory[36]=117;
        this->memory[37]=3;
        this->memory[38]=95;
        this->memory[39]=7;
        this->memory[40]=127;
        this->memory[41]=255;
        this->memory[44]=29;
        this->memory[45]=1;
        this->memory[48]=255;
        this->memory[49]=15;
        // initialize RAM
        this->memory[68]=2;
        this->memory[76]=80;
        this->memory[77]=7;
        this->memory[78]=100;
        this->memory[84]=52;
        this->memory[85]=3;
        this->memory[116]=255;
        this->memory[117]=7;
      }
      else if(model=="MX106v2")
      {
        this->memory[0]=65;
        this->memory[1]=1;
        this->memory[36]=117;
        this->memory[37]=3;
        this->memory[38]=255;
        this->memory[39]=7;
        this->memory[40]=127;
        this->memory[41]=255;
        this->memory[44]=210;
        this->memory[48]=255;
        this->memory[49]=15;
        // initialize RAM
        this->memory[68]=2;
        this->memory[76]=80;
        this->memory[77]=7;
        this->memory[78]=100;
        this->memory[84]=52;
        this->memory[85]=3;
        this->memory[116]=255;
        this->memory[117]=7;
      }
    }
  }

  void CDynServo::set_current_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")
    {
      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;
      this->memory[133]=(value>>8)&0x000000FF;
      this->memory[134]=(value>>16)&0x000000FF;
      this->memory[135]=(value>>24)&0x000000FF;
    }
  }

  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;
   
    value=((speed*180.0)/3.14159)/6.0;//RPM
    if(model=="AX12W" || model=="AX12A" || model=="AX18A")
    {
      value/=0.111;
      this->memory[38]=value&0x000000FF;
      this->memory[39]=(value>>8)&0x000000FF;
    }
    else if(model=="MX12W")
    {
      value/=0.916;
      this->memory[38]=value&0x000000FF;
      this->memory[39]=(value>>8)&0x000000FF;
    }
    else if(model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
    {
      value/=0.114;
      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;
      this->memory[128]=value&0x000000FF;
      this->memory[129]=(value>>8)&0x000000FF;
      this->memory[130]=(value>>16)&0x000000FF;
      this->memory[131]=(value>>24)&0x000000FF;
    }
  }

  void CDynServo::set_current_pwm(double pwm)
  {
    unsigned int value;

    if(pwm<0.0)
      value=((fabs(pwm)*1023.0)/this->max_torque)+1024;
    else
      value=((fabs(pwm)*1023.0)/this->max_torque);
    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
    {
      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;
      this->memory[125]=(value>>8)&0x000000FF;
    }
  }

  double CDynServo::saturate_command(double command)
  {
    if(command>this->get_max_pwm())
      command=this->get_max_pwm();
    else if(command<-this->get_max_pwm())
      command=-this->get_max_pwm();

    return command;
  }

  double CDynServo::get_target_angle(void)
  {
    unsigned int value=0;
    double angle;

    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[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;
  }

  double CDynServo::get_target_speed(void)
  {
    unsigned int value=0;
    double speed;

    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1" || model=="XL320")
    {
      value=this->memory[32];
      value+=this->memory[33]<<8;
    }
    else
    {
      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" || model=="XL320")
      value*=0.111;
    else if (model=="MX12W")
      value*=0.916;
    else if(model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
      value*=0.114;
    else
      value*=0.229;
    speed=((value*6.0)*3.14159)/180.0;
 
    return speed;
  }

  double CDynServo::get_target_pwm(void)
  {
    short int value=0;
    double pwm;

    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1" || model=="XL320")
    {
      value=this->memory[32];
      value+=this->memory[33]<<8;
      if(value>1024)
        pwm=-(value-1024)*this->max_torque/1024.0;
      else
        pwm=value*this->max_torque/1024.0;
    }
    else
    {
      value=this->memory[100];
      value+=this->memory[101]<<8;
      pwm=value*this->max_torque/885.0;
    }
 
    return pwm;
  }

  double CDynServo::get_max_pwm(void)
  {
    unsigned int value=0;
    double pwm;

    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
    {
      value=this->memory[14];
      value+=this->memory[15]<<8;
      pwm=this->max_torque*value/1024;
    }
    else if(model=="XL320")
    {
      value=this->memory[15];
      value+=this->memory[16]<<8;
      pwm=this->max_torque*value/1024;
    }
    else
    {
      value=this->memory[36];
      value+=this->memory[37]<<8;
      pwm=this->max_torque*value/885;
    }
 
    return pwm;
  }

  bool CDynServo::is_position_control(void)
  {
    if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="MX12W" || model=="MX28v1" || model=="MX64v1" || model=="MX106v1" || model=="XL320")
    {
      if(this->memory[6]==0x00 && this->memory[7]==0x00 && this->memory[8]==0x00 && this->memory[9]==0x00)
        return false;
      else
        return true;
    }
    else
    {
      if(this->memory[11]==0x03)
        return true;
      else
        return false;
    }
  }

  void CDynServo::on_ping(void)
  {
  }

  unsigned char CDynServo::on_read(unsigned short int address, unsigned short int length, unsigned char *data)
  {
    for(unsigned int i=0;i<length;i++)
      data[i]=this->memory[address+i];

    return 0;
  }

  unsigned char CDynServo::on_write(unsigned short int address, unsigned short int length, unsigned char *data)
  {
    for(unsigned int i=0;i<length;i++)
      this->memory[address+i]=data[i];
    return 0;
  }

  void CDynServo::update(const ros::Duration& period)
  {
    double real_angle,target_angle,target_pwm,command=0.0;

    real_angle=this->joint.getPosition();
    this->set_current_angle(real_angle);
    this->set_current_speed(this->joint.getVelocity());
    this->set_current_pwm(this->joint.getEffort());
    if(this->is_position_control())
    {
      target_angle=this->get_target_angle();
      if(this->pid!=NULL)
        command=this->pid->computeCommand(target_angle-real_angle,period);
      else if(this->compliance!=NULL)
        command=this->compliance->control(target_angle-real_angle);
      command=this->saturate_command(command);
      this->joint.setCommand(command);
    }
    else
    {
      target_pwm=this->get_target_pwm();
      command=this->saturate_command(target_pwm);
      this->joint.setCommand(command);
    }
  }

  unsigned char CDynServo::get_id(void)
  {
    return this->id;
  }

  CDynServo::~CDynServo()
  {
    if(this->pid!=NULL)
    {
      delete this->pid;
      this->pid=NULL;
    }
    if(this->compliance!=NULL)
    {
      delete this->compliance;
      this->compliance=NULL;
    }
  }
}