From 640a64e8ebb7b51add294cb632d3550363d8348f Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 5 Jul 2023 12:36:56 +0200 Subject: [PATCH 1/4] use same dynup from backwards as subroutine for side fall --- bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd b/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd index 393e4f39..20bf40a4 100644 --- a/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd +++ b/bitbots_hcm/bitbots_hcm/hcm_dsd/hcm.dsd @@ -27,10 +27,10 @@ $StartHCM OKAY --> $PickedUp PICKED_UP --> @PlayAnimationDynup + direction:walkready, @StayPickedUp ON_GROUND --> $Fallen - FALLEN_FRONT --> @CancelGoals, @PlayAnimationDynup + direction:front - FALLEN_BACK --> @CancelGoals, @SetFootZero, @PlayAnimationDynup + direction:back - FALLEN_RIGHT --> @CancelGoals, @TurnRightToBack + duration:1.5, @PlayAnimationDynup + direction:back - FALLEN_LEFT --> @CancelGoals, @TurnLeftToBack + duration:1.5, @PlayAnimationDynup + direction:back + FALLEN_FRONT --> @CancelGoals, @PlayAnimationDynup + direction:front + r:false + FALLEN_BACK --> @CancelGoals, @SetFootZero, @PlayAnimationDynup + direction:back + r:false + FALLEN_RIGHT --> @CancelGoals, @TurnRightToBack + duration:1.5 + r:false, @FallingPoseBack + duration:0.5 + r:false, @Wait + time:0.5 + r:false, @PlayAnimationDynup + direction:back + r:false + FALLEN_LEFT --> @CancelGoals, @TurnLeftToBack + duration:1.5 + r:false, @FallingPoseBack + duration:0.5 + r:false, @Wait + time:0.5 + r:false, @PlayAnimationDynup + direction:back + r:false NOT_FALLEN --> $Falling FALLING_LEFT --> @CancelGoals, @FallingPoseLeft + duration:0.5 FALLING_RIGHT --> @CancelGoals, @FallingPoseRight + duration:0.5 From d868c088bc5bd3a0d8f9eea8faf2a24688f7a17c Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 5 Jul 2023 12:37:24 +0200 Subject: [PATCH 2/4] add arm keyframe --- bitbots_dynup/src/dynup_engine.cpp | 19 ++++++++++++++++++- 1 file changed, 18 insertions(+), 1 deletion(-) diff --git a/bitbots_dynup/src/dynup_engine.cpp b/bitbots_dynup/src/dynup_engine.cpp index 8f466ba6..353bef4a 100644 --- a/bitbots_dynup/src/dynup_engine.cpp +++ b/bitbots_dynup/src/dynup_engine.cpp @@ -558,10 +558,27 @@ double DynupEngine::calcBackSplines() { l_foot_spline_.pitch()->addPoint(time, 0); l_foot_spline_.yaw()->addPoint(time, 0); + /* + * Pose 4.5: Move hands to body + */ + time += params_["arm_to_body_time"].get_value(); + l_hand_spline_.x()->addPoint(time, 0); + l_hand_spline_.y()->addPoint(time, params_["arm_side_offset_back"].get_value()); + l_hand_spline_.z()->addPoint(time, -params_["arm_extended_length"].get_value()); + l_hand_spline_.roll()->addPoint(time, 0); + l_hand_spline_.pitch()->addPoint(time, M_PI / 2); + l_hand_spline_.yaw()->addPoint(time, 0); + r_hand_spline_.x()->addPoint(time, 0); + r_hand_spline_.y()->addPoint(time, -params_["arm_side_offset_back"].get_value()); + r_hand_spline_.z()->addPoint(time, -params_["arm_extended_length"].get_value()); + r_hand_spline_.roll()->addPoint(time, 0); + r_hand_spline_.pitch()->addPoint(time, M_PI / 2); + r_hand_spline_.yaw()->addPoint(time, 0); + /* * Pose 5: Wait in squat to let instabilities settle */ - time += params_["wait_in_squat_back"].get_value(); + time += params_["wait_in_squat_back"].get_value() ; l_foot_spline_.x()->addPoint(time, 0); l_foot_spline_.y()->addPoint(time, params_["foot_distance"].get_value()); l_foot_spline_.z()->addPoint(time, 0); From 744539cfd4c9634a910712938b96efcdde9e9c7a Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 5 Jul 2023 12:37:42 +0200 Subject: [PATCH 3/4] change parameter --- bitbots_dynup/config/dynup_robot.yaml | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/bitbots_dynup/config/dynup_robot.yaml b/bitbots_dynup/config/dynup_robot.yaml index ac516be6..29561a16 100644 --- a/bitbots_dynup/config/dynup_robot.yaml +++ b/bitbots_dynup/config/dynup_robot.yaml @@ -19,6 +19,7 @@ dynup: com_shift_1: 0.051 com_shift_2: 0.0 foot_angle: 51.76 + hands_behind_back_x: 0.162 hands_behind_back_z: 0.183 leg_min_length_back: 0.253 @@ -27,8 +28,9 @@ dynup: time_full_squat_legs: 0.196 time_legs_close: 0.068 trunk_height_back: 0.179 - trunk_overshoot_angle_back: 5.95 - wait_in_squat_back: 1.0 + trunk_overshoot_angle_back: 1.0 + arm_to_body_time: 1.0 + wait_in_squat_back: 3.0 # Front arm_side_offset_front: 0.13 @@ -44,11 +46,11 @@ dynup: time_torso_45: 0.462 trunk_overshoot_angle_front: -10.54 trunk_x_front: 0.091 - wait_in_squat_front: 1.2 + wait_in_squat_front: 1.5 time_walkready: 2.0 # Rise - rise_time: 1.0 + rise_time: 2.0 # Descend descend_time: 0.25 From 4ccfbea15576b8fcde72c23868414a8d996351f6 Mon Sep 17 00:00:00 2001 From: Valerie Bartel Date: Wed, 5 Jul 2023 13:35:25 +0200 Subject: [PATCH 4/4] update params and add parameter --- bitbots_dynup/config/dynup_robot.yaml | 12 ++++++------ bitbots_dynup/src/dynup_node.cpp | 1 + 2 files changed, 7 insertions(+), 6 deletions(-) diff --git a/bitbots_dynup/config/dynup_robot.yaml b/bitbots_dynup/config/dynup_robot.yaml index 29561a16..bfac4ab1 100644 --- a/bitbots_dynup/config/dynup_robot.yaml +++ b/bitbots_dynup/config/dynup_robot.yaml @@ -68,9 +68,9 @@ 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 @@ -78,9 +78,9 @@ dynup_pid_trunk_fused_roll: publish_state: False dynup_pid_trunk_fused_pitch: ros__parameters: - p: -0.42 - i: -5.31646 - d: -0.008295 + p: -0.2 + i: 0.0 + d: 0.0 i_clamp: 0.0 i_clamp_min: 0.0 i_clamp_max: 0.0 diff --git a/bitbots_dynup/src/dynup_node.cpp b/bitbots_dynup/src/dynup_node.cpp index d8472dec..b2ea10c9 100644 --- a/bitbots_dynup/src/dynup_node.cpp +++ b/bitbots_dynup/src/dynup_node.cpp @@ -35,6 +35,7 @@ namespace bitbots_dynup { l_wrist_frame_ = this->get_parameter("l_wrist_frame").get_value(); param_names_ = { + "arm_to_body_time", "engine_rate", "arm_extended_length", "foot_distance",