RSDK-10371 provide more helpful error messages when IK fails to produce a solution

motionplan/check.go

@@ -270,9 +270,10 @@ func checkSegmentsFS(sfPlanner *planManager, segments []*ik.SegmentFS, lookAhead
 			if lastValid != nil {
 				checkConf = lastValid.EndConfiguration
 			}
-			ok, reason := sfPlanner.planOpts.CheckStateFSConstraints(&ik.StateFS{Configuration: checkConf, FS: sfPlanner.fs})
-			if !ok {
-				reason = " reason: " + reason
+			var reason string
+			err := sfPlanner.planOpts.CheckStateFSConstraints(&ik.StateFS{Configuration: checkConf, FS: sfPlanner.fs})
+			if err != nil {
+				reason = " reason: " + err.Error()
 			} else {
 				reason = ""
 			}
@@ -354,12 +355,12 @@ func checkSegments(sfPlanner *planManager, segments []*ik.Segment, lookAheadDist
 			}
 
 			// Checks for collision along the interpolated route and returns a the first interpolated pose where a collision is detected.
-			if isValid, err := sfPlanner.planOpts.CheckStateConstraints(interpolatedState); !isValid {
-				return fmt.Errorf("found constraint violation or collision in segment between %v and %v at %v: %s",
+			if err := sfPlanner.planOpts.CheckStateConstraints(interpolatedState); err != nil {
+				return errors.Wrapf(err,
+					"found constraint violation or collision in segment between %v and %v at %v",
 					segment.StartPosition.Point(),
 					segment.EndPosition.Point(),
 					poseInPath.Point(),
-					err,
 				)
 			}
 		}

motionplan/constraint.go

@@ -16,6 +16,19 @@ import (
 	spatial "go.viam.com/rdk/spatialmath"
 )
 
+// short descriptions of constraints used in error messages.
+const (
+	linearConstraintDescription      = "linear constraint"
+	orientationConstraintDescription = "orientation constraint"
+	planarConstraintDescription      = "planar constraint"
+
+	// various collision constraints that have different names in order to be unique keys in maps of constraints that are created.
+	boundingRegionConstraintDescription = "bounding region constraint"
+	obstacleConstraintDescription       = "obstacle constraint"
+	selfCollisionConstraintDescription  = "self-collision constraint"
+	robotCollisionConstraintDescription = "robot constraint" // collision between a moving robot component and one that is stationary
+)
+
 // Given a constraint input with only frames and input positions, calculates the corresponding poses as needed.
 func resolveSegmentsToPositions(segment *ik.Segment) error {
 	if segment.StartPosition == nil {
@@ -75,20 +88,20 @@ func resolveStatesToPositions(state *ik.State) error {
 }
 
 // SegmentFSConstraint tests whether a transition from a starting robot configuration to an ending robot configuration is valid.
-// If the returned bool is true, the constraint is satisfied and the segment is valid.
-type SegmentFSConstraint func(*ik.SegmentFS) bool
+// If the returned error is nil, the constraint is satisfied and the segment is valid.
+type SegmentFSConstraint func(*ik.SegmentFS) error
 
 // SegmentConstraint tests whether a transition from a starting robot configuration to an ending robot configuration is valid.
-// If the returned bool is true, the constraint is satisfied and the segment is valid.
-type SegmentConstraint func(*ik.Segment) bool
+// If the returned error is nil, the constraint is satisfied and the segment is valid.
+type SegmentConstraint func(*ik.Segment) error
 
 // StateFSConstraint tests whether a given robot configuration is valid
-// If the returned bool is true, the constraint is satisfied and the state is valid.
-type StateFSConstraint func(*ik.StateFS) bool
+// If the returned error is nil, the constraint is satisfied and the state is valid.
+type StateFSConstraint func(*ik.StateFS) error
 
 // StateConstraint tests whether a given robot configuration is valid
-// If the returned bool is true, the constraint is satisfied and the state is valid.
-type StateConstraint func(*ik.State) bool
+// If the returned error is nil, the constraint is satisfied and the state is valid.
+type StateConstraint func(*ik.State) error
 
 func createAllCollisionConstraints(
 	movingRobotGeometries, staticRobotGeometries, worldGeometries, boundingRegions []spatial.Geometry,
@@ -132,14 +145,14 @@ func createAllCollisionConstraints(
 			return nil, nil, err
 		}
 		// TODO: TPspace currently still uses the non-FS constraint, this should be removed once TPspace is fully migrated to frame systems
-		constraintMap[defaultObstacleConstraintDesc] = obstacleConstraint
-		constraintFSMap[defaultObstacleConstraintDesc] = obstacleConstraintFS
+		constraintMap[obstacleConstraintDescription] = obstacleConstraint
+		constraintFSMap[obstacleConstraintDescription] = obstacleConstraintFS
 	}
 
 	if len(boundingRegions) > 0 {
 		// create constraint to keep moving geometries within the defined bounding regions
 		interactionSpaceConstraint := NewBoundingRegionConstraint(movingRobotGeometries, boundingRegions, collisionBufferMM)
-		constraintMap[defaultBoundingRegionConstraintDesc] = interactionSpaceConstraint
+		constraintMap[boundingRegionConstraintDescription] = interactionSpaceConstraint
 	}
 
 	if len(staticRobotGeometries) > 0 {
@@ -162,8 +175,8 @@ func createAllCollisionConstraints(
 		if err != nil {
 			return nil, nil, err
 		}
-		constraintMap[defaultRobotCollisionConstraintDesc] = robotConstraint
-		constraintFSMap[defaultRobotCollisionConstraintDesc] = robotConstraintFS
+		constraintMap[robotCollisionConstraintDescription] = robotConstraint
+		constraintFSMap[robotCollisionConstraintDescription] = robotConstraintFS
 	}
 
 	// create constraint to keep moving geometries from hitting themselves
@@ -172,12 +185,12 @@ func createAllCollisionConstraints(
 		if err != nil {
 			return nil, nil, err
 		}
-		constraintMap[defaultSelfCollisionConstraintDesc] = selfCollisionConstraint
+		constraintMap[selfCollisionConstraintDescription] = selfCollisionConstraint
 		selfCollisionConstraintFS, err := NewCollisionConstraintFS(movingRobotGeometries, nil, allowedCollisions, false, collisionBufferMM)
 		if err != nil {
 			return nil, nil, err
 		}
-		constraintFSMap[defaultSelfCollisionConstraintDesc] = selfCollisionConstraintFS
+		constraintFSMap[selfCollisionConstraintDescription] = selfCollisionConstraintFS
 	}
 	return constraintFSMap, constraintMap, nil
 }
@@ -213,13 +226,13 @@ func NewCollisionConstraint(
 	}
 
 	// create constraint from reference collision graph
-	constraint := func(state *ik.State) bool {
+	constraint := func(state *ik.State) error {
 		var internalGeoms []spatial.Geometry
 		switch {
 		case state.Configuration != nil:
 			internal, err := state.Frame.Geometries(state.Configuration)
 			if err != nil {
-				return false
+				return err
 			}
 			internalGeoms = internal.Geometries()
 		case state.Position != nil:
@@ -228,21 +241,26 @@ func NewCollisionConstraint(
 			// transform them to the Position
 			internal, err := state.Frame.Geometries(make([]referenceframe.Input, len(state.Frame.DoF())))
 			if err != nil {
-				return false
+				return err
 			}
 			movedGeoms := internal.Geometries()
 			for _, geom := range movedGeoms {
 				internalGeoms = append(internalGeoms, geom.Transform(state.Position))
 			}
 		default:
-			return false
+			return errors.New("need either a Position or Configuration to be set for a ik.State")
 		}
 
 		cg, err := newCollisionGraph(internalGeoms, static, zeroCG, reportDistances, collisionBufferMM)
 		if err != nil {
-			return false
+			return err
+		}
+		cs := cg.collisions(collisionBufferMM)
+		if len(cs) != 0 {
+			// we could choose to amalgamate all the collisions into one error but its probably saner not to and choose just the first
+			return fmt.Errorf("violation between %s and %s geometries", cs[0].name1, cs[0].name2)
 		}
-		return len(cg.collisions(collisionBufferMM)) == 0
+		return nil
 	}
 	return constraint, nil
 }
@@ -267,11 +285,11 @@ func NewCollisionConstraintFS(
 	}
 
 	// create constraint from reference collision graph
-	constraint := func(state *ik.StateFS) bool {
+	constraint := func(state *ik.StateFS) error {
 		// Use FrameSystemGeometries to get all geometries in the frame system
 		internalGeometries, err := referenceframe.FrameSystemGeometries(state.FS, state.Configuration)
 		if err != nil {
-			return false
+			return err
 		}
 
 		// We only want to compare *moving* geometries, so we filter what we get from the framesystem against what we were passed.
@@ -286,9 +304,14 @@ func NewCollisionConstraintFS(
 
 		cg, err := newCollisionGraph(internalGeoms, static, zeroCG, reportDistances, collisionBufferMM)
 		if err != nil {
-			return false
+			return err
+		}
+		cs := cg.collisions(collisionBufferMM)
+		if len(cs) != 0 {
+			// we could choose to amalgamate all the collisions into one error but its probably saner not to and choose just the first
+			return fmt.Errorf("violation between %s and %s geometries", cs[0].name1, cs[0].name2)
 		}
-		return len(cg.collisions(collisionBufferMM)) == 0
+		return nil
 	}
 	return constraint, nil
 }
@@ -309,8 +332,8 @@ func NewAbsoluteLinearInterpolatingConstraint(from, to spatial.Pose, linTol, ori
 	lineConstraint, lineMetric := NewLineConstraint(from.Point(), to.Point(), linTol)
 	interpMetric := ik.CombineMetrics(orientMetric, lineMetric)
 
-	f := func(state *ik.State) bool {
-		return orientConstraint(state) && lineConstraint(state)
+	f := func(state *ik.State) error {
+		return errors.Join(orientConstraint(state), lineConstraint(state))
 	}
 	return f, interpMetric
 }
@@ -345,12 +368,15 @@ func NewSlerpOrientationConstraint(start, goal spatial.Pose, tolerance float64)
 		return (sDist + gDist) - origDist
 	}
 
-	validFunc := func(state *ik.State) bool {
+	validFunc := func(state *ik.State) error {
 		err := resolveStatesToPositions(state)
 		if err != nil {
-			return false
+			return err
 		}
-		return gradFunc(state) < tolerance
+		if gradFunc(state) < tolerance {
+			return nil
+		}
+		return errors.New(orientationConstraintDescription + " violated")
 	}
 
 	return validFunc, gradFunc
@@ -383,12 +409,15 @@ func NewPlaneConstraint(pNorm, pt r3.Vector, writingAngle, epsilon float64) (Sta
 		return pDist*pDist + oDist*oDist
 	}
 
-	validFunc := func(state *ik.State) bool {
+	validFunc := func(state *ik.State) error {
 		err := resolveStatesToPositions(state)
 		if err != nil {
-			return false
+			return err
+		}
+		if gradFunc(state) < epsilon*epsilon {
+			return nil
 		}
-		return gradFunc(state) < epsilon*epsilon
+		return errors.New(planarConstraintDescription + " violated")
 	}
 
 	return validFunc, gradFunc
@@ -403,57 +432,38 @@ func NewLineConstraint(pt1, pt2 r3.Vector, tolerance float64) (StateConstraint,
 		return math.Max(spatial.DistToLineSegment(pt1, pt2, state.Position.Point())-tolerance, 0)
 	}
 
-	validFunc := func(state *ik.State) bool {
+	validFunc := func(state *ik.State) error {
 		err := resolveStatesToPositions(state)
 		if err != nil {
-			return false
+			return err
+		}
+		if gradFunc(state) == 0 {
+			return nil
 		}
-		return gradFunc(state) == 0
+		return errors.New(linearConstraintDescription + " violated")
 	}
 
 	return validFunc, gradFunc
 }
 
-// NewOctreeCollisionConstraint takes an octree and will return a constraint that checks whether any geometries
-// intersect with points in the octree. Threshold sets the confidence level required for a point to be considered, and buffer is the
-// distance to a point that is considered a collision in mm.
-func NewOctreeCollisionConstraint(octree *pointcloud.BasicOctree, threshold int, buffer, collisionBufferMM float64) StateConstraint {
-	constraint := func(state *ik.State) bool {
-		geometries, err := state.Frame.Geometries(state.Configuration)
-		if err != nil && geometries == nil {
-			return false
-		}
-
-		for _, geom := range geometries.Geometries() {
-			collides, err := octree.CollidesWithGeometry(geom, threshold, buffer, collisionBufferMM)
-			if err != nil || collides {
-				return false
-			}
-		}
-		return true
-	}
-	return constraint
-}
-
 // NewBoundingRegionConstraint will determine if the given list of robot geometries are in collision with the
 // given list of bounding regions.
 func NewBoundingRegionConstraint(robotGeoms, boundingRegions []spatial.Geometry, collisionBufferMM float64) StateConstraint {
-	return func(state *ik.State) bool {
+	return func(state *ik.State) error {
 		var internalGeoms []spatial.Geometry
 		switch {
 		case state.Configuration != nil:
 			internal, err := state.Frame.Geometries(state.Configuration)
 			if err != nil {
-				return false
+				return err
 			}
 			internalGeoms = internal.Geometries()
 		case state.Position != nil:
-			// TODO(RSDK-5391): remove this case
 			// If we didn't pass a Configuration, but we do have a Position, then get the geometries at the zero state and
 			// transform them to the Position
 			internal, err := state.Frame.Geometries(make([]referenceframe.Input, len(state.Frame.DoF())))
 			if err != nil {
-				return false
+				return err
 			}
 			movedGeoms := internal.Geometries()
 			for _, geom := range movedGeoms {
@@ -464,9 +474,13 @@ func NewBoundingRegionConstraint(robotGeoms, boundingRegions []spatial.Geometry,
 		}
 		cg, err := newCollisionGraph(internalGeoms, boundingRegions, nil, true, collisionBufferMM)
 		if err != nil {
-			return false
+			return err
+		}
+		cs := cg.collisions(collisionBufferMM)
+		if len(cs) == 0 {
+			return errors.New("violation of bounding region constraint")
 		}
-		return len(cg.collisions(collisionBufferMM)) != 0
+		return nil
 	}
 }
 
@@ -713,24 +727,23 @@ type fsPathConstraint struct {
 	fs            referenceframe.FrameSystem
 }
 
-func (fpc *fsPathConstraint) constraint(state *ik.StateFS) bool {
+func (fpc *fsPathConstraint) constraint(state *ik.StateFS) error {
 	for frame, goal := range fpc.goalMap {
 		if constraint, ok := fpc.constraintMap[frame]; ok {
 			currPose, err := fpc.fs.Transform(state.Configuration, referenceframe.NewZeroPoseInFrame(frame), goal.Parent())
 			if err != nil {
-				return false
+				return err
 			}
-			pass := constraint(&ik.State{
+			if err := constraint(&ik.State{
 				Configuration: state.Configuration[frame],
 				Position:      currPose.(*referenceframe.PoseInFrame).Pose(),
 				Frame:         fpc.fs.Frame(frame),
-			})
-			if !pass {
-				return false
+			}); err != nil {
+				return err
 			}
 		}
 	}
-	return true
+	return nil
 }
 
 func (fpc *fsPathConstraint) metric(state *ik.StateFS) float64 {