Making CBiRRT exactly like BiRRT. We need the project_z_val_manip method now to update the code.