RSDK-4051 tp space should be able to continuously sample ptgs #2792
Conversation
…o-continuously-sample-pt-gs
…o-continuously-sample-pt-gs
…. Smoothing not there yet.
viambot
added
the
safe to test
viambot
added
safe to test
…o-continuously-sample-pt-gs
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
viambot
added
safe to test
| } | ||
|
|
||
| func (pf *ptgIKFrame) Geometries(inputs []referenceframe.Input) (*referenceframe.GeometriesInFrame, error) { | ||
| return nil, errors.New("geometries not implemented for ptg IK frame") |
There was a problem hiding this comment.
No, because this frame is only used for IK. We care about Geometries in the PTG group frame, where they are implemented.
Probably the constructor here should be private to make sure no one uses these elsewhere inadvertently.
There was a problem hiding this comment.
I'm confused why we even need this structure to be honest
There was a problem hiding this comment.
Like why can't we just use the group frame to do this?
There was a problem hiding this comment.
The 3dof structure of the group frame does not play nicely with the IK solver, on account of the IK solver assuming a continuous distribution of float change -> position gradient, while the first Input to a group frame is an integer representing which PTG family to use. We want the 3dof frame for actually interacting with the base and forming the plan, but a 2dof frame for solving.
viambot
added
safe to test
Availability
Quality
Performance
The above data was generated by running scenes defined in the
|
Code Coverage
|
| defer close(m1chan) | ||
| defer close(m2chan) | ||
|
|
||
| dist := math.Sqrt(mp.planOpts.DistanceFunc(&ik.Segment{StartPosition: startPose, EndPosition: goalPose})) |
There was a problem hiding this comment.
Shouldn't the distance function either be responsible for taking the sqrt if necessary?
There was a problem hiding this comment.
Generally yes. In this case we're just defining a region within which to sample, and it's better to under-estimate than over estimate. This is the only place we sqrt the distance, and it's deliberate here.
| nm.parallelNeighbors = 10 | ||
|
|
||
| var successNode node | ||
| // Get the distance function that will find the nearest RRT map node in TP-space of *this* PTG | ||
| ptgDistOpt := mp.algOpts.distOptions[curPtg] | ||
| if invert { | ||
| ptgDistOpt = mp.algOpts.invertDistOptions[curPtg] |
There was a problem hiding this comment.
I understand why the invert bool exists but it seems really hacky and I wonder if theres a better way to do this. Can't we just get the invert distance by sampleNode->treeNode and get normal distance by going treeNode->sample? So instead of adding a bool to these functions just change the order of the arguments being passed in?
There was a problem hiding this comment.
I agree this is really hacky. I'll brainstorm different ways to do this.
Can't we just get the invert distance by sampleNode->treeNode and get normal distance by going treeNode->sample?
That's what I originally implemented; unfortunately it doesn't work, as that's not quite how the problem needs to be formulated, due to the directionality of PTGs. You can't just invert the pose; you have to say "I need a pose X, such that when I compose pose A with the inverse of pose X, yields something close to pose B".
There may be a way to get it to fit together differently but my first three attempts failed.
| } | ||
| lastDist = trajPt.Dist | ||
| } | ||
| if mp.algOpts.pathdebug { |
There was a problem hiding this comment.
Do we want to be committing this into main?
There was a problem hiding this comment.
This has been SO darn useful and is the fourth time I've had to implement and then remove it. I think it's worth hiding behind an internal feature flag.
There was a problem hiding this comment.
I really like the idea actually and think if we're going to commit to it here we should also slowly work this in elsewhere
There was a problem hiding this comment.
There's a lot going on here and I'm good with the general shape of it. Its going to be a lot easier to iterate on it once its merged in rather than trying to change it too much in review, so I'm good to merge with the understanding that we'll be doing some future passes on it soon
This is a huge PR that fundamentally reworks the way that PTGs are used.
All computable PTGs have been given a
Transformfunction allowing a given pose on a trajectory to be computed. A ptg IK frame struct has been created allowing our IK solvers to solve a given PTG for a pose. The precomputed grid simulation of PTGs has been removed, instead we now precompute only the endpoint of each PTG trajectory.TP-space RRT now solves bidirectionally, and is highly effective at solving for orientation. This also solves RSDK-3598. Path smoothing has been implemented for TP-space paths and is able to improve the paths of TP-space as part of a normal motion planning call.
Most testing of this as of this writing has centered around the TP-space algorithm itself. I have not tested this on hardware yet. I would like to write a few more
*_test.gofiles as well.More documentation of code, and passes removing old obsolete comments, will be done in the next few commits.