static void unbreak_backslash_broken_lines(struct token_list *tl, tok_message_queue *mq) {
const char *s = tl->orig, *e = s+tl->orig_size;
darray_char *txt = talloc_darray(tl);
darray(const char*) *olines = talloc_darray(tl);
darray(const char*) *tlines = talloc_darray(tl);
do {
const char *line_start = s, *line_end;
const char *lnw; //last non-white
size_t start_offset = txt->size;
//scan to the next line and find the last non-white character in the line
while (s<e && !creturn(*s)) s++;
line_end = s;
lnw = s;
while (lnw>line_start && cspace(lnw[-1])) lnw--;
if (s<e && creturn(*s)) {
s++;
//check for non-standard newlines (i.e. "\r", "\r\n", or "\n\r")
if (s<e && *s=='\n'+'\r'-s[-1])
s++;
}
//add the backslash-break-free version of the text
if (lnw>line_start && lnw[-1]=='\\' && line_end<e) {
darray_append_items(*txt, line_start, lnw-1-line_start);
if (lnw<e && cspace(*lnw)) {
tok_msg_warn(spaces_after_backslash_break, lnw,
"Trailing spaces after backslash-broken line");
}
} else
darray_append_items(*txt, line_start, s-line_start);
//add the line starts for this line
darray_append(*olines, line_start);
darray_append(*tlines, (const char*)start_offset);
//Since the txt buffer moves when expanded, we're storing offsets
// for now. Once we're done building txt, we can add the base
// of it to all the offsets to make them pointers.
} while (s<e);
//stick a null terminator at the end of the text
darray_realloc(*txt, txt->size+1);
txt->item[txt->size] = 0;
//convert the line start offsets to pointers
{
const char **i;
darray_foreach(i, *tlines)
*i = txt->item + (size_t)(*i);
}
tl->olines = olines->item;
tl->olines_size = olines->size;
tl->txt = txt->item;
tl->txt_size = txt->size;
tl->tlines = tlines->item;
tl->tlines_size = tlines->size;
}
void Compute_dQdBeta_CTSE(Real* dQdB, SolutionSpace<Real>& space, Int beta)
{
RCmplx perturb(0.0, 1.0e-11);
Mesh<Real>* rm = space.m;
SolutionSpace<RCmplx> cspace(space);
Mesh<RCmplx>* cm = cspace.m;
PObj<RCmplx>* cp = cspace.p;
EqnSet<RCmplx>* ceqnset = cspace.eqnset;
std::vector<SolutionSpaceBase<RCmplx>*> cSolSpaces;
cSolSpaces.push_back(&cspace);
SolutionOrdering<RCmplx> operations;
std::string name = cspace.name;
operations.Insert("Iterate " + name);
operations.Finalize(cSolSpaces);
Int cnnode = cm->GetNumNodes();
Real* dxdb = new Real[cnnode*3];
Get_dXdB(space.param->path+space.param->spacename, dxdb, rm, beta);
//perturb complex part by dxdb*perturb
for(Int i = 0; i < cnnode; i++){
for(Int j = 0; j < 3; j++){
cm->xyz[i*3 + j] += dxdb[i*3 + j]*perturb;
}
}
//update xyz coords
cp->UpdateXYZ(cm->xyz);
//calculate metrics with new grid positions
cm->CalcMetrics();
//create a temporary operations vector with just a single space iterator on it
//run solver
Solve(cSolSpaces, operations);
for(Int i = 0; i < cnnode; i++){
for(Int j = 0; j < ceqnset->neqn; j++){
dQdB[i*ceqnset->neqn + j] =
imag(cspace.q[i*(ceqnset->neqn+ceqnset->nauxvars) + j]) / imag(perturb);
}
}
delete [] dxdb;
return;
}
void dump_raw_meta(const Grid & grid,
ssize_t x0, ssize_t y0, ssize_t x1, ssize_t y1,
FILE * stream)
{
fprintf(stream, "# x: %zd...%zd\n# y: %zd...%zd\n", x0, x1, y0, y1);
shared_ptr<GridCSpace const> cspace(grid.GetCSpace());
for (ssize_t x(x0); x <= x1; x++) {
for (ssize_t y(y0); y <= y1; y++) {
shared_ptr<GridNode const> node(grid.GetNode(x, y));
if (node)
fprintf(stream, "%zd %zd %f\n", x, y,
cspace->GetMeta(node->vertex));
}
fprintf(stream, "\n");
}
}
Real Compute_dObjdX_dXdBeta(SolutionSpace<Real>& space, Int beta)
{
RCmplx perturb (0.0, 1.0e-11);
Real dObjdBeta;
RCmplx Obj;
Mesh<Real>* m = space.m;
EqnSet<Real>* eqnset = space.eqnset;
Param<Real>* param = space.param;
BoundaryConditions<Real>* bc = space.bc;
SolutionSpace<RCmplx> cspace(space);
Mesh<RCmplx>* cm = cspace.m;
PObj<RCmplx>* cp = cspace.p;
EqnSet<RCmplx>* ceqnset = cspace.eqnset;
Int nnode = m->GetNumNodes();
Int cnnode = cm->GetNumNodes();
Real* dxdb = new Real[nnode*3];
Get_dXdB(space.param->path+space.param->spacename, dxdb, m, beta);
//perturb complex part by dxdb*perturb
for(Int i = 0; i < cnnode; i++){
for(Int j = 0; j < 3; j++){
cm->xyz[i*3 + j] += dxdb[i*3 + j]*perturb;
}
}
//update xyz coords
cp->UpdateXYZ(cm->xyz);
//calculate metrics with new grid positions
cm->CalcMetrics();
ComputeSurfaceAreas(&cspace, 0);
Obj = Compute_Obj_Function(cspace);
dObjdBeta = imag(Obj) / imag(perturb);
delete [] dxdb;
return dObjdBeta;
}
Real Compute_dObjdQ_dQdBeta(Real* dQdB, SolutionSpace<Real>& space)
{
RCmplx perturb (0.0, 1.0e-11);
RCmplx Obj;
EqnSet<Real>* eqnset = space.eqnset;
Mesh<Real>* m = space.m;
Param<Real>* param = space.param;
BoundaryConditions<Real>* bc = space.bc;
SolutionSpace<RCmplx> cspace(space);
Mesh<RCmplx>* cm = cspace.m;
PObj<RCmplx>* cp = cspace.p;
EqnSet<RCmplx>* ceqnset = cspace.eqnset;
Int cnnode = cm->GetNumNodes();
Real dObjdBeta;
Int i, j;
Int neqn = eqnset->neqn;
Int nauxvars = eqnset->nauxvars;
//replace all Q values in mesh by dQdB*perturb and compute cost function
for(i = 0; i < cnnode; i++){
for(j = 0; j < neqn; j++){
cspace.q[i*(neqn+nauxvars) + j] += dQdB[i*neqn + j]*perturb;
}
ceqnset->ComputeAuxiliaryVariables(&cspace.q[i*(neqn+nauxvars)]);
}
cp->UpdateGeneralVectors(cspace.q, neqn+nauxvars);
UpdateBCs(&cspace);
cp->UpdateGeneralVectors(cspace.q, neqn+nauxvars);
ComputeSurfaceAreas(&cspace, 0);
Obj = Compute_Obj_Function(cspace);
dObjdBeta = imag(Obj)/imag(perturb);
return dObjdBeta;
}
void Compute_dRdBeta_CTSE(Real* dRdB, SolutionSpace<Real>& space, Int beta)
{
RCmplx perturb (0.0, 1.0e-11);
Mesh<Real>* rm = space.m;
Int nnode = rm->GetNumNodes();
Int gnode = rm->GetNumParallelNodes();
Int nbnode = rm->GetNumBoundaryNodes();
std::ifstream fin;
SolutionSpace<RCmplx> cspace(space);
Mesh<RCmplx>* cm = cspace.m;
PObj<RCmplx>* cp = cspace.p;
EqnSet<RCmplx>* ceqnset = cspace.eqnset;
Param<RCmplx>* param = cspace.param;
Int cnnode = cm->GetNumNodes();
Int cgnode = cm->GetNumParallelNodes();
Int cnbnode = cm->GetNumBoundaryNodes();
RCmplx* q = cspace.q;
Int neqn = ceqnset->neqn;
Int nvars = neqn + ceqnset->nauxvars;
Real* dxdb = new Real[nnode*3];
Get_dXdB(space.param->path + space.param->spacename, dxdb, rm, beta);
//perturb complex part by dxdb*perturb
for(Int i = 0; i < nnode; i++){
for(Int j = 0; j < 3; j++){
cm->xyz[i*3 + j] += dxdb[i*3 + j]*perturb;
}
}
//update xyz coords
cp->UpdateXYZ(cm->xyz);
//calculate metrics with new grid positions
cm->CalcMetrics();
//compute gradient coefficients once
ComputeNodeLSQCoefficients(&cspace);
//perturb any parameters which we are getting sensitivities for
Perturb_Param(beta, *cspace.param);
//reset any interesting field which might depend on perturbations
cspace.RefreshForParam();
//the gaussian source is sometimes used to modify boundary velocities, etc.
//pre-compute it for bc call
if(cspace.param->gaussianSource){
cspace.gaussian->ApplyToResidual();
}
//update boundary conditions
cspace.p->UpdateGeneralVectors(q, nvars);
UpdateBCs(&cspace);
cspace.p->UpdateGeneralVectors(q, nvars);
//now, compute gradients and limiters
if(param->sorder > 1 || param->viscous){
for(Int i = 0; i < (cnnode+cnbnode+cgnode); i++){
ceqnset->NativeToExtrapolated(&q[i*nvars]);
}
cspace.grad->Compute();
cspace.limiter->Compute(&cspace);
for(Int i = 0; i < (cnnode+cnbnode+cgnode); i++){
ceqnset->ExtrapolatedToNative(&q[i*nvars]);
}
}
//compute spatial residual
SpatialResidual(&cspace);
ExtraSourceResidual(&cspace);
for(Int i = 0; i < nnode; i++){
for(Int j = 0; j < ceqnset->neqn; j++){
dRdB[i*ceqnset->neqn + j] = imag(cspace.crs->b[i*ceqnset->neqn + j])/imag(perturb);
}
}
delete [] dxdb;
}
void ComputedQdBeta_HardsetBC(Int nnodes, Int* nodes, Real* dQdB, const SolutionSpace<Real>& space, Int beta)
{
Real h = 1.0e-11;
RCmplx I(0.0, 1.0);
RCmplx perturb = h*I;
Mesh<Real>* rm = space.m;
SolutionSpace<RCmplx> cspace(space);
Mesh<RCmplx>* cm = cspace.m;
PObj<RCmplx>* cp = cspace.p;
EqnSet<RCmplx>* ceqnset = cspace.eqnset;
Param<RCmplx>* param = cspace.param;
RCmplx* q = cspace.q;
Int nnode = rm->GetNumNodes();
Int neqn = ceqnset->neqn;
Int nvars = neqn + ceqnset->nauxvars;
Real* dxdb = new Real[nnode*3];
Get_dXdB(space.param->path + space.param->spacename, dxdb, rm, beta);
//perturb complex part by dxdb*perturb
for(Int i = 0; i < nnode; i++){
for(Int j = 0; j < 3; j++){
cm->xyz[i*3 + j] += dxdb[i*3 + j]*perturb;
}
}
//update xyz coords
cp->UpdateXYZ(cm->xyz);
//calculate metrics with new grid positions
cm->CalcMetrics();
//compute gradient coefficients once
ComputeNodeLSQCoefficients(&cspace);
//perturb any parameters which we are getting sensitivities for
Perturb_Param(beta, *cspace.param);
//reset any interesting field which might depend on perturbations
cspace.RefreshForParam();
//the gaussian source is sometimes used to modify boundary velocities, etc.
//pre-compute it for bc call
if(cspace.param->gaussianSource){
cspace.gaussian->ApplyToResidual();
}
//update boundary conditions
cspace.p->UpdateGeneralVectors(q, nvars);
UpdateBCs(&cspace);
cspace.p->UpdateGeneralVectors(q, nvars);
for(Int i = 0; i < nnodes; i++){
Int node = nodes[i];
for(Int j = 0; j < neqn; j++){
dQdB[i*neqn + j] = imag(q[node*nvars + j])/h;
std::cout << "dqdb: " << i << ": -" << j << " " << dQdB[i*neqn + j] << std::endl;
}
}
}
void Compute_dQdBeta_II(Real* dQdB, SolutionSpace<Real>& space, Int beta)
{
Mesh<Real>* m = space.m;
EqnSet<Real>* eqnset = space.eqnset;
Param<Real>* param = space.param;
Int neqn = eqnset->neqn;
Int nnode = m->GetNumNodes();
std::cout << "\n\nCOMPUTING dQ/dBeta - Incremental Iterative - Beta: " << beta << "\n";
//allocate enough memory for dRdB
Real* dRdB = new Real[nnode*neqn];
Compute_dRdBeta_CTSE(dRdB, space, beta);
SolutionSpace<RCmplx> cspace(space);
//zero dQdB
MemSet(dQdB, 0.0, nnode*neqn);
//Get the direct computed dQdB on boundaries which are dirchlet(TODO) or viscous
Int* vnodeslist;
Int vnodes = GetNodesOnBCType(m, space.bc, &vnodeslist, NoSlip);
Real* vnodesdQdB = new Real[vnodes*neqn];
ComputedQdBeta_HardsetBC(vnodes, vnodeslist, vnodesdQdB, space, beta);
//now hardset for the hard bcs
for(Int i = 0; i < vnodes; i++){
Int node = vnodeslist[i];
for(Int j = 0; j < neqn; j++){
//only non-zero entries should be hardset, the rest should float above
if(Abs(vnodesdQdB[i*neqn + j]) >= 1.0e-16){
dQdB[node*neqn + j] = vnodesdQdB[i*neqn + j];
}
}
}
for(Int it = 0; it < space.temporalControl.nSteps; it++){
//build RHS of our problem
//
//compute the rhs contribution of dR/dQ*dQ/dB
Compute_dRdQ_Product_MatrixFree(cspace, dQdB, space.crs->b);
//flip the sign and add the dR/dB contribution
for(Int i = 0; i < nnode*neqn; i++){
space.crs->b[i] = -space.crs->b[i] - dRdB[i];
}
//this is a hook to modify any residuals directly due to boundary
//conditions, most notably hardset viscous BCs (zeros appropriate entries)
Kernel<Real> ResModifyBC(Bkernel_BC_Res_Modify);
BdriverNoScatter(&space, ResModifyBC, eqnset->neqn, NULL);
//compute the global residual for the linear system
Real residGlobal = ParallelL2Norm(space.p, space.crs->b, nnode*neqn);
space.residOutFile << it << ": ||II_res||: " << residGlobal;
//set initial guess to zero
space.crs->BlankX();
Real deltaDq = space.crs->SGS(param->designNsgs, NULL, NULL, NULL, true);
space.residOutFile << " ||II_sgs_res||: " << deltaDq << std::endl;
std::cout << it << ": ||II_res||: " << residGlobal << " ||II_dq||: " << deltaDq << std::endl;
//update our estimate of dQdB
for(Int i = 0; i < nnode*neqn; i++){
dQdB[i] -= space.crs->x[i];
}
//now hardset for the hard bcs
for(Int i = 0; i < vnodes; i++){
Int node = vnodeslist[i];
for(Int j = 0; j < neqn; j++){
//only non-zero entries should be hardset, the rest should float above
if(Abs(vnodesdQdB[i*neqn + j]) >= 1.0e-16){
dQdB[node*neqn + j] = vnodesdQdB[i*neqn + j];
}
}
}
}
delete [] dRdB;
delete [] vnodeslist;
delete [] vnodesdQdB;
}
Real Compute_dObjdBeta_CTSE(SolutionSpace<Real>& space, SolutionOrdering<Real>& operations, Int beta)
{
Int i;
Real h = 1.0e-11;
RCmplx I(0.0, 1.0);
RCmplx perturb = h*I;
Real dObjdBeta;
RCmplx Obj;
Mesh<Real>* rm = space.m;
Param<Real>* param = space.param;
BoundaryConditions<Real>* bc = space.bc;
PObj<Real>* p = space.p;
Int nnode = rm->GetNumNodes();
SolutionSpace<RCmplx> cspace(space);
Mesh<RCmplx>* cm = cspace.m;
PObj<RCmplx>* cp = cspace.p;
EqnSet<RCmplx>* ceqnset = cspace.eqnset;
Int cnnode = cm->GetNumNodes();
std::cout << "\n\nCOMPUTING dObj/dBeta_CTSE\n" << std::endl;
Real* dxSurf = new Real[nnode*3];
RCmplx* dxSurfC = new RCmplx[nnode*3];
Compute_dXdB_Surface(space.param->path + space.param->spacename, rm, bc, dxSurf, beta);
std::vector<SolutionSpaceBase<RCmplx>*> cSpaces;
cSpaces.push_back(&cspace);
for(i = 0; i < nnode*3; i++){
//perturb complex part by displacement
dxSurfC[i] = dxSurf[i]*perturb;
}
Int smoothingPasses = 1000;
MoveMeshLinearElastic(cm, bc, dxSurfC, smoothingPasses);
cm->CalcMetrics();
//compute gradient coefficients once
ComputeNodeLSQCoefficients(&cspace);
SolutionOrdering<RCmplx> cOperations;
cOperations = operations;
cOperations.Finalize(cSpaces);
//perturb parameters as required
Perturb_Param(beta, *cspace.param);
//reset any interesting field which might depend on perturbations
cspace.RefreshForParam();
//run solver
Solve(cSpaces, cOperations);
Obj = Compute_Obj_Function(cspace);
dObjdBeta = imag(Obj) / h;
std::cout << "\n\nComplex objective function value: " << Obj << std::endl;
delete [] dxSurf;
delete [] dxSurfC;
return dObjdBeta;
}
void
process(void)
{
struct s_command *cp;
SPACE tspace;
size_t oldpsl = 0;
char *p;
p = NULL;
for (linenum = 0; mf_fgets(&PS, REPLACE);) {
pd = 0;
top:
cp = prog;
redirect:
while (cp != NULL) {
if (!applies(cp)) {
cp = cp->next;
continue;
}
switch (cp->code) {
case '{':
cp = cp->u.c;
goto redirect;
case 'a':
if (appendx >= appendnum)
if ((appends = realloc(appends,
sizeof(struct s_appends) *
(appendnum *= 2))) == NULL)
err(1, "realloc");
appends[appendx].type = AP_STRING;
appends[appendx].s = cp->t;
appends[appendx].len = strlen(cp->t);
appendx++;
break;
case 'b':
cp = cp->u.c;
goto redirect;
case 'c':
pd = 1;
psl = 0;
if (cp->a2 == NULL || lastaddr || lastline())
(void)fprintf(outfile, "%s", cp->t);
break;
case 'd':
pd = 1;
goto new;
case 'D':
if (pd)
goto new;
if (psl == 0 ||
(p = memchr(ps, '\n', psl)) == NULL) {
pd = 1;
goto new;
} else {
psl -= (p + 1) - ps;
memmove(ps, p + 1, psl);
goto top;
}
case 'g':
cspace(&PS, hs, hsl, REPLACE);
break;
case 'G':
cspace(&PS, "\n", 1, APPEND);
cspace(&PS, hs, hsl, APPEND);
break;
case 'h':
cspace(&HS, ps, psl, REPLACE);
break;
case 'H':
cspace(&HS, "\n", 1, APPEND);
cspace(&HS, ps, psl, APPEND);
break;
case 'i':
(void)fprintf(outfile, "%s", cp->t);
break;
case 'l':
lputs(ps, psl);
break;
case 'n':
if (!nflag && !pd)
OUT();
flush_appends();
if (!mf_fgets(&PS, REPLACE))
exit(0);
pd = 0;
break;
case 'N':
flush_appends();
cspace(&PS, "\n", 1, APPEND);
if (!mf_fgets(&PS, APPEND))
exit(0);
break;
case 'p':
if (pd)
break;
OUT();
break;
case 'P':
if (pd)
break;
if ((p = memchr(ps, '\n', psl)) != NULL) {
oldpsl = psl;
psl = p - ps;
}
OUT();
if (p != NULL)
psl = oldpsl;
break;
case 'q':
if (!nflag && !pd)
OUT();
flush_appends();
exit(0);
case 'r':
if (appendx >= appendnum)
if ((appends = realloc(appends,
sizeof(struct s_appends) *
(appendnum *= 2))) == NULL)
err(1, "realloc");
appends[appendx].type = AP_FILE;
appends[appendx].s = cp->t;
appends[appendx].len = strlen(cp->t);
appendx++;
break;
case 's':
sdone |= substitute(cp);
break;
case 't':
if (sdone) {
sdone = 0;
cp = cp->u.c;
goto redirect;
}
break;
case 'w':
if (pd)
break;
if (cp->u.fd == -1 && (cp->u.fd = open(cp->t,
O_WRONLY|O_APPEND|O_CREAT|O_TRUNC,
DEFFILEMODE)) == -1)
err(1, "%s", cp->t);
if (write(cp->u.fd, ps, psl) != (ssize_t)psl ||
write(cp->u.fd, "\n", 1) != 1)
err(1, "%s", cp->t);
break;
case 'x':
/*
* If the hold space is null, make it empty
* but not null. Otherwise the pattern space
* will become null after the swap, which is
* an abnormal condition.
*/
if (hs == NULL)
cspace(&HS, "", 0, REPLACE);
tspace = PS;
PS = HS;
HS = tspace;
break;
case 'y':
if (pd || psl == 0)
break;
do_tr(cp->u.y);
break;
case ':':
case '}':
break;
case '=':
(void)fprintf(outfile, "%lu\n", linenum);
}
cp = cp->next;
} /* for all cp */
/** @brief Performs path planning in collision-free space for the
* given robot at the stance s between start configuration qstart and
* destination qgoal.
* Stability is tested at each configuration.
*
* The output is given in path. cond controls the number of iterations/time
* for planning.
*
* The constraint specifications are given in WorldPlannerSettings. If you
* have custom requirements, you will need to set them up.
*/
bool StancePlan(RobotWorld& world,int robot,Config& qstart,Config& qgoal,const Stance& s,MilestonePath& path,
const HaltingCondition& cond,const string& plannerSettings="")
{
WorldPlannerSettings settings;
settings.InitializeDefault(world);
//do more constraint setup here (modifying the settings object) if desired,
//e.g., set collision margins, edge collision checking resolution, etc.
StanceCSpace cspace(world,robot,&settings);
cspace.SetStance(s);
cspace.CalculateSP();
if(gSupportPolygonMargin != 0)
cspace.SetSPMargin(gSupportPolygonMargin);
//sanity checking
if(!cspace.CheckContact(qstart)) {
printf("Start configuration does not meet contact constraint, error %g\n",cspace.ContactDistance(qstart));
if(!cspace.SolveContact()) {
printf(" Solving for contact failed.\n");
return false;
}
else {
printf(" IK problem was solved, using new start configuration\n");
qstart = cspace.GetRobot()->q;
}
}
if(!cspace.CheckContact(qgoal)) {
printf("Goal configuration does not meet contact constraint, error %g\n",cspace.ContactDistance(qgoal));
if(!cspace.SolveContact()) {
printf(" Solving for contact failed.\n");
return false;
}
else {
printf(" IK problem was solved, using new goal configuration\n");
qgoal = cspace.GetRobot()->q;
}
}
if(!cspace.IsFeasible(qstart)) {
cout<<"Start configuration is infeasible, violated constraints:"<<endl;
vector<bool> infeasible;
cspace.CheckObstacles(qstart,infeasible);
for(size_t i=0;i<infeasible.size();i++)
if(infeasible[i]) cout<<" "<<cspace.ObstacleName(i)<<endl;
return false;
}
if(!cspace.IsFeasible(qgoal)) {
cout<<"Goal configuration is infeasible, violated constraints:"<<endl;
vector<bool> infeasible;
cspace.CheckObstacles(qgoal,infeasible);
for(size_t i=0;i<infeasible.size();i++)
if(infeasible[i]) cout<<" "<<cspace.ObstacleName(i)<<endl;
return false;
}
MotionPlannerFactory factory;
if(!plannerSettings.empty())
factory.LoadJSON(plannerSettings);
//do more planner setup here if desired, e.g., change planner type,
//perturbation size, connection radius, etc
//make the planner and do the planning
MotionPlannerInterface* planner = factory.Create(&cspace,qstart,qgoal);
string res = planner->Plan(path,cond);
cout<<"Planner terminated with condition "<<res<<endl;
printf(" Stats: SolveContact %d, %gs. IsFeasible %d, %gs\n",cspace.numSolveContact,cspace.solveContactTime,cspace.numIsFeasible,cspace.isFeasibleTime);
delete planner;
return !path.edges.empty();
}
void planWithSimpleSetupPen(std::string title = "Default")
{
// x, theta, velocity, angular veloctiy
ompl::base::StateSpacePtr space;
ompl::base::StateSpacePtr pos(new ompl::base::RealVectorStateSpace(1));
ompl::base::StateSpacePtr theta(new ompl::base::SO2StateSpace());
ompl::base::StateSpacePtr vel(new ompl::base::RealVectorStateSpace(1));
ompl::base::StateSpacePtr omega(new ompl::base::SO2StateSpace());
ompl::base::RealVectorBounds position_limit(1);
position_limit.setLow(-xaxis_limit);
position_limit.setHigh(xaxis_limit);
pos->as<ompl::base::RealVectorStateSpace>()->setBounds(position_limit);
ompl::base::RealVectorBounds vel_limit(1);
vel_limit.setLow(-std::numeric_limits<double>::infinity());
vel_limit.setHigh(std::numeric_limits<double>::infinity());
vel->as<ompl::base::RealVectorStateSpace>()->setBounds(vel_limit);
space = pos + theta + vel + omega;
// create a control space
oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 1));
// set the bounds for the control space
//ob::RealVectorBounds cbounds(1);
//cbounds.setLow(0);
//cbounds.setHigh(acceleration_limit);
//cspace->as<RealVectorControlSpace>()->setBounds(cbounds);
// define a simple setup class
oc::SimpleSetup ss(cspace);
// set state validity checking for this space
ss.setStateValidityChecker(isStateValidPen);
// Use the ODESolver to propagate the system. Call KinematicCarPostIntegration
// when integration has finished to normalize the orientation values.
oc::ODESolverPtr odeSolver(new oc::ODEBasicSolver<> (ss.getSpaceInformation(), &PendulumODE));
ss.setStatePropagator(oc::ODESolver::getStatePropagator(odeSolver, PendulumPostIntegration));
/// create a start state
ob::ScopedState<> start(space);
start[0] = 0.0;
start[1] = -0.35;
start[2] = 0.0;
start[3] = 0.0;
/// create a goal state; use the hard way to set the elements
ob::ScopedState<> goal(space);
goal[1] = 0.0;
goal[3] = 0.0;
/// set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05);
ss.getSpaceInformation()->setMinMaxControlDuration(1, 10);
ss.getSpaceInformation()->setPropagationStepSize(0.01);
ompl::base::PlannerPtr planner(new ompl::control::PID(ss.getSpaceInformation()));
ss.setPlanner(planner);
ss.setup();
/// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = ss.solve(10);
/*
if (solved)
{
ompl::control::PathControl& path = ss.getSolutionPath();
path.printAsMatrix(std::cout);
// print path to file
std::ofstream fout("path.txt");
path.printAsMatrix(fout);
fout.close();
}
*/
}
void planWithSimpleSetup(double start_x, double start_y, double start_theta,
double goal_x, double goal_y, double goal_theta)
{
/// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE2StateSpace());
/// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(-1);
bounds.setHigh(1);
space->as<ob::SE2StateSpace>()->setBounds(bounds);
// create a control space
oc::ControlSpacePtr cspace(new DemoControlSpace(space));
// set the bounds for the control space
ob::RealVectorBounds cbounds(2);
cbounds.setLow(-0.3);
cbounds.setHigh(0.3);
cspace->as<DemoControlSpace>()->setBounds(cbounds);
// define a simple setup class
oc::SimpleSetup ss(cspace);
// set state validity checking for this space
ss.setStateValidityChecker(boost::bind(&isStateValid, ss.getSpaceInformation().get(), _1));
// Setting the propagation routine for this space:
// KinematicCarModel does NOT use ODESolver
//ss.setStatePropagator(oc::StatePropagatorPtr(new KinematicCarModel(ss.getSpaceInformation())));
// Use the ODESolver to propagate the system. Call KinematicCarPostIntegration
// when integration has finished to normalize the orientation values.
// oc::ODEBasicSolver<> odeSolver(ss.getSpaceInformation(), &KinematicCarODE);
// ss.setStatePropagator(odeSolver.getStatePropagator(&KinematicCarPostIntegration));
oc::ODESolverPtr odeSolver(new oc::ODEBasicSolver<> (ss.getSpaceInformation(), &KinematicCarODE));
ss.setStatePropagator(oc::ODESolver::getStatePropagator(odeSolver/*, &KinematicCarPostIntegration*/));
/// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(start_x);
start->setY(start_y);
start->setYaw(start_theta);
/// create a goal state; use the hard way to set the elements
ob::ScopedState<ob::SE2StateSpace> goal(space);
goal->setX(goal_x);
goal->setY(goal_y);
goal->setYaw(goal_theta);
/// set the start and goal states
ss.setStartAndGoalStates(start, goal, 0.05);
/// we want to have a reasonable value for the propagation step size
ss.setup();
/// attempt to solve the problem within one second of planning time
ob::PlannerStatus solved = ss.solve(10.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
/// print the path to screen
ss.getSolutionPath().asGeometric().print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
}
bool GenerateAndTimeOptimizeMultiPath(Robot& robot,MultiPath& multipath,Real xtol,Real dt)
{
Timer timer;
vector<GeneralizedCubicBezierSpline > paths;
if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol))
return false;
printf("Generated interpolating path in time %gs\n",timer.ElapsedTime());
RobotCSpace cspace(robot);
RobotGeodesicManifold manifold(robot);
for(size_t i=0;i<multipath.sections.size();i++) {
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].space = &cspace;
paths[i].segments[j].manifold = &manifold;
}
for(int iters=0;iters<gNumTimescaleBisectIters;iters++)
paths[i].Bisect();
}
#if SAVE_INTERPOLATING_CURVES
int index=0;
printf("Saving sections, element %d to section_x_bezier.csv\n",index);
for(size_t i=0;i<paths.size();i++) {
{
stringstream ss;
ss<<"section_"<<i<<"_bezier.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"duration,x0,x1,x2,x3"<<endl;
for(size_t j=0;j<paths[i].segments.size();j++) {
out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_vel.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"v(0),v(0.5),v(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Deriv(0,temp);
temp /= paths[i].durations[j];
out<<temp[index];
paths[i].segments[j].Deriv(0.5,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
paths[i].segments[j].Deriv(1,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
out<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_acc.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"a(0),a(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Accel(0,temp);
temp /= Sqr(paths[i].durations[j]);
out<<temp[index];
paths[i].segments[j].Accel(1,temp);
temp /= Sqr(paths[i].durations[j]);
out<<","<<temp[index];
out<<endl;
}
out.close();
}
}
#endif //SAVE_INTERPOLATING_CURVES
#if SAVE_LORES_INTERPOLATING_CURVES
paths.clear();
if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*2.0))
return false;
for(size_t i=0;i<multipath.sections.size();i++) {
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].space = &cspace;
paths[i].segments[j].manifold = &manifold;
}
}
for(size_t i=0;i<paths.size();i++) {
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_x2.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"duration,x0,x1,x2,x3"<<endl;
for(size_t j=0;j<paths[i].segments.size();j++) {
out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_vel_x2.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"v(0),v(0.5),v(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Deriv(0,temp);
temp /= paths[i].durations[j];
out<<temp[index];
paths[i].segments[j].Deriv(0.5,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
paths[i].segments[j].Deriv(1,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
out<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_acc_x2.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"a(0),a(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Accel(0,temp);
temp /= Sqr(paths[i].durations[j]);
out<<temp[index];
paths[i].segments[j].Accel(1,temp);
temp /= Sqr(paths[i].durations[j]);
out<<","<<temp[index];
out<<endl;
}
out.close();
}
}
paths.clear();
if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*4.0))
return false;
for(size_t i=0;i<multipath.sections.size();i++) {
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].space = &cspace;
paths[i].segments[j].manifold = &manifold;
}
}
for(size_t i=0;i<paths.size();i++) {
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_x4.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"duration,x0,x1,x2,x3"<<endl;
for(size_t j=0;j<paths[i].segments.size();j++) {
out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_vel_x4.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"v(0),v(0.5),v(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Deriv(0,temp);
temp /= paths[i].durations[j];
out<<temp[index];
paths[i].segments[j].Deriv(0.5,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
paths[i].segments[j].Deriv(1,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
out<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_acc_x4.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"a(0),a(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Accel(0,temp);
temp /= Sqr(paths[i].durations[j]);
out<<temp[index];
paths[i].segments[j].Accel(1,temp);
temp /= Sqr(paths[i].durations[j]);
out<<","<<temp[index];
out<<endl;
}
out.close();
}
}
paths.clear();
if(!InterpolateConstrainedMultiPath(robot,multipath,paths,xtol*8.0))
return false;
for(size_t i=0;i<multipath.sections.size();i++) {
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].space = &cspace;
paths[i].segments[j].manifold = &manifold;
}
}
for(size_t i=0;i<paths.size();i++) {
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_x8.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"duration,x0,x1,x2,x3"<<endl;
for(size_t j=0;j<paths[i].segments.size();j++) {
out<<paths[i].durations[j]<<","<<paths[i].segments[j].x0[index]<<","<<paths[i].segments[j].x1[index]<<","<<paths[i].segments[j].x2[index]<<","<<paths[i].segments[j].x3[index]<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_vel_x8.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"v(0),v(0.5),v(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Deriv(0,temp);
temp /= paths[i].durations[j];
out<<temp[index];
paths[i].segments[j].Deriv(0.5,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
paths[i].segments[j].Deriv(1,temp);
temp /= paths[i].durations[j];
out<<","<<temp[index];
out<<endl;
}
out.close();
}
{
stringstream ss;
ss<<"section_"<<i<<"_bezier_acc_x8.csv";
ofstream out(ss.str().c_str(),ios::out);
out<<"a(0),a(1)"<<endl;
Vector temp;
for(size_t j=0;j<paths[i].segments.size();j++) {
paths[i].segments[j].Accel(0,temp);
temp /= Sqr(paths[i].durations[j]);
out<<temp[index];
paths[i].segments[j].Accel(1,temp);
temp /= Sqr(paths[i].durations[j]);
out<<","<<temp[index];
out<<endl;
}
out.close();
}
}
#endif //SAVE_LORES_INTERPOLATING_CURVES
//concatenate sections into a single curve
TimeScaledBezierCurve traj;
vector<int> edgeToSection,sectionEdges(1,0);
for(size_t i=0;i<multipath.sections.size();i++) {
traj.path.Concat(paths[i]);
for(size_t j=0;j<paths[i].segments.size();j++)
edgeToSection.push_back((int)i);
sectionEdges.push_back(sectionEdges.back()+(int)paths[i].segments.size());
}
timer.Reset();
bool res=traj.OptimizeTimeScaling(robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
if(!res) {
printf("Failed to optimize time scaling\n");
return false;
}
else {
printf("Optimized into a path with duration %g, (took %gs)\n",traj.EndTime(),timer.ElapsedTime());
}
double T = traj.EndTime();
int numdivs = (int)Ceil(T/dt);
printf("Discretizing at time resolution %g\n",T/numdivs);
numdivs++;
Vector x,v;
int sCur = -1;
for(int i=0;i<numdivs;i++) {
Real t=T*Real(i)/Real(numdivs-1);
int trajEdge = traj.timeScaling.TimeToSegment(t);
if(trajEdge == (int)edgeToSection.size()) trajEdge--; //end of path
Assert(trajEdge < (int)edgeToSection.size());
int s=edgeToSection[trajEdge];
if(s < sCur) {
fprintf(stderr,"Strange: edge index is going backward? %d -> %d\n",sCur,s);
fprintf(stderr," time %g, division %d, traj segment %d\n",t,i,trajEdge);
}
Assert(s - sCur >=0);
while(sCur < s) {
//close off the current section and add a new one
Real switchTime=traj.timeScaling.times[sectionEdges[sCur+1]];
Assert(switchTime <= t);
traj.Eval(switchTime,x);
traj.Deriv(switchTime,v);
if(sCur >= 0) {
multipath.sections[sCur].times.push_back(switchTime);
multipath.sections[sCur].milestones.push_back(x);
multipath.sections[sCur].velocities.push_back(v);
}
multipath.sections[sCur+1].milestones.resize(0);
multipath.sections[sCur+1].velocities.resize(0);
multipath.sections[sCur+1].times.resize(0);
multipath.sections[sCur+1].milestones.push_back(x);
multipath.sections[sCur+1].velocities.push_back(v);
multipath.sections[sCur+1].times.push_back(switchTime);
sCur++;
}
if(t == multipath.sections[s].times.back()) continue;
traj.Eval(t,x);
traj.Deriv(t,v);
multipath.sections[s].times.push_back(t);
multipath.sections[s].milestones.push_back(x);
multipath.sections[s].velocities.push_back(v);
}
#if DO_TEST_TRIANGULAR
timer.Reset();
TimeScaledBezierCurve trajTri;
Real Ttrap = 0;
printf("%d paths?\n",paths.size());
for(size_t i=0;i<paths.size();i++)
Ttrap += OptimalTriangularTimeScaling(paths[i],robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax,trajTri);
printf("Optimal trapezoidal time scaling duration %g, calculated in %gs\n",Ttrap,timer.ElapsedTime());
printf("Saving plot to opt_tri_multipath.csv\n");
trajTri.Plot("opt_tri_multipath.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
#endif // DO_TEST_TRAPEZOIDAL
#if DO_CHECK_BOUNDS
CheckBounds(robot,traj,dt);
#endif // DO_CHECK_BOUNDS
#if DO_SAVE_PLOT
printf("Saving plot to opt_multipath.csv\n");
traj.Plot("opt_multipath.csv",robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
#endif //DO_SAVE_PLOT
#if DO_SAVE_LIMITS
SaveLimits(robot,traj,dt,"opt_multipath_limits.csv");
#endif // DO_SAVE_LIMITS
{
multipath.settings.set("resolution",xtol);
multipath.settings.set("program","GenerateAndTimeOptimizeMultiPath");
}
return true;
}
/*
* Like fgets, but go through the list of files chaining them together.
* Set len to the length of the line.
*/
int
mf_fgets(SPACE *sp, enum e_spflag spflag)
{
struct stat sb;
size_t len;
char *p;
int c;
static int firstfile;
if (infile == NULL) {
/* stdin? */
if (files->fname == NULL) {
if (inplace != NULL)
fprintf(stderr, "sed: -i may not be used with stdin\n"); //errx(1, "-i may not be used with stdin");
infile = stdin;
fname = "stdin";
outfile = stdout;
outfname = "stdout";
}
firstfile = 1;
}
for (;;) {
if (infile != NULL && (c = getc(infile)) != EOF) {
(void)ungetc(c, infile);
break;
}
/* If we are here then either eof or no files are open yet */
if (infile == stdin) {
sp->len = 0;
return (0);
}
if (infile != NULL) {
if (infile != stdin) fclose(infile);
if (*oldfname != '\0') {
if (rename(fname, oldfname) != 0) {
fprintf(stderr, "sed: rename(): %s\n", strerror(errno)); // warn("rename()");
unlink(tmpfname);
pthread_exit(NULL);
// exit(1);
}
*oldfname = '\0';
}
if (*tmpfname != '\0') {
if (outfile != NULL && outfile != stdout)
fclose(outfile);
outfile = NULL;
rename(tmpfname, fname);
*tmpfname = '\0';
}
outfname = NULL;
}
if (firstfile == 0)
files = files->next;
else
firstfile = 0;
if (files == NULL) {
sp->len = 0;
return (0);
}
fname = files->fname;
if (inplace != NULL) {
if (lstat(fname, &sb) != 0)
fprintf(stderr, "sed: %s: %s\n", fname, strerror(errno)); // err(1, "%s", fname);
if (!(sb.st_mode & S_IFREG))
fprintf(stderr, "sed: %s: %s %s\n", fname,
"in-place editing only",
"works for regular files");
//errx(1, "%s: %s %s", fname,
// "in-place editing only",
// "works for regular files");
if (*inplace != '\0') {
strlcpy(oldfname, fname,
sizeof(oldfname));
len = strlcat(oldfname, inplace,
sizeof(oldfname));
if (len > sizeof(oldfname))
fprintf(stderr, "sed: %s: name too long\n", fname);
// errx(1, "%s: name too long", fname);
}
len = snprintf(tmpfname, sizeof(tmpfname),
"%s/.!%ld!%s", dirname(fname), (long)getpid(),
basename(fname));
if (len >= sizeof(tmpfname))
fprintf(stderr, "sed: %s: name too long\n", fname);
// errx(1, "%s: name too long", fname);
unlink(tmpfname);
if ((outfile = fopen(tmpfname, "w")) == NULL)
fprintf(stderr, "sed: %s: %s\n", fname, strerror(errno)); // err(1, "%s", fname);
fchown(fileno(outfile), sb.st_uid, sb.st_gid);
fchmod(fileno(outfile), sb.st_mode & ALLPERMS);
outfname = tmpfname;
} else {
outfile = stdout;
outfname = "stdout";
}
if ((infile = fopen(fname, "r")) == NULL) {
fprintf(stderr, "sed: %s: %s\n", fname, strerror(errno)); // warn("%s", fname);
rval = 1;
continue;
}
}
/*
* We are here only when infile is open and we still have something
* to read from it.
*
* Use fgetln so that we can handle essentially infinite input data.
* Can't use the pointer into the stdio buffer as the process space
* because the ungetc() can cause it to move.
*/
p = fgetln(infile, &len);
if (ferror(infile))
fprintf(stderr, "sed: %s: %s\n", fname, strerror(errno ? errno : EIO));
// errx(1, "%s: %s", fname, strerror(errno ? errno : EIO));
if (len != 0 && p[len - 1] == '\n')
len--;
cspace(sp, p, len, spflag);
linenum++;
return (1);
}
bool InterpolateConstrainedMultiPath(Robot& robot,const MultiPath& path,vector<GeneralizedCubicBezierSpline>& paths,Real xtol)
{
//sanity check -- make sure it's a continuous path
if(!path.IsContinuous()) {
fprintf(stderr,"InterpolateConstrainedMultiPath: path is discontinuous\n");
return false;
}
if(path.sections.empty()) {
fprintf(stderr,"InterpolateConstrainedMultiPath: path is empty\n");
return false;
}
if(path.settings.contains("resolution")) {
//see if the resolution is high enough to just interpolate directly
Real res=path.settings.as<Real>("resolution");
if(res <= xtol) {
printf("Direct interpolating trajectory with res %g\n",res);
//just interpolate directly
RobotCSpace space(robot);
RobotGeodesicManifold manifold(robot);
paths.resize(path.sections.size());
for(size_t i=0;i<path.sections.size();i++) {
if(path.sections[i].times.empty()) {
SPLINE_INTERPOLATE_FUNC(path.sections[i].milestones,paths[i].segments,
&space,&manifold);
//uniform timing
paths[i].durations.resize(paths[i].segments.size());
Real dt=1.0/Real(paths[i].segments.size());
for(size_t j=0;j<paths[i].segments.size();j++)
paths[i].durations[j] = dt;
}
else {
SPLINE_INTERPOLATE_FUNC(path.sections[i].milestones,path.sections[i].times,paths[i].segments,
&space,&manifold);
//get timing from path
paths[i].durations.resize(paths[i].segments.size());
for(size_t j=0;j<paths[i].segments.size();j++)
paths[i].durations[j] = path.sections[i].times[j+1]-path.sections[i].times[j];
}
}
return true;
}
}
printf("Discretizing constrained trajectory at res %g\n",xtol);
RobotCSpace cspace(robot);
RobotGeodesicManifold manifold(robot);
//create transition constraints and derivatives
vector<vector<IKGoal> > stanceConstraints(path.sections.size());
vector<vector<IKGoal> > transitionConstraints(path.sections.size()-1);
vector<Config> transitionDerivs(path.sections.size()-1);
for(size_t i=0;i<path.sections.size();i++)
path.GetIKProblem(stanceConstraints[i],i);
for(size_t i=0;i+1<path.sections.size();i++) {
//put all nonredundant constraints into transitionConstraints[i]
transitionConstraints[i]=stanceConstraints[i];
for(size_t j=0;j<stanceConstraints[i+1].size();j++) {
bool res=AddGoalNonredundant(stanceConstraints[i+1][j],transitionConstraints[i]);
if(!res) {
fprintf(stderr,"Conflict between goal %d of stance %d and stance %d\n",j,i+1,i);
fprintf(stderr," Link %d\n",stanceConstraints[i+1][j].link);
return false;
}
}
const Config& prev=path.sections[i].milestones[path.sections[i].milestones.size()-2];
const Config& next=path.sections[i+1].milestones[1];
manifold.InterpolateDeriv(prev,next,0.5,transitionDerivs[i]);
transitionDerivs[i] *= 0.5;
//check for overshoots a la MonotonicInterpolate
Vector inslope,outslope;
manifold.InterpolateDeriv(prev,path.sections[i].milestones.back(),1.0,inslope);
manifold.InterpolateDeriv(path.sections[i].milestones.back(),next,0.0,outslope);
for(int j=0;j<transitionDerivs[i].n;j++) {
if(Sign(transitionDerivs[i][j]) != Sign(inslope[j]) || Sign(transitionDerivs[i][j]) != Sign(outslope[j])) transitionDerivs[i][j] = 0;
else {
if(transitionDerivs[i][j] > 0) {
if(transitionDerivs[i][j] > 3.0*outslope[j])
transitionDerivs[i][j] = 3.0*outslope[j];
if(transitionDerivs[i][j] > 3.0*inslope[j])
transitionDerivs[i][j] = 3.0*inslope[j];
}
else {
if(transitionDerivs[i][j] < 3.0*outslope[j])
transitionDerivs[i][j] = 3.0*outslope[j];
if(transitionDerivs[i][j] < 3.0*inslope[j])
transitionDerivs[i][j] = 3.0*inslope[j];
}
}
}
//project "natural" derivative onto transition manifold
RobotIKFunction f(robot);
f.UseIK(transitionConstraints[i]);
GetDefaultIKDofs(robot,transitionConstraints[i],f.activeDofs);
Vector temp(f.activeDofs.Size()),dtemp(f.activeDofs.Size()),dtemp2;
f.activeDofs.InvMap(path.sections[i].milestones.back(),temp);
f.activeDofs.InvMap(transitionDerivs[i],dtemp);
Matrix J;
f.PreEval(temp);
f.Jacobian(temp,J);
RobustSVD<Real> svd;
if(!svd.set(J)) {
fprintf(stderr,"Unable to set SVD of transition constraints %d\n",i);
return false;
}
svd.nullspaceComponent(dtemp,dtemp2);
dtemp -= dtemp2;
f.activeDofs.Map(dtemp,transitionDerivs[i]);
}
//start constructing path
paths.resize(path.sections.size());
for(size_t i=0;i<path.sections.size();i++) {
paths[i].segments.resize(0);
paths[i].durations.resize(0);
Vector dxprev,dxnext;
if(i>0)
dxprev.setRef(transitionDerivs[i-1]);
if(i<transitionDerivs.size())
dxnext.setRef(transitionDerivs[i]);
if(stanceConstraints[i].empty()) {
SPLINE_INTERPOLATE_FUNC(path.sections[i].milestones,paths[i].segments,&cspace,&manifold);
DiscretizeSpline(paths[i],xtol);
//Note: discretizeSpline will fill in the spline durations
}
else {
RobotSmoothConstrainedInterpolator interp(robot,stanceConstraints[i]);
interp.xtol = xtol;
if(!MultiSmoothInterpolate(interp,path.sections[i].milestones,dxprev,dxnext,paths[i])) {
/** TEMP - test no inter-section smoothing**/
//if(!MultiSmoothInterpolate(interp,path.sections[i].milestones,paths[i])) {
fprintf(stderr,"Unable to interpolate section %d\n",i);
return false;
}
}
//set the time scale if the input path is timed
if(!path.sections[i].times.empty()) {
//printf("Time scaling section %d to duration %g\n",i,path.sections[i].times.back()-path.sections[i].times.front());
paths[i].TimeScale(path.sections[i].times.back()-path.sections[i].times.front());
}
}
return true;
}
bool TimeOptimizePath(Robot& robot,const vector<Real>& oldtimes,const vector<Config>& oldconfigs,Real dt,vector<Real>& newtimes,vector<Config>& newconfigs)
{
//make a smooth interpolator
Vector dx0,dx1,temp;
RobotCSpace cspace(robot);
RobotGeodesicManifold manifold(robot);
TimeScaledBezierCurve traj;
traj.path.durations.resize(oldconfigs.size()-1);
for(size_t i=0;i+1<oldconfigs.size();i++) {
traj.path.durations[i] = oldtimes[i+1]-oldtimes[i];
if(!(traj.path.durations[i] > 0)) {
fprintf(stderr,"TimeOptimizePath: input path does not have monotonically increasing time: segment %d range [%g,%g]\n",i,oldtimes[i],oldtimes[i+1]);
return false;
}
Assert(traj.path.durations[i] > 0);
}
traj.path.segments.resize(oldconfigs.size()-1);
for(size_t i=0;i+1<oldconfigs.size();i++) {
traj.path.segments[i].space = &cspace;
traj.path.segments[i].manifold = &manifold;
traj.path.segments[i].x0 = oldconfigs[i];
traj.path.segments[i].x3 = oldconfigs[i+1];
if(i > 0) {
InterpolateDerivative(robot,oldconfigs[i],oldconfigs[i+1],dx0);
InterpolateDerivative(robot,oldconfigs[i],oldconfigs[i-1],temp);
dx0 -= temp*(traj.path.durations[i]/traj.path.durations[i-1]);
dx0 *= 0.5;
}
else dx0.resize(0);
if(i+2 < oldconfigs.size()) {
InterpolateDerivative(robot,oldconfigs[i+1],oldconfigs[i+2],dx1);
InterpolateDerivative(robot,oldconfigs[i+1],oldconfigs[i],temp);
dx1 *= traj.path.durations[i]/traj.path.durations[i+1];
dx1 -= temp;
dx1 *= 0.5;
}
else dx1.resize(0);
traj.path.segments[i].SetNaturalTangents(dx0,dx1);
}
Timer timer;
bool res=traj.OptimizeTimeScaling(robot.velMin,robot.velMax,-1.0*robot.accMax,robot.accMax);
if(!res) {
printf("Failed to optimize time scaling\n");
return false;
}
else {
printf("Optimized into a path with duration %g, (took %gs)\n",traj.EndTime(),timer.ElapsedTime());
}
double T = traj.EndTime();
int numdivs = (int)Ceil(T/dt);
printf("Discretizing at time resolution %g\n",T/numdivs);
numdivs++;
newtimes.resize(numdivs);
newconfigs.resize(numdivs);
for(int i=0;i<numdivs;i++) {
newtimes[i] = T*Real(i)/Real(numdivs-1);
traj.Eval(newtimes[i],newconfigs[i]);
}
#if DO_CHECK_BOUNDS
CheckBounds(robot,traj,newtimes);
#endif //DO_CHECKBOUNDS
return true;
}
KPIECE(const Workspace &workspace, const Agent &agent, const InstanceFileMap &args) :
workspace(workspace), agent(agent), goalEdge(NULL), samplesGenerated(0), edgesAdded(0), edgesRejected(0) {
collisionCheckDT = args.doubleVal("Collision Check Delta t");
dfpair(stdout, "collision check dt", "%g", collisionCheckDT);
const typename Workspace::WorkspaceBounds &agentStateVarRanges = agent.getStateVarRanges(workspace.getBounds());
stateSpaceDim = agentStateVarRanges.size();
StateSpace *baseSpace = new StateSpace(stateSpaceDim);
ompl::base::RealVectorBounds bounds(stateSpaceDim);
for(unsigned int i = 0; i < stateSpaceDim; ++i) {
bounds.setLow(i, agentStateVarRanges[i].first);
bounds.setHigh(i, agentStateVarRanges[i].second);
}
baseSpace->setBounds(bounds);
ompl::base::StateSpacePtr space = ompl::base::StateSpacePtr(baseSpace);
const std::vector< std::pair<double, double> > &controlBounds = agent.getControlBounds();
controlSpaceDim = controlBounds.size();
ompl::control::RealVectorControlSpace *baseCSpace = new ompl::control::RealVectorControlSpace(space, controlSpaceDim);
ompl::base::RealVectorBounds cbounds(controlSpaceDim);
for(unsigned int i = 0; i < controlSpaceDim; ++i) {
cbounds.setLow(i, controlBounds[i].first);
cbounds.setHigh(i, controlBounds[i].second);
}
baseCSpace->setBounds(cbounds);
ompl::control::ControlSpacePtr cspace(baseCSpace);
// construct an instance of space information from this control space
spaceInfoPtr = ompl::control::SpaceInformationPtr(new ompl::control::SpaceInformation(space, cspace));
// set state validity checking for this space
spaceInfoPtr->setStateValidityChecker(boost::bind(&KPIECE::isStateValid, this, spaceInfoPtr.get(), _1));
// set the state propagation routine
spaceInfoPtr->setStatePropagator(boost::bind(&KPIECE::propagate, this, _1, _2, _3, _4));
pdef = ompl::base::ProblemDefinitionPtr(new ompl::base::ProblemDefinition(spaceInfoPtr));
spaceInfoPtr->setPropagationStepSize(args.doubleVal("Steering Delta t"));
dfpair(stdout, "steering dt", "%g", args.doubleVal("Steering Delta t"));
spaceInfoPtr->setMinMaxControlDuration(stol(args.value("KPIECE Min Control Steps")),stol(args.value("KPIECE Max Control Steps")));
dfpair(stdout, "min control duration", "%u", stol(args.value("KPIECE Min Control Steps")));
dfpair(stdout, "max control duration", "%u", stol(args.value("KPIECE Max Control Steps")));
spaceInfoPtr->setup();
kpiece = new ompl::control::KPIECE1(spaceInfoPtr);
kpiece->setGoalBias(args.doubleVal("KPIECE Goal Bias"));
dfpair(stdout, "goal bias", "%g", args.doubleVal("KPIECE Goal Bias"));
kpiece->setBorderFraction(args.doubleVal("KPIECE Border Fraction"));
dfpair(stdout, "border fraction", "%g", args.doubleVal("KPIECE Border Fraction"));
kpiece->setCellScoreFactor(args.doubleVal("KPIECE Cell Score Good"), args.doubleVal("KPIECE Cell Score Bad"));
dfpair(stdout, "cell score good", "%g", args.doubleVal("KPIECE Cell Score Good"));
dfpair(stdout, "cell score bad", "%g", args.doubleVal("KPIECE Cell Score Bad"));
kpiece->setMaxCloseSamplesCount(stol(args.value("KPIECE Max Close Samples")));
dfpair(stdout, "max closed samples", "%u", stol(args.value("KPIECE Max Close Samples")));
ompl::base::RealVectorRandomLinearProjectionEvaluator *projectionEvaluator = new ompl::base::RealVectorRandomLinearProjectionEvaluator(baseSpace, stateSpaceDim);
ompl::base::ProjectionEvaluatorPtr projectionPtr = ompl::base::ProjectionEvaluatorPtr(projectionEvaluator);
kpiece->setProjectionEvaluator(projectionPtr);
}
void
process(void)
{
struct s_command *cp;
SPACE tspace;
size_t len, oldpsl;
char *p;
oldpsl = 0;
for (linenum = 0; mf_fgets(&PS, REPLACE);) {
pd = 0;
top:
cp = prog;
redirect:
while (cp != NULL) {
if (!applies(cp)) {
cp = cp->next;
continue;
}
switch (cp->code) {
case '{':
cp = cp->u.c;
goto redirect;
case 'a':
if (appendx >= appendnum) {
appends = xrealloc(appends,
sizeof(struct s_appends) *
(appendnum * 2));
appendnum *= 2;
}
appends[appendx].type = AP_STRING;
appends[appendx].s = cp->t;
appends[appendx].len = strlen(cp->t);
appendx++;
break;
case 'b':
cp = cp->u.c;
goto redirect;
case 'c':
pd = 1;
psl = 0;
if (cp->a2 == NULL || lastaddr)
(void)printf("%s", cp->t);
break;
case 'd':
pd = 1;
goto new;
case 'D':
if (psl == 0)
pd = 1;
if (pd)
goto new;
if ((p = memchr(ps, '\n', psl - 1)) == NULL) {
pd = 1;
goto new;
} else {
psl -= (p + 1) - ps;
memmove(ps, p + 1, psl);
goto top;
}
case 'g':
cspace(&PS, hs, hsl, REPLACE);
break;
case 'G':
if (hs == NULL)
cspace(&HS, "\n", 1, REPLACE);
cspace(&PS, hs, hsl, 0);
break;
case 'h':
cspace(&HS, ps, psl, REPLACE);
break;
case 'H':
cspace(&HS, ps, psl, 0);
break;
case 'i':
(void)printf("%s", cp->t);
break;
case 'l':
lputs(ps);
break;
case 'n':
if (!nflag && !pd)
OUT(ps)
flush_appends();
if (!mf_fgets(&PS, REPLACE))
exit(0);
pd = 0;
break;
case 'N':
flush_appends();
if (!mf_fgets(&PS, 0)) {
if (!nflag && !pd)
OUT(ps)
exit(0);
}
break;
case 'p':
if (pd)
break;
OUT(ps)
break;
case 'P':
if (pd)
break;
if ((p = memchr(ps, '\n', psl - 1)) != NULL) {
oldpsl = psl;
psl = (p + 1) - ps;
}
OUT(ps)
if (p != NULL)
psl = oldpsl;
break;
case 'q':
if (!nflag && !pd)
OUT(ps)
flush_appends();
exit(0);
case 'r':
if (appendx >= appendnum) {
appends = xrealloc(appends,
sizeof(struct s_appends) *
(appendnum * 2));
appendnum *= 2;
}
appends[appendx].type = AP_FILE;
appends[appendx].s = cp->t;
appends[appendx].len = strlen(cp->t);
appendx++;
break;
case 's':
sdone |= substitute(cp);
break;
case 't':
if (sdone) {
sdone = 0;
cp = cp->u.c;
goto redirect;
}
break;
case 'w':
if (pd)
break;
if (cp->u.fd == -1 && (cp->u.fd = open(cp->t,
O_WRONLY|O_APPEND|O_CREAT|O_TRUNC,
DEFFILEMODE)) == -1)
err(FATAL, "%s: %s",
cp->t, strerror(errno));
if ((size_t)write(cp->u.fd, ps, psl) != psl)
err(FATAL, "%s: %s",
cp->t, strerror(errno));
break;
case 'x':
if (hs == NULL)
cspace(&HS, "\n", 1, REPLACE);
tspace = PS;
PS = HS;
HS = tspace;
break;
case 'y':
if (pd)
break;
for (p = ps, len = psl; --len; ++p)
*p = cp->u.y[(int)*p];
break;
case ':':
case '}':
break;
case '=':
(void)printf("%lu\n", linenum);
}
cp = cp->next;
} /* for all cp */
/*
* Like fgets, but go through the list of files chaining them together.
* Set len to the length of the line.
*/
int
mf_fgets(SPACE *sp, enum e_spflag spflag)
{
struct stat sb;
ssize_t len;
char *dirbuf, *basebuf;
static char *p = NULL;
static size_t plen = 0;
int c;
static int firstfile;
if (infile == NULL) {
/* stdin? */
if (files->fname == NULL) {
if (inplace != NULL)
errx(1, "-I or -i may not be used with stdin");
infile = stdin;
fname = "stdin";
outfile = stdout;
outfname = "stdout";
}
firstfile = 1;
}
for (;;) {
if (infile != NULL && (c = getc(infile)) != EOF) {
(void)ungetc(c, infile);
break;
}
/* If we are here then either eof or no files are open yet */
if (infile == stdin) {
sp->len = 0;
return (0);
}
if (infile != NULL) {
fclose(infile);
if (*oldfname != '\0') {
/* if there was a backup file, remove it */
unlink(oldfname);
/*
* Backup the original. Note that hard links
* are not supported on all filesystems.
*/
if ((link(fname, oldfname) != 0) &&
(rename(fname, oldfname) != 0)) {
warn("rename()");
if (*tmpfname)
unlink(tmpfname);
exit(1);
}
*oldfname = '\0';
}
if (*tmpfname != '\0') {
if (outfile != NULL && outfile != stdout)
if (fclose(outfile) != 0) {
warn("fclose()");
unlink(tmpfname);
exit(1);
}
outfile = NULL;
if (rename(tmpfname, fname) != 0) {
/* this should not happen really! */
warn("rename()");
unlink(tmpfname);
exit(1);
}
*tmpfname = '\0';
}
outfname = NULL;
}
if (firstfile == 0)
files = files->next;
else
firstfile = 0;
if (files == NULL) {
sp->len = 0;
return (0);
}
fname = files->fname;
if (inplace != NULL) {
if (lstat(fname, &sb) != 0)
err(1, "%s", fname);
if (!(sb.st_mode & S_IFREG))
errx(1, "%s: %s %s", fname,
"in-place editing only",
"works for regular files");
if (*inplace != '\0') {
strlcpy(oldfname, fname,
sizeof(oldfname));
len = strlcat(oldfname, inplace,
sizeof(oldfname));
if (len > (ssize_t)sizeof(oldfname))
errx(1, "%s: name too long", fname);
}
if ((dirbuf = strdup(fname)) == NULL ||
(basebuf = strdup(fname)) == NULL)
err(1, "strdup");
len = snprintf(tmpfname, sizeof(tmpfname),
"%s/.!%ld!%s", dirname(dirbuf), (long)getpid(),
basename(basebuf));
free(dirbuf);
free(basebuf);
if (len >= (ssize_t)sizeof(tmpfname))
errx(1, "%s: name too long", fname);
unlink(tmpfname);
if (outfile != NULL && outfile != stdout)
fclose(outfile);
if ((outfile = fopen(tmpfname, "w")) == NULL)
err(1, "%s", fname);
fchown(fileno(outfile), sb.st_uid, sb.st_gid);
fchmod(fileno(outfile), sb.st_mode & ALLPERMS);
outfname = tmpfname;
if (!ispan) {
linenum = 0;
resetstate();
}
} else {
outfile = stdout;
outfname = "stdout";
}
if ((infile = fopen(fname, "r")) == NULL) {
warn("%s", fname);
rval = 1;
continue;
}
}
/*
* We are here only when infile is open and we still have something
* to read from it.
*
* Use getline() so that we can handle essentially infinite input
* data. The p and plen are static so each invocation gives
* getline() the same buffer which is expanded as needed.
*/
len = getline(&p, &plen, infile);
if (len == -1)
err(1, "%s", fname);
if (len != 0 && p[len - 1] == '\n') {
sp->append_newline = 1;
len--;
} else if (!lastline()) {
sp->append_newline = 1;
} else {
sp->append_newline = 0;
}
cspace(sp, p, len, spflag);
linenum++;
return (1);
}
void Compute_dObjdQ(Real* dIdQ, SolutionSpace<Real>& space)
{
RCmplx perturb (0.0, 1.0e-11);
Int i, j;
EqnSet<Real>* eqnset = space.eqnset;
Mesh<Real>* rm = space.m;
Param<Real>* param = space.param;
BoundaryConditions<Real>* bc = space.bc;
Int node;
Int nnode = rm->GetNumNodes();
Int gnode = rm->GetNumParallelNodes();
Int nbnode = rm->GetNumBoundaryNodes();
Int neqn = eqnset->neqn;
Int nvars = neqn + eqnset->nauxvars;
MPI_Datatype mpit = MPI_GetType(nnode);
//get global number of nodes since processes need to be synced
//build lookup maps from local to global id
Int* globalToLocal;
Int* localToGlobal;
Int globalNodes = space.p->BuildGlobalMaps(&localToGlobal, &globalToLocal);
RCmplx Obj;
std::cout << "\n\nCOMPUTING dI/dQ\n\n" << std::endl;
SolutionSpace<RCmplx> cspace(space);
Mesh<RCmplx>* cm = cspace.m;
PObj<RCmplx>* cp = cspace.p;
EqnSet<RCmplx>* ceqnset = cspace.eqnset;
//init dIdQ
MemBlank(dIdQ, neqn*nnode);
//compute surface areas once to ensure they are valid
ComputeSurfaceAreas(&cspace, 0);
for(i = 0; i < globalNodes; i++){
node = globalToLocal[i];
for(j = 0; j < neqn; j++){
//perturb q value if node is local to process
if(node >= 0){
cspace.q[node*nvars + j] += perturb;
ceqnset->ComputeAuxiliaryVariables(&cspace.q[node*nvars + 0]);
}
//compute perturbed objective function
//no need for a call to compute_surface_areas here since we aren't changing them
cp->UpdateGeneralVectors(cspace.q, nvars);
Obj = Compute_Obj_Function(cspace);
if(node >= 0){
if(node < nnode){
dIdQ[node*neqn + j] = imag(Obj) / imag(perturb);
}
//reset q value
cspace.q[node*nvars + j] = space.q[node*nvars + j];
//compute aux vars one last time for all values reset to original
ceqnset->ComputeAuxiliaryVariables(&cspace.q[node*nvars + 0]);
}
}
}
delete [] localToGlobal;
delete [] globalToLocal;
return;
}
void plan(void)
{
// construct the state space we are planning in
ob::StateSpacePtr space(new ob::SE2StateSpace());
// set the bounds for the R^2 part of SE(2)
ob::RealVectorBounds bounds(2);
bounds.setLow(0);
bounds.setHigh(2);
space->as<ob::SE2StateSpace>()->setBounds(bounds);
// create triangulation that ignores obstacle and respects propositions
MyDecomposition* ptd = new MyDecomposition(bounds);
// helper method that adds an obstacle, as well as three propositions p0,p1,p2
addObstaclesAndPropositions(ptd);
ptd->setup();
oc::PropositionalDecompositionPtr pd(ptd);
// create a control space
oc::ControlSpacePtr cspace(new oc::RealVectorControlSpace(space, 2));
// set the bounds for the control space
ob::RealVectorBounds cbounds(2);
cbounds.setLow(-.5);
cbounds.setHigh(.5);
cspace->as<oc::RealVectorControlSpace>()->setBounds(cbounds);
oc::SpaceInformationPtr si(new oc::SpaceInformation(space, cspace));
si->setStateValidityChecker(std::bind(&isStateValid, si.get(), ptd, std::placeholders::_1));
si->setStatePropagator(std::bind(&propagate, std::placeholders::_1,
std::placeholders::_2, std::placeholders::_3, std::placeholders::_4));
si->setPropagationStepSize(0.025);
//LTL co-safety sequencing formula: visit p2,p0 in that order
std::vector<unsigned int> sequence(2);
sequence[0] = 2;
sequence[1] = 0;
oc::AutomatonPtr cosafety = oc::Automaton::SequenceAutomaton(3, sequence);
//LTL safety avoidance formula: never visit p1
std::vector<unsigned int> toAvoid(1);
toAvoid[0] = 1;
oc::AutomatonPtr safety = oc::Automaton::AvoidanceAutomaton(3, toAvoid);
//construct product graph (propDecomp x A_{cosafety} x A_{safety})
oc::ProductGraphPtr product(new oc::ProductGraph(pd, cosafety, safety));
// LTLSpaceInformation creates a hybrid space of robot state space x product graph.
// It takes the validity checker from SpaceInformation and expands it to one that also
// rejects any hybrid state containing rejecting automaton states.
// It takes the state propagator from SpaceInformation and expands it to one that
// follows continuous propagation with setting the next decomposition region
// and automaton states accordingly.
//
// The robot state space, given by SpaceInformation, is referred to as the "lower space".
oc::LTLSpaceInformationPtr ltlsi(new oc::LTLSpaceInformation(si, product));
// LTLProblemDefinition creates a goal in hybrid space, corresponding to any
// state in which both automata are accepting
oc::LTLProblemDefinitionPtr pdef(new oc::LTLProblemDefinition(ltlsi));
// create a start state
ob::ScopedState<ob::SE2StateSpace> start(space);
start->setX(0.2);
start->setY(0.2);
start->setYaw(0.0);
// addLowerStartState accepts a state in lower space, expands it to its
// corresponding hybrid state (decomposition region containing the state, and
// starting states in both automata), and adds that as an official start state.
pdef->addLowerStartState(start.get());
//LTL planner (input: LTL space information, product automaton)
oc::LTLPlanner* ltlPlanner = new oc::LTLPlanner(ltlsi, product);
ltlPlanner->setProblemDefinition(pdef);
// attempt to solve the problem within thirty seconds of planning time
// considering the above cosafety/safety automata, a solution path is any
// path that visits p2 followed by p0 while avoiding obstacles and avoiding p1.
ob::PlannerStatus solved = ltlPlanner->as<ob::Planner>()->solve(30.0);
if (solved)
{
std::cout << "Found solution:" << std::endl;
// The path returned by LTLProblemDefinition is through hybrid space.
// getLowerSolutionPath() projects it down into the original robot state space
// that we handed to LTLSpaceInformation.
pdef->getLowerSolutionPath()->print(std::cout);
}
else
std::cout << "No solution found" << std::endl;
delete ltlPlanner;
}
/*
* Like fgets, but go through the list of files chaining them together.
* Set len to the length of the line.
*/
int
mf_fgets(SPACE *sp, enum e_spflag spflag)
{
struct stat sb, nsb;
ssize_t len;
static char *p = NULL;
static size_t plen = 0;
int c;
static int firstfile;
if (infile == NULL) {
/* stdin? */
if (files->fname == NULL) {
if (inplace != NULL)
errx(1,
_("-I or -i may not be used with stdin"));
infile = stdin;
fname = "stdin";
outfile = stdout;
outfname = "stdout";
}
firstfile = 1;
}
for (;;) {
if (infile != NULL && (c = getc(infile)) != EOF) {
(void) ungetc(c, infile);
break;
}
/* If we are here then either eof or no files are open yet */
if (infile == stdin) {
sp->len = 0;
return (0);
}
if (infile != NULL) {
(void) fclose(infile);
if (*oldfname != '\0') {
/* if there was a backup file, remove it */
(void) unlink(oldfname);
/*
* Backup the original. Note that hard links
* are not supported on all filesystems.
*/
if ((link(fname, oldfname) != 0) &&
(rename(fname, oldfname) != 0)) {
warn("rename()");
if (*tmpfname)
(void) unlink(tmpfname);
exit(1);
}
*oldfname = '\0';
}
if (*tmpfname != '\0') {
if (outfile != NULL && outfile != stdout)
if (fclose(outfile) != 0) {
warn("fclose()");
(void) unlink(tmpfname);
exit(1);
}
outfile = NULL;
if (rename(tmpfname, fname) != 0) {
/* this should not happen really! */
warn("rename()");
(void) unlink(tmpfname);
exit(1);
}
*tmpfname = '\0';
}
outfname = NULL;
}
if (firstfile == 0)
files = files->next;
else
firstfile = 0;
if (files == NULL) {
sp->len = 0;
return (0);
}
fname = files->fname;
if (inplace != NULL) {
char bn[PATH_MAX];
char dn[PATH_MAX];
(void) strlcpy(bn, fname, sizeof (bn));
(void) strlcpy(dn, fname, sizeof (dn));
if (lstat(fname, &sb) != 0)
err(1, "%s", fname);
if (!(sb.st_mode & S_IFREG))
fatal(_("in-place editing only "
"works for regular files"));
if (*inplace != '\0') {
(void) strlcpy(oldfname, fname,
sizeof (oldfname));
len = strlcat(oldfname, inplace,
sizeof (oldfname));
if (len > sizeof (oldfname))
fatal(_("name too long"));
}
len = snprintf(tmpfname, sizeof (tmpfname),
"%s/.!%ld!%s", dirname(dn), (long)getpid(),
basename(bn));
if (len >= sizeof (tmpfname))
fatal(_("name too long"));
(void) unlink(tmpfname);
if ((outfile = fopen(tmpfname, "w")) == NULL)
err(1, "%s", fname);
/*
* Some file systems don't support chown or
* chmod fully. On those, the owner/group and
* permissions will already be set to what
* they need to be.
*/
if (fstat(fileno(outfile), &nsb) != 0) {
warn("fstat()");
}
if (((sb.st_uid != nsb.st_uid) ||
(sb.st_gid != nsb.st_gid)) &&
(fchown(fileno(outfile), sb.st_uid, sb.st_gid)
!= 0))
warn("fchown()");
if ((sb.st_mode != nsb.st_mode) &&
(fchmod(fileno(outfile), sb.st_mode & 07777) != 0))
warn("fchmod()");
outfname = tmpfname;
if (!ispan) {
linenum = 0;
resetstate();
}
} else {
outfile = stdout;
outfname = "stdout";
}
if ((infile = fopen(fname, "r")) == NULL) {
warn("%s", fname);
rval = 1;
continue;
}
}
/*
* We are here only when infile is open and we still have something
* to read from it.
*
* Use getline() so that we can handle essentially infinite
* input data. The p and plen are static so each invocation gives
* getline() the same buffer which is expanded as needed.
*/
len = getline(&p, &plen, infile);
if (len == -1)
err(1, "%s", fname);
if (len != 0 && p[len - 1] == '\n')
len--;
cspace(sp, p, len, spflag);
linenum++;
return (1);
}
/*
* Like fgets, but go through the list of files chaining them together.
* Set len to the length of the line.
*/
int
mf_fgets(SPACE *sp, enum e_spflag spflag)
{
static FILE *f; /* Current open file */
size_t len;
char *p;
int c;
if (f == NULL)
/* Advance to first non-empty file */
for (;;) {
if (files == NULL) {
lastline = 1;
return (0);
}
if (files->fname == NULL) {
f = stdin;
fname = "stdin";
} else {
fname = files->fname;
if ((f = fopen(fname, "r")) == NULL)
err(FATAL, "%s: %s",
fname, strerror(errno));
}
if ((c = getc(f)) != EOF) {
(void)ungetc(c, f);
break;
}
(void)fclose(f);
files = files->next;
}
if (lastline) {
sp->len = 0;
return (0);
}
/*
* Use fgetln so that we can handle essentially infinite input data.
* Can't use the pointer into the stdio buffer as the process space
* because the ungetc() can cause it to move.
*/
p = fgetln(f, &len);
if (ferror(f))
err(FATAL, "%s: %s", fname, strerror(errno ? errno : EIO));
cspace(sp, p, len, spflag);
linenum++;
/* Advance to next non-empty file */
while ((c = getc(f)) == EOF) {
(void)fclose(f);
files = files->next;
if (files == NULL) {
lastline = 1;
return (1);
}
if (files->fname == NULL) {
f = stdin;
fname = "stdin";
} else {
fname = files->fname;
if ((f = fopen(fname, "r")) == NULL)
err(FATAL, "%s: %s", fname, strerror(errno));
}
}
(void)ungetc(c, f);
return (1);
}
