(ubr1_moveit) add costs