Adds a intermediate step when rising for more stability

bitbots_dynup/config/dynup_robot.yaml

@@ -9,7 +9,7 @@ dynup:
     hand_walkready_pitch: -25.0
     hand_walkready_height: -0.35
     trunk_height: 0.4
-    trunk_pitch: 0.0
+    trunk_pitch: 0.5
     trunk_x_final: 0.0
 
     arm_side_offset_back: 0.148
@@ -27,7 +27,7 @@ dynup:
     time_full_squat_legs: 0.196
     time_legs_close: 0.068
     trunk_height_back: 0.179
-    trunk_overshoot_angle_back: 5.95
+    trunk_overshoot_angle_back: 15.95
     wait_in_squat_back: 0.6
 
     # Front
@@ -40,20 +40,22 @@ dynup:
     time_hands_front: 0.3
     time_hands_rotate: 0.3
     time_hands_side: 0.3
-    time_to_squat: 0.924
+    time_to_squat: 1.5
     time_torso_45: 0.462
-    trunk_overshoot_angle_front: -10.54
-    trunk_x_front: 0.091
+    trunk_overshoot_angle_front: 0.0
+    trunk_x_front: 0.1
     wait_in_squat_front: 1.5
 
     time_walkready: 2.0
     # Rise
+    rise_intermediate_time: 1.5
     rise_time: 0.84
+    trunk_intermediate_height: 0.3
 
     # Descend
     descend_time: 0.25
 
-    # Stabilier
+    # Stabilizer
     stabilizing: False
     minimal_displacement: False
     stable_threshold: 0.05
@@ -66,19 +68,19 @@ dynup:
 
 dynup_pid_trunk_fused_roll:
   ros__parameters:
-    p: 0.36
-    i: 0.8674
-    d: 0.03735
+    p: 0.0
+    i: 0.0
+    d: 0.0
     i_clamp: 0
     i_clamp_min: 0.0
     i_clamp_max: 0.0
     antiwindup: True
     publish_state: False
 dynup_pid_trunk_fused_pitch:
   ros__parameters:
-    p: -0.42
-    i: -5.31646
-    d: -0.008295
+    p: 0.0
+    i: 0.0
+    d: 0.0
     i_clamp: 0.0
     i_clamp_min: 0.0
     i_clamp_max: 0.0

bitbots_dynup/src/dynup_engine.cpp

@@ -590,6 +590,37 @@ double DynupEngine::calcRiseSplines(double time) {
 
   // all positions relative to right foot
   // foot_trajectories_ are for left foot
+
+  // intermediate rise frame for more stability
+  time += params_["rise_intermediate_time"].get_value<double>();
+  l_foot_spline_.x()->addPoint(time, 0);
+  l_foot_spline_.y()->addPoint(time, params_["foot_distance"].get_value<double>());
+  l_foot_spline_.z()->addPoint(time, 0);
+  l_foot_spline_.roll()->addPoint(time, 0);
+  l_foot_spline_.pitch()->addPoint(time, 0);
+  l_foot_spline_.yaw()->addPoint(time, 0);
+
+  r_foot_spline_.x()->addPoint(time, -params_["trunk_x_final"].get_value<double>());
+  r_foot_spline_.y()->addPoint(time, -params_["foot_distance"].get_value<double>() / 2.0);
+  r_foot_spline_.z()->addPoint(time, -params_["trunk_intermediate_height"].get_value<double>());
+  r_foot_spline_.roll()->addPoint(time, 0);
+  r_foot_spline_.pitch()->addPoint(time, M_PI * -params_["trunk_pitch"].get_value<double>() / 180);
+  r_foot_spline_.yaw()->addPoint(time, 0);
+
+
+  l_hand_spline_.x()->addPoint(time, 0);
+  l_hand_spline_.y()->addPoint(time, 0);
+  l_hand_spline_.z()->addPoint(time, params_["hand_walkready_height"].get_value<double>());
+  l_hand_spline_.roll()->addPoint(time, 0);
+  l_hand_spline_.pitch()->addPoint(time, params_["hand_walkready_pitch"].get_value<double>() * M_PI / 180);
+  l_hand_spline_.yaw()->addPoint(time, 0);
+  r_hand_spline_.x()->addPoint(time, 0);
+  r_hand_spline_.y()->addPoint(time, 0);
+  r_hand_spline_.z()->addPoint(time, params_["hand_walkready_height"].get_value<double>());
+  r_hand_spline_.roll()->addPoint(time, 0);
+  r_hand_spline_.pitch()->addPoint(time, params_["hand_walkready_pitch"].get_value<double>() * M_PI / 180);
+  r_hand_spline_.yaw()->addPoint(time, 0);
+
   time += params_["rise_time"].get_value<double>();
   l_foot_spline_.x()->addPoint(time, 0);
   l_foot_spline_.y()->addPoint(time, params_["foot_distance"].get_value<double>());
@@ -605,6 +636,7 @@ double DynupEngine::calcRiseSplines(double time) {
   r_foot_spline_.pitch()->addPoint(time, M_PI * -params_["trunk_pitch"].get_value<double>() / 180);
   r_foot_spline_.yaw()->addPoint(time, 0);
 
+
   l_hand_spline_.x()->addPoint(time, 0);
   l_hand_spline_.y()->addPoint(time, 0);
   l_hand_spline_.z()->addPoint(time, params_["hand_walkready_height"].get_value<double>());