CMakeLists.txt

@@ -79,7 +79,7 @@ FIND_PACKAGE(dynamixel REQUIRED)
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
  INCLUDE_DIRS include 
- LIBRARIES ${PROJECT_NAME} 
+ LIBRARIES ${PROJECT_NAME} gazebo_dyn_device
  CATKIN_DEPENDS roscpp urdf controller_interface hardware_interface controller_manager sensor_msgs gazebo_ros
 #  DEPENDS system_lib
 )
@@ -105,8 +105,10 @@ include_directories(${dynamixel_INCLUDE_DIR})
 # )
 add_library(${PROJECT_NAME} src/dynamixel_robot_gazebo.cpp
                             src/compliance_control.cpp
-                            src/dynamixel_servo.cpp)
+                            src/dynamixel_servo.cpp
+                            src/dynamixel_device.cpp)
 
+add_library(gazebo_dyn_device src/dynamixel_device.cpp)
 ## Declare a cpp executable
 # add_executable(dynamixel_controller_node src/dynamixel_controller_node.cpp)
 
@@ -123,6 +125,10 @@ TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${iriutils_LIBRARY})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${comm_LIBRARY})
 TARGET_LINK_LIBRARIES(${PROJECT_NAME} ${dynamixel_LIBRARY})
 
+target_link_libraries(gazebo_dyn_device ${catkin_LIBRARIES})
+TARGET_LINK_LIBRARIES(gazebo_dyn_device ${iriutils_LIBRARY})
+TARGET_LINK_LIBRARIES(gazebo_dyn_device ${comm_LIBRARY})
+TARGET_LINK_LIBRARIES(gazebo_dyn_device ${dynamixel_LIBRARY})
 #############
 ## Install ##
 #############

include/dynamixel_robot_gazebo.h

@@ -44,9 +44,11 @@
 #include <controller_interface/controller.h>
 #include <hardware_interface/joint_command_interface.h>
 #include <pluginlib/class_list_macros.h>
+#include <pluginlib/class_loader.hpp>
 
 #include "dynamixel_slave_serial.h"
 #include "dynamixel_servo.h"
+#include "dynamixel_robot_gazebo/dynamixel_device.h"
 
 namespace dynamixel_robot_gazebo
 {
@@ -72,6 +74,9 @@ namespace dynamixel_robot_gazebo
       std::string name;            
       std::vector<std::string> joint_names;
 
+      pluginlib::ClassLoader<dynamixel_robot_gazebo::CDynDevice> device_loader;
+      std::vector<boost::shared_ptr<dynamixel_robot_gazebo::CDynDevice>> devices;
+
       // ROS API
       ros::NodeHandle controller_nh;
   };

include/dynamixel_robot_gazebo/dynamixel_device.h

+#ifndef DYNAMIXEL_DEVICE_H
+#define DYNAMIXEL_DEVICE_H
+
+#include <boost/shared_ptr.hpp>
+#include <ros/ros.h>
+
+#include "dynamixel_slave_serial.h"
+#include "mutex.h"
+
+namespace dynamixel_robot_gazebo
+{
+  class CDynDevice
+  { 
+    private:
+      std::string name;
+      unsigned char *memory;
+      unsigned char id;
+      CMutex access;
+      void on_ping(void);
+      unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data);
+      unsigned char on_write(unsigned short int address, unsigned short int length, unsigned char *data);
+    protected:
+      bool in_window(unsigned short int start_reg,unsigned short int reg_length,unsigned short int start_address,unsigned short int address_length);
+      virtual void device_on_ping(void)=0;
+      virtual unsigned char device_on_read(unsigned short int address, unsigned short int length, unsigned char *data)=0;
+      virtual unsigned char device_on_write(unsigned short int address, unsigned short int length, unsigned char *data)=0;
+      virtual void device_init(ros::NodeHandle &device_nh)=0;
+    public:
+      CDynDevice();
+      void init(const std::string &name,CDynamixelSlaveSerial *dyn_slave,ros::NodeHandle &controller_nh);
+      virtual void update(const ros::Duration& period)=0;
+      unsigned char get_id(void);
+      std::string get_name(void);
+      virtual ~CDynDevice();
+  };
+}
+
+#endif

include/dynamixel_servo.h

@@ -7,6 +7,7 @@
 
 #include "dynamixel_slave_serial.h"
 #include "compliance_control.h"
+#include "mutex.h"
 
 namespace dynamixel_robot_gazebo
 {
@@ -27,11 +28,17 @@ namespace dynamixel_robot_gazebo
       double max_torque;
       double max_speed;
       unsigned char memory[SERVO_MEM_SIZE];
+
+      std::vector<double> trajectory;
+      unsigned int traj_index=0;
+      CMutex traj_access;
     protected:
       void init_pid(const std::string &joint_name,ros::NodeHandle &controller_nh);
       void init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh);
       void init_memory(void);
+      bool in_window(unsigned short int start_reg,unsigned short int reg_length,unsigned short int start_address,unsigned short int address_length);
       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);
@@ -39,7 +46,10 @@ namespace dynamixel_robot_gazebo
       double get_target_speed(void);
       double get_target_pwm(void);
       double get_max_pwm(void);
+      double get_profile_speed(void);
+      double get_profile_accel(void);
       bool is_position_control(void);
+      bool is_time_based(void);
       void on_ping(void);
       unsigned char on_read(unsigned short int address, unsigned short int length, unsigned char *data);
       unsigned char on_write(unsigned short int address, unsigned short int length, unsigned char *data);

src/dynamixel_device.cpp

+#include "dynamixel_robot_gazebo/dynamixel_device.h"
+
+namespace dynamixel_robot_gazebo
+{
+  CDynDevice::CDynDevice()
+  {
+    this->memory=NULL;
+    this->id=-1;
+  }
+
+  void CDynDevice::on_ping(void)
+  {
+    this->device_on_ping();
+  }
+
+  unsigned char CDynDevice::on_read(unsigned short int address, unsigned short int length, unsigned char *data)
+  {
+    unsigned char error;
+
+    error=this->device_on_read(address,length,this->memory);
+    if(!error)
+    {
+      for(unsigned int i=0;i<length;i++)
+        data[i]=this->memory[address+i];
+    }
+
+    return error;
+  }
+
+  unsigned char CDynDevice::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 this->device_on_write(address,length,this->memory);
+  }
+
+  bool CDynDevice::in_window(unsigned short int start_reg,unsigned short int reg_length,unsigned short int start_address,unsigned short int address_length)
+  {
+    unsigned short int end_reg=start_reg+reg_length-1;
+    unsigned short int end_address=start_address+address_length-1;
+
+    if((start_reg>=start_address && start_reg<=end_address) || (end_reg>=start_address && end_reg<=end_address) || (start_address>=start_reg && start_address<=end_reg))
+      return true;
+    else
+      return false;
+  }
+
+  void CDynDevice::init(const std::string &name,CDynamixelSlaveSerial *dyn_slave,ros::NodeHandle &controller_nh)
+  {
+    int mem_size,id;
+
+    ros::NodeHandle device_nh(controller_nh,name);
+    if(!device_nh.getParam("id",id))
+    {
+      ROS_ERROR_STREAM("Dynamixel device " << name << ": No Id present");
+      throw;
+    }
+    this->id=id;
+    if(!device_nh.getParam("mem_size",mem_size))
+    {
+      ROS_ERROR_STREAM("Dynamixel device " << name << ": No memory size specified");
+      throw;
+    }
+    this->memory=new unsigned char[mem_size];
+    this->memory[3]=this->id;
+    this->name=name;
+
+    dyn_slave->add_slave(id,boost::bind(&CDynDevice::on_ping,this),boost::bind(&CDynDevice::on_read,this,_1,_2,_3),boost::bind(&CDynDevice::on_write,this,_1,_2,_3));
+
+    this->device_init(device_nh);
+  }
+
+  unsigned char CDynDevice::get_id(void)
+  {
+    return this->id;
+  }
+
+  std::string CDynDevice::get_name(void)
+  {
+    return this->name;
+  }
+
+  CDynDevice::~CDynDevice()
+  {
+    if(this->memory!=NULL)
+    {
+      delete[] this->memory;
+      this->memory=NULL;
+    }
+  }
+}

src/dynamixel_robot_gazebo.cpp

@@ -63,7 +63,7 @@ namespace dynamixel_robot_gazebo
       return urdf;
     }
 
-    typedef boost::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
+    typedef std::shared_ptr<const urdf::Joint> UrdfJointConstPtr;
     std::vector<UrdfJointConstPtr> getUrdfJoints(const urdf::Model& urdf, const std::vector<std::string>& joint_names)
     {
       std::vector<UrdfJointConstPtr> out;
@@ -104,13 +104,15 @@ namespace dynamixel_robot_gazebo
       this->dyn_slave->stop();
   }
 
-  DynamixelRobotGazebo::DynamixelRobotGazebo()
+  DynamixelRobotGazebo::DynamixelRobotGazebo() :
+    device_loader("dynamixel_robot_gazebo", "dynamixel_robot_gazebo::CDynDevice")
   {
     this->dyn_slave=NULL; 
   }
 
   bool DynamixelRobotGazebo::init(hardware_interface::EffortJointInterface* hw,ros::NodeHandle& root_nh,ros::NodeHandle& controller_nh)
   {
+    boost::shared_ptr<dynamixel_robot_gazebo::CDynDevice> new_device;
     unsigned int n_joints;
     std::string device_name,name;
     int return_delay=0,return_level=2,protocol=2,baudrate=1000000;
@@ -201,6 +203,19 @@ namespace dynamixel_robot_gazebo
     for(unsigned int i=0;i<n_joints;++i)
       this->servos[i]=new CDynServo(this->joint_names[i],this->dyn_slave,hw,controller_nh);
 
+    // load dynamixel devices
+    std::vector<std::string> device_names=internal::getStrings(controller_nh,"devices");
+    for(unsigned int i=0;i<device_names.size();i++)
+    {
+      ros::NodeHandle device_nh(this->controller_nh,device_names[i]);
+      std::string plugin_name;
+      device_nh.getParam("plugin",plugin_name);
+
+      new_device=this->device_loader.createInstance(plugin_name);
+      new_device->init(device_names[i],this->dyn_slave,controller_nh);
+      this->devices.push_back(new_device);
+    }
+
     ROS_DEBUG_STREAM("Dynamixel gazebo plugin: Initialized controller '" << this->name << "' with:" << "\n- Number of joints: " << this->servos.size() << "\n- Hardware interface type: '" << this->getHardwareInterfaceType() << "'");
 
     return true;
@@ -212,6 +227,8 @@ namespace dynamixel_robot_gazebo
       /* get the actual simulation angles */
       for(unsigned int i=0;i<this->servos.size();++i)
         this->servos[i]->update(period);
+      for(unsigned int i=0;i<this->devices.size();++i)
+        this->devices[i]->update(period);
     }catch(CException &e){
       
     }

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,12 @@ 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);
+    this->trajectory.resize(1,0.0);
+    this->traj_index=0;
     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));
   }
 
@@ -447,6 +454,17 @@ namespace dynamixel_robot_gazebo
     }
   }
 
+  bool CDynServo::in_window(unsigned short int start_reg,unsigned short int reg_length,unsigned short int start_address,unsigned short int address_length)
+  {
+    unsigned short int end_reg=start_reg+reg_length-1;
+    unsigned short int end_address=start_address+address_length-1;
+
+    if((start_reg>=start_address && start_reg<=end_address) || (end_reg>=start_address && end_reg<=end_address) || (start_address>=start_reg && start_address<=end_reg))
+      return true;
+    else
+      return false;
+  }
+
   void CDynServo::set_current_angle(double angle)
   {
     unsigned int value;
@@ -471,6 +489,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 +591,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;
@@ -577,18 +615,69 @@ namespace dynamixel_robot_gazebo
       value+=this->memory[107]<<24;
     }
     if(model=="AX12W" || model=="AX12A" || model=="AX18A" || model=="XL320")
-      value*=0.111;
+      speed=value*0.111;
     else if (model=="MX12W")
-      value*=0.916;
+      speed=value*0.916;
     else if(model=="MX28v1" || model=="MX64v1" || model=="MX106v1")
-      value*=0.114;
+      speed=value*0.114;
     else
-      value*=0.229;
-    speed=((value*6.0)*3.14159)/180.0;
+      speed=value*0.229;
+    speed=((speed*6.0)*3.14159)/180.0;
  
     return speed;
   }
 
+
+  double CDynServo::get_profile_speed(void)
+  {
+    unsigned int value=0;
+    double speed;
+
+    if(model=="MX28v2" || model=="MX64v2" || model=="MX106v2")
+    {
+      value=this->memory[112];
+      value+=this->memory[113]<<8;
+      value+=this->memory[114]<<16;
+      value+=this->memory[115]<<24;      
+      if(this->is_time_based())
+        speed=value/1000.0;
+      else
+      {
+        speed=value/0.229;
+        speed=((speed*6.0)*3.14159)/180.0;
+      }
+    }
+    else
+      speed=0.0;
+
+    return speed;
+  }
+
+  double CDynServo::get_profile_accel(void)
+  {
+    unsigned int value=0;
+    double accel;
+
+    if(model=="MX28v2" || model=="MX64v2" || model=="MX106v2")
+    {
+      value=this->memory[108];
+      value+=this->memory[109]<<8;
+      value+=this->memory[110]<<16;
+      value+=this->memory[111]<<24;
+      if(this->is_time_based())
+        accel=value/1000.0;
+      else
+      {
+        accel=value/214.577;
+        accel=((accel*10.0)*3.14159)/180.0;
+      }
+    }
+    else
+      accel=0.0;
+
+    return accel;    
+  }
+
   double CDynServo::get_target_pwm(void)
   {
     short int value=0;
@@ -658,6 +747,19 @@ namespace dynamixel_robot_gazebo
     }
   }
 
+  bool CDynServo::is_time_based(void)
+  {
+    if(model=="MX28v2" || model=="MX64v2" || model=="MX106v2")
+    {
+      if(this->memory[10]&0x04)
+        return true;
+      else
+        return false;
+    }
+    else
+      return false;
+  }
+
   void CDynServo::on_ping(void)
   {
   }
@@ -672,8 +774,112 @@ namespace dynamixel_robot_gazebo
 
   unsigned char CDynServo::on_write(unsigned short int address, unsigned short int length, unsigned char *data)
   {
+    bool update=false;
+    static double angle=0.0,speed=0.0,accel=0.0,accel_time=0.0,traj_time=0.0;
+    double current_angle,current_speed=0.0;
+
     for(unsigned int i=0;i<length;i++)
       this->memory[address+i]=data[i];
+    this->traj_access.enter();
+    /* update the trajectory if necessary */
+    if(model=="MX28v2" || model=="MX64v2" || model=="MX106v2")
+    {
+      if(this->in_window(108,4,address,length))// profile acceleration
+      {
+        if(this->is_time_based())
+          accel_time=this->get_profile_accel();
+        else
+          accel=this->get_profile_accel();
+        update=true;
+      }
+      if(this->in_window(112,4,address,length))// profile speed
+      {
+        if(this->is_time_based())
+          traj_time=this->get_profile_speed();
+        else
+          speed=this->get_profile_speed();
+        update=true;
+      }
+      if(this->in_window(116,4,address,length))// target angle
+      {
+        angle=this->get_target_angle();
+        update=true;
+      }
+      if(update)
+      {
+        this->trajectory.clear();
+        this->traj_index=0;
+        if((!this->is_time_based() && speed!=0.0 && accel!=0.0) || (this->is_time_based() && traj_time!=0.0 && accel_time!=0.0))
+        {
+          current_angle=this->joint.getPosition();
+          if(this->is_time_based())
+          {
+            speed=fabs(current_angle-angle)/(traj_time-accel_time);
+            accel=speed/accel_time;
+          }
+          else
+          {
+            accel_time=speed/accel;
+            traj_time=(fabs(current_angle-angle)+accel_time*speed)/speed;
+          }
+          this->trajectory.push_back(current_angle);
+          for(double time=0.0;time<traj_time;time+=0.0078)
+          {
+            if(time<accel_time)
+            {
+              if(angle>current_angle)
+                current_speed+=accel*0.0078;
+              else
+                current_speed-=accel*0.0078;
+            }
+            else if(time>(traj_time-accel_time))
+            {
+              if(angle>current_angle)
+                current_speed-=accel*0.0078;
+              else
+                current_speed+=accel*0.0078;
+            }
+            this->trajectory.push_back(this->trajectory.back()+current_speed*0.0078);
+          }
+        }
+        this->trajectory.push_back(angle);
+      }
+    }
+    else
+    {
+      if(this->in_window(32,2,address,length))// target speed
+      {
+        speed=this->get_target_speed();
+        update=true;
+      }
+      if(this->in_window(30,2,address,length))// target angle
+      {
+        angle=this->get_target_angle();
+        update=true;
+      }
+      if(update)
+      {
+        this->trajectory.clear();
+        this->traj_index=0;
+        if(speed!=0.0)
+        {
+          current_angle=this->joint.getPosition();
+          this->trajectory.push_back(current_angle);
+          if(angle>current_angle)
+          {
+            while(this->trajectory.back()<=angle)
+              this->trajectory.push_back(this->trajectory.back()+speed*0.0078);
+          }
+          else
+          {
+            while(this->trajectory.back()>=angle)
+              this->trajectory.push_back(this->trajectory.back()-speed*0.0078);
+          }
+        }
+        this->trajectory.push_back(angle);
+      }
+    }
+    this->traj_access.exit();
     return 0;
   }
 
@@ -687,13 +893,17 @@ namespace dynamixel_robot_gazebo
     this->set_current_pwm(this->joint.getEffort());
     if(this->is_position_control())
     {
-      target_angle=this->get_target_angle();
+      this->traj_access.enter();
+      target_angle=this->trajectory[this->traj_index];
       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);
+      if(this->traj_index<(this->trajectory.size()-1))
+        this->traj_index++;
+      this->traj_access.exit();
     }
     else
     {