void permutation_matrix<T, X>::apply_reverse_from_left(indexed_vector<L> & w) {
// the result will be w = p(-1) * w
#ifdef LEAN_DEBUG
// dense_matrix<L, X> deb(get_reverse());
// L * deb_w = clone_vector<L>(w.m_data, row_count());
// deb.apply_from_left(deb_w);
#endif
std::vector<L> t(w.m_index.size());
std::vector<unsigned> tmp_index(w.m_index.size());
copy_aside(t, tmp_index, w);
clear_data(w);
// set the new values
for (unsigned i = t.size(); i > 0;) {
i--;
unsigned j = m_permutation[tmp_index[i]];
w[j] = t[i];
w.m_index[i] = j;
}
#ifdef LEAN_DEBUG
// lean_assert(vectors_are_equal<L>(deb_w, w.m_data, row_count()));
// delete [] deb_w;
#endif
}
//-------------------------------------------------------------
//
// this[i] = alpha[i] * this[i]
//
//-------------------------------------------------------------
void EpetraOpMultiVec::MvScale ( const std::vector<double>& alpha )
{
// Check to make sure the vector is as long as the multivector has columns.
int numvecs = this->GetNumberVecs();
TEUCHOS_TEST_FOR_EXCEPTION( (int)alpha.size() != numvecs, std::invalid_argument,
"Anasazi::EpetraOpMultiVec::MvScale() alpha argument size was inconsistent with number of vectors in mv.");
std::vector<int> tmp_index( 1, 0 );
for (int i=0; i<numvecs; i++) {
Epetra_MultiVector temp_vec(Epetra_DataAccess::View, *Epetra_MV, &tmp_index[0], 1);
TEUCHOS_TEST_FOR_EXCEPTION(
temp_vec.Scale( alpha[i] ) != 0,
EpetraSpecializedMultiVecFailure, "Anasazi::EpetraOpMultiVec::MvScale() call to Epetra_MultiVector::Scale() returned a nonzero value.");
tmp_index[0]++;
}
}
void permutation_matrix<T, X>::apply_from_left_perm(indexed_vector<L> & w, lp_settings &) {
#ifdef LEAN_DEBUG
// dense_matrix<T, L> deb(*this);
// T * deb_w = clone_vector<T>(w.m_data, row_count());
// deb.apply_from_right(deb_w);
#endif
std::vector<L> t(w.m_index.size());
std::vector<unsigned> tmp_index(w.m_index.size());
copy_aside(t, tmp_index, w);
clear_data(w);
// set the new values
for (unsigned i = t.size(); i > 0;) {
i--;
unsigned j = m_rev[tmp_index[i]];
w[j] = t[i];
w.m_index[i] = j;
}
#ifdef LEAN_DEBUG
// lean_assert(vectors_are_equal<T>(deb_w, w.m_data, row_count()));
// delete [] deb_w;
#endif
}
EReturn OMPLsolver::specifyProblem(PlanningProblem_ptr goals,
PlanningProblem_ptr costs, PlanningProblem_ptr goalBias,
PlanningProblem_ptr samplingBias)
{
if (goals->type().compare(std::string("exotica::OMPLProblem")) != 0)
{
ERROR("This solver can't use problem of type '" << goals->type() << "'!");
return PAR_INV;
}
if (costs
&& costs->type().compare(std::string("exotica::OMPLProblem")) != 0)
{
ERROR("This solver can't use problem of type '" << costs->type() << "'!");
return PAR_INV;
}
if (goalBias
&& goalBias->type().compare(std::string("exotica::OMPLProblem")) != 0)
{
ERROR(
"This solver can't use problem of type '" << goalBias->type() << "'!");
return PAR_INV;
}
if (samplingBias
&& samplingBias->type().compare(std::string("exotica::OMPLProblem"))
!= 0)
{
ERROR(
"This solver can't use problem of type '" << samplingBias->type() << "'!");
return PAR_INV;
}
MotionSolver::specifyProblem(goals);
prob_ = boost::static_pointer_cast<OMPLProblem>(goals);
costs_ = boost::static_pointer_cast<OMPLProblem>(costs);
goalBias_ = boost::static_pointer_cast<OMPLProblem>(goalBias);
samplingBias_ = boost::static_pointer_cast<OMPLProblem>(samplingBias);
for (auto & it : prob_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
if (costs)
{
for (auto & it : costs_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
if (goalBias_)
{
for (auto & it : goalBias_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
if (samplingBias_)
{
for (auto & it : samplingBias_->getScenes())
{
if (!ok(it.second->activateTaskMaps()))
{
INDICATE_FAILURE
;
return FAILURE;
}
}
}
compound_ = prob_->isCompoundStateSpace();
if (compound_)
{
state_space_ = ob::StateSpacePtr(
OMPLSE3RNCompoundStateSpace::FromProblem(prob_, server_));
}
else
{
state_space_ = OMPLStateSpace::FromProblem(prob_);
}
HIGHLIGHT_NAMED(object_name_,
"Using state space: "<<state_space_->getName());
ompl_simple_setup_.reset(new og::SimpleSetup(state_space_));
ompl_simple_setup_->setStateValidityChecker(
ob::StateValidityCheckerPtr(new OMPLStateValidityChecker(this)));
ompl_simple_setup_->setPlannerAllocator(
boost::bind(known_planners_[selected_planner_], _1,
this->getObjectName() + "_" + selected_planner_));
if (compound_)
ompl_simple_setup_->getStateSpace()->setStateSamplerAllocator(
boost::bind(&allocSE3RNStateSampler,
(ob::StateSpace*) state_space_.get()));
ompl_simple_setup_->getSpaceInformation()->setup();
std::vector<std::string> jnts;
prob_->getScenes().begin()->second->getJointNames(jnts);
if (projector_.compare("Joints") == 0)
{
bool projects_ok_ = true;
std::vector<int> vars(projection_components_.size());
for (int i = 0; i < projection_components_.size(); i++)
{
bool found = false;
for (int j = 0; j < jnts.size(); j++)
{
if (projection_components_[i].compare(jnts[j]) == 0)
{
vars[i] = j;
found = true;
break;
}
}
if (!found)
{
WARNING(
"Projection joint ["<<projection_components_[i]<<"] does not exist, OMPL Projection Evaluator not used");
projects_ok_ = false;
break;
}
}
if (projects_ok_)
{
ompl_simple_setup_->getStateSpace()->registerDefaultProjection(
ompl::base::ProjectionEvaluatorPtr(
new exotica::OMPLProjection(state_space_, vars)));
std::string tmp;
for (int i = 0; i < projection_components_.size(); i++)
tmp = tmp + "[" + projection_components_[i] + "] ";
HIGHLIGHT_NAMED(object_name_, " Using projection joints "<<tmp);
}
}
else if (projector_.compare("DMesh") == 0)
{
// Construct default DMesh projection relationship
std::vector<std::pair<int, int> > tmp_index(
projection_components_.size() - 1);
for (int i = 0; i < tmp_index.size(); i++)
{
tmp_index[i].first = i;
tmp_index[i].second = tmp_index.size();
}
ompl_simple_setup_->getStateSpace()->registerDefaultProjection(
ompl::base::ProjectionEvaluatorPtr(
new exotica::DMeshProjection(state_space_, projection_components_,
tmp_index, prob_->getScenes().begin()->second)));
std::string tmp;
for (int i = 0; i < projection_components_.size(); i++)
tmp = tmp + "[" + projection_components_[i] + "] ";
HIGHLIGHT_NAMED(object_name_, " Using DMesh Projection links "<<tmp);
}
else
{
WARNING_NAMED(object_name_,
"Unknown projection type "<<projector_<<". Setting ProjectionEvaluator failed.");
}
ompl_simple_setup_->setGoal(constructGoal());
ompl_simple_setup_->setup();
if (isFlexiblePlanner())
{
INFO_NAMED(object_name_,
"Setting up FRRT Local planner from file\n"<<prob_->local_planner_config_);
if (!ompl_simple_setup_->getPlanner()->as<ompl::geometric::FlexiblePlanner>()->setUpLocalPlanner(
prob_->local_planner_config_, prob_->scenes_.begin()->second))
{
INDICATE_FAILURE
return FAILURE;
}
}
returnValue DiscreteTimeExport::setup( )
{
int useOMP;
get(CG_USE_OPENMP, useOMP);
ExportStruct structWspace;
structWspace = useOMP ? ACADO_LOCAL : ACADO_WORKSPACE;
// non equidistant integration grids not implemented for NARX integrators
if( !equidistant ) return ACADOERROR( RET_INVALID_OPTION );
String fileName( "integrator.c" );
int printLevel;
get( PRINTLEVEL,printLevel );
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "--> Preparing to export %s... ",fileName.getName() );
ExportIndex run( "run" );
ExportIndex i( "i" );
ExportIndex j( "j" );
ExportIndex k( "k" );
ExportIndex tmp_index("tmp_index");
uint diffsDim = NX*(NX+NU);
uint inputDim = NX*(NX+NU+1) + NU + NP;
// setup INTEGRATE function
rk_index = ExportVariable( "rk_index", 1, 1, INT, ACADO_LOCAL, BT_TRUE );
rk_eta = ExportVariable( "rk_eta", 1, inputDim, REAL );
if( equidistantControlGrid() ) {
integrate = ExportFunction( "integrate", rk_eta, reset_int );
}
else {
integrate = ExportFunction( "integrate", rk_eta, reset_int, rk_index );
}
integrate.setReturnValue( error_code );
integrate.addIndex( run );
integrate.addIndex( i );
integrate.addIndex( j );
integrate.addIndex( k );
integrate.addIndex( tmp_index );
rhs_in = ExportVariable( "x", inputDim-diffsDim, 1, REAL, ACADO_LOCAL );
rhs_out = ExportVariable( "f", NX, 1, REAL, ACADO_LOCAL );
fullRhs = ExportFunction( "full_rhs", rhs_in, rhs_out );
rk_xxx = ExportVariable( "rk_xxx", 1, inputDim-diffsDim, REAL, structWspace );
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
rk_diffsPrev1 = ExportVariable( "rk_diffsPrev1", NX1, NX1+NU, REAL, structWspace );
rk_diffsPrev2 = ExportVariable( "rk_diffsPrev2", NX2, NX1+NX2+NU, REAL, structWspace );
}
rk_diffsNew1 = ExportVariable( "rk_diffsNew1", NX1, NX1+NU, REAL, structWspace );
rk_diffsNew2 = ExportVariable( "rk_diffsNew2", NX2, NX1+NX2+NU, REAL, structWspace );
ExportVariable numInt( "numInts", 1, 1, INT );
if( !equidistantControlGrid() ) {
ExportVariable numStepsV( "numSteps", numSteps, STATIC_CONST_INT );
integrate.addStatement( String( "int " ) << numInt.getName() << " = " << numStepsV.getName() << "[" << rk_index.getName() << "];\n" );
}
integrate.addStatement( rk_xxx.getCols( NX,inputDim-diffsDim ) == rk_eta.getCols( NX+diffsDim,inputDim ) );
integrate.addLinebreak( );
// integrator loop:
ExportForLoop tmpLoop( run, 0, grid.getNumIntervals() );
ExportStatementBlock *loop;
if( equidistantControlGrid() ) {
loop = &tmpLoop;
}
else {
loop = &integrate;
loop->addStatement( String("for(") << run.getName() << " = 0; " << run.getName() << " < " << numInt.getName() << "; " << run.getName() << "++ ) {\n" );
}
loop->addStatement( rk_xxx.getCols( 0,NX ) == rk_eta.getCols( 0,NX ) );
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
// Set rk_diffsPrev:
loop->addStatement( String("if( run > 0 ) {\n") );
if( NX1 > 0 ) {
ExportForLoop loopTemp1( i,0,NX1 );
loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,0,NX1 ) == rk_eta.getCols( i*NX+NX+NXA,i*NX+NX+NXA+NX1 ) );
if( NU > 0 ) loopTemp1.addStatement( rk_diffsPrev1.getSubMatrix( i,i+1,NX1,NX1+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1),i*NU+(NX+NXA)*(NX+1)+NU ) );
loop->addStatement( loopTemp1 );
}
if( NX2 > 0 ) {
ExportForLoop loopTemp2( i,0,NX2 );
loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,0,NX1+NX2 ) == rk_eta.getCols( i*NX+NX+NXA+NX1*NX,i*NX+NX+NXA+NX1*NX+NX1+NX2 ) );
if( NU > 0 ) loopTemp2.addStatement( rk_diffsPrev2.getSubMatrix( i,i+1,NX1+NX2,NX1+NX2+NU ) == rk_eta.getCols( i*NU+(NX+NXA)*(NX+1)+NX1*NU,i*NU+(NX+NXA)*(NX+1)+NX1*NU+NU ) );
loop->addStatement( loopTemp2 );
}
loop->addStatement( String("}\n") );
}
// evaluate states:
if( NX1 > 0 ) {
loop->addFunctionCall( lin_input.getName(), rk_xxx, rk_eta.getAddress(0,0) );
}
if( NX2 > 0 ) {
loop->addFunctionCall( getNameRHS(), rk_xxx, rk_eta.getAddress(0,NX1) );
}
// evaluate sensitivities:
if( NX1 > 0 ) {
for( uint i1 = 0; i1 < NX1; i1++ ) {
for( uint i2 = 0; i2 < NX1; i2++ ) {
loop->addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,i2,i2+1) == A11(i1,i2) );
}
for( uint i2 = 0; i2 < NU; i2++ ) {
loop->addStatement( rk_diffsNew1.getSubMatrix(i1,i1+1,NX1+i2,NX1+i2+1) == B11(i1,i2) );
}
}
}
if( NX2 > 0 ) {
loop->addFunctionCall( getNameDiffsRHS(), rk_xxx, rk_diffsNew2.getAddress(0,0) );
}
// computation of the sensitivities using chain rule:
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
loop->addStatement( String( "if( run == 0 ) {\n" ) );
}
// PART 1
updateInputSystem(loop, i, j, tmp_index);
// PART 2
updateImplicitSystem(loop, i, j, tmp_index);
if( grid.getNumIntervals() > 1 || !equidistantControlGrid() ) {
loop->addStatement( String( "}\n" ) );
loop->addStatement( String( "else {\n" ) );
// PART 1
propagateInputSystem(loop, i, j, k, tmp_index);
// PART 2
propagateImplicitSystem(loop, i, j, k, tmp_index);
loop->addStatement( String( "}\n" ) );
}
// end of the integrator loop.
if( !equidistantControlGrid() ) {
loop->addStatement( "}\n" );
}
else {
integrate.addStatement( *loop );
}
// PART 1
if( NX1 > 0 ) {
Matrix zeroR = zeros(1, NX2);
ExportForLoop loop1( i,0,NX1 );
loop1.addStatement( rk_eta.getCols( i*NX+NX+NXA+NX1,i*NX+NX+NXA+NX ) == zeroR );
integrate.addStatement( loop1 );
}
if ( (PrintLevel)printLevel >= HIGH )
acadoPrintf( "done.\n" );
return SUCCESSFUL_RETURN;
}
