Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

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

Open
wants to merge 4 commits into
base: main
Choose a base branch
from
Open

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

Show file tree
Hide file tree
Changes from 3 commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
fcefce4
wip
raybjork Apr 8, 2025
0ca3302
wip
raybjork Apr 9, 2025
d8b554a
we appear to be passing tests
raybjork Apr 9, 2025
a547f0d
order the errors correctly
raybjork Apr 9, 2025
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
9 changes: 5 additions & 4 deletions motionplan/check.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 = ""
}
Expand Down Expand Up @@ -354,7 +355,7 @@ 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 {
if err := sfPlanner.planOpts.CheckStateConstraints(interpolatedState); err != nil {
return fmt.Errorf("found constraint violation or collision in segment between %v and %v at %v: %s",
segment.StartPosition.Point(),
segment.EndPosition.Point(),
Expand Down
155 changes: 84 additions & 71 deletions motionplan/constraint.go
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Expand Down Expand Up @@ -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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Weren't we going to remove these? Since we're making a breaking change anyway it's a great time

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

That change takes this from a couple hours worth of change to a lot more than that. Its going to take me some time to ramp on the TP space stuff to not mess that change up

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

What are the chances we're going to do more 2d base stuff in the next ~year?

If that's very low, then sure, I agree.

If it's higher, then now/this change might be a good time to ramp you up before I go on leave

Copy link
Member Author

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

How about this. I will timebox this change to no more than a few hours and if I cant do it then we will merge this?

Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

👍


// 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,
Expand Down Expand Up @@ -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 {
Expand All @@ -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
Expand All @@ -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
}
Expand Down Expand Up @@ -213,36 +226,40 @@ 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:
// 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 {
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
Copy link
Member

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

If we're not combining all, could we sort so that for a given collision scenario you always get the same pair back?

Copy link
Member Author

@raybjork raybjork Apr 9, 2025 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

This is going to run a fair number of times considering it will be hit every time an iteration fails for any reason. I thought about doing that but I dont think its worth the performance hit. I also think the stochasticity is slightly good because it shows all the possibilities

return fmt.Errorf(obstacleConstraintDescription+" violation between %s and %s geometries", cs[0].name1, cs[0].name2)
}
return len(cg.collisions(collisionBufferMM)) == 0
return nil
}
return constraint, nil
}
Expand All @@ -267,11 +284,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.
Expand All @@ -286,9 +303,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(obstacleConstraintDescription+" violation between %s and %s geometries", cs[0].name1, cs[0].name2)
}
return len(cg.collisions(collisionBufferMM)) == 0
return nil
}
return constraint, nil
}
Expand All @@ -309,8 +331,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
}
Expand Down Expand Up @@ -345,12 +367,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
}
if gradFunc(state) < tolerance {
return nil
}
return gradFunc(state) < tolerance
return errors.New(orientationConstraintDescription + " violated")
}

return validFunc, gradFunc
Expand Down Expand Up @@ -383,12 +408,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
Expand All @@ -403,57 +431,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
}
return gradFunc(state) == 0
if gradFunc(state) == 0 {
return nil
}
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 {
Expand All @@ -464,9 +473,14 @@ 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 {
// 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(boundingRegionConstraintDescription+" violation between %s and %s geometries", cs[0].name1, cs[0].name2)
}
return len(cg.collisions(collisionBufferMM)) != 0
return nil
}
}

Expand Down Expand Up @@ -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 {
Expand Down
Loading
Loading