RSDK-9587 generate series of waypoints to cover surface given a pcd mesh (#4719)

Co-authored-by: Ray Bjorkman <[email protected]>

Author: biotinker

motionplan/cBiRRT.go

@@ -142,7 +142,12 @@ func (mp *cBiRRTMotionPlanner) rrtBackgroundRunner(
 			break
 		}
 	}
-	mp.logger.CInfof(ctx, "goal node: %v\n", rrt.maps.optNode.Q())
+	mp.logger.CDebugf(ctx, "goal node: %v\n", rrt.maps.optNode.Q())
+	for n := range rrt.maps.startMap {
+		mp.logger.CDebugf(ctx, "start node: %v\n", n.Q())
+		break
+	}
+	mp.logger.Debug("DOF", mp.lfs.dof)
 	interpConfig, err := referenceframe.InterpolateFS(mp.fs, seed, rrt.maps.optNode.Q(), 0.5)
 	if err != nil {
 		rrt.solutionChan <- &rrtSolution{err: err}

motionplan/motionPlanner.go

@@ -21,6 +21,10 @@ import (
 	"go.viam.com/rdk/spatialmath"
 )
 
+// When we generate solutions, if a new solution is within this level of similarity to an existing one, discard it as a duplicate.
+// This prevents seeding the solution tree with 50 copies of essentially the same configuration.
+const defaultSimScore = 0.05
+
 // motionPlanner provides an interface to path planning methods, providing ways to request a path to be planned, and
 // management of the constraints used to plan paths.
 type motionPlanner interface {
@@ -464,6 +468,17 @@ IK:
 						// good solution, stopping early
 						break IK
 					}
+					for _, oldSol := range solutions {
+						similarity := &ik.SegmentFS{
+							StartConfiguration: oldSol,
+							EndConfiguration:   step,
+							FS:                 mp.fs,
+						}
+						simscore := mp.planOpts.configurationDistanceFunc(similarity)
+						if simscore < defaultSimScore {
+							continue IK
+						}
+					}
 
 					solutions[score] = step
 					if len(solutions) >= nSolutions {

motionplan/planManager.go

@@ -63,11 +63,6 @@ type atomicWaypoint struct {
 // planMultiWaypoint plans a motion through multiple waypoints, using identical constraints for each
 // Any constraints, etc, will be held for the entire motion.
 func (pm *planManager) planMultiWaypoint(ctx context.Context, request *PlanRequest, seedPlan Plan) (Plan, error) {
-	startPoses, err := request.StartState.ComputePoses(request.FrameSystem)
-	if err != nil {
-		return nil, err
-	}
-
 	opt, err := pm.plannerSetupFromMoveRequest(
 		request.StartState,
 		request.Goals[0],
@@ -101,26 +96,12 @@ func (pm *planManager) planMultiWaypoint(ctx context.Context, request *PlanReque
 
 	waypoints := []atomicWaypoint{}
 
-	runningStart := startPoses
-	for i, goal := range request.Goals {
-		goalPoses, err := goal.ComputePoses(request.FrameSystem)
-		if err != nil {
-			return nil, err
-		}
-
-		// Log each requested motion
-		// TODO: PlanRequest.String() could begin to exist
-		for frame, stepgoal := range goalPoses {
-			request.Logger.CInfof(ctx,
-				"setting up motion for frame %s\nGoal: %v\nstartPose %v\nworldstate: %v\n",
-				frame,
-				referenceframe.PoseInFrameToProtobuf(stepgoal),
-				referenceframe.PoseInFrameToProtobuf(runningStart[frame]),
-				request.WorldState.String(),
-			)
+	for i := range request.Goals {
+		select {
+		case <-ctx.Done():
+			return nil, ctx.Err()
+		default:
 		}
-		runningStart = goalPoses
-
 		// Solving highly constrained motions by breaking apart into small pieces is much more performant
 		goalWaypoints, err := pm.generateWaypoints(request, seedPlan, i)
 		if err != nil {
@@ -156,13 +137,17 @@ func (pm *planManager) planAtomicWaypoints(
 	var seed referenceframe.FrameSystemInputs
 
 	// try to solve each goal, one at a time
-	for _, wp := range waypoints {
+	for i, wp := range waypoints {
 		// Check if ctx is done between each waypoint
 		select {
 		case <-ctx.Done():
 			return nil, ctx.Err()
 		default:
 		}
+		pm.logger.Info("planning step ", i, " of ", len(waypoints), ":", wp.goalState)
+		for k, v := range wp.goalState.Poses() {
+			pm.logger.Info(k, v)
+		}
 
 		var maps *rrtMaps
 		if seedPlan != nil {
@@ -227,7 +212,16 @@ func (pm *planManager) planSingleAtomicWaypoint(
 	wp atomicWaypoint,
 	maps *rrtMaps,
 ) (referenceframe.FrameSystemInputs, *resultPromise, error) {
-	pm.logger.Debug("start planning for ", wp.goalState.configuration, wp.goalState.poses)
+	fromPoses, err := wp.startState.ComputePoses(pm.fs)
+	if err != nil {
+		return nil, nil, err
+	}
+	toPoses, err := wp.goalState.ComputePoses(pm.fs)
+	if err != nil {
+		return nil, nil, err
+	}
+	pm.logger.Debug("start configuration", wp.startState.Configuration())
+	pm.logger.Debug("start planning from\n", fromPoses, "\nto\n", toPoses)
 
 	if _, ok := wp.mp.(rrtParallelPlanner); ok {
 		// rrtParallelPlanner supports solution look-ahead for parallel waypoint solving

motionplan/plannerOptions.go

@@ -27,7 +27,7 @@ const (
 	defaultPseudolinearTolerance = 0.8
 
 	// Number of IK solutions that should be generated before stopping.
-	defaultSolutionsToSeed = 50
+	defaultSolutionsToSeed = 100
 
 	// Check constraints are still met every this many mm/degrees of movement.
 	defaultResolution = 2.0

services/motion/builtin/builtin.go

@@ -417,6 +417,12 @@ 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")
 	}
+	// The contents of waypoints can be gigantic, and if so, making copies of `extra` becomes the majority of motion planning runtime.
+	// As the meaning from `waypoints` has already been extracted above into its proper data structure, there is no longer a need to
+	// keep it in `extra`.
+	if req.Extra != nil {
+		req.Extra["waypoints"] = nil
+	}
 
 	// re-evaluate goal poses to be in the frame of World
 	// TODO (RSDK-8847) : this is a workaround to help account for us not yet being able to properly synchronize simultaneous motion across

spatialmath/box.go

@@ -56,7 +56,7 @@ type box struct {
 	halfSize        [3]float64
 	boundingSphereR float64
 	label           string
-	mesh            *mesh
+	mesh            *Mesh
 	rotMatrix       *RotationMatrix
 	once            sync.Once
 }
@@ -240,14 +240,14 @@ func (b *box) vertices() []r3.Vector {
 	return verts
 }
 
-// vertices returns the vertices defining the box.
-func (b *box) toMesh() *mesh {
+// toMesh returns a 12-triangle mesh representation of the box, 2 right triangles for each face.
+func (b *box) toMesh() *Mesh {
 	if b.mesh == nil {
-		m := &mesh{pose: b.pose}
-		triangles := make([]*triangle, 0, 12)
+		m := &Mesh{pose: b.pose}
+		triangles := make([]*Triangle, 0, 12)
 		verts := b.vertices()
 		for _, tri := range boxTriangles {
-			triangles = append(triangles, newTriangle(verts[tri[0]], verts[tri[1]], verts[tri[2]]))
+			triangles = append(triangles, NewTriangle(verts[tri[0]], verts[tri[1]], verts[tri[2]]))
 		}
 		m.triangles = triangles
 		b.mesh = m

spatialmath/capsule.go

@@ -261,7 +261,7 @@ func capsuleVsBoxDistance(c *capsule, other *box) float64 {
 
 // IMPORTANT: meshes are not considered solid. A mesh is not guaranteed to represent an enclosed area. This will measure ONLY the distance
 // to the closest triangle in the mesh.
-func capsuleVsMeshDistance(c *capsule, other *mesh) float64 {
+func capsuleVsMeshDistance(c *capsule, other *Mesh) float64 {
 	lowDist := math.Inf(1)
 	for _, t := range other.triangles {
 		// Measure distance to each mesh triangle
@@ -273,7 +273,7 @@ func capsuleVsMeshDistance(c *capsule, other *mesh) float64 {
 	return lowDist
 }
 
-func capsuleVsTriangleDistance(c *capsule, other *triangle) float64 {
+func capsuleVsTriangleDistance(c *capsule, other *Triangle) float64 {
 	capPt, triPt := closestPointsSegmentTriangle(c.segA, c.segB, other)
 	return capPt.Sub(triPt).Norm() - c.radius
 }

spatialmath/geometry_utils.go

@@ -100,7 +100,7 @@ func BoundingSphere(geometry Geometry) (Geometry, error) {
 }
 
 // closestSegmentTrianglePoints takes a line segment and a triangle, and returns the point on each closest to the other.
-func closestPointsSegmentTriangle(ap1, ap2 r3.Vector, t *triangle) (bestSegPt, bestTriPt r3.Vector) {
+func closestPointsSegmentTriangle(ap1, ap2 r3.Vector, t *Triangle) (bestSegPt, bestTriPt r3.Vector) {
 	// The closest triangle point is either on the edge or within the triangle.
 
 	// First, handle the case where the closest triangle point is inside the
@@ -109,7 +109,7 @@ func closestPointsSegmentTriangle(ap1, ap2 r3.Vector, t *triangle) (bestSegPt, b
 	// If the line overlaps the triangle and is parallel to the triangle plane,
 	// the chosen triangle point is arbitrary.
 	segPt, _ := closestPointsSegmentPlane(ap1, ap2, t.p0, t.normal)
-	triPt, inside := t.closestInsidePoint(segPt)
+	triPt, inside := ClosestTriangleInsidePoint(t, segPt)
 	if inside {
 		// If inside is false, then these will not be the best points, because they are based on the segment-plane intersection
 		return segPt, triPt

spatialmath/mesh.go

@@ -1,115 +1,38 @@
 package spatialmath
 
-import (
-	"github.com/golang/geo/r3"
-)
-
 // This file incorporates work covered by the Brax project -- https://github.com/google/brax/blob/main/LICENSE.
 // Copyright 2021 The Brax Authors, which is licensed under the Apache License Version 2.0 (the “License”).
 // You may obtain a copy of the license at http://www.apache.org/licenses/LICENSE-2.0.
 
-// 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 {
 	pose      Pose
-	triangles []*triangle
+	triangles []*Triangle
 }
 
-type triangle struct {
-	p0 r3.Vector
-	p1 r3.Vector
-	p2 r3.Vector
-
-	normal r3.Vector
-}
-
-func newTriangle(p0, p1, p2 r3.Vector) *triangle {
-	return &triangle{
-		p0:     p0,
-		p1:     p1,
-		p2:     p2,
-		normal: PlaneNormal(p0, p1, p2),
+// NewMesh creates a mesh from the given triangles and pose.
+func NewMesh(pose Pose, triangles []*Triangle) *Mesh {
+	return &Mesh{
+		pose:      pose,
+		triangles: triangles,
 	}
 }
 
-// closestPointToCoplanarPoint takes a point, and returns the closest point on the triangle to the given point
-// The given point *MUST* be coplanar with the triangle. If it is known ahead of time that the point is coplanar, this is faster.
-func (t *triangle) closestPointToCoplanarPoint(pt r3.Vector) r3.Vector {
-	// Determine whether point is inside all triangle edges:
-	c0 := pt.Sub(t.p0).Cross(t.p1.Sub(t.p0))
-	c1 := pt.Sub(t.p1).Cross(t.p2.Sub(t.p1))
-	c2 := pt.Sub(t.p2).Cross(t.p0.Sub(t.p2))
-	inside := c0.Dot(t.normal) <= 0 && c1.Dot(t.normal) <= 0 && c2.Dot(t.normal) <= 0
-
-	if inside {
-		return pt
-	}
-
-	// Edge 1:
-	refPt := ClosestPointSegmentPoint(t.p0, t.p1, pt)
-	bestDist := pt.Sub(refPt).Norm2()
-
-	// Edge 2:
-	point2 := ClosestPointSegmentPoint(t.p1, t.p2, pt)
-	if distsq := pt.Sub(point2).Norm2(); distsq < bestDist {
-		refPt = point2
-		bestDist = distsq
-	}
-
-	// Edge 3:
-	point3 := ClosestPointSegmentPoint(t.p2, t.p0, pt)
-	if distsq := pt.Sub(point3).Norm2(); distsq < bestDist {
-		return point3
-	}
-	return refPt
+// Pose returns the pose of the mesh.
+func (m *Mesh) Pose() Pose {
+	return m.pose
 }
 
-// closestPointToPoint takes a point, and returns the closest point on the triangle to the given point, as well as whether the point
-// is on the edge of the triangle.
-// This is slower than closestPointToCoplanarPoint.
-func (t *triangle) closestPointToPoint(point r3.Vector) r3.Vector {
-	closestPtInside, inside := t.closestInsidePoint(point)
-	if inside {
-		return closestPtInside
-	}
-
-	// If the closest point is outside the triangle, it must be on an edge, so we
-	// check each triangle edge for a closest point to the point pt.
-	closestPt := ClosestPointSegmentPoint(t.p0, t.p1, point)
-	bestDist := point.Sub(closestPt).Norm2()
-
-	newPt := ClosestPointSegmentPoint(t.p1, t.p2, point)
-	if newDist := point.Sub(newPt).Norm2(); newDist < bestDist {
-		closestPt = newPt
-		bestDist = newDist
-	}
-
-	newPt = ClosestPointSegmentPoint(t.p2, t.p0, point)
-	if newDist := point.Sub(newPt).Norm2(); newDist < bestDist {
-		return newPt
-	}
-	return closestPt
+// Triangles returns the triangles associated with the mesh.
+func (m *Mesh) Triangles() []*Triangle {
+	return m.triangles
 }
 
-// closestInsidePoint returns the closest point on a triangle IF AND ONLY IF the query point's projection overlaps the triangle.
-// Otherwise it will return the query point.
-// To visualize this- if one draws a tetrahedron using the triangle and the query point, all angles from the triangle to the query point
-// must be <= 90 degrees.
-func (t *triangle) closestInsidePoint(point r3.Vector) (r3.Vector, bool) {
-	// Parametrize the triangle s.t. a point inside the triangle is
-	// Q = p0 + u * e0 + v * e1, when 0 <= u <= 1, 0 <= v <= 1, and
-	// 0 <= u + v <= 1. Let e0 = (p1 - p0) and e1 = (p2 - p0).
-	// We analytically minimize the distance between the point pt and Q.
-	e0 := t.p1.Sub(t.p0)
-	e1 := t.p2.Sub(t.p0)
-	a := e0.Norm2()
-	b := e0.Dot(e1)
-	c := e1.Norm2()
-	d := point.Sub(t.p0)
-	// The determinant is 0 only if the angle between e1 and e0 is 0
-	// (i.e. the triangle has overlapping lines).
-	det := (a*c - b*b)
-	u := (c*e0.Dot(d) - b*e1.Dot(d)) / det
-	v := (-b*e0.Dot(d) + a*e1.Dot(d)) / det
-	inside := (0 <= u) && (u <= 1) && (0 <= v) && (v <= 1) && (u+v <= 1)
-	return t.p0.Add(e0.Mul(u)).Add(e1.Mul(v)), inside
+// Transform transforms the mesh. As triangles are in the mesh's frame, they are unchanged.
+func (m *Mesh) Transform(pose Pose) *Mesh {
+	// Triangle points are in frame of mesh, like the corners of a box, so no need to transform them
+	return &Mesh{
+		pose:      Compose(pose, m.pose),
+		triangles: m.triangles,
+	}
 }