Rebased  #148 (Added pos+vel and pos+vel+acc command interfaces)

Rebased #148 to indigo-devel
It compiles, but I haven't tested it yet.