RSDK-8852 migrate motionplan to work on framesystems rather than individual frames #4559
Conversation
…k-on-framesystems-rather-than-individual-frames
…k-on-framesystems-rather-than-individual-frames
viambot
added
the
safe to test
viambot
added
safe to test
| @@ -587,11 +566,13 @@ func NewEmptyConstraints() *Constraints { | |||
| // NewConstraints initializes a Constraints object with user-defined LinearConstraint, OrientationConstraint, and CollisionSpecification. | |||
| func NewConstraints( | |||
| linConstraints []LinearConstraint, | |||
| pseudoConstraints []PseudolinearConstraint, | |||
There was a problem hiding this comment.
Not a fan of adding this here. This should be just for the constraints we expose through proto. You can always append to the Constraints with custom constraints after the fact
There was a problem hiding this comment.
Fair, I think I did this as a quick one-off to get some tests working. Will update before final review
There was a problem hiding this comment.
Actually, not sure on this.
Since this is the structure we use for our own consumption and passing around of constraints and initialization of actual constraint functions, these structures should I think be the union of what we support in proto, and what we support via things like extra. This allows us to better prototype what will eventually be added into proto. The alternative is creating a parallel data structure that we pass around to do the same thing, but which adds parameters, masks the putative new API, and makes it easier to forget things.
There was a problem hiding this comment.
Just saw this second reply now. I'm forgetting why we have the constraints typed this way in the first place. IIRC it used to just be a slice of Constraints. My comment makes a lot less sense now that I see this is no longer the case
| segmentConstraints map[string]SegmentConstraint | ||
| segmentFSConstraints map[string]SegmentFSConstraint | ||
| stateConstraints map[string]StateConstraint | ||
| stateFSConstraints map[string]StateFSConstraint |
There was a problem hiding this comment.
Could consider making these interfaces to cut down on the need for duplication. But if the vision is we're moving away from the original versions then probably not necessary
There was a problem hiding this comment.
I think they would need to be generics, not just interfaces.
But yes this is a stepping stone towards the eventual removal of the originals
There was a problem hiding this comment.
Are we even using the originals anymore? I gotta admit its a little hard for me to follow everything thats happening here
There was a problem hiding this comment.
Yes; they still are used for TPspace, which still operates on a single frame.
There was a problem hiding this comment.
Additionally, one of the intentions of this PR is that it does not break anything public so that the scope of the PR can be more limited. All public methods, etc remain valid everywhere. A future PR will be the one which does the breaking by removing these once they are no longer needed.
Adding in the public breaking changes will make it hard to know if any problems post-merge are caused by the breaking changes or the new behavior.
viambot
added
safe to test
…k-on-framesystems-rather-than-individual-frames
viambot
added
safe to test
viambot
added
safe to test
viambot
added
the
safe to test
viambot
added
safe to test
viambot
added
safe to test
| BoundingRegions []spatialmath.Geometry | ||
| Constraints *Constraints | ||
| Options map[string]interface{} | ||
| allGoals PathStep // TODO: this should replace Goal and Frame |
There was a problem hiding this comment.
[q]: will this be capitalized in future work?
[q]: why not make this a slice in case there is are specific poses we would like to move through?
There was a problem hiding this comment.
It will indeed be eventually capitalized.
I don't think there's a need for this to take an array; when pre-planning is fully operational, calling pre-plans iteratively will yield the same result while giving the caller extra control.
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
| segmentConstraints map[string]SegmentConstraint | ||
| segmentFSConstraints map[string]SegmentFSConstraint | ||
| stateConstraints map[string]StateConstraint | ||
| stateFSConstraints map[string]StateFSConstraint |
There was a problem hiding this comment.
Are we even using the originals anymore? I gotta admit its a little hard for me to follow everything thats happening here
viambot
added
safe to test
| @@ -217,20 +235,47 @@ func (p *plannerOptions) SetMinScore(minScore float64) { | |||
|
|
|||
| // addPbConstraints will add all constraints from the protobuf constraint specification. This will deal with only the topological | |||
| // constraints. It will return a bool indicating whether there are any to add. | |||
| func (p *plannerOptions) addPbTopoConstraints(from, to spatialmath.Pose, constraints *Constraints) bool { | |||
| func (p *plannerOptions) addPbTopoConstraints( | |||
There was a problem hiding this comment.
This function references pb Constraints, which is no longer the case we have our own notion of Constraints that we construct from it. We should remove the reference to this in the comment and function name.
I think if we do this, and recontextualize the Constraints object as "our version of constraints which go above and beyond the nominal set defined by proto" then I am on board with adding Psudeolinear to the constructor per our other discussion
This PR migrates
motionplanto removesolverFrames, and in general have all algorithms operate on entire framesystems rather than single frames.This allows new features such as enabling planning for multiple independent frames simultaneously, such as several arms moving near but independently from one another.
Most of the frame-specific motionplan functions and structures, such as constraints, now have a framesystem-specific counterpart.
The entire file for solver frames has been removed, and what little remains are now known as
motionChains, of which a planner may have many and which are used to inform which frames may move but do not limit planning. This PR does not change the external public Plan() function, and motion calls should continue to work the same way as they have, with few exceptions. An example of such an exception would be that since all components can now move, one may move out of the way of another if need be, something not possible before now.