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; }