diff --git a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_robot.yaml b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_robot.yaml index 1084d1662..a6feb011b 100644 --- a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_robot.yaml +++ b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_robot.yaml @@ -19,6 +19,11 @@ walking: kick_put_down_phase: 0.8 kick_rise_factor: 1.5 kick_vel: 0.2 + kick_early_pitch_prevention_phase: 0.4 + kick_pitch_downwards_phase: 0.9 + kick_pitch_downwards: 0.8 + kick_pitch_upwards_phase: 1.1 + kick_pitch_upwards: -0.9 trunk_height: 0.38 trunk_pause: 0.0 trunk_phase: -0.1 diff --git a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml index 843c69281..51c79a4dd 100644 --- a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml +++ b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml @@ -13,6 +13,11 @@ walking: kick_phase: 0.28 kick_put_down_phase: 0.8 kick_rise_factor: 1.5 + kick_early_pitch_prevention_phase: 0.4 + kick_pitch_downwards_phase: 0.9 + kick_pitch_downwards: 0.8 + kick_pitch_upwards_phase: 1.1 + kick_pitch_upwards: -0.9 double_support_ratio: 0.0264282002140171 first_step_swing_factor: 2.9 diff --git a/bitbots_motion/bitbots_quintic_walk/src/parameters.yaml b/bitbots_motion/bitbots_quintic_walk/src/parameters.yaml index fd124c79f..1286f6eae 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/parameters.yaml +++ b/bitbots_motion/bitbots_quintic_walk/src/parameters.yaml @@ -151,6 +151,36 @@ walking: description: "Scales the step rise parameter, as the kick is a special case, where the foot is moved higher of the ground than in normal walking (ratio)" validation: bounds<>: [0.0, 5.0] + kick_early_pitch_prevention_phase: + type: double + description: "Time at which the foot is not supposed to pitch yet, in order to prevent falling to the side (time relative within the kick phase)" + default_value: 0.4 + validation: + bounds<>: [0.2, 0.5] + kick_pitch_downwards_phase: + type: double + description: "Time at which the foot pitches downwards during the kick phase" + default_value: 0.9 + validation: + bounds<>: [0.6, 0.9] + kick_pitch_downwards: + type: double + description: "Angle at which the foot pitches down (in rad)" + default_value: 0.8 + validation: + bounds<>: [0.6, 0.9] + kick_pitch_upwards_phase: + type: double + description: "Time at which the foot pitches upwards during the kick phase" + default_value: 1.1 + validation: + bounds<>: [0.95, 1.5] + kick_pitch_upwards: + type: double + description: "Angle at which the foot pitches up (in rad)" + default_value: -0.9 + validation: + bounds<>: [-1.5, -0.5] node: engine_freq: type: double diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp index 6cc63a138..58899dada 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp @@ -476,6 +476,19 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { support_to_next_.getOrigin().x() + config_.kick_length, config_.kick_vel); foot_spline_.x()->addPoint(double_support_length + single_support_length * config_.kick_put_down_phase, support_to_next_.getOrigin().x()); + // playing around with pitch + foot_spline_.pitch()->addPoint( + double_support_length + single_support_length * config_.kick_phase * config_.kick_early_pitch_prevention_phase, + 0.0, + config_.kick_vel); // prevent fall pls + foot_spline_.pitch()->addPoint( + double_support_length + single_support_length * config_.kick_phase * config_.kick_pitch_downwards_phase, + config_.kick_pitch_downwards, config_.kick_vel); + foot_spline_.pitch()->addPoint( + double_support_length + single_support_length * config_.kick_phase * config_.kick_pitch_upwards_phase, + config_.kick_pitch_upwards, config_.kick_vel); + // foot_spline_.pitch()->addPoint(double_support_length + single_support_length * + // config_.kick_put_down_phase,0,config_.kick_vel); } else { foot_spline_.x()->addPoint( double_support_length + single_support_length * config_.foot_put_down_phase * config_.foot_overshoot_phase,