Skip to content

Commit

Permalink
Rewrite linekicker (#2163)
Browse files Browse the repository at this point in the history 
Co-authored-by: Sid Parikh <[email protected]>
Co-authored-by: github-actions[bot] <41898282+github-actions[bot]@users.noreply.github.com>
Co-authored-by: petergarud <[email protected]>
Co-authored-by: sid-parikh <[email protected]>
  • Loading branch information
@petergarud @sid-parikh @github-actions
5 people authored Jan 27, 2024
1 parent 9d6acba commit 26caeb4
Show file tree
Hide file tree
Showing 6 changed files with 105 additions and 236 deletions.
3 changes: 3 additions & 0 deletions rj_constants/include/rj_constants/constants.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -30,5 +30,8 @@ constexpr float kDotsSmallOffset = 0.035;
constexpr float kDotsLargeOffset = 0.054772;
constexpr float kDotsRadius = 0.02;

/** constants for planning */
constexpr double kAvoidBallDistance = 0.01;

const std::string kTeamNameLower = "robojackets";
const std::string kTeamName = "RoboJackets";
279 changes: 58 additions & 221 deletions soccer/src/soccer/planning/planner/line_kick_path_planner.cpp
View file
Original file line number Diff line number Diff line change
@@ -1,4 +1,4 @@
#include "line_kick_path_planner.hpp"
#include "planning/planner/line_kick_path_planner.hpp"

#include <rj_geometry/util.hpp>

Expand All @@ -13,30 +13,13 @@ using namespace rj_geometry;
namespace planning {

Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) {
// TODO(?): ros param these
const float approach_speed = 0.05;

const float ball_avoid_distance = 0.10;

const MotionCommand& command = plan_request.motion_command;

if (plan_request.virtual_obstacles.hit(plan_request.start.position())) {
prev_path_ = Trajectory{};
return prev_path_;
if (plan_request.play_state == PlayState::halt() ||
plan_request.play_state == PlayState::stop()) {
return Trajectory{};
}

if (target_kick_pos_.has_value() &&
command.target.position.dist_to(target_kick_pos_.value()) > 0.1) {
prev_path_ = Trajectory{};
target_kick_pos_ = std::nullopt;
}
const BallState& ball = plan_request.world_state->ball;

const RobotInstant& start_instant = plan_request.start;
const auto& motion_constraints = plan_request.constraints.mot;
const auto& rotation_constraints = plan_request.constraints.rot;
const auto& ball = plan_request.world_state->ball;

// track ball velocity to know if done or not
if (!average_ball_vel_initialized_) {
average_ball_vel_ = ball.velocity;
average_ball_vel_initialized_ = true;
Expand All @@ -50,230 +33,84 @@ Trajectory LineKickPathPlanner::plan(const PlanRequest& plan_request) {
average_ball_vel_ = apply_low_pass_filter(average_ball_vel_, ball.velocity, 0.8);
}

if (prev_path_.empty()) {
final_approach_ = false;
// only process state transition if already started the planning
if (has_created_plan) {
process_state_transition();
}

ShapeSet static_obstacles;
std::vector<DynamicObstacle> dynamic_obstacles;
fill_obstacles(plan_request, &static_obstacles, &dynamic_obstacles, false, nullptr);

auto obstacles_with_ball = static_obstacles;
const RJ::Time cur_time = start_instant.stamp;
obstacles_with_ball.add(
make_shared<Circle>(ball.predict_at(cur_time).position, ball_avoid_distance));

// This segfaults. Please rewrite the entire thing.
#if 0
if (final_approach_ && target_kick_pos_) {
RJ::Seconds duration_into_path = cur_time - prev_path_.begin_time();

RobotInstant target = prev_path_.last();
RJ::Time time = ball.query_time_near(*target_kick_pos_);

auto time_left = prev_path_.duration() - duration_into_path;

if (time_left < RJ::Seconds(-0.3) || time_left > RJ::Seconds(5.0)) {
final_approach_ = false;
prev_path_ = {};
} else if (time_left < RJ::Seconds(0)) {
prev_path_.set_debug_text("reuse past done " + std::to_string(time_left.count()));
return prev_path_;
} else {
RJ::Seconds time_for_ball = time - cur_time;
prev_path_.scale_duration(prev_path_.duration() * (time_left / time_for_ball),
start_instant.stamp);
prev_path_.set_debug_text("reuse final slow " + std::to_string(time_for_ball.count()) +
" " + std::to_string(time_left.count()));
prev_path_.stamp(RJ::now());
return prev_path_;
}
switch (current_state_) {
case INITIAL_APPROACH:
prev_path_ = initial(plan_request);
break;
case FINAL_APPROACH:
prev_path_ = final(plan_request);
break;
}
#endif

// only plan line kick if not is_done
if (!this->is_done()) {
LinearMotionInstant target{
ball.position, (command.target.position - ball.position).normalized(approach_speed)};

auto ball_trajectory = ball.make_trajectory();

Trajectory path;
if (std::abs(target.velocity.angle_between((target.position - start_instant.position()))) >
degrees_to_radians(10)) {
target.position -= target.velocity.normalized(ball_avoid_distance * 2 + kRobotRadius);
if (!prev_path_.empty() &&
target.position.dist_to(prev_path_.last().position()) <
Replanner::goal_pos_change_threshold() &&
reuse_path_count_ < 20) {
target.velocity = prev_path_.last().linear_velocity();
reuse_path_count_++;
} else {
reuse_path_count_ = 0;
}
prev_path_.stamp(RJ::now());
has_created_plan = true;
return prev_path_;
}

Replanner::PlanParams params{
start_instant,
target,
obstacles_with_ball,
dynamic_obstacles,
plan_request.constraints,
AngleFns::face_angle(ball.position.angle_to(command.target.position))};
path = Replanner::create_plan(params, prev_path_);
path.set_debug_text("slow ball 1");
} else {
if (!prev_path_.empty() &&
target.position.dist_to(prev_path_.last().position()) <
Replanner::goal_pos_change_threshold() &&
reuse_path_count_ < 20) {
target.velocity = prev_path_.last().linear_velocity();
reuse_path_count_++;
} else {
reuse_path_count_ = 0;
}
Trajectory LineKickPathPlanner::initial(const PlanRequest& plan_request) {
// Getting ball info
const BallState& ball = plan_request.world_state->ball;

target.position += target.velocity.normalized(kRobotRadius);
// Creating a modified plan_request to send to PathTargetPlanner
PlanRequest modified_request = plan_request;

Replanner::PlanParams params{start_instant,
target,
static_obstacles,
dynamic_obstacles,
plan_request.constraints,
AngleFns::face_point(command.target.position)};
path = Replanner::create_plan(params, prev_path_);
path.set_debug_text("slow ball 2");
}
target_kick_pos_ = command.target.position;
path.stamp(RJ::now());
prev_path_ = path;
return path;
}
// Where to navigate to
auto distance_from_ball = kBallRadius + kRobotRadius + kAvoidBallBy;

return Trajectory{};
// (Kevin) pretty sure everything under this line is not used
auto ball_position = ball.predict_at(RJ::now() + RJ::Seconds{kPredictIn}).position;

if (!prev_path_.empty() && target_kick_pos_) {
auto previous_duration_remaining = prev_path_.end_time() - start_instant.stamp;
auto goal_to_ball = (plan_request.motion_command.target.position - ball_position);
auto offset_from_ball = distance_from_ball * goal_to_ball.normalized();

LinearMotionInstant target;
RJ::Time intercept_time = ball.query_time_near(*target_kick_pos_, &target.position);
RJ::Time end_time_adjusted = prev_path_.end_time() - RJ::Seconds(1.0);
if (previous_duration_remaining < RJ::Seconds(0.0)) {
target.velocity =
(command.target.position - target.position).normalized(approach_speed);
target.position -= target.velocity.normalized(kRobotRadius + kBallRadius * 2);
// Updating the motion command
LinearMotionInstant target{ball_position - offset_from_ball};

Replanner::PlanParams params{start_instant,
target,
static_obstacles,
dynamic_obstacles,
plan_request.constraints,
AngleFns::face_point(command.target.position)};
Trajectory path = Replanner::create_plan(params, prev_path_);
MotionCommand modified_command{"path_target", target,
FacePoint{plan_request.motion_command.target.position}};
modified_request.motion_command = modified_command;

if (!path.empty()) {
path.set_debug_text(
"FinalPath" + std::to_string(path.duration().count()) + " " +
std::to_string(RJ::Seconds(intercept_time - start_instant.stamp).count()) +
" " + std::to_string(intercept_time.time_since_epoch().count()));
path.stamp(RJ::now());
prev_path_ = path;
return path;
}
}
// Getting the new path from PathTargetPlanner
Trajectory path = path_target_.plan(modified_request);

if (prev_path_.check_time(start_instant.stamp) &&
!trajectory_hits_static(prev_path_, static_obstacles, start_instant.stamp, nullptr) &&
end_time_adjusted < intercept_time && reuse_path_count_ < 10) {
reuse_path_count_++;
Point near_point;
prev_path_.set_debug_text("Reuse prev_path");
if (ball.query_time_near(prev_path_.last().position(), &near_point) >=
end_time_adjusted) {
return prev_path_;
}
}
}
return path;
}

Trajectory partial_path;
RJ::Seconds partial_path_time = 0ms;
auto tmp_start_instant = start_instant;
const auto partial_replan_lead_time = RJ::Seconds(Replanner::partial_replan_lead_time());
Trajectory LineKickPathPlanner::final(const PlanRequest& plan_request) {
// remove ball from obstacles not needed?

if (!prev_path_.empty() && prev_path_.check_time(start_instant.stamp)) {
if (start_instant.stamp < prev_path_.end_time() - partial_replan_lead_time * 2) {
partial_path = prev_path_.sub_trajectory(
start_instant.stamp, start_instant.stamp + partial_replan_lead_time);
partial_path_time = partial_replan_lead_time;
tmp_start_instant = partial_path.last();
}
}
const BallState& ball = plan_request.world_state->ball;

for (auto t = RJ::Seconds(0.4); t < RJ::Seconds(6); t += RJ::Seconds(0.2)) {
RJ::Time rollout_time = cur_time + t;
// Creating a modified plan_request to send to PathTargetPlanner
PlanRequest modified_request = plan_request;

auto ball_state_predicted = ball.predict_at(rollout_time);
LinearMotionInstant target{ball_state_predicted.position};
target_kick_pos_ = target.position;
target.velocity = (command.target.position - target.position).normalized(approach_speed);
target.position -= target.velocity.normalized(kRobotRadius + kBallRadius * 2);
// velocity
auto goal_to_ball = (plan_request.motion_command.target.position - ball.position);
auto vel = goal_to_ball.normalized() * kFinalRobotSpeed;

vector<Point> intermediate_points;
if (std::abs(target.velocity.angle_between(
(target.position - tmp_start_instant.position()))) > degrees_to_radians(60)) {
intermediate_points.push_back(
target.position -
target.velocity.normalized(kRobotRadius * 2.0 + kBallRadius * 2.0));
}
LinearMotionInstant target{ball.position, vel};
MotionCommand modified_command{"path_target", target,
FacePoint{plan_request.motion_command.target.position}, true};
modified_request.motion_command = modified_command;

Trajectory path = CreatePath::simple(tmp_start_instant.linear_motion(), target,
plan_request.constraints.mot, tmp_start_instant.stamp,
intermediate_points);
// Getting the new path from PathTargetPlanner
Trajectory path = path_target_.plan(modified_request);

if (!path.empty()) {
if (path.duration() + partial_path_time <= t) {
if (!partial_path.empty()) {
path = Trajectory(std::move(partial_path), path);
}
plan_angles(&path, tmp_start_instant, AngleFns::face_point(target.position),
plan_request.constraints.rot);
return path;
}

path.set_debug_text("FoundPath" + std::to_string(path.duration().count()));
reuse_path_count_ = 0;
path.stamp(RJ::now());
prev_path_ = path;
return path;
}
}
void LineKickPathPlanner::process_state_transition() {
if (current_state_ == INITIAL_APPROACH && (path_target_.is_done())) {
current_state_ = FINAL_APPROACH;
}

auto ball_predicted = ball.predict_at(cur_time);
LinearMotionInstant target{ball_predicted.position};
target.velocity = (command.target.position - target.position).normalized(approach_speed);
target.position -= target.velocity.normalized(kRobotRadius * 3);

auto ball_path = ball.make_trajectory();
dynamic_obstacles.emplace_back(kBallRadius, &ball_path);

Replanner::PlanParams params{start_instant,
target,
static_obstacles,
dynamic_obstacles,
plan_request.constraints,
AngleFns::face_point(command.target.position)};
Trajectory path = Replanner::create_plan(params, prev_path_);

path.set_debug_text("Approaching cautious");

path.stamp(RJ::now());
prev_path_ = path;
return path;
}

bool LineKickPathPlanner::is_done() const {
// if ball is fast, assume we have kicked it correctly
// (either way we can't go recapture it)
return average_ball_vel_.mag() > IS_DONE_BALL_VEL;
return average_ball_vel_.mag() > kIsDoneBallVel;
}

} // namespace planning
35 changes: 26 additions & 9 deletions soccer/src/soccer/planning/planner/line_kick_path_planner.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,7 +2,9 @@

#include <optional>

#include "planning/planner/collect_path_planner.hpp"
#include "planning/planner/path_planner.hpp"
#include "planning/planner/path_target_path_planner.hpp"
#include "planning/trajectory.hpp"

class Configuration;
Expand All @@ -29,25 +31,40 @@ class LineKickPathPlanner : public PathPlanner {

void reset() override {
prev_path_ = {};
final_approach_ = false;
target_kick_pos_ = std::nullopt;
reuse_path_count_ = 0;
current_state_ = INITIAL_APPROACH;
average_ball_vel_initialized_ = false;
has_created_plan = false;
}
[[nodiscard]] bool is_done() const override;

private:
enum State { INITIAL_APPROACH, FINAL_APPROACH };
State current_state_ = INITIAL_APPROACH;
PathTargetPathPlanner path_target_{};
CollectPathPlanner collect_planner_{};
Trajectory prev_path_;
bool final_approach_ = false;
std::optional<rj_geometry::Point> target_kick_pos_;
int reuse_path_count_ = 0;

// ball velocity filtering vars
// These constants could be tuned more
static constexpr double kIsDoneBallVel = 1.5;
static constexpr double kFinalRobotSpeed = 1.0;
static constexpr double kPredictIn = 0.5; // seconds
static constexpr double kAvoidBallBy = 0.05;

rj_geometry::Point average_ball_vel_;
bool average_ball_vel_initialized_ = false;
bool has_created_plan = false;

Check warning on line 56 in soccer/src/soccer/planning/planner/line_kick_path_planner.hpp

View workflow job for this annotation

GitHub Actions / build-and-test 

invalid case style for private member 'has_created_plan'
std::optional<rj_geometry::Point> target_kick_pos_;

// Trajectory initial(BallState ball, MotionCommand command, RobotInstant start_instant,
// ShapeSet static_obstacles, std::vector<DynamicObstacle> dynamic_obstacles);
Trajectory initial(const PlanRequest& plan_request);
Trajectory final(const PlanRequest& plan_request);
// Trajectory final(BallState ball, MotionCommand command, RobotInstant start_instant, ShapeSet
// static_obstacles, std::vector<DynamicObstacle> dynamic_obstacles);
void process_state_transition();

// TODO(Kevin): make this a common param ("ball is slow" used
// in a lot of places)
double IS_DONE_BALL_VEL = 0.5; // m/s
// PlayState::State current_state_;
};

} // namespace planning
Loading

0 comments on commit 26caeb4

Please sign in to comment.