add assertion, fix to work on simulation

@130s, if you intend to use this code on real robot, please ignore this
also we need to include this code in test-hironx.test, i think.