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.

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

Conversation

biotinker
Copy link
Member

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:

  • The starting state of the robot may be specified as a set of poses, rather than a configuration (current or otherwise). If so, IK will be used to find a starting configuration.
  • The goal state of the robot may be specified as a configuration, rather than a set of poses. This allows to plan to a known-good state, such as from a prior executed plan, or to connect to a plan created by specifying a start pose as mentioned above.

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature.
… goals, which can be configurations or sets of poses.
…-planning-both-from-and-to-either-a-pose-or-a-list-of-possible-configurations
@viambot viambot added the safe to test This pull request is marked safe to test from a trusted zone label Dec 12, 2024
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.

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.
@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 12, 2024
@biotinker biotinker requested a review from raybjork December 16, 2024 21:32
@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 16, 2024
@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 16, 2024
if err != nil {
return err
// If we have a start configuration, check for correctness
for fName, seed := range req.StartState.configuration {
Copy link
Member

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 {
Copy link
Member

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?

Copy link
Member Author

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
Copy link
Member

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
Copy link
Member

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?

Copy link
Member Author

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

if err != nil {
return err
// If we have a start configuration, check for correctness
for fName, seed := range req.StartState.configuration {
Copy link
Member

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

if err != nil {
return err
// If we have a start configuration, check for correctness
for fName, seed := range req.StartState.configuration {
Copy link
Member

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

@biotinker biotinker requested a review from raybjork December 18, 2024 01:25
@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 18, 2024
@@ -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)
Copy link
Member

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?

Copy link
Member Author

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

Copy link
Member Author

Choose a reason for hiding this comment

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

fixed

@biotinker biotinker requested a review from raybjork December 18, 2024 16:24
@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 18, 2024
…-planning-both-from-and-to-either-a-pose-or-a-list-of-possible-configurations
@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 18, 2024
@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 18, 2024
@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 18, 2024
@biotinker biotinker merged commit 594ca25 into viamrobotics:main Dec 18, 2024
17 of 18 checks passed
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
safe to test This pull request is marked safe to test from a trusted zone
Projects
None yet
Development

Successfully merging this pull request may close these issues.

None yet

4 participants