Add arm trajectory control

[marinagmoreira]

• Adding arm to plangen in trapezoidal fplanner (used in executive through rosrun executive plan_pub $1) and bin (used in rosrun mobility teleop -exec $1)
• Modified the arm logic to be self contained in the arm behaviour instead of the choreographer (initial approach to this)
• Arm set point callback that allows a ros node control the arm at will, by publishing waypoints allowing for any trajectory (with collision check)

[bcoltin]

@marinagmoreira what is left on this?

[marinagmoreira]

I think something involving the fplan wasn't working properly, I really want to revisit this :)