minor changes
sampling over weighted poses
tweaked params
resolved conflicts
adjusted parameters
goal from action message
added sanity checks
demo tweaks
using goal from action message