Eigen::MatrixXf BezierQuadraticGenerator::generateGuess(std::vector<double> start, std::vector<double> end, std::vector<double> parameters){
Eigen::Map<Eigen::VectorXd> startVec(start.data(), start.size());
Eigen::Map<Eigen::VectorXd> endVec(end.data(), end.size());
Eigen::Map<Eigen::VectorXd> midVec(parameters.data(), parameters.size());
//Error Checking Crap
// if (startVec.size() != endVec.size() || startVec.size() != midVec.size()){
// std::cout<< "Dimensions for generateGuess are incorrect"<<std::endl;
// std::cout<< "Size of start = " << startVec.size();
// std::cout<< ", Size of mid = " << midVec.size();
// std::cout<< ", Size of end = " << endVec.size();
// std::cout<<std::endl;
// }
Eigen::MatrixXf trajectory(num_waypoints,start.size());
for( int row = 0; row < num_waypoints; row++){
double t = (double) row / (double) (num_waypoints-1);
//Bezier curves are defined as
// B(t) = (1-t)^2*P0 + 2*(1-t)*t*P1 +
Eigen::VectorXd waypoint = ((1-t)*(1-t)*startVec + 2*(1-t)*t*midVec + t*t*endVec);
trajectory.row(row) = waypoint.cast<float>().transpose();
}
return trajectory;
}
void DensoSetup::initialize(RobotBasePtr probot)
{
const ob::StateSpacePtr &sSpace = getStateSpace();
sSpace->setup();
ob::ScopedState<> start(sSpace);
ob::ScopedState<> goal(sSpace);
std::vector<double> startVec(sSpace->getDimension(), 0.);
std::vector<double> goalVec(sSpace->getDimension(), 0.);
unsigned int ndof = si_->getStateSpace()->as<DensoStateSpace>()->
_probot->GetActiveDOF();
for (unsigned int i = 0; i < ndof; i++)
goalVec[i] = 1.0;
if (_useRealScene)
{
startVec[0] = -1.576;
startVec[1] = 1.374;
startVec[2] = 0.0;
startVec[3] = 0.0;
startVec[4] = -1.36751533;
startVec[5] = 1.615;
goalVec[0] = 1.55;
goalVec[1] = 1.35;
goalVec[2] = 0.1;
goalVec[3] = -000057;
goalVec[4] = -1.40754;
goalVec[5] = 1.6;
}
sSpace->copyFromReals(start.get(), startVec);
sSpace->copyFromReals(goal.get(), goalVec);
/* set start, goal, and threshold*/
setStartAndGoalStates(start, goal, threshold);
/* set propagation step size and min-max propagation steps*/
si_->setPropagationStepSize(propagationStepSize);
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
/* initialize a new KPIECE planner */
setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_)));
/* set default parameters */
getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount);
getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias);
/* set a state validity checker */
setStateValidityChecker(ob::StateValidityCheckerPtr
(new DensoStateValidityChecker(si_)));
si_->setStateValidityCheckingResolution(0.05); //5%
/* set a state propagator */
setStatePropagator(oc::StatePropagatorPtr(new DensoStatePropagator(si_)));
}
void KoulesSetup::initialize(unsigned int numKoules, const std::string& plannerName,
const std::vector<double>& stateVec)
{
const ob::StateSpacePtr& space = getStateSpace();
space->setup();
// setup start state
ob::ScopedState<> start(space);
if (stateVec.size() == space->getDimension())
space->copyFromReals(start.get(), stateVec);
else
{
// Pick koule positions evenly radially distributed, but at a linearly
// increasing distance from the center. The ship's initial position is
// at the center. Initial velocities are 0.
std::vector<double> startVec(space->getDimension(), 0.);
double r, theta = boost::math::constants::pi<double>(), delta = 2.*theta / numKoules;
startVec[0] = .5 * sideLength;
startVec[1] = .5 * sideLength;
startVec[4] = .5 * delta;
for (unsigned int i = 0; i < numKoules; ++i, theta += delta)
{
r = .1 + i * .1 / numKoules;
startVec[4 * i + 5] = .5 * sideLength + r * cos(theta);
startVec[4 * i + 6] = .5 * sideLength + r * sin(theta);
}
space->copyFromReals(start.get(), startVec);
}
setStartState(start);
// set goal
setGoal(std::make_shared<KoulesGoal>(si_));
// set propagation step size
si_->setPropagationStepSize(propagationStepSize);
// set min/max propagation steps
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// set directed control sampler; when using the PDST planner, propagate as long as possible
bool isPDST = plannerName == "pdst";
const ompl::base::GoalPtr& goal = getGoal();
si_->setDirectedControlSamplerAllocator(
[&goal, isPDST](const ompl::control::SpaceInformation *si)
{
return KoulesDirectedControlSamplerAllocator(si, goal, isPDST);
});
// set planner
setPlanner(getConfiguredPlannerInstance(plannerName));
// set validity checker
setStateValidityChecker(std::make_shared<KoulesStateValidityChecker>(si_));
// set state propagator
setStatePropagator(std::make_shared<KoulesStatePropagator>(si_));
}
void SuperBotSetup::initialize(RobotBasePtr probot)
{
const ob::StateSpacePtr &sSpace = getStateSpace();
sSpace->setup();
ob::ScopedState<> start(sSpace);
ob::ScopedState<> goal(sSpace);
std::vector<double> startVec(sSpace->getDimension(), 0.);
std::vector<double> goalVec(sSpace->getDimension(), 0.);
unsigned int ndof = si_->getStateSpace()->as<SuperBotStateSpace>()->_probot->GetActiveDOF();
for (unsigned int i = 0; i < ndof; i++)
goalVec[i] = 1.0;
sSpace->copyFromReals(start.get(), startVec);
sSpace->copyFromReals(goal.get(), goalVec);
// set start, goal, and threshold
setStartAndGoalStates(start, goal, threshold);
// set propagation step size and min-max propagation steps
si_->setPropagationStepSize(propagationStepSize);
si_->setMinMaxControlDuration(propagationMinSteps, propagationMaxSteps);
// initialize a new KPIECE planner
setPlanner(ob::PlannerPtr(new oc::KPIECE1(si_)));
// set default parameters
getPlanner()->as<oc::KPIECE1>()->setMaxCloseSamplesCount(maxCloseSamplesCount);
getPlanner()->as<oc::KPIECE1>()->setGoalBias(goalBias);
// set a state validity checker
setStateValidityChecker(ob::StateValidityCheckerPtr(new SuperBotStateValidityChecker(si_)));
si_->setStateValidityCheckingResolution(0.05); //5%
// set a state propagator
setStatePropagator(oc::StatePropagatorPtr(new SuperBotStatePropagator(si_)));
}
int main(int argc, char **argv)
{
if (argc < 2)
{
std::cout << "Usage:\n" << argv[0] << " <num_links>\n";
exit(0);
}
unsigned int numLinks = boost::lexical_cast<unsigned int>(std::string(argv[1]));
Environment env = createHornEnvironment(numLinks, log((double)numLinks) / (double)numLinks);
ompl::base::StateSpacePtr chain(new KinematicChainSpace(numLinks, 1. / (double)numLinks, &env));
ompl::geometric::SimpleSetup ss(chain);
ss.setStateValidityChecker(ompl::base::StateValidityCheckerPtr(
new KinematicChainValidityChecker(ss.getSpaceInformation())));
ompl::base::ScopedState<> start(chain), goal(chain);
std::vector<double> startVec(numLinks, boost::math::constants::pi<double>() / (double)numLinks);
std::vector<double> goalVec(numLinks, 0.);
startVec[0] = 0.;
goalVec[0] = boost::math::constants::pi<double>() - .001;
chain->setup();
chain->copyFromReals(start.get(), startVec);
chain->copyFromReals(goal.get(), goalVec);
ss.setStartAndGoalStates(start, goal);
// SEKRIT BONUS FEATURE:
// if you specify a second command line argument, it will solve the
// problem just once with STRIDE and print out the solution path.
if (argc > 2)
{
ss.setPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
ss.setup();
ss.print();
ss.solve(3600);
ss.simplifySolution();
ompl::geometric::PathGeometric path = ss.getSolutionPath();
std::vector<double> v;
for(unsigned int i = 0; i < path.getStateCount(); ++i)
{
chain->copyToReals(v, path.getState(i));
std::copy(v.begin(), v.end(), std::ostream_iterator<double>(std::cout, " "));
std::cout << std::endl;
}
exit(0);
}
// by default, use the Benchmark class
double runtime_limit = 60, memory_limit = 1024;
int run_count = 20;
ompl::tools::Benchmark::Request request(runtime_limit, memory_limit, run_count, 0.5);
ompl::tools::Benchmark b(ss, "KinematicChain");
b.addExperimentParameter("num_links", "INTEGER", std::to_string(numLinks));
b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::STRIDE(ss.getSpaceInformation())));
b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::EST(ss.getSpaceInformation())));
b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::KPIECE1(ss.getSpaceInformation())));
b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::RRT(ss.getSpaceInformation())));
b.addPlanner(ompl::base::PlannerPtr(new ompl::geometric::PRM(ss.getSpaceInformation())));
b.benchmark(request);
b.saveResultsToFile(boost::str(boost::format("kinematic_%i.log") % numLinks).c_str());
exit(0);
}
