Cleaned up the partol code.

Added collision debug code in partol.
Merge branch 'testing' of https://github.com/jappeace/hw-isgp-kbs into testing