search

RethinkRobotics-opensource / sns_ik

Saturation in the Null Space (SNS) Inverse Kinematic Library
86 stars 41 forks source link

Test nullspace #32

Closed rethink-imcmahon closed 8 years ago

rethink-imcmahon commented 8 years ago
IanTheEngineer commented 8 years ago

Alrighty, lemme rebase these onto master before we merge.

IanTheEngineer commented 8 years ago

The only new commit is b774007.

rethink-forrest commented 8 years ago

Can you include the NS performance metrics in the Position and Velocity summaries as well so everything is one location.

rethink-forrest commented 8 years ago

Thanks for those changes. LGTM

IanTheEngineer commented 8 years ago

For completeness, here are the results with the default params (except using use_nullspace_bias_task=true).

[ INFO] [1461341743.896496751]: IK Using joint right_j0 -3.0503 3.0503
[ INFO] [1461341743.896578772]: IK Using joint right_j1 -3.8183 2.2824
[ INFO] [1461341743.896606040]: IK Using joint right_j2 -3.0514 3.0514
[ INFO] [1461341743.896626996]: IK Using joint right_j3 -3.0514 3.0514
[ INFO] [1461341743.896649653]: IK Using joint right_j4 -2.9842 2.9842
[ INFO] [1461341743.896667993]: IK Using joint right_j5 -2.9842 2.9842
[ INFO] [1461341743.896691020]: IK Using joint right_j6 -4.71039 4.71039
[ INFO] [1461341743.956677016]: Using 7 joints
[ INFO] [1461341743.957666871]: *** Testing KDL with 1000 random samples
[ INFO] [1461341743.982158012]: 0% done
[ INFO] [1461341745.193315415]: 50% done
[ INFO] [1461341746.394022153]: KDL found 260 solutions (26%) with an average of 0.00144343 secs per sample
[ INFO] [1461341746.394108103]: KDL nullspace success rate: 27.1, avg l2_norm ratio: 0.899508, avg time: 0.000988268
[ INFO] [1461341746.394157923]: *** Testing TRAC-IK with 1000 random samples
[ WARN] [1461341746.394214226]: TRAC-IK does not support secondary tasks.
[ INFO] [1461341746.408067766]: 0% done
[ INFO] [1461341746.992215642]: TRAC-IK found 991 solutions (99.1%) with an average of 0.000592326 secs per sample
[ INFO] [1461341746.999857467]: sns_ik: Using joint right_j0 lb: -3.050, ub: 3.050, v: 1.480, a: 5.000
[ INFO] [1461341747.001965459]: sns_ik: Using joint right_j1 lb: -3.818, ub: 2.282, v: 1.130, a: 5.000
[ INFO] [1461341747.004412820]: sns_ik: Using joint right_j2 lb: -3.051, ub: 3.051, v: 1.660, a: 5.000
[ INFO] [1461341747.007023211]: sns_ik: Using joint right_j3 lb: -3.051, ub: 3.051, v: 1.660, a: 5.000
[ INFO] [1461341747.009058416]: sns_ik: Using joint right_j4 lb: -2.984, ub: 2.984, v: 2.960, a: 5.000
[ INFO] [1461341747.010876278]: sns_ik: Using joint right_j5 lb: -2.984, ub: 2.984, v: 2.960, a: 5.000
[ INFO] [1461341747.012528324]: sns_ik: Using joint right_j6 lb: -4.710, ub: 4.710, v: 3.860, a: 5.000
[ INFO] [1461341747.012622332]: SNS_IK: Set Velocity solver to Standard SNS solver.
[ INFO] [1461341747.013194819]: *** Testing SNS-IK with 1000 random samples
[ INFO] [1461341747.033983070]: 0% done
[ INFO] [1461341748.276084382]: 30% done
[ INFO] [1461341749.606743716]: 60% done
[ INFO] [1461341750.936873469]: 90% done
[ INFO] [1461341751.367950361]: SNS found 890 solutions (89%) with an average of 0.00127273 secs per sample
[ INFO] [1461341751.368002531]: SNS nullspace success rate: 0.803, avg l2_norm ratio: 0.900416, avg time: 0.0030746
[ INFO] [1461341751.368035502]: SNS_IK: Set Velocity solver to SNS Optimal Scale Margin solver.
[ INFO] [1461341751.368087816]: *** Testing SNS-IK with 1000 random samples
[ INFO] [1461341751.944951418]: 10% done
[ INFO] [1461341753.150207836]: 30% done
[ INFO] [1461341754.393838593]: 50% done
[ INFO] [1461341755.628362122]: 70% done
[ INFO] [1461341756.801302060]: 90% done
[ INFO] [1461341757.407791177]: SNS Optimal Scale Margin found 894 solutions (89.4%) with an average of 0.00199007 secs per sample
[ INFO] [1461341757.407855813]: SNS Optimal Scale Margin nullspace success rate: 0.834, avg l2_norm ratio: 0.807989, avg time: 0.00404168
[ INFO] [1461341757.407881207]: SNS_IK: Set Velocity solver to SNS Optimal solver.
[ INFO] [1461341757.407934373]: *** Testing SNS-IK with 1000 random samples
[ INFO] [1461341757.836961103]: 10% done
[ INFO] [1461341758.849896456]: 30% done
[ INFO] [1461341759.885076172]: 50% done
[ INFO] [1461341760.885432336]: 70% done
[ INFO] [1461341761.889746473]: 90% done
[ INFO] [1461341762.356757680]: SNS Optimal found 897 solutions (89.7%) with an average of 0.00153498 secs per sample
[ INFO] [1461341762.356814682]: SNS Optimal nullspace success rate: 0.824, avg l2_norm ratio: 0.827498, avg time: 0.00340614
[ INFO] [1461341762.356843691]: SNS_IK: Set Velocity solver to Fast SNS solver.
[ INFO] [1461341762.356893672]: *** Testing SNS-IK with 1000 random samples
[ INFO] [1461341762.893926760]: 20% done
[ INFO] [1461341764.033028090]: 60% done
[ INFO] [1461341765.230812932]: SNS Fast found 889 solutions (88.9%) with an average of 0.000607025 secs per sample
[ INFO] [1461341765.230890337]: SNS Fast nullspace success rate: 0.829, avg l2_norm ratio: 0.951593, avg time: 0.00225986
[ INFO] [1461341765.230922919]: SNS_IK: Set Velocity solver to Fast Optimal SNS solver.
[ INFO] [1461341765.230983269]: *** Testing SNS-IK with 1000 random samples
[ INFO] [1461341765.234653267]: 0% done
[ INFO] [1461341766.245564067]: 30% done
[ INFO] [1461341767.361993474]: 60% done
[ INFO] [1461341768.455637966]: 90% done
[ INFO] [1461341768.857045167]: SNS Fast Optimal found 895 solutions (89.5%) with an average of 0.000888509 secs per sample
[ INFO] [1461341768.857110393]: SNS Fast Optimal nullspace success rate: 0.823, avg l2_norm ratio: 0.881156, avg time: 0.00273017
[ INFO] [1461341768.857136567]: 
************************************
[ INFO] [1461341768.857159516]: Position IK Summary:
[ INFO] [1461341768.857235346]: SNS: 89.00% success rate with (time: 1.27 ± 1.31 ms)
[ INFO] [1461341768.857264321]: SNS Optimal Scale Margin: 89.40% success rate with (time: 1.99 ± 2.19 ms)
[ INFO] [1461341768.857286257]: SNS Optimal: 89.70% success rate with (time: 1.53 ± 1.74 ms)
[ INFO] [1461341768.857306692]: SNS Fast: 88.90% success rate with (time: 0.61 ± 0.62 ms)
[ INFO] [1461341768.857325351]: SNS Fast Optimal: 89.50% success rate with (time: 0.89 ± 0.92 ms)
[ INFO] [1461341768.857342822]: KDL: 26.00% success rate with (time: 1.44 ± 0.63 ms)
[ INFO] [1461341768.857358850]: TRAC: 99.10% success rate with (time: 0.59 ± 0.77 ms)
[ INFO] [1461341768.857372484]: 
************************************

[ INFO] [1461341768.857827022]: SNS_IK: Set Velocity solver to Standard SNS solver.
[ INFO] [1461341768.902962905]: SNS_IK: Set Velocity solver to SNS Optimal Scale Margin solver.
[ INFO] [1461341768.983392135]: SNS_IK: Set Velocity solver to SNS Optimal solver.
[ INFO] [1461341769.031318101]: SNS_IK: Set Velocity solver to Fast SNS solver.
[ INFO] [1461341769.067404657]: SNS_IK: Set Velocity solver to Fast Optimal SNS solver.
[ INFO] [1461341769.123358455]: *** Testing KDL-IK Velocities with 1000 random samples
[ INFO] [1461341769.123450433]: 0% done
[ INFO] [1461341769.137961443]: KDL Velocity found 694 solutions (69.4%) with an average of 1.1353e-05 secs per sample
[ INFO] [1461341769.138011247]: KDL Velocity Scaling Score 694 solutions (69.4%)
[ INFO] [1461341769.138028822]: 
************************************
[ INFO] [1461341769.138052484]: Velocity IK Summary:
[ INFO] [1461341769.138075390]: SNS: 97.30% w/o and 100.00% w/ scaling success rates with an average time of 0.042 ms
[ INFO] [1461341769.138093096]: SNS Optimal Scale Margin: 41.40% w/o and 100.00% w/ scaling success rates with an average time of 0.077 ms
[ INFO] [1461341769.138109519]: SNS Optimal: 86.00% w/o and 88.00% w/ scaling success rates with an average time of 0.045 ms
[ INFO] [1461341769.138126070]: SNS Fast: 97.30% w/o and 100.00% w/ scaling success rates with an average time of 0.033 ms
[ INFO] [1461341769.138140807]: SNS Fast Optimal: 58.60% w/o and 100.00% w/ scaling success rates with an average time of 0.052 ms
[ INFO] [1461341769.138158141]: KDL Velocity: 69.40% w/o and 69.40% w/ scaling success rates with an average time of 0.011 ms
[ INFO] [1461341769.138171992]: 
************************************
IanTheEngineer commented 8 years ago

And a valgrind run using only SNS (standard, Fast, and Fast Optimal):

==19345== 
==19345== HEAP SUMMARY:
==19345==     in use at exit: 5,915 bytes in 78 blocks
==19345==   total heap usage: 6,141,521 allocs, 6,141,443 frees, 1,032,094,055 bytes allocated
==19345== 
==19345== LEAK SUMMARY:
==19345==    definitely lost: 0 bytes in 0 blocks
==19345==    indirectly lost: 0 bytes in 0 blocks
==19345==      possibly lost: 1,171 bytes in 23 blocks
==19345==    still reachable: 4,744 bytes in 55 blocks
==19345==         suppressed: 0 bytes in 0 blocks
==19345== Rerun with --leak-check=full to see details of leaked memory
==19345== 
==19345== For counts of detected and suppressed errors, rerun with: -v
==19345== ERROR SUMMARY: 0 errors from 0 contexts (suppressed: 0 from 0)

Looks clean enough to me. I'm not going to post the detailed report, as it is silly long. Every one of the "possibly lost" comes from ROS and it's logging system library, log4cxx.