Substituted the servo PID's controllers for compliance controllers, more...

Substituted the servo PID's controllers for compliance controllers, more similar to the real servos.
The walk timeout is updated each time a new cmd_vel is received, whatever the current state.

bioloid_control/config/bioloid_control.yaml

@@ -27,92 +27,92 @@ bioloid:
       - j_ankle_roll_l
     gains:
       j_shoulder_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_high_arm_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_low_arm_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_shoulder_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_high_arm_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_low_arm_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_yaw_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_roll_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_pitch_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_knee_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_ankle_pitch_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_ankle_roll_l:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_yaw_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_roll_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_pelvis_pitch_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_knee_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_ankle_pitch_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5
       j_ankle_roll_r:
-        p: 8.0
-        d: 0.0
-        i: 0.0
-        i_clamp: 0.0
+        punch: 32
+        margin: 1
+        slope: 32
+        max_torque: 1.5

bioloid_controller/include/bioloid_controller.h

@@ -53,7 +53,6 @@
 // ros_controls
 #include <controller_interface/controller.h>
 #include <hardware_interface/joint_command_interface.h>
-#include <control_toolbox/pid.h>
 
 #include <iri_action_server/iri_action_server.h>
 #include <humanoid_common_msgs/humanoid_motionAction.h>
@@ -65,6 +64,13 @@
 
 namespace bioloid_controller
 {
+  typedef struct{
+    int punch;
+    int margin;
+    int slope;
+    double max_torque;
+  }TCompliance;
+
   /**
    * \brief 
    *
@@ -80,15 +86,17 @@ namespace bioloid_controller
       void starting(const ros::Time& time);
       void stopping(const ros::Time& time);
       void update(const ros::Time& time, const ros::Duration& period);
+   
+    protected:
+      double compliance_control(TCompliance &comp,double error);
 
     private:
       typedef typename HardwareInterface::ResourceHandleType JointHandle;
-      typedef boost::shared_ptr<control_toolbox::Pid> PidPtr;
 
       std::string name_;               ///< Controller name.
       std::vector<JointHandle> joints_;             ///< Handles to controlled joints.
       std::vector<std::string> joint_names_;        ///< Controlled joint names.
-      std::vector<PidPtr> pids_;
+      std::vector<TCompliance> compliances_;
 
       // motion action subscriber
       IriActionServer<humanoid_common_msgs::humanoid_motionAction> *motion_action_aserver_;

bioloid_controller/include/bioloid_controller_impl.h

@@ -151,7 +151,6 @@ namespace bioloid_controller
     {
       for(unsigned int i=0;i<this->joints_.size();++i)
       {
-        pids_[i]->reset();
         this->joints_[i].setCommand(0.0);
       }
     }
@@ -219,7 +218,7 @@ namespace bioloid_controller
       num_servos=n_joints;// set the number of servos to the number of joint of the urdf model
       // Initialize members
       this->joints_.resize(n_joints);
-      this->pids_.resize(n_joints);
+      this->compliances_.resize(n_joints);
       for (unsigned int i=0;i<n_joints;++i)
       {
 	// Joint handle
@@ -233,13 +232,15 @@ namespace bioloid_controller
         // Node handle to PID gains
         ros::NodeHandle joint_nh(this->controller_nh_,std::string("gains/")+joint_names_[i]);
 
-        // Init PID gains from ROS parameter server
-        this->pids_[i].reset(new control_toolbox::Pid());
-        if(!this->pids_[i]->init(joint_nh))
-        {
-          ROS_WARN_STREAM("Failed to initialize PID gains from ROS parameter server.");
-          return false;
-        }
+        // init the compliance controllers
+        this->compliances_[i].punch=32;
+        joint_nh.getParam("punch",this->compliances_[i].punch);
+        this->compliances_[i].margin=1;
+        joint_nh.getParam("margin",this->compliances_[i].margin);
+        this->compliances_[i].slope=32;
+        joint_nh.getParam("slope",this->compliances_[i].slope);
+        this->compliances_[i].max_torque=1.5;
+        joint_nh.getParam("max_torque",this->compliances_[i].max_torque);
 
 	// Whether a joint is continuous (ie. has angle wraparound)
 	const std::string not_if=urdf_joints[i]->type==urdf::Joint::CONTINUOUS ? "" : "non-";
@@ -269,6 +270,7 @@ namespace bioloid_controller
     void BioloidController<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
     {
       static bool gyro_calibrated=false;
+      static ros::Time last_time=ros::Time(0,0);
       ros::Time current_time=ros::Time::now();
       std::vector<double> target_angles(joints_.size());
 
@@ -283,20 +285,56 @@ namespace bioloid_controller
 
       /* get the actual simulation angles */
       for(unsigned int i=0;i<this->joints_.size();++i)
+      {
         real_angles[i]=joints_[i].getPosition();
-      __HAL_TIM_SET_FLAG(TIM3,TIM_FLAG_CC1);
-      __HAL_TIM_SET_IT_SOURCE(TIM3,TIM_IT_CC1);
-      TIM3_IRQHandler();
+        std::cout << real_angles[i] << ",";
+      }
+      std::cout << std::endl;
+      if((current_time-last_time).toSec()>0.0078)
+      {
+        __HAL_TIM_SET_FLAG(TIM3,TIM_FLAG_CC1);
+        __HAL_TIM_SET_IT_SOURCE(TIM3,TIM_IT_CC1);
+        TIM3_IRQHandler();
+        last_time=current_time;
+      }
       for (unsigned int i = 0; i < this->joints_.size(); ++i)
       {
         target_angles[i] = ((manager_current_angles[i+1]*3.14159)/180.0)/65536.0;
-        const double command = pids_[i]->computeCommand(target_angles[i]-real_angles[i],period);
+        const double command = this->compliance_control(this->compliances_[i],target_angles[i]-real_angles[i]);
+        std::cout << target_angles[i] << ",";
         this->joints_[i].setCommand(command);
       }
+      std::cout << std::endl;
       if(is_walking() && ((current_time-this->walk_timeout).toSec()>1.0))
         walking_stop();
     }
 
+  template <class HardwareInterface>
+    double BioloidController<HardwareInterface>::compliance_control(TCompliance &comp,double error)
+    {
+      double p,cmd;
+
+      if(fabs(error)<comp.margin*300.0*3.14159/(1024.0*180.0))
+        return 0.0;
+      else
+      {
+        p=comp.max_torque*(1024.0-comp.punch)*180.0/(comp.slope*300.0*3.14159);
+        if(error<0.0)
+        {
+          cmd=p*error-comp.max_torque*comp.punch/1024.0;
+          if(cmd<-comp.max_torque)
+            cmd=-comp.max_torque;
+        }
+        else
+        {
+          cmd=p*error+comp.max_torque*comp.punch/1024.0;
+          if(cmd>comp.max_torque)
+            cmd=comp.max_torque;
+        }
+        return cmd;
+      }
+    }
+
   template <class HardwareInterface>
     void BioloidController<HardwareInterface>::motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal)
     {
@@ -461,8 +499,7 @@ namespace bioloid_controller
         ROS_INFO("Start walking");
         walking_start();
       }
-      else
-        this->walk_timeout=ros::Time::now();
+      this->walk_timeout=ros::Time::now();
       this->walk_access.exit();
     }
 

bioloid_description/urdf/bioloid.gazebo

@@ -438,7 +438,7 @@
   <gazebo>
     <plugin name="gazebo_ros_control" filename="libgazebo_ros_control.so">
       <robotNamespace>/bioloid</robotNamespace>
-      <controlPeriod>0.001</controlPeriod>
+<!--      <controlPeriod>0.001</controlPeriod>-->
     </plugin>
   </gazebo>
 

bioloid_gazebo/worlds/bioloid.world

@@ -2,17 +2,17 @@
 <sdf version="1.4">
   <world name="default">
     <physics type="ode">
-      <real_time_factor>1.0</real_time_factor>
-      <real_time_update_rate>1000</real_time_update_rate>
-<!--      <real_time_factor>0.1</real_time_factor>
-      <real_time_update_rate>100</real_time_update_rate>-->
+<!--      <real_time_factor>1.0</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>-->
+      <real_time_factor>0.1</real_time_factor>
+      <real_time_update_rate>100</real_time_update_rate>
       <max_step_size>0.001</max_step_size>
       <gravity>0 0 -9.8</gravity>
       <ode>
         <solver>
           <type>quick</type>
-          <min_step_size>0.0001</min_step_size>
-          <iters>100</iters>
+          <min_step_size>0.00025</min_step_size>
+          <iters>200</iters>
           <sor>0.7</sor>
         </solver>
       </ode>