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