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 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_)));
}
void plAvBrainCritter::IEvalGoal()
{
// TODO: Implement pathfinding logic here
// (for now, this runs directly towards the goal)
fImmediateGoalPos = fFinalGoalPos;
// where am I relative to my goal?
const plSceneObject* creatureObj = fArmature->GetTarget(0);
hsVector3 view(creatureObj->GetCoordinateInterface()->GetLocalToWorld().GetAxis(hsMatrix44::kView));
hsPoint3 creaturePos;
hsQuat creatureRot;
fAvMod->GetPositionAndRotationSim(&creaturePos, &creatureRot);
hsVector3 goalVec(creaturePos - fImmediateGoalPos);
goalVec.Normalize();
fDotGoal = goalVec * view; // 1 = directly facing, 0 = 90 deg off, -1 = facing away
// calculate a vector pointing to the creature's right
hsQuat invRot = creatureRot.Conjugate();
hsPoint3 globRight = invRot.Rotate(&kAvatarRight);
fAngRight = globRight.InnerProduct(goalVec); // dot product, 1 = goal is 90 to the right, 0 = goal is in front or behind, -1 = goal is 90 to the left
if (fAvoidingAvatars)
{
// check to see we can see anyone in our way (if we can't see them, we can't avoid them)
std::vector<plArmatureMod*> playersICanSee = IAvatarsICanSee();
for (unsigned i = 0; i < playersICanSee.size(); ++i)
{
hsPoint3 avPos;
hsQuat avRot;
playersICanSee[i]->GetPositionAndRotationSim(&avPos, &avRot);
hsVector3 avVec(creaturePos - avPos);
avVec.Normalize();
float dotAv = avVec * goalVec;
if (dotAv > 0.5f) // within a 45deg angle in front of us
{
// a player is in the way, so we will change our "goal" to a 90deg angle from the player
// then we stop searching, since any other players in the way will just produce the same (or similar) result
avVec.fZ = goalVec.fZ = 0.f;
goalVec = goalVec % avVec;
fAngRight = globRight.InnerProduct(goalVec);
break;
}
}
}
// are we at our final goal?
if (AtGoal())
{
if (RunningBehavior(kDefaultRunBehName)) // don't do anything if we're not running!
{
// we're close enough, stop running and pick an idle
fNextMode = IPickBehavior(kIdle);
// tell everyone who cares that we have arrived
for (unsigned i = 0; i < fReceivers.size(); ++i)
{
plAIArrivedAtGoalMsg* msg = new plAIArrivedAtGoalMsg(fArmature->GetKey(), fReceivers[i]);
msg->Goal(fFinalGoalPos);
msg->Send();
}
}
}
}
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);
}
