HEAPUtil accepted to IEEE Robotics and Automation Letters (RA-L) and IROS 2021!