Added sampling from goal region for BITStar. Added BITStar for cartpole, but need to tune SQP. Plotting for cartpole works.