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..5e931a30d 100644 --- a/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml +++ b/bitbots_motion/bitbots_quintic_walk/config/walking_wolfgang_simulator.yaml @@ -14,6 +14,13 @@ walking: kick_put_down_phase: 0.8 kick_rise_factor: 1.5 + fast_kick_knee_offset: 0.05 + fast_kick_hip_offset: 0.288 + fast_kick_phase_start: 0.19 + fast_kick_phase_end: 0.5 + + + double_support_ratio: 0.0264282002140171 first_step_swing_factor: 2.9 foot_distance: 0.179900277671633 diff --git a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp index a57676d96..10b7f5585 100644 --- a/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp +++ b/bitbots_motion/bitbots_quintic_walk/include/bitbots_quintic_walk/walk_engine.hpp @@ -16,6 +16,7 @@ The original files can be found at: #include "bitbots_splines/abstract_engine.hpp" #include "bitbots_splines/pose_spline.hpp" #include "bitbots_splines/smooth_spline.hpp" +#include "bitbots_splines/abstract_ik.hpp" namespace bitbots_quintic_walk { @@ -61,6 +62,8 @@ class WalkEngine : public bitbots_splines::AbstractEngine: [0.0, 5.0] + fast_kick_knee_offset: + type: double + description: "Offset of knee for fast kick" + validation: + bounds<>: [0, 1.2] + fast_kick_hip_offset: + type: double + description: "Offset of hip for fast kick" + validation: + bounds<>: [0, 1.2] + fast_kick_phase_start: + type: double + description: "Offset of hip for fast kick" + validation: + bounds<>: [0, 1] + fast_kick_phase_end: + type: double + description: "Offset of hip for fast kick" + validation: + bounds<>: [0, 1] 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 331dcc627..e8957092f 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_engine.cpp @@ -472,6 +472,7 @@ void WalkEngine::buildTrajectories(WalkEngine::TrajectoryType type) { foot_pos_acc_at_foot_change_.x()); foot_spline_.x()->addPoint(double_support_length, foot_pos_at_foot_change_.x()); if (kick_step) { + foot_spline_.x()->addPoint(t,x); foot_spline_.x()->addPoint(double_support_length + single_support_length * config_.kick_phase, 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, @@ -804,6 +805,46 @@ double WalkEngine::getTrajsTime() const { return t; } +void WalkEngine::fastKick(bitbots_splines::JointGoals &motor_goals_) { + // in this state we do a kick while doing a step + if (engine_state_ == WalkState::KICK and is_left_support_foot_ && (phase_ > config_.fast_kick_phase_start && phase_ < config_.fast_kick_phase_end)) { + RCLCPP_WARN(node_->get_logger(), + "fastKick right"); + for (size_t i = 0; i < motor_goals_.first.size(); i++) { + if (motor_goals_.first[i] == "RHipPitch") { + motor_goals_.second[i] -= config_.fast_kick_hip_offset; + } + } + + for (size_t i = 0; i < motor_goals_.first.size(); i++) { + if (motor_goals_.first[i] == "RKnee") { + motor_goals_.second[i] += config_.fast_kick_knee_offset; + RCLCPP_WARN(node_->get_logger(), + "change goal of joint %ld at %s", i, motor_goals_.first[i].c_str()); + } + } + } + if (engine_state_ == WalkState::KICK and !is_left_support_foot_ && (phase_ > (config_.fast_kick_phase_start + 0.5) && phase_ < (config_.fast_kick_phase_end + 0.5))) { + RCLCPP_WARN(node_->get_logger(), + "fastKick left"); + + for (size_t i = 0; i < motor_goals_.first.size(); i++) { + if (motor_goals_.first[i] == "LHipPitch") { + motor_goals_.second[i] += config_.fast_kick_hip_offset; + } + } + + for (size_t i = 0; i < motor_goals_.first.size(); i++) { + if (motor_goals_.first[i] == "LKnee") { + motor_goals_.second[i] -= config_.fast_kick_knee_offset; + } + } + + } +} + + + bool WalkEngine::isLeftSupport() const { return is_left_support_foot_; } bool WalkEngine::isDoubleSupport() { diff --git a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp index af15bbfc9..0a70189a8 100644 --- a/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp +++ b/bitbots_motion/bitbots_quintic_walk/src/walk_node.cpp @@ -198,24 +198,7 @@ bitbots_msgs::msg::JointCommand WalkNode::step(double dt) { // compute motor goals from IK motor_goals_ = ik_.calculate(current_stabilized_response_); - if (engine_state_ == WalkState::KICK) { - // in this state we do a kick while doing a step - if (left_kick_requested_ && !is_left_support_foot_) { - for (int i = 0; i < motor_goal_.first.size() - 1; i++) { - if (motor_goal.first[i].compare("LKnee")) { - motor_goal.second[i] += 0.1; - } - } - } - if (right_kick_requested_ && is_left_support_foot_) { - } - - if (half_step_finished) { - // kick step is finished, go on walking - buildTrajectories(TrajectoryType::NORMAL); - engine_state_ = WalkState::WALKING; - } - } + walk_engine_.fastKick(motor_goals_); command.header.stamp = node_->get_clock()->now(); /*