Add ability to briefly summarize trajectory collisions

[marrts]

This is to address the problem brought up here

The output looks like this:

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

This is intended to help debug if the user realizes that the haven't disabled a collision, or give the user a place to look for what might be causing problems

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 89.86%. Comparing base (37092b7) to head (5cf57de).

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1011      +/-   ##
==========================================
- Coverage   90.13%   89.86%   -0.28%     
==========================================
  Files         280      280              
  Lines       15859    15908      +49     
==========================================
  Hits        14295    14295              
- Misses       1564     1613      +49     

see 1 file with indirect coverage changes

[Levi-Armstrong]

Lets wait to merge until what is decided on the output from this discussion.

[marrts]

Changed to output something this:

                         |    0|    1|    2|    3|    4|    5|    6|
                         |-----|-----|-----|-----|-----|-----|-----|
0                env_obj1|
1            robot_link_4|   11|
2                env_obj2|    0|    0|
3            robot_link_2|  136|    0|   42|
4            robot_link_3|   14|    0|    0|    0|
5                env_obj3|    0|    0|    0|    6|    0|
6                env_obj4|    0|    0|    0|    8|    0|    0|

I chose for it to just be a bottom left triangle matrix becuase the rest of the information is redundant and with the console log we were running into a character output limit so this reduces the number of characters needed.

[Levi-Armstrong]

LGTM, Once you have CI passing let me know and I will review.

[Levi-Armstrong]

@marrts I fixed the clang-tidy errors so I should be able to squash merged after CI finishes.