diff --git a/motionplan/cBiRRT.go b/motionplan/cBiRRT.go index cf9d05cb634..141d553530d 100644 --- a/motionplan/cBiRRT.go +++ b/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} diff --git a/motionplan/motionPlanner.go b/motionplan/motionPlanner.go index 8e6a910d2c2..7816b802d9e 100644 --- a/motionplan/motionPlanner.go +++ b/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 { diff --git a/motionplan/planManager.go b/motionplan/planManager.go index db9000de6fe..f367cfd0bcd 100644 --- a/motionplan/planManager.go +++ b/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 diff --git a/motionplan/plannerOptions.go b/motionplan/plannerOptions.go index 1591b1a3e91..ed35a56f90c 100644 --- a/motionplan/plannerOptions.go +++ b/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 diff --git a/services/motion/builtin/builtin.go b/services/motion/builtin/builtin.go index a5fd728b601..a9bd6d552c2 100644 --- a/services/motion/builtin/builtin.go +++ b/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 diff --git a/spatialmath/box.go b/spatialmath/box.go index 9a56662c192..d58030583b7 100644 --- a/spatialmath/box.go +++ b/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 diff --git a/spatialmath/capsule.go b/spatialmath/capsule.go index 9c88e6d7207..0b7d274dc48 100644 --- a/spatialmath/capsule.go +++ b/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 } diff --git a/spatialmath/geometry_utils.go b/spatialmath/geometry_utils.go index 5dcbfad6a6d..d40152db4e2 100644 --- a/spatialmath/geometry_utils.go +++ b/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 diff --git a/spatialmath/mesh.go b/spatialmath/mesh.go index bcb46c10fa0..43ab03d58f2 100644 --- a/spatialmath/mesh.go +++ b/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, + } } diff --git a/spatialmath/mesh_test.go b/spatialmath/mesh_test.go deleted file mode 100644 index 4dd30946eec..00000000000 --- a/spatialmath/mesh_test.go +++ /dev/null @@ -1,23 +0,0 @@ -package spatialmath - -import ( - "testing" - - "github.com/golang/geo/r3" - "go.viam.com/test" -) - -func TestClosestPoint(t *testing.T) { - p0 := r3.Vector{0, 0, 0} - p1 := r3.Vector{1, 0, 0} - p2 := r3.Vector{0, 0, 2} - tri := newTriangle(p0, p1, p2) - - qp1 := r3.Vector{-1, 0, 0} - cp1 := tri.closestPointToCoplanarPoint(qp1) - cp2 := tri.closestPointToPoint(qp1) - cp3, inside := tri.closestInsidePoint(qp1) - test.That(t, inside, test.ShouldBeFalse) - test.That(t, cp3.ApproxEqual(qp1), test.ShouldBeTrue) - test.That(t, cp1.ApproxEqual(cp2), test.ShouldBeTrue) -} diff --git a/spatialmath/triangle.go b/spatialmath/triangle.go new file mode 100644 index 00000000000..40f6b11c103 --- /dev/null +++ b/spatialmath/triangle.go @@ -0,0 +1,60 @@ +package spatialmath + +import ( + "github.com/golang/geo/r3" +) + +// Triangle is three points and a normal vector. +type Triangle struct { + p0 r3.Vector + p1 r3.Vector + p2 r3.Vector + + normal r3.Vector +} + +// NewTriangle creates a Triangle from three points. The Normal is computed; directionality is random. +func NewTriangle(p0, p1, p2 r3.Vector) *Triangle { + return &Triangle{ + p0: p0, + p1: p1, + p2: p2, + normal: PlaneNormal(p0, p1, p2), + } +} + +// Points returns the three points associated with the triangle. +func (t *Triangle) Points() []r3.Vector { + return []r3.Vector{t.p0, t.p1, t.p2} +} + +// Normal returns the triangle's normal vector. +func (t *Triangle) Normal() r3.Vector { + return t.normal +} + +// ClosestTriangleInsidePoint 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 ClosestTriangleInsidePoint(t *Triangle, point r3.Vector) (r3.Vector, bool) { + eps := 1e-6 + + // 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+eps) && (u <= 1+eps) && (0 <= v+eps) && (v <= 1+eps) && (u+v <= 1+eps) + return t.p0.Add(e0.Mul(u)).Add(e1.Mul(v)), inside +}