RSDK-9587 generate series of waypoints to cover surface given a pcd mesh #4719
Conversation
viambot
added
the
safe to test
| mp.logger.CInfof(ctx, "start node: %v\n", n.Q()) | ||
| break | ||
| } | ||
| mp.logger.Info("DOF", mp.lfs.dof) |
There was a problem hiding this comment.
I think debug level would be better
| @@ -463,6 +463,17 @@ IK: | |||
| // good solution, stopping early | |||
| break IK | |||
| } | |||
| for _, oldSol := range solutions { | |||
There was a problem hiding this comment.
Something I noticed was that when creating IK solutions, what was meant to be a diversity of different solutions was in fact essentially the exact same set of joint positions repeated 50 times over.
This forces deduplication of IK solutions, so that if multiple are returned, they are guaranteed to be at least some distance apart.
This should really be a top level default though, not a magic number in the code
There was a problem hiding this comment.
This should really be a top level default though, not a magic number in the code
if simscore < 0.1 {
continue IK
}
0.1 could be a constant
There was a problem hiding this comment.
Cool I like this optimization good idea. I can see another situation arising from this where we have more difficulty finding sufficiently different solutions. I assume there is a iteration cap putting a limit on how hard we try here?
| request.WorldState.String(), | ||
| ) | ||
| for i := range request.Goals { | ||
| request.Logger.Info("i", i) |
There was a problem hiding this comment.
In this current state this log message is not useful
| @@ -417,6 +417,7 @@ func (ms *builtIn) plan(ctx context.Context, req motion.MoveReq) (motionplan.Pla | |||
| if len(waypoints) == 0 { | |||
| return nil, errors.New("could not find any waypoints to plan for in MoveRequest. Fill in Destination or goal_state") | |||
| } | |||
| req.Extra["waypoints"] = nil | |||
There was a problem hiding this comment.
This line looks odd to me why do we need it?
There was a problem hiding this comment.
We've already extracted the data out of the "waypoints" entry.
Waypoints can be gigantic. The contents of extra is copied many times over, for example every time we spin up a new thread during motion planning. We may have hundreds or thousands of waypoints.
During earlier sanding testing before this line was added, something like 70% of planning runtime was making copies of extra.
There was a problem hiding this comment.
Sounds good can you comment something like this? Manipulating extras like this in this location is outside the norm and I think this context will be lost otherwise
| @@ -241,13 +241,13 @@ func (b *box) vertices() []r3.Vector { | |||
| } | |||
|
|
|||
| // vertices returns the vertices defining the box. | |||
| // mesh is a collision geometry that represents a set of triangles that represent a mesh. | ||
| type mesh struct { | ||
| // Mesh is a set of triangles at some pose. Triangle points are in the frame of the mesh. | ||
| type Mesh struct { |
There was a problem hiding this comment.
the way we've set spatialmath up has been to keep structs private with Geometry as the interface we use. I do not want to break this pattern. I will keep reading to see if there was a reason for this
edit: ok I see now that you arent implementing it as a Geometry. I don't want to put this in the public spatialmath package because you once it is public it becomes a lot harder to change. This is pretty experimental work and I think subject to change so either we ease off our stance of not making breaking changes to public structs/functions in packages, or we just put it into its own package in a private repo, since nothing upstream in RDK really relies on this at the moment
Add assorted public methods associated with Triangles and Meshes.
Also adds several helpful debug statements.