void SceneCamera::BrainOrder(const State& state)
{
if (state.GetStateType() == (int)State::TYPE::PAUSE )
{
if (state.IsNew())
{
SwitchCameraMode(CONTROL_TYPE::CHARACTER);
}
}
if (state.GetStateType() == (int)State::TYPE::MOTION )
{
if (state.IsNew())
{
SwitchCameraMode(CONTROL_TYPE::CHARACTER);
}
Motion* motion = (Motion*)&state;
cameraControl.Move(motion->GetSpeed(), motion->GetDir());
}
if (state.GetStateType() == (int)State::TYPE::SPACE_LOC ) {
SpaceLocation* space_loc = (SpaceLocation*)&state;
if (state.IsNew())
{
SwitchCameraMode(CONTROL_TYPE::AUTO_CONTROL);
}
((CameraAutoControl*)control[currentType])->SetPosition(space_loc->GetPos());
((CameraAutoControl*) control[currentType])->SetDirection(space_loc->GetDir());
}
}
int main()
{
Motion motion;
//motion.moveRight(1000);
//usleep(5000000);
motion.moveForward(1000);
motion.moveForward(200);
//usleep(5000000);
//motion.spinACW(90);
//usleep(5000000);
//motion.spinCW(90);
//motion.moveQ2(1000);
//motion.moveQ1(500);
//usleep(5000000);
//motion.moveQ2(500);
//usleep(5000000);
//motion.moveQ3(500);
//usleep(5000000);
//motion.moveQ4(500);
//usleep(5000000);
}
void ompl::geometric::FMT::sampleFree(const base::PlannerTerminationCondition &ptc)
{
unsigned int nodeCount = 0;
unsigned int sampleAttempts = 0;
Motion *motion = new Motion(si_);
// Sample numSamples_ number of nodes from the free configuration space
while (nodeCount < numSamples_ && !ptc)
{
sampler_->sampleUniform(motion->getState());
sampleAttempts++;
bool collision_free = si_->isValid(motion->getState());
if (collision_free)
{
nodeCount++;
nn_->add(motion);
motion = new Motion(si_);
} // If collision free
} // While nodeCount < numSamples
si_->freeState(motion->getState());
delete motion;
// 95% confidence limit for an upper bound for the true free space volume
freeSpaceVolume_ = boost::math::binomial_distribution<>::find_upper_bound_on_p(sampleAttempts, nodeCount, 0.05) * si_->getStateSpace()->getMeasure();
}
void Motion::playMotions() {
vector<Motion *>::iterator motionIt;
for (motionIt = cMotions.begin() ; motionIt != cMotions.end(); ++motionIt) {
Motion *motion = *motionIt;
motion->playStep();
}
}
inline Motion operator^( const Motion& m1, const MotionRevoluteUnaligned & m2)
{
/* m1xm2 = [ v1xw2 + w1xv2; w1xw2 ] = [ v1xw2; w1xw2 ] */
const Motion::Vector3& v1 = m1.linear();
const Motion::Vector3& w1 = m1.angular();
const Motion::Vector3& w2 = m2.axis * m2.w ;
return Motion( v1.cross(w2),w1.cross(w2));
}
void Pose::set(const Motion &motion, MotionChannel::PosType custom_postype, MotionChannel::IsOpen custom_isopen, float keytime)
{
for(Motion::const_iterator i = motion.begin(); i != motion.end(); ++i) {
for(MotionChannelVector::const_iterator j = i->second.begin(); j != i->second.end(); ++j) {
set(i->first.c_str(), **j, custom_postype, custom_isopen, keytime);
}
}
}
friend Motion operator* (const ConstraintPlanar &, const Eigen::MatrixBase<D> & v)
{
EIGEN_STATIC_ASSERT_VECTOR_SPECIFIC_SIZE(D,3);
Motion result (Motion::Zero ());
result.linear ().template head<2> () = v.template topRows<2> ();
result.angular ().template tail<1> () = v.template bottomRows<1> ();
return result;
}
bool MotionManager::saveMotion(const std::string &animName, const std::string& fileName) //const
{
Motion* anim = getMotion(animName);
if(anim)
{
anim->writeToFile(fileName);
return true;
}
return false;
}
friend Motion operator+ (const MotionPlanar & m1, const Motion & m2)
{
Motion result (m2);
result.linear ()[0] += m1.x_dot_;
result.linear ()[1] += m1.y_dot_;
result.angular ()[2] += m1.theta_dot_;
return result;
}
int main(){
int width = 800;
int height = 600;
//Game Window
static Window window("Interstellar Explorer", width, height);
glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
Motion motion;
Orbit orb1; //Also this
Orbit orb2;
// Construction
PlayerObject player("Imgs/ship.png"); //Set filename to empty quotes to leave player as polygon
vector<CircleObject> test = loadPlanets(test, "level.txt");
//OpenGl Coordinate Grid Setup
glViewport(0, 0, width, height);
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-width / 2, width / 2, -height / 2, height / 2, 0, 1);
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
while (!window.closed()){
window.clear();
checkForInput(&window, &motion);
// Translation
glPushMatrix();
motion.applySpeed();
for (unsigned int i = 0; i < test.size(); i++){
test[i].Draw();
}
glPopMatrix();
// Rotation
glPushMatrix();
motion.applyRotation();
player.Draw();
glPopMatrix();
//Orbits
glPushMatrix();
orb1.orbit(test);
glPopMatrix();
window.update();
Sleep(0.5); //Controls how fast the game loop runs
}
return 0;
}
// Update with new frames
void LKTracker::Update(cv::Mat& frame, cv::Mat& next)
{
MotionVector::iterator iter;
// Update all tracked region
for(iter = this->regions.begin(); iter != this->regions.end(); ++iter)
{
Motion *motionRegion = *iter;
motionRegion->Update(frame, next);
}
}
Motion operator^ (const Motion & m1, const JointPlanar::MotionPlanar & m2)
{
Motion result;
const Motion::Vector3 & m1_t = m1.linear();
const Motion::Vector3 & m1_w = m1.angular();
result.angular () << m1_w(1) * m2.theta_dot_, - m1_w(0) * m2.theta_dot_, 0.;
result.linear () << m1_t(1) * m2.theta_dot_ - m1_w(2) * m2.y_dot_, - m1_t(0) * m2.theta_dot_ + m1_w(2) * m2.x_dot_, m1_w(0) * m2.y_dot_ - m1_w(1) * m2.x_dot_;
return result;
}
std::string MotionManager::loadMotion(const std::string& filename)
{
MatrixXd mat = loadData(filename);
if (mat.rows() > 0)
{
int pos = filename.find_last_of('\\') + 1;
std::string motionName = filename.substr(pos);
Motion *anim = new Motion(mat, motionName);
mMotion.push_back(anim);
return anim->getName();
}
return "";
}
void loop(){
motion.run();
delay(1000);
}
void primary_timer_func() override
{
motion.update(Framework::time_now);
update_reposition_camera();
NewWorldScreen::primary_timer_func();
}
void ompl::geometric::FMT::traceSolutionPathThroughTree(Motion *goalMotion)
{
std::vector<Motion*> mpath;
Motion *solution = goalMotion;
// Construct the solution path
while (solution != nullptr)
{
mpath.push_back(solution);
solution = solution->getParent();
}
// Set the solution path
PathGeometric *path = new PathGeometric(si_);
int mPathSize = mpath.size();
for (int i = mPathSize - 1 ; i >= 0 ; --i)
path->append(mpath[i]->getState());
pdef_->addSolutionPath(base::PathPtr(path), false, -1.0, getName());
}
void TileEntity::Update() {
Entity::Update();
// update the fire flicker
if ( IsOnFire() ) {
fireDurationCounter++;
if ( !(fireFlickerVisual->IsPlayingAnimation()) ) {
fireFlickerVisual->PlayAnimation( fireFlickerAnim );
}
}
// update the motion, if we have one
if ( !motionQueue.empty() ) {
Motion * curMotion = motionQueue.front();
curMotion->Update();
if ( curMotion->IsDone() ) {
motionQueue.pop_front();
pos->SetToClosestTile();
if ( sourceTileLoc != -1 ) {
// grab current chamber and remove from location
Chamber * curChamber = (ChamberManager::GetInstance()).GetCurrentChamber();
curChamber->UnregisterTileEntityInTile( this, sourceTileLoc );
// Call onExtited tile on all tile entities in previous tile
curChamber->OnEntityExitedTile( this, sourceTileLoc );
// Call onEntered tile on all tile entities except for this one in current tile
curChamber->OnEntityEnteredTile( this, Chamber::GetTileNumFromPos( pos->GetTileX(), pos->GetTileY() ) );
// Call onMoveCompleted on self
OnMoveCompleted( curMotion );
}
delete curMotion;
}
}
}
inline Motion operator^( const Motion& m1, const MotionPrismatic<2>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
* (x,y,z)^(0,0,v) = ( yv,-xv,0 )
* nu1^(0,vx) = ( vy1 vx,-vx1 vx, 0, 0, 0, 0 )
*/
const Motion::Vector3& w = m1.angular();
const double & vx = m2.v;
return Motion( Motion::Vector3(w[1]*vx,-w[0]*vx,0),
Motion::Vector3::Zero());
}
inline Motion operator^( const Motion& m1, const MotionPrismatic<1>& m2)
{
/* nu1^nu2 = ( v1^w2+w1^v2, w1^w2 )
* nu1^(v2,0) = ( w1^v2 , 0 )
* (x,y,z)^(0,v,0) = ( -zv,0,xv )
* nu1^(0,vx) = ( -vz1 vx,0,vx1 vx, 0, 0, 0)
*/
const Motion::Vector3& w = m1.angular();
const double & vx = m2.v;
return Motion( Motion::Vector3(-w[2]*vx,0,w[0]*vx),
Motion::Vector3::Zero());
}
void ompl::geometric::FMT::assureGoalIsSampled(const ompl::base::GoalSampleableRegion *goal)
{
// Ensure that there is at least one node near each goal
while (const base::State *goalState = pis_.nextGoal())
{
Motion *gMotion = new Motion(si_);
si_->copyState(gMotion->getState(), goalState);
std::vector<Motion*> nearGoal;
nn_->nearestR(gMotion, goal->getThreshold(), nearGoal);
// If there is no node in the goal region, insert one
if (nearGoal.empty())
{
OMPL_DEBUG("No state inside goal region");
if (si_->getStateValidityChecker()->isValid(gMotion->getState()))
{
nn_->add(gMotion);
goalState_ = gMotion->getState();
}
else
{
si_->freeState(gMotion->getState());
delete gMotion;
}
}
else // There is already a sample in the goal region
{
goalState_ = nearGoal[0]->getState();
si_->freeState(gMotion->getState());
delete gMotion;
}
} // For each goal
}
double MotionGraph::distance(int ma, int mb, int i, int j, double *theta, double *x0, double *z0)
{
int k = numBlendingFrames;
Skeleton *skeleton = library->getSkeleton();
Motion *motionA = library->getMotion(ma);
Motion *motionB = library->getMotion(mb);
if (skeleton == NULL || motionA == NULL || motionB == NULL)
{
printf("Error: in distance().\n");
exit(-1);
}
PointCloud **pcListA = new PointCloud *[k];
PointCloud **pcListB = new PointCloud *[k];
for (int f = i, ii = 0; f < i + k; f++, ii++)
{
skeleton->setPosture(*(motionA->GetPosture(f)));
pcListA[ii] = new PointCloud(skeleton);
}
for (int f = j - k + 1, ii = 0; f < j + 1; f++, ii++)
{
skeleton->setPosture(*(motionB->GetPosture(f)));
pcListB[ii] = new PointCloud(skeleton);
}
PointCloud pcA(&pcListA[0], k);
PointCloud pcB(&pcListB[0], k);
double dist = distance(&pcA, &pcB, theta, x0, z0);
for (int i = 0; i < k; i++)
delete pcListA[i];
for (int i = 0; i < k; i++)
delete pcListB[i];
delete [] pcListA;
delete [] pcListB;
return dist;
}
// For some reason draws points instead of lines
void Frame::drawMotion(MotionHandler * mh)
{
if (mh == NULL)
return;
if (mh->getState() == MotionHandler::RECORDING)
{
Motion motion = mh->getMotion();
if(motion.length() < 2)
return;
// Here's the problem - cur and last are the same
Candidate *last = motion.getCandidate(0),
*cur = motion.getCandidate(1);
float lastX = last->getX(),
lastY = last->getY(),
x = cur->getX(),
y = cur->getY();
cout << "last: " << lastX << ", " << lastY << endl;
cout << "cur: " << x << ", " << y << endl;
cvLine(motionTrack, cvPoint(x, y), cvPoint(lastX, lastY), cvScalar(0,0,255), 6);
cvAdd(curFrame, motionTrack, curFrame);
}
}
// Add region to tracking
void LKTracker::AddRegion(cv::Vec2i position, cv::Size regionSize, cv::Mat& frame, cv::Mat& next)
{
Motion * motionRegion = new Motion(position, regionSize, frame, next);
this->regions.push_back(motionRegion);
int num_regions = regions.size();
std::stringstream xStream, yStream, tStream;
xStream << num_regions << " X derivative";
yStream << num_regions << " Y derivative";
tStream << num_regions << " T derivative";
std::string x, y, t;
x = xStream.str();
y = yStream.str();
t = tStream.str();
//cv::namedWindow(x, 2);
//cv::namedWindow(y, 2);
//cv::namedWindow(t, 2);
motionRegion->SetWindowNames(x, y, t);
}
void MotionPanel::rotateMotion(MyGUI::Widget* sender)
{
std::size_t index = mCBAnim->getIndexSelected();
if (index != MyGUI::ITEM_NONE)
{
std::string motionName = mCBAnim->getItemNameAt(index);
Motion* mo = MotionManager::getSingleton().getMotion(motionName);
if(mo)
{
MyGUI::UString strAngle = static_cast<MyGUI::EditBox*>(mMainWidget->findWidget(mPrefix + "EBAngle"))->getCaption();
MyGUI::UString strAxisX = static_cast<MyGUI::EditBox*>(mMainWidget->findWidget(mPrefix + "EBAxisX"))->getCaption();
MyGUI::UString strAxisY = static_cast<MyGUI::EditBox*>(mMainWidget->findWidget(mPrefix + "EBAxisY"))->getCaption();
MyGUI::UString strAxisZ = static_cast<MyGUI::EditBox*>(mMainWidget->findWidget(mPrefix + "EBAxisZ"))->getCaption();
float angle = Ogre::StringConverter::parseReal(strAngle);
float axisX = Ogre::StringConverter::parseReal(strAxisX);
float axisY = Ogre::StringConverter::parseReal(strAxisY);
float axisZ = Ogre::StringConverter::parseReal(strAxisZ);
mo->rotateMotion(angle, Ogre::Vector3(axisX, axisY, axisZ));
}
}
}
void update_reposition_camera()
{
static float reposition_timer = 0;
// update the camera's scale and rotation
// initiate motion with the camera
reposition_timer -= 1/60.0;
if (reposition_timer <= 0)
{
reposition_timer = 3;
motion.cmove_to(&camera.placement.position.x, random_float(0, 40*21), 3, interpolator::double_slow_in_out);
motion.cmove_to(&camera.placement.position.y, random_float(0, 20*21), 3, interpolator::double_slow_in_out);
}
camera.placement.scale.x = sin(al_get_time())*0.1 + 2;
camera.placement.scale.y = camera.placement.scale.x;
camera.placement.rotation = sin(al_get_time())*TAU * 0.03;
}
void setup(){
//Initialization
Serial.begin(9600);
Wire.begin();
pinMode(13, OUTPUT);
digitalWrite(13, HIGH);
turnSensorSetup();
Serial.println("Press button to start calibration...");
button.waitForButton();
motion.setupReflectanceSensors();
Serial.println("Calibrated. Press button to start...");
button.waitForButton();
digitalWrite(13, LOW);
}
void LKTracker::ShowAll()
{
MotionVector::iterator iter;
// For each region, show derivatives
for(iter = this->regions.begin(); iter != this->regions.end(); ++iter)
{
Motion *motion = (*iter);
cv::Mat a, b, c;
motion->getIx().convertTo(a, CV_8U);
motion->getIy().convertTo(b, CV_8U);
motion->getIt().convertTo(c, CV_8U);
cv::normalize(a, a, 0, 255, cv::NORM_MINMAX);
cv::normalize(b, b, 0, 255, cv::NORM_MINMAX);
cv::normalize(c, c, 0, 255, cv::NORM_MINMAX);
cv::imshow(motion->getWindowTitleX(), a);
cv::imshow(motion->getWindowTitleY(), b);
cv::imshow(motion->getWindowTitleT(), c);
}
}
int main()
{
TypeNode type("test_type");
Entity tlve("0", 0), ent("1", 1), other("2", 2);
ent.m_location.m_loc = &tlve;
ent.m_location.m_pos = Point3D(1, 1, 0);
ent.m_location.m_velocity = Vector3D(1,0,0);
ent.setType(&type);
// Set up another entity to test collisions with.
other.m_location.m_loc = &tlve;
other.m_location.m_pos = Point3D(10, 0, 0);
other.setType(&type);
tlve.m_contains = new LocatedEntitySet;
tlve.m_contains->insert(&ent);
tlve.m_contains->insert(&other);
tlve.setType(&type);
tlve.incRef();
tlve.incRef();
Motion * motion = new Motion(ent);
std::string example_mode("walking");
motion->setMode(example_mode);
assert(motion->mode() == example_mode);
motion->adjustPostion();
motion->genUpdateOperation();
motion->genMoveOperation();
// No collisions yet
motion->checkCollisions();
assert(!motion->collision());
// Set up our moving entity with a bbox so collisions can be checked for.
ent.m_location.m_bBox = BBox(Point3D(-1,-1,-1), Point3D(1,1,1));
// No collision yet, as other still has no big box
motion->checkCollisions();
assert(!motion->collision());
// Set up the other entity with a bbox so collisions can be checked for.
other.m_location.m_bBox = BBox(Point3D(-1,-1,-1), Point3D(5,1,1));
// No collision yet, as other is still too far away
motion->checkCollisions();
assert(!motion->collision());
// Move it closer
other.m_location.m_pos = Point3D(3, 0, 0);
// Now it can collide
motion->checkCollisions();
assert(motion->collision());
motion->resolveCollision();
// Put the velocity back, as it was affected by the collision
ent.m_location.m_velocity = Vector3D(1,0,0);
// Set up the collision again
motion->checkCollisions();
assert(motion->collision());
// But this time break the hierarchy to hit the error message
other.m_location.m_loc = &ent;
motion->resolveCollision();
// Repair the hierarchy, and restore the velocity
other.m_location.m_loc = &tlve;
ent.m_location.m_velocity = Vector3D(1,0,0);
// Set up the collision again
motion->checkCollisions();
assert(motion->collision());
// Re-align the velocity so some is preserved by the collision normal
ent.m_location.m_velocity = Vector3D(1,1,0);
motion->resolveCollision();
// Put the velocity back, as it was affected by the collision
ent.m_location.m_velocity = Vector3D(1,0,0);
// Add another entity inside other
Entity inner("3", 3);
inner.m_location.m_loc = &other;
inner.m_location.m_pos = Point3D(0,0,0);
other.m_contains = new LocatedEntitySet;
other.m_contains->insert(&inner);
// Make other non-simple so that collision checks go inside
other.m_location.setSimple(false);
motion->checkCollisions();
other.m_location.m_orientation = Quaternion(1,0,0,0);
motion->checkCollisions();
inner.m_location.m_bBox = BBox(Point3D(-0.1,-0.1,-0.1), Point3D(0.1,0.1,0.1));
motion->checkCollisions();
// Move the inner entity too far away for collision this interval
inner.m_location.m_pos = Point3D(3,0,0);
motion->checkCollisions();
delete motion;
ent.m_location.m_loc = 0;
other.m_location.m_loc = 0;
inner.m_location.m_loc = 0;
return 0;
}
bool ompl::geometric::FMT::expandTreeFromNode(Motion **z)
{
// Find all nodes that are near z, and also in set Unvisited
std::vector<Motion*> xNear;
const std::vector<Motion*> &zNeighborhood = neighborhoods_[*z];
const unsigned int zNeighborhoodSize = zNeighborhood.size();
xNear.reserve(zNeighborhoodSize);
for (unsigned int i = 0; i < zNeighborhoodSize; ++i)
{
Motion *x = zNeighborhood[i];
if (x->getSetType() == Motion::SET_UNVISITED)
{
saveNeighborhood(x);
if (nearestK_)
{
// Only include neighbors that are mutually k-nearest
// Relies on NN datastructure returning k-nearest in sorted order
const base::Cost connCost = opt_->motionCost((*z)->getState(), x->getState());
const base::Cost worstCost = opt_->motionCost(neighborhoods_[x].back()->getState(), x->getState());
if (opt_->isCostBetterThan(worstCost, connCost))
continue;
else
xNear.push_back(x);
}
else
xNear.push_back(x);
}
}
// For each node near z and in set Unvisited, attempt to connect it to set Open
std::vector<Motion*> yNear;
std::vector<Motion*> Open_new;
const unsigned int xNearSize = xNear.size();
for (unsigned int i = 0 ; i < xNearSize; ++i)
{
Motion *x = xNear[i];
// Find all nodes that are near x and in set Open
const std::vector<Motion*> &xNeighborhood = neighborhoods_[x];
const unsigned int xNeighborhoodSize = xNeighborhood.size();
yNear.reserve(xNeighborhoodSize);
for (unsigned int j = 0; j < xNeighborhoodSize; ++j)
{
if (xNeighborhood[j]->getSetType() == Motion::SET_OPEN)
yNear.push_back(xNeighborhood[j]);
}
// Find the lowest cost-to-come connection from Open to x
base::Cost cMin(std::numeric_limits<double>::infinity());
Motion *yMin = getBestParent(x, yNear, cMin);
yNear.clear();
// If an optimal connection from Open to x was found
if (yMin != nullptr)
{
bool collision_free = false;
if (cacheCC_)
{
if (!yMin->alreadyCC(x))
{
collision_free = si_->checkMotion(yMin->getState(), x->getState());
++collisionChecks_;
// Due to FMT* design, it is only necessary to save unsuccesful
// connection attemps because of collision
if (!collision_free)
yMin->addCC(x);
}
}
else
{
++collisionChecks_;
collision_free = si_->checkMotion(yMin->getState(), x->getState());
}
if (collision_free)
{
// Add edge from yMin to x
x->setParent(yMin);
x->setCost(cMin);
x->setHeuristicCost(opt_->motionCostHeuristic(x->getState(), goalState_));
yMin->getChildren().push_back(x);
// Add x to Open
Open_new.push_back(x);
// Remove x from Unvisited
x->setSetType(Motion::SET_CLOSED);
}
} // An optimal connection from Open to x was found
} // For each node near z and in set Unvisited, try to connect it to set Open
// Update Open
Open_.pop();
(*z)->setSetType(Motion::SET_CLOSED);
// Add the nodes in Open_new to Open
unsigned int openNewSize = Open_new.size();
for (unsigned int i = 0; i < openNewSize; ++i)
{
Open_.insert(Open_new[i]);
Open_new[i]->setSetType(Motion::SET_OPEN);
}
Open_new.clear();
if (Open_.empty())
{
if(!extendedFMT_)
OMPL_INFORM("Open is empty before path was found --> no feasible path exists");
return false;
}
// Take the top of Open as the new z
*z = Open_.top()->data;
return true;
}
ompl::base::PlannerStatus ompl::geometric::FMT::solve(const base::PlannerTerminationCondition &ptc)
{
if (lastGoalMotion_) {
OMPL_INFORM("solve() called before clear(); returning previous solution");
traceSolutionPathThroughTree(lastGoalMotion_);
OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
return base::PlannerStatus(true, false);
}
else if (Open_.size() > 0)
{
OMPL_INFORM("solve() called before clear(); no previous solution so starting afresh");
clear();
}
checkValidity();
base::GoalSampleableRegion *goal = dynamic_cast<base::GoalSampleableRegion*>(pdef_->getGoal().get());
Motion *initMotion = nullptr;
if (!goal)
{
OMPL_ERROR("%s: Unknown type of goal", getName().c_str());
return base::PlannerStatus::UNRECOGNIZED_GOAL_TYPE;
}
// Add start states to V (nn_) and Open
while (const base::State *st = pis_.nextStart())
{
initMotion = new Motion(si_);
si_->copyState(initMotion->getState(), st);
Open_.insert(initMotion);
initMotion->setSetType(Motion::SET_OPEN);
initMotion->setCost(opt_->initialCost(initMotion->getState()));
nn_->add(initMotion); // V <-- {x_init}
}
if (!initMotion)
{
OMPL_ERROR("Start state undefined");
return base::PlannerStatus::INVALID_START;
}
// Sample N free states in the configuration space
if (!sampler_)
sampler_ = si_->allocStateSampler();
sampleFree(ptc);
assureGoalIsSampled(goal);
OMPL_INFORM("%s: Starting planning with %u states already in datastructure", getName().c_str(), nn_->size());
// Calculate the nearest neighbor search radius
/// \todo Create a PRM-like connection strategy
if (nearestK_)
{
NNk_ = std::ceil(std::pow(2.0 * radiusMultiplier_, (double)si_->getStateDimension()) *
(boost::math::constants::e<double>() / (double)si_->getStateDimension()) *
log((double)nn_->size()));
OMPL_DEBUG("Using nearest-neighbors k of %d", NNk_);
}
else
{
NNr_ = calculateRadius(si_->getStateDimension(), nn_->size());
OMPL_DEBUG("Using radius of %f", NNr_);
}
// Execute the planner, and return early if the planner returns a failure
bool plannerSuccess = false;
bool successfulExpansion = false;
Motion *z = initMotion; // z <-- xinit
saveNeighborhood(z);
while (!ptc)
{
if ((plannerSuccess = goal->isSatisfied(z->getState())))
break;
successfulExpansion = expandTreeFromNode(&z);
if (!extendedFMT_ && !successfulExpansion)
break;
else if (extendedFMT_ && !successfulExpansion)
{
//Apply RRT*-like connections: sample and connect samples to tree
std::vector<Motion*> nbh;
std::vector<base::Cost> costs;
std::vector<base::Cost> incCosts;
std::vector<std::size_t> sortedCostIndices;
// our functor for sorting nearest neighbors
CostIndexCompare compareFn(costs, *opt_);
Motion *m = new Motion(si_);
while (!ptc && Open_.empty())
{
sampler_->sampleUniform(m->getState());
if (!si_->isValid(m->getState()))
continue;
if (nearestK_)
nn_->nearestK(m, NNk_, nbh);
else
nn_->nearestR(m, NNr_, nbh);
// Get neighbours in the tree.
std::vector<Motion*> yNear;
yNear.reserve(nbh.size());
for (std::size_t j = 0; j < nbh.size(); ++j)
{
if (nbh[j]->getSetType() == Motion::SET_CLOSED)
{
if (nearestK_)
{
// Only include neighbors that are mutually k-nearest
// Relies on NN datastructure returning k-nearest in sorted order
const base::Cost connCost = opt_->motionCost(nbh[j]->getState(), m->getState());
const base::Cost worstCost = opt_->motionCost(neighborhoods_[nbh[j]].back()->getState(), nbh[j]->getState());
if (opt_->isCostBetterThan(worstCost, connCost))
continue;
else
yNear.push_back(nbh[j]);
}
else
yNear.push_back(nbh[j]);
}
}
// Sample again if the new sample does not connect to the tree.
if (yNear.empty())
continue;
// cache for distance computations
//
// Our cost caches only increase in size, so they're only
// resized if they can't fit the current neighborhood
if (costs.size() < yNear.size())
{
costs.resize(yNear.size());
incCosts.resize(yNear.size());
sortedCostIndices.resize(yNear.size());
}
// Finding the nearest neighbor to connect to
// By default, neighborhood states are sorted by cost, and collision checking
// is performed in increasing order of cost
//
// calculate all costs and distances
for (std::size_t i = 0 ; i < yNear.size(); ++i)
{
incCosts[i] = opt_->motionCost(yNear[i]->getState(), m->getState());
costs[i] = opt_->combineCosts(yNear[i]->getCost(), incCosts[i]);
}
// sort the nodes
//
// we're using index-value pairs so that we can get at
// original, unsorted indices
for (std::size_t i = 0; i < yNear.size(); ++i)
sortedCostIndices[i] = i;
std::sort(sortedCostIndices.begin(), sortedCostIndices.begin() + yNear.size(),
compareFn);
// collision check until a valid motion is found
for (std::vector<std::size_t>::const_iterator i = sortedCostIndices.begin();
i != sortedCostIndices.begin() + yNear.size();
++i)
{
if (si_->checkMotion(yNear[*i]->getState(), m->getState()))
{
m->setParent(yNear[*i]);
yNear[*i]->getChildren().push_back(m);
const base::Cost incCost = opt_->motionCost(yNear[*i]->getState(), m->getState());
m->setCost(opt_->combineCosts(yNear[*i]->getCost(), incCost));
m->setHeuristicCost(opt_->motionCostHeuristic(m->getState(), goalState_));
m->setSetType(Motion::SET_OPEN);
nn_->add(m);
saveNeighborhood(m);
updateNeighborhood(m,nbh);
Open_.insert(m);
z = m;
break;
}
}
} // while (!ptc && Open_.empty())
} // else if (extendedFMT_ && !successfulExpansion)
} // While not at goal
if (plannerSuccess)
{
// Return the path to z, since by definition of planner success, z is in the goal region
lastGoalMotion_ = z;
traceSolutionPathThroughTree(lastGoalMotion_);
OMPL_DEBUG("Final path cost: %f", lastGoalMotion_->getCost().value());
return base::PlannerStatus(true, false);
} // if plannerSuccess
else
{
// Planner terminated without accomplishing goal
return base::PlannerStatus(false, false);
}
}
