Skip to content

if an arm is in collision at start, anything downstream of the arm is…#5809

Draft
erh wants to merge 1 commit into
viamrobotics:mainfrom
erh:20260301-descendant-collisions
Draft

if an arm is in collision at start, anything downstream of the arm is…#5809
erh wants to merge 1 commit into
viamrobotics:mainfrom
erh:20260301-descendant-collisions

Conversation

@erh
Copy link
Copy Markdown
Member

@erh erh commented Mar 2, 2026

… allowed to be in that collision during this motion, i think this is right...

this fixes an issue you had from email "Armplanning error when starting point is violating an obstacle" on Wed, Dec 17, 2025, 12:45 PM

… allowed to be in that collision during this motion, i think this is right...
@viambot viambot added the safe to test This pull request is marked safe to test from a trusted zone label Mar 2, 2026
@github-actions
Copy link
Copy Markdown
Contributor

github-actions Bot commented Mar 2, 2026

Availability

Scene # viamrobotics:main erh:20260301-descendant-collisions Percent Improvement Health
1 100% 100% 0%
2 100% 100% 0%
3 100% 100% 0%
4 100% 100% 0%
5 100% 100% 0%
6 100% 100% 0%
7 100% 100% 0%
8 100% 100% 0%
9 100% 100% 0%
10 100% 100% 0%
11 100% 100% 0%

Quality

Scene # viamrobotics:main erh:20260301-descendant-collisions Percent Improvement Probability of Improvement Health
1 1.31±0.00 1.31±0.00 -0% 42%
2 0.94±0.01 0.94±0.01 -0% 50%
3 5.98±0.29 6.01±0.29 -0% 47%
4 1.98±0.27 1.98±0.27 0% 50%
5 8.78±1.87 8.80±1.87 -0% 50%
6 8.94±2.48 8.62±2.52 4% 54%
7 1.95±0.38 1.95±0.38 0% 50%
8 0.94±0.01 0.94±0.01 0% 55%
9 4.17±0.00 4.17±0.00 -0% 50%
10 12.63±0.03 12.63±0.03 0% 50%
11 0.62±0.00 0.62±0.00 -0% 39%

Performance

Scene # viamrobotics:main erh:20260301-descendant-collisions Percent Improvement Probability of Improvement Health
1 0.02±0.00 0.02±0.00 1% 58%
2 0.03±0.00 0.03±0.00 15% 92%
3 0.02±0.00 0.02±0.00 2% 59%
4 0.25±0.04 0.25±0.06 -4% 45%
5 1.41±0.13 1.40±0.14 1% 52%
6 1.61±0.32 1.58±0.29 2% 53%
7 1.31±0.10 1.31±0.08 0% 50%
8 0.03±0.00 0.03±0.00 10% 82%
9 1.71±0.09 1.72±0.10 -1% 45%
10 3.56±0.42 3.50±0.40 2% 54%
11 0.52±0.01 0.58±0.08 -12% 22%

The above data was generated by running scenes defined in the motion-testing repository
The SHA1 for viamrobotics:main is: 56429e7f3402c283974c725fd2725a0b654ff64c
The SHA1 for erh:20260301-descendant-collisions is: 56429e7f3402c283974c725fd2725a0b654ff64c

  • 11 samples were taken for each scene

Copy link
Copy Markdown
Member

@biotinker biotinker left a comment

Choose a reason for hiding this comment

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

Wouldn't this allow an end effector to collide with the table an arm is mounted on? Or the rest of the arm for that matter?

It's one thing for us to allow "X and anything downstream" to collide with Y on a per-call basis but making this the default seems like it is going to have a lot of real world breakage.

@erh
Copy link
Copy Markdown
Member Author

erh commented Mar 3, 2026

Wouldn't this allow an end effector to collide with the table an arm is mounted on? Or the rest of the arm for that matter?

It's one thing for us to allow "X and anything downstream" to collide with Y on a per-call basis but making this the default seems like it is going to have a lot of real world breakage.

if the arm is already in collision.
if it was really in collision, it couldn't move.
this whole behavior is odd.
right now it's just wrong.
so either we lean in or out.

@biotinker
Copy link
Copy Markdown
Member

if it was really in collision, it couldn't move.

Due to inaccuracy of models (and likely after we have mesh models everywhere) the base of arms is often in collision with the table on which it is mounted. We don't want anything else to collide with a table. With this change, the result of mildly overinflating or slightly misplacing an important obstacle like a table would be that the obstacle gets ignored.

Maybe we want something more like manually specifying allowed collisions that are not direct parent-child relationships and otherwise this scenario would be a very clear error "forearm in mesh guard not allowed to collide but starting in collision"?

@NickPPC
Copy link
Copy Markdown
Member

NickPPC commented Mar 4, 2026

Due to inaccuracy of models (and likely after we have mesh models everywhere) the base of arms is often in collision with the table on which it is mounted. We don't want anything else to collide with a table. With this change, the result of mildly overinflating or slightly misplacing an important obstacle like a table would be that the obstacle gets ignored.

The table being ignored for all children component would be a big problem.
We can, at least conceptually, distinguish the 2 cases:

  1. The mesh guard: obstacle we are usually not colliding with but we are in the starting joint positions
  2. The table the arm is mounted on: always colliding no matter the joint positions
    We could theoretically treat differently a fixed component, the base of the arm (which cannot move no matter the joint positions we use), and a moveable component, any arm component downstream from the first joint.
    This change would works well for any of the components downstream from a joint (DoF). By restricting this to those components it would allow us to exclude the case of the base mounted on the table.

Likely Problem: However I think the base of the arm is not split into a fixed and a moveable piece (the part below and above the base joint) so the base would be considered a frame with a DoF, so it is not in our frame system a fixed component, and this which would break us again

@biotinker
Copy link
Copy Markdown
Member

I don't think either of those saves the approach.

Right now we have a limitation, which is that the starting state of the robot influences what motion plan can be generated at a more fundamental level than people might expect.

This approach keeps that, and adds a new surprise where the backend ordering of static frames matters, and spatially identical robots will behave radically differently if someone describes three static pieces as x->y->z vs z->y->x.

I would rather us try to remove the above limitation, which only exists because it was a quick and dirty heuristic to get reasonable behavior most of the time without forcing the user to specify every pair of objects allowed to collide.

@erh
Copy link
Copy Markdown
Member Author

erh commented Mar 5, 2026

i agree with peter, i think the original approach was wrong so therefore this is wrong.

@erh erh marked this pull request as draft March 5, 2026 12:25
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.

4 participants