diff --git a/CMakeLists.txt b/CMakeLists.txt
index 887093d2b5fb651c23de33fa68853d1bb0ad0c93..40215409008df7521b3ede66d1468ca43f264386 100644
--- a/CMakeLists.txt
+++ b/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
diff --git a/include/compliance_control.h b/include/compliance_control.h
new file mode 100644
index 0000000000000000000000000000000000000000..afe9d9a072feea8e8dbfebadf66077ca1d0482dc
--- /dev/null
+++ b/include/compliance_control.h
@@ -0,0 +1,26 @@
+#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
diff --git a/src/compliance_control.cpp b/src/compliance_control.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..03faba88cd5df5855000fde03042513090f5326c
--- /dev/null
+++ b/src/compliance_control.cpp
@@ -0,0 +1,76 @@
+#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()
+{
+
+}
+