Adding ompl control type motion model and filters