RSDK-8683 - Checking if start is within planDevMM of goal should be handled by motionplan/ik package #4426

nfranczak · 2024-10-07T20:44:09Z

This is an improvement for this ticket RSDK-8481

The planner still runs, but on the first iteration of RRT we check if we are within PlanDevMM of the startPosition.
If we are, our returned plan is:

[0, 0, 0, 0],
[0, 0, 0, 0]

In this PR, we remove atGoalCheck from the moveRequest struct.
Since it is removed we redefine how we check for success once a plan has been completed.

…for tpSpace RRT

…ns struct

raybjork · 2024-10-08T17:58:24Z

motionplan/plannerOptions.go

@@ -186,6 +186,9 @@ type plannerOptions struct {
 // profile is the string representing the motion profile
 profile string

+ // planDeviationMM is a float representing how close we must be to the goal in order for planning to finish
+ planDeviationMM float64


How is this distinct from GoalThreshold?

let delta = abs(startPose - goalPose).
suppose we are on the first iteration of planning.
if delta <= planDevMM (should it exist) then we are done planning.

The difference is that GoalThreshold exists for all invocations of the planner, but planDevMM only exists for TPSpace RRT and is only used for the first iteration of planning.

Why would we not want the check if delta <= GoalThreshold for all planners? In this case there is no work to be done you are already there

I agree we don't want two separate values here. We should have the motion service pass the planDeviationMM as the goal threshold if we want to do the check.

Unsure at the moment if we want to pass the smaller or larger of the two numbers, thoughts?

I think that is makes more sense to use the larger of the two numbers.

At plan time, for a given moveBase call, we may already be within PlanDevMM of a goal but not within the goal threshold.

I'm now wondering if perhaps we do after all want planDeviationMm to be fully handled in the motion service.

I am also thinking about this. As you mentioned on the merged PR related to this (#4327 (comment)) this should be handled within the motionplan package. If not, then there is no work to be done other than perhaps adding some comments around existing code.

IMO, the only way to avoid the scenario where the base ends up outside the shed is to have plannerOptions include a field for planDevMM, and a supplementary field for planDevMM's success function only pertinent to the first iteration of our planner.

This itself however, highlights an improper abstraction we've made which makes me wonder how else to approach the problem/issue. I say this is an improper abstraction since planDevMM and its success function are un-used for plans not related to bases.

Okay, I'm looking more at the comment thread on that old PR, thinking about the behavior we've got, and here's what I've come up with.

The idea of planDevMM relies on the existence of a plan, and the fact that we may have deviated from it. It doesn't actually have anything to do with the generation of a plan.

Thinking along those lines, I think we should instead revisit the assumption that we should do nothing if we request a plan while being within planDevMM of the goal. Another interpretation would be simply that we would try to plan and move regardless, and this would simply disable replanning.

Bumping this comment thread since I think it got missed and deciding the behavior here is core to what this PR should do @raybjork @nfranczak

oop, I did miss this comment. Thank you for bumping the thread

The idea of planDevMM relies on the existence of a plan, and the fact that we may have deviated from it. It doesn't actually have anything to do with the generation of a plan.

This is a good point.

I think we should instead revisit the assumption that we should do nothing if we request a plan while being within planDevMM of the goal.

I think this is a safe assumption to be made. Suppose we are within planDevMM of the goal at start and we've generated some plan whose inputs are non-zero. Then executing that generated plan is pointless.

Another interpretation would be simply that we would try to plan and move regardless, and this would simply disable replanning.

I do not think this is safe. If the kinematic base driver is faulty for some reason we will end up executing the plan and might wind up somewhere that is outside of the defined planDevMM. This would then cause for a replan to be issued and we might end up stuck in a cycle of perpetual replanning.

Overall, I think the best way to handle this is to return a plan whose inputs are: [0, 0, 0, 0], [0, 0, 0, 0].

@raybjork what are your thoughts on this?

biotinker · 2024-10-14T16:50:02Z

motionplan/tpSpaceRRT.go

+
+ // Check to see if the startPose is already within some predefined delta of the goalPose
+ if iter == 0 {
+ atGoal := func(startPose, goalPose spatialmath.Pose) bool {


This is already handled (more accurately) by mp.planOpts.DistanceFunc. Use that instead of this.

will do.
question: we define DistanceFunc as L2InputMetric

InputsL2Distance returns the square of the two-norm between the from and to vectors.

does this account for when we are not operating in PositionOnly mode?

Ah, you're right, it wouldn't.

However, within the algorithm itself is not an appropriate place to be checking business logic such as Position Only Mode.

I think we need a third way.

However, within the algorithm itself is not an appropriate place to be checking business logic such as Position Only Mode.

It is not immediately obvious to me why you say that this shouldn't live at the algorithm level. Is it because it introduces special handling code/tech-debt? What are your thoughts?

If it does not live on the algorithm level, "it" being the early return from the planner, then "it" must live either before we enter the motionplan package or once we return from it. The main branch currently handles the latter -- the check existing once we exit the planner.

biotinker · 2024-10-14T16:51:20Z

motionplan/tpSpaceRRT.go

+ }
+ if atGoal(startPose, goalPose) {
+ path := extractTPspacePath(rrt.maps.startMap, rrt.maps.goalMap,
+ &nodePair{


Let's use the actual nodes in the map, rather than create new ones.

biotinker · 2024-10-14T16:54:37Z

motionplan/plannerOptions.go

@@ -186,6 +186,9 @@ type plannerOptions struct {
 // profile is the string representing the motion profile
 profile string

+ // planDeviationMM is a float representing how close we must be to the goal in order for planning to finish
+ planDeviationMM float64


I agree we don't want two separate values here. We should have the motion service pass the planDeviationMM as the goal threshold if we want to do the check.

Unsure at the moment if we want to pass the smaller or larger of the two numbers, thoughts?

biotinker · 2024-10-14T16:56:17Z

motionplan/tpSpaceRRT.go

@@ -306,6 +306,36 @@ func (mp *tpSpaceRRTMotionPlanner) rrtBackgroundRunner(
 rrt.solutionChan <- &rrtSolution{err: fmt.Errorf("TP Space RRT timeout %w", ctx.Err()), maps: rrt.maps}
 return
 }
+
+ // Check to see if the startPose is already within some predefined delta of the goalPose
+ if iter == 0 {


We should probably actually move this up to where we iterate the goal map on line 224 and check for every goal node.

biotinker · 2024-10-14T16:56:54Z

motionplan/motionPlanner.go

@@ -108,6 +108,16 @@ func (req *PlanRequest) validatePlanRequest() error {
 }
 }

+ _, ok := req.Options["planDeviationMM"].(float64)


This should be able to go away when this is merged with GoalThreshold as discussed below.

nfranczak · 2024-10-15T16:01:41Z

motionplan/planManager.go

@@ -905,6 +905,17 @@ func (pm *planManager) planRelativeWaypoint(ctx context.Context, request *PlanRe
 return nil, err
 }

+ if opt.profile == PositionOnlyMotionProfile {


@biotinker let me know your thoughts on this.
Business logic of caring for position only mode is not in the algorithm, however I did need to introduce a new field to plannerOpts.

github-actions · 2024-10-15T18:50:34Z

Availability

Scene #	viamrobotics:main	nfranczak:RSDK-8683	Percent Improvement	Health
1	100%	100%	0%	➖
2	100%	100%	0%	➖
3	100%	100%	0%	➖
4	100%	100%	0%	➖
5	90%	90%	0%	➖
6	70%	80%	14%	✅
7	20%	20%	0%	➖
8	100%	100%	0%	➖
9	90%	90%	0%	➖
10	100%	100%	0%	➖
11	100%	100%	0%	➖
12	100%	100%	0%	➖
13	100%	100%	0%	➖
14	100%	100%	0%	➖
15	100%	100%	0%	➖
16	90%	90%	0%	➖
17	100%	100%	0%	➖
18	70%	70%	0%	➖

Quality

Scene #	viamrobotics:main	nfranczak:RSDK-8683	Percent Improvement	Probability of Improvement	Health
1	1.31±0.00	1.31±0.00	-0%	50%	➖
2	0.90±0.00	0.90±0.00	-0%	50%	➖
3	2.48±0.13	2.48±0.13	-0%	50%	➖
4	4.04±1.26	3.86±1.12	4%	54%	➖
5	14.40±5.95	14.08±4.98	2%	52%	➖
6	17.56±4.22	18.28±3.52	-4%	45%	➖
7	5.28±1.61	5.28±1.61	-0%	50%	➖
8	4.99±1.08	4.99±1.08	-0%	50%	➖
9	4.52±0.14	4.52±0.14	0%	50%	➖
10	4.13±0.34	4.12±0.33	0%	51%	➖
11	3.13±0.00	3.13±0.00	-0%	50%	➖
12	3.91±0.85	4.00±0.98	-2%	47%	➖
13	904.77±13.78	904.77±13.78	-0%	50%	➖
14	2035.67±601.28	2035.67±601.28	-0%	50%	➖
15	50790.10±9415.58	50790.10±9415.58	-0%	50%	➖
16	59103.42±7272.96	59103.42±7272.96	-0%	50%	➖
17	15936.40±3302.53	15936.40±3302.53	-0%	50%	➖
18	120076.85±14640.44	119821.27±14955.31	0%	50%	➖

Performance

Scene #	viamrobotics:main	nfranczak:RSDK-8683	Percent Improvement	Probability of Improvement	Health
1	0.09±0.01	0.09±0.00	4%	66%	➖
2	0.10±0.01	0.11±0.01	-1%	46%	➖
3	1.01±0.62	1.01±0.62	0%	50%	➖
4	2.01±0.12	2.00±0.13	0%	52%	➖
5	2.69±0.93	2.80±0.91	-4%	47%	➖
6	3.06±0.89	3.15±1.04	-3%	48%	➖
7	2.89±0.58	2.92±0.58	-1%	48%	➖
8	0.39±0.15	0.39±0.15	0%	50%	➖
9	4.96±0.21	4.98±0.21	-0%	48%	➖
10	0.14±0.02	0.14±0.02	2%	53%	➖
11	0.10±0.00	0.10±0.00	-1%	43%	➖
12	0.12±0.01	0.13±0.01	-1%	44%	➖
13	0.07±0.00	0.07±0.01	-4%	38%	➖
14	0.78±0.27	0.79±0.27	-2%	48%	➖
15	1.10±0.17	1.11±0.17	-1%	48%	➖
16	2.48±0.81	2.52±0.81	-2%	49%	➖
17	1.28±0.14	1.29±0.15	-0%	49%	➖
18	4.65±0.79	4.65±0.79	-0%	50%	➖

The above data was generated by running scenes defined in the motion-testing repository
The SHA1 for viamrobotics:main is: 900738751a11aa6be03dd90a3932f38e1c35f187
The SHA1 for nfranczak:RSDK-8683 is: 900738751a11aa6be03dd90a3932f38e1c35f187

10 samples were taken for each scene
A timeout of 5.0 seconds was imposed for each trial

nfranczak added 3 commits October 7, 2024 16:03

support early exit from planner if start is within planDevMM of goal …

37aa50d

…for tpSpace RRT

no need to define a metric in plannerOptions.go

5cab475

update comment describing what planDeviationMM is in the plannerOptio…

57e2042

…ns struct

nfranczak requested review from raybjork and biotinker October 7, 2024 20:44

viambot added the safe to test This pull request is marked safe to test from a trusted zone label Oct 7, 2024

raybjork reviewed Oct 8, 2024

View reviewed changes

nfranczak requested a review from raybjork October 8, 2024 19:46

biotinker requested changes Oct 14, 2024

View reviewed changes

have planDevMM remain distinct from goalThreshold

cfe285d

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 Oct 15, 2024

nfranczak commented Oct 15, 2024

View reviewed changes

fix test

0e04e43

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 Oct 15, 2024

fix TestPtgRrtBidirectional

7f7b026

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 Oct 15, 2024

nfranczak requested a review from biotinker October 15, 2024 18:44

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

RSDK-8683 - Checking if start is within planDevMM of goal should be handled by motionplan/ik package #4426

RSDK-8683 - Checking if start is within planDevMM of goal should be handled by motionplan/ik package #4426

nfranczak commented Oct 7, 2024 •

edited by jira bot

Loading

raybjork Oct 8, 2024

nfranczak Oct 8, 2024

raybjork Oct 10, 2024

biotinker Oct 14, 2024

nfranczak Oct 14, 2024

nfranczak Oct 14, 2024 •

edited

Loading

biotinker Oct 15, 2024

biotinker Oct 18, 2024

nfranczak Oct 18, 2024

nfranczak Oct 21, 2024

biotinker Oct 14, 2024

nfranczak Oct 14, 2024

biotinker Oct 14, 2024

nfranczak Oct 14, 2024

biotinker Oct 14, 2024

biotinker Oct 14, 2024

biotinker Oct 14, 2024

biotinker Oct 14, 2024

nfranczak Oct 15, 2024

github-actions bot commented Oct 15, 2024

RSDK-8683 - Checking if start is within planDevMM of goal should be handled by motionplan/ik package #4426

Are you sure you want to change the base?

RSDK-8683 - Checking if start is within planDevMM of goal should be handled by motionplan/ik package #4426

Conversation

nfranczak commented Oct 7, 2024 • edited by jira bot 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

nfranczak Oct 14, 2024 • 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

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

github-actions bot commented Oct 15, 2024

Availability

Quality

Performance

nfranczak commented Oct 7, 2024 •

edited by jira bot

Loading

nfranczak Oct 14, 2024 •

edited

Loading