Add option to command position control, specifying separate sets of constants in MBS.

[mikepurvis]

Combined with https://github.com/g/grizzly_robot/tree/active-brake, this solves two long-standing Grizzly motion-control issues:

• Creeping forward when stopped on a level.
• Unable to hold position when stopped on a steep incline.

Note that this change breaks MD5 roboteq_msgs/Command. It was felt important that the position/vel decision be make at a higher level than in the driver, therefore it was necessary to modify the message.

Development was done on a bench rig, end-to-end verification on Cuddles.

To review: @paulbovbel, @tonybaltovski, @servos, @abencz

[mikepurvis]

An additional point observed during additional bench testing: The Roboteq does not enforce command timeouts when in operating Mode 3 (closed-loop count position). So if you command a closed-loop position, and then cease sending that command, the controller will continue to seek to that position until commanded otherwise. This could be resolved by switching back to Mode 1 (or simply sending MSTOP) upon cessation of user commands, but this requires introducing a supervisory timer to the driver.

[tonybaltovski]

LGTM.

[mikepurvis]

Based on tests on Friday, it actually has to be both switching back to mode 1 and issuing MSTOP. MSTOP alone only stops a G (velocity) command, not a P (position) one.