void CtcFirstOrderTest::contract(IntervalVector& box, ContractContext& context) { if(box.size() == 2) { return; } BxpNodeData* node_data = (BxpNodeData*) context.prop[BxpNodeData::id]; if(node_data == nullptr) { ibex_error("CtcFirstOrderTest: BxpNodeData must be set"); } vector<IntervalVector> gradients; for (int i = 0; i < system_.normal_constraints_.size() - 1; ++i) { if (!system_.normal_constraints_[i].isSatisfied(box)) { gradients.push_back(system_.normal_constraints_[i].gradient(box)); } } for (int i = 0; i < system_.sic_constraints_.size(); ++i) { if (!system_.sic_constraints_[i].isSatisfied(box, node_data->sic_constraints_caches[i])) { gradients.push_back(system_.sic_constraints_[i].gradient(box, node_data->sic_constraints_caches[i])); } } // Without the goal variable IntervalMatrix matrix(nb_var - 1, gradients.size() + 1); matrix.set_col(0, system_.goal_function_->gradient(box.subvector(0, nb_var - 2))); for (int i = 0; i < gradients.size(); ++i) { matrix.set_col(i + 1, gradients[i].subvector(0, nb_var - 2)); } bool testfailed = true; if (matrix.nb_cols() == 1) { if (matrix.col(0).contains(Vector::zeros(nb_var - 2))) { testfailed = false; } } else { int* pr = new int[matrix.nb_rows()]; int* pc = new int[matrix.nb_cols()]; IntervalMatrix LU(matrix.nb_rows(), matrix.nb_cols()); testfailed = true; try { interval_LU(matrix, LU, pr, pc); } catch(SingularMatrixException&) { testfailed = false; } delete[] pr; delete[] pc; } if(testfailed) { box.set_empty(); } }
void FncKhunTucker::jacobian(const IntervalVector& x_lambda, IntervalMatrix& J, const BitSet& components, int v) const { if (components.size()!=n+nb_mult) { not_implemented("FncKhunTucker: 'jacobian' for selected components"); //J.resize(n+nb_mult,n+nb_mult); } IntervalVector x=x_lambda.subvector(0,n-1); int lambda0=n; // The index of lambda0 in the box x_lambda is nb_var. int l=lambda0; // mutipliers indices counter. The first multiplier is lambda0. // matrix corresponding to the "Hessian expression" lambda_0*d^2f+lambda_1*d^2g_1+...=0 IntervalMatrix hessian=x_lambda[l] * df.jacobian(x,v<n? v : -1); // init if (v==-1 || v==l) J.put(0, l, df.eval_vector(x), false); l++; IntervalVector gx; if (!active.empty()) gx = sys.f_ctrs.eval_vector(x,active); // normalization equation (init) J[lambda0].put(0,Vector::zeros(n)); J[lambda0][lambda0]=1.0; IntervalVector dgi(n); // store dg_i([x]) (used in several places) for (BitSet::const_iterator i=ineq.begin(); i!=ineq.end(); ++i) { hessian += x_lambda[l] * dg[i].jacobian(x,v<n? v : -1); dgi=dg[i].eval_vector(x); J.put(0, l, dgi, false); J.put(l, 0, (x_lambda[l]*dgi), true); J.put(l, n, Vector::zeros(nb_mult), true); J[l][l]=gx[i]; J[lambda0][l] = 1.0; l++; } for (BitSet::const_iterator i=ineq.begin(); i!=ineq.end(); ++i) { hessian += x_lambda[l] * dg[i].jacobian(x,v<n? v : -1); dgi=dg[i].eval_vector(x); J.put(0, l, dgi, false); J.put(l, 0, dgi, true); J.put(l, n, Vector::zeros(nb_mult), true); J[lambda0][l] = 2*x_lambda[l]; l++; } for (BitSet::const_iterator v=bound_left.begin(); v!=bound_left.end(); ++v) { // this constraint does not contribute to the "Hessian expression" dgi=Vector::zeros(n); dgi[v]=-1.0; J.put(0, l, dgi, false); J.put(l, 0, (x_lambda[l]*dgi), true); J.put(l, n, Vector::zeros(nb_mult), true); J[l][l]=(-x[v]+sys.box[v].lb()); J[lambda0][l] = 1.0; l++; } for (BitSet::const_iterator v=bound_right.begin(); v!=bound_right.end(); ++v) { // this constraint does not contribute to the "Hessian expression" dgi=Vector::zeros(n); dgi[v]=1.0; J.put(0, l, dgi, false); J.put(l, 0, (x_lambda[l]*dgi), true); J.put(l, n, Vector::zeros(nb_mult), true); J[l][l]=(x[v]-sys.box[v].ub()); J[lambda0][l] = 1.0; l++; } assert(l==nb_mult+n); J.put(0,0,hessian); }
IntervalVector FncKhunTucker::eval_vector(const IntervalVector& x_lambda, const BitSet& components) const { if (components.size()!=n+nb_mult) { not_implemented("FncKhunTucker: 'eval_vector' for selected components"); //J.resize(n+nb_mult,n+nb_mult); } IntervalVector res(n+nb_mult); // Variables in x_lambda are organized as follows: // x - lambda0 - mu - lambda - beta IntervalVector x=x_lambda.subvector(0,n-1); int lambda0=n; // The index of lambda0 in the box x_lambda is nb_var. int l=lambda0; // multipliers indices counter. The first multiplier is lambda0. // vector corresponding to the "gradient expression" lambda_0*dg + lambda_1*dg_1 + ... (init IntervalVector grad=x_lambda[l] * df.eval_vector(x); // init l++; IntervalVector gx; if (!active.empty()) { gx=sys.f_ctrs.eval_vector(x,active); } // normalization equation lambda_0 + ... = 1.0 res[lambda0] = x_lambda[lambda0] - 1.0; // init for (BitSet::const_iterator i=ineq.begin(); i!=ineq.end(); ++i) { grad += x_lambda[l] * dg[i].eval_vector(x); res[l] = x_lambda[l] * gx[i]; res[lambda0] += x_lambda[l]; l++; } for (BitSet::const_iterator i=eq.begin(); i!=eq.end(); ++i) { grad += x_lambda[l] * dg[i].eval_vector(x); res[l] = gx[i]; res[lambda0] += sqr(x_lambda[l]); l++; } for (BitSet::const_iterator v=bound_left.begin(); v!=bound_left.end(); ++v) { grad[v] -= x_lambda[l]; res[l] = x_lambda[l] * (-x[v]+sys.box[v].lb()); res[lambda0] += x_lambda[l]; l++; } for (BitSet::const_iterator v=bound_right.begin(); v!=bound_right.end(); ++v) { grad[v] += x_lambda[l]; res[l] = x_lambda[l] * (x[v]-sys.box[v].ub()); res[lambda0] += x_lambda[l]; l++; } assert(l==nb_mult+n); res.put(0, grad); return res; }
/// Processes the data using contractors and bissections. Classifies the boxes in outside (grey), back_in(yellow) and unsafe (red) void Sivia::do_Sivia(Ctc& tubeConstraints, Data &data, Function gdot, bool calcInner){ QTime tSivia; tSivia.start(); if (calcInner) //inner approximation calculation { int count=0; while (!data.boxes.empty()) { IntervalVector currentBox = data.boxes.front(); //start from the first one data.boxes.pop_front(); //once it has been copied remove the first box IntervalVector auxBox=currentBox; //store it in aux variable to compare later tubeConstraints.contract(currentBox); //contract the current box using the previously calculated constraints if (currentBox!=auxBox){ //if the box has been contracted IntervalVector* removedByContractorInner; int setDiff=auxBox.diff(currentBox, removedByContractorInner); //set difference between the contracted box and the original box for (int i = 0; i < setDiff; ++i) { bool testInside=true; IntervalVector gg=data.g->eval_vector(removedByContractorInner[i]); for(int j = 0; j<gg.size(); j++){ testInside = testInside && (gg[j].ub()<=0); } if (testInside) { data.boxesInside.append(removedByContractorInner[i]); } } delete[] removedByContractorInner; } if(data.realTimeDraw){ //draw the boxes processing in real time draw_update(data, auxBox, currentBox); } bool allBoxesLessEpsilon=true; //check if all the boxes are smaler than epsilon for (int i=0;(i<(currentBox.size()-1));i++){ allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i])); } allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt)); //check the time box also bool boxesLessEpsilon=false; //check if at least one box is smaller than epsilon for (int i=0;(i<(currentBox.size()-1));i++){ boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]); } boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt); //check time box if (allBoxesLessEpsilon) { //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop (data.boxesInsideUnsafe).push_back(currentBox); count++; if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){ //If I have more boxes than nbPerhaps I stop the loop and I display the results break; } } else { //Otherwise we bissect following the widest diameter double l = 0; double l_temp = 0; int v = -1; for(int i = 0; i<currentBox.size()-1; i++){ //test that the diameter of the boxes doesnt depend on time if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){ l_temp = currentBox[i].diam(); if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){ l = l_temp/(data.epsilons[i]); v = i; } } } l_temp = currentBox[currentBox.size()-1].diam(); //test the time interval if(l_temp>=data.dt && l_temp/(data.dt) > l){ v = currentBox.size()-1; } if(v != -1 && currentBox[v].is_bisectable()){ // then the test interval of the state variables, and then it bisects the interval which has the largest diameter pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5); (data.boxes).push_back(boxes.first); (data.boxes).push_back(boxes.second); } else{ if (data.myDebug){ std::cout<<"Cannot be bisected \n"; } } } } } else //outer approximation { int count=0; //process all the boxes in data while (!data.boxes.empty()) { IntervalVector currentBox = data.boxes.front(); //start from the first one data.boxes.pop_front(); //once it has been copied remove the first box IntervalVector auxBox=currentBox; //store it in aux variable to compare later tubeConstraints.contract(currentBox); //contract the current box using the previously calculated constraints if (currentBox!=auxBox){ //if the box has been contracted IntervalVector* removedByContractor; int setDiff=auxBox.diff(currentBox, removedByContractor); //set difference between the contracted box and the original box for (int i = 0; i < setDiff; ++i) { data.boxesOutside.push_back(removedByContractor[i]); //add the areas removed by the contractor to the outside set } delete[] removedByContractor; } if(data.realTimeDraw){ //draw the boxes processing in real time draw_update(data, auxBox, currentBox); } bool allBoxesLessEpsilon=true; //check if all the boxes are smaler than epsilon for (int i=0;(i<(currentBox.size()-1));i++){ allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i])); } allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt)); //check the time box also bool boxesLessEpsilon=false; //check if at least one box is smaller than epsilon for (int i=0;(i<(currentBox.size()-1));i++){ boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]); } boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt); //check time box if (boxesLessEpsilon && !allBoxesLessEpsilon){ IntervalVector xnext = currentBox.subvector(0, data.numVarF-1).mid(); //using the middle point of the box calculate the future positions using euler method IntervalVector x = currentBox.mid(); bool testBackIn; for (int i = 0;i<data.numFuturePos;i++){ // Euler method: x(n+1)=x(n)+dt*fx x[data.numVarF]= x[data.numVarF].mid(); testBackIn = true; xnext=xnext+(data.dt)*data.f->eval_vector(x); x.put(0, xnext); x[data.numVarF] = x[data.numVarF]+(data.dt); IntervalVector gg=data.g->eval_vector(x); for(int j = 0; j<gg.size(); j++){ testBackIn = testBackIn && (gg[j].ub()<0); //test if it comes back to the bubble } if(testBackIn == true){ break; } } if(testBackIn == true && data.enableBackIn){ //If my box was back in the bubble after integration, I store it in boxesbackin (data.boxesBackIn).append(currentBox); continue; } } if (allBoxesLessEpsilon) { //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop (data.boxesUnsafe).push_back(currentBox); count++; if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){ //If I have more boxes than nbPerhaps I stop the loop and I display the results break; } } else { //Otherwise we bissect following the widest diameter double l = 0; double l_temp = 0; int v = -1; for(int i = 0; i<currentBox.size()-1; i++){ //test that the diameter of the boxes doesnt depend on time if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){ l_temp = currentBox[i].diam(); if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){ l = l_temp/(data.epsilons[i]); v = i; } } } l_temp = currentBox[currentBox.size()-1].diam(); //test the time interval if(l_temp>=data.dt && l_temp/(data.dt) > l){ v = currentBox.size()-1; } if(v != -1 && currentBox[v].is_bisectable()){ // then the test interval of the state variables, and then it bisects the interval which has the largest diameter pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5); (data.boxes).push_back(boxes.first); (data.boxes).push_back(boxes.second); } else{ if (data.myDebug){ std::cout<<"Can not be bisected \n"; } } } } double maxGValues[data.numVarG-1]; //init vector to store the max values of G for (int i = 0; i < data.numVarG-1; ++i) { maxGValues[i]=0; } for(int i=0; i<data.boxesUnsafe.size();i++) { //process unsafe boxes IntervalVector currentBox=data.boxesUnsafe.at(i); IntervalVector nextBox = currentBox.subvector(0, data.numVarF-1); if (data.intMethod==0){ //Guaranteed integration // State variables Variable y(data.numVarF); // Initial conditions IntervalVector yinit(data.numVarF); for (int i = 0; i < data.numVarF; ++i) { yinit[i] = currentBox[i]; cout<<currentBox[i]<<endl; } // system fn has to be re entered here, cannot be loaded directly from text file //pendulum Function ydot = Function (y,Return (y[1], -sin(y[0])-0.15*y[1])); //non holonomic // Interval t = currentBox[data.numVarF]; // Interval xd = 7*t; // Interval xdd = 7; // Interval yd = sin(0.1*t); // Interval ydd = 0.1*cos(0.1*t); // Interval xdiff = (xd-y[0]+xdd); // Interval ydiff = (yd-y[1]+ydd); // Interval norm = ( sqrt((xdiff)^2 +(ydiff)^2) ); // Function ydot = Function (y,Return (( sqrt((xd-y[0]+xdd)*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*cos(y[2]), ( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*sin(y[2]), 10*(cos(y[2])*((yd-y[1]+ydd))-sin(y[2])*((xd-y[0]+xdd)))/( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) ))); // Ivp contruction (initial time is 0.0) QTime t1; t1.start(); ivp_ode problem = ivp_ode (ydot, 0.0 , yinit); // Simulation construction and run simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__); //uses Runge-kutta4 method data.boxesUnsafeFuture.append(simu.run_simulation()); //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one double timeSiviaCalculations1=t1.elapsed()/1000.0; double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0; cout<<endl<<"Unsafe # "<<i<<" , Box time = "<<timeSiviaCalculations1<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl; } if (data.intMethod==1){ //euler method for (int i = 0;i<data.numFuturePos;i++){ IntervalVector gdotValue=gdot.eval_vector(currentBox); //evaluate the g and gdot functions to inspect the constraints IntervalVector gValue=data.g->eval_vector(currentBox); if (data.myDebug){ cout<<"box = "<<currentBox<<endl; for(int j = 0; j<gValue.size(); j++){ cout<<"gdot"<<j<<" = "<<gdotValue<<" / "<<(gdotValue[j].lb()>0) <<endl; //gdot i values } } for(int j = 0; j<gValue.size(); j++){ if (data.myDebug){ cout<<"g"<<j<<" = "<<gValue[j]<<" / "<<((gValue[j].ub()>0)&&(gValue[j].lb()<0)) <<endl;} //print gi values if((gValue[j].ub()>maxGValues[j])&&(gValue[j].ub()<999999)){ //check max values for each gi, ignore if system goes to infinity maxGValues[j]=gValue[j].ub();} } nextBox=nextBox+(data.dt)*data.f->eval_vector(currentBox); //euler method data.boxesUnsafeFuture.append(nextBox); currentBox.put(0, nextBox); currentBox[data.numVarF] = currentBox[data.numVarF]+(data.dt); //increase time for the next step } } } for(int i=0; i<data.boxesBackIn.size();i++){ //process back_in boxes IntervalVector currentBox=data.boxesBackIn.at(i); IntervalVector nextBox = currentBox.subvector(0, data.numVarF-1); if (data.intMethod==0){ //guaranteed integration //Guaranteed integration // State variables Variable y(data.numVarF); // Initial conditions IntervalVector yinit(data.numVarF); for (int i = 0; i < data.numVarF; ++i) { yinit[i] = currentBox[i]; cout<<currentBox[i]<<endl; } QTime t2; t2.start(); // system fn has to be re entered here, cannot be loaded directly from text file //pendulum Function ydot = Function (y,Return (y[1], -sin(y[0])-0.15*y[1])); //non holonomic // Interval t = currentBox[data.numVarF]; // Interval xd = 7*t; // Interval xdd = 7; // Interval yd = sin(0.1*t); // Interval ydd = 0.1*cos(0.1*t); // Interval xdiff = (xd-y[0]+xdd); // Interval ydiff = (yd-y[1]+ydd); // Interval norm = ( sqrt((xdiff)^2 +(ydiff)^2) ); // Function ydot = Function (y,Return (( sqrt((xd-y[0]+xdd)*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*cos(y[2]), ( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) )*sin(y[2]), 10*(cos(y[2])*((yd-y[1]+ydd))-sin(y[2])*((xd-y[0]+xdd)))/( sqrt(((xd-y[0]+xdd))*(xd-y[0]+xdd) +((yd-y[1]+ydd))*(yd-y[1]+ydd)) ))); // Ivp contruction (initial time is 0.0) ivp_ode problem = ivp_ode (ydot, currentBox[data.numVarF].lb() , yinit); // Simulation construction and run simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__); //uses Runge-kutta4 method data.boxesUnsafeFuture.append(simu.run_simulation()); //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one double timeSiviaCalculations2=t2.elapsed()/1000.0; double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0; cout<<endl<<"Back_in # "<<i<<" , Box time = "<<timeSiviaCalculations2<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl; } for (int i = 0;i<data.numFuturePos;i++){ //euler method if (data.intMethod==1){ IntervalVector gdotValue=gdot.eval_vector(currentBox); //evaluate the g and gdot functions to inspect the constraints IntervalVector gValue=data.g->eval_vector(currentBox); if (data.myDebug){ cout<<"box = "<<currentBox<<endl; for(int j = 0; j<gValue.size(); j++){ cout<<"gdot"<<j<<" = "<<gdotValue<<" / "<<(gdotValue[j].lb()>0) <<endl; //gdoti values } } bool testBackIn= true; for(int j = 0; j<gValue.size(); j++){ if (data.myDebug){ cout<<"g"<<j<<" = "<<gValue[j]<<" / "<<((gValue[j].ub()>0)&&(gValue[j].lb()<0)) <<endl;} //print gi values testBackIn = testBackIn && (gValue[j].ub()<0); //test if it comes back to the bubble if((gValue[j].ub()>maxGValues[j])&&(gValue[j].ub()<999999)){ //check max values for each gi, ignore if system goes to infinity maxGValues[j]=gValue[j].ub(); } } nextBox=nextBox+(data.dt)*data.f->eval_vector(currentBox); // euler method if (!testBackIn){ data.boxesBackInFuture.append(nextBox); } currentBox.put(0, nextBox); currentBox[data.numVarF] = currentBox[data.numVarF]+(data.dt); //increase time for the next step } } } if (data.myDebug){ for (int i = 0; i < data.numVarG-1; ++i) { cout<<"Max G"<<i<<" = "<<maxGValues[i]<<endl;} } } }
IntervalVector sip_from_ext_box(const IntervalVector& ext_box) { return ext_box.subvector(0, ext_box.size()-2); }
/// Processes the data using contractors and bissections. Classifies the boxes in outside (grey), back_in(yellow) and unsafe (red) void Sivia::do_Sivia(Ctc& tubeConstraints, Data &data, Function gdot, bool calcInner) { QTime tSivia; tSivia.start(); if (calcInner) //inner approximation calculation { int count=0; while (!data.boxes.empty()) { IntervalVector currentBox = data.boxes.front(); //start from the first one data.boxes.pop_front(); //once it has been copied remove the first box IntervalVector auxBox=currentBox; //store it in aux variable to compare later tubeConstraints.contract(currentBox); //contract the current box using the previously calculated constraints if (currentBox!=auxBox){ //if the box has been contracted IntervalVector* removedByContractorInner; int setDiff=auxBox.diff(currentBox, removedByContractorInner); //set difference between the contracted box and the original box for (int i = 0; i < setDiff; ++i) { //data.boxesOutside.push_back(removedByContractor[i]); //add the areas removed by the contractor to the outside set bool testInside=true; IntervalVector gg=data.g->eval_vector(removedByContractorInner[i]); for(int j = 0; j<gg.size(); j++){ testInside = testInside && (gg[j].ub()<=0); } if (testInside) { data.boxesInside.append(removedByContractorInner[i]); } } delete[] removedByContractorInner; } if(data.realTimeDraw){ //draw the boxes processing in real time draw_update(data, auxBox, currentBox); } bool allBoxesLessEpsilon=true; //check if all the boxes are smaler than epsilon for (int i=0;(i<(currentBox.size()-1));i++){ allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i])); } allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt)); //check the time box also bool boxesLessEpsilon=false; //check if at least one box is smaller than epsilon for (int i=0;(i<(currentBox.size()-1));i++){ boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]); } boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt); //check time box if (boxesLessEpsilon && !allBoxesLessEpsilon){ IntervalVector xnext = currentBox.subvector(0, data.numVarF-1).mid(); //using the middle point of the box calculate the future positions using euler method IntervalVector x = currentBox.mid(); bool testBackIn; for (int i = 0;i<data.numFuturePos;i++){ // Euler method: x(n+1)=x(n)+dt*fx x[data.numVarF]= x[data.numVarF].mid(); testBackIn = true; xnext=xnext+(data.dt)*data.f->eval_vector(x); x.put(0, xnext); x[data.numVarF] = x[data.numVarF]+(data.dt); IntervalVector gg=data.g->eval_vector(x); for(int j = 0; j<gg.size(); j++){ testBackIn = testBackIn && (gg[j].ub()<0); //test if it comes back to the bubble } if(testBackIn == true){ //If so we calculate the max deviation break; } } if(testBackIn == true){ //If my box was back in the bubble after integration, I store it in boxesbackin (data.boxesInsideBackIn).append(currentBox); continue; } } if (allBoxesLessEpsilon) { //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop (data.boxesInsideUnsafe).push_back(currentBox); count++; if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){ //If I have more boxes than nbPerhaps I stop the loop and I display the results break; } } else { //Otherwise we bissect following the widest diameter double l = 0; double l_temp = 0; int v = -1; for(int i = 0; i<currentBox.size()-1; i++){ //test that the diameter of the boxes doesnt depend on time if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){ l_temp = currentBox[i].diam(); if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){ l = l_temp/(data.epsilons[i]); v = i; } } } l_temp = currentBox[currentBox.size()-1].diam(); //test the time interval if(l_temp>=data.dt && l_temp/(data.dt) > l){ v = currentBox.size()-1; } if(v != -1 && currentBox[v].is_bisectable()){ // then the test interval of the state variables, and then it bisects the interval which has the largest diameter pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5); (data.boxes).push_back(boxes.first); (data.boxes).push_back(boxes.second); } else{ if (data.myDebug){ std::cout<<"Cannot be bisected \n"; } } } } } else //outer approximation { int count=0; //SIVIA //process all the boxes in data while (!data.boxes.empty()) { IntervalVector currentBox = data.boxes.front(); //start from the first one data.boxes.pop_front(); //once it has been copied remove the first box IntervalVector auxBox=currentBox; //store it in aux variable to compare later tubeConstraints.contract(currentBox); //contract the current box using the previously calculated constraints if (currentBox!=auxBox){ //if the box has been contracted IntervalVector* removedByContractor; int setDiff=auxBox.diff(currentBox, removedByContractor); //set difference between the contracted box and the original box for (int i = 0; i < setDiff; ++i) { data.boxesOutside.push_back(removedByContractor[i]); //add the areas removed by the contractor to the outside set } delete[] removedByContractor; } if(data.realTimeDraw){ //draw the boxes processing in real time draw_update(data, auxBox, currentBox); } bool allBoxesLessEpsilon=true; //check if all the boxes are smaler than epsilon for (int i=0;(i<(currentBox.size()-1));i++){ allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox.diam()[i])<=data.epsilons[i])); } allBoxesLessEpsilon = (allBoxesLessEpsilon && ((currentBox[currentBox.size()-1].diam())<=data.dt)); //check the time box also bool boxesLessEpsilon=false; //check if at least one box is smaller than epsilon for (int i=0;(i<(currentBox.size()-1));i++){ boxesLessEpsilon = boxesLessEpsilon||((currentBox[i].diam())<=data.epsilons[i]); } boxesLessEpsilon = boxesLessEpsilon&&((currentBox[currentBox.size()-1].diam())<=data.dt); //check time box if (boxesLessEpsilon && !allBoxesLessEpsilon){ IntervalVector xnext = currentBox.subvector(0, data.numVarF-1).mid(); //using the middle point of the box calculate the future positions using euler method IntervalVector x = currentBox.mid(); bool testBackIn; for (int i = 0;i<data.numFuturePos;i++){ // Euler method: x(n+1)=x(n)+dt*fx x[data.numVarF]= x[data.numVarF].mid(); testBackIn = true; xnext=xnext+(data.dt)*data.f->eval_vector(x); x.put(0, xnext); x[data.numVarF] = x[data.numVarF]+(data.dt); IntervalVector gg=data.g->eval_vector(x); for(int j = 0; j<gg.size(); j++){ testBackIn = testBackIn && (gg[j].ub()<0); //test if it comes back to the bubble } if(testBackIn == true){ //If so we calculate the max deviation break; } } // if(testBackIn == true){ //If my box was back in the bubble after integration, I store it in boxesbackin // (data.boxesBackIn).append(currentBox); // continue; // } } if (allBoxesLessEpsilon) { //if allBoxesLessEpsilon = true the box is unsafe and I continue my loop (data.boxesUnsafe).push_back(currentBox); count++; if (count >=data.maxNumUnsafeBoxes && data.maxNumUnsafeBoxesActivated){ //If I have more boxes than nbPerhaps I stop the loop and I display the results break; } } else { //Otherwise we bissect following the widest diameter double l = 0; double l_temp = 0; int v = -1; for(int i = 0; i<currentBox.size()-1; i++){ //test that the diameter of the boxes doesnt depend on time if(currentBox[i].is_bisectable()||!(currentBox[i].is_degenerated())){ l_temp = currentBox[i].diam(); if(l_temp>=data.epsilons[i] && l_temp/(data.epsilons[i]) > l){ l = l_temp/(data.epsilons[i]); v = i; } } } l_temp = currentBox[currentBox.size()-1].diam(); //test the time interval if(l_temp>=data.dt && l_temp/(data.dt) > l){ v = currentBox.size()-1; } if(v != -1 && currentBox[v].is_bisectable()){ // then the test interval of the state variables, and then it bisects the interval which has the largest diameter pair<IntervalVector,IntervalVector> boxes=currentBox.bisect(v, 0.5); (data.boxes).push_back(boxes.first); (data.boxes).push_back(boxes.second); } else{ if (data.myDebug){ std::cout<<"Cannot be bisected \n"; } } } } for(int i=0; i<data.boxesUnsafe.size();i++) { //process unsafe boxes IntervalVector currentBox=data.boxesUnsafe.at(i); IntervalVector nextBox = currentBox.subvector(0, data.numVarF-1); if (data.intMethod==0){ //Guaranteed integration // State variables Variable y(data.numVarF); // Initial conditions IntervalVector yinit(data.numVarF); for (int i = 0; i < data.numVarF; ++i) { yinit[i] = currentBox[i]; cout<<currentBox[i]<<endl; } Interval t = currentBox[data.numVarF]; Interval xd = 7*t; Interval xdd = 7; Interval yd = sin(0.1*t); Interval ydd = 0.1*cos(0.1*t); // Interval xdiff = (xd-y[0]+xdd); // Interval ydiff = (yd-y[1]+ydd); // Interval norm = (sqrt((xdiff)^2 +(ydiff)^2)); // system fn has to be re entered here, cannot be loaded directly from text file //pendulum Function ydot = Function (y,Return (y[1],-1*sin(y[0])-0.15*y[1])); // Ivp contruction (initial time is 0.0) //non holonomic // Function ydot = Function (y, Return ((sqrt(((xd-y[0]+xdd)*(xd-y[0]+xdd)) +((yd-y[1]+ydd)*(yd-y[1]+ydd))))*cos(y[2]), (sqrt(((xd-y[0]+xdd)*(xd-y[0]+xdd)) +((yd-y[1]+ydd)*(yd-y[1]+ydd))))*sin(y[2]),10*(cos(y[2])*((yd-y[1]+ydd))-sin(y[2])*((xd-y[0]+xdd)))/(sqrt(((xd-y[0]+xdd)*(xd-y[0]+xdd)) +((yd-y[1]+ydd)*(yd-y[1]+ydd)))))); // Ivp contruction (initial time is 0.0) QTime t1; t1.start(); ivp_ode problem = ivp_ode (ydot,0.0 , yinit); // Simulation construction and run simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__); //uses Runge-kutta4 method data.boxesUnsafeFuture.append(simu.run_simulation()); //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one double timeSiviaCalculations1=t1.elapsed()/1000.0; double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0; cout<<endl<<"Unsafe # "<<i<<" , Box time = "<<timeSiviaCalculations1<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl; } if (data.intMethod==1){ //euler method for (int i = 0;i<data.numFuturePos;i++){ IntervalVector gdotValue=gdot.eval_vector(currentBox); //evaluate the g and gdot functions to inspect the constraints IntervalVector gValue=data.g->eval_vector(currentBox); if (data.myDebug){ cout<<"box = "<<currentBox<<endl; for(int j = 0; j<gValue.size(); j++){ cout<<"gdot"<<j<<" = "<<gdotValue<<" / "<<(gdotValue[j].lb()>0) <<endl; //gdot i values } } nextBox=nextBox+(data.dt)*data.f->eval_vector(currentBox); //euler method data.boxesUnsafeFuture.append(nextBox); currentBox.put(0, nextBox); currentBox[data.numVarF] = currentBox[data.numVarF]+(data.dt); //increase time for the next step } } if (data.intMethod==2){ //vertex unsafe //check derivative of the unsafe boxes of outer g wrt the inner g // Function g_inner("g_inner.txt"); // Function dg_inner(g_inner, Function::DIFF); // d/dx(gi)(x,t) // Variable x(data.numVarF),t; //we have x[] and t as variables for our fns // // initialize auxMat and auxVector to the correct sizes and fill with zeros // IntervalMatrix auxMat(data.numVarF+1, data.numVarF,Interval::ZERO); // IntervalVector auxVector(data.numVarF+1,Interval::ZERO); // //put 1 in the diagonal of auxMat // for (int i=0; i<data.numVarF; i++){ // auxMat[i][i]=1;} // auxVector[data.numVarF]=1; // Function f=("f.txt"); // Function gdot_inner(x,t,dg_inner(x,t)*(auxMat*transpose(f(x,t))+auxVector)); // cout<<"gdot: "<<gdot_inner<<endl; // IntervalVector gdot_inner_Result=gdot_inner.eval_vector(currentBox); // bool testNegative=true; // for(int j = 0; j<gdot_inner_Result.size(); j++){ // testNegative = testNegative && (gdot_inner_Result[j].ub()<0); // } // if (testNegative) { // data.boxesOuterGSafeForInnerG.append(currentBox); // cout<<"Safe for inner G: "<<currentBox<<" result: "<<gdot_inner_Result<<endl; // } // else{ // data.boxesOuterGUnsafeForInnerG.append(currentBox); // cout<<"Unsafe for inner G: "<<currentBox<<" result: "<<gdot_inner_Result<<endl; // } //system double x_k_lb[currentBox.size()], x_k_ub[currentBox.size()]; for (int j = 0; j < currentBox.size(); ++j) { x_k_ub[j]=currentBox[j].ub(); x_k_lb[j]=currentBox[j].lb(); } int numVar=currentBox.size()-1; int numSamples=data.dt*data.numFuturePos*1000; double x_k_uu[numVar][numSamples]; double x_k_ul[numVar][numSamples]; double x_k_lu[numVar][numSamples]; double x_k_ll[numVar][numSamples]; x_k_uu[0][0]=x_k_ub[0]; x_k_ll[0][0]=x_k_lb[0]; x_k_lu[0][0]=x_k_lb[0]; x_k_ul[0][0]=x_k_ub[0]; x_k_uu[1][0]=x_k_ub[1]; x_k_ll[1][0]=x_k_lb[1]; x_k_lu[1][0]=x_k_ub[1]; x_k_ul[1][0]=x_k_lb[1]; double g_outer_uu, g_outer_lu, g_outer_ul, g_outer_ll, g_inner_uu, g_inner_lu, g_inner_ul, g_inner_ll; bool unsafeBox=false; bool safeBox=false; //pendulum system for (int j = 1; j < data.dt*data.numFuturePos*1000; ++j) { x_k_uu[1][j]=data.dt/10.0*(-sin(x_k_uu[0][j-1])-0.15*x_k_uu[1][j-1])+x_k_uu[1][j-1]; x_k_ll[1][j]=data.dt/10.0*(-sin(x_k_ll[0][j-1])-0.15*x_k_ll[1][j-1])+x_k_ll[1][j-1]; x_k_lu[1][j]=data.dt/10.0*(-sin(x_k_lu[0][j-1])-0.15*x_k_lu[1][j-1])+x_k_lu[1][j-1]; x_k_ul[1][j]=data.dt/10.0*(-sin(x_k_ul[0][j-1])-0.15*x_k_ul[1][j-1])+x_k_ul[1][j-1]; x_k_uu[0][j]=(x_k_uu[1][j-1])*(data.dt/10.0)+x_k_uu[0][j-1]; x_k_ll[0][j]=(x_k_ll[1][j-1])*(data.dt/10.0)+x_k_uu[0][j-1]; x_k_lu[0][j]=(x_k_lu[1][j-1])*(data.dt/10.0)+x_k_uu[0][j-1]; x_k_ul[0][j]=(x_k_ul[1][j-1])*(data.dt/10.0)+x_k_uu[0][j-1]; //add discretized sys //check if they leave the outer g g_outer_uu= (x_k_uu[0][j]*x_k_uu[0][j])+(x_k_uu[1][j]*x_k_uu[1][j])-1; g_outer_lu= (x_k_lu[0][j]*x_k_lu[0][j])+(x_k_lu[1][j]*x_k_lu[1][j])-1; g_outer_ul= (x_k_ul[0][j]*x_k_ul[0][j])+(x_k_ul[1][j]*x_k_ul[1][j])-1; g_outer_ll= (x_k_ll[0][j]*x_k_ll[0][j])+(x_k_ll[1][j]*x_k_ll[1][j])-1; //check if the trajectories reenter the inner g if (data.ellipseInner) { g_inner_uu= (x_k_uu[0][j]*x_k_uu[0][j])/0.81+(x_k_uu[1][j]*x_k_uu[1][j])/(0.4*0.4)-1; g_inner_lu= (x_k_lu[0][j]*x_k_lu[0][j])/0.81+(x_k_lu[1][j]*x_k_lu[1][j])/(0.4*0.4)-1; g_inner_ul= (x_k_ul[0][j]*x_k_ul[0][j])/0.81+(x_k_ul[1][j]*x_k_ul[1][j])/(0.4*0.4)-1; g_inner_ll= (x_k_ll[0][j]*x_k_ll[0][j])/0.81+(x_k_ll[1][j]*x_k_ll[1][j])/(0.4*0.4)-1; } else{ g_inner_uu= (x_k_uu[0][j]*x_k_uu[0][j])/(0.99*0.99)+(x_k_uu[1][j]*x_k_uu[1][j])/(0.96*0.96)-1; g_inner_lu= (x_k_lu[0][j]*x_k_lu[0][j])/(0.99*0.99)+(x_k_lu[1][j]*x_k_lu[1][j])/(0.96*0.96)-1; g_inner_ul= (x_k_ul[0][j]*x_k_ul[0][j])/(0.99*0.99)+(x_k_ul[1][j]*x_k_ul[1][j])/(0.96*0.96)-1; g_inner_ll= (x_k_ll[0][j]*x_k_ll[0][j])/(0.99*0.99)+(x_k_ll[1][j]*x_k_ll[1][j])/(0.96*0.96)-1; } double myFutureBox[j][2][2]; double myMaxPos= qMax(qMax(qMax(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]); double myMinPos= qMin(qMin(qMin(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]); double myMaxVel= qMax(qMax(qMax(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]); double myMinVel= qMin(qMin(qMin(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]); myFutureBox[j][0][0]=myMinPos; myFutureBox[j][0][1]=myMaxPos; myFutureBox[j][1][0]=myMinVel; myFutureBox[j][1][1]=myMaxVel; IntervalVector BoxFuture(data.numVarG, myFutureBox[j]); data.boxesVertexFuture.append(BoxFuture); if (true &&(g_outer_uu>=0 || g_outer_lu>=0 || g_outer_ul>=0 || g_outer_ll>=0)){ unsafeBox=true; data.boxesUnsafeOuterG.append(currentBox); double myUnsafeFutureBox[j][2][2]; for (int k = 0; k < j; ++k) { myMaxPos= qMax(qMax(qMax(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]); myMinPos= qMin(qMin(qMin(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]); myMaxVel= qMax(qMax(qMax(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]); myMinVel= qMin(qMin(qMin(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]); myUnsafeFutureBox[j][0][0]=myMinPos; myUnsafeFutureBox[j][0][1]=myMaxPos; myUnsafeFutureBox[j][1][0]=myMinVel; myUnsafeFutureBox[j][1][1]=myMaxVel; IntervalVector BoxUnsafeFuture(data.numVarG, myUnsafeFutureBox[k]); data.boxesUnsafeOuterGFuture.append(BoxUnsafeFuture); // cout<<"Unsafe vertex box "<<BoxUnsafeFuture<<endl; } break; } if (j>20 && g_inner_uu<0 && g_inner_lu<0 && g_inner_ul<0 && g_inner_ll<0){ safeBox=true; //data.boxesUnsafeOuterG.append(currentBox); double mySafeFutureBox[j][2][2]; for (int k = 0; k < j; ++k) { myMaxPos= qMax(qMax(qMax(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]); myMinPos= qMin(qMin(qMin(x_k_uu[0][j],x_k_ll[0][j]),x_k_lu[0][j]),x_k_ul[0][j]); myMaxVel= qMax(qMax(qMax(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]); myMinVel= qMin(qMin(qMin(x_k_uu[1][j],x_k_ll[1][j]),x_k_lu[1][j]),x_k_ul[1][j]); mySafeFutureBox[j][0][0]=myMinPos; mySafeFutureBox[j][0][1]=myMaxPos; mySafeFutureBox[j][1][0]=myMinVel; mySafeFutureBox[j][1][1]=myMaxVel; IntervalVector BoxSafeFuture(data.numVarG, mySafeFutureBox[k]); data.boxesGuaranteedIntegrationUnsafe.append(BoxSafeFuture); // cout<<"Safe vertex box "<<BoxSafeFuture<<endl; } break; } } if (unsafeBox==false && safeBox==true){ data.boxesUncertain.append(currentBox); } } } for (int i = 0; i < data.boxesUncertain.size(); ++i) { //process uncertain boxes IntervalVector uncertainBox=data.boxesUncertain.at(i); Variable y(data.numVarF); // Initial conditions IntervalVector yinit(data.numVarF); for (int j = 0; j < data.numVarF; ++j) { yinit[j] = uncertainBox[j]; cout<<uncertainBox[j]<<endl; } Function ydot = Function (y,Return (y[1],-1*sin(y[0])-0.15*y[1])); // Ivp contruction (initial time is 0.0) QTime t1; t1.start(); ivp_ode problem = ivp_ode (ydot, 0.0 , yinit); // Simulation construction and run int prevSize=data.boxesUncertainFuture.size(); simulation simu = simulation (&problem,data.dt*data.numFuturePos, __METH__, __PREC__); //uses Runge-kutta4 method data.boxesUncertainFuture.append(simu.run_simulation()); //modified ibex_simulation.h to make it return a list with all the solutions, not just the last one int afterSize=data.boxesUncertainFuture.size(); for (int k = 0; k < (afterSize-prevSize); ++k) { data.boxesUncertainFutureIndex.append(i); } double timeSiviaCalculations1=t1.elapsed()/1000.0; double timeSiviaCalculationsTotal=tSivia.elapsed()/1000.0; cout<<endl<<"Unsafe # "<<i<<" , Box time = "<<timeSiviaCalculations1<<" , Total elapsed time = "<<timeSiviaCalculationsTotal<<endl; } for (int j = 0; j < data.boxesUncertainFuture.size(); ++j) { IntervalVector myBox=data.boxesUncertainFuture.at(j); bool testInsideOuterG = true; Function g_outer("g_outer.txt"); IntervalVector gg=g_outer.eval_vector(myBox); for(int j = 0; j<gg.size(); j++){ testInsideOuterG = (testInsideOuterG && (gg[j].ub()<0)); } if (testInsideOuterG==false){ data.boxesUnsafeOuterGFuture.append(myBox); data.boxesUnsafeOuterG.append(data.boxesUncertain.at(data.boxesUncertainFutureIndex.at(j))); } else{ data.boxesSafeOuterGFuture.append(myBox); } } } }