Exemple #1
0
void
ITM<ITM_TRAITS>::handleDeletions( node_type best, node_type second )
{
    observation_type & bestCentroid = graph_[best].centroid;
    observation_type & secondCentroid = graph_[second].centroid;

    // Don't know of other way to avoid iterator invalidation than to do this
    // twice.
    out_edge_iterator iChild;
    out_edge_iterator eChild;
    typename std::list<node_type> erase;

    for ( boost::tie( iChild, eChild ) = boost::out_edges( best, graph_ );
            iChild != eChild; ++iChild
        ) {
        node_type child = boost::target( *iChild, graph_ );
        if ( child != best ) { // Do not try to delete self-edges
            observation_type centroid = graph_[child].centroid;
            observation_type center = ( bestCentroid + centroid ) * 0.5;
            if (      distance_( center, secondCentroid )
                      < distance_( center, centroid )
               ) {
                erase.push_back( child );
            }
        }
    }

    typename std::list<node_type>::iterator iErase;
    typename std::list<node_type>::iterator eErase = erase.end();
    for ( iErase = erase.begin(); iErase != eErase; ++iErase ) {
        boost::remove_edge( best, *iErase, graph_ );
        boost::remove_edge( *iErase, best, graph_ );

        boost::tie( iChild, eChild ) = boost::out_edges( *iErase, graph_ );

        // The or condition is to handle self-edges
        if (   iChild == eChild
                || (    std::distance( iChild, eChild ) == 1
                        && boost::target( *iChild, graph_ ) == *iErase )
           ) {
            boost::clear_vertex( *iErase, graph_ );
            boost::remove_vertex( *iErase, graph_ );
        }
    }
}
static void dispatch(int n, int m, double *x_ptr, double *c_ptr, double *d_ptr, int nlhs, mxArray *plhs[])
{
  // Read parameters
  typename KDTree<K, double>::points_type X;
  
  typename KDTree<K, double>::point_type v;
  
  for (int i = 0; i < n; ++i)
    {
      for (unsigned k = 0; k < K; ++k)
	v[k] = x_ptr[i + (n * k)];
      X.push_back(v);
    }

  typename KDTree<K, double>::points_type C;

  std::vector<double> R;

  for (int i = 0; i < m; ++i)
    {
      for (unsigned k = 0; k < K; ++k)
	v[k] = c_ptr[i + (m * k)];
      C.push_back(v);
      R.push_back(d_ptr[i]);
    }

  // Build kd-tree
  KDTree<K, double> kdtree(X);

  // Compute queries
  std::list<typename KDTree<K, double>::set_type > res_list;
  typename KDTree<K, double>::set_type res;
	
  for (int i = 0; i < m; ++i)
    {
      kdtree.ball_query(C[i], R[i], res);

      res_list.push_back(res);
			
      res.clear();
    }

  // Write output
  if (nlhs > 0)
    {
      const mwSize size[2] = {res_list.size(), 1};
      plhs[0] = mxCreateCellArray(2, size);
			
      int cnt = 0;
      for (typename std::list<typename KDTree<K, double>::set_type>::const_iterator it = res_list.begin(); it != res_list.end(); ++it, ++cnt)
	{
	  if (it->size())
	    {
	      mxArray * pArray = mxCreateDoubleMatrix(it->size(), 1, mxREAL);
	      double * p = mxGetPr(pArray);
				
	      for (typename KDTree<K, double>::set_type::const_iterator it2 = it->begin(); it2 != it->end(); ++it2)
		*p++ = it2->second + 1;
				
	      mxSetCell(plhs[0], cnt, pArray);
	    }
	}
    }

  if (nlhs > 1)
    {
      const mwSize size[2] = {res_list.size(), 1};
      plhs[1] = mxCreateCellArray(2, size);
			
      int cnt = 0;
      for (typename std::list<typename KDTree<K, double>::set_type>::const_iterator it = res_list.begin(); it != res_list.end(); ++it, ++cnt)
	{
	  if (it->size())
	    {
	      mxArray * pArray = mxCreateDoubleMatrix(it->size(), 1, mxREAL);
	      double * p = mxGetPr(pArray);
				
	      for (typename KDTree<K, double>::set_type::const_iterator it2 = it->begin(); it2 != it->end(); ++it2)
		*p++ = it2->first;
				
	      mxSetCell(plhs[1], cnt, pArray);
	    }
	}
    }
}
Exemple #3
0
template <typename Robot> double SAC<Robot>::sac_ctrl(typename std::list<typename Robot::U> & list_u, typename Robot::U umin, typename Robot::U umax, double K, typename Robot::U u0)
{
	double J1=J0;

	typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.cbegin();
	typename std::list<typename Robot::State>::const_iterator itr_state=list_states.cbegin();
	typename std::list<typename Robot::coState>::const_iterator itr_costate=list_costates.cbegin();

	double Ti=params.Tc+params.Ts;

	params.alpha*=J0;

	double t;


	for(t=sim.dt*1e-3; t<params.Tcal; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
		list_u2.push_back(*itr_u1);

	typename Robot::U u_curr;
	if(list_u2.empty())
		u_curr=u0;
	else
		u_curr=list_u2.front();

	for(; t<Ti; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
	{
		Eigen::Matrix<double,Robot::M,Robot::N> h=Robot::h(sys,*itr_state);
		Eigen::Matrix<double,Robot::N,1> gamma=h.transpose()*(itr_costate->costate);
		Eigen::Matrix<double,Robot::N,Robot::N> lamada=gamma*gamma.transpose();

		Eigen::Matrix<double,Robot::N,1> u=*itr_u1+(lamada+params.R).ldlt().solve(gamma*params.alpha);

		for(int i=0;i<Robot::N;i++)
			if(u(i)<umin(i))
				u(i)=umin(i);
			else
				if(u(i)>umax(i))
					u(i)=umax(i);

		list_u2.push_back(u);
	}

	int index=-1;

	double Aht=exp(-K*sim.dt*0.5);
	std::list<typename Robot::U> list_ud;

	double Tc=params.Tc;

	for(int n=0; n<params.kmax;n++, Tc/=2.0)
	{
		typename std::list<typename Robot::Ref>::const_iterator itr_ref;
		typename std::list<typename Robot::U>::const_iterator itr_u;


		double Tm=params.Tcal+Tc; 

		sim.init(list_states.front());
		
		itr_u=list_u2.begin();
	
		std::list<typename Robot::U> list_ut;

		double t=sim.dt*1e-3;
		for(; t<params.Tcal; t+=sim.dt, itr_u++)
		{
			sim.update(*itr_u);
			list_ut.push_back(*itr_u);
		}

		for(; t<Tm; t+=sim.dt, itr_u++)
		{
			u_curr=*itr_u+Aht*(u_curr-*itr_u);
			sim.update(u_curr);
			list_ut.push_back(u_curr);

			u_curr=*itr_u+Aht*(u_curr-*itr_u);
		}

		itr_u=list_u1.begin();
		std::advance(itr_u, int(Tm/sim.dt+0.5));
		for(; t<params.Tp; t+=sim.dt, itr_u++)
		{
			u_curr=*itr_u+Aht*(u_curr-*itr_u);
			sim.update(u_curr);
			list_ut.push_back(u_curr);

			u_curr=*itr_u+Aht*(u_curr-*itr_u);
		}

		double Jn=J(sim.get_states());
		
		if (Jn<J1)
		{
			J1=Jn;
			index=n;
			list_ud=list_ut;
		}
	}	

	if(list_u.size()<int(params.Tp/sim.dt))
		list_u=list_u1;

	if(index>=0)
	{
		typename std::list<typename Robot::U>::iterator itr_u=list_u.begin();
		typename std::list<typename Robot::U>::const_iterator itr_ud=list_ud.begin();

		for(;itr_ud!=list_ud.end();)
			*itr_u++=*itr_ud++;

//		std::cout<<index<<std::endl;
	}
//	else
//	{
//		std::cout<<"Failture"<<std::endl;
//	}
	return J1;
}
Exemple #4
0
template <typename Robot> double SAC<Robot>::sac_ctrl(typename std::list<typename Robot::U> & list_u, typename Robot::U umin, typename Robot::U umax, typename Robot::U du, typename Robot::U u0)
{
	double J1=J0;

	typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.cbegin();
	typename std::list<typename Robot::State>::const_iterator itr_state=list_states.cbegin();
	typename std::list<typename Robot::coState>::const_iterator itr_costate=list_costates.cbegin();

	double Ti=params.Tc+params.Ts;

	params.alpha*=J0;

	double t;

	typename Robot::U u_curr;

	for(t=0; t<params.Tcal-1e-10; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
		list_u2.push_back(*itr_u1);

	if(list_u2.empty())
		u_curr=u0;
	else
		u_curr=list_u2.front();

	for(; t<Ti-1e-10; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
	{
		Eigen::Matrix<double,Robot::M,Robot::N> h=Robot::h(sys,*itr_state);
		Eigen::Matrix<double,Robot::N,1> gamma=h.transpose()*(itr_costate->costate);
		Eigen::Matrix<double,Robot::N,Robot::N> lamada=gamma*gamma.transpose();

		Eigen::Matrix<double,Robot::N,1> u=*itr_u1+(lamada+params.R).ldlt().solve(gamma*params.alpha);

		typename Robot::U u_min=u_curr-du;
		typename Robot::U u_max=u_curr+du;

		for(int i=0;i<Robot::N;i++)
		{
			if(u_min(i)<umin(i))
				u_min(i)=umin(i);

			if(u_max(i)>umax(i))
				u_max(i)=umax(i);
		}

		for(int i=0;i<Robot::N;i++)
			if(u(i)<u_min(i))
				u(i)=u_min(i);
			else
				if(u(i)>u_max(i))
					u(i)=u_max(i);

		list_u2.push_back(u);
		
		u_curr=u;
	}

	int index=-1;

	double Tc=params.Tc;
	double Tm=params.Tcal+Tc; 

	sim.init(list_states.front());
	for(int n=0; n<params.kmax; n++)
	{
		typename std::list<typename Robot::U>::const_iterator itr_u;

		itr_u=list_u2.begin();

		double t;
		if(!n)
			for(t=sim.dt*1e-3; t<Tm; t+=sim.dt, itr_u++)
				sim.update(*itr_u);
		else
			t=Tm+sim.dt*1e-3;
	
		itr_u=list_u1.begin();
		
		std::advance(itr_u, int(Tm/sim.dt+0.5));

		for(; t<params.Tp; t+=sim.dt, itr_u++)
			sim.update(*itr_u);

		double Jn=J(sim.get_states());
		
		if (Jn<J1)
		{
			J1=Jn;
			index=n;
		}

		Tc/=2.0;
		Tm=params.Tcal+Tc;
		sim.erase_last_n(int((params.Tp-Tm)/sim.dt+0.5));
	}

/*************************************************************************
	for(int n=0; n<params.kmax;n++, Tc/=2.0)
	{
		typename std::list<typename Robot::U>::const_iterator itr_u;

		double Tm=params.Tcal+Tc; 

		sim.init(list_states.front());
		
		itr_u=list_u2.begin();
	
		double t;
		for(t=sim.dt*1e-3; t<Tm; t+=sim.dt, itr_u++)
			sim.update(*itr_u);
	
		itr_u=list_u1.begin();
		
		std::advance(itr_u, int(Tm/sim.dt+0.5));

		for(; t<params.Tp; t+=sim.dt, itr_u++)
			sim.update(*itr_u);

		double Jn=J(sim.get_states());
		
		if (Jn<J1)
		{
			J1=Jn;
			index=n;
		}
	}	
*************************************************************************/

	if(list_u.size()<int(params.Tp/sim.dt))
		list_u=list_u1;

	if(index>=0)
	{
		double Tm=params.Tcal+params.Tc/pow(2.0,index);
		typename std::list<typename Robot::U>::iterator itr_u=list_u.begin();
		typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.begin();
		typename std::list<typename Robot::U>::const_iterator itr_u2=list_u2.begin();

		double t=0;
		for(; t<Tm-1e-10; t+=sim.dt, itr_u1++)
			*itr_u++=*itr_u2++;

		for(; t<params.Tp-1e-10; t+=sim.dt)
			*itr_u++=*itr_u1++;

//		std::cout<<index<<std::endl;
	}
//	else
//	{
//		std::cout<<"Failture"<<std::endl;
//	}
	return J1;
}
Exemple #5
0
template <typename Robot> double SAC<Robot>::sac_ctrl(typename std::list<typename Robot::U> & list_u, typename Robot::U umin, typename Robot::U umax)
{
	double J1=J0;

	typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.cbegin();
	typename std::list<typename Robot::State>::const_iterator itr_state=list_states.cbegin();
	typename std::list<typename Robot::coState>::const_iterator itr_costate=list_costates.cbegin();

	double Ti=params.Tc+params.Ts;

	params.alpha*=J0;

	double t;

	for(t=0; t<params.Tcal-1e-10; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
		list_u2.push_back(*itr_u1);

	for(; t<Ti-1e-10; t+=sim.dt, itr_u1++, itr_state++, itr_costate++)
	{
		Eigen::Matrix<double,Robot::M,Robot::N> h=Robot::h(sys,*itr_state);
		Eigen::Matrix<double,Robot::N,1> gamma=h.transpose()*(itr_costate->costate);
		Eigen::Matrix<double,Robot::N,Robot::N> lamada=gamma*gamma.transpose();

		Eigen::Matrix<double,Robot::N,1> u=(lamada+params.R).ldlt().solve(lamada*(*itr_u1)+gamma*params.alpha);

		for(int i=0;i<Robot::N;i++)
			if(u(i)<umin(i))
				u(i)=umin(i);
			else
				if(u(i)>umax(i))
					u(i)=umax(i);

		list_u2.push_back(u);
	}

	int index=-1;

	double Tc=params.Tc;

	for(int n=0; n<params.kmax;n++, Tc/=2.0)
	{
		double t;
		typename std::list<typename Robot::Ref>::const_iterator itr_par;
		typename std::list<typename Robot::U>::const_iterator itr_u;

		double Tm=params.Tcal+Tc; 

		sim.init(list_states.front());
		
		itr_u=list_u2.begin();
	
		for(t=0; t<Tm-1e-10; t+=sim.dt, itr_u++)
			sim.update(*itr_u);
	
		itr_u=list_u1.begin();
		
		std::advance(itr_u, int(Tm/sim.dt+0.5));

		for(; t<params.Tp-1e-10; t+=sim.dt, itr_u++)
			sim.update(*itr_u);

		double Jn=J(sim.get_states());
	
		if (Jn<J1)
		{
			J1=Jn;
			index=n;
		}
	}

	if(list_u.size()<int(params.Tp/sim.dt))
		list_u=list_u1;

	if(index>=0)
	{
		double Tm=params.Tcal+params.Tc/pow(2.0,index);
		typename std::list<typename Robot::U>::iterator itr_u=list_u.begin();
		typename std::list<typename Robot::U>::const_iterator itr_u1=list_u1.begin();
		typename std::list<typename Robot::U>::const_iterator itr_u2=list_u2.begin();

		double t=0;
		for(; t<Tm-1e-10; t+=sim.dt, itr_u1++)
			*itr_u++=*itr_u2++;

		for(; t<params.Tp-1e-10; t+=sim.dt)
			*itr_u++=*itr_u1++;
	}

	return J1;
}