Adding trajectory_gen

Moving over old trajectory generation utilities from ur_cart_move.
Everything is integrated and works with bin_manager