diff --git a/include/dynamixel_servo.h b/include/dynamixel_servo.h
index 294edcc5bd3d8a8cecbc308f52dbf7237793791e..bd15fb6dbdecd4311416add58f60a391087bb4cc 100644
--- a/include/dynamixel_servo.h
+++ b/include/dynamixel_servo.h
@@ -6,19 +6,26 @@
#include <boost/shared_ptr.hpp>
#include "dynamixel_slave_serial.h"
+#include "compliance_control.h"
namespace dynamixel_robot_gazebo
{
+ #define SERVO_MEM_SIZE 147
+
class CDynServo
{
private:
typedef typename hardware_interface::EffortJointInterface::ResourceHandleType JointHandle;
JointHandle joint;
- typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
- PidPtr pid;
+ control_toolbox::Pid *pid;
+ CComplianceControl *compliance;
unsigned char id;
+ std::string model;
+ unsigned char memory[SERVO_MEM_SIZE];
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,double max_torque,unsigned int enc_res,double max_angle);
+ void init_memory(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);
diff --git a/src/dynamixel_servo.cpp b/src/dynamixel_servo.cpp
index 7141fdd7b8597929a11bb08b547dc902eb78b297..21a71ff682ec42229d713fb4c561b27e5cf43b8c 100644
--- a/src/dynamixel_servo.cpp
+++ b/src/dynamixel_servo.cpp
@@ -1,11 +1,9 @@
-#include "dynamixel_slave_serial.h"
#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)
{
- std::string model;
int id;
// Joint handle
@@ -16,19 +14,29 @@ namespace dynamixel_robot_gazebo
throw;
}
// Node handle to PID gains
+ this->pid=NULL;
+ this->compliance=NULL;
ros::NodeHandle servo_nh(controller_nh,joint_name);
- servo_nh.getParam("model",model);
+ if(!servo_nh.getParam("model",this->model))
+ {
+ ROS_ERROR_STREAM("Dynamixel servo: No servo model specified");
+ throw;
+ }
if(model=="AX12W")
{
+ this->init_compliance(joint_name,controller_nh,1.5,1024,300.0);
}
else if(model=="AX12A")
{
+ this->init_compliance(joint_name,controller_nh,1.5,1024,300.0);
}
else if(model=="AX18A")
{
+ this->init_compliance(joint_name,controller_nh,1.8,1024,300.0);
}
else if(model=="MX12W")
{
+ this->init_pid(joint_name,controller_nh);
}
else if(model=="MX28v1")
{
@@ -69,35 +77,283 @@ namespace dynamixel_robot_gazebo
}
// Init PID gains from ROS parameter server
- servo_nh.getParam("id",id);
+ if(!servo_nh.getParam("id",id))
+ {
+ ROS_ERROR_STREAM("Dynamixel servo: No ID for the servo");
+ throw;
+ }
+ else
+ this->id=id;
+ this->init_memory();
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));
- this->id=id;
}
void CDynServo::init_pid(const std::string &joint_name,ros::NodeHandle &controller_nh)
{
ros::NodeHandle gain_nh(controller_nh,joint_name+std::string("/gains"));
- this->pid.reset(new control_toolbox::Pid());
+ 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_;
+ }
+ }
}
- void CDynServo::on_ping(void)
+ void CDynServo::init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh,double max_torque,unsigned int enc_res,double max_angle)
+ {
+ ros::NodeHandle params_nh(controller_nh,joint_name+std::string("/params"));
+ this->compliance=new CComplianceControl();
+ if(!this->compliance->init(params_nh,max_torque,enc_res,max_angle))
+ {
+ ROS_WARN_STREAM("Dynamixel gazebo plugin: Failed to initialize Complaince 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[28]=64;
+ 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[28]=32;
+ 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[28]=32;
+ 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[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[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[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[34]=255;
+ this->memory[35]=3;
+ }
+ }
+ 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;
+ }
+ 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;
+ }
+ 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;
+ }
+ }
+ }
+ void CDynServo::on_ping(void)
+ {
+ std::cout << "Servo " << this->id << " pinged" << std::endl;
}
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];
}
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];
}
void CDynServo::update(const ros::Duration& period)
@@ -106,7 +362,8 @@ namespace dynamixel_robot_gazebo
real_angle=this->joint.getPosition();
target_angle=0.0;//((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0;
- command=this->pid->computeCommand(target_angle-real_angle,period);
+ if(this->pid!=NULL)
+ command=this->pid->computeCommand(target_angle-real_angle,period);
this->joint.setCommand(command);
}
@@ -117,6 +374,15 @@ namespace dynamixel_robot_gazebo
CDynServo::~CDynServo()
{
-
+ if(this->pid!=NULL)
+ {
+ delete this->pid;
+ this->pid=NULL;
+ }
+ if(this->compliance!=NULL)
+ {
+ delete this->compliance;
+ this->compliance=NULL;
+ }
}
}