void GridManager::LinkCells() {
const Vector3i& grid_size = Config::vGridSize;
for (int x = 0; x < grid_size.x(); x++) {
for (int y = 0; y < grid_size.y(); y++) {
for (int z = 0; z < grid_size.z(); z++) {
Vector2i v_p(x, y);
InitCellData* p_init_cell = grid_->GetInitCell(v_p);
const GasesConfigsMap& init_conds = p_init_cell->m_mInitConds;
if (p_init_cell->m_eType == sep::EMPTY_CELL)
continue;
sep::NeighborType neighbor;
sep::Axis ax;
int q;
// Link to normal
FindNeighbour(v_p, sep::NORMAL_CELL, ax, neighbor, q);
for (int i = 0; i < q; i++) {
FindNeighbourWithIndex(v_p, sep::NORMAL_CELL, i, ax, neighbor);
LinkNeighbors(v_p, ax, neighbor);
}
if (p_init_cell->m_eType != sep::FAKE_CELL) {
// Link to fake
FindNeighbour(v_p, sep::FAKE_CELL, ax, neighbor, q);
for (int i = 0; i < q; i++) {
FindNeighbourWithIndex(v_p, sep::FAKE_CELL, i, ax, neighbor);
LinkNeighbors(v_p, ax, neighbor);
}
}
}
}
}
}
void GridManager::FillInGrid() {
const Vector3i& grid_size = Config::vGridSize;
for (int x = 0; x < grid_size.x(); x++) {
for (int y = 0; y < grid_size.y(); y++) {
for (int z = 0; z < grid_size.z(); z++) {
Vector2i v_p(x, y);
if (grid_->GetInitCell(v_p)->m_eType == sep::EMPTY_CELL)
continue;
Cell* p_cell = new Cell();
grid_->AddCell(p_cell);
grid_->GetInitCell(v_p)->m_pCell = p_cell;
}
}
}
}
void GridManager::PrintLinkage(sep::Axis axis) {
const Vector3i& grid_size = Config::vGridSize;
for (int z = 0; z < grid_size.z(); z++) {
for (int y = 0; y < grid_size.y(); y++) {
for (int x = 0; x < grid_size.x(); x++) {
Vector2i v_p(x, y);
if (grid_->GetInitCell(v_p)->m_eType == sep::EMPTY_CELL) {
std::cout << "x ";
continue;
}
std::cout << grid_->GetInitCell(v_p)->m_pCell->m_vType[axis] << " ";
}
std::cout << std::endl;
}
}
std::cout << std::endl;
}
void GridManager::AdoptInitialCells() {
const Vector3i& grid_size = Config::vGridSize;
for (int x = 0; x < grid_size.x(); x++) {
for (int y = 0; y < grid_size.y(); y++) {
for (int z = 0; z < grid_size.z(); z++) {
Vector2i v_p(x, y);
InitCellData* init_cell = grid_->GetInitCell(v_p);
GasesConfigsMap& init_conds = init_cell->m_mInitConds;
if (init_cell->m_eType != sep::FAKE_CELL)
continue;
sep::NeighborType e_neighbor;
sep::Axis e_ax;
int i_q;
// Remove alone fake cells (which without normal neighbour)
FindNeighbour(v_p, sep::NORMAL_CELL, e_ax, e_neighbor, i_q);
if (!i_q)
init_cell->m_eType = sep::EMPTY_CELL;
}
}
}
}
/*------------------------------------------------------------------*/
void AzTaskTools::eval(const char *ite_str,
AzLossType loss_type,
const AzIntArr *ia_dx,
const double p_coeff[2], /* for normalized loss, may be NULL */
const AzDvect *v_test_pval,
const AzDvect *v_test_yval, /* assume y in {+1,-1} */
const char *tt_eyec,
const AzOut &test_out,
AzPerfResult *result)
{
bool doBreakEven = true;
int data_num = v_test_pval->rowNum();
const double *p = v_test_pval->point();
const double *y = v_test_yval->point();
if (v_test_yval->rowNum() != data_num) {
throw new AzException("AzTaskTools::eval", "#data conflict");
}
AzIntArr ia_cat_breakEven;
double tp=0, tn=0, fp=0, fn=0;
double posi=0, rmse=0;
int num = data_num;
const int *dxs = NULL;
if (ia_dx != NULL) {
dxs = ia_dx->point(&num);
}
AzDvect v_p(num);
int ix;
for (ix = 0; ix < num; ++ix) {
int dx = ix;
if (dxs != NULL) dx = dxs[ix];
double p_val = p[dx];
if (p_val > 0) {
if (y[dx] > 0) ++tp;
else ++fp;
}
else {
if (y[dx] > 0) ++fn;
else ++tn;
}
if (y[dx] > 0) {
++posi;
}
if (doBreakEven) {
if (y[dx] > 0) ia_cat_breakEven.put(0); /* target */
else ia_cat_breakEven.put(1); /* other */
v_p.set(ix, p_val);
}
double diff = p_val - y[dx];
rmse += (diff*diff);
}
double uloss_avg =
analyzeLoss(loss_type, v_test_pval, v_test_yval, ia_dx, 1);
double nloss1_avg = 0, nloss2_avg = 0;
if (AzLoss::isExpoFamily(loss_type) &&
p_coeff != NULL) {
nloss1_avg = analyzeLoss(loss_type, v_test_pval, v_test_yval, ia_dx, p_coeff[0]);
nloss2_avg = analyzeLoss(loss_type, v_test_pval, v_test_yval, ia_dx, p_coeff[1]);
}
rmse = sqrt(rmse/(double)num);
double acc = (tp+tn) / (double)num;
double precision = 0;
if (tp + fp > 0) {
precision = tp / (tp + fp);
}
double recall = 0;
if (posi > 0) {
recall = tp / posi;
}
double f1 = 0;
if (precision + recall > 0) {
f1 = 2 * precision * recall / (precision + recall);
}
double breakEven_f=-1,breakEven_acc=-1;
if (doBreakEven) {
AzStrPool sp_cat_breakEven;
sp_cat_breakEven.put("target");
sp_cat_breakEven.put(AzOtherCat);
eval_breakEven(&ia_cat_breakEven, &v_p,
&sp_cat_breakEven, tt_eyec,
&breakEven_f, &breakEven_acc);
}
if (!test_out.isNull()) {
AzPrint o(test_out);
o.printBegin("", ",");
o.print(ite_str); o.print_cont(tt_eyec, "-ite", ite_str);
o.print("prf"); o.print(precision,4); o.print(recall,4); o.print(f1,4);
o.print("acc", acc,4);
o.print("be_f", breakEven_f,4);
o.print("be_acc", breakEven_acc,4);
o.print("rmse", rmse,4);
o.print(AzLoss::lossName(loss_type));
o.print("uloss", uloss_avg, 5);
if (AzLoss::isExpoFamily(loss_type)) {
o.print("nloss1", nloss1_avg, 5);
o.print("nloss2", nloss2_avg, 5);
}
o.print("ok",(int)tp); o.print("t",(int)(tp+fp)); o.print("g",(int)posi);
o.print("#data", num);
o.printEnd();
}
if (result != NULL) {
result->put(precision, recall, f1, acc, breakEven_f, breakEven_acc, rmse, uloss_avg);
}
}
void GridManager::InitCells() {
GasVector& gases = Config::vGas;
double min_mass = 100.0;
double max_mass = 0.0;
std::for_each(gases.begin(), gases.end(), [&](Gas* gas) {
const double& m = gas->getMass();
min_mass = std::min(m, min_mass);
max_mass = std::max(m, max_mass);
});
double max_impulse = GetSolver()->GetImpulse()->getMaxImpulse();
// Set parameters
Vector2d cell_size = Config::vCellSize; // in mm
cell_size /= 1e3; // in m
cell_size /= Config::l_normalize; // normalized
Vector3d area_step(cell_size.x(), cell_size.y(), 0.1);
// decreasing time step if needed
double time_step = min_mass * std::min(area_step.x(), area_step.y()) / max_impulse;
//Config::dTimestep = std::min(Config::dTimestep, time_step);
Config::dTimestep = time_step;
//std::cout << "Time step: " << Config::dTimestep << " s" << std::endl;
std::cout << "Time step: " << Config::dTimestep * Config::tau_normalize << " s" << std::endl;
const Vector3i& grid_size = Config::vGridSize;
for (int x = 0; x < grid_size.x(); x++) {
for (int y = 0; y < grid_size.y(); y++) {
for (int z = 0; z < grid_size.z(); z++) {
Vector2i v_p(x, y);
if (grid_->GetInitCell(v_p)->m_eType == sep::EMPTY_CELL)
continue;
Cell* p_cell = grid_->GetInitCell(v_p)->m_pCell;
InitCellData* p_init_cell = grid_->GetInitCell(v_p);
const GasesConfigsMap& init_conds = p_init_cell->m_mInitConds;
for (auto val : init_conds) {
const int& gas_number = val.first;
if (gas_number >= Config::iGasesNumber)
continue;
const CellConfig& cond = val.second;
p_cell->setParameters(
cond.pressure,
cond.T,
area_step,
gas_number,
cond.locked_axes
);
p_cell->setBoundaryType(
cond.boundary_cond,
cond.boundary_T,
cond.boundary_stream,
cond.boundary_pressure,
gas_number
);
}
p_cell->Init(this);
}
}
}
}
// Find the minimum fitness value close to a discrete GA gene using
// inverse hessian minimization
double US_MPI_Analysis::minimize_dmga( DGene& dgene, double fitness )
{
DbgLv(1) << my_rank << "dg:IHM:minimize dgene comps" << dgene.components.size() << fitness;
int vsize = nfloatc;
US_Vector vv( vsize ); // Input values
US_Vector uu( vsize ); // Vector of derivatives
US_Vector zz( vsize ); // Vector of normalizing factors
// Create hessian as identity matrix
QVector< QVector< double > > hessian( vsize );
for ( int ii = 0; ii < vsize; ii++ )
{
hessian[ ii ] = QVector< double >( vsize, 0.0 );
hessian[ ii ][ ii ] = 1.0;
}
dgmarker.resize( vsize );
marker_from_dgene( dgmarker, dgene );
// Convert gene to array of normalized doubles and save normalizing factors
for ( int ii = 0; ii < vsize; ii++ )
{
double vval = dgmarker[ ii ];
double vpwr = (double)qFloor( log10( vval ) );
double vnorm = pow( 10.0, -vpwr );
vv.assign( ii, vval * vnorm );
zz.assign( ii, vnorm );
DbgLv(1) << my_rank << "dg:IHM: ii" << ii << "vval vnorm" << vval << vnorm
<< "vpwr" << vpwr << "vvi" << vv[ii];
}
lamm_gsm_df_dmga( vv, uu, zz ); // uu is vector of derivatives
static const double epsilon_f = 1.0e-7;
static const int max_iterations = 20;
int iteration = 0;
double epsilon = epsilon_f * fitness * 4.0;
bool neg_cnstr = ( vv[ 0 ] < 0.1 ); // Negative constraint?
while ( uu.L2norm() >= epsilon_f && iteration < max_iterations )
{
iteration++;
if ( fitness == 0.0 ) break;
US_Vector v_s1 = vv;
double g_s1 = fitness;
double s1 = 0.0;
double s2 = 0.5;
double s3 = 1.0;
DbgLv(1) << my_rank << "dg:IHM: iteration" << iteration << "fitness" << fitness;
// v_s2 = vv - uu * s2
US_Vector v_s2( vsize );
vector_scaled_sum( v_s2, uu, -s2, vv );
if ( neg_cnstr && v_s2[ 0 ] < 0.1 )
{
v_s2.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
double g_s2 = get_fitness_v_dmga( v_s2, zz );
DbgLv(1) << my_rank << "dg:IHM: g_s2" << g_s2 << "s2" << s2 << "epsilon" << epsilon;
// Cut down until we have a decrease
while ( s2 > epsilon && g_s2 > g_s1 )
{
s3 = s2;
s2 *= 0.5;
// v_s2 = vv - uu * s2
vector_scaled_sum( v_s2, uu, -s2, vv );
if ( neg_cnstr && v_s2[ 0 ] < 0.1 )
{
v_s2.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s2 = get_fitness_v_dmga( v_s2, zz );
}
DbgLv(1) << my_rank << "dg:IHM: g_s2" << g_s2;
// Test for initial decrease
if ( s2 <= epsilon || ( s3 - s2 ) < epsilon ) break;
US_Vector v_s3( vsize );
// v_s3 = vv - uu * s3
vector_scaled_sum( v_s3, uu, -s3, vv );
if ( neg_cnstr && v_s3[ 0 ] < 0.1 )
{
v_s3.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
double g_s3 = get_fitness_v_dmga( v_s3, zz );
int reps = 0;
static const int max_reps = 100;
while ( ( ( s2 - s1 ) > epsilon ) &&
( ( s3 - s2 ) > epsilon ) &&
( reps++ < max_reps ) )
{
double s1_s2 = 1.0 / ( s1 - s2 );
double s1_s3 = 1.0 / ( s1 - s3 );
double s2_s3 = 1.0 / ( s2 - s3 );
double s1_2 = sq( s1 );
double s2_2 = sq( s2 );
double s3_2 = sq( s3 );
double aa = ( ( g_s1 - g_s3 ) * s1_s3 -
( g_s2 - g_s3 ) * s2_s3
) * s1_s2;
double bb = ( g_s3 * ( s2_2 - s1_2 ) +
g_s2 * ( s1_2 - s3_2 ) +
g_s1 * ( s3_2 - s2_2 )
) *
s1_s2 * s1_s3 * s2_s3;
static const double max_a = 1.0e-25;
if ( qAbs( aa ) < max_a )
{
// Restore gene from array of normalized doubles
for ( int ii = 0; ii < vsize; ii++ )
{
dgmarker[ ii ] = vv[ ii ] / zz[ ii ];
}
dgene_from_marker( dgmarker, dgene );
return fitness;
}
double xx = -bb / ( 2.0 * aa );
double prev_g_s2 = g_s2;
if ( xx < s1 )
{
if ( xx < ( s1 + s1 - s2 ) ) // Keep it close
{
xx = s1 + s1 - s2; // xx <- s1 + ds
if ( xx < 0 ) xx = s1 / 2.0;
}
if ( xx < 0 ) // Wrong direction!
{
if ( s1 < 0 ) s1 = 0.0;
xx = 0;
}
// OK, take xx, s1, s2
v_s3 = v_s2;
g_s3 = g_s2; // 3 <- 2
s3 = s2;
v_s2 = v_s1;
g_s2 = g_s1;
s2 = s1; // 2 <- 1
s1 = xx; // 1 <- xx
// v_s1 = vv - uu * s1
vector_scaled_sum( v_s1, uu, -s1, vv );
if ( neg_cnstr && v_s1[ 0 ] < 0.1 )
{
v_s1.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s1 = get_fitness_v_dmga( v_s1, zz );
}
else if ( xx < s2 ) // Take s1, xx, s2
{
v_s3 = v_s2;
g_s3 = g_s2; // 3 <- 2
s3 = s2;
s2 = xx; // 2 <- xx
// v_s2 = vv - uu * s2
vector_scaled_sum( v_s2, uu, -s2, vv );
if ( neg_cnstr && v_s2[ 0 ] < 0.1 )
{
v_s2.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s2 = get_fitness_v_dmga( v_s2, zz );
}
else if ( xx < s3 ) // Take s2, xx, s3
{
v_s1 = v_s2;
g_s1 = g_s2;
s1 = s2; // 2 <- 1
s2 = xx; // 2 <- xx
// v_s2 = vv - uu * s2
vector_scaled_sum( v_s2, uu, -s2, vv );
if ( neg_cnstr && v_s2[ 0 ] < 0.1 )
{
v_s2.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s2 = get_fitness_v_dmga( v_s2, zz );
}
else // xx >= s3
{
if ( xx > ( s3 + s3 - s2 ) ) // if xx > s3 + ds/2
{
// v_s4 = vv - uu * xx
US_Vector v_s4( vsize );
vector_scaled_sum( v_s4, uu, -xx, vv );
if ( neg_cnstr && v_s4[ 0 ] < 0.1 )
{
v_s4.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
double g_s4 = get_fitness_v_dmga( v_s4, zz );
if ( g_s4 > g_s2 && g_s4 > g_s3 && g_s4 > g_s1 )
{
xx = s3 + s3 - s2; // xx = s3 + ds/2
}
}
// Take s2, s3, xx
v_s1 = v_s2;
g_s1 = g_s2; // 1 <- 2
s1 = s2;
v_s2 = v_s3;
g_s2 = g_s3;
s2 = s3; // 2 <- 3
s3 = xx; // 3 <- xx
// v_s3 = vv - uu * s3
vector_scaled_sum( v_s3, uu, -s3, vv );
if ( neg_cnstr && v_s3[ 0 ] < 0.1 )
{
v_s3.assign( 0, 0.1 + u_random( 100 ) * 0.001 );
}
g_s3 = get_fitness_v_dmga( v_s3, zz );
}
if ( qAbs( prev_g_s2 - g_s2 ) < epsilon ) break;
} // end of inner loop
US_Vector v_p( vsize );
if ( g_s2 < g_s3 && g_s2 < g_s1 )
{
v_p = v_s2;
fitness = g_s2;
}
else if ( g_s1 < g_s3 )
{
v_p = v_s1;
fitness = g_s1;
}
else
{
v_p = v_s3;
fitness = g_s3;
}
US_Vector v_g( vsize ); // Vector of derivatives
lamm_gsm_df_dmga( v_p, v_g, zz ); // New gradient in v_g (old in uu)
US_Vector v_dx( vsize );
// v_dx = v_p - vv
vector_scaled_sum( v_dx, vv, -1.0, v_p );
vv = v_p; // vv = v_p
// dgradient v_dg = v_g - uu
US_Vector v_dg( vsize );
vector_scaled_sum( v_dg, uu, -1.0, v_g );
US_Vector v_hdg( vsize );
// v_hdg = hessian * v_dg ( matrix * vector )
for ( int ii = 0; ii < vsize; ii++ )
{
double dotprod = 0.0;
for ( int jj = 0; jj < vsize; jj++ )
dotprod += ( hessian[ ii ][ jj ] * v_dg[ jj ] );
v_hdg.assign( ii, dotprod );
}
double fac = v_dg.dot( v_dx );
double fae = v_dg.dot( v_hdg );
double sumdg = v_dg.dot( v_dg );
double sumxi = v_dx.dot( v_dx );
if ( fac > sqrt( epsilon * sumdg * sumxi ) )
{
fac = 1.0 / fac;
double fad = 1.0 / fae;
for ( int ii = 0; ii < vsize; ii++ )
{
v_dg.assign( ii, fac * v_dx[ ii ] - fad * v_hdg[ ii ] );
}
for ( int ii = 0; ii < vsize; ii++ )
{
for ( int jj = ii; jj < vsize; jj++ )
{
hessian[ ii ][ jj ] +=
fac * v_dx [ ii ] * v_dx [ jj ] -
fad * v_hdg[ ii ] * v_hdg[ jj ] +
fae * v_dg [ ii ] * v_dg [ jj ];
// It's a symmetrical matrix
hessian[ jj ][ ii ] = hessian[ ii ][ jj ];
}
}
}
// uu = hessian * v_g ( matrix * vector )
for ( int ii = 0; ii < vsize; ii++ )
{
double dotprod = 0.0;
for ( int jj = 0; jj < vsize; jj++ )
dotprod += ( hessian[ ii ][ jj ] * v_g[ jj ] );
uu.assign( ii, dotprod );
}
} // end while ( uu.L2norm() > epsilon )
// Restore gene from array of normalized doubles
for ( int ii = 0; ii < vsize; ii++ )
{
dgmarker[ ii ] = vv[ ii ] / zz[ ii ];
DbgLv(1) << my_rank << "dg:IHM: ii" << ii << "vvi zzi dgmi"
<< vv[ii] << zz[ii] << dgmarker[ii];
}
dgene_from_marker( dgmarker, dgene );
DbgLv(1) << my_rank << "dg:IHM: FITNESS" << fitness;
return fitness;
}
Eigen::VectorXd controlEnv::mobile_manip_inverse_kinematics_siciliano(void){
// Siciliano; modelling, planning and control; pag 139
// --------------------------------------------------------------------------------
// std::cout << "AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA: " << n_iteration_ << std::endl;
// Position
VectorXd p_err(3);
for (unsigned int i=0; i<3; i++) p_err(i) = pose_error_.p()(i);
MatrixXd Kp(3,3);
Kp = MatrixXd::Zero(3,3);
Kp(0,0) = 1.0;
Kp(1,1) = 1.0;
Kp(2,2) = 1.0;
VectorXd vd(3);
desired_pose_velocity_ = compute_pose_velocity(desired_pose_, past_desired_pose_, time_increment_);
vd = desired_pose_velocity_.p();
VectorXd v_p(3);
v_p = vd + Kp * p_err;
// Orientation
Quaternion<double> rot_current, rot_desired;
rot_current = mobile_manip_.tcp_pose().o();
rot_desired = desired_pose_.o();
MatrixXd L(3,3);
L = Lmat(rot_current, rot_desired);
MatrixXd Sne(3,3), Sse(3,3), Sae(3,3);
Sne = cross_product_matrix_S(rot_current.toRotationMatrix().col(0));
Sse = cross_product_matrix_S(rot_current.toRotationMatrix().col(1));
Sae = cross_product_matrix_S(rot_current.toRotationMatrix().col(2));
VectorXd nd(3), sd(3), ad(3);
nd = rot_desired.toRotationMatrix().col(0);
sd = rot_desired.toRotationMatrix().col(1);
ad = rot_desired.toRotationMatrix().col(2);
VectorXd o_err(3);
o_err = 0.5 * ( Sne*nd + Sse*sd + Sae*ad );
MatrixXd Ko(3,3);
Ko = MatrixXd::Zero(3,3);
Ko(0,0) = 1.0;
Ko(1,1) = 1.0;
Ko(2,2) = 1.0;
VectorXd wd(3);
wd = desired_pose_velocity_.o_angaxis_r3();
VectorXd v_o(3);
v_o = L.inverse() * (L.transpose() * wd + Ko * o_err );
// Put all together
VectorXd v(6);
for (unsigned int i=0; i<3; i++){
v(i) = v_p(i);
v(i+3) = v_o(i);
}
return pinv(jacobian(mobile_manip_), 0.001)*v;
}
