Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Motion planning cleanup #1629

Merged
merged 17 commits into from
Dec 1, 2022
Merged

Motion planning cleanup #1629

merged 17 commits into from
Dec 1, 2022

Conversation

biotinker
Copy link
Member

This is a followup to this PR: #1604

It does a lot of organizational things that would have been nice to have in the original but were left out, for time/size purposes, namely:

  • Remove public SolvableFrameSystem
  • Reorganize the way planner options are set up to simplify access
  • Remove IK from the low-level planners and move to Planager
  • Reorganization/expansion of the planner generic from which all of our planners inherit

Additionally, it makes the following behavior changes to improve quality of path:

  • Implements rudimentary smoothing for RRT* paths (checks of simple interpolation of halfway points in RRT map); this was picked because it's very fast, super simple, and provably can only increase quality.
  • Previously we were doing direct interpolation only in the case where the best IK solution could interpolate. We change this to instead interpolate for any solution within some multiple of the best score.
  • Reorganize the fallback system to seed slightly differently, namely in the case where the first try produces a connected but bad path.

@viambot viambot added the safe to test This pull request is marked safe to test from a trusted zone label Nov 30, 2022
@biotinker
Copy link
Member Author

Here are the quality metrics; current main on the left, this PR on the right.

Massive increase in quality, and a pretty okay bump to perf.
5_seconds_results

@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Nov 30, 2022
@spieswl
Copy link
Contributor

spieswl commented Nov 30, 2022

Love the chart, BTW.

spieswl
spieswl approved these changes Nov 30, 2022
View reviewed changes
Copy link
Contributor

@spieswl spieswl left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

You're already addressing my comments, but in my opinion, nothing I've picked out needs to be adjusted.

raybjork
raybjork requested changes Nov 30, 2022
View reviewed changes
Copy link
Member

@raybjork raybjork left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Looking good! I left a bunch of comments about organization. Going to come back to this for a second pass and take a closer look at the smoothing we're doing because there are documented algorithms that we might want to look into rather than do our own thing

motionplan/planManager.go Outdated Show resolved Hide resolved
motionplan/planManager.go Outdated Show resolved Hide resolved
motionplan/planManager.go Outdated Show resolved Hide resolved
motionplan/planManager.go Show resolved Hide resolved
motionplan/planManager.go Outdated Show resolved Hide resolved
motionplan/rrtStarConnect.go Outdated Show resolved Hide resolved
motionplan/rrt.go Outdated Show resolved Hide resolved
motionplan/rrt.go Outdated Show resolved Hide resolved
motionplan/motionPlanner.go Outdated Show resolved Hide resolved
motionplan/motionPlanner.go Show resolved Hide resolved
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Nov 30, 2022
@biotinker biotinker requested a review from raybjork November 30, 2022 22:28
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Nov 30, 2022
raybjork
raybjork requested changes Dec 1, 2022
View reviewed changes
motionplan/utils.go Show resolved Hide resolved
motionplan/motionPlanner.go Outdated Show resolved Hide resolved
motionplan/planManager.go Outdated Show resolved Hide resolved
motionplan/plannerOptions.go Outdated Show resolved Hide resolved
motionplan/motionPlanner.go Show resolved Hide resolved
@biotinker biotinker requested a review from raybjork December 1, 2022 16:48
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Dec 1, 2022
@github-actions
Copy link
Contributor

github-actions bot commented Dec 1, 2022

Code Coverage

Package Line Rate Delta Health
go.viam.com/rdk/components/arm 59% 0.00% ➖
go.viam.com/rdk/components/arm/universalrobots 13% 0.00% ❌
go.viam.com/rdk/components/arm/xarm 2% 0.00% ❌
go.viam.com/rdk/components/arm/yahboom 7% 0.00% ❌
go.viam.com/rdk/components/audioinput 55% 0.00% ➖
go.viam.com/rdk/components/base 67% 0.00% ➖
go.viam.com/rdk/components/base/agilex 62% 0.00% ➖
go.viam.com/rdk/components/base/boat 41% 0.00% ❌
go.viam.com/rdk/components/base/wheeled 76% 0.00% ✅
go.viam.com/rdk/components/board 68% 0.00% ➖
go.viam.com/rdk/components/board/arduino 10% 0.00% ❌
go.viam.com/rdk/components/board/commonsysfs 47% 0.00% ❌
go.viam.com/rdk/components/board/fake 38% 0.00% ❌
go.viam.com/rdk/components/board/numato 19% 0.00% ❌
go.viam.com/rdk/components/board/pi 50% 0.00% ➖
go.viam.com/rdk/components/camera 65% 0.00% ➖
go.viam.com/rdk/components/camera/fake 67% 0.00% ➖
go.viam.com/rdk/components/camera/ffmpeg 77% 0.00% ✅
go.viam.com/rdk/components/camera/transformpipeline 77% -0.66% ✅
go.viam.com/rdk/components/camera/videosource 55% 0.00% ➖
go.viam.com/rdk/components/encoder/fake 77% 0.00% ✅
go.viam.com/rdk/components/gantry 68% 0.00% ➖
go.viam.com/rdk/components/gantry/multiaxis 84% 0.00% ✅
go.viam.com/rdk/components/gantry/oneaxis 86% 0.00% ✅
go.viam.com/rdk/components/generic 83% 0.00% ✅
go.viam.com/rdk/components/gripper 80% 0.00% ✅
go.viam.com/rdk/components/input 87% 0.00% ✅
go.viam.com/rdk/components/input/gpio 83% 0.00% ✅
go.viam.com/rdk/components/motor 81% 0.00% ✅
go.viam.com/rdk/components/motor/dimensionengineering 63% 0.00% ➖
go.viam.com/rdk/components/motor/dmc4000 69% 0.00% ➖
go.viam.com/rdk/components/motor/fake 57% 0.00% ➖
go.viam.com/rdk/components/motor/gpio 64% 0.00% ➖
go.viam.com/rdk/components/motor/gpiostepper 56% -0.59% ➖
go.viam.com/rdk/components/motor/tmcstepper 62% 0.00% ➖
go.viam.com/rdk/components/movementsensor 75% 0.00% ✅
go.viam.com/rdk/components/movementsensor/cameramono 40% 0.00% ❌
go.viam.com/rdk/components/movementsensor/gpsnmea 37% 0.00% ❌
go.viam.com/rdk/components/movementsensor/gpsrtk 28% 0.00% ❌
go.viam.com/rdk/components/posetracker 86% 0.00% ✅
go.viam.com/rdk/components/sensor 86% 0.00% ✅
go.viam.com/rdk/components/sensor/ultrasonic 34% 0.00% ❌
go.viam.com/rdk/components/servo 78% 0.00% ✅
go.viam.com/rdk/components/servo/gpio 71% 0.00% ✅
go.viam.com/rdk/config 76% 0.00% ✅
go.viam.com/rdk/control 57% 0.00% ➖
go.viam.com/rdk/data 78% +0.81% ✅
go.viam.com/rdk/grpc 25% 0.00% ❌
go.viam.com/rdk/ml 67% 0.00% ➖
go.viam.com/rdk/ml/inference 70% 0.00% ✅
go.viam.com/rdk/motionplan 59% -8.66% ➖
go.viam.com/rdk/octree 93% 0.00% ✅
go.viam.com/rdk/operation 82% 0.00% ✅
go.viam.com/rdk/pointcloud 71% 0.00% ✅
go.viam.com/rdk/protoutils 62% 0.00% ➖
go.viam.com/rdk/referenceframe 69% 0.00% ➖
go.viam.com/rdk/registry 88% 0.00% ✅
go.viam.com/rdk/resource 83% 0.00% ✅
go.viam.com/rdk/rimage 78% 0.00% ✅
go.viam.com/rdk/rimage/depthadapter 94% 0.00% ✅
go.viam.com/rdk/rimage/transform 73% 0.00% ✅
go.viam.com/rdk/rimage/transform/cmd/extrinsic_calibration 67% 0.00% ➖
go.viam.com/rdk/robot 93% 0.00% ✅
go.viam.com/rdk/robot/client 79% 0.00% ✅
go.viam.com/rdk/robot/framesystem 68% 0.00% ➖
go.viam.com/rdk/robot/impl 80% 0.00% ✅
go.viam.com/rdk/robot/server 59% 0.00% ➖
go.viam.com/rdk/robot/web 60% 0.00% ➖
go.viam.com/rdk/robot/web/stream 87% 0.00% ✅
go.viam.com/rdk/services/armremotecontrol 71% 0.00% ✅
go.viam.com/rdk/services/armremotecontrol/builtin 25% 0.00% ❌
go.viam.com/rdk/services/baseremotecontrol 71% 0.00% ✅
go.viam.com/rdk/services/baseremotecontrol/builtin 66% 0.00% ➖
go.viam.com/rdk/services/datamanager 65% 0.00% ➖
go.viam.com/rdk/services/datamanager/builtin 80% 0.00% ✅
go.viam.com/rdk/services/datamanager/datacapture 21% 0.00% ❌
go.viam.com/rdk/services/datamanager/datasync 72% 0.00% ✅
go.viam.com/rdk/services/motion 63% 0.00% ➖
go.viam.com/rdk/services/motion/builtin 88% 0.00% ✅
go.viam.com/rdk/services/navigation 54% 0.00% ➖
go.viam.com/rdk/services/sensors 77% 0.00% ✅
go.viam.com/rdk/services/sensors/builtin 97% 0.00% ✅
go.viam.com/rdk/services/shell 14% 0.00% ❌
go.viam.com/rdk/services/slam 84% 0.00% ✅
go.viam.com/rdk/services/slam/builtin 72% 0.00% ✅
go.viam.com/rdk/services/vision 80% 0.00% ✅
go.viam.com/rdk/services/vision/builtin 74% 0.00% ✅
go.viam.com/rdk/spatialmath 85% 0.00% ✅
go.viam.com/rdk/subtype 96% 0.00% ✅
go.viam.com/rdk/utils 71% 0.00% ✅
go.viam.com/rdk/vision 26% 0.00% ❌
go.viam.com/rdk/vision/chess 80% 0.00% ✅
go.viam.com/rdk/vision/delaunay 87% 0.00% ✅
go.viam.com/rdk/vision/keypoints 92% 0.00% ✅
go.viam.com/rdk/vision/objectdetection 82% 0.00% ✅
go.viam.com/rdk/vision/odometry 60% 0.00% ➖
go.viam.com/rdk/vision/odometry/cmd 0% 0.00% ❌
go.viam.com/rdk/vision/segmentation 49% 0.00% ❌
go.viam.com/rdk/web/server 26% 0.00% ❌
Summary 65% (19526 / 29922) -0.57% ➖

raybjork
raybjork approved these changes Dec 1, 2022
View reviewed changes
Copy link
Member

@raybjork raybjork left a comment

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

LGTM!

@biotinker biotinker merged commit 5544cbf into viamrobotics:main Dec 1, 2022
@bazile-clyde bazile-clyde mentioned this pull request Feb 7, 2023
Closed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers

@spieswl spieswl spieswl approved these changes

@raybjork raybjork raybjork approved these changes

Assignees
No one assigned
Labels
safe to test This pull request is marked safe to test from a trusted zone
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
4 participants
@biotinker @spieswl @raybjork @viambot