diff --git a/include/dynamixel_servo.h b/include/dynamixel_servo.h
index bd15fb6dbdecd4311416add58f60a391087bb4cc..bd043068e4903ff0c0716a3a40c7e00baa0366b8 100644
--- a/include/dynamixel_servo.h
+++ b/include/dynamixel_servo.h
@@ -21,11 +21,21 @@ namespace dynamixel_robot_gazebo
CComplianceControl *compliance;
unsigned char id;
std::string model;
+ unsigned int encoder_res;
+ double max_angle;
+ double gear_ratio;
+ double max_torque;
+ double max_speed;
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_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh);
void init_memory(void);
+ void set_current_angle(double angle);
+ void set_current_speed(double speed);
+ void set_current_effort(double effort);
+ double get_target_angle(void);
+ double get_target_speed(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 21a71ff682ec42229d713fb4c561b27e5cf43b8c..e976ea39898b4c60179e67bbbfd68008a00c53b6 100644
--- a/src/dynamixel_servo.cpp
+++ b/src/dynamixel_servo.cpp
@@ -24,42 +24,92 @@ namespace dynamixel_robot_gazebo
}
if(model=="AX12W")
{
- this->init_compliance(joint_name,controller_nh,1.5,1024,300.0);
+ 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->init_compliance(joint_name,controller_nh,1.5,1024,300.0);
+ 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->init_compliance(joint_name,controller_nh,1.8,1024,300.0);
+ 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")
@@ -119,11 +169,11 @@ namespace dynamixel_robot_gazebo
}
}
- void CDynServo::init_compliance(const std::string &joint_name,ros::NodeHandle &controller_nh,double max_torque,unsigned int enc_res,double max_angle)
+ 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,max_torque,enc_res,max_angle))
+ if(!this->compliance->init(params_nh,this->max_torque,this->encoder_res,this->max_angle))
{
ROS_WARN_STREAM("Dynamixel gazebo plugin: Failed to initialize Complaince params from ROS parameter server.");
delete this->compliance;
@@ -166,7 +216,9 @@ namespace dynamixel_robot_gazebo
this->memory[26]=4;
this->memory[27]=4;
this->memory[28]=64;
- 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;
@@ -183,7 +235,9 @@ namespace dynamixel_robot_gazebo
this->memory[26]=1;
this->memory[27]=1;
this->memory[28]=32;
- 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;
@@ -199,7 +253,9 @@ namespace dynamixel_robot_gazebo
this->memory[26]=1;
this->memory[27]=1;
this->memory[28]=32;
- 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;
@@ -216,6 +272,8 @@ namespace dynamixel_robot_gazebo
// 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;
@@ -230,6 +288,8 @@ namespace dynamixel_robot_gazebo
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;
}
@@ -244,6 +304,8 @@ namespace dynamixel_robot_gazebo
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;
}
@@ -258,6 +320,8 @@ namespace dynamixel_robot_gazebo
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;
}
@@ -292,6 +356,8 @@ namespace dynamixel_robot_gazebo
this->memory[78]=100;
this->memory[84]=52;
this->memory[85]=3;
+ this->memory[116]=255;
+ this->memory[117]=7;
}
else if(model=="MX64v2")
{
@@ -314,6 +380,8 @@ namespace dynamixel_robot_gazebo
this->memory[78]=100;
this->memory[84]=52;
this->memory[85]=3;
+ this->memory[116]=255;
+ this->memory[117]=7;
}
else if(model=="MX106v2")
{
@@ -335,25 +403,147 @@ namespace dynamixel_robot_gazebo
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
+ {
+ 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_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
+ {
+ 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_effort(double effort)
+ {
+ unsigned int value;
+
+ if(effort<0.0)
+ value=((fabs(effort)*1023.0)/this->max_torque)+1024;
+ else
+ value=((fabs(effort)*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
+ {
+ this->memory[124]=value&0x000000FF;
+ this->memory[125]=(value>>8)&0x000000FF;
+ }
+ }
+
+ 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")
+ 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);
+ 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")
+ {
+ this->memory[32]=value&0x000000FF;
+ this->memory[33]=(value>>8)&0x000000FF;
+ }
+ else
+ {
+ this->memory[104]=value&0x000000FF;
+ this->memory[105]=(value>>8)&0x000000FF;
+ this->memory[106]=(value>>16)&0x000000FF;
+ this->memory[107]=(value>>24)&0x000000FF;
+ }
+ if(model=="AX12W" || model=="AX12A" || model=="AX18A")
+ 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;
+ }
+
void CDynServo::on_ping(void)
{
- std::cout << "Servo " << this->id << " pinged" << std::endl;
+ std::cout << "Servo " << (int)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];
+
+ 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)
@@ -361,9 +551,14 @@ namespace dynamixel_robot_gazebo
double real_angle,target_angle,command;
real_angle=this->joint.getPosition();
- target_angle=0.0;//((manager_servos[i+1].current_angle*3.14159)/180.0)/128.0;
+ this->set_current_angle(real_angle);
+ this->set_current_speed(this->joint.getVelocity());
+ this->set_current_effort(this->joint.getEffort());
+ 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);
this->joint.setCommand(command);
}