Add floating joint support

[Levi-Armstrong]

• Update unit tests
  • Update existing tests
  • Add setState test with changing floating joint state values
• Profile state solver

Master Branch:

Running ./tesseract_environment_kinematics
Run on (16 X 4600 MHz CPU s)
CPU Caches:
  L1 Data 48K (x8)
  L1 Instruction 32K (x8)
  L2 Unified 1280K (x8)
  L3 Unified 24576K (x1)
Load Average: 0.26, 1.58, 1.59
-----------------------------------------------------------------------------------------------------
Benchmark                                                           Time             CPU   Iterations
-----------------------------------------------------------------------------------------------------
BM_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS/real_time               6.76 us         6.76 us       104520
BM_SET_AND_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS/real_time       6.35 us         6.35 us       109127
BM_GET_JACOBIAN_JOINT_NAMES_JOINT_VALUES_SS/real_time            4.09 us         4.09 us       170754
BM_CALC_FWD_KIN_MANIP/real_time                                  8.60 us         8.60 us        80563
BM_GET_JACOBIAN_MANIP/real_time                                  3.23 us         3.23 us       217344

Post Changes:

Running ./tesseract_environment_kinematics
Run on (16 X 4600 MHz CPU s)
CPU Caches:
  L1 Data 48K (x8)
  L1 Instruction 32K (x8)
  L2 Unified 1280K (x8)
  L3 Unified 24576K (x1)
Load Average: 0.38, 0.68, 0.92
-----------------------------------------------------------------------------------------------------
Benchmark                                                           Time             CPU   Iterations
-----------------------------------------------------------------------------------------------------
BM_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS/real_time               6.71 us         6.71 us       103161
BM_SET_AND_GET_STATE_JOINT_NAMES_JOINT_VALUES_SS/real_time       6.37 us         6.37 us       110148
BM_GET_JACOBIAN_JOINT_NAMES_JOINT_VALUES_SS/real_time            4.25 us         4.25 us       165280
BM_CALC_FWD_KIN_MANIP/real_time                                  8.14 us         8.14 us        85492
BM_GET_JACOBIAN_MANIP/real_time                                  3.35 us         3.35 us       209746

[codecov]

Codecov Report

All modified and coverable lines are covered by tests ✅

Project coverage is 88.75%. Comparing base (c0162ef) to head (3dbc1a3).
Report is 2 commits behind head on master.

Additional details and impacted files

@@            Coverage Diff             @@
##           master    #1060      +/-   ##
==========================================
- Coverage   89.00%   88.75%   -0.26%     
==========================================
  Files         294      294              
  Lines       16460    16598     +138     
==========================================
+ Hits        14650    14731      +81     
- Misses       1810     1867      +57     

see 8 files with indirect coverage changes