Utilize walk stabilization (#574)

Author: ayin21

bitbots_motion/bitbots_quintic_walk/config/robots/jack.yaml

@@ -4,6 +4,8 @@ walking:
   ros__parameters:
     engine:
       trunk_x_offset: 0.0
+      trunk_y_offset: -0.01
+
     node:
       x_bias: -0.005
       y_bias: -0.008

bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_robot.yaml

@@ -7,8 +7,8 @@ walking:
       foot_apex_phase: 0.5
       foot_apex_pitch: 0.0
       foot_distance: 0.2
-      foot_overshoot_phase: 0.67
-      foot_overshoot_ratio: 0.1
+      foot_overshoot_phase: 0.92
+      foot_overshoot_ratio: 0.0
       foot_put_down_phase: 1.0
       foot_put_down_z_offset: 0.0
       foot_rise: 0.04
@@ -26,7 +26,7 @@ walking:
       trunk_pitch_p_coef_forward: 0.0
       trunk_pitch_p_coef_turn: 0.0
       trunk_swing: 0.25
-      trunk_x_offset: -0.02
+      trunk_x_offset: -0.01
       trunk_x_offset_p_coef_forward: 0.1
       trunk_x_offset_p_coef_turn: 0.0
       trunk_y_offset: 0.0
@@ -74,11 +74,11 @@ walking:
       trunk_pid:
         pitch:
           antiwindup: false
-          d: 0.0
+          d: 0.004
           i: 0.0
           i_clamp_max: 0.0
           i_clamp_min: 0.0
-          p: 0.0
+          p: 0.0035
         roll:
           antiwindup: false
           d: 0.0

bitbots_motion/bitbots_quintic_walk/src/walk_stabilizer.cpp

@@ -15,7 +15,7 @@ void WalkStabilizer::reset() {
   pid_trunk_fused_roll_.reset();
 }
 
-WalkResponse WalkStabilizer::stabilize(const WalkResponse &response, const rclcpp::Duration &dt) {
+WalkResponse WalkStabilizer::stabilize(const WalkResponse& response, const rclcpp::Duration& dt) {
   // compute orientation with PID control
   double goal_pitch, goal_roll, goal_yaw;
   tf2::Matrix3x3(response.support_foot_to_trunk.getRotation()).getRPY(goal_roll, goal_pitch, goal_yaw);
@@ -33,14 +33,15 @@ WalkResponse WalkStabilizer::stabilize(const WalkResponse &response, const rclcp
   double fused_pitch_correction =
       pid_trunk_fused_pitch_.computeCommand(goal_fused.fusedPitch - response.current_fused_pitch, dt);
 
-  tf2::Quaternion corrected_orientation;
-  goal_fused.fusedRoll += fused_roll_correction;
-  goal_fused.fusedPitch += fused_pitch_correction;
-  Eigen::Quaterniond goal_orientation_eigen_corrected = rot_conv::QuatFromFused(goal_fused);
-  tf2::convert(goal_orientation_eigen_corrected, corrected_orientation);
+  // Change trunk x offset (in the trunks frame of reference) based on the PID controllers
+  WalkResponse stabilized_response{response};
 
-  WalkResponse stabilized_response = response;
-  stabilized_response.support_foot_to_trunk.setRotation(corrected_orientation);
+  tf2::Transform trunk_offset;
+  trunk_offset.setOrigin(tf2::Vector3(fused_pitch_correction, fused_roll_correction, 0.0));
+  trunk_offset.setRotation(tf2::Quaternion::getIdentity());
+
+  // apply the trunk offset to the trunk pose
+  stabilized_response.support_foot_to_trunk = trunk_offset * response.support_foot_to_trunk;
 
   return stabilized_response;
 }

bitbots_navigation/bitbots_path_planning/config/path_planning_parameters.yaml

@@ -85,19 +85,19 @@ bitbots_path_planning:
         bounds<>: [0.0, 1.0]
     max_vel_x:
       type: double
-      default_value: 0.07
+      default_value: 0.12
       description: 'Maximum velocity we want to reach in different directions (base_footprint coordinate system)'
       validation:
         bounds<>: [0.0, 1.0]
     min_vel_x:
       type: double
-      default_value: -0.05
+      default_value: -0.06
       description: 'Minimum velocity we want to reach in different directions (base_footprint coordinate system)'
       validation:
         bounds<>: [-1.0, 0.0]
     max_vel_y:
       type: double
-      default_value: 0.06
+      default_value: 0.07
       description: 'Maximum velocity we want to reach in different directions (base_footprint coordinate system)'
       validation:
         bounds<>: [0.0, 1.0]