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

Jump to bottom

Better debugging feedback on failed Descartes plan #401

Merged
merged 3 commits into from
Jun 3, 2024
Merged

Better debugging feedback on failed Descartes plan #401

merged 3 commits into from
Jun 3, 2024

Conversation

marrts
Copy link
Contributor

@marrts marrts commented Oct 25, 2023

In conjunction with this PR on descartes_light. Gives a detailed breakdown of all the collisions seen when a descartes plan fails or explicitly says if the failure was from IK being unsolvable

@marrts marrts mentioned this pull request Oct 25, 2023
Merged
@marrts marrts changed the title Better debugging feedback on failed plan Better debugging feedback on failed Descartes plan Oct 25, 2023
@marrts
Copy link
Contributor Author

marrts commented Oct 25, 2023

Snippet of example output:

Debug:   Descartes took 2.9540 seconds to build vertices.
[ebtr_toolpath_refinement_node-6] Debug:   Failed vertices and positions: 
[ebtr_toolpath_refinement_node-6] 66: Working Frame: seat, TCP Frame: tcp
[ebtr_toolpath_refinement_node-6] Target Pose:
[ebtr_toolpath_refinement_node-6]          -1           0           0   -0.121299
[ebtr_toolpath_refinement_node-6]           0           1 1.22465e-16   -0.055044
[ebtr_toolpath_refinement_node-6]           0 1.22465e-16          -1     0.18661
[ebtr_toolpath_refinement_node-6]           0           0           0           1
[ebtr_toolpath_refinement_node-6] TCP Offset:
[ebtr_toolpath_refinement_node-6] 1 0 0 0
[ebtr_toolpath_refinement_node-6] 0 1 0 0
[ebtr_toolpath_refinement_node-6] 0 0 1 0
[ebtr_toolpath_refinement_node-6] 0 0 0 1
[ebtr_toolpath_refinement_node-6] Error string:
[ebtr_toolpath_refinement_node-6] Descartes vertex failure: All IK solutions found were in collision or invalid. 36 samples tried.
[ebtr_toolpath_refinement_node-6] For sample 10 4 IK solutions were found, with a collision summary of:
[ebtr_toolpath_refinement_node-6] 
[ebtr_toolpath_refinement_node-6]   STEP          JOINT NAMES   STATE0   STATE1|  SUBSTEP         LINK1           LINK2  DISTANCE
[ebtr_toolpath_refinement_node-6] *********************************************|*************************************************
[ebtr_toolpath_refinement_node-6]    0/4   shoulder_pan_joint  -2.1868  -2.1868|      1/1  forearm_link            part    0.0009
[ebtr_toolpath_refinement_node-6]    0/4  shoulder_lift_joint  -0.4292  -0.4292|-------------------------------------------------
[ebtr_toolpath_refinement_node-6]    0/4          elbow_joint   0.4448   0.4448|
[ebtr_toolpath_refinement_node-6]    0/4        wrist_1_joint   3.1260   3.1260|
[ebtr_toolpath_refinement_node-6]    0/4        wrist_2_joint   0.4415   0.4415|
[ebtr_toolpath_refinement_node-6]    0/4        wrist_3_joint  -0.0000  -0.0000|
[ebtr_toolpath_refinement_node-6] *********************************************|*************************************************
[ebtr_toolpath_refinement_node-6]    1/4   shoulder_pan_joint  -2.1868  -2.1868|      1/1           car  upper_arm_link   -0.0203
[ebtr_toolpath_refinement_node-6]    1/4  shoulder_lift_joint  -0.0001  -0.0001|-------------------------------------------------
[ebtr_toolpath_refinement_node-6]    1/4          elbow_joint  -0.4448  -0.4448|
[ebtr_toolpath_refinement_node-6]    1/4        wrist_1_joint  -2.6966  -2.6966|
[ebtr_toolpath_refinement_node-6]    1/4        wrist_2_joint   0.4415   0.4415|
[ebtr_toolpath_refinement_node-6]    1/4        wrist_3_joint  -0.0000  -0.0000|
[ebtr_toolpath_refinement_node-6] *********************************************|*************************************************
[ebtr_toolpath_refinement_node-6]    2/4   shoulder_pan_joint   1.2610   1.2610|      1/1  forearm_link             tcp   -0.0258
[ebtr_toolpath_refinement_node-6]    2/4  shoulder_lift_joint  -3.1415  -3.1415|-------------------------------------------------
[ebtr_toolpath_refinement_node-6]    2/4          elbow_joint   0.4448   0.4448|
[ebtr_toolpath_refinement_node-6]    2/4        wrist_1_joint  -0.4450  -0.4450|
[ebtr_toolpath_refinement_node-6]    2/4        wrist_2_joint  -3.0063  -3.0063|
[ebtr_toolpath_refinement_node-6]    2/4        wrist_3_joint  -0.0000  -0.0000|
[ebtr_toolpath_refinement_node-6] *********************************************|*************************************************
[ebtr_toolpath_refinement_node-6]    3/4   shoulder_pan_joint   1.2610   1.2610|      1/1  forearm_link             tcp   -0.0261
[ebtr_toolpath_refinement_node-6]    3/4  shoulder_lift_joint  -2.7124  -2.7124|-------------------------------------------------
[ebtr_toolpath_refinement_node-6]    3/4          elbow_joint  -0.4448  -0.4448|
[ebtr_toolpath_refinement_node-6]    3/4        wrist_1_joint   0.0156   0.0156|
[ebtr_toolpath_refinement_node-6]    3/4        wrist_2_joint  -3.0063  -3.0063|
[ebtr_toolpath_refinement_node-6]    3/4        wrist_3_joint  -0.0000  -0.0000|
[ebtr_toolpath_refinement_node-6] *********************************************|*************************************************
[ebtr_toolpath_refinement_node-6] For sample 11 4 IK solutions were found, with a collision summary of:
[ebtr_toolpath_refinement_node-6] 
[ebtr_toolpath_refinement_node-6]   STEP          JOINT NAMES   STATE0   STATE1|  SUBSTEP         LINK1  LINK2  DISTANCE
[ebtr_toolpath_refinement_node-6] *********************************************|****************************************
[ebtr_toolpath_refinement_node-6]    0/4   shoulder_pan_joint  -2.1974  -2.1974|      1/1  forearm_link   part   -0.0134
[ebtr_toolpath_refinement_node-6]    0/4  shoulder_lift_joint  -0.5330  -0.5330|----------------------------------------
[ebtr_toolpath_refinement_node-6]    0/4          elbow_joint   0.6475   0.6475|
[ebtr_toolpath_refinement_node-6]    0/4        wrist_1_joint   3.0271   3.0271|
[ebtr_toolpath_refinement_node-6]    0/4        wrist_2_joint   0.2775   0.2775|
[ebtr_toolpath_refinement_node-6]    0/4        wrist_3_joint  -0.0000  -0.0000|
[ebtr_toolpath_refinement_node-6] *********************************************|****************************************
[ebtr_toolpath_refinement_node-6]    1/4   shoulder_pan_joint  -2.1974  -2.1974|      1/1  forearm_link   part   -0.0249
[ebtr_toolpath_refinement_node-6]    1/4  shoulder_lift_joint   0.0912   0.0912|----------------------------------------
[ebtr_toolpath_refinement_node-6]    1/4          elbow_joint  -0.6475  -0.6475|
[ebtr_toolpath_refinement_node-6]    1/4        wrist_1_joint  -2.5852  -2.5852|
[ebtr_toolpath_refinement_node-6]    1/4        wrist_2_joint   0.2775   0.2775|
[ebtr_toolpath_refinement_node-6]    1/4        wrist_3_joint  -0.0000  -0.0000|
[ebtr_toolpath_refinement_node-6] *********************************************|****************************************
[ebtr_toolpath_refinement_node-6]    2/4   shoulder_pan_joint   1.2595   1.2595|      1/1  forearm_link    tcp   -0.0217
[ebtr_toolpath_refinement_node-6]    2/4  shoulder_lift_joint   3.0505   3.0505|----------------------------------------
[ebtr_toolpath_refinement_node-6]    2/4          elbow_joint   0.6474   0.6474|
[ebtr_toolpath_refinement_node-6]    2/4        wrist_1_joint  -0.5562  -0.5562|
[ebtr_toolpath_refinement_node-6]    2/4        wrist_2_joint   3.1038   3.1038|
[ebtr_toolpath_refinement_node-6]    2/4        wrist_3_joint   0.0001   0.0001|
[ebtr_toolpath_refinement_node-6] *********************************************|****************************************
[ebtr_toolpath_refinement_node-6]    3/4   shoulder_pan_joint   1.2595   1.2595|      1/1  forearm_link    tcp   -0.0217
[ebtr_toolpath_refinement_node-6]    3/4  shoulder_lift_joint  -2.6086  -2.6086|----------------------------------------
[ebtr_toolpath_refinement_node-6]    3/4          elbow_joint  -0.6474  -0.6474|
[ebtr_toolpath_refinement_node-6]    3/4        wrist_1_joint   0.1145   0.1145|
[ebtr_toolpath_refinement_node-6]    3/4        wrist_2_joint   3.1038   3.1038|
[ebtr_toolpath_refinement_node-6]    3/4        wrist_3_joint   0.0001   0.0001|
[ebtr_toolpath_refinement_node-6] *********************************************|****************************************

@Levi-Armstrong Levi-Armstrong force-pushed the feat/better-descartes-logging branch from 6fe2c3b to 9d57806 Compare June 3, 2024 17:10
@codecov Codecov
Copy link

codecov bot commented Jun 3, 2024 •
edited
Loading

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 85.65%. Comparing base (06bdebc) to head (3a229a6).

Additional details and impacted files

Impacted file tree graph

@@            Coverage Diff             @@
##           master     #401      +/-   ##
==========================================
- Coverage   85.77%   85.65%   -0.13%     
==========================================
  Files         229      229              
  Lines       15205    15238      +33     
==========================================
+ Hits        13042    13052      +10     
- Misses       2163     2186      +23

see 1 file with indirect coverage changes

@Levi-Armstrong Levi-Armstrong force-pushed the feat/better-descartes-logging branch from 9905211 to 3a229a6 Compare June 3, 2024 18:09
@marrts
Copy link
Contributor Author

marrts commented Jun 3, 2024

I eventually want to update this to produce a more compact collision summary. Currently it can produce a table that is rather large and takes up an annoying amount to terminal lines.

@Levi-Armstrong Levi-Armstrong merged commit 98c2a3a into tesseract-robotics:master Jun 3, 2024
10 of 13 checks passed
@Levi-Armstrong
Copy link
Contributor

I eventually want to update this to produce a more compact collision summary. Currently it can produce a table that is rather large and takes up an annoying amount to terminal lines.

I went ahead and merged but open a MR with with the changes making it compact.

This was referenced Jun 3, 2024
Merged
Merged
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Reviewers
No reviews
Assignees
No one assigned
Labels
None yet
Projects
None yet
Milestone
No milestone
Development

Successfully merging this pull request may close these issues.
2 participants
@marrts @Levi-Armstrong