Added a module to implement the compliance control.

CMakeLists.txt

@@ -104,6 +104,7 @@ include_directories(${dynamixel_INCLUDE_DIR})
 #   src/${PROJECT_NAME}/dynamixel_controller.cpp
 # )
 add_library(${PROJECT_NAME} src/dynamixel_robot_gazebo.cpp
+                            src/compliance_control.cpp
                             src/dynamixel_servo.cpp)
 
 ## Declare a cpp executable

include/compliance_control.h

+#ifndef _COMPLIANCE_CONTROL_H
+#define _COMPLIANCE_CONTROL_H
+
+#include <ros/node_handle.h>
+
+class CComplianceControl
+{
+  private:
+    int punch;
+    int margin;
+    int slope;
+    unsigned int enc_res;
+    double max_angle;
+    double max_torque;
+  protected: 
+  public:
+    CComplianceControl();
+    bool init(ros::NodeHandle &nh,double max_torque,unsigned int enc_res=1024,double max_angle=300);
+    double control(double error);
+    int get_punch(void);
+    int get_margin(void);
+    int get_slope(void);
+    ~CComplianceControl();
+};
+
+#endif

src/compliance_control.cpp

+#include "compliance_control.h"
+
+CComplianceControl::CComplianceControl()
+{
+}
+
+bool CComplianceControl::init(ros::NodeHandle &nh,double max_torque,unsigned int enc_res,double max_angle)
+{
+  this->punch=32;      
+  if(!nh.getParam("punch",this->punch))
+  {
+    ROS_ERROR("Dynamixel servo: No punch value specified");
+    throw;
+  }
+  this->margin=1;      
+  if(!nh.getParam("margin",this->margin))
+  {
+    ROS_ERROR("Dynamixel servo: No margin value specified");
+    throw;
+  }
+  this->slope=32;      
+  if(!nh.getParam("slope",this->slope))
+  {
+    ROS_ERROR("Dynamixel servo: No slope value specified");
+    throw;
+  }
+  this->enc_res=enc_res;
+  this->max_angle=max_angle;
+  this->max_torque=max_torque;
+}
+
+double CComplianceControl::control(double error)
+{
+  double p,cmd;
+
+  if(fabs(error)<this->margin*this->max_angle*3.14159/(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);
+    if(error<0.0)
+    {
+      cmd=p*error-this->max_torque*this->punch/this->enc_res;
+      if(cmd<-this->max_torque)
+        cmd=-this->max_torque;
+    }
+    else
+    {
+      cmd=p*error+this->max_torque*this->punch/this->enc_res;
+      if(cmd>this->max_torque)
+        cmd=this->max_torque;
+    }
+    return cmd;
+  }
+}
+
+int CComplianceControl::get_punch(void)
+{
+  return this->punch;
+}
+
+int CComplianceControl::get_margin(void)
+{
+  return this->margin;
+}
+
+int CComplianceControl::get_slope(void)
+{
+  return this->slope;
+}
+
+CComplianceControl::~CComplianceControl()
+{
+
+}
+