RSDK-5391 - Create distinct planning and execution frames for PTG KinematicWrapper #3876

nfranczak · 2024-05-01T16:25:43Z

Want:

getTransientDetections uses framesystem, (FS), object to place obstacles into the world frame
only use start and end configurations for ik.State which is checked with CheckStateConstraints within CheckPlan
want currentInputs to tell us where we actually are, not where we think we are

ToDos:

remove localizationFrame as an argument to CheckPlan
run obstacle avoidance hardware test on a viam rover equipped with a vision service to ensure functionality is preserved

… off the planningFrame within execution.go

…ts and not transform off a pose if we are relative

…ntroduced absolute frame system which houses the execution frame + planning frame

components/base/kinematicbase/fake_kinematics.go

nfranczak · 2024-05-01T16:31:24Z

motionplan/solverFrame.go

@@ -148,21 +149,22 @@ func newSolverFrame(fs frame.FrameSystem, solveFrameName, goalFrameName string,
 	}

 	var ptgs []tpspace.PTGSolver
-	anyPTG := false     // Whether PTG frames have been observed
-	anyNonzero := false // Whether non-PTG frames
+	anyPTG := false // Whether PTG frames have been observed


I needed to comment this out since we have introduced an executionFrame into the mix for solving...
Not immediately sure how to circumvent this.

bump looking for advice on this

We may want to move

// re-root the frame system on the relative frame relativeOnlyFS, err := pm.frame.fss.FrameSystemSubset(request.Frame)

upstream of this so that this is a non-issue.

hmmmmm, this is quite tricky...
I will attempt to describe why I think this is tricky best I can. First we describe our nomenclature.
Let AbsFS := localizationFrame - planningFrame - camera and,
Let RelFS := planningFrame - camera

solveFrame.go wants to be fed in RelFS. The "why" it wants RelFS is the result of anyNonzero and anyPTG being true should we choose to pass in AbsFS.

Once we are done with constructing our solverFrame, we then proceed to PlanSingleWaypoint and then into planRelativeWaypoint. From there, we enter plannerSetupFromMoveRequest.

We note that: One of the goals of the body of work is the allow placing of robot geometries exclusively through a FrameSystem and its corresponding seedMap.

This is where things get tricky. We have simplified the code to become

// create robot collision entities movingGeometriesInFrame, err := pm.frame.Geometries(frameInputs)

Where pm.Frame is actually the solverFrame entity we have created upstream.
If we step into the solverFrame.Geometries method we iterate over sf.movingFS which was constructed using the RelFS. This will end up incorrectly placing the robot collision entities.

Hence, solverFrame code wants RelFS while plannerSetupFromMoveRequest code wants AbsFS.
For this reason, I say that

We may want to move

// re-root the frame system on the relative frame relativeOnlyFS, err := pm.frame.fss.FrameSystemSubset(request.Frame)

upstream of this so that this is a non-issue.

is not the solution, given what we want to accomplish.

I am certain that there is a clean solution to the issue I have described above, I just need to think on it a little longer.
Would you agree with my problem description?

I take this comment back, I think what I wrote is wrong

fleshing out the details now but I think the solution here is that we will want to combine the
LocalizationFrame (poseFrame) with the PlanningFrame(ptgGroupFrame) into a singular frame called WrappedFrame. This will circumvent this issue and we will allows to un-comment this code.

Yesterday, I went down the wrong train of thought as I thought this is an issue from plan construction. Instead, this is an issue for interacting with CheckPlan.

spoke with ray, he recommended I use a referenceframe.Model instead of building a one off WrapperFrame

spoke with Peter, moving the any-non zero check into planRelativeWayPoint circumvents this issue and we can pass in AbsFS := localizationFrame - planningFrame - camera to solverFrame construction

motionplan/tpSpaceRRT_test.go

nfranczak · 2024-05-01T16:33:40Z

services/motion/builtin/move_request.go

@@ -58,7 +58,6 @@ type moveRequest struct {
 	requestType requestType
 	// geoPoseOrigin is only set if requestType == requestTypeMoveOnGlobe
 	geoPoseOrigin     *spatialmath.GeoPose
-	poseOrigin        spatialmath.Pose


no need to have this anymore since we use the absoluteFS to directly place observed obstacles into the world frame.

raybjork

This is really cool, just one request for a rename. Looks clean now thanks for following through on all our comments

raybjork · 2024-06-11T15:40:19Z

services/motion/builtin/move_request.go

@@ -67,6 +66,7 @@ type moveRequest struct {
 	obstacleDetectors map[vision.Service][]resource.Name
 	replanCostFactor  float64
 	fsService         framesystem.Service
+	absoluteFS        referenceframe.FrameSystem


can we just name this fs or frameSystem?

I would like to have a descriptive name for the field name, what are your thoughts on checkPlanFS.
I want to communicate through this name that we have the following elements in the FS:

checkPlanFS := localizationFrame -- planningFrame -- other_children

opposed to

regular_FS_topology_we've_used_until_now: := planningFrame -- other_children

How about localizingFS then? I think the word absolute can be a little confusing

sounds good to me!

biotinker

Overall looking really nice. I think there's a substantial simplification left to be made to CheckPlan, but otherwise only small comments

biotinker · 2024-06-13T20:54:35Z

components/base/kinematicbase/fake_kinematics.go

+			// we intentionally set our OX, OY, OZ values to be greater than 1 since
+			// a possible orientation which a planner produces may have the following
+			// values: OZ: 1.0000000002, Theta: t.
+			{Min: -1.5, Max: 1.5},


I wonder if all of these should be +/- infinity, and then normalized to a unit vector and +/- pi. That makes mildly more sense to me than having a buffer of 0.5.

biotinker · 2024-06-13T20:55:47Z

referenceframe/frame.go

+}
+
+// NewPoseFrame created a frame with 7 degrees of freedom, x, y, z, OX, OY, OZ, and theta in radians.
+func NewPoseFrame(name string, limits []Limit, geometry spatial.Geometry) (Frame, error) {


Shouldn't the limits be always the same? Why pass them in?

good point, I will update this.

biotinker · 2024-06-13T20:57:13Z

motionplan/check.go

@@ -168,7 +173,7 @@ func checkPlanRelative(
 	expectedCurrentPose := spatialmath.PoseBetweenInverse(remainingArcPose, expectedArcEndInWorld.Pose())
 	errorState := spatialmath.PoseBetween(expectedCurrentPose, currentPoseInWorld.Pose())

-	planStartPiF, ok := plan.Path()[0][checkFrame.Name()]
+	planStartPiF, ok := plan.Path()[wayPointIdx][checkFrame.Name()]


Why is this changing?

Good question.
planStartPiF is turned into planStartPoseWorld which is then passed into plannerSetupFromMoveRequest.

On main and within plannerSetupFromMoveRequest we would then transform the solverFrame off our seedMap of type map[string][]referenceframe.Input. But, since this seedMap contains relative inputs, the transform's result would incorrectly place us in a relative position. We were then able to position ourselves absolutely since we were composing the relative position with planStartPoseWorld.

Now suppose that our wayPointIdx is 2. Then, the planStartPiF should reflect that.
However, this itself is not entirely correct either.

Since solverFrame.Transform(seedMap) will now place us in our absolute position from the get-go, we do not need to compose its output with anything else. Hence, I believe the correct course of action here is to actually remove planStartPiF, and planStartPoseWorld and instead pass in currentPoseInWorld into plannerSetupFromMoveRequest

The intended use of planStartPoseWorld is to place all frames where they were when the plan was created, as this will accurately reflect which things may or may not be in collision, something the current inputs may not do.

As the first element of the plan will generally be all 0 inputs for a relative plan, that should not have placed the frame incorrectly.

That said, relying on the first set of inputs to be 0 is bad, and thus this PR will be an improvement. But we will still want to use index 0 of the plan.

as this will accurately reflect which things may or may not be in collision, something the current inputs may not do.

This was my missing mental link. Thank you for explaining :) I am pretty sure I get it now.

I want to ensure that I understand the "why" we may not rely on current inputs to correctly inform us about allowed/un-allowed collisions.

Suppose we have just started executing a plan and we are still close to our start pose.
Suppose that we have some transient obstacle named X which is positioned on the path, e.g. 2 meters ahead.
If for some reason at this iteration of CheckPlan we do not correctly identify collision with X then we risk having it being included in our zeroCg?

motionplan/check.go

biotinker · 2024-06-13T21:01:38Z

motionplan/check.go

+		return err
+	}
+	currentWayPointTraj := plan.Trajectory()[wayPointIdx]
+	currentWayPointTraj[localizationFrame.Name()] = referenceframe.FloatsToInputs([]float64{


IMO it would make more sense for this to live in the motion service. I think if we do that, it removes the need to pass in the localization frame specially.

hmmmmm, this is an interesting suggestion.

The first route I took to attempt at accomplishing this suggestion was to augment the existing plan object we pass into CheckPlan to house inputs information relating to the LocalizationFrame.

However, I do not believe that it is possible to accomplish this (this way) due to:

nextInputs[localizationFrame.Name()] = startInputs[localizationFrame.Name()]

Above, is a part of the for loop we construct to create the necessary segments to iterate over.
Since we want our nextInputs to equal startInputs for the LocalizationFrame, there is no way AFAIK to augment the existing plan to accomplish this.

biotinker · 2024-06-13T21:03:44Z

referenceframe/frame.go

+	geometry spatial.Geometry
+}
+
+// NewPoseFrame created a frame with 7 degrees of freedom, x, y, z, OX, OY, OZ, and theta in radians.


Let's specifically call out that this is an Orientation Vector

referenceframe/frame.go

biotinker · 2024-06-13T21:07:42Z

services/motion/builtin/move_request.go

@@ -305,6 +301,21 @@ func (mr *moveRequest) obstaclesIntersectPlan(
 	return state.ExecuteResponse{}, nil
 }

+func (mr *moveRequest) createInputMap(baseExecutionState motionplan.ExecutionState) map[string][]referenceframe.Input {
+	inputMap := referenceframe.StartPositions(mr.localizaingFS)


This should call CurrentInputs() on everything InputEnabled, not assume their inputs are zero.

Otherwise we're laying landmines for ourselves to hit when we start allowing arms on bases

github-actions · 2024-06-17T19:58:49Z

Availability

Scene #	viamrobotics:main	nfranczak:RSDK-5391	Percent Improvement	Health
1	100%	100%	0%	➖
2	100%	100%	0%	➖
3	100%	100%	0%	➖
4	100%	100%	0%	➖
5	90%	90%	0%	➖
6	90%	90%	0%	➖
7	90%	80%	-11%	❌
8	100%	100%	0%	➖
9	100%	100%	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	100%	100%	0%	➖
17	100%	100%	0%	➖
18	80%	80%	0%	➖

Quality

Scene #	viamrobotics:main	nfranczak:RSDK-5391	Percent Improvement	Probability of Improvement	Health
1	1.31±0.00	1.31±0.00	-0%	50%	➖
2	0.91±0.00	0.91±0.00	-0%	50%	➖
3	2.51±0.23	2.51±0.23	0%	50%	➖
4	3.62±0.99	3.66±1.10	-1%	49%	➖
5	16.56±10.61	18.09±12.83	-9%	46%	➖
6	20.80±13.35	18.31±10.39	12%	56%	➖
7	5.81±2.55	5.63±2.87	3%	52%	➖
8	5.05±1.04	5.05±1.03	-0%	50%	➖
9	4.63±0.82	4.63±0.66	0%	50%	➖
10	4.13±0.34	4.26±0.38	-3%	40%	➖
11	3.13±0.00	3.13±0.00	-0%	50%	➖
12	4.18±1.30	3.91±0.85	7%	57%	➖
13	869.73±143.97	869.47±143.73	0%	50%	➖
14	1685.06±410.69	1685.06±410.69	-0%	50%	➖
15	57034.21±1352.64	57034.23±1352.62	-0%	50%	➖
16	60745.27±6621.97	60541.90±6799.53	0%	51%	➖
17	13085.85±2789.97	13085.85±2789.97	-0%	50%	➖
18	119141.26±8891.71	119721.84±7692.19	-0%	48%	➖

Performance

Scene #	viamrobotics:main	nfranczak:RSDK-5391	Percent Improvement	Probability of Improvement	Health
1	5.50±2.87	5.50±2.87	-0%	50%	➖
2	5.50±2.87	5.50±2.87	-0%	50%	➖
3	5.50±2.87	5.50±2.87	-0%	50%	➖
4	5.50±2.87	5.50±2.87	-0%	50%	➖
5	5.89±2.77	5.89±2.77	-0%	50%	➖
6	5.89±2.77	5.89±2.77	-0%	50%	➖
7	5.33±2.98	5.75±2.90	-8%	46%	➖
8	5.50±2.87	5.50±2.87	-0%	50%	➖
9	5.50±2.87	5.50±2.87	-0%	50%	➖
10	5.50±2.87	5.50±2.87	-0%	50%	➖
11	5.50±2.87	5.50±2.87	-0%	50%	➖
12	5.50±2.87	5.50±2.87	-0%	50%	➖
13	5.50±2.87	5.50±2.87	-0%	50%	➖
14	5.50±2.87	5.50±2.87	-0%	50%	➖
15	5.50±2.87	5.50±2.87	-0%	50%	➖
16	5.50±2.87	5.50±2.87	-0%	50%	➖
17	5.50±2.87	5.50±2.87	-0%	50%	➖
18	5.00±2.92	5.00±2.92	-0%	50%	➖

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

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

nfranczak added 23 commits April 1, 2024 13:13

initial add planning and execution frames for bases commit

247d4d1

continue with solverFrame - not the best idea

652ddcc

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

685d518

merge with main:

de3189d

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

413cfa3

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

14fc7fe

initial commit - rework move_req to rely on fs for placing obstacles

2aab600

have fs place objects into worldframe within getTransientDetections

6a49f8a

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

aa98dea

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

ff9e945

clean up + note about how to interpolate

44b3287

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

cde23a6

remove '&' from orientation printout when doing plan.Path.String()

ba2be7d

add ExecutionFrame function impl + correct references to transforming…

526b14d

… off the planningFrame within execution.go

simplify how we create robot collision entities to only use frameInpu…

656d154

…ts and not transform off a pose if we are relative

fulfill placing obstacles into the world frame using only the newly i…

09c27a8

…ntroduced absolute frame system which houses the execution frame + planning frame

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

662ef08

clean up remove print statements

a8eca26

clean up remove print statements

61d8fc1

clean up remove print statements

5bc3a0a

remove print stmts

7c07b15

remove note - clean up

7185d6e

logic complete-ish, sans tests

d454d21

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

nfranczak commented May 1, 2024

View reviewed changes

components/base/kinematicbase/fake_kinematics.go Outdated Show resolved Hide resolved

nfranczak commented May 1, 2024

View reviewed changes

motionplan/tpSpaceRRT_test.go Show resolved Hide resolved

nfranczak commented May 1, 2024

View reviewed changes

fix segment logic

5e39ce8

viambot removed the safe to test This pull request is marked safe to test from a trusted zone label May 1, 2024

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

nfranczak added 2 commits June 6, 2024 12:16

remove comment

03ed80a

update poseFrame interpolate method to use spatial.Interpolate

3978b66

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 Jun 6, 2024

update testptgcheckplan to have correct starting inputs

56636f4

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 Jun 6, 2024

nfranczak added 3 commits June 7, 2024 13:54

debugging TestObstacleReplanningGlobe

ba682e7

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

7afc8ed

fixed TestObstacleReplanningGlobe

ee14bf1

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 Jun 7, 2024

nfranczak requested a review from biotinker June 7, 2024 18:51

raybjork approved these changes Jun 11, 2024

View reviewed changes

nfranczak added 2 commits June 12, 2024 10:06

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

9864457

rename absoluteFS to localizingFS

f43114d

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 Jun 12, 2024

update naming

34352cd

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 Jun 12, 2024

biotinker requested changes Jun 13, 2024

View reviewed changes

nfranczak added 4 commits June 14, 2024 16:36

Merge branch 'main' of github.com:viamrobotics/rdk into RSDK-5391

59481af

generate seed map with all input-enabled components

d4e1c11

use zeroth index of plan to construct our zeroCG

3fc360b

noramlize the orientation vector limits when set to infinity

3409b40

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 Jun 17, 2024

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

RSDK-5391 - Create distinct planning and execution frames for PTG KinematicWrapper #3876

RSDK-5391 - Create distinct planning and execution frames for PTG KinematicWrapper #3876

nfranczak commented May 1, 2024 •

edited

nfranczak May 1, 2024

nfranczak May 29, 2024

biotinker May 29, 2024

nfranczak May 30, 2024

nfranczak May 31, 2024

nfranczak May 31, 2024

nfranczak Jun 5, 2024

nfranczak Jun 5, 2024

nfranczak May 1, 2024

raybjork left a comment

raybjork Jun 11, 2024

nfranczak Jun 12, 2024

raybjork Jun 12, 2024

nfranczak Jun 12, 2024

biotinker left a comment

biotinker Jun 13, 2024

biotinker Jun 13, 2024

nfranczak Jun 17, 2024

biotinker Jun 13, 2024

nfranczak Jun 17, 2024

biotinker Jun 17, 2024

biotinker Jun 17, 2024

nfranczak Jun 17, 2024 •

edited

biotinker Jun 13, 2024

nfranczak Jun 17, 2024 •

edited

biotinker Jun 13, 2024

biotinker Jun 13, 2024

github-actions bot commented Jun 17, 2024

RSDK-5391 - Create distinct planning and execution frames for PTG KinematicWrapper #3876

Are you sure you want to change the base?

RSDK-5391 - Create distinct planning and execution frames for PTG KinematicWrapper #3876

Conversation

nfranczak commented May 1, 2024 • edited

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

raybjork 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

biotinker 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

nfranczak Jun 17, 2024 • edited

Choose a reason for hiding this comment

Choose a reason for hiding this comment

nfranczak Jun 17, 2024 • edited

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 Jun 17, 2024

Availability

Quality

Performance

nfranczak commented May 1, 2024 •

edited

nfranczak Jun 17, 2024 •

edited

nfranczak Jun 17, 2024 •

edited