String DFA::debInfo() const { String s("DFA"); s << " # states=" << numStates(); s << " startState=" << startState() << "\n"; for (int i = 0; i < numStates(); i++) { DFAState &state = getState(i); #if !AUGMENT String work; if (state.flag(DFAState::F_FINAL)) { work << "F"; if (state.finalCode() >= 0) work << state.finalCode(); } work.pad(3); s << work; #else s << (char)(state.flag(DFAState::F_FINAL) ? '*' : ' '); #endif s << (char)(state.flag(DFAState::F_DELETE) ? 'D' : ' '); s << (char)(state.flag(DFAState::F_VISITED) ? 'V' : ' '); s << fmt(i,3) << " "; s << state.transitionInfo(); s << '\n'; } return s; }
void AddProcess(IntervalModel& interval, Process::ProcessModel* proc) { interval.processes.add(proc); const auto& scenar = *dynamic_cast<ScenarioInterface*>(interval.parent()); AddProcessAfterState(startState(interval, scenar), *proc); AddProcessBeforeState(endState(interval, scenar), *proc); }
void Particle::updateQueue(float timeInF){ if( stateQueue.size() ){ if( stateQueue.front().timeTill <= timeInF ){ startState(stateQueue.front().state); stateQueue.erase(stateQueue.begin() ); } } }
void EraseProcess( IntervalModel& interval, const Id<Process::ProcessModel>& proc_id) { auto& proc = interval.processes.at(proc_id); const auto& scenar = *dynamic_cast<ScenarioInterface*>(interval.parent()); RemoveProcessAfterState(startState(interval, scenar), proc); RemoveProcessBeforeState(endState(interval, scenar), proc); interval.processes.erase(proc); }
GoalLineUp::GoalLineUp( ):Environment(),currPos(3,0.0f),kickPosition(2,0),targetPosition(2,0),currVel(3,0.0f), ballPos(2,0.0f), oldBallPos(2,0.0f) { /* Default constructor. Initializes state according to start state distribution */ bool t; CurrentState.x = new double[4]; startState(CurrentState, t); reward=-1; transVel = 0; dirTheta = 0; rotVel = 0; blindFrameCount = 0; }
void GapCalculator::LaserScanGapCal(const sensor_msgs::LaserScan laser) { minVDistance = 0; angle = laser.angle_max; for (unsigned int i = laser.ranges.size() - 1; i > (laser.ranges.size()) / 2; i--) { HDistance = laser.ranges[i] * cos(laser.angle_max - angle); currentSearchState = startState(currentSearchState); currentSearchState = calculateBase(currentSearchState, laser, i); currentSearchState = calculateMax(currentSearchState, laser, i); currentSearchState = calculateMin(currentSearchState, laser, i); angle -= laser.angle_increment; } }
void TimeWidget::toggleState() { m_active ? cancelState() : startState(); }
// The style context cache impl nsStyleContext* nsTreeStyleCache::GetStyleContext(nsICSSPseudoComparator* aComparator, nsPresContext* aPresContext, nsIContent* aContent, nsStyleContext* aContext, nsIAtom* aPseudoElement, nsISupportsArray* aInputWord) { PRUint32 count; aInputWord->Count(&count); nsDFAState startState(0); nsDFAState* currState = &startState; // Go ahead and init the transition table. if (!mTransitionTable) { // Automatic miss. Build the table mTransitionTable = new nsObjectHashtable(nsnull, nsnull, DeleteDFAState, nsnull); } // The first transition is always made off the supplied pseudo-element. nsTransitionKey key(currState->GetStateID(), aPseudoElement); currState = static_cast<nsDFAState*>(mTransitionTable->Get(&key)); if (!currState) { // We had a miss. Make a new state and add it to our hash. currState = new nsDFAState(mNextState); mNextState++; mTransitionTable->Put(&key, currState); } for (PRUint32 i = 0; i < count; i++) { nsCOMPtr<nsIAtom> pseudo = getter_AddRefs(static_cast<nsIAtom*>(aInputWord->ElementAt(i))); nsTransitionKey key(currState->GetStateID(), pseudo); currState = static_cast<nsDFAState*>(mTransitionTable->Get(&key)); if (!currState) { // We had a miss. Make a new state and add it to our hash. currState = new nsDFAState(mNextState); mNextState++; mTransitionTable->Put(&key, currState); } } // We're in a final state. // Look up our style context for this state. nsStyleContext* result = nsnull; if (mCache) result = static_cast<nsStyleContext*>(mCache->Get(currState)); if (!result) { // We missed the cache. Resolve this pseudo-style. result = aPresContext->StyleSet()-> ResolveXULTreePseudoStyle(aContent->AsElement(), aPseudoElement, aContext, aComparator).get(); // Put the style context in our table, transferring the owning reference to the table. if (!mCache) { mCache = new nsObjectHashtable(nsnull, nsnull, ReleaseStyleContext, nsnull); } mCache->Put(currState, result); } return result; }
bool DamaPrimTransferRigid::propagate(::std::vector< ::rl::math::Vector >& vecRes, const ::rl::math::Vector& endState, ::std::vector< ::std::string >& vecEdgeAction) { dama::Timer timerIK; ::std::vector< ::rl::math::Vector > vecResSim = vecRes; ::std::vector< ::std::string > vecEdgeActionSim = vecEdgeAction; ::rl::math::Vector startState = vecResSim.back(); // choose the object which to move depending on the robot travel distance to the location of the respective object // TODO: solve traveling salesman problem? Or maybe a bit better (but more time consuming): take pushing pose instead of object's location // TODO: optimize: there should be only one object which could possibly "fly in the air" ::std::size_t subspacePushObject; ::rl::math::Real bestDistRobotObject = ::std::numeric_limits< double >::max(); ::rl::math::Real currDistRobotObject; const ::rl::math::Transform T = dModel->calcForwardKinematics(startState.segment(0, dModel->getDof(0)), 0, this->dModel->mdlGrasp); for(::std::size_t i=dModel->numRobots; i<dModel->getNumMovableComponents(); ++i) { ::std::size_t offset = this->dModel->indexFromSubspace(i); //::rl::math::Real distPlane = ::std::fabs(startState(offset+0) - endState(offset+0)) + ::std::fabs(startState(offset+1) - endState(offset+1)); //::rl::math::Real distHeight = ::std::fabs(startState(offset+2) - endState(offset+2)); ::rl::math::Real distEuclidTransformed = ::std::pow(startState(offset+0) - endState(offset+0), 2) + ::std::pow(startState(offset+1) - endState(offset+1), 2) + ::std::pow(startState(offset+2) - endState(offset+2), 2); if(!this->dModel->isOnSupportSurface(startState, i) && (distEuclidTransformed > dModel->epsilonTransformed || dModel->isMovingSubspace(startState, endState, 0))) { currDistRobotObject = dModel->cartesianRobotDistanceToObject(T, startState, i); if(currDistRobotObject < bestDistRobotObject) { bestDistRobotObject = currDistRobotObject; subspacePushObject = i; } } } ::std::size_t indexObject = dModel->indexFromSubspace(subspacePushObject); if(this->dModel->debugMode) ::std::cout << "DamaPrimTransferRigid::propagate: transfer object nr. " << (subspacePushObject) << " from (" << startState(indexObject) << ", " << startState(indexObject+1) << ", " << startState(indexObject+2) << ") to (" << endState(indexObject) << ", " << endState(indexObject+1) << ", " << endState(indexObject+2) << ")" << std::endl; //TODO: maybe we don't need the IK anymore, because sample comes with right robot arm pose already now?! ::rl::math::Vector interConf2(startState); if(this->dModel->isOnSupportSurface(endState, subspacePushObject)) { // calculate the robot transfer end position rl::math::Transform T2_transfer; T2_transfer.translation().x() = endState(indexObject); T2_transfer.translation().y() = endState(indexObject+1); T2_transfer.translation().z() = endState(indexObject+2) + DamaPrimPickup::HEIGHT_OFFSET_OBJECT; T2_transfer.linear() = ( rl::math::AngleAxis(DamaPrimPickup::PICKUP_Z_AXIS, rl::math::Vector3::UnitZ()) * // WILL BE FREE AXIS rl::math::AngleAxis(DamaPrimPickup::PICKUP_Y_AXIS, rl::math::Vector3::UnitY()) * rl::math::AngleAxis(DamaPrimPickup::PICKUP_X_AXIS, rl::math::Vector3::UnitX()) ).toRotationMatrix(); rl::math::Matrix66 constraint_position_orient = rl::math::Matrix66::Zero(); constraint_position_orient.diagonal() << 1, 1, 1, 1, 1, 0; rl::math::Vector q_ik_start2 = startState.segment(0, dModel->getDof(0)); if(this->dModel->debugMode) { ::std::cout << "Calculate IK2 to T = ("; this->dModel->printTransform(T2_transfer, false); ::std::cout << ") starting at q_deg = ("; this->dModel->printQLine(q_ik_start2 * rl::math::RAD2DEG, false, false, false, false); ::std::cout << ")" << ::std::endl; } timerIK.start(); bool calcInverse2 = DamaModel::calcInversePositionTaskPosture(dModel->mdlGrasp, T2_transfer, q_ik_start2, constraint_position_orient, this->dModel->coupleJoint1And2); timerIK.stop(); this->dModel->timeIK += timerIK.elapsed(); if(!calcInverse2) { if(this->dModel->debugMode) ::std::cout << "Result: IK2 failed. Exit." << ::std::endl; return false; } this->dModel->updateRobotPosition(interConf2, this->dModel->mdlGrasp); } else { if(this->dModel->debugMode) ::std::cout << "Calculate IK2 not needed, just take the pose from the sample, resp. end-state" << ::std::endl; for(::std::size_t i=0; i<this->dModel->getDof(0); ++i) interConf2(i) = endState(i); } if(this->dModel->debugMode) { ::std::cout << "Result: IK2 succeeded with q_deg = ("; this->dModel->printQLine(interConf2, false, false, false, true); ::std::cout << ")" << ::std::endl; } interConf2(indexObject) = endState(indexObject+0); interConf2(indexObject+1) = endState(indexObject+1); interConf2(indexObject+2) = endState(indexObject+2); vecResSim.push_back(interConf2); vecEdgeActionSim.push_back(this->getName() + " " + boost::lexical_cast<std::string>(subspacePushObject)); vecRes = vecResSim; vecEdgeAction = vecEdgeActionSim; return true; }
std::pair<std::vector<StateOfCar>, Seed> AStarSeed::findPathToTargetWithAstar(const cv::Mat& img,const State& start,const State& goal) { // USE : for garanteed termination of planner int no_of_iterations = 0; fusionMap = img; MAP_MAX_ROWS = img.rows; MAP_MAX_COLS = img.cols; if(DT==1) distanceTransform(); if (DEBUG) { image = fusionMap.clone(); } StateOfCar startState(start), targetState(goal); std::map<StateOfCar, open_map_element> openMap; std::map<StateOfCar,StateOfCar, comparatorMapState> came_from; SS::PriorityQueue<StateOfCar> openSet; openSet.push(startState); if (startState.isCloseTo(targetState)) { std::cout<<"Bot is On Target"<<std::endl; return std::make_pair(std::vector<StateOfCar>(), Seed()); } else if(isOnTheObstacle(startState)){ std::cout<<"Bot is on the Obstacle Map \n"; return std::make_pair(std::vector<StateOfCar>(), Seed()); } else if(isOnTheObstacle(targetState)){ std::cout<<"Target is on the Obstacle Map \n"; return std::make_pair(std::vector<StateOfCar>(), Seed()); } while (!openSet.empty() && ros::ok()) { // std::cout<<"openSet size : "<<openSet.size()<<"\n"; if(no_of_iterations > MAX_ITERATIONS){ std::cerr<<"Overflow : openlist size : "<<openSet.size()<<"\n"; return std::make_pair(std::vector<StateOfCar>(), Seed()); } StateOfCar currentState=openSet.top(); if (openMap.find(currentState)!=openMap.end() && openMap[currentState].membership == CLOSED) { openSet.pop(); } currentState=openSet.top(); if (DEBUG) { std::cout<<"current x : "<<currentState.x()<<" current y : "<<currentState.y()<<std::endl; plotPointInMap(currentState); cv::imshow("[PLANNER] Map", image); cvWaitKey(0); } // TODO : use closeTo instead of onTarget if (currentState.isCloseTo(targetState)) { std::cout<<"openSet size : "<<openSet.size()<<"\n"; // std::cout<<"Target Reached"<<std::endl; return reconstructPath(currentState, came_from); } openSet.pop(); openMap[currentState].membership = UNASSIGNED; openMap[currentState].cost=-currentState.gCost(); openMap[currentState].membership=CLOSED; std::vector<StateOfCar> neighborsOfCurrentState = neighborNodesWithSeeds(currentState); for (unsigned int iterator=0; iterator < neighborsOfCurrentState.size(); iterator++) { StateOfCar neighbor = neighborsOfCurrentState[iterator]; double tentativeGCostAlongFollowedPath = neighbor.gCost() + currentState.gCost(); double admissible = neighbor.distanceTo(targetState); double consistent = admissible; double intensity = fusionMap.at<uchar>(neighbor.y(), neighbor.x()); double consistentAndIntensity = (neighbor.hCost()*neighbor.hCost()+2 + intensity*intensity)/(neighbor.hCost()+intensity+2); if (!((openMap.find(neighbor) != openMap.end()) && (openMap[neighbor].membership == OPEN))) { came_from[neighbor] = currentState; neighbor.gCost( tentativeGCostAlongFollowedPath) ; if(DT==1) neighbor.hCost(consistentAndIntensity); else neighbor.hCost( consistent) ; neighbor.updateTotalCost(); openSet.push(neighbor); openMap[neighbor].membership = OPEN; openMap[neighbor].cost = neighbor.gCost(); } } no_of_iterations++; } std::cerr<<"NO PATH FOUND"<<std::endl; return std::make_pair(std::vector<StateOfCar>(), Seed()); }
bool DamaPrimPushMobile::propagate(::std::vector< ::rl::math::Vector >& vecRes, const ::rl::math::Vector& endState, ::std::vector< ::std::string >& vecEdgeAction) { dama::Timer timerIK; ::std::vector< ::rl::math::Vector > vecResSim = vecRes; ::std::vector< ::std::string > vecEdgeActionSim = vecEdgeAction; ::rl::math::Vector startState = vecResSim.back(); // TODO: only inspect the distance on a surface // choose the object which to move depending on the robot travel distance to the location of the respective object // TODO: solve traveling salesman problem? Or maybe a bit better (but more time consuming): take pushing pose instead of object's location ::std::size_t subspacePushObject; ::rl::math::Real bestDistRobotObject = ::std::numeric_limits< double >::max(); ::rl::math::Real currDistRobotObject; const ::rl::math::Transform T = dModel->calcForwardKinematics(startState.segment(0, dModel->getDof(0)), 0); for(::std::size_t i=dModel->numRobots; i<dModel->getNumMovableComponents(); ++i) { if(dModel->isMovingSubspace(startState, endState, i)) { currDistRobotObject = dModel->cartesianRobotDistanceToObject(T, startState, i); if(currDistRobotObject < bestDistRobotObject) { bestDistRobotObject = currDistRobotObject; subspacePushObject = i; } } } ::std::size_t indexObject = dModel->indexFromSubspace(subspacePushObject); if(this->dModel->debugMode) ::std::cout << "DamaPrimPushMobile::propagate: pushing object nr. " << (subspacePushObject) << " from (" << startState(indexObject) << ", " << startState(indexObject+1) << ", " << startState(indexObject+2) << ") to (" << endState(indexObject) << ", " << endState(indexObject+1) << ", " << endState(indexObject+2) << ")" << std::endl; // calculate the 2D surface directional vector from object start to object goal position ::rl::math::Vector dirVecMovingObject(2); dirVecMovingObject(0) = endState(indexObject) - startState(indexObject); dirVecMovingObject(1) = endState(indexObject+1) - startState(indexObject+1); ::rl::math::Real lengthMovingObject = dirVecMovingObject.norm(); // calculate the real endstate, because we have a limited push distance -> endstate might get cut ::rl::math::Vector realEndState(3); for(::std::size_t i=0; i<3; ++i) realEndState(i) = endState(indexObject+i); if(lengthMovingObject > DamaPrimPush::MAX_PUSH_DIST) { realEndState(0) = startState(indexObject) + dirVecMovingObject(0)/lengthMovingObject*DamaPrimPush::MAX_PUSH_DIST; realEndState(1) = startState(indexObject+1) + dirVecMovingObject(1)/lengthMovingObject*DamaPrimPush::MAX_PUSH_DIST; if(this->dModel->debugMode) ::std::cout << "Adjusting goal (cut from " << lengthMovingObject << " down to " << DamaPrimPush::MAX_PUSH_DIST << " length): (" << realEndState(0) << ", " << realEndState(1) << ", " << realEndState(2) << ")" << ::std::endl; } // special code for mobile robot dirVecMovingObject.normalize(); dirVecMovingObject *= DamaPrimPush::DIST_TO_OBJECT_XY; ::rl::math::Vector interConf1(startState); interConf1(0) = startState(indexObject) - dirVecMovingObject(0); interConf1(1) = startState(indexObject+1) - dirVecMovingObject(1); // first move to the pushing position if not already there if(dModel->isMovingSubspace(startState, interConf1, 0)) { // TODO: investigate why algorithm currently needs about 5.7% more time just by using the uncommented code instead of the one in comments?! //vecResSim.push_back(interConf1); //vecEdgeActionSim.push_back(DamaPrimTransit::getInstance()->getName()); DamaPrimTransit::getInstance()->propagate(vecResSim, interConf1, vecEdgeActionSim); } // now push the object to its end position ::rl::math::Vector interConf2(startState); interConf2(0) = realEndState(0) - dirVecMovingObject(0); interConf2(1) = realEndState(1) - dirVecMovingObject(1); interConf2(indexObject) = realEndState(0); interConf2(indexObject+1) = realEndState(1); vecResSim.push_back(interConf2); vecEdgeActionSim.push_back(this->getName() + " " + boost::lexical_cast<std::string>(subspacePushObject)); vecRes = vecResSim; vecEdgeAction = vecEdgeActionSim; /*::rl::math::Vector* test = NULL; test->normalize();*/ return true; }
void ClockWidget::restart() { if( utils::Properties::get( utils::Property::ClosedWhileRunning).toInt() ) startState(); }
EndCriteria::Type FireflyAlgorithm::minimize(Problem &P, const EndCriteria &endCriteria) { QL_REQUIRE(!P.constraint().empty(), "Firefly Algorithm is a constrained optimizer"); EndCriteria::Type ecType = EndCriteria::None; P.reset(); Size iteration = 0; Size iterationStat = 0; Size maxIteration = endCriteria.maxIterations(); Size maxIStationary = endCriteria.maxStationaryStateIterations(); startState(P, endCriteria); bool isFA = Mfa_ > 0 ? true : false; //Variables for DE Array z(N_, 0.0); Size indexBest, indexR1, indexR2; //Set best value & position Real bestValue = values_[0].first; Size bestPosition = 0; for (Size i = 1; i < M_; i++) { if (values_[i].first < bestValue) { bestPosition = i; bestValue = values_[i].first; } } Array bestX = x_[bestPosition]; //Run optimization do { iteration++; iterationStat++; //Check if stopping criteria is met if (iteration > maxIteration || iterationStat > maxIStationary) break; //Divide into two subpopulations //First sort values std::sort(values_.begin(), values_.end()); //Differential evolution if(Mfa_ < M_){ Size indexBest = values_[0].second; Array& xBest = x_[indexBest]; for (Size i = Mfa_; i < M_; i++) { if (!isFA) { //Pure DE requires random index indexBest = drawIndex_(); xBest = x_[indexBest]; } do { indexR1 = drawIndex_(); } while(indexR1 == indexBest); do { indexR2 = drawIndex_(); } while(indexR2 == indexBest || indexR2 == indexR1); Size index = values_[i].second; Array& x = x_[index]; Array& xR1 = x_[indexR1]; Array& xR2 = x_[indexR2]; for (Size j = 0; j < N_; j++) { if (rng_.nextReal() <= crossover_) { //Change x[j] according to crossover z[j] = xBest[j] + mutation_*(xR1[j] - xR2[j]); } else { z[j] = x[j]; } } Real val = P.value(z); if (val < values_[index].first) { //Accept new point x = z; values_[index].first = val; //mark best if (val < bestValue) { bestValue = val; bestX = x; iterationStat = 0; } } } } //Firefly algorithm if(isFA){ //According to the intensity, determine best global position intensity_->findBrightest(); //Prepare random walk randomWalk_->walk(); //Loop over particles for (Size i = 0; i < Mfa_; i++) { Size index = values_[i].second; Array& x = x_[index]; Array& xI = xI_[index]; Array& xRW = xRW_[index]; //Loop over dimensions for (Size j = 0; j < N_; j++) { //Update position x[j] += xI[j] + xRW[j]; //Enforce bounds on positions if (x[j] < lX_[j]) { x[j] = lX_[j]; } else if (x[j] > uX_[j]) { x[j] = uX_[j]; } } //Evaluate x & mark best values_[index].first = P.value(x); if (values_[index].first < bestValue) { bestValue = values_[index].first; bestX = x; iterationStat = 0; } } } } while (true); if (iteration > maxIteration) ecType = EndCriteria::MaxIterations; else ecType = EndCriteria::StationaryPoint; //Set result to best point P.setCurrentValue(bestX); P.setFunctionValue(bestValue); return ecType; }
void CreateCurvesFromAddresses( const QList<const Scenario::ConstraintModel*>& selected_constraints, const std::vector<Device::FullAddressSettings>& addresses, const iscore::CommandStackFacade& stack) { if(selected_constraints.empty()) return; // They should all be in the same scenario so we can select the first. // FIXME check that the other "cohesion" methods also use ScenarioInterface and not Scenario::ProcessModel auto scenar = dynamic_cast<Scenario::ScenarioInterface*>( selected_constraints.first()->parent()); int added_processes = 0; // Then create the commands auto big_macro = new Scenario::Command::AddMultipleProcessesToMultipleConstraintsMacro; for(const auto& constraint : selected_constraints) { // Generate brand new ids for the processes auto process_ids = getStrongIdRange<Process::ProcessModel>(addresses.size(), constraint->processes); auto macro_tuple = Scenario::Command::makeAddProcessMacro(*constraint, addresses.size()); auto macro = std::get<0>(macro_tuple); auto& bigLayerVec = std::get<1>(macro_tuple); Path<Scenario::ConstraintModel> constraintPath{*constraint}; const Scenario::StateModel& ss = startState(*constraint, *scenar); const auto& es = endState(*constraint, *scenar); std::vector<State::Address> existing_automations; for(const auto& proc : constraint->processes) { if(auto autom = dynamic_cast<const Automation::ProcessModel*>(&proc)) existing_automations.push_back(autom->address()); } int i = 0; for(const Device::FullAddressSettings& as : addresses) { // First, we skip the curve if there is already a curve // with this address in the constraint. if(contains(existing_automations, as.address)) continue; // Then we set-up all the necessary values // min / max double min = as.domain.min.val.isNumeric() ? State::convert::value<double>(as.domain.min) : 0; double max = as.domain.max.val.isNumeric() ? State::convert::value<double>(as.domain.max) : 1; // start value / end value double start = std::min(min, max); double end = std::max(min, max); Process::MessageNode* s_node = Device::try_getNodeFromString( ss.messages().rootNode(), stringList(as.address)); if(s_node) { if(auto val = s_node->value()) { start = State::convert::value<double>(*val); min = std::min(start, min); max = std::max(start, max); } } Process::MessageNode* e_node = Device::try_getNodeFromString( es.messages().rootNode(), stringList(as.address)); if(e_node) { if(auto val = e_node->value()) { end = State::convert::value<double>(*val); min = std::min(end, min); max = std::max(end, max); } } // Send the command. macro->addCommand(new Scenario::Command::CreateCurveFromStates{ Path<Scenario::ConstraintModel>{constraintPath}, bigLayerVec[i], process_ids[i], as.address, start, end, min, max }); i++; added_processes++; } big_macro->addCommand(macro); } if(added_processes > 0) { CommandDispatcher<> disp{stack}; disp.submitCommand(big_macro); } else { delete big_macro; } }