add release obj function
 try too catch only the closest 3 edges
add wait-until-udpate for joint-state