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);
}
}
void loadAffyTranscriptome(struct track *tg)
/* Convert sample info in window to linked feature. */
{
struct sqlConnection *conn = hAllocConn(database);
struct sqlResult *sr;
char **row;
int rowOffset;
struct sample *sample;
struct linkedFeatures *lfList = NULL, *lf;
char *hasDense = NULL;
char *where = NULL;
char query[256];
char tableName[256];
int zoom1 = 23924, zoom2 = 2991; /* bp per data point */
float pixPerBase = 0;
/* Set it up so we don't have linear interpretation.. */
char *noLinearInterpString = wiggleEnumToString(wiggleNoInterpolation);
cartSetString(cart, "affyTranscriptome.linear.interp", noLinearInterpString);
if(tl.picWidth == 0)
errAbort("hgTracks.c::loadAffyTranscriptome() - can't have pixel width of 0");
pixPerBase = (winEnd - winStart)/ tl.picWidth;
/* Determine zoom level. */
if(pixPerBase >= zoom1)
safef(tableName, sizeof(tableName), "%s_%s", "zoom1", tg->table);
else if(pixPerBase >= zoom2)
safef(tableName, sizeof(tableName), "%s_%s", "zoom2", tg->table);
else
safef(tableName, sizeof(tableName), "%s", tg->table);
/*see if we have a summary table*/
if(hTableExists(database, tableName))
safef(query, sizeof(query), "select name from %s where name = '%s' limit 1", tableName, tg->shortLabel);
else
{
warn("<p>Couldn't find table %s<br><br>", tableName);
safef(query, sizeof(query), "select name from %s where name = '%s' limit 1", tg->table, tg->shortLabel);
safef(tableName, sizeof(tableName), "%s", tg->table);
}
hasDense = sqlQuickQuery(conn, query, query, sizeof(query));
/* If we're in dense mode and have a summary table load it. */
if(tg->visibility == tvDense)
{
if(hasDense != NULL)
{
safef(query, sizeof(query), " name = '%s' ", tg->shortLabel);
where = cloneString(query);
}
}
sr = hRangeQuery(conn, tableName, chromName, winStart, winEnd, where, &rowOffset);
while ((row = sqlNextRow(sr)) != NULL)
{
sample = sampleLoad(row+rowOffset);
lf = lfFromSample(sample);
slAddHead(&lfList, lf);
sampleFree(&sample);
}
if(where != NULL)
freez(&where);
sqlFreeResult(&sr);
hFreeConn(&conn);
slReverse(&lfList);
/* sort to bring items with common names to the same line
but only for tracks with a summary table (with name=shortLabel) in
dense mode*/
if( hasDense != NULL )
{
sortGroupList = tg; /* used to put track name at top of sorted list. */
slSort(&lfList, lfNamePositionCmp);
sortGroupList = NULL;
}
tg->items = lfList;
}
void loadHumMusL(struct track *tg)
/* Load humMusL track with 2 zoom levels and one normal level.
* Also used for loading the musHumL track (called Human Cons)
* on the mouse browser. It decides which of 4 tables to
* load based on how large of a window the user is looking at*/
{
struct sqlConnection *conn = hAllocConn(database);
struct sqlResult *sr;
char **row;
int rowOffset;
struct sample *sample;
struct linkedFeatures *lfList = NULL, *lf;
char *hasDense = NULL;
char *where = NULL;
char tableName[256];
int z;
float pixPerBase = 0;
if(tl.picWidth == 0)
errAbort("hgTracks.c::loadHumMusL() - can't have pixel width of 0");
pixPerBase = (winEnd - winStart)/ tl.picWidth;
/* Determine zoom level. */
if (!strstr(tg->table,"HMRConservation"))
z = humMusZoomLevel();
else z=0;
if(z == 1 )
safef(tableName, sizeof(tableName), "%s_%s", "zoom1",
tg->table);
else if( z == 2)
safef(tableName, sizeof(tableName), "%s_%s", "zoom50",
tg->table);
else if(z == 3)
safef(tableName, sizeof(tableName), "%s_%s",
"zoom2500", tg->table);
else
safef(tableName, sizeof(tableName), "%s", tg->table);
//printf("(%s)", tableName );
sr = hRangeQuery(conn, tableName, chromName, winStart, winEnd,
where, &rowOffset);
while ((row = sqlNextRow(sr)) != NULL)
{
sample = sampleLoad(row+rowOffset);
lf = lfFromSample(sample);
slAddHead(&lfList, lf);
sampleFree(&sample);
}
if(where != NULL)
freez(&where);
sqlFreeResult(&sr);
hFreeConn(&conn);
slReverse(&lfList);
/* sort to bring items with common names to the same line
but only for tracks with a summary table (with name=shortLabel)
in
dense mode*/
if( hasDense != NULL )
{
sortGroupList = tg; /* used to put track name at
* top of sorted list. */
slSort(&lfList, lfNamePositionCmp);
sortGroupList = NULL;
}
tg->items = lfList;
}
void loadSampleZoo(struct track *tg)
/* Convert sample info in window to linked feature. */
{
int maxWiggleTrackHeight = 2500;
struct sqlConnection *conn = hAllocConn(database);
struct sqlResult *sr;
char **row;
int rowOffset;
struct sample *sample;
struct linkedFeatures *lfList = NULL, *lf;
char *hasDense = NULL;
char *where = NULL;
char query[256];
char option[64];
zooSpeciesHashInit();
/*see if we have a summary table*/
safef(query, sizeof(query), "select name from %s where name = '%s' limit 1", tg->table, tg->shortLabel);
//errAbort( "%s", query );
hasDense = sqlQuickQuery(conn, query, query, sizeof(query));
/* If we're in dense mode and have a summary table load it. */
if(tg->visibility == tvDense)
{
if(hasDense != NULL)
{
safef(query, sizeof(query), " name = '%s' ", tg->shortLabel);
where = cloneString(query);
}
}
sr = hRangeQuery(conn, tg->table, chromName, winStart, winEnd, where, &rowOffset);
while ((row = sqlNextRow(sr)) != NULL)
{
sample = sampleLoad(row + rowOffset);
lf = lfFromSample(sample);
safef( option, sizeof(option), "zooSpecies.%s", sample->name );
if( cartUsualBoolean(cart, option, TRUE ))
slAddHead(&lfList, lf);
sampleFree(&sample);
}
if(where != NULL)
freez(&where);
sqlFreeResult(&sr);
hFreeConn(&conn);
slReverse(&lfList);
/* sort to bring items with common names to the same line
* but only for tracks with a summary table
* (with name=shortLabel) in dense mode */
if( hasDense != NULL )
{
sortGroupList = tg; /* used to put track name at top of sorted list. */
slSort(&lfList, lfNamePositionCmp);
sortGroupList = NULL;
}
/* Sort in species phylogenetic order */
slSort(&lfList, lfZooCmp);
tg->items = lfList;
/*turn off full mode if there are too many rows or each row is too
* large. A total of maxWiggleTrackHeight is allowed for number of
* rows times the rowHeight*/
if( tg->visibility == tvFull && sampleTotalHeight( tg, tvFull ) > maxWiggleTrackHeight )
{
tg->limitedVisSet = TRUE;
tg->limitedVis = tvDense;
}
}
void RRT::plan(const Eigen::VectorXd &start, const KDL::Frame &x_goal, double goal_tolerance, std::list<KDL::Frame > &path_x, std::list<Eigen::VectorXd > &path_q, MarkerPublisher &markers_pub) {
V_.clear();
E_.clear();
path_x.clear();
path_q.clear();
int q_new_idx = 0;
RRTState state_start;
kin_model_->calculateFk(state_start.T_B_E_, effector_name_, start);
state_start.q_ = start;
V_[0] = state_start;
bool goal_found = false;
int m_id = 0;
for (int step = 0; step < 1200; step++) {
bool sample_goal = randomUniform(0,1) < 0.05;
KDL::Frame x_rand;
if (sample_goal) {
x_rand = x_goal;
}
else {
if (!sampleFree(x_rand)) {
std::cout << "ERROR: RRT::plan: could not sample free space" << std::endl;
return;
}
}
// get the closest pose
int x_nearest_idx = nearest(x_rand);
RRTState state_nearest( V_.find(x_nearest_idx)->second );
const KDL::Frame &x_nearest = state_nearest.T_B_E_;
KDL::Frame x_new;
// get the new pose
steer(x_nearest, x_rand, 2.0, 170.0/180.0*3.1415, x_new);
// std::cout << x_nearest.p[0] << " " << x_nearest.p[1] << " " << x_nearest.p[2] << std::endl;
// std::cout << x_new.p[0] << " " << x_new.p[1] << " " << x_new.p[2] << std::endl;
Eigen::VectorXd q_new(ndof_);
bool added_new = false;
KDL::Frame T_B_E;
if (collisionFree(state_nearest.q_, x_nearest, x_new, 0, q_new, T_B_E)) {
added_new = true;
RRTState state_new;
state_new.T_B_E_ = T_B_E;//x_new;
state_new.q_ = q_new;
q_new_idx++;
V_[q_new_idx] = state_new;
E_[q_new_idx] = x_nearest_idx;
m_id = markers_pub.addVectorMarker(q_new_idx, x_nearest.p, T_B_E.p, 0, 0.7, 0, 0.5, 0.01, "base");
KDL::Twist goal_diff( KDL::diff(T_B_E, x_goal, 1.0) );
if (goal_diff.vel.Norm() < 0.03 && goal_diff.rot.Norm() < 10.0/180.0*3.1415) {
goal_found = true;
std::cout << "goal found" << std::endl;
}
}
else {
KDL::Twist goal_diff( KDL::diff(x_new, x_goal, 1.0) );
if (goal_diff.vel.Norm() < 0.03 && goal_diff.rot.Norm() < 10.0/180.0*3.1415) {
std::list<int> nodes_to_remove_list;
getPath(x_nearest_idx, nodes_to_remove_list);
if (nodes_to_remove_list.size() > 1) {
nodes_to_remove_list.pop_front();
std::set<int> nodes_to_remove;
int removed_nodes_count = 0;
for (std::list<int>::const_iterator it = nodes_to_remove_list.begin(); it != nodes_to_remove_list.end(); it++) {
nodes_to_remove.insert( (*it) );
}
while (nodes_to_remove.size() > 0) {
std::set<int> orphan_nodes;
for (std::map<int, int >::const_iterator e_it = E_.begin(); e_it != E_.end(); e_it++) {
if (nodes_to_remove.find(e_it->second) != nodes_to_remove.end()) {
orphan_nodes.insert(e_it->first);
}
}
for (std::set<int>::const_iterator it = nodes_to_remove.begin(); it != nodes_to_remove.end(); it++) {
V_.erase( (*it) );
E_.erase( (*it) );
markers_pub.addEraseMarkers( (*it), (*it)+1);
removed_nodes_count++;
}
nodes_to_remove = orphan_nodes;
}
std::cout << "removed nodes: " << removed_nodes_count << std::endl;
}
}
if (sample_goal) {
// remove the nearest node to the goal
// V_.erase(x_nearest_idx);
// E_.erase(x_nearest_idx);
// markers_pub.addEraseMarkers(x_nearest_idx, x_nearest_idx+1);
}
}
if (added_new) {
std::cout << "step " << step << " nearest_idx " << x_nearest_idx << std::endl;
}
markers_pub.publish();
ros::spinOnce();
// getchar();
if (goal_found) {
break;
}
}
/*
double min_cost = 0.0;
int min_goal_idx = -1;
for (std::map<int, Eigen::VectorXd >::const_iterator v_it = V_.begin(); v_it != V_.end(); v_it++) {
double dist = (v_it->second - x_goal).norm();
double c = cost(v_it->first);
if (dist < goal_tolerance && (min_goal_idx < 0 || min_cost > c)) {
min_cost = c;
min_goal_idx = v_it->first;
}
}
if (min_goal_idx == -1) {
// path not found
return;
}
*/
if (goal_found) {
std::list<int > idx_path;
getPath(q_new_idx, idx_path);
for (std::list<int >::const_iterator p_it = idx_path.begin(); p_it != idx_path.end(); p_it++) {
const RRTState ¤t = V_.find(*p_it)->second;
path_q.push_back(current.q_);
path_x.push_back(current.T_B_E_);
}
}
/*
int q_vec_idx = V_[q_new_idx].q_vec_.size()-1;
std::list<int > idx_path;
getPath(q_new_idx, idx_path);
for (std::list<int >::const_reverse_iterator p_it = idx_path.rbegin(); p_it != idx_path.rend(); p_it++) {
// std::list<int >::const_reverse_iterator p_it2 = p_it;
// p_it2++;
const RRTState ¤t = V_.find(*p_it)->second;
// if (p_it2 == idx_path.rend()) {
// path_q.push_back(current.q_vec_[q_vec_idx]->second);
// }
// else {
path_q.push_back(current.q_vec_[q_vec_idx].second);
q_vec_idx = current.q_vec_[q_vec_idx].first;
// }
}
std::reverse(path_q.begin(), path_q.end());
*/
/*
for (std::list<int >::const_iterator p_it = idx_path.begin(); p_it != idx_path.end(); p_it++) {
const RRTState ¤t = V_.find(*p_it)->second;
if (p_it == idx_path.begin()) {
path_q.push_back(current.q_vec_[0]);
}
else {
std::list<int >::const_iterator p_it_prev = p_it;
p_it_prev--;
const RRTState &prev = V_.find(*p_it_prev)->second;
path_q.push_back(current.q_vec_[0]);
}
}
*/
}
