-
Notifications
You must be signed in to change notification settings - Fork 113
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.
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
RSDK-8835 motionplan should support planning both from and to either a pose or a list of possible configurations #4623
RSDK-8835 motionplan should support planning both from and to either a pose or a list of possible configurations #4623
Conversation
… goals, which can be configurations or sets of poses.
…-planning-both-from-and-to-either-a-pose-or-a-list-of-possible-configurations
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Overall I like the shape of it. I think we should think a little more about how we want to call these structs which will become the building blocks of this package. If we come up with something coherent I think it will make the mental load of understanding this package lighter to new developers
…, more to do on creating motion chains when goal is in frame of itself.
motionplan/motionPlanner.go
Outdated
if err != nil { | ||
return err | ||
// If we have a start configuration, check for correctness | ||
for fName, seed := range req.StartState.configuration { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
But poses aren't being computed here? I'm confused. The checks here are just that passed configurations are the correct DoF, and have a corresponding frame.
Based on the what validatePlanRequest
currently does on main this chunk of code does not compute poses. As you said, we make sure that passed configurations have the correct length, i.e. DoF.
// Theoretically, a plan could be made between two poses, by running IK on both the start and end poses to create sets of seed and | ||
// goal configurations. However, the blocker here is the lack of a "known good" configuration used to determine which obstacles | ||
// are allowed to collide with one another. | ||
if request.StartState.configuration == nil { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
a bit confused about this. Shouldn't the goal of validatePlanRequest
be to make sure that startState.configuration can be populated?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
A start configuration is not necessary if planning for TPspace, which is not something knowable at the time validatePlanRequest
is run. We need to do the extra processing up until this point to know if this is an error state or not.
…-planning-both-from-and-to-either-a-pose-or-a-list-of-possible-configurations
Start map[string][]frame.Input | ||
Goal PathStep | ||
Start *PlanState | ||
Goal *PlanState |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
[opt] could be Start, Goal *PlanState
@@ -34,11 +34,8 @@ const ( | |||
// Intended information flow should be: | |||
// motionplan.PlanMotion() -> SolvableFrameSystem.SolveWaypointsWithOptions() -> planManager.planSingleWaypoint(). | |||
type planManager struct { | |||
*planner | |||
*planner // TODO: This should probably be removed |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This whole struct more or less goes away doesn't it?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
There are a few things it manages still, like the RNG
motionplan/motionPlanner.go
Outdated
if err != nil { | ||
return err | ||
// If we have a start configuration, check for correctness | ||
for fName, seed := range req.StartState.configuration { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Sorry I wasn't very clear with my second comment. I think a Transform method on Framesystem would be a good method to have to convert a Configuration to a PathStep. I understand you aren't using that path step here, but we do this elsewhere in the codebase where we transform a model by an Input, throw away the result and check the error to see if the calculation succeeded. I think a corrolary to this operating on the Framesystem would be useful not only here but elsewhere as we move away from Inputs and towards PlanStates as the atomic data type
motionplan/motionPlanner.go
Outdated
if err != nil { | ||
return err | ||
// If we have a start configuration, check for correctness | ||
for fName, seed := range req.StartState.configuration { |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
This lets us not have iterate over frames and check their DOF which should be the purview of the referenceframe package and not the motionplan package. Its the blurring of the abstractions that we've established that I am hoping to correct
motionplan/rrt.go
Outdated
@@ -161,8 +161,41 @@ func newRRTPlan(solution []node, fs referenceframe.FrameSystem, relative bool, o | |||
return nil, errors.New("cannot construct a Plan using fewer than two nodes") | |||
} | |||
} | |||
traj := nodesToTrajectory(solution) |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Why does this need to change?
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Whoops. Didn't mean to commit this.
While making the sanding demo, I noticed some slight jitter in the arm at the same points each stroke.
The root cause was when we split linear motions into smaller segments for planning, the start/end states of each plan are duplicative with the previous and subsequent plans. Doubling up the configurations caused jitter.
However, this was an unintentional, earlier solution, there's a much better one. One sec
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
fixed
…-planning-both-from-and-to-either-a-pose-or-a-list-of-possible-configurations
This PR is not quite complete; there are not yet tests for the new behavior. Tests will be added before merge, and bugs turned up by the tests may merit minor changes to the body of code presented here.
This PR is a breaking change to
motionplan
. It introduces a new struct,PlanState
, which may be a PathStep, a configuration, or both.PlanRequest
undergoes the breaking change, removing several public fields and replacing them with new fields which must be PlanStates.The changes here enable an arbitrary number of waypoints to be specified, and the planner will plan to each one in turn. It will do so faster than it would if each one were an individual Plan call.
Additionally, it permits two new interesting pieces of behavior: