More compact descartes collision logging output

[marrts]

Goes along with tesseract-robotics/tesseract#1011.

Output looks like:

0: Working Frame: world, TCP Frame: robot_tcp
Target Pose:
  0.835179   0.532937   0.135848    13.2953
 -0.536658   0.843732 -0.0106764   0.306787
  -0.12031 -0.0639875   0.990672   0.701792
         0          0          0          1
TCP Offset:
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
Error string:
Descartes vertex failure: No IK solutions were found. 1 samples tried.

28: Working Frame: world, TCP Frame: robot_tcp
Target Pose:
-0.0125163   0.991927  -0.126189   -3.10113
 -0.959721  -0.047341  -0.276939   0.256233
 -0.280677    0.11764   0.952566   0.722154
         0          0          0          1
TCP Offset:
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
Error string:
Descartes vertex failure: All IK solutions found were in collision or invalid. 1 samples tried.
For sample 0 91 IK solutions were found, with a collision summary of:
Link Name                      Collisions
-----------------------------------------
robot_link_2                   55
environment_object1            52
robot_link_3                   30
environment_object3            19
environment_object2            8
robot_link_4                   6
environment_object5            6
environment_object6            4
environment_object4            2

48: Working Frame: world, TCP Frame: robot_tcp
Target Pose:
    0.3922   0.902551  -0.177712   -2.92833
 -0.827438   0.430546   0.360523   0.158298
  0.401903 0.00564852   0.915665   0.753267
         0          0          0          1
TCP Offset:
1 0 0 0
0 1 0 0
0 0 1 0
0 0 0 1
Error string:
Descartes vertex failure: All IK solutions found were in collision or invalid. 1 samples tried.
For sample 0 261 IK solutions were found, with a collision summary of:
Link Name                      Collisions
-----------------------------------------
robot_link_2                   105
robot_link_3                   95
environment_object1            64
robot_link_4                   61
environment_object2            60
environment_object3            58
environment_object4            28
environment_object5            27
environment_object6            16
environment_object7            6
environment_object8            2

Meant to address #401 (comment)

[marip8]

Food for thought, but have we considered logging collisions in the form of a matrix? That might be a concise representation of colliding link pairs that would give a little bit more information than just a number next to link name. The column headers could be numbers that correspond the link name row to save space as well, like below:

-	1	2	3	4
link_1	-	27	0	2
link_2	27	-	0	0
link_3	0	0	-	0
link_4	2	0	0	-

[Levi-Armstrong]

Food for thought, but have we considered logging collisions in the form of a matrix? That might be a concise representation of colliding link pairs that would give a little bit more information than just a number next to link name. The column headers could be numbers that correspond the link name row to save space as well, like below:

• 1 	2 	3 	4
  

link_1 - 27 0 2
link_2 27 - 0 0
link_3 0 0 - 0
link_4 2 0 0 -

I like this approach!

[Levi-Armstrong]

@marrts Is there more work that needs to be done or can it be merged if CI passes?

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 85.70%. Comparing base (f095dd3) to head (deac15a).

Additional details and impacted files

@@           Coverage Diff           @@
##           master     #460   +/-   ##
=======================================
  Coverage   85.70%   85.70%           
=======================================
  Files         233      233           
  Lines       15324    15324           
=======================================
  Hits        13134    13134           
  Misses       2190     2190           

[marrts]

@marrts Is there more work that needs to be done or can it be merged if CI passes?

I don't have any other plans for this