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.

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

Conversation

raybjork
Copy link
Member

@raybjork raybjork commented Apr 9, 2025

This came up because I got really frustrated with the error messages when plans fail because of IK reasons. Before it simply reads something like (this output is gotten by running TestArmObstacleSolve)

all IK solutions failed constraints. Failures: { Collision between the robot and an obstacle_0x1055b5470: 94.74% }, { Collision between a robot component that is moving and one that is stationary_0x1055b5470: 4.21% }, { Collision between two robot components that are moving_0x1055b5470: 1.05% }, 
PASS

and with this change it will read

all IK solutions failed constraints. Failures: { obstacle constraint: violation between UR5e:ee_link and unnamedWorldStateGeometry_0 geometries: 91.58% }, { robot constraint: violation between UR5e:forearm_link and xArmVgripper geometries: 4.21% }, { robot constraint: violation between UR5e:upper_arm_link and xArmVgripper geometries: 2.11% }, { self-collision constraint: violation between xArm6:base_top and UR5e:wrist_2_link geometries: 1.05% }, { robot constraint: violation between xArm6:base_top and xArmVgripper geometries: 1.05% }, 

which I think is more helpful.

In order to do this I had to change the Constraint types to return error instead of bool, and this change rippled out more than I would have expected.

@viambot viambot added the safe to test This pull request is marked safe to test from a trusted zone label Apr 9, 2025
@raybjork raybjork requested a review from biotinker April 9, 2025 19:42
// 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.

👍

@raybjork raybjork changed the title Rsdk 10371 RSDK-10371 provide more helpful error messages when IK fails to produce a solution Apr 9, 2025
@viambot viambot added safe to test This pull request is marked safe to test from a trusted zone and removed safe to test This pull request is marked safe to test from a trusted zone labels Apr 9, 2025
}
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

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

Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
safe to test This pull request is marked safe to test from a trusted zone
Projects
None yet
Development

Successfully merging this pull request may close these issues.

3 participants