int cConceptualNN::evaluateSOM()
{
pnnLayer tempLayer;
pnnStruct tempNN;
pnnStruct tempPrevNN;
pnnWeight tempWgt;
double sum;
double firstInput;
//cuidado con este for... revisar
// printf("peso1 %f", layerList->nn->wgth->wgth);
// getchar();
tempLayer = layerList; //inicialización
// printf("peso1 %f", tempLayer->nn->wgth->wgth);
// getchar();
for( tempNN = tempLayer->nn; //inicialización
tempNN != NULL; //mientras la capa tenga neuronas
tempNN = tempNN->nn ) //avanza una neurona
{
sum = 0.0;
if( tempLayer->idLayer == 0 )
{
//poner procedimiento caso primera capa neurona
//this->loadImg2NN(i); //se carga el input en la primera capa de neuronas
firstInput = (tempNN->input)*(tempNN->wgth->wgth);
tempNN->output = sigmoide(firstInput); //se calcula el output en base al input
}
else
{
for( tempPrevNN = tempLayer->prevLayer->nn, //tempPrevNN = la primera neurona de la capa previa (l-1)
tempWgt = tempNN->wgth; // se inicializa además el puntero al peso de la neurona
//correspondiente a la capa (l)
tempPrevNN != NULL; //mientras haya neuronas en la capa previa
tempWgt = tempWgt->nextwgth, //se avanza un peso
tempPrevNN = tempPrevNN->nn ) //y se avanza una neurona
{
sum += (tempWgt->wgth)*(tempPrevNN->output);
}
tempNN->input = sum;
tempNN->output = sigmoide(tempNN->input); //se calcula el output en base al input
//poner procedimiento resto de las neuronas
//falta poner el resultado que entrega la capa de neuronas Nº 7
if( tempLayer->idLayer == 6 ) printf("%f ", tempNN->output);
}
}
return 1;
}
fmat predict(const fmat& X_test, const fmat& Theta) {
fmat resultado = sigmoide(X_test * Theta);
return resultado;
}
fmat gdStep(const fmat& Theta, const fmat& X, const fmat& Y, double alpha, double lambda) {
fmat gradient = (alpha / X.n_rows) * X.t() * (sigmoide(X * Theta) - Y);
fmat reg = (lambda / X.n_rows) * Theta;
reg.row(0) = zeros<frowvec>(Y.n_cols);
return Theta - gradient - reg;
}
void SpecificWorker::compute()
{
RoboCompDifferentialRobot::TBaseState bState;
differentialrobot_proxy->getBaseState ( bState );
RoboCompLaser::TLaserData datosLaser;
datosLaser= laser_proxy->getLaserData();//Obtenemos datos del Laser
inner->updateTransformValues ( "robot",bState.x,0,bState.z,0,bState.alpha,0 );
// datosLaser= laser_proxy->getLaserData();//Obtenemos datos del Laser
std::sort ( datosLaser.begin() +20, datosLaser.end()-20,[] ( auto a, auto b )
{
return a.dist< b.dist;
} ); //Ordenamos las distancias del frente
tR = inner->transform ( "robot" ,QVec::vec3 ( parxz.first, 0, parxz.second ),"world" );
d = tR.norm2(); // distancia del robot al punto marcado
float vRot = atan2 ( tR.x(),tR.z() ); //devuelve radianes del angulo q forma donde apunta el robot con el punto destino.
//Refresca el estado
if ( target.isEmpty() == false && target.haCambiado())
{
//Calcular punto inicial,punto final,y trayectoria entre ellos
// parIni = //Falta el punto inicial
parxz = target.get();//Punto final
tR = inner->transform ( "robot" ,QVec::vec3 ( parxz.first, 0, parxz.second ),"world" );
d = tR.norm2(); // distancia del robot al punto marcado
estado= Estado::AVANZANDO;
target.setCambiado(false);
}
switch ( estado )
{
case Estado::PARADO:
qDebug() << "PARADO";
if ( target.isEmpty() == false)
{
//Calcular punto inicial,punto final,y trayectoria entre ellos
// parIni = //Falta el punto inicial
parxz = target.get();//Punto final
tR = inner->transform ( "robot" ,QVec::vec3 ( parxz.first, 0, parxz.second ),"world" );
d = tR.norm2(); // distancia del robot al punto marcado
estado= Estado::AVANZANDO;
target.setCambiado(false);
}
break;
case Estado::AVANZANDO:
qDebug() << "AVANZANDO";
if ( datosLaser[20].dist < UMBRAL ) //Si hay obstaculo
{
//Pasa a estado de Giro
estado = Estado::GIRANDO;
break ;
}
if ( d > 50 )
{
//Si no ha llegado
float velAvance = d;// version antigua
if ( vRot > MAX_ROT )
{
vRot = MAX_ROT;
}
velAvance=MAX_ADV*sigmoide ( d ) *gauss ( vRot,0.3,0.5 );
differentialrobot_proxy->setSpeedBase ( velAvance,vRot );
}
else
{
//Si ha llegado al sitio
estado = Estado::LLEGADO;
}
break;
case Estado::GIRANDO:
qDebug() << "GIRANDO";
if ( datosLaser[20].dist>UMBRAL )
{
estado=Estado::BORDEANDO;
}
else
{
//float d= rand()%1000000+100000;
differentialrobot_proxy->setSpeedBase ( 0,0.75 );
//usleep(d);
}
break;
case Estado::BORDEANDO:
qDebug() << "BORDEANDO";
if ( datosLaser[20].dist>UMBRAL && ( vRot<ANGULO_VISION && vRot > -(ANGULO_VISION) )) //Que no haya obstaculos en el frente y vaya hacia el objetivo
{
estado=Estado::AVANZANDO;
}
if ( datosLaser[20].dist<UMBRAL ) //Si hay dos obstaculos en L, debe volver a girar para no tener obstaculo en frente
{
estado=Estado::GIRANDO;
}
if ( d < 280 )
{
estado=Estado::LLEGADO;
}
//Condicion 2 para llegar al objetivo
//if (
//Ordenar los 20 primeros
std::sort ( datosLaser.end()-19, datosLaser.end()-10,[] ( auto a, auto b )
{
return a.dist< b.dist;
} ); //Valores de la izda
//Evaluamos distancia izda
//qDebug() << "Distancia:" <<datosLaser[81].dist;
if ( datosLaser[81].dist < 400 ) //Que la distancia izda sea esa
{
//Avanzar
differentialrobot_proxy->setSpeedBase ( 100,0 );
}
else
{
differentialrobot_proxy->setSpeedBase ( 0,-0.75 );
}
break;
case Estado::LLEGADO:
qDebug() << "LLEGADO";
differentialrobot_proxy->setSpeedBase ( 0,0 );
target.setEmpty();
estado=Estado::PARADO;
break;
}
}
