Is done planners #1911

kfu02 · 2022-06-21T01:42:34Z

Description

Adds is_done methods to each planner, which return true if the planner is done with its job and false otherwise. This sets the groundwork for more advanced planner behavior (e.g. ball placement #1904 ) on the C++ side.

Also tuned the capture skill for ER-force simulator. Did not tune other skills as that would have taken too long.

Associated / Resolved Issue

Resolves # or ClickUp card

Design Documents

None, based the new is_done() methods off of the existing planner logic + gameplay Skill equivalents.

Steps to test

Test Case 1

Run motion planning test play named test_motion_planning.AllBots (second test_motion_planning play).
Notice the debug messages.

Expected result: Debug msgs say "robot x's PathTargetPlanner is_done"

Note: other planners have is_done, but are annoying to test because gameplay Skill is_dones override the planner is_dones. (Ask Kevin to provide more clarification if that doesn't make sense.)

Key Files to Review

Gameplay Changes

rj_gameplay/skill/* - simple tweaks to existing is_dones for better performance
other rj_gameplay/* and config/ - bugfixes or new motion planning test play

new nearly_equals()

rj_common/*
rj_geometry/*
soccer/src/soccer/planning/instant.hpp

Main Planning is_done changes

soccer/src/soccer/planning/*

Review Checklist

Docstrings: All methods and classes should have the file appropriate docstrings which follow the guidelines in the "Contributing" page of our docs.
Remove extra print statements: Any print statements used for debugging should be removed*
Tag reviewers: Tag some people for review and ping them on Slack

*intentionally left some debug prints to illustrate working behavior, can delete after

kasohrab

looks great! I left some small comments that should mostly be quick to address.

kasohrab · 2022-07-07T02:54:24Z

soccer/src/soccer/planning/planner/intercept_planner.hpp

@@ -18,5 +18,7 @@ class InterceptPlanner : public PlannerForCommandType<InterceptCommand> {
        : PlannerForCommandType<InterceptCommand>("InterceptPlanner"){};

    Trajectory plan(const PlanRequest& request) override;
+
+    [[nodiscard]] bool is_done() const override;


we should make all of these is_done methods also constexpr. You cannot make virtual functions constexpr in cpp17 which we use right now, but when I switched to cpp20, this change worked. Marking as constexpr offloads all the work to compile time. Since doing this requires changing our cpp version, we can do it in a different pr if you want. I still think it should be done at some point.

I'm a little hesitant to introduce large changes to our build system at the moment. I think we should focus on functionality rather than staying on top of the latest C++/ROS/Ubuntu version. Obviously we will have to move at some point but right now a move would prevent us from building a working product. Maybe a GH issue # for later?

referenced in issue #1920

kasohrab · 2022-07-07T02:55:47Z

soccer/src/soccer/planning/planner/settle_planner.cpp

+    // Dampen is done when the ball is slow-ish, let collect handle actually
+    // taking possession
+    // TODO(Kevin): consider making settle + collect one planner
+    double SETTLE_BALL_SPEED_THRESHOLD = 0.75;


github actions is complaining about the case style, even though it eventually will be a param.

hmm. I can fix all the style issues or let the warnings persist. what do you think?

the reason I didn't make any of them into ROS params already is because I don't love our existing custom param server and wanted to experiment with using default ROS params first. but that seemed like its own PR

referenced in clickup card https://app.clickup.com/t/31g2yfy

kasohrab · 2022-07-07T02:57:35Z

soccer/src/soccer/planning/planner_node.cpp

@@ -22,7 +22,7 @@ PlannerNode::PlannerNode()
    robots_planners_.reserve(kNumShells);
    for (int i = 0; i < kNumShells; i++) {
        auto planner =
-            std::make_unique<PlannerForRobot>(i, this, &robot_trajectories_, &shared_state_);
+            std::make_shared<PlannerForRobot>(i, this, &robot_trajectories_, &shared_state_);


Is there a reason this need to be a shared pointer now? My understanding is that shared pointers have a lot more overhead.

If my understanding is correct, this needed to be shared so I could assign the current_planner_ field to whichever planner is the current planner (see line 128). Let me know if that's incorrect usage

yes that's correct. and looking into it a little more, it's not a big deal actually, since these pointers are only created at startup and destroyed on shutdown. And the overhead comes from those activities primarily.

soccer/src/soccer/planning/planner/settle_planner.hpp

kasohrab · 2022-07-07T03:02:03Z

soccer/src/soccer/planning/planner_node.cpp

@@ -158,11 +163,26 @@ Trajectory PlannerForRobot::plan_for_robot(const planning::PlanRequest& request)

    debug_draw_.publish();

+    // TODO(Kevin): delete this debug print
+    if (this->is_done()) {
+        SPDLOG_ERROR("robot {}'s {} is_done", robot_id_, current_planner_->name());


this was helpful for reviewing 👍🏽

soccer/src/soccer/planning/planner_node.hpp

soccer/src/soccer/planning/planner/path_target_planner.hpp

soccer/src/soccer/planning/planner/planner.hpp

soccer/src/soccer/planning/planner/path_target_planner.hpp

soccer/src/soccer/planning/planner/pivot_path_planner.hpp

Co-authored-by: Kasra Alexander Sohrab <[email protected]>

kasohrab

LGTM. I would still make ClickUp cards/issues for the commented todos and small things I mentioned when reviewing.

p-nayak11

I left a couple comments about issues that I noticed while testing. They aren't major issues; however, if the suggestions help, it would avoid future problems. Overall, planners are working much better. Steady progress being made :)

p-nayak11 · 2022-07-08T02:08:59Z

rj_gameplay/rj_gameplay/role/seeker.py

@@ -32,7 +32,9 @@ def get_open_point(self, world_state, region: List, centroid) -> np.ndarray:
        # line-of-sight calculations
        # (LOS is maximized by making dot product of angle between unit vectors as close to 0 as possible)
        ball_dir = lambda pos: pos - world_state.ball.pos
-        ball_dir_norm = lambda pos: ball_dir(pos) / np.linalg.norm(ball_dir(pos))
+        ball_dir_norm = lambda pos: ball_dir(pos) / (
+            1e-9 + np.linalg.norm(ball_dir(pos))


What difference did this addition make?

if the norm of the ball dir vector is exactly 0, this causes a div by 0 error without the + tiny value

p-nayak11 · 2022-07-08T02:55:31Z

soccer/src/soccer/planning/planner_node.cpp

            trajectory = planner->plan(request);
        }

        // If it fails, or if the planner was not used, the trajectory will
        // still be empty. Reset the planner.
        if (trajectory.empty()) {
+            current_planner_ = nullptr;


I noticed that while running sim2play, either blue or yellow would often not move until the last minute. I am assuming that this could be stemming from this section right here. Could you reduce the number of resets from every tick to a range of ticks similar to how seeker is given a new target point.

I'm not sure if this would work, but it could at least let the robots being the trajectory and let the replanner do its purpose rather than resetting the whole trajectory from null.

the current_planner_ var doesn't actually do anything other than store a ptr to the planner selected in the for-loop above (lines 127-130). Notice there that the trajectory is taken directly from one of the planners in the for-loop.

the replanner resetting from null is because it gets a fresh RobotIntent from gameplay every tick. another reason why we need to overhaul gameplay to use actions instead of a blind gameplay->RobotIntent->planning pipeline

(for clarification, the ptr is only necessary here for the debug prints below, where I need to get the name of whatever planner is saying it's done)

p-nayak11 · 2022-07-08T02:59:50Z

soccer/src/soccer/planning/planning_params.cpp

@@ -62,7 +62,7 @@ DEFINE_NS_FLOAT64(
 //
 // High number indicates that it will always try to choose a point nearest
 // to the current robot position
-DEFINE_NS_FLOAT64(kPlanningParamModule, collect, ball_speed_approach_direction_cutoff, 0.1,
+DEFINE_NS_FLOAT64(kPlanningParamModule, collect, ball_speed_approach_direction_cutoff, 1.0,


Isn't there a higher chance of us bouncing into the ball if we increase the speed at which we approach for capture?

the relevant lines of code in the collect planner are here. you can see that when the ball speed is < this thresh, the robot will try to go directly at it, while if the speed is > this thresh, the robot will try to serve as a backstop.

what I noticed in my testing is that the robot didn't backstop successfully very often, so I tuned a bunch of params including this and the accel_scale ones below. those changes combined actually led to less bouncing, but it's hard to isolate any of them. if you want, you can make a card that's like "tune capture" but I think a) that's not in the scope of this PR and b) that's not easy to do when gameplay-controlled passing has some timing bugs. I think once ball placement is up (with passing) it will be easier to see, so I decided to wait on tuning this and any other skills

p-nayak11

Your explanations make sense. LGTM

kfu02 added 5 commits June 20, 2022 15:03

add some comments

60c6921

stub out isdone

91a1361

Merge branch 'ros2' into is-done-planners

038188f

make is_done return false for every planner

19e3064

add pathtarget isdone

261904b

kfu02 mentioned this pull request Jun 26, 2022

[TODO] Connect gameplay to new is_done() in planner #1913

Open

kfu02 added 8 commits June 26, 2022 19:31

add nearly_equals to linear motion instant

c4c6399

change pathtarget and python is_done to check desired vel

e32a2a3

cleanup pass

7e92cb8

fix clang warnings

582d150

add new capture only play

3e5ae27

tweak capture is_done python, fix bugs

fe5e463

tune capture params

d6741cb

add collect is_done

4fa0488

kfu02 force-pushed the is-done-planners branch from 6c6bedd to 4fa0488 Compare July 5, 2022 23:09

kfu02 added 12 commits July 5, 2022 19:27

apply_low_pass_filter commented and used

30633fe

add settle is done

60cc38a

"add" escape obstacles is_done

e33dd11

add kick test play, reenable capture is done

427d91a

add line kick is done

69b4f41

pivot is done

677c1d5

fix style

96c4e5a

bugfix

3ff66a6

more style fixes

80f952e

add docstrings for new nearly_equals

b319af4

sad admission

5ae4ae6

add docstring

9427f4b

kfu02 requested review from kasohrab and p-nayak11 July 6, 2022 01:57

kfu02 marked this pull request as ready for review July 6, 2022 01:58

kasohrab suggested changes Jul 7, 2022

View reviewed changes

kfu02 and others added 3 commits July 7, 2022 13:35

add nodiscard flags

174e131

Co-authored-by: Kasra Alexander Sohrab <[email protected]>

add another nodiscard

28a13a8

Co-authored-by: Kasra Alexander Sohrab <[email protected]>

make cached vars private

4d5f97e

kfu02 requested a review from kasohrab July 7, 2022 17:42

kasohrab approved these changes Jul 7, 2022

View reviewed changes

kfu02 mentioned this pull request Jul 7, 2022

Upgrade to cpp20 #1920

Open

Merge branch 'ros2' into is-done-planners

dc31aa1

p-nayak11 requested changes Jul 8, 2022

View reviewed changes

kfu02 requested a review from p-nayak11 July 8, 2022 04:55

p-nayak11 approved these changes Jul 8, 2022

View reviewed changes

kfu02 merged commit 1c8d2d6 into ros2 Jul 8, 2022

kfu02 deleted the is-done-planners branch July 8, 2022 05:05

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Is done planners #1911

Is done planners #1911

kfu02 commented Jun 21, 2022 •

edited

Loading

kasohrab left a comment

kasohrab Jul 7, 2022

kfu02 Jul 7, 2022

kfu02 Jul 7, 2022

kasohrab Jul 7, 2022

kfu02 Jul 7, 2022 •

edited

Loading

kfu02 Jul 7, 2022

kasohrab Jul 7, 2022

kfu02 Jul 7, 2022

kasohrab Jul 7, 2022

kasohrab Jul 7, 2022

kasohrab left a comment

p-nayak11 left a comment

p-nayak11 Jul 8, 2022

kfu02 Jul 8, 2022

p-nayak11 Jul 8, 2022

p-nayak11 Jul 8, 2022

kfu02 Jul 8, 2022

kfu02 Jul 8, 2022

kfu02 Jul 8, 2022

p-nayak11 Jul 8, 2022

kfu02 Jul 8, 2022

p-nayak11 left a comment

Is done planners #1911

Is done planners #1911

Conversation

kfu02 commented Jun 21, 2022 • edited Loading

Description

Associated / Resolved Issue

Design Documents

Steps to test

Test Case 1

Key Files to Review

Review Checklist

kasohrab left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kfu02 Jul 7, 2022 • edited Loading

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kasohrab left a comment

Choose a reason for hiding this comment

p-nayak11 left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

p-nayak11 left a comment

Choose a reason for hiding this comment

kfu02 commented Jun 21, 2022 •

edited

Loading

kfu02 Jul 7, 2022 •

edited

Loading