// asynchronous step solver
int e_trsolver::stepsolve_async(nr_double_t steptime)
{
// Start to sweep through time.
int error = 0;
convError = 0;
time = steptime;
// update the interpolation time of any externally controlled
// components which require it.
updateExternalInterpTime(time);
// make the stored histories for all ircuits that have
// requested them at least as long as the next major time
// step so we can reject the step later if needed and
// restore all the histories to their previous state
updateHistoryAges (time - lastasynctime);
//delta = (steptime - time) / 10;
//if (progress) logprogressbar (i, swp->getSize (), 40);
#if DEBUG && 0
messagefcn (LOG_STATUS, "NOTIFY: %s: solving netlist for t = %e\n",
getName (), (double) time);
#endif
do
{
#if STEPDEBUG
if (delta == deltaMin)
{
messagefcn (LOG_ERROR,
"WARNING: %s: minimum delta h = %.3e at t = %.3e\n",
getName (), (double) delta, (double) current);
}
#endif
// update the integration coefficients
updateCoefficients (delta);
// Run predictor to get a start value for the solution vector for
// the successive iterative corrector process
error += predictor ();
// restart Newton iteration
if (rejected)
{
restart (); // restart non-linear devices
rejected = 0;
}
// Run corrector process with appropriate exception handling.
// The corrector iterates through the solutions of the integration
// process until a certain error tolerance has been reached.
try_running () // #defined as: do {
{
error += corrector ();
}
catch_exception () // #defined as: } while (0); if (estack.top ()) switch (estack.top()->getCode ())
{
case EXCEPTION_NO_CONVERGENCE:
pop_exception ();
// Reduce step-size (by half) if failed to converge.
if (current > 0) current -= delta;
delta /= 2;
if (delta <= deltaMin)
{
delta = deltaMin;
adjustOrder (1);
}
if (current > 0) current += delta;
// Update statistics.
statRejected++;
statConvergence++;
rejected++;
converged = 0;
error = 0;
// Start using damped Newton-Raphson.
convHelper = CONV_SteepestDescent;
convError = 2;
#if DEBUG
messagefcn (LOG_ERROR, "WARNING: delta rejected at t = %.3e, h = %.3e "
"(no convergence)\n", (double) saveCurrent, (double) delta);
#endif
break;
default:
// Otherwise return.
estack.print ();
error++;
break;
}
if (error) return -1;
if (rejected) continue;
// check whether Jacobian matrix is still non-singular
if (!A->isFinite ())
{
messagefcn (LOG_ERROR, "ERROR: %s: Jacobian singular at t = %.3e, "
"aborting %s analysis\n", getName (), (double) current,
getDescription ().c_str());
return -1;
}
// Update statistics and no more damped Newton-Raphson.
statIterations += iterations;
if (--convError < 0) convHelper = 0;
// Now advance in time or not...
if (running > 1)
{
adjustDelta (time);
adjustOrder ();
}
else
{
fillStates ();
nextStates ();
rejected = 0;
}
saveCurrent = current;
current += delta;
running++;
converged++;
// Tell integrators to be running.
setMode (MODE_NONE);
// Initialize or update history.
if (running > 1)
{
updateHistory (saveCurrent);
}
else
{
initHistory (saveCurrent);
}
}
while (saveCurrent < time); // Hit a requested time point?
return 0;
}
/* synchronous step solver for external ode routine
*
* This function solves the circuit for a single time delta provided
* by an external source. Convergence issues etc. are expected to
* be handled by the external solver, as it is in full control of the
* time stepping.
*/
int e_trsolver::stepsolve_sync(nr_double_t synctime)
{
int error = 0;
convError = 0;
time = synctime;
// update the interpolation time of any externally controlled
// components which require it.
updateExternalInterpTime(time);
// copy the externally chosen time step to delta
delta = time - lastsynctime;
// get the current solution time
//current += delta;
// updates the integrator coefficients, and updates the array of prev
// 8 deltas with the new delta for this step
updateCoefficients (delta);
// Run predictor to get a start value for the solution vector for
// the successive iterative corrector process
error += predictor ();
// restart Newton iteration
restart (); // restart non-linear devices
// Attempt to solve the circuit with the given delta
try_running () // #defined as: do {
{
//error += solve_nonlinear_step ();
error += corrector ();
}
catch_exception () // #defined as: } while (0); if (estack.top ()) switch (estack.top()->getCode ())
{
case EXCEPTION_NO_CONVERGENCE:
pop_exception ();
// Retry using damped Newton-Raphson.
this->convHelper = CONV_SteepestDescent;
convError = 2;
#if DEBUG
messagefcn (LOG_ERROR, "WARNING: delta rejected at t = %.3e, h = %.3e "
"(no convergence)\n", (double) saveCurrent, (double) delta);
#endif
try_running () // #defined as: do {
{
// error += solve_nonlinear_step ();
error += solve_nonlinear ();
}
catch_exception () // #defined as: } while (0); if (estack.top ()) switch (estack.top()->getCode ())
{
case EXCEPTION_NO_CONVERGENCE:
pop_exception ();
// Update statistics.
statRejected++;
statConvergence++;
rejected++;
converged = 0;
error = 0;
break;
default:
// Otherwise return.
estack.print ();
error++;
break;
}
// Update statistics and no more damped Newton-Raphson.
// statIterations += iterations;
// if (--convError < 0) this->convHelper = 0;
break;
default:
// Otherwise return.
estack.print ();
error++;
break;
}
// if there was an error other than non-convergence, return -1
if (error) return -1;
// check whether Jacobian matrix is still non-singular
if (!A->isFinite ())
{
// messagefcn (LOG_ERROR, "ERROR: %s: Jacobian singular at t = %.3e, "
// "aborting %s analysis\n", getName (), (double) current,
// getDescription ());
return -1;
}
return 0;
}
int main(int argc, char* argv[]) {
// Let us make libsvm quiet
gaml::libsvm::quiet();
// random seed initialization
std::srand(std::time(0));
try {
// Let us collect samples.
DataSet basis;
basis.resize(100);
for(auto& data : basis) {
double theta = gaml::random::uniform(0,2*M_PI);
data = Data(theta,oracle(theta));
}
// Let us set configure a svm
struct svm_parameter params;
gaml::libsvm::init(params);
params.kernel_type = RBF; // RBF kernel
params.gamma = 1; // k(u,v) = exp(-gamma*(u-v)^2)
params.svm_type = EPSILON_SVR;
params.p = .01; // epsilon
params.C = 10;
params.eps = 1e-10; // numerical tolerence
// This sets up a svm learning algorithm for predicting a scalar.
auto scalar_learner = gaml::libsvm::supervized::learner<double,double>(params, nb_nodes_of, fill_nodes);
// Let us use it for learning x, y and z.
auto learner = gaml::multidim::learner<Point,3>(scalar_learner, array_of_output, output_of_array);
// Let us train it and get some predictor f. f is a function, as the oracle.
std::cout << "Learning..." << std::endl;
auto f = learner(basis.begin(), basis.end(), input_of, output_of);
// Let us retrieve the three SVMs in order to save each one.
auto f_predictors = f.predictors();
std::array<std::string,3> filenames = {{std::string("x.pred"),"y.pred","z.pred"}};
auto name_iter = filenames.begin();
for(auto& pred : f_predictors) pred.save_model(*(name_iter++));
// We can compute the empirical risk with gaml tools.
auto evaluator = gaml::risk::empirical(Loss());
double risk = evaluator(f, basis.begin(), basis.end(), input_of, output_of);
std::cout << "Empirical risk : " << risk << std::endl
<< " sqrt(risk) : " << sqrt(risk) << std::endl;
// Let us load our predictor. We need first to gather each loaded
// scalar predictor in a collection...
std::list<gaml::libsvm::Predictor<double,double>> predictors;
name_iter= filenames.begin();
for(unsigned int dim = 0; dim < 3; ++dim) {
gaml::libsvm::Predictor<double,double> predictor(nb_nodes_of, fill_nodes);
predictor.load_model(*(name_iter++));
predictors.push_back(predictor);
}
// ... and create the 3D predictor (variable g) from the
// collection of scalar predictors.
gaml::multidim::Predictor<Point,
gaml::libsvm::Predictor<double,double>,
3> g(output_of_array, predictors.begin(), predictors.end());
// let us gnuplot what we have.
std::ofstream dataset_file("dataset.data");
for(auto& data : basis)
dataset_file << data.first << ' ' << data.second.x << ' ' << data.second.y << ' ' << data.second.z << std::endl;
dataset_file.close();
std::ofstream prediction_file("prediction.data");
for(double theta = 0; theta <= 2*M_PI; theta += .01) {
Point p = g(theta);
prediction_file << theta << ' ' << p.x << ' ' << p.y << ' ' << p.z << std::endl;
}
prediction_file.close();
std::ofstream gnuplot_file_3d("3D.plot");
gnuplot_file_3d << "set ticslevel 0" << std::endl
<< "set xlabel 'x(theta)'" << std::endl
<< "set ylabel 'y(theta)'" << std::endl
<< "set zlabel 'z(theta)' " << std::endl
<< "splot 'dataset.data' using 2:3:4 with points notitle, "
<< "'prediction.data' using 2:3:4 with lines notitle" << std::endl;
gnuplot_file_3d.close();
std::cout << std::endl
<< "Try 'gnuplot -p 3D.plot'" << std::endl;
std::ofstream gnuplot_file_1d("1D-x.plot");
gnuplot_file_1d << "set xlabel 'theta'" << std::endl
<< "set ylabel 'x(theta)'" << std::endl
<< "plot 'dataset.data' using 1:2 with points notitle, "
<< "'prediction.data' using 1:2 with lines notitle" << std::endl;
gnuplot_file_1d.close();
std::cout << "Try 'gnuplot -p 1D-x.plot'" << std::endl
<< std::endl;
}
catch(gaml::exception::Any& e) {
std::cout << e.what() << std::endl;
}
return 0;
}
void Cconfig::iterate(double time_step)
{
dt=time_step;
dt_on_2 = dt/2.;
dt2_on_2=dt*dt/2.;
predictor(); //motion integration
update_contact(); //find contact and get the force and torque
sum_force(); //sum the force moment of each contact on particle
if(simule_thermal_conduction) sum_heat(); //Heat transfer
if(LIQUID_TRANSFER) liquid_transfer();
// water input, controlling water volume
if(LIQUID_TRANSFER)
{
if(dt==0) flag_wetting = true;
for(int ip=0; ip< P.size(); ip++)
{
// initial
if(dt==0) {P[ip].water_volume = parameter.INITIAL_SATURATION * P[ip].void_volume;
P[ip].water_volume_old = P[ip].water_volume; }
if(P[ip].void_volume <= 1e-3 * P[ip].grain_volume)
P[ip].void_volume = 1e-3 * P[ip].grain_volume; // avoid negative Void volume
P[ip].saturation = P[ip].water_volume/P[ip].void_volume; // update the saturation of each cell
if(P[ip].saturation > MAX_SATURATION) P[ip].saturation = MAX_SATURATION; //exp
}}
if(!LIQUID_TRANSFER){ // for pre-packing stage
for(int ip=0; ip< P.size(); ip++)
{
P[ip].saturation = 0.1;
P[ip].water_volume = 0.1 * P[ip].void_volume; }}
cell.rigid_velocity *= 0.0;
corrector(); //acceleration of particles according to the sum of force/moment they experience
cell.rigid_velocity /= parameter.total_mass;
// Velocity offset by rigid motion
//#pragma omp parallel for num_threads(NTHREADS) // YG, MPI
if(cell.boundary == "PERIODIC_SHEAR" ||
cell.boundary == "PERIODIC_TILT" ||
cell.boundary == "PERIODOC_BOX" ||
cell.boundary == "PERIODOC_BOX_XYZ")
{for(int ip=0; ip< P.size(); ip++)
P[ip].V -= cell.rigid_velocity;}
// if(dt==0) {
// for(int ip=0; ip< P.size(); ip++) {
// P[ip].V *= 0.0; P[ip].Ome *= 0.0;
// }}
for(int ip=0; ip< P.size(); ip++) {
P[ip].V *= (1.0 - GLOBAL_DAMPING*dt); // Global damping
P[ip].Ome *= (1.0 - GLOBAL_DAMPING*dt);
}
// calculation of global and local water pressure.
for(int ip=0; ip< P.size(); ip++) {
P[ip].water_pressure = 0.0;}
if(LIQUID_TRANSFER){
for(int ic=0;ic<C.size();ic++) if(C[ic].fcap >0) {
P[C[ic].A].water_pressure -= C[ic].fcap * C[ic].dx * P[C[ic].A].R /(P[C[ic].A].R+P[C[ic].B].R);
if(C[ic].B >=0)
P[C[ic].B].water_pressure -= C[ic].fcap * C[ic].dx * P[C[ic].B].R /(P[C[ic].A].R+P[C[ic].B].R);
}
for(int ip=0; ip< P.size(); ip++) {
// Modified effective stress term with the degree of saturation (micro-scale).
if(P[ip].voronoi_volume>1.0e-10) P[ip].water_pressure /= (3.0 *P[ip].voronoi_volume);
P[ip].positive_pressure = 0.0;
if(P[ip].saturation > MAX_SATURATION_AIR && P[ip].saturation < 1.0) {
P[ip].positive_pressure =
AIR_K *(P[ip].saturation - MAX_SATURATION_AIR)/(1.0 - P[ip].saturation); // positive pressure
P[ip].water_pressure += P[ip].positive_pressure*1.0; // air compression the whole cell experiencing the pressure
}
if(P[ip].saturation>=1.0e-10) P[ip].water_pressure /= P[ip].saturation;
}
//Overall saturation and pressure
saturation = 0.0;
cap_pressure = 0.0;
double void_volume=0.0;
double water_volume=0.0;
double total_volume_mid = 0.0;
cap_pressure_mid = 0.0;
double void_volume_mid=0.0;
double water_volume_mid=0.0;
for(int ip=0; ip< P.size(); ip++){
void_volume += P[ip].void_volume;
water_volume += P[ip].water_volume;
// Modified effective stress term with the degree of saturation (macro-scale).
// if(P[ip].saturation <= 1.0) cap_pressure -= P[ip].water_pressure * P[ip].water_volume; // modified cap pressure
// else cap_pressure -= P[ip].water_pressure * P[ip].void_volume;
cap_pressure -= P[ip].water_pressure * P[ip].saturation *P[ip].voronoi_volume;
if(P[ip].X.x[1] <= 5.0 && P[ip].X.x[1] >= -5.0){
void_volume_mid += P[ip].void_volume;
water_volume_mid += P[ip].water_volume;
cap_pressure_mid -= P[ip].water_pressure * P[ip].saturation *P[ip].voronoi_volume;
total_volume_mid += P[ip].voronoi_volume;
}
}
saturation = water_volume / void_volume;
if(saturation < 1.0e-10) saturation = 1.e-10;
double total_volume = cell.L.x[0]* cell.L.x[1]* cell.L.x[2];
cap_pressure /= saturation * total_volume;
water_content = water_volume /total_volume;
saturation_mid = water_volume_mid / void_volume_mid;
if(saturation_mid<1.0e-10) saturation_mid = 1.e-10;
if(total_volume_mid > 0.0){
cap_pressure_mid /= saturation_mid * total_volume_mid;
water_content_mid = water_volume_mid /total_volume_mid;
}
if(saturation >= MAX_SCAN && t>1.0 && flag_wetting) flag_wetting=false;
if(saturation <= MIN_SCAN && t>1.0 && !flag_wetting) flag_wetting=true;
}
}
double trajOptimizerplus::eval(vector<double> ¶ms, vector<double> &gradient) {
cout << "IN EVAL "<<params.size()<<endl;
for (int i=0; i < params.size(); i++)
cout << "PARAMS IN: "<<i<<" "<<params.at(i)<<endl;
int factor = evidence.getFactor();
pair<int, int> dims = grid.dims();
int v_dim = seqFeat.num_V();
/*
pair<int, int> lowDims((int)ceil((float)dims.first/factor),
(int)ceil((float)dims.second/factor));
*/
vector<vector<vector<double> > >
prior(dims.first, vector<vector<double> >(dims.second,
vector<double> (v_dim,-HUGE_VAL)));
double obj = 0.0;
gradient.clear();
gradient.resize(params.size(), 0.0);
vector<vector<vector<double> > > occupancy;
vector<vector<double> > layerOccupancy;
layerOccupancy.resize(dims.first,vector<double>(dims.second,-HUGE_VAL));
vector<double> modelFeats, pathFeats;
for (int i=0; i < evidence.size(); i++) {
for (int j=0; j < params.size(); j++){
cout << " "<<j<<" "<<params.at(j);
}
cout<<endl;
cout << "Evidence #"<<i<<endl;
vector<pair<int, int> >& trajectory = evidence.at(i);
vector<double>& velocityseq = evidence.at_v(i);
pair<int,int>& bot = evidence.at_bot(i);
// robot local blurres features
for (int r=1; r <= NUMROBFEAT; r++) {
cout << "Adding Robot Feature "<<r<<endl;
RobotLocalBlurFeature robblurFeat(grid,bot,10*r);
// RobotGlobalFeature robFeat(grid,bot);
posFeatures.push_back(robblurFeat);
}
cout << " Creating feature array"<<endl;
FeatureArray featArray2(posFeatures);
FeatureArray featArray(featArray2, factor);
for (int rr=1;rr<= NUMROBFEAT;rr++)
posFeatures.pop_back();
// split different posfeatures and seqfeature weights
vector<double> p_weights,s_weights;
int itr = 0;
for (;itr<featArray.size();itr++)
p_weights.push_back(params[itr]);
for (;itr<params.size();itr++)
s_weights.push_back(params[itr]);
//cout<<"Params"<<endl;
Parameters p_parameters(p_weights), s_parameters(s_weights);
/* cout<<featArray.size()<<endl;
cout<<params.size()<<endl;
cout<<p_weights.size()<<endl;
cout<<s_weights.size()<<endl;
cout<<p_parameters.size()<<endl;
cout<<s_parameters.size()<<endl;
*/
//cout<<"Reward"<<endl;
RewardMap rewards(featArray,seqFeat,p_parameters,s_parameters);
DisSeqPredictor predictor(grid, rewards, engine);
// sum of reward along the trajectory
double cost = 0.0;
//cout<< trajectory.size()<<endl;
for (int j=0; j < trajectory.size(); j++){
//cout<<j<<" "<<trajectory.at(j).first<<" "<< trajectory.at(j).second<< " "<< seqFeat.getFeat(velocityseq.at(j))<<endl;
cost+=rewards.at(trajectory.at(j).first, trajectory.at(j).second, seqFeat.getFeat(velocityseq.at(j)));
}
State initial(trajectory.front(),seqFeat.getFeat(velocityseq.front()));
State destination(trajectory.back(),seqFeat.getFeat(velocityseq.back()));
//for (int k=0;k<v_dim;k++)
prior.at(destination.x()).at(destination.y()).at(destination.disV) = 0.0;
cout << "Initial: "<<initial.x()<<" "<<initial.y()<<" "<<initial.disV<<endl;
cout << "Destination: "<<destination.x()<<" "
<<destination.y()<<" "<<destination.disV<<endl;
predictor.setStart(initial);
predictor.setPrior(prior);
double norm = predictor.forwardBackwardInference(initial, occupancy);
for (int l=0;l<v_dim;l++){
BMPFile gridView(dims.first, dims.second);
for (int x= 0;x<dims.first;x++){
for(int y=0;y<dims.second;y++){
layerOccupancy.at(x).at(y) = occupancy.at(x).at(y).at(l);
}
}
char buf[1024];
/*
RobotGlobalFeature robblurFeat(grid,bot);
gridView.addBelief(robblurFeat.getMap(), 0.0, 25, white, red);
gridView.addVector(trajectory, blue, factor);
gridView.addLabel(bot,green);
sprintf(buf, "../figures/feat%04d_%d.bmp",i,l);
gridView.write(buf);
*/
gridView.addBelief(layerOccupancy, -300.0, 5.0, white, red);
//grid.addObstacles(gridView, black);
gridView.addLabel(bot,green);
gridView.addVector(trajectory, blue, factor);
sprintf(buf, "../figures/train%04d_%d.bmp",i,l);
gridView.write(buf);
}
/*
for (int i=0; i < occupancy.size(); i++)
for (int j=0; j < occupancy.at(i).size(); j++)
if (occupancy.at(i).at(j) > -10)
cout << i <<" "<<j<<" "<<occupancy.at(i).at(j)<<endl;
*/
featArray.featureCounts(occupancy, modelFeats);
featArray.featureCounts(trajectory, pathFeats);
seqFeat.featureCounts_vec(occupancy,modelFeats);
seqFeat.featureCounts_vec(velocityseq,pathFeats);
for (int k=0; k < params.size(); k++) {
double diff = pathFeats.at(k) - modelFeats.at(k);
gradient.at(k) -= diff;
cout <<" Gradient ("<< k << " -grad: "<< gradient.at(k) <<" -path: "<<
pathFeats.at(k)<<" -model: "<< modelFeats.at(k)<<")";
}
cout<<endl;
cout << "OBJ: "<<cost-norm<< " "<<cost<<" "<<norm<<endl;
obj += (cost - norm);
/* obj is the path probability
* cost is the sum of rewards: sum f(s,a)
* norm is V(s_1->G), since here s_T = G, V(s_T->G) = 0*/
prior.at(destination.x()).at(destination.y()).at(destination.disV)
= -HUGE_VAL;
}
cout << "RETURN OBJ: "<<-obj<<endl;
return -obj;
}
double trajectoryOptimizer::eval(vector<double> ¶ms, vector<double> &gradient) {
cout << "IN EVAL "<<params.size()<<endl;
for (int i=0; i < params.size(); i++)
cout << "PARAMS IN: "<<i<<" "<<params.at(i)<<endl;
int factor = evidence.getFactor();
// cout << "FACTOR: "<<factor<<endl;
FeatureArray featArray2(features);
FeatureArray featArray(featArray2, factor);
//cout<<"Dims featarray "<<featArray.dims().first<<" "<<featArray.dims().second<<endl;
Parameters parameters(params);
//cout << "Calculating rewards"<<endl;
RewardMap rewards(featArray, parameters);
pair<int, int> dims = grid.dims();
BMPFile gridView(dims.first, dims.second);
pair<int, int> lowDims((int)ceil((float)dims.first/factor),
(int)ceil((float)dims.second/factor));
//cout << "Computing prior"<<endl;
vector<vector<double> > prior(lowDims.first, vector<double>(lowDims.second,
-HUGE_VAL));
double obj = 0.0;
gradient.clear();
gradient.resize(params.size(), 0.0);
for (int i=0; i < evidence.size(); i++) {
Predictor predictor(grid, rewards, engine);
cout << "Evidence #"<<i<<endl;
vector<pair<int, int> > trajectory = evidence.at(i);
double cost = 0.0;
for (int j=0; j < trajectory.size(); j++){
double temp = rewards.at(trajectory.at(j).first,
trajectory.at(j).second);
cost += temp;
}
pair<int, int> initial = trajectory.front();
pair<int, int> destination = trajectory.back();
prior.at(destination.first).at(destination.second) = 0.0;
#if 0
cout << "Initial: "<<initial.first<<" "<<initial.second<<endl;
cout << "Destination: "<<destination.first<<" "
<<destination.second<<endl;
#endif
predictor.setStart(initial);
predictor.setPrior(prior);
vector<vector<double> > occupancy;
double norm = predictor.predict(initial, occupancy);
gridView.addBelief(occupancy, -300.0, 0.0, white, red);
gridView.addVector(trajectory, blue, factor);
char buf[1024];
sprintf(buf, "../figures/train%04d.bmp", i);
gridView.write(buf);
vector<double> modelFeats, pathFeats;
//cout << "Computing feature counts"<<endl;
/*
for (int i=0; i < occupancy.size(); i++)
for (int j=0; j < occupancy.at(i).size(); j++)
if (occupancy.at(i).at(j) > -10)
cout << i <<" "<<j<<" "<<occupancy.at(i).at(j)<<endl;
*/
featArray.featureCounts(occupancy, modelFeats);
featArray.featureCounts(trajectory, pathFeats);
cout << "GRADIENT"<<endl;
for (int k=0; k < params.size(); k++) {
double diff = pathFeats.at(k) - modelFeats.at(k);
gradient.at(k) -= diff;
cout << k << ": " << gradient.at(k) << " " << pathFeats.at(k)
<< " " << modelFeats.at(k) <<endl;
}
cout << "OBJ: "<<cost-norm<<endl;
cout << " "<<cost<<" "<<norm<<endl;
obj += (cost - norm);
prior.at(destination.first).at(destination.second) = -HUGE_VAL;
}
cout << "RETURN OBJ: "<<-obj<<endl;
return -obj;
}
