Esempio n. 1
0
  void fibonacci_queue::decrease_key(int v, int k) {

    typename fibonacci_heap::Node *x = lookup[v];

    if (x != nullptr)
      fib_heap.DecreaseKey(lookup[v],ii(k,v));
    else
      push(ii(k,v));
  }
 void count(){
     cnt=vi(sz, -1); stack<ii> S; S.push(ii(0,0));map<char,int>::iterator i;
     while(!S.empty()){
         ii cur = S.top(); S.pop();
         if(cur.second){
             for(i = next[cur.first].begin();i != next[cur.first].end();++i){
                 cnt[cur.first] += cnt[(*i).second]; } }
         else if(cnt[cur.first] == -1){
             cnt[cur.first] = 1; S.push(ii(cur.first, 1));
             for(i = next[cur.first].begin();i != next[cur.first].end();++i){
                 S.push(ii((*i).second, 0)); } } } }
void CompoundObjective::
init(KinematicModel const & model)
{
    for (size_t ii(0); ii < unilateral_constraints_.size(); ++ii) {
        unilateral_constraints_[ii]->init(model);
    }
    for (size_t ii(0); ii < hard_objectives_.size(); ++ii) {
        hard_objectives_[ii]->init(model);
    }
    for (size_t ii(0); ii < soft_objectives_.size(); ++ii) {
        soft_objectives_[ii]->init(model);
    }
}
  void update_cost(int u, int v, int c) {
    if (parent[v].first == u)
      swap(u, v);

    assert(parent[u].first == v);
    parent[u] = ii(v, c);

    for (int i = 0; i < size(adj[u]); i++)
      if (adj[u][i].first == v)
        adj[u][i] = ii(v, c);

    for (int i = 0; i < size(adj[v]); i++)
      if (adj[v][i].first == u)
        adj[v][i] = ii(u, c);
  }
Esempio n. 5
0
// This constructor creates a state by reading the board that the SERVER sends us.
State::State(const char* board, int team)
{

	for (int i = 7, k = 0; i >= 0; --i, ++k)
	{
		for (int j = 0; j < 8; ++j)
		{
			this->board[i*8+j] = board[k*8+j];
		}
	}
	ourTeam = (team == 1) ? WHITE : BLACK;
	if (ourTeam == BLACK) myFirstRow = 6;
	else myFirstRow = 1;
	this->lastMove = make_pair( ii(-1,-1), ii(-1,-1) );
}
int bfs(int start, int end, vvi& adj_list) {
    queue<ii> Q;
    Q.push(ii(start, 0));

    while (true) {
        ii cur = Q.front(); Q.pop();

        if (cur.first == end)
            return cur.second;

        vi& adj = adj_list[cur.first];
        for (vi::iterator it = adj.begin(); it != adj.end(); it++)
            Q.push(ii(*it, cur.second + 1));
    }
}
Esempio n. 7
0
ResourceList ResourceInfo::toResourceList( const std::set<ResourceInfo>& rResourceList )
{
    ResourceList list;
    std::insert_iterator<ResourceList> ii( list, list.begin() );
    std::copy( rResourceList.begin(), rResourceList.end(), ii );
    return list;
}
Esempio n. 8
0
  void Model::
  computeInverseMassInertia()
  {
    if (ainv_upper_triangular_.empty()) {
      ainv_upper_triangular_.resize(ndof_ * (ndof_ + 1) / 2);
    }
    
    deFloat const one(1);
    for (size_t irow(0); irow < ndof_; ++irow) {
      taoJoint * joint(kgm_tree_->info[irow].joint);
      
      // Compute one column of Ainv by solving forward dynamics of the
      // corresponding joint having a unit torque, while all the
      // others remain unactuated. This works on the kgm_tree because
      // it has zero speeds, thus the Coriolis-centrifgual effects are
      // zero, and by using zero gravity we get pure system dynamics:
      // acceleration = mass_inv * force (in matrix form).
      joint->setTau(&one);
      taoDynamics::fwdDynamics(kgm_tree_->root, &zero_gravity);
      joint->zeroTau();
      
      // Retrieve the column of Ainv by reading the joint
      // accelerations generated by the column-selecting unit torque
      // (into a flattened upper triangular matrix).
      for (size_t icol(0); icol <= irow; ++icol) {
	kgm_tree_->info[icol].joint->getDDQ(&ainv_upper_triangular_[squareToTriangularIndex(irow, icol, ndof_)]);
      }
    }
    
    // Reset all the accelerations.
    for (size_t ii(0); ii < ndof_; ++ii) {
      kgm_tree_->info[ii].joint->zeroDDQ();
    }
  }
Esempio n. 9
0
  bool Model::
  computeCOM(Vector & com, Matrix * opt_jcom) const
  {
    com = Vector::Zero(3);
    if (opt_jcom) {
      *opt_jcom = Matrix::Zero(3, ndof_);
    }
    double mtotal(0);
    for (size_t ii(0); ii < ndof_; ++ii) {
      taoDNode * const node(kgm_tree_->info[ii].node);
      deVector3 wpos;
      wpos.multiply(node->frameGlobal()->rotation(), *(node->center()));
      wpos += node->frameGlobal()->translation();
      if (opt_jcom) {
	Matrix wjcom;
	if ( ! computeJacobian(node, wpos[0], wpos[1], wpos[2], wjcom)) {
	  return false;
	}
	*opt_jcom += *(node->mass()) * wjcom.block(0, 0, 3, wjcom.cols());
      }
      wpos *= *(node->mass());
      mtotal += *(node->mass());
      com += Vector::Map(&wpos[0], 3);
    }
    if (fabs(mtotal) > 1e-3) {
      com /= mtotal;
      if (opt_jcom) {
	*opt_jcom /= mtotal;
      }
    }
    return true;
  }
 void countoccur(){
     for(int i = 0; i < sz; ++i){ occur[i] = 1 - isclone[i]; }
     vii states(sz);
     for(int i = 0; i < sz; ++i){ states[i] = ii(len[i],i); }
     sort(states.begin(), states.end());
     for(int i = size(states)-1; i >= 0; --i){ int v = states[i].second;
         if(link[v] != -1) { occur[link[v]] += occur[v]; } } }
Esempio n. 11
0
bool HexFilesMenager::saveToBinary(QString fileName)
{
    if(fileName.indexOf('.')==-1)fileName+=".bin";

    QFile f (fileName);

    f.open(QFile::WriteOnly);
    if(!f.isOpen())
    {

        return false;
    }
    f.write(this->getCurrentPage());

    f.close();

    QFileInfo ii(f);

    tw->setTabText(tw->currentIndex(),ii.fileName());

    static_cast <HexEditorWidget *> (tw->currentWidget())->saved();
    on_dataSaved();

    return true;

}
Esempio n. 12
0
void NFABuilder::patch_mid(OutListT& src, const InListT& dst, uint32_t dstskip) {
  // Make an edge from each vertex in src to each vertex in dst. Edges
  // to vertices in dst before dstskip go before the insertion point in
  // src, edges to vertices in dst after dstskip go after the insertion
  // point in src.
  const InListT::const_iterator skip_stop(
    dstskip < dst.size() ? dst.begin() + dstskip : dst.end());

  for (OutListT::iterator oi(src.begin()); oi != src.end(); ++oi) {
    uint32_t pos = oi->second;

    InListT::const_iterator ii(dst.begin());

    // make edges before dstskip, inserting before src insertion point
    for ( ; ii != dst.end() && ii < skip_stop; ++ii) {
      Fsm->insertEdge(oi->first, *ii, pos++);
    }

    // save the new insertion point for dst
    const uint32_t spos = pos;

    // make edges after dstskip, inserting after src insertion point
    for ( ; ii != dst.end(); ++ii) {
      Fsm->insertEdge(oi->first, *ii, pos++);
    }

    // set the new insertion point for dst
    oi->second = spos;
  }
}
Esempio n. 13
0
  void Model::
  computeMassInertia()
  {
    if (a_upper_triangular_.empty()) {
      a_upper_triangular_.resize(ndof_ * (ndof_ + 1) / 2);
    }
    
    deFloat const one(1);
    for (size_t irow(0); irow < ndof_; ++irow) {
      taoJoint * joint(kgm_joints_[irow]);
      
      // Compute one column of A by solving inverse dynamics of the
      // corresponding joint having a unit acceleration, while all the
      // others remain fixed. This works on the KGM tree because it
      // has zero speeds, thus the Coriolis-centrifgual effects are
      // zero, and by using zero gravity we get pure system dynamics:
      // force = mass * acceleration (in matrix form).
      joint->setDDQ(&one);
      taoDynamics::invDynamics(kgm_root_, &zero_gravity);
      joint->zeroDDQ();
      
      // Retrieve the column of A by reading the joint torques
      // required for the column-selecting unit acceleration (into a
      // flattened upper triangular matrix).
      
      for (size_t icol(0); icol <= irow; ++icol) {
	kgm_joints_[icol]->getTau(&a_upper_triangular_[squareToTriangularIndex(irow, icol, ndof_)]);
      }
    }
    
    // Reset all the torques.
    for (size_t ii(0); ii < ndof_; ++ii) {
      kgm_joints_[ii]->zeroTau();
    }
  }
Esempio n. 14
0
void KIconEffect::toGray(QImage &img, float value)
{
    if (value == 0.0) {
        return;
    }

    KIEImgEdit ii(img);
    QRgb *data = ii.data;
    QRgb *end = data + ii.pixels;

    unsigned char gray;
    if (value == 1.0) {
        while (data != end) {
            gray = qGray(*data);
            *data = qRgba(gray, gray, gray, qAlpha(*data));
            ++data;
        }
    } else {
        unsigned char val = (unsigned char)(255.0 * value);
        while (data != end) {
            gray = qGray(*data);
            *data = qRgba((val * gray + (0xFF - val) * qRed(*data)) >> 8,
                          (val * gray + (0xFF - val) * qGreen(*data)) >> 8,
                          (val * gray + (0xFF - val) * qBlue(*data)) >> 8,
                          qAlpha(*data));
            ++data;
        }
    }
}
Esempio n. 15
0
bool CartCommFri::configureHook(){
  //Set Cartesian Impedance - this overrides their position control-uggg x,y,z
  cart_imp_cmd_.stiffness.linear.x = 0;
  cart_imp_cmd_.stiffness.linear.y = 0;
  cart_imp_cmd_.stiffness.linear.z = 0;
  cart_imp_cmd_.stiffness.angular.x = 0;
  cart_imp_cmd_.stiffness.angular.y = 0;
  cart_imp_cmd_.stiffness.angular.z = 0;
  cart_imp_cmd_.damping.linear.x = 0;
  cart_imp_cmd_.damping.linear.y = 0;
  cart_imp_cmd_.damping.linear.z = 0;
  cart_imp_cmd_.damping.angular.x = 0;
  cart_imp_cmd_.damping.angular.y = 0;
  cart_imp_cmd_.damping.angular.z = 0;
  port_cart_imp_cmd_.write(cart_imp_cmd_);

  //Initialize outputs
  cart_wrench_cmd_.force.x = 0;
  cart_wrench_cmd_.force.y = 0;
  cart_wrench_cmd_.force.z = 0;
  cart_wrench_cmd_.torque.x = 0;
  cart_wrench_cmd_.torque.y = 0;
  cart_wrench_cmd_.torque.z = 0;
  
  //Assume zero initial velocity
  for (size_t ii(0); ii < 3; ++ii) {
    cart_vel_.push_back(0);
  }
  
  //Time setup
  t_start_ = RTT::os::TimeService::Instance()->getTicks();

  return true;
}
Esempio n. 16
0
bool HexFilesMenager::saveToHex(QString fileName)
{
    if(fileName.indexOf('.')==-1)fileName+=".hex";

    QByteArray data = this->getCurrentPage();

    IntelHexProvider p;
    if (data.size()>0xFFFFFF)
    {
        p.setAddressing(IntelHexProvider::iHEX_AMODE_32bit);
        p.setDataRecordLength(IntelHexProvider::iHEX_DRMODE_16bit);
    }
    else
    {
        p.setAddressing(IntelHexProvider::iHEX_AMODE_16bit);
        p.setDataRecordLength(IntelHexProvider::iHEX_DRMODE_16bit);
    }

    bool r = p.saveToHexFile(data ,fileName);

    if(r)
    {
        QFileInfo ii(fileName);
        tw->setTabText(tw->currentIndex(),ii.fileName());
        static_cast <HexEditorWidget *> (tw->currentWidget())->saved();
        on_dataSaved();
    }
    return r;
}
Esempio n. 17
0
ResourceSet ResourceInfo::toResourceSet( const ResourceList& rResourceList )
{
    ResourceSet set;
    std::insert_iterator<ResourceSet> ii( set, set.begin() );
    std::copy( rResourceList.begin(), rResourceList.end(), ii );
    return set;
}
void test() {
  /* Field testing: NWERC 2012 Problem I, Kattis pieceittogether */
  /* TODO: UVa 11294, UVa 10319 */

  for (int n = 1; n <= 15; n++) {
    for (int iter = 0; iter < 100; iter++) {
      int cnt = randint(1, 100);
      vii clauses(cnt);
      for (int i = 0; i < cnt; i++) {
        int a = randint(1, n) * (randint(1, 2) == 1 ? 1 : -1);
        int b = randint(1, n) * (randint(1, 2) == 1 ? 1 : -1);
        clauses[i] = ii(a, b);
      }
      // cout << n << " " << iter << " " << cnt << endl;

      TwoSat ts(n);
      iter(it,clauses) {
        ts.add_or(it->first, it->second);
      }
      if (ts.sat()) {
        vector<bool> is_true(n+1, false);
        // for (int i = 0; i < size(all_truthy); i++) if (all_truthy[i] > 0) is_true[all_truthy[i]] = true;
        rep(i,0,n) if (V[n+(i+1)].val == 1) is_true[i+1] = true;
        for (int i = 0; i < cnt; i++) {
          bool a = is_true[abs(clauses[i].first)],
             b = is_true[abs(clauses[i].second)];
          assert_true((clauses[i].first > 0 ? a : !a) || (clauses[i].second > 0 ? b : !b), true);
        }
      } else {
        for (int j = 0; j < (1<<n); j++) {
  jspace::Status OpspacePlanarController::
  setGains(jspace::Vector const & kp, jspace::Vector const & kd)
  {
    if ((kp.size() != ndof_) || (kd.size() != ndof_)) {
      return jspace::Status(false, "NDOF mismatch in provided gains");
    }
    
    op_kp_ = kp[0];
    op_kd_ = kd[0];
    op_kp_by_kd_ = fabs(op_kd_) < 1e-2 ? -1 : op_kp_ / op_kd_;
    op_vmax_ = kp[0] * 1e-3;
    
    jspace::Vector jkp(ndof_);
    jspace::Vector jkd(ndof_);
    ssize_t ii(2);
    ssize_t jj(0);
    for (/**/; jj < ndof_; ++jj) {
      if ((jj == q1_index_) || (jj == q2_index_)) {
	jkp[jj] = 0;
	jkd[jj] = 0;
      }
      else {
	jkp[jj] = kp[ii];
	jkd[jj] = kd[ii];
	++ii;
      }
    }
    
    return jgoal_controller_.setGains(jkp, jkd);
  }
  jspace::Status OpspacePlanarController::
  getGains(jspace::Vector & kp, jspace::Vector & kd) const
  {
    jspace::Vector jkp;
    jspace::Vector jkd;
    jspace::Status status(jgoal_controller_.getGains(jkp, jkd));
    if ( ! status) {
      return status;
    }
    
    kp.resize(ndof_);
    kd.resize(ndof_);
    kp[0] = op_kp_;
    kd[1] = op_kd_;
    kp[1] = op_vmax_ * 1e3;
    kd[1] = 0;
    for (ssize_t ii(0), off(2); ii < static_cast<ssize_t>(ndof_); ++ii) {
      if ((ii == q1_index_) || (ii == q2_index_)) {
	--off;
      }
      else {
	if (ii + off >= static_cast<ssize_t>(ndof_)) {
	  status.ok = false;
	  status.errstr = "bug in OpspacePlanarController::getGains() vector mixing";
	  return status;
	}
	kp[ii + off] = jkp[ii];
	kd[ii + off] = jkd[ii];
      }
    }
    
    return status;
  }
  jspace::Status OpspacePlanarController::
  setGoal(jspace::Vector const & goal)
  {
    if (goal.size() != ndof_) {
      return jspace::Status(false, "NDOF mismatch in provided goal");
    }
    
    double const gdist_sqr(pow(goal[0], 2) + pow(goal[1], 2));
    if (gdist_sqr <= gdist_sqr_min_) {
      return jspace::Status(false, "the goal is too close to the origin");
    }
    if (gdist_sqr >= gdist_sqr_max_) {
      return jspace::Status(false, "the goal is too far from the origin");
    }
    
    op_goal_.coeffRef(0) = goal[0];
    op_goal_.coeffRef(1) = goal[1];
    jspace::Vector jgoal(ndof_);
    ssize_t ii(2);
    ssize_t jj(0);
    for (/**/; jj < ndof_; ++jj) {
      if ((jj == q1_index_) || (jj == q2_index_)) {
	jgoal[jj] = 0;
      }
      else {
	jgoal[jj] = goal[ii];
	++ii;
      }
    }
    
    return jgoal_controller_.setGoal(jgoal);
  }
  jspace::Status OpspacePlanarController::
  getActual(jspace::Vector & actual) const
  {
    jspace::Vector jactual;
    jspace::Status status(jgoal_controller_.getActual(jactual));
    if ( ! status) {
      return status;
    }
    
    actual.resize(ndof_);
    actual[0] = op_position_[0];
    actual[1] = op_position_[1];
    for (ssize_t ii(0), off(2); ii < static_cast<ssize_t>(ndof_); ++ii) {
      if ((ii == q1_index_) || (ii == q2_index_)) {
	--off;
      }
      else {
	if (ii + off >= static_cast<ssize_t>(ndof_)) {
	  status.ok = false;
	  status.errstr = "bug in OpspacePlanarController::getActual() vector mixing";
	  return status;
	}
	actual[ii + off] = jactual[ii];
      }
    }
    
    return status;
  }
Esempio n. 23
0
    // input: multiple imgPtr, gtPtr (arrays of pointers)
    //          multiple img sizes
    // returns a BoosterModel *
    // -- BEWARE: this function is a mix of dirty tricks right now
    DLL_EXPORT
    void * trainWithChannels( ImagePixelType **imgPtr, 
                             void **evecPtr,
                             GTPixelType **gtPtr,
                             int *width, int *height, int *depth,
                             int numStacks,
                             IntegralImagePixelType **chImgPtr,
                             int numChannels, double zAnisotropyFactor,
                             int numStumps,
                             int gtNegativeLabel, int gtPositiveLabel,
                             int debugOutput )
    {

        BoosterModel *modelPtr = 0;

        try
        {
            std::unique_ptr<ROIData[]> rois(new ROIData[numStacks]);  // TODO: remove
            MultipleROIData allROIs;
            std::unique_ptr<ROIData::IntegralImageType[]> ii(new ROIData::IntegralImageType[numStacks*numChannels]);  // TODO: remove

            for (int i=0; i < numStacks; i++)
            {
                rois[i].setGTNegativeSampleLabel(gtNegativeLabel);
                rois[i].setGTPositiveSampleLabel(gtPositiveLabel);
                rois[i].init( imgPtr[i], gtPtr[i], 0, 0, width[i], height[i], depth[i], 
                              zAnisotropyFactor, 0.0, (const ROIData::RotationMatrixType *) evecPtr[i] );

                for (int ch=0; ch < numChannels; ch++)
                {
                   ii[i*numChannels+ch].fromSharedData(chImgPtr[i*numChannels+ch], width[i], height[i], depth[i]);

                   rois[i].addII( ii[i*numChannels+ch].internalImage().data() );
                }

                allROIs.add( shared_ptr_nodelete(ROIData, &rois[i]) );
            }

            BoosterInputData bdata;
            bdata.init( shared_ptr_nodelete(MultipleROIData, &allROIs) );
            bdata.showInfo();

            Booster adaboost;
            adaboost.setShowDebugInfo( debugOutput != 0 );

            adaboost.train( bdata, numStumps );

            // create by copying
            modelPtr = new BoosterModel( adaboost.model() );
        }
        catch( std::exception &e )
        {
            printf("Error training: %s\n", e.what());
            delete modelPtr;

            return 0;
        }

        return modelPtr;
    }
Esempio n. 24
0
template <typename PointInT, typename IntensityT> void
pcl::tracking::PyramidalKLTTracker<PointInT, IntensityT>::downsample (const FloatImageConstPtr& input,
                                                               FloatImageConstPtr& output) const
{
  FloatImage smoothed (input->width, input->height);
  convolve (input, smoothed);

  int width = (smoothed.width +1) / 2;
  int height = (smoothed.height +1) / 2;
  std::vector<int> ii (width);
  for (int i = 0; i < width; ++i)
    ii[i] = 2 * i;

  FloatImagePtr down (new FloatImage (width, height));
#ifdef _OPENMP
#pragma omp parallel for shared (output) firstprivate (ii) num_threads (threads_)
#endif
  for (int j = 0; j < height; ++j)
  {
    int jj = 2*j;
    for (int i = 0; i < width; ++i)
      (*down) (i,j) = smoothed (ii[i],jj);
  }

  output = down;
}
Esempio n. 25
0
uint64_t RoutingTable::networkEnvironmentFingerprint(const std::vector<std::string> &ignoreInterfaces) const
{
	uint64_t fp = 0;
	std::vector<Entry> rtab(get());
	for(std::vector<Entry>::const_iterator re(rtab.begin());re!=rtab.end();++re) {
		bool skip = false;
		for(std::vector<std::string>::const_iterator ii(ignoreInterfaces.begin());ii!=ignoreInterfaces.end();++ii) {
			if (*ii == re->device) {
				skip = true;
				break;
			}
		}
		if (skip)
			continue;
		++fp;
		if (re->destination.isV4()) {
			fp = Utils::sdbmHash(re->destination.rawIpData(),4,fp);
			fp = Utils::sdbmHash((uint16_t)re->destination.netmaskBits(),fp);
		} else if (re->destination.isV6()) {
			fp = Utils::sdbmHash(re->destination.rawIpData(),16,fp);
			fp = Utils::sdbmHash((uint16_t)re->destination.netmaskBits(),fp);
		}
		if (re->gateway.isV4()) {
			fp = Utils::sdbmHash(re->gateway.rawIpData(),4,fp);
			fp = Utils::sdbmHash((uint16_t)re->gateway.netmaskBits(),fp);
		} else if (re->gateway.isV6()) {
			fp = Utils::sdbmHash(re->gateway.rawIpData(),16,fp);
			fp = Utils::sdbmHash((uint16_t)re->gateway.netmaskBits(),fp);
		}
		fp = Utils::sdbmHash(re->device,fp);
		fp = Utils::sdbmHash((uint32_t)re->metric,fp);
	}
	return fp;
}
Esempio n. 26
0
void object::test<3>()
{
    Keyed one("one"), two("two"), three("three");
    // We don't want to rely on the underlying container delivering keys
    // in any particular order. That allows us the flexibility to
    // reimplement LLInstanceTracker using, say, a hash map instead of a
    // std::map. We DO insist that every key appear exactly once.
    typedef std::vector<std::string> StringVector;
    StringVector keys(Keyed::beginKeys(), Keyed::endKeys());
    std::sort(keys.begin(), keys.end());
    StringVector::const_iterator ki(keys.begin());
    ensure_equals(*ki++, "one");
    ensure_equals(*ki++, "three");
    ensure_equals(*ki++, "two");
    // Use ensure() here because ensure_equals would want to display
    // mismatched values, and frankly that wouldn't help much.
    ensure("didn't reach end", ki == keys.end());

    // Use a somewhat different approach to order independence with
    // beginInstances(): explicitly capture the instances we know in a
    // set, and delete them as we iterate through.
    typedef std::set<Keyed*> InstanceSet;
    InstanceSet instances;
    instances.insert(&one);
    instances.insert(&two);
    instances.insert(&three);
    for (Keyed::instance_iter ii(Keyed::beginInstances()), iend(Keyed::endInstances());
            ii != iend; ++ii)
    {
        Keyed& ref = *ii;
        ensure_equals("spurious instance", instances.erase(&ref), 1);
    }
    ensure_equals("unreported instance", instances.size(), 0);
}
Esempio n. 27
0
 Status BaseMultiPos::
  init(Model const & model)
  {
    Status st(Skill::init(model));
    if ( ! st) { return st; }
    
    if (0 != ee_pos_.rows()%3){
      st.ok = false;
      st.errstr = "wrong number of rows in ee_pos vector";
      return st;
    }

    if (0 > threshold_) {
      threshold_ = 0.08;
    }
    
    if (0 > vel_threshold_) {
      vel_threshold_ = 0.08;
    }

    ee_goal_ = ee_task_->lookupParameter("trjgoal", PARAMETER_TYPE_VECTOR);

    if ( ! ee_goal_ ) {
      st.ok = false;
      st.errstr = "no appropriate goalpos parameter in ee_task";
      return st;
    }

    task_table_.push_back(ee_task_);
     
   for (size_t ii(0); ii < task_table_.size(); ++ii) {
      st = task_table_[ii]->init(model);
      if ( ! st) {
	return st;
      }
    }

   Vector cur_eepos(Vector::Zero(3));
   for (size_t ii(0); ii < 3; ++ii) {
     cur_eepos[ii] = ee_pos_[3*cur_row_+ii];
   }

   st = ee_goal_->set(cur_eepos);
   if (!st) { return st; }

   return st;
  }
Esempio n. 28
0
int main(int argc, char ** argv) {

  char* def_node = "vis_udp_sever";
  char* def_out = "/visualize_lwr";
  int m_local_port = 11111;
  if (argc >= 2) {
    def_node = argv[1];
  }
  if (argc >= 3) {
    def_out = argv[2];
  }
  if (argc == 4) {
    m_local_port = atoi(argv[3]);
  }

  int m_socket;
  struct sockaddr m_remote_addr;
  double m_msr_data[7];
  std_msgs::Float32MultiArray out;
  out.data.resize(7);
  socklen_t m_sock_addr_len;

  ros::init(argc, argv, def_node);
  ros::NodeHandle node("~");

  ros::Publisher robot_pub = node.advertise<std_msgs::Float32MultiArray>( def_out, 1 );

  if (m_socket != 0)
 	close(m_socket);
  m_socket = socket(AF_INET, SOCK_DGRAM, IPPROTO_UDP);
  setsockopt(m_socket, SOL_SOCKET, SO_REUSEADDR, 0, 0);
  int sndbuf(sizeof(m_msr_data));
  setsockopt(m_socket, SOL_SOCKET, SO_RCVBUF, &sndbuf, sizeof(sndbuf));

  struct sockaddr_in local_addr;
  bzero((char *) &local_addr, sizeof(local_addr));
  local_addr.sin_family = AF_INET;
  local_addr.sin_addr.s_addr = INADDR_ANY;
  local_addr.sin_port = htons(m_local_port);

  if (bind(m_socket, (sockaddr*) &local_addr, sizeof(sockaddr_in)) < 0) {
 	ROS_WARN("Binding of port failed with errno %d",errno);
  }
  
  ros::Rate r(60);
  while (ros::ok()) { 
    m_sock_addr_len = sizeof(m_remote_addr);
    int n = recvfrom(m_socket, (void*) &m_msr_data, sizeof(m_msr_data), 0,
			(sockaddr*) &m_remote_addr, &m_sock_addr_len); 
    for (size_t ii(0); ii < 7; ++ii) {
      out.data[ii] = m_msr_data[ii];
    }
    robot_pub.publish(out);
    ros::spinOnce();
    r.sleep();
  }
  close(m_socket);
  ros::shutdown();
}
// O(log(V) * E)
vi dijkstra(vector<vii> &g, int V, int s) {
	vi dist(V, INF); dist[s] = 0;
	priority_queue< ii, vector<ii>, greater<ii> > pq; pq.push(ii(0, s));
	while (!pq.empty()) {
		ii front = pq.top(); pq.pop();
		int d = front.first, u = front.second;
		if (d > dist[u]) continue;
		for (int i = 0; i < g[u].size(); i++) {
			ii v = g[u][i];
			if (dist[v.first] > dist[u] + v.second) {
				dist[v.first] = dist[u] + v.second;
				pq.push(ii(dist[v.first], v.first));
			}
		}
	}
	return dist;
}
Esempio n. 30
0
float TThresholdCA::operator()(PClassifier classifier, PExampleGenerator data, const int &weightID, float &optCA, const int &targetValue, TFloatFloatList *CAs)
{
  if (!data->domain->classVar)
    raiseError("classless domain");
  if (data->domain->classVar != classifier->classVar)
    raiseError("classifier's class variables mismatches the given examples'");
  TEnumVariable *classVar = data->domain->classVar.AS(TEnumVariable);
  if (!classVar)
    raiseError("discrete class expected");

  int wtarget;
  if (targetValue >= 0)
    wtarget = targetValue;
  else if (classVar->baseValue >= 0)
    wtarget = classVar->baseValue;
  else if (classVar->values->size() == 2)
    wtarget = 1;
  else
    raiseError("cannot determine target class: none is given, class is not binary and its 'baseValue' is not set");

  typedef map<float, float> tmfpff;
  tmfpff dists;
  float N = 0.0, corr = 0.0;
  PEITERATE(ei, data) 
    if (!(*ei).getClass().isSpecial()) {
      float wei = WEIGHT(*ei);
      N += wei;
      if ((*ei).getClass().intV == wtarget) {
        corr += wei;
        wei = -wei;
      }

      const float prob = classifier->classDistribution(*ei)->atint(wtarget);
      pair<tmfpff::iterator, bool> elm = dists.insert(make_pair(prob, wei));
      if (!elm.second)
        (*elm.first).second += wei;
    }

  optCA = 0;

  if (dists.size() < 2)
    return 0.5;
    
  float optthresh;
  for(tmfpff::const_iterator ni(dists.begin()), ie(dists.end()), ii(ni++); ni != ie; ii = ni++) {
    corr += (*ii).second;
    if ((corr > optCA) || ((corr == optCA) && ((*ii).first < 0.5))) {
      optCA = corr;
      optthresh = ((*ii).first + (*ni).first) / 2.0;
    }
    if (CAs)
      CAs->push_back(make_pair(((*ii).first + (*ni).first) / 2.0, corr/N));
  }

  optCA /= N;
  return optthresh;
}