AnytimeBidirectionalEST(const Workspace &workspace, const Agent &agent, const InstanceFileMap &args, bool quiet = false,
		double gBound = std::numeric_limits<double>::infinity()) :
		workspace(workspace), agent(agent),
		forwardKDTree(KDTreeType(), agent.getDistanceEvaluator(), agent.getTreeStateSize()), backwardKDTree(KDTreeType(), agent.getDistanceEvaluator(), agent.getTreeStateSize()),
		quiet(quiet), gBound(gBound) {
		
		collisionCheckDT = args.doubleVal("Collision Check Delta t");
		goalBias = args.doubleVal("Goal Bias");
		howManySamplePoints = args.integerVal("Number of Sample Points");
		samplePointRadius = args.doubleVal("Sample Point Radius");
		linkRadius = args.doubleVal("Link Radius");

		if(!quiet) {
			dfpair(stdout, "collision check dt", "%g", collisionCheckDT);
			dfpair(stdout, "number of sample points", "%u", howManySamplePoints);
		}

		unsigned int regionCount = discretization.getCellCount();
		forwardRegions.reserve(regionCount);
		backwardRegions.reserve(regionCount);
		for(unsigned int i = 0; i < regionCount; ++i) {
			forwardRegions.push_back(new RegionNode(i));
			backwardRegions.push_back(new RegionNode(i));
		}

		samplesGenerated = edgesGenerated = samplesAdded = edgesAdded = 0;

		timeout = args.doubleVal("Timeout");

		selfPtr = this;
	}
void go_AnytimeRRT(const InstanceFileMap &args, const Agent &agent, const Workspace &workspace,
            const typename Agent::State &start, const typename Agent::State &goal) {

	clock_t startT = clock();

	dfpair(stdout, "planner", "%s", "Anytime RRT");
	// typedef flann::KDTreeSingleIndexParams KDTreeType;
	typedef flann::KDTreeIndexParams KDTreeType;
	typedef FLANN_KDTreeWrapper<KDTreeType, typename Agent::DistanceEvaluator, typename Agent::Edge> KDTree;
	typedef UniformSampler<Workspace, Agent, KDTree> USampler;
	typedef GoalBiasSampler<Agent, USampler> GBSampler;
	typedef TreeInterface<Agent, KDTree, GBSampler> TreeInterface;
	typedef AnytimeRRT<Workspace, Agent, TreeInterface> Planner;

	/* planner config */

	KDTreeType kdtreeType(1);
	KDTree kdtree(kdtreeType, agent.getDistanceEvaluator(), agent.getTreeStateSize());
	USampler uniformsampler(workspace, agent, kdtree);

	double goalBias = args.exists("Goal Bias") ? args.doubleVal("Goal Bias") : 0;
	dfpair(stdout, "goal bias", "%g", goalBias);

	GBSampler goalbiassampler(uniformsampler, goal, goalBias);
	TreeInterface treeInterface(kdtree, goalbiassampler);
	Planner planner(workspace, agent, treeInterface, args);

	go_COMMONANYTIME<Planner, Workspace, Agent>(args, planner, workspace, agent, start, goal, startT);
}
Ejemplo n.º 3
0
	SST(const Workspace &workspace, const Agent &agent,
		InsertionInteface &insertionInterface, QueryInterface &queryInterface, double startingRadius, double resizeThreshold,
		unsigned int historySize = 100) : insertionInterface(insertionInterface), queryInterface(queryInterface),
		witnessInterface(KDTreeType(), agent.getDistanceEvaluator(), agent.getTreeStateSize()), radius(startingRadius), resizeThreshold(resizeThreshold),
		history(historySize), historyIndex(0), historyFails(0), historyFilled(false) {}