Enabled the balance of the robot while moving.

bioloid_control/config/bioloid_control.yaml

@@ -27,7 +27,7 @@ bioloid:
       - j_ankle_roll_l
     gains:
       j_shoulder_l:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -35,7 +35,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_high_arm_l:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -43,7 +43,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_low_arm_l:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -51,7 +51,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_shoulder_r:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -59,7 +59,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_high_arm_r:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -67,7 +67,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_low_arm_r:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -75,7 +75,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_pelvis_yaw_l:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -83,7 +83,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_pelvis_roll_l:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -91,7 +91,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_pelvis_pitch_l:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -99,7 +99,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_knee_l:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -107,7 +107,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_ankle_pitch_l:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -115,7 +115,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_ankle_roll_l:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -123,7 +123,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_pelvis_yaw_r:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -131,7 +131,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_pelvis_roll_r:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -139,7 +139,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_pelvis_pitch_r:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -147,7 +147,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_knee_r:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -155,7 +155,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_ankle_pitch_r:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:
@@ -163,7 +163,7 @@ bioloid:
           vel_limit: 6.0
           effort_limit: 1.5
       j_ankle_roll_r:
-        p: 7.5
+        p: 8.0
         d: 0.0
         i: 0.0
         proxy:

bioloid_controller/CMakeLists.txt

@@ -121,6 +121,7 @@ add_library(${PROJECT_NAME} src/bioloid_controller.cpp
                             /home/sergi/catkin_ws/src/bioloid_robot/bioloid_controller/src/sim/stm32_dyn_master.c
                             /home/sergi/catkin_ws/src/bioloid_robot/bioloid_controller/src/sim/stm32_dyn_master_servos.c
                             /home/sergi/catkin_ws/src/bioloid_robot/bioloid_controller/src/sim/stm32_eeprom.c
+                            /home/sergi/catkin_ws/src/bioloid_robot/bioloid_controller/src/sim/bioloid_balance.c
                             include/bioloid_controller.h
                             include/bioloid_controller_impl.h)
 

bioloid_controller/include/bioloid_controller.h

@@ -90,16 +90,6 @@ namespace bioloid_controller
       std::vector<std::string> joint_names_;        ///< Controlled joint names.
       std::vector<PidPtr> pids_;
 
-      double balance_pelvis_roll_left;
-      double balance_pelvis_roll_right;
-      double balance_ankle_roll_left;
-      double balance_ankle_roll_right;
-
-      double balance_ankle_pitch_left;
-      double balance_ankle_pitch_right;
-      double balance_knee_left;
-      double balance_knee_right;
-
       // motion action subscriber
       IriActionServer<humanoid_common_msgs::humanoid_motionAction> *motion_action_aserver_;
       void motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal);

bioloid_controller/include/bioloid_controller_impl.h

@@ -7,6 +7,8 @@
 #include "action_id.h"
 #include "walking.h"
 #include "ram.h"
+#include "bioloid_math.h"
+#include "bioloid_gyro.h"
 
 extern "C" {
   // motion_manager private function declaration
@@ -20,6 +22,10 @@ extern double real_angles[PAGE_MAX_NUM_SERVOS];
 // action module external variables
 extern long int manager_current_angles[PAGE_MAX_NUM_SERVOS];
 
+// balance external variables
+extern unsigned short int gyro_fb_value;
+extern unsigned short int gyro_lr_value;
+
 // joint names
 const std::string servo_names[MANAGER_MAX_NUM_SERVOS]={std::string("Servo0"),
                                                        std::string("j_shoulder_pitch_r"),
@@ -249,7 +255,7 @@ namespace bioloid_controller
       ram_init();
       /* initialize motion modules */
       manager_init(7800);// motion manager period in us
-      std::cout << "motion manager initialized" << std::endl;
+      ROS_INFO("motion manager initialized");
       /* go to the walk ready position */
       for(unsigned int i=0;i<n_joints;++i)
         manager_set_module(i+1,MM_ACTION); 
@@ -262,9 +268,19 @@ namespace bioloid_controller
   template <class HardwareInterface>
     void BioloidController<HardwareInterface>::update(const ros::Time& time, const ros::Duration& period)
     {
+      static bool gyro_calibrated=false;
       ros::Time current_time=ros::Time::now();
       std::vector<double> target_angles(joints_.size());
 
+      if(!gyro_calibrated)
+      {
+        gyro_calibrated=true;
+        ROS_INFO("Calibrating gyro ...");
+        gyro_init();
+        ROS_INFO("Gyro calibrated");
+        manager_enable_balance();
+      }    
+
       /* get the actual simulation angles */
       for(unsigned int i=0;i<this->joints_.size();++i)
         real_angles[i]=joints_[i].getPosition();
@@ -284,13 +300,13 @@ namespace bioloid_controller
   template <class HardwareInterface>
     void BioloidController<HardwareInterface>::motion_action_startCallback(const humanoid_common_msgs::humanoid_motionGoalConstPtr& goal)
     {
-      std::cout << "Start action" << std::endl;
+      ROS_INFO("Start action");
       /* load a page and start execution */
       if(!action_load_page(goal->motion_id))
-        std::cout << "Impossible to load the desired page" << std::endl;
+        ROS_WARN("Impossible to load the desired page");
       else
       {
-        std::cout << "Page loaded successfully" << std::endl;
+        ROS_INFO("Page loaded successfully");
         action_start_page();
       }
     }
@@ -298,7 +314,7 @@ namespace bioloid_controller
   template <class HardwareInterface>
     void BioloidController<HardwareInterface>::motion_action_stopCallback(void)
     {
-      std::cout << "Stop action" << std::endl;
+      ROS_INFO("Stop action");
       action_stop_page();
     }
      
@@ -360,6 +376,7 @@ namespace bioloid_controller
   template <class HardwareInterface>
     bool BioloidController<HardwareInterface>::set_walk_paramsCallback(humanoid_common_msgs::set_walk_params::Request &req, humanoid_common_msgs::set_walk_params::Response &res)
     {
+      ROS_INFO("Setting walk parameters ...");
       this->walk_access.enter(); 
       ram_write_byte(BIOLOID_WALK_SWING_RIGHT_LEFT,(unsigned char)req.params.Y_SWAP_AMPLITUDE);
       ram_write_byte(BIOLOID_WALK_SWING_TOP_DOWN,(unsigned char)req.params.Z_SWAP_AMPLITUDE);
@@ -376,7 +393,7 @@ namespace bioloid_controller
       ram_write_byte(BIOLOID_WALK_DSP_RATIO,(unsigned char)(req.params.DSP_RATIO*256.0));
       ram_write_byte(BIOLOID_WALK_STEP_FW_BW_RATIO,(unsigned char)(req.params.STEP_FB_RATIO*256.0));
       ram_write_byte(BIOLOID_WALK_MAX_VEL,(unsigned char)(req.params.MAX_VEL*1000.0));
-      ram_write_byte(BIOLOID_WALK_MAX_ROT_VEL,(unsigned char)(req.params.MAX_ROT_VEL*8.0));
+      ram_write_byte(BIOLOID_WALK_MAX_ROT_VEL,(unsigned char)((req.params.MAX_ROT_VEL*1440.0)/PI));
       this->walk_access.exit();
 
       res.ret=true;
@@ -388,6 +405,7 @@ namespace bioloid_controller
       unsigned char byte_value;
       unsigned short int word_value;
 
+      ROS_INFO("Getting current walk parameters ...");
       this->walk_access.enter();
       ram_read_byte(BIOLOID_WALK_SWING_RIGHT_LEFT,&byte_value);
       res.params.Y_SWAP_AMPLITUDE=byte_value;
@@ -420,7 +438,7 @@ namespace bioloid_controller
       ram_read_byte(BIOLOID_WALK_MAX_VEL,&byte_value);
       res.params.MAX_VEL=byte_value/1000.0;
       ram_read_byte(BIOLOID_WALK_MAX_ROT_VEL,&byte_value);
-      res.params.MAX_ROT_VEL=((float)byte_value)/8.0;
+      res.params.MAX_ROT_VEL=(((float)byte_value)*PI)/1440.0;
       this->walk_access.exit();
     }
 
@@ -428,14 +446,18 @@ namespace bioloid_controller
     void BioloidController<HardwareInterface>::cmd_vel_callback(const geometry_msgs::Twist::ConstPtr& msg)
     {
       unsigned short int period;
+      unsigned char step;
 
       this->walk_access.enter();
       ram_read_word(BIOLOID_WALK_PERIOD_TIME_L,&period);
       ram_write_byte(BIOLOID_WALK_STEP_FW_BW,(unsigned char)(msg->linear.x*period));
       ram_write_byte(BIOLOID_WALK_STEP_LEFT_RIGHT,(unsigned char)(msg->linear.y*period));
-      ram_write_byte(BIOLOID_WALK_STEP_DIRECTION,(unsigned char)(msg->angular.z*period));
+      ram_write_byte(BIOLOID_WALK_STEP_DIRECTION,(unsigned char)(((msg->angular.z*1.44)/PI)*period));
       if(!is_walking())
+      {
+        ROS_INFO("Start walking");
         walking_start();
+      }
       else
         this->walk_timeout=ros::Time::now();
       this->walk_access.exit();
@@ -444,23 +466,20 @@ namespace bioloid_controller
   template <class HardwareInterface>
     void BioloidController<HardwareInterface>::imu_callback(const sensor_msgs::Imu::ConstPtr& msg)
     {
-      double x_error=-msg->angular_velocity.x;
-      double y_error=-msg->angular_velocity.z;
-      double gain=3.0;
-
-      balance_pelvis_roll_left=gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/40.0)*300)/1024)*3.14159)/180.0;
-      balance_pelvis_roll_right=gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/40.0)*300)/1024)*3.14159)/180.0;
-      balance_ankle_roll_left=-gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/20.0)*300)/1024)*3.14159)/180.0;
-      balance_ankle_roll_right=-gain*((((y_error*3.3*1024.0)/(600.0*5.0)*(4.0/20.0)*300)/1024)*3.14159)/180.0;
-
-      balance_ankle_pitch_left=-gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/18.0)*300)/1024)*3.14159)/180.0;
-      balance_ankle_pitch_right=gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/18.0)*300)/1024)*3.14159)/180.0;
-      balance_knee_left=-gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/54.0)*300)/1024)*3.14159)/180.0;
-      balance_knee_right=gain*((((x_error*3.3*1024.0)/(600.0*5.0)*(4.0/54.0)*300)/1024)*3.14159)/180.0;
-
-//      std::cout << balance_pelvis_roll_left << "," << balance_pelvis_roll_right << "," << balance_ankle_roll_left << "," << balance_ankle_roll_right << std::endl;
-//      std::cout << balance_ankle_pitch_left << "," << balance_ankle_pitch_right << "," << balance_knee_left << "," << balance_knee_right << std::endl;
-
+      double fb_imu_value=-(msg->angular_velocity.x*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;
+      else if(fb_imu_value<-300.0)
+        fb_imu_value=-300.0;
+      gyro_fb_value=((fb_imu_value*512)/300.0)+512;
+      if(lr_imu_value>300.0)
+        lr_imu_value=300.0;
+      else if(lr_imu_value<-300.0)
+        lr_imu_value=-300.0;
+      gyro_lr_value=((lr_imu_value*512)/300.0)+512;
+      std::cout << "fb_gyro: " << gyro_fb_value << ", lr_gyro: " << gyro_lr_value << std::endl;
     }
 
   template <class HardwareInterface>

bioloid_controller/src/sim/bioloid_balance.c

+#include "eeprom_init.h"
+
+// variables used externally
+unsigned short int gyro_fb_value;
+unsigned short int gyro_lr_value;
+// internal offset values
+unsigned short int gyro_fb_offset;
+unsigned short int gyro_lr_offset;
+
+void gyro_init(void)
+{
+  unsigned int i=0;
+  int fb_offset=0,lr_offset=0; 
+
+  for(i=0;i<1000;i++)
+  {
+    fb_offset+=gyro_fb_value;
+    lr_offset+=gyro_lr_value;
+    usleep(1000);
+  }
+  gyro_fb_offset=fb_offset/1000;
+  gyro_lr_offset=lr_offset/1000;
+}
+
+adc_ch_t gyro_get_fb_adc_channel(void)
+{
+  return DEFAULT_GYRO_FB_ADC_CH;
+}
+
+adc_ch_t gyro_get_lr_adc_channel(void)
+{
+  return DEFAULT_GYRO_LR_ADC_CH;
+}
+
+void gyro_get_offsets(uint16_t *fb_offset,uint16_t *lr_offset)
+{
+  (*fb_offset)=gyro_fb_offset;
+  (*lr_offset)=gyro_lr_offset;
+}
+
+uint16_t adc_get_channel_raw(adc_ch_t channel)
+{
+  if(channel==DEFAULT_GYRO_FB_ADC_CH)
+    return gyro_fb_value;
+  else if(channel==DEFAULT_GYRO_LR_ADC_CH)
+    return gyro_lr_value;
+  else 
+    return 0x0000;
+}

bioloid_controller/src/sim/stm32_eeprom.c

 #include "stm32f4xx_hal.h"
+#include "eeprom_init.h"
 #include "adc_dma.h"
 
-#define    DEFAULT_DEVICE_MODEL             0x7300
-#define    DEFAULT_FIRMWARE_VERSION         0x0001
-#define    DEFAULT_DEVICE_ID                0x0002
-#define    DEFAULT_BAUDRATE                 0x0001
-#define    DEFAULT_RETURN_DELAY             0x0000
-#define    DEFAULT_MM_PERIOD                0x1E78
-#define    DEFAULT_BAL_KNEE_GAIN            0x04BE // 1/54 in fixed point format 0|16
-#define    DEFAULT_BAL_ANKLE_ROLL_GAIN      0x0CCD // 1/20
-#define    DEFAULT_BAL_ANKLE_PITCH_GAIN     0x0E39 // 1/18
-#define    DEFAULT_BAL_HIP_ROLL_GAIN        0x0666 // 1/40
-#define    DEFAULT_GYRO_FB_ADC_CH           (uint16_t)ADC_CH1
-#define    DEFAULT_GYRO_LR_ADC_CH           (uint16_t)ADC_CH2
-#define    DEFAULT_RETURN_LEVEL             0x0002
-#define    DEFAULT_SERVO0_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO1_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO2_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO3_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO4_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO5_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO6_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO7_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO8_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO9_OFFSET            0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO10_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO11_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO12_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO13_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO14_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO15_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO16_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO17_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO18_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO19_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO20_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO21_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO22_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO23_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO24_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO25_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO26_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO27_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO28_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO29_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO30_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_SERVO31_OFFSET           0x0000 // 0 in fixed point format 4 (1+3) | 4
-#define    DEFAULT_WALK_X_OFFSET            0xFFF6 // -10 mm      
-#define    DEFAULT_WALK_Y_OFFSET            0x0005 // 5 mm
-#define    DEFAULT_WALK_Z_OFFSET            0x0014 // 20 mm
-#define    DEFAULT_WALK_ROLL_OFFSET         0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
-#define    DEFAULT_WALK_PITCH_OFFSET        0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
-#define    DEFAULT_WALK_YAW_OFFSET          0x0000 // 0 degrees in fixed point format 5 (1+4) | 3
-#define    DEFAULT_WALK_HIP_PITCH_OFF       0x0000 // 0 degrees in fixed point format 6 (1+5) | 10 
-#define    DEFAULT_WALK_PERIOD_TIME         0x0258 // 600 ms
-#define    DEFAULT_WALK_DSP_RATIO           0x0019 // 0.1 in fixed point format 0 | 8
-#define    DEFAULT_WALK_STEP_FW_BW_RATIO    0x004C // 0.3 in fixed point format 0 | 8
-#define    DEFAULT_WALK_FOOT_HEIGHT         0x0028 // 40 mm
-#define    DEFAULT_WALK_SWING_RIGHT_LEFT    0x0014 // 20 mm
-#define    DEFAULT_WALK_SWING_TOP_DOWN      0x0005 // 5 mm
-#define    DEFAULT_WALK_PELVIS_OFFSET       0x0018 // 3 degrees in fixed point format 5 (1+4) | 3
-#define    DEFAULT_WALK_ARM_SWING_GAIN      0x0030 // 1.5 in fixed point format 3 | 5
-#define    DEFAULT_WALK_MAX_VEL             0x0016 // 20 mm/s
-#define    DEFAULT_WALK_MAX_ROT_VEL         0x0008 // 8 degrees/s in fixed point format 5 | 3
-
 unsigned char eeprom_data[]={DEFAULT_DEVICE_MODEL&0xFF,
                              DEFAULT_DEVICE_MODEL>>8,
                              DEFAULT_FIRMWARE_VERSION,

bioloid_gazebo/worlds/bioloid.world

@@ -2,6 +2,8 @@
 <sdf version="1.4">
   <world name="default">
     <physics type="ode">
+      <real_time_factor>1</real_time_factor>
+      <real_time_update_rate>1000</real_time_update_rate>
       <max_step_size>0.001</max_step_size>
       <gravity>0 0 -9.8</gravity>
       <ode>