unsigned long count(const K& key)
{
unsigned long tot = 0;
typename std::list<Pair<K, V> >::iterator debut = _liste.begin();
typename std::list<Pair<K, V > >::iterator fin = _liste.end();
for (; debut != fin; ++debut)
{
if (key == debut->getKey())
tot++;
}
return tot;
}
const std::list< Pair<K, V> >* find(const K& key)
{
std::list< Pair<K, V> >* new_liste = new std::list< Pair<K, V> >();
typename std::list<Pair<K, V> >::iterator debut = _liste.begin();
typename std::list<Pair<K, V > >::iterator fin = _liste.end();
for (; debut != fin; ++debut)
{
if (debut->getKey() == key)
{
Pair<K, V> p(debut->getKey(), debut->getValue());
new_liste->push_back(p);
}
}
return new_liste;
}
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_ );
}
}
}
float do_kmeans( const std::vector<sample_type>& samples)
{
for( int iters = 0; iters < max_iters_; ++iters)
{
int num_swaps = assign_nearest_cluster( samples);
if( num_swaps == 0)
break;
for( typename std::list<cluster_t>::iterator it( clusters_.begin()); it != clusters_.end(); ++it)
{
it->num_samples = 0;
it->radius = traits_type::zero();
for( int i = 0; i < samples.size(); ++i)
{
if( labels_[i] == &(*it))
it->add_sample( samples[i]);
}
if( !it->empty())
it->mean /= it->num_samples;
for( int i = 0; i < samples.size(); ++i)
{
if( labels_[i] == &(*it))
it->update_radius( samples[i]);
}
}
clusters_.erase( std::remove_if( clusters_.begin(), clusters_.end(),
boost::bind( &kmeans_t::cluster_t::empty, _1)), clusters_.end());
RAMEN_ASSERT( !clusters_.empty());
}
float max_radius = traits_type::zero();
for( const_iterator it( begin()); it != end(); ++it)
max_radius = std::max( max_radius, it->radius);
return max_radius;
}
template <typename Robot> double SAC<Robot>::J(typename std::list<typename Robot::State> const & list_s)
{
typename std::list<typename Robot::State>::const_iterator itr_state=list_s.cbegin();
typename std::list<typename Robot::Ref>::const_iterator itr_ref=list_refs.cbegin();
double J=L(*itr_state, *itr_ref);
itr_state++;
std::advance(itr_ref,2);
size_t N=list_s.size();
for(int n=2; n<N; n++, itr_state++, std::advance(itr_ref,2))
J+=2*L(*itr_state, *itr_ref);
J+=L(*itr_state, *itr_ref);
J=J*sim.dt/2.0+Phi(*itr_state, *itr_ref);
return J;
}
/**
* @param[in] dp Center of 3D point (dp[0], dp[1], dp[2]).
* @param[in] radius Radius of a sphere.
* @param[out] result resutl of the points.
*/
void find(const T &pnt, const double radius, typename std::list<T>& result) {
if (this->_list.empty()) return;
if (this->isLeaf()) {
typename std::list<T>::iterator iter;
const double sqr = radius * radius;
for (iter = this->_list.begin() ; iter != this->_list.end() ; iter++) {
double check = 0;
for ( size_t i = 0 ; i < Dim ; i++) check += (iter->operator[](i) - pnt[i])*(iter->operator[](i) - pnt[i]);
if ( check <= sqr ) result.push_back(*iter);
}
} else {
const double p = this->_list.begin()->operator[](this->_dim);
const double x = pnt[this->_dim];
if ( fabs(x-p) <= radius ) {
this->_child[0].find(pnt, radius, result);
this->_child[1].find(pnt, radius, result);
} else {
if ( x < p )this->_child[0].find(pnt, radius, result);
else this->_child[1].find(pnt, radius, result);
}
}
return;
}
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);
}
}
}
}
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;
}
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;
}
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;
}
