Solved a problem with the balance feature. The sign of the forward/backeard compensation was wrong.

Used the low resolution model for the basic Bioloid CEABOT robot.

bioloid_control/config/bioloid_ceabot.yaml

@@ -18,7 +18,7 @@ bioloid:
     exp_gpio3_frame: right_front_fwd_ir_link
     exp_gpio4_frame: right_front_dwn_ir_link
     exp_gpio5_frame: right_lateral_dwn_ir_link
-    serial_console_device: /dev/pts/18
+    serial_console_device: /dev/pts/3
     joints:
       - j_shoulder_r
       - j_shoulder_l

bioloid_control/config/bioloid_ceabot_pan_tilt.yaml

@@ -20,7 +20,7 @@ bioloid:
     exp_gpio5_frame: right_lateral_dwn_ir_link
     pan_servo_id: 19
     tilt_servo_id: 20
-    serial_console_device: /dev/pts/18
+    serial_console_device: /dev/pts/3
     joints:
       - j_shoulder_r
       - j_shoulder_l

bioloid_controller/include/bioloid_controller_impl.h

@@ -190,7 +190,7 @@ namespace bioloid_controller
       this->get_walk_params_server_=root_nh.advertiseService("robot/get_walk_params", &BioloidController<HardwareInterface>::get_walk_paramsCallback, this);
 
       // initialize topics
-      this->imu_sub=controller_nh.subscribe("raw_imu", 1, &BioloidController<HardwareInterface>::imu_callback,this);
+      this->imu_sub=root_nh.subscribe("sensors/raw_imu", 1, &BioloidController<HardwareInterface>::imu_callback,this);
       this->cmd_vel_sub=root_nh.subscribe("robot/cmd_vel", 1, &BioloidController<HardwareInterface>::cmd_vel_callback,this);
 
       // Controller name
@@ -285,11 +285,7 @@ namespace bioloid_controller
 
       /* get the actual simulation angles */
       for(unsigned int i=0;i<this->joints_.size();++i)
-      {
         real_angles[i]=joints_[i].getPosition();
-        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);
@@ -301,10 +297,8 @@ namespace bioloid_controller
       {
         target_angles[i] = ((manager_current_angles[i+1]*3.14159)/180.0)/65536.0;
         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();
     }
@@ -507,7 +501,7 @@ namespace bioloid_controller
     void BioloidController<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
     {
       double fb_imu_value=-(msg->angular_velocity.x*180.0)/PI;
-      double lr_imu_value=-(msg->angular_velocity.z*180.0)/PI;
+      double lr_imu_value=(msg->angular_velocity.z*180.0)/PI;
 
       if(fb_imu_value>300.0)
         fb_imu_value=300.0;

bioloid_controller_cm510/include/bioloid_controller_cm510_impl.h

@@ -404,7 +404,7 @@ namespace bioloid_controller_cm510
   template <class HardwareInterface>
     void BioloidControllerCM510<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
     {
-      double fb_imu_value=-(msg->angular_velocity.x*180.0)/3.14159;
+      double fb_imu_value=(msg->angular_velocity.x*180.0)/3.14159;
       double lr_imu_value=(msg->angular_velocity.z*180.0)/3.14159;
 
       if(this->found_gyro_x && this->found_gyro_y)

bioloid_description/urdf/bioloid_ceabot.xacro

 <robot name="bioloid" xmlns:xacro="http://www.ros.org/wiki/xacro">
  
-  <xacro:include filename="$(find bioloid_description)/urdf/bioloid.xacro" />
+  <xacro:include filename="$(find bioloid_description)/urdf/bioloid_low_res.xacro" />
   <xacro:include filename="$(find bioloid_description)/urdf/bioloid_ceabot.gazebo" />
   <xacro:include filename="$(find bioloid_description)/urdf/sensors/sharp_ir.xacro" />
   <xacro:include filename="$(find bioloid_description)/urdf/sensors/feet_ir.xacro" />