EReturn OMPLsolver::Solve(Eigen::VectorXdRefConst q0,
Eigen::MatrixXd & solution)
{
ros::Time startTime = ros::Time::now();
finishedSolving_ = false;
ompl::base::ScopedState<> ompl_start_state(state_space_);
if (ok(
compound_ ?
boost::static_pointer_cast<OMPLSE3RNCompoundStateSpace>(
state_space_)->EigenToOMPLState(q0, ompl_start_state.get()) :
boost::static_pointer_cast<OMPLStateSpace>(state_space_)->copyToOMPLState(
ompl_start_state.get(), q0)))
{
if (compound_)
boost::static_pointer_cast<OMPLSE3RNCompoundStateSpace>(state_space_)->setStart(
q0);
ompl_simple_setup_->setStartState(ompl_start_state);
preSolve();
// Solve here
ompl::time::point start = ompl::time::now();
ob::PlannerTerminationCondition ptc =
ob::timedPlannerTerminationCondition(
timeout_ - ompl::time::seconds(ompl::time::now() - start));
registerTerminationCondition(ptc);
if (ompl_simple_setup_->solve(ptc)
== ompl::base::PlannerStatus::EXACT_SOLUTION)
{
double last_plan_time_ =
ompl_simple_setup_->getLastPlanComputationTime();
unregisterTerminationCondition();
finishedSolving_ = true;
if (!ompl_simple_setup_->haveSolutionPath()) return FAILURE;
planning_time_ = ros::Time::now() - startTime;
getSimplifiedPath(ompl_simple_setup_->getSolutionPath(), solution, ptc);
planning_time_ = ros::Time::now() - startTime;
if (saveResults_->data) recordData();
postSolve();
succ_cnt_++;
return SUCCESS;
}
else
{
finishedSolving_ = true;
planning_time_ = ros::Time::now() - startTime;
if (saveResults_->data) recordData();
postSolve();
return FAILURE;
}
}
else
{
ERROR("Can't copy start state!");
planning_time_ = ros::Time::now() - startTime;
return FAILURE;
}
}
void ContactListener::PostSolve(b2Contact *contact, const b2ContactImpulse *impulse)
{
if (postSolve != NULL)
{
postSolve(contact, impulse);
}
}
void
Steady::execute()
{
if (_app.isRecovering())
return;
preExecute();
_problem.advanceState();
// first step in any steady state solve is always 1 (preserving backwards compatibility)
_time_step = 1;
_time = _time_step; // need to keep _time in sync with _time_step to get correct output
#ifdef LIBMESH_ENABLE_AMR
// Define the refinement loop
unsigned int steps = _problem.adaptivity().getSteps();
for (unsigned int r_step=0; r_step<=steps; r_step++)
{
#endif //LIBMESH_ENABLE_AMR
preSolve();
_problem.timestepSetup();
_problem.execute(EXEC_TIMESTEP_BEGIN);
_problem.outputStep(EXEC_TIMESTEP_BEGIN);
// Update warehouse active objects
_problem.updateActiveObjects();
_problem.solve();
postSolve();
if (!lastSolveConverged())
{
_console << "Aborting as solve did not converge\n";
break;
}
_problem.onTimestepEnd();
_problem.execute(EXEC_TIMESTEP_END);
_problem.computeIndicatorsAndMarkers();
_problem.outputStep(EXEC_TIMESTEP_END);
#ifdef LIBMESH_ENABLE_AMR
if (r_step != steps)
{
_problem.adaptMesh();
}
_time_step++;
_time = _time_step; // need to keep _time in sync with _time_step to get correct output
}
#endif
postExecute();
}
void
Steady::execute()
{
if (_app.isRecovering())
return;
if (_app.hasLegacyOutput())
Moose::out << "Time: " << _time_step << '\n';
preExecute();
// first step in any steady state solve is always 1 (preserving backwards compatibility)
_time_step = 1;
_time = _time_step; // need to keep _time in sync with _time_step to get correct output
#ifdef LIBMESH_ENABLE_AMR
// Define the refinement loop
unsigned int steps = _problem.adaptivity().getSteps();
for(unsigned int r_step=0; r_step<=steps; r_step++)
{
#endif //LIBMESH_ENABLE_AMR
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::PRE_AUX);
preSolve();
_problem.updateMaterials();
_problem.timestepSetup();
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::POST_AUX);
_problem.solve();
postSolve();
_problem.computeUserObjects(EXEC_TIMESTEP, UserObjectWarehouse::PRE_AUX);
_problem.onTimestepEnd();
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP);
_problem.computeUserObjects(EXEC_TIMESTEP, UserObjectWarehouse::POST_AUX);
_problem.computeIndicatorsAndMarkers();
_output_warehouse.outputStep();
_problem.output();
_problem.outputPostprocessors();
_problem.outputRestart();
#ifdef LIBMESH_ENABLE_AMR
if (r_step != steps)
{
_problem.adaptMesh();
}
_time_step++;
_time = _time_step; // need to keep _time in sync with _time_step to get correct output
}
#endif
postExecute();
}
void
NonlinearEigen::takeStep()
{
_console << " Nonlinear iteration starts" << std::endl;
// nonlinear solve
preSolve();
_problem.timestepSetup();
_problem.advanceState();
_problem.execute(EXEC_TIMESTEP_BEGIN);
nonlinearSolve(_rel_tol, _abs_tol, _pfactor, _eigenvalue);
postSolve();
printEigenvalue();
_problem.onTimestepEnd();
_problem.execute(EXEC_TIMESTEP_END);
}
void
PetscTSExecutioner::execute()
{
TimeStepperStatus status = STATUS_ITERATING;
Real ftime = -1e100;
_fe_problem.initialSetup();
Moose::setup_perf_log.push("Output Initial Condition","Setup");
if (_output_initial)
{
_fe_problem.output();
_fe_problem.outputPostprocessors();
_problem.outputRestart();
}
Moose::setup_perf_log.pop("Output Initial Condition","Setup");
_time_stepper->setup(*_fe_problem.getNonlinearSystem().sys().solution);
preExecute();
// Start time loop...
while (keepGoing(status,ftime))
{
status = _time_stepper->step(&ftime);
_fe_problem.getNonlinearSystem().update();
_fe_problem.getNonlinearSystem().setSolution(*_fe_problem.getNonlinearSystem().sys().current_local_solution);
postSolve();
_fe_problem.onTimestepEnd();
_fe_problem.computeUserObjects();
bool reset_dt = false; /* has some meaning in Transient::computeConstrainedDT, but we do not use that logic here */
_fe_problem.output(reset_dt);
_fe_problem.outputPostprocessors(reset_dt);
_problem.outputRestart(reset_dt);
#ifdef LIBMESH_ENABLE_AMR
if (_fe_problem.adaptivity().isOn())
{
_fe_problem.adaptMesh();
}
#endif
}
postExecute();
}
void
InversePowerMethod::takeStep()
{
// save the initial guess and mark a new time step
_problem.advanceState();
preSolve();
Real initial_res;
inversePowerIteration(_min_iter, _max_iter, _pfactor, _cheb_on, _eig_check_tol, true,
_solution_diff_name, _sol_check_tol,
_eigenvalue, initial_res);
postSolve();
if (lastSolveConverged())
{
printEigenvalue();
_problem.onTimestepEnd();
_problem.execute(EXEC_TIMESTEP_END);
}
}
void
NonlinearEigen::takeStep()
{
_console << " Nonlinear iteration starts" << std::endl;
// nonlinear solve
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::PRE_AUX);
preSolve();
_problem.timestepSetup();
_problem.advanceState();
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::POST_AUX);
nonlinearSolve(_rel_tol, _abs_tol, _pfactor, _eigenvalue);
postSolve();
printEigenvalue();
_problem.computeUserObjects(EXEC_TIMESTEP_END, UserObjectWarehouse::PRE_AUX);
_problem.onTimestepEnd();
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP_END);
_problem.computeUserObjects(EXEC_TIMESTEP_END, UserObjectWarehouse::POST_AUX);
}
bool QPSolver::solve(VectorXd& sol)
{
bool ret = false;
#ifdef _WIN32
VectorXd solution;
preSolve();
convertMatrixVectorFormat();
MSKenv_t env;
MSKtask_t task;
MSKrescodee r;
r = MSK_makeenv(&env, NULL, NULL, NULL, NULL);
if (r == MSK_RES_OK)
{
r = MSK_linkfunctoenvstream(env, MSK_STREAM_LOG, NULL, printstr);
}
r = MSK_initenv(env);
if (r == MSK_RES_OK)
{
r = MSK_maketask(env, mNumCon, mNumVar, &task);
if (r == MSK_RES_OK)
{
r = MSK_linkfunctotaskstream(task, MSK_STREAM_LOG, NULL, printstr);
}
if (r == MSK_RES_OK)
r = MSK_putmaxnumvar(task, mNumVar);
if (r == MSK_RES_OK)
r = MSK_putmaxnumcon(task, mNumCon);
/* Append ¡¯NUMCON ¡¯ empty constraints .
The constraints will initially have no bounds . */
if (r == MSK_RES_OK)
r = MSK_append(task, MSK_ACC_CON, mNumCon);
/* Append ¡¯NUMVAR ¡¯ variables .
The variables will initially be fixed at zero (x =0). */
if (r == MSK_RES_OK)
r = MSK_append(task, MSK_ACC_VAR, mNumVar);
/* Optionally add a constant term to the objective . */
if (r == MSK_RES_OK)
r = MSK_putcfix(task, mConstant);
for (int j = 0; j < mNumVar && r == MSK_RES_OK; ++j)
{
/* Set the linear term c_j in the objective .*/
if (r == MSK_RES_OK)
r = MSK_putcj(task, j, mCU[j]);
/* Set the bounds on variable j.*/
if (r == MSK_RES_OK)
{
if (mbLowerBounded[j] && mbUpperBounded[j])
{
if (mlb[j] == mub[j])
r = MSK_putbound(task, MSK_ACC_VAR, j, MSK_BK_FX, mlb[j], mub[j]);
else
{
CHECK(mlb[j] < mub[j]);
r = MSK_putbound(task, MSK_ACC_VAR, j, MSK_BK_RA, mlb[j], mub[j]);
}
}
else if (mbLowerBounded[j])
{
r = MSK_putbound(task, MSK_ACC_VAR, j , MSK_BK_LO, mlb[j], +MSK_INFINITY);
}
else if (mbUpperBounded[j])
{
r = MSK_putbound(task, MSK_ACC_VAR, j, MSK_BK_UP, -MSK_INFINITY, mub[j]);
}
else
{
r = MSK_putbound(task, MSK_ACC_VAR, j, MSK_BK_FR, -MSK_INFINITY, +MSK_INFINITY);
}
}
/* Input column j of A */
if (r == MSK_RES_OK && mNumCon)
{
int currentColumnIdx = mAColumnStartIdx[j];
int nextColumnIdx = mAColumnStartIdx[j + 1];
r = MSK_putavec(task, MSK_ACC_VAR, j, nextColumnIdx - currentColumnIdx, &(mARowIdx[currentColumnIdx]), &(mAValues[currentColumnIdx]));
}
}
/* Set the bounds on constraints .
for i=1, ... , NUMCON : blc [i] <= constraint i <= buc [i] */
for (int i = 0; i < mNumCon && r == MSK_RES_OK; ++i)
{
if (mbConstraintLowerBounded[i] && mbConstraintUpperBounded[i])
{
if (mlbc[i] == mubc[i])
{
r = MSK_putbound(task, MSK_ACC_CON, i, MSK_BK_FX, mlbc[i], mubc[i]);
}
else
{
r = MSK_putbound(task, MSK_ACC_CON, i, MSK_BK_RA, mlbc[i], mubc[i]);
}
}
else if (mbConstraintLowerBounded[i])
{
r = MSK_putbound(task, MSK_ACC_CON, i, MSK_BK_LO, mlbc[i], +MSK_INFINITY);
}
else if (mbConstraintUpperBounded[i])
{
r = MSK_putbound(task, MSK_ACC_CON, i, MSK_BK_UP, -MSK_INFINITY, mubc[i]);
}
else
{
LOG(WARNING) << "Every constraint should not be free.";
}
}
if (r == MSK_RES_OK)
{
/* Input the Q for the objective . */
r = MSK_putqobj(task, mQValues.size(), &(mQSubi[0]), &(mQSubj[0]), &(mQValues[0]));
}
if (r == MSK_RES_OK)
{
MSKrescodee trmcode;
r = MSK_optimizetrm(task, &trmcode);
MSK_solutionsummary(task, MSK_STREAM_LOG);
if (r == MSK_RES_OK)
{
MSKsolstae solsta;
MSK_getsolutionstatus(task, MSK_SOL_ITR, NULL, &solsta);
double* result = new double[mNumVar];
switch (solsta)
{
case MSK_SOL_STA_OPTIMAL:
case MSK_SOL_STA_NEAR_OPTIMAL:
MSK_getsolutionslice(task, MSK_SOL_ITR, MSK_SOL_ITEM_XX, 0, mNumVar, result);
LOG(INFO) << "Optimal primal solution";
ret = true;
solution = VectorXd::Zero(mNumVar);
for (int k = 0; k < mNumVar; ++k)
solution[k] = result[k];
break;
case MSK_SOL_STA_DUAL_INFEAS_CER:
case MSK_SOL_STA_PRIM_INFEAS_CER:
case MSK_SOL_STA_NEAR_DUAL_INFEAS_CER:
case MSK_SOL_STA_NEAR_PRIM_INFEAS_CER:
LOG(WARNING) << "Primal or dual infeasibility certificate found.";
break;
case MSK_SOL_STA_UNKNOWN:
LOG(WARNING) << "The status of the solution could not be determined.";
break;
default:
LOG(WARNING) << "Other solution status.";
break;
}
delete[] result;
}
}
else
{
LOG(WARNING) << "Error while optimizing.";
}
if (r != MSK_RES_OK)
{
char symname[MSK_MAX_STR_LEN];
char desc[MSK_MAX_STR_LEN];
LOG(WARNING) << "An error occurred while optimizing.";
MSK_getcodedesc(r, symname, desc);
LOG(WARNING) << "Error " << symname << " - " << desc;
}
}
MSK_deletetask(&task);
MSK_deleteenv(&env);
postSolve(solution, ret, sol);
#endif
return ret;
}
void
Transient::solveStep(Real input_dt)
{
_dt_old = _dt;
if (input_dt == -1.0)
_dt = computeConstrainedDT();
else
_dt = input_dt;
Real current_dt = _dt;
_problem.onTimestepBegin();
// Increment time
_time = _time_old + _dt;
_problem.execTransfers(EXEC_TIMESTEP_BEGIN);
_problem.execMultiApps(EXEC_TIMESTEP_BEGIN, _picard_max_its == 1);
preSolve();
_time_stepper->preSolve();
_problem.timestepSetup();
// Compute Pre-Aux User Objects (Timestep begin)
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::PRE_AUX);
// Compute TimestepBegin AuxKernels
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP_BEGIN);
// Compute Post-Aux User Objects (Timestep begin)
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::POST_AUX);
// Perform output for timestep begin
_problem.outputStep(EXEC_TIMESTEP_BEGIN);
if (_picard_max_its > 1)
{
Real current_norm = _problem.computeResidualL2Norm();
if (_picard_it == 0) // First Picard iteration - need to save off the initial nonlinear residual
{
_picard_initial_norm = current_norm;
_console << "Initial Picard Norm: " << _picard_initial_norm << '\n';
}
else
_console << "Current Picard Norm: " << current_norm << '\n';
Real relative_drop = current_norm / _picard_initial_norm;
if (current_norm < _picard_abs_tol || relative_drop < _picard_rel_tol)
{
_console << "Picard converged!" << std::endl;
_picard_converged = true;
_time_stepper->acceptStep();
return;
}
}
_time_stepper->step();
// We know whether or not the nonlinear solver thinks it converged, but we need to see if the executioner concurs
if (lastSolveConverged())
{
_console << COLOR_GREEN << " Solve Converged!" << COLOR_DEFAULT << std::endl;
if (_picard_max_its <= 1)
_time_stepper->acceptStep();
_solution_change_norm = _problem.solutionChangeNorm();
_problem.computeUserObjects(EXEC_TIMESTEP_END, UserObjectWarehouse::PRE_AUX);
#if 0
// User definable callback
if (_estimate_error)
estimateTimeError();
#endif
_problem.onTimestepEnd();
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP_END);
_problem.computeUserObjects(EXEC_TIMESTEP_END, UserObjectWarehouse::POST_AUX);
_problem.execTransfers(EXEC_TIMESTEP_END);
_problem.execMultiApps(EXEC_TIMESTEP_END, _picard_max_its == 1);
}
else
{
_console << COLOR_RED << " Solve Did NOT Converge!" << COLOR_DEFAULT << std::endl;
// Perform the output of the current, failed time step (this only occurs if desired)
_problem.outputStep(EXEC_FAILED);
}
postSolve();
_time_stepper->postSolve();
_dt = current_dt; // _dt might be smaller than this at this point for multistep methods
_time = _time_old;
}
void
Transient::takeStep(Real input_dt)
{
_problem.out().setOutput(false);
_dt_old = _dt;
if (input_dt == -1.0)
_dt = computeConstrainedDT();
else
_dt = input_dt;
_problem.onTimestepBegin();
if (lastSolveConverged())
_problem.updateMaterials(); // Update backward material data structures
// Increment time
_time = _time_old + _dt;
_problem.execTransfers(EXEC_TIMESTEP_BEGIN);
_problem.execMultiApps(EXEC_TIMESTEP_BEGIN);
// Only print this if the 'Output' block exists
/// \todo{Remove when old output system is removed}
if (_app.hasLegacyOutput())
{
Moose::out << "\nTime Step ";
{
std::ostringstream out;
out << std::setw(2)
<< _t_step
<< ", time = "
<< std::setw(9)
<< std::setprecision(6)
<< std::setfill('0')
<< std::showpoint
<< std::left
<< _time;
Moose::out << out.str() << std::endl;
}
{
std::ostringstream tstepstr;
tstepstr << _t_step;
unsigned int tsteplen = tstepstr.str().size();
if (tsteplen < 2)
tsteplen = 2;
std::ostringstream out;
if (_verbose)
{
out << std::setw(tsteplen)
<< " old time = "
<< std::setw(9)
<< std::setprecision(6)
<< std::setfill('0')
<< std::showpoint
<< std::left
<< _time_old
<< std::endl;
}
out << std::setw(tsteplen)
<<" dt = "
<< std::setw(9)
<< std::setprecision(6)
<< std::setfill('0')
<< std::showpoint
<< std::left
<< _dt;
Moose::out << out.str() << std::endl;
}
}
preSolve();
_time_stepper->preSolve();
_problem.timestepSetup();
// Compute Pre-Aux User Objects (Timestep begin)
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::PRE_AUX);
// Compute TimestepBegin AuxKernels
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP_BEGIN);
// Compute Post-Aux User Objects (Timestep begin)
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::POST_AUX);
_time_stepper->step();
// We know whether or not the nonlinear solver thinks it converged, but we need to see if the executioner concurs
if (lastSolveConverged())
{
Moose::out << "Solve Converged!" << std::endl;
_time_stepper->acceptStep();
_solution_change_norm = _problem.solutionChangeNorm();
_problem.computeUserObjects(EXEC_TIMESTEP, UserObjectWarehouse::PRE_AUX);
#if 0
// User definable callback
if (_estimate_error)
{
estimateTimeError();
}
#endif
_problem.onTimestepEnd();
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP);
_problem.computeUserObjects(EXEC_TIMESTEP, UserObjectWarehouse::POST_AUX);
_problem.execTransfers(EXEC_TIMESTEP);
_problem.execMultiApps(EXEC_TIMESTEP);
}
else
Moose::out << "Solve Did NOT Converge!" << std::endl;
postSolve();
_time_stepper->postSolve();
}
void
cpSpaceStep(cpSpace *space, cpFloat dt)
{
// don't step if the timestep is 0!
if(dt == 0.0f) return;
space->stamp++;
cpFloat prev_dt = space->curr_dt;
space->curr_dt = dt;
// Reset and empty the arbiter list.
cpArray *arbiters = space->arbiters;
for(int i=0; i<arbiters->num; i++){
cpArbiter *arb = (cpArbiter *)arbiters->arr[i];
arb->state = cpArbiterStateNormal;
// If both bodies are awake, unthread the arbiter from the contact graph.
if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)){
cpArbiterUnthread(arb);
}
}
arbiters->num = 0;
// Integrate positions
cpArray *bodies = space->bodies;
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->position_func(body, dt);
}
// Find colliding pairs.
cpSpaceLock(space); {
cpSpacePushFreshContactBuffer(space);
cpSpatialIndexEach(space->activeShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL);
cpSpatialIndexReindexQuery(space->activeShapes, (cpSpatialIndexQueryFunc)collideShapes, space);
} cpSpaceUnlock(space, cpFalse);
// If body sleeping is enabled, do that now.
if(space->sleepTimeThreshold != INFINITY || space->enableContactGraph){
cpSpaceProcessComponents(space, dt);
}
// Clear out old cached arbiters and call separate callbacks
cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space);
// Prestep the arbiters and constraints.
cpFloat slop = space->collisionSlop;
cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt);
for(int i=0; i<arbiters->num; i++){
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef);
}
cpArray *constraints = space->constraints;
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
cpConstraintPreSolveFunc preSolve = constraint->preSolve;
if(preSolve) preSolve(constraint, space);
constraint->klass->preStep(constraint, dt);
}
// Integrate velocities.
cpFloat damping = cpfpow(space->damping, dt);
cpVect gravity = space->gravity;
for(int i=0; i<bodies->num; i++){
cpBody *body = (cpBody *)bodies->arr[i];
body->velocity_func(body, gravity, damping, dt);
}
// Apply cached impulses
cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt);
for(int i=0; i<arbiters->num; i++){
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef);
}
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
constraint->klass->applyCachedImpulse(constraint, dt_coef);
}
// Run the impulse solver.
for(int i=0; i<space->iterations; i++){
for(int j=0; j<arbiters->num; j++){
cpArbiterApplyImpulse((cpArbiter *)arbiters->arr[j]);
}
for(int j=0; j<constraints->num; j++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[j];
constraint->klass->applyImpulse(constraint);
}
}
// Run the constraint post-solve callbacks
for(int i=0; i<constraints->num; i++){
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
cpConstraintPostSolveFunc postSolve = constraint->postSolve;
if(postSolve) postSolve(constraint, space);
}
// run the post-solve callbacks
cpSpaceLock(space);
for(int i=0; i<arbiters->num; i++){
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
cpCollisionHandler *handler = arb->handler;
handler->postSolve(arb, space, handler->data);
}
cpSpaceUnlock(space, cpTrue);
}
void
AdaptiveTransient::takeStep(Real input_dt)
{
if (_converged)
_problem.copyOldSolutions();
else
_problem.restoreSolutions();
_dt_old = _dt;
if (input_dt == -1.0)
_dt = computeConstrainedDT();
else
_dt = input_dt;
_problem.onTimestepBegin();
if (_converged)
{
// Update backward material data structures
_problem.updateMaterials();
}
// Increment time
_time = _time_old + _dt;
// Compute TimestepBegin AuxKernels
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP_BEGIN);
// Compute TimestepBegin Postprocessors
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN);
Moose::out << "Solving time step ";
{
std::ostringstream out;
out << std::setw(2)
<< _t_step
<< ", old time="
<< std::setw(9)
<< std::setprecision(6)
<< std::setfill('0')
<< std::showpoint
<< std::left
<< _time_old
<< ", time="
<< std::setw(9)
<< std::setprecision(6)
<< std::setfill('0')
<< std::showpoint
<< std::left
<< _time
<< ", dt="
<< std::setw(9)
<< std::setprecision(6)
<< std::setfill('0')
<< std::showpoint
<< std::left
<< _dt;
Moose::out << out.str() << std::endl;
}
preSolve();
_problem.timestepSetup();
try
{
// Compute Pre-Aux User Objects (Timestep begin)
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::PRE_AUX);
// Compute TimestepBegin AuxKernels
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP_BEGIN);
// Compute Post-Aux User Objects (Timestep begin)
_problem.computeUserObjects(EXEC_TIMESTEP_BEGIN, UserObjectWarehouse::POST_AUX);
_problem.solve();
_converged = _problem.converged();
}
catch (MooseException &e)
{
_caught_exception = true;
_converged = false;
}
if (!_caught_exception)
{
// We know whether or not the nonlinear solver thinks it converged, but we need to see if the executioner concurs
bool last_solve_converged = lastSolveConverged();
if (last_solve_converged)
_problem.computeUserObjects(EXEC_TIMESTEP, UserObjectWarehouse::PRE_AUX);
postSolve();
_problem.onTimestepEnd();
// We know whether or not the nonlinear solver thinks it converged, but we need to see if the executioner concurs
if (last_solve_converged)
{
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP);
_problem.computeUserObjects();
}
Moose::out << std::endl;
}
}
void
AutoRBTransient::solveStep(Real input_dt)
{
_dt_old = _dt;
if (input_dt == -1.0)
_dt = computeConstrainedDT();
else
_dt = input_dt;
Real current_dt = _dt;
_problem.onTimestepBegin();
// Increment time
_time = _time_old + _dt;
// You could evaluate/store _his_initial_norm_old here
_problem.execTransfers(EXEC_TIMESTEP_BEGIN);
_multiapps_converged = _problem.execMultiApps(EXEC_TIMESTEP_BEGIN, _picard_max_its == 1);
if (!_multiapps_converged)
return;
preSolve();
_time_stepper->preSolve();
_problem.timestepSetup();
_problem.execute(EXEC_TIMESTEP_BEGIN);
// Perform output for timestep begin
_problem.outputStep(EXEC_TIMESTEP_BEGIN);
// Update warehouse active objects
_problem.updateActiveObjects();
_my_current_norm = _problem.computeResidualL2Norm(); // the norm from this app only
_his_initial_norm_old = _his_initial_norm;
_his_initial_norm = getPostprocessorValue("InitialResidual");
//for sub-app@timestep_begin: his_initial_norm should be zero at each timestep
// if this is sub app and its the initial_residual wasn't updated
// or use his_initial_norm_old
// On the first iteration Old should not match _his_initial if sub-app comes
// second (timestep_end).
//if (_picard_max_its==1 && _his_initial_norm == getPostprocessorValueOld("InitialResidual"))
if (_picard_max_its==1 && _his_initial_norm == _his_initial_norm_old)
_his_initial_norm = 0;
_his_final_norm = getPostprocessorValue("FinalResidual");
if (_picard_max_its > 1)
{
// is there a way to get his_final_norm without a postprocessor transfer?
//std::vector<MultiApp *> multi_apps = _problem._multi_apps(EXEC_TIMESTEP_END)[0].all();
// unfortunately _multi_apps is protected
//Real his_last_norm = multi_apps[i]._subproblem.finalNonlinearResidual();
//_console << "Multi-app end residual" << his_final_norm << std::endl; // did it work?
if (_picard_it == 0) // First Picard iteration - need to save off the initial nonlinear residual
{
_current_norm = _my_current_norm;
//his_initial_norm_old) //EXEC_TIMESTEP_END
//if (_his_initial_norm == getPostprocessorValueOld("InitialResidual"))
if (_his_initial_norm == _his_initial_norm_old)
_his_initial_norm = 0;
// set his_initial_norm to 0 so that we can start a new timestep properly
_picard_initial_norm = _current_norm + _his_initial_norm;
//_console << "Initial Picard Norm: " << _picard_initial_norm << '\n';
if (_his_initial_norm == 0)
_adjust_initial_norm = true;
_spectral_radius = pow(_new_tol_mult, 0.5);//*#*//
}
else
{
_current_norm_old = _current_norm;
_current_norm = _my_current_norm + _his_final_norm;
//@^&// These changes smooth out the change in _spectral_radius
//@^&//_spectral_radius = _current_norm / _current_norm_old;
//@^&//_spectral_radius = 0.5 * (_current_norm / _current_norm_old + _spectral_radius);
_spectral_radius = pow(_current_norm / _current_norm_old * _spectral_radius, 0.5);
_console << "Current Picard Norm: " << _current_norm << '\n';
}
if (_picard_it == 1 && _adjust_initial_norm == true)
{
_picard_initial_norm = _picard_initial_norm + _his_initial_norm;
_console << "Adjusted Initial Picard Norm: " << _picard_initial_norm << '\n';
//@^&//_spectral_radius = _current_norm / _picard_initial_norm;
//@^&//_spectral_radius = 0.5 * (_current_norm / _picard_initial_norm + _spectral_radius);
_spectral_radius = pow(_current_norm / _picard_initial_norm * _spectral_radius, 0.5);
}
Real _relative_drop = _current_norm / _picard_initial_norm;
if (_current_norm < _picard_abs_tol || _relative_drop < _picard_rel_tol)
{
_console << "Picard converged!" << std::endl;
_picard_converged = true;
_time_stepper->acceptStep();
// Accumulator Postprocessor goes now, at the actual timestep_end, but
// only if _picard_max_its>1:
//_problem.computeUserObjects(EXEC_CUSTOM, UserObjectWarehouse::POST_AUX);
_problem.execute(EXEC_CUSTOM); // new method
return;
}
}
else // this is the sub-app
{
// If this is the first Picard iteration of the timestep
// Second condition ensures proper treatment the very first time
if ( _his_initial_norm == 0 || _current_norm_old == 0 )
{
_spectral_radius = pow(_new_tol_mult, 0.5);//*#*//
_current_norm = _his_final_norm + _my_current_norm;
}
else
{
_current_norm_old = _current_norm;
_current_norm = _his_final_norm + _my_current_norm;
//@^&//_spectral_radius = _current_norm / _current_norm_old;
//@^&//_spectral_radius = 0.5 * (_current_norm / _current_norm_old + _spectral_radius);
_spectral_radius = pow(_current_norm / _current_norm_old * _spectral_radius, 0.5);
}
}
// For multiple sub-apps you'll need an aggregate PP to collect residuals in
// or some way to keep the number of PPs low.
if (_his_initial_norm == 0)
_his_initial_norm = _my_current_norm;
//assume the other residual is comparable to this one
//_new_tol = std::min(_his_initial_norm*_new_tol_mult, 0.95*_my_current_norm);
_new_tol = std::min(_his_initial_norm*_spectral_radius*_spectral_radius, 0.95*_my_current_norm);
// you may want 0.95 to be a parameter for the user to (not) change
if (_new_tol < _min_abs_tol)
_new_tol = _min_abs_tol;
_console << "New Abs_Tol = " << _new_tol << std::endl;
_problem.es().parameters.set<Real> ("nonlinear solver absolute residual tolerance") = _new_tol;
_time_stepper->step();
// We know whether or not the nonlinear solver thinks it converged, but we need to see if the executioner concurs
if (lastSolveConverged())
{
_console << COLOR_GREEN << " Solve Converged!" << COLOR_DEFAULT << std::endl;
if (_picard_max_its <= 1 )
_time_stepper->acceptStep();
_solution_change_norm = _problem.relativeSolutionDifferenceNorm();
_problem.onTimestepEnd();
_problem.execute(EXEC_TIMESTEP_END);
_problem.execTransfers(EXEC_TIMESTEP_END);
_multiapps_converged = _problem.execMultiApps(EXEC_TIMESTEP_END, _picard_max_its == 1);
if (!_multiapps_converged)
return;
}
else
{
_console << COLOR_RED << " Solve Did NOT Converge!" << COLOR_DEFAULT << std::endl;
// Perform the output of the current, failed time step (this only occurs if desired)
_problem.outputStep(EXEC_FAILED);
}
postSolve();
_time_stepper->postSolve();
_dt = current_dt; // _dt might be smaller than this at this point for multistep methods
_time = _time_old;
}
void
Transient::solveStep(Real input_dt)
{
_dt_old = _dt;
if (input_dt == -1.0)
_dt = computeConstrainedDT();
else
_dt = input_dt;
Real current_dt = _dt;
_problem.onTimestepBegin();
// Increment time
_time = _time_old + _dt;
_problem.execTransfers(EXEC_TIMESTEP_BEGIN);
_multiapps_converged = _problem.execMultiApps(EXEC_TIMESTEP_BEGIN, _picard_max_its == 1);
if (!_multiapps_converged)
return;
preSolve();
_time_stepper->preSolve();
_problem.timestepSetup();
_problem.execute(EXEC_TIMESTEP_BEGIN);
if (_picard_max_its > 1)
{
_picard_timestep_begin_norm = _problem.computeResidualL2Norm();
_console << "Picard Norm after TIMESTEP_BEGIN MultiApps: " << _picard_timestep_begin_norm << '\n';
}
// Perform output for timestep begin
_problem.outputStep(EXEC_TIMESTEP_BEGIN);
// Update warehouse active objects
_problem.updateActiveObjects();
_time_stepper->step();
// We know whether or not the nonlinear solver thinks it converged, but we need to see if the executioner concurs
if (lastSolveConverged())
{
_console << COLOR_GREEN << " Solve Converged!" << COLOR_DEFAULT << std::endl;
if (_picard_max_its <= 1)
_time_stepper->acceptStep();
_solution_change_norm = _problem.solutionChangeNorm();
_problem.onTimestepEnd();
_problem.execute(EXEC_TIMESTEP_END);
_problem.execTransfers(EXEC_TIMESTEP_END);
_multiapps_converged = _problem.execMultiApps(EXEC_TIMESTEP_END, _picard_max_its == 1);
if (!_multiapps_converged)
return;
}
else
{
_console << COLOR_RED << " Solve Did NOT Converge!" << COLOR_DEFAULT << std::endl;
// Perform the output of the current, failed time step (this only occurs if desired)
_problem.outputStep(EXEC_FAILED);
}
postSolve();
_time_stepper->postSolve();
_dt = current_dt; // _dt might be smaller than this at this point for multistep methods
_time = _time_old;
}
void
cpHastySpaceStep(cpSpace *space, cpFloat dt)
{
// don't step if the timestep is 0!
if(dt == 0.0f) return;
space->stamp++;
cpFloat prev_dt = space->curr_dt;
space->curr_dt = dt;
cpArray *bodies = space->dynamicBodies;
cpArray *constraints = space->constraints;
cpArray *arbiters = space->arbiters;
// Reset and empty the arbiter list.
for(int i=0; i<arbiters->num; i++) {
cpArbiter *arb = (cpArbiter *)arbiters->arr[i];
arb->state = CP_ARBITER_STATE_NORMAL;
// If both bodies are awake, unthread the arbiter from the contact graph.
if(!cpBodyIsSleeping(arb->body_a) && !cpBodyIsSleeping(arb->body_b)) {
cpArbiterUnthread(arb);
}
}
arbiters->num = 0;
cpSpaceLock(space);
{
// Integrate positions
for(int i=0; i<bodies->num; i++) {
cpBody *body = (cpBody *)bodies->arr[i];
body->position_func(body, dt);
}
// Find colliding pairs.
cpSpacePushFreshContactBuffer(space);
cpSpatialIndexEach(space->dynamicShapes, (cpSpatialIndexIteratorFunc)cpShapeUpdateFunc, NULL);
cpSpatialIndexReindexQuery(space->dynamicShapes, (cpSpatialIndexQueryFunc)cpSpaceCollideShapes, space);
}
cpSpaceUnlock(space, cpFalse);
// Rebuild the contact graph (and detect sleeping components if sleeping is enabled)
cpSpaceProcessComponents(space, dt);
cpSpaceLock(space);
{
// Clear out old cached arbiters and call separate callbacks
cpHashSetFilter(space->cachedArbiters, (cpHashSetFilterFunc)cpSpaceArbiterSetFilter, space);
// Prestep the arbiters and constraints.
cpFloat slop = space->collisionSlop;
cpFloat biasCoef = 1.0f - cpfpow(space->collisionBias, dt);
for(int i=0; i<arbiters->num; i++) {
cpArbiterPreStep((cpArbiter *)arbiters->arr[i], dt, slop, biasCoef);
}
for(int i=0; i<constraints->num; i++) {
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
cpConstraintPreSolveFunc preSolve = constraint->preSolve;
if(preSolve) preSolve(constraint, space);
constraint->klass->preStep(constraint, dt);
}
// Integrate velocities.
cpFloat damping = cpfpow(space->damping, dt);
cpVect gravity = space->gravity;
for(int i=0; i<bodies->num; i++) {
cpBody *body = (cpBody *)bodies->arr[i];
body->velocity_func(body, gravity, damping, dt);
}
// Apply cached impulses
cpFloat dt_coef = (prev_dt == 0.0f ? 0.0f : dt/prev_dt);
for(int i=0; i<arbiters->num; i++) {
cpArbiterApplyCachedImpulse((cpArbiter *)arbiters->arr[i], dt_coef);
}
for(int i=0; i<constraints->num; i++) {
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
constraint->klass->applyCachedImpulse(constraint, dt_coef);
}
// Run the impulse solver.
cpHastySpace *hasty = (cpHastySpace *)space;
if((unsigned long)(arbiters->num + constraints->num) > hasty->constraint_count_threshold) {
RunWorkers(hasty, Solver);
} else {
Solver(space, 0, 1);
}
// Run the constraint post-solve callbacks
for(int i=0; i<constraints->num; i++) {
cpConstraint *constraint = (cpConstraint *)constraints->arr[i];
cpConstraintPostSolveFunc postSolve = constraint->postSolve;
if(postSolve) postSolve(constraint, space);
}
// run the post-solve callbacks
for(int i=0; i<arbiters->num; i++) {
cpArbiter *arb = (cpArbiter *) arbiters->arr[i];
cpCollisionHandler *handler = arb->handler;
handler->postSolveFunc(arb, space, handler->userData);
}
}
cpSpaceUnlock(space, cpTrue);
}
void
NonlinearEigen::execute()
{
preExecute();
// save the initial guess
_problem.copyOldSolutions();
Real initial_res = 0.0;
if (_free_iter>0)
{
// free power iterations
Moose::out << std::endl << " Free power iteration starts" << std::endl;
inversePowerIteration(_free_iter, _free_iter, _pfactor, false,
std::numeric_limits<Real>::min(), std::numeric_limits<Real>::max(),
true, _output_pi, 0.0,
_eigenvalue, initial_res);
}
if (!getParam<bool>("output_on_final"))
{
_problem.timeStep() = POWERITERATION_END;
Real t = _problem.time();
_problem.time() = _problem.timeStep();
_output_warehouse.outputStep();
_problem.time() = t;
}
Moose::out << " Nonlinear iteration starts" << std::endl;
_problem.timestepSetup();
_problem.copyOldSolutions();
Real rel_tol = _rel_tol;
Real abs_tol = _abs_tol;
if (_free_iter>0)
{
Moose::out << " Initial |R| = " << initial_res << std::endl;
abs_tol = _rel_tol*initial_res;
if (abs_tol<_abs_tol) abs_tol = _abs_tol;
rel_tol = 1e-50;
}
// nonlinear solve
preSolve();
nonlinearSolve(rel_tol, abs_tol, _pfactor, _eigenvalue);
postSolve();
_problem.computeUserObjects(EXEC_TIMESTEP, UserObjectWarehouse::PRE_AUX);
_problem.onTimestepEnd();
_problem.computeAuxiliaryKernels(EXEC_TIMESTEP);
_problem.computeUserObjects(EXEC_TIMESTEP, UserObjectWarehouse::POST_AUX);
if (_run_custom_uo) _problem.computeUserObjects(EXEC_CUSTOM);
if (!getParam<bool>("output_on_final"))
{
_problem.timeStep() = NONLINEAR_SOLVE_END;
Real t = _problem.time();
_problem.time() = _problem.timeStep();
_output_warehouse.outputStep();
_problem.time() = t;
}
Real s = normalizeSolution(_norm_execflag!=EXEC_CUSTOM && _norm_execflag!=EXEC_TIMESTEP &&
_norm_execflag!=EXEC_RESIDUAL);
Moose::out << " Solution is rescaled with factor " << s << " for normalization!" << std::endl;
if (getParam<bool>("output_on_final") || std::fabs(s-1.0)>std::numeric_limits<Real>::epsilon())
{
_problem.timeStep() = FINAL;
Real t = _problem.time();
_problem.time() = _problem.timeStep();
_output_warehouse.outputStep();
_problem.time() = t;
}
postExecute();
}
