diff --git a/include/dynamixel_servo.h b/include/dynamixel_servo.h
index bd043068e4903ff0c0716a3a40c7e00baa0366b8..880d2c5b8d6b3031cbcab2f90c08f5aa8ad69ec5 100644
--- a/include/dynamixel_servo.h
+++ b/include/dynamixel_servo.h
@@ -33,9 +33,13 @@ namespace dynamixel_robot_gazebo
void init_memory(void);
void set_current_angle(double angle);
void set_current_speed(double speed);
- void set_current_effort(double effort);
+ void set_current_pwm(double pwm);
+ double saturate_command(double command);
double get_target_angle(void);
double get_target_speed(void);
+ double get_target_pwm(void);
+ double get_max_pwm(void);
+ bool is_position_control(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/compliance_control.cpp b/src/compliance_control.cpp
index 03faba88cd5df5855000fde03042513090f5326c..50d5c23b2d29fec29ad6852951cc8e24de66adcd 100644
--- a/src/compliance_control.cpp
+++ b/src/compliance_control.cpp
@@ -27,17 +27,19 @@ bool CComplianceControl::init(ros::NodeHandle &nh,double max_torque,unsigned int
this->enc_res=enc_res;
this->max_angle=max_angle;
this->max_torque=max_torque;
+
+ return true;
}
double CComplianceControl::control(double error)
{
double p,cmd;
- if(fabs(error)<this->margin*this->max_angle*3.14159/(this->enc_res*180.0))
+ if(fabs(error)<this->margin*this->max_angle*3.14159/(2.0*this->enc_res*180.0))
return 0.0;
else
{
- p=this->max_torque*(this->enc_res-this->punch)*180.0/(this->slope*this->max_angle*3.14159);
+ p=this->max_torque*(this->enc_res-this->punch)*180.0/(this->enc_res*this->slope*(this->max_angle/2.0)*3.14159);
if(error<0.0)
{
cmd=p*error-this->max_torque*this->punch/this->enc_res;
diff --git a/src/dynamixel_servo.cpp b/src/dynamixel_servo.cpp
index 6839362745d7a466cdc44a63549dbf9fe964ae94..5ca6b0802fed0456c8722e00d3ecce480bfa90c2 100644
--- a/src/dynamixel_servo.cpp
+++ b/src/dynamixel_servo.cpp
@@ -186,7 +186,7 @@ namespace dynamixel_robot_gazebo
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 Complaince params from ROS parameter server.");
+ ROS_WARN_STREAM("Dynamixel gazebo plugin: Failed to initialize Compliance params from ROS parameter server.");
delete this->compliance;
this->compliance=NULL;
throw;
@@ -510,14 +510,14 @@ namespace dynamixel_robot_gazebo
}
}
- void CDynServo::set_current_effort(double effort)
+ void CDynServo::set_current_pwm(double pwm)
{
unsigned int value;
- if(effort<0.0)
- value=((fabs(effort)*1023.0)/this->max_torque)+1024;
+ if(pwm<0.0)
+ value=((fabs(pwm)*1023.0)/this->max_torque)+1024;
else
- value=((fabs(effort)*1023.0)/this->max_torque);
+ 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;
@@ -535,6 +535,16 @@ namespace dynamixel_robot_gazebo
}
}
+ 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;
@@ -579,6 +589,75 @@ namespace dynamixel_robot_gazebo
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)
{
}
@@ -600,18 +679,28 @@ namespace dynamixel_robot_gazebo
void CDynServo::update(const ros::Duration& period)
{
- double real_angle,target_angle,command=0.0;
+ 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_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);
+ 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)