//*************************************************************************** // // file: detour.C // // // TITLE: Learning to Detour. // GOAL: Schema Construction and Dynamics based on SBL. // // REFERENCES: // ------------ // Refer to Corbacho and Arbib (1995) for details on detour itself. // Refer to Corbacho and Arbib (1997) for further details on Learning. // // ARCHITECTURE: // ------------ // Perceptual Schemas as functional units. // Fields corresponding to schema instances; // Integration of fields; WTA; Sensorimotor Xform; // Motor schemas as functional units. // // // SCHEMA DYNAMICS: // -------- // Relaxation Labeling (Hummel and Zucker, 1983). // Control of schemas based on distributed C&C dynamics. // Inclusion of NeuroSchemas. // // // SIMPLIFICATIONS: // --------------- // Perceptual Schemas (not Neural Implementation). // Motor Schemas as functional units. // // // SCHEMA CONSTRUCTION: // -------------------- // Autonomous Schema Construction in Run time. // Coherence and Performance Maximization. // // // AUTHOR: Fernando Jose Corbacho // DATE (last update): July 3rd, 98 (prev): July 2nd, 98. // // // *************************************************************************** #include "nsl_include.h" #include "detour.h" int obj_type = 0; // 0 - prey only, 1 - predator void gauss1D(nsl_vector& kernel,nsl_data& sigma) { int xn = kernel.get_imax(); float sig = sigma.get_data(); float k = xn/2; const float PI = 3.14159; float f= sig*sqrt(2.0*PI); for (int i=-k; i <= k - (1 - xn%2); i++) kernel.elem(i+k) = exp(-(i*i)/(2*sig*sig))/f; } void gauss2D(nsl_matrix& kernel,nsl_data& sigma) { int xn = kernel.get_imax(); int yn = kernel.get_jmax(); float sig = sigma.get_data(); int k1 = xn/2; int k2 = yn/2; const float PI = 3.14159; float f= sig*sqrt(2.0*PI); for (int i=-k1; i<=k1 - (1 - xn%2); i++) for (int j=-k2; j<=k2 - (1 - yn%2); j++) kernel.elem(i+k1,j+k2) = exp(-(i*i+j*j)/(2*sig*sig))/f; } nsl_vector maxim(nsl_vector& u) // returns the vector normalized (substraction) by its maximum. // only the maximum is above threshold (by 0.01). { int imax = u.get_imax(); nsl_vector c(imax); float max = 0; // Maximum value int max_i=0; // Coordinates of max value max = NSLmax(u); if (max == 0) for (int i=0; i < imax; i++) c.elem(i)= - 0.01; else for (int i=0; i < imax; i++) c.elem(i)= u.elem(i) - max + 0.01; c = NSLstep(c); return c; } nsl_matrix filter(nsl_matrix& view, int f) // filters out all the elements in the matrix that do not have a // value of "f". Simplified feature detector where "f" is the feature. { int i,j; int imax = view.get_imax(); nsl_matrix c(imax, imax); for (i=0; i worm_speed) xw = xw - worm_speed; else localfg = 4; } if (localfg == 4) { if (yw > worm_speed) yw = yw - worm_speed; else localfg = 1; } // worm_speed = worm_speed * -1; // Oscillatory behavior } //CambiosPredador 3 Predator::Predator(char* str, NslModule* parent) : NslModule(str,parent), predator_speed("predator_speed", this), boundary("boundary",this), xp("xp", this), yp("yp", this), zp("zp", this) { } void Predator::simRun() { if (xp < boundary) xp = xp - predator_speed; // worm_speed = worm_speed * -1; // Oscillatory behavior } //FinCambiosPredador 3 VisualField::VisualField(char* str, NslModule* parent,int wx,int wz,int recsize) : NslModule(str,parent), visual_field("visual_field", this, recsize, recsize), worldXZ("worldXZ",this, wx,wz), zf("zf", this), xf("xf", this) { } void VisualField::simRun() { int recsize = visual_field.get_imax(); int isize = worldXZ.get_imax(); int jsize = worldXZ.get_jmax(); int i2size = isize/2; int j2size = jsize/2; visual_field = 0; visual_field = worldXZ.get_sector(isize-recsize,isize-1,j2size-recsize/2,j2size+recsize/2-1); } DepthField::DepthField(char* str, NslModule* parent) : NslModule(str,parent), yf("yf",this), xf("xf",this), yw("yw",this), xw("xw",this), yb("yb",this), xp("xp",this), yp("yp",this), xb_init("xb_init",this), xb_end("xb_end",this), safe_distance("safe_distance",this), depthY("depthY",this), depthX("depthX",this), ps("ps",this), th("th",this) { } void DepthField::initSys() { ps = 0; } void DepthField::simRun() { /* if (yf.elem() > yb.elem()) //si la rana pasó la barrera en y depthY = yf - yb; else depthY = 0; if (xf.elem() > xb_end.elem()) // si la rana está más a la der que la barrera depthX = xf - xb_end; else if (xf.elem() < xb_init.elem()) depthX = xb_init - xf; else depthX = 0; // this is not used if (depthY.elem() > safe_distance.elem() || depthX.elem() > safe_distance.elem()) // safe_distance.elem() ps= 2; // Go up fast else */ ps = 0.0; triggerSchema(); } void DepthField::triggerSchema() { if (ps.elem() > th.elem()) { printf("Depth \t"); } } Tactile::Tactile(char* str, NslModule* parent) : NslModule(str,parent), safe_distance("safe_distance",this), depthY("depthY",this), depthX("depthX",this), ps("ps",this), th("th",this) { } void Tactile::initSys() { ps = 0; } void Tactile::simRun() { if (depthY.elem() > 0 && depthY.elem() <= safe_distance.elem() && depthX.elem() <= safe_distance.elem()) ps = 2; // Go up fast else ps = 0.0; // Go down fast: -2.0 updateLabel(); triggerSchema(); } void Tactile::updateLabel() { // ps = ps + qs; ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void Tactile::triggerSchema() { if (ps.elem() > th.elem()) { printf("Tactile \t"); NSLoutput("Tactile \t"); } } FeedMotivation::FeedMotivation(char* str, NslModule* parent) : NslModule(str,parent), date("date", this), dayhour("dayhour",this), hunger("hunger",this), ps("ps",this), month("month",this), day("day",this), hour("hour",this), minute("minute",this) { } void FeedMotivation::initSys() { } void FeedMotivation::simRun() { ps=0; month = date / 100; day = date - (date / 100)*100; //date%100 hour = dayhour/100; minute = dayhour - (dayhour / 100)*100 ; //dayhour%100; time (&timeini); updateLabel(); triggerSchema(); } void FeedMotivation::updateLabel() { time_t timefin; time (&timefin); if (difftime(timefin,timeini)>60) //ha transcurrido un minuto { minute = minute+1; timeini=timefin; } if (minute>=60) { minute=0; hour=hour+1; } if (hour >= 24) { hour=0; day=day+1; } if (day>=30) { day=1; month=month+1; } if (month>=12) month=1; } void FeedMotivation::triggerSchema() { ps = 0; } PreyRec::PreyRec(char* str, NslModule* parent, int recsize,int tx,int ty) : NslModule(str,parent), visual_field("visual_field", this, recsize, recsize), t5_2f("t5_2f", this,tx,ty), // Output Data Port. t5_2("t5_2", this,tx,ty), ps("ps",this), ps_th3("ps_th3",this), rs_th3("rs_th3",this), psm("psm",this), rsm("rsm",this), th("th",this) { } void PreyRec::initSys() { ps = 0; // psm = 0.1; rsm = 0.44; //rsm=0.4/psm rs_th3 = 0.66; } void PreyRec::simRun() { t5_2 = filter(visual_field,2); // INSTANTIATED Schema. if (t5_2.sum() >= 1) // orig >1 //ps=0.9; // Confidence ps=0.5; // Confidence else ps=0; updateLabel(); triggerSchema(); } void PreyRec::updateLabel() { // ps = ps + qs; ps = ps - ps_th3*rs_th3 + psm*rsm ; if (ps<0) ps=0; ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void PreyRec::triggerSchema() { if (ps.elem() > th.elem()) { printf("Prey Recog \t"); // ASSERTED Schema. t5_2f = NSLramp(t5_2); // Output to other Schemas } else //t5_2f = 0; t5_2f = NSLramp(t5_2); // Output to other Schemas } //CambiosPredador PredatorRec::PredatorRec(char* str, NslModule* parent, int recsize,int tx,int ty) : NslModule(str,parent), visual_field("visual_field", this, recsize, recsize), th3f("th3f", this,tx,ty), // Output Data Port. th3("th3", this,tx,ty), ps("ps",this), th("th",this) { } void PredatorRec::initSys() { ps = 0; } void PredatorRec::simRun() { th3 = filter(visual_field,3); // INSTANTIATED Schema. if (th3.sum() >= 1) // orig >1 ps=0.9; // Confidence else ps=0; updateLabel(); triggerSchema(); } void PredatorRec::updateLabel() { // ps = ps + qs; ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void PredatorRec::triggerSchema() { if (ps.elem() > th.elem()) { printf("Predator Recog \t"); // ASSERTED Schema. th3f = NSLramp(th3); // Output to other Schemas obj_type = 1; } else { th3f = 0; obj_type = 0; } } //FinCambiosPredador SorRec::SorRec(char* str, NslModule* parent, int recsize,int tx,int ty) : NslModule(str,parent), visual_field("visual_field", this, recsize, recsize), th10f("th10f", this,tx,ty), th10("th10", this,tx,ty), depthY("depthY", this), depthX("depthXY", this), edge("edge",this), // the edge of barrier ps("ps",this), th("th",this) { } void SorRec::initSys() { ps = 0; } void SorRec::simRun() { th10 = filter(visual_field, 1); // view far from barrier. if (th10.sum() >= 1) // orig >1 ps=0.9; // Confidence else ps=0; updateLabel(); triggerSchema(); } void SorRec::updateLabel() { // ps = ps + qs; ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void SorRec::triggerSchema() { if (ps.elem() > th.elem()) { printf("SO Recog \t"); th10f = NSLramp(th10); // Output to other Schemas } else th10f = 0; } PreyApproach::PreyApproach(char* str, NslModule* parent,int tx,int ty) : NslModule(str,parent), t5_2f("t5_2f", this,tx,ty), // Output Data Port. prey_hor_f("prey_hor_f", this, tx), // OUTPUT Data Port. prey_hor("prey_hor", this, tx), // Parcellation (1D). prey_hor_default("prey_hor_default", this, tx), prey_f("prey_f", this, tx, ty), // PREY ATRACTANT FIELD (2D). prey_norm("prey_norm", this), // Normalization t5_2_rf("t5_2_rf", this, 80, 80), // KERNEL for Prey Attractant Field. t5_2_erf("t5_2_erf", this, 80, 80), // t5_2_irf("t5_2_irf", this, 40, 40), t5_2_erf_sig("t5_2_erf_sig", this), t5_2_erf_wgt("t5_2_erf_wgt", this), psfo("psfo",this), rsfo("rsfo",this), ps("ps",this), th("th",this) { } void PreyApproach::initSys() { rsfo = 1.0; // Prey & Prey Approach. ps = 0; // t5_2_erf = t5_2_erf_wgt * gauss2D(t5_2_erf_sig, t5_2_erf_dia, t5_2_erf_dia); // t5_2_rf = t5_2_erf; gauss2D(t5_2_erf,t5_2_erf_sig); t5_2_rf = t5_2_erf_wgt * t5_2_erf; } void PreyApproach::simRun() { prey_f = t5_2_rf * t5_2f; //** 2D: newconv(t5_2_rf, T5_2); prey_hor = prey_f.reduce_row(); // Parcellation: horizontal comp. updateLabel(); triggerSchema(); } void PreyApproach::updateLabel() { ps = rsfo*psfo; // ps + ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void PreyApproach::triggerSchema() { /* if (ps.elem() > th.elem()) { printf("Prey Appro \t"); float mx = NSLmax(prey_hor); if (mx != 0.0) { prey_hor_f = prey_hor/mx; // Normalize. // ASSERTED: Affect other schemas. // prey_hor_f = prey_hor_f * psfo; } } else // prey_hor_f = 0; { prey_hor_f = prey_hor_f * psfo; ps = 0; //SIG31052001 } */ float mx = NSLmax(prey_hor); if (mx != 0.0) { prey_hor_f = prey_hor/mx; // Normalize. // ASSERTED: Affect other schemas. } if (ps.elem() > th.elem()) { } else { prey_hor_f = prey_hor_f * psfo; ps = 0; //SIG31052001 } } //CambiosPredador PredatorAvoid::PredatorAvoid(char* str, NslModule* parent,int tx,int ty) : NslModule(str,parent), th3f("th3f", this,tx,ty), // Output Data Port. predator_hor_f("predator_hor_f", this, tx), // OUTPUT Data Port. predator_hor("predator_hor", this, tx), // Parcellation (1D). predator_hor_default("predator_hor_default", this, tx), predator_f("predator_f", this, tx, ty), // PREDATOR RETRACTANT FIELD (2D). predator_norm("predator_norm", this), // Normalization th3_rf("th3_rf", this, 80, 80), // KERNEL for Predator Attractant Field. th3_erf("th3_erf", this, 80, 80), // t5_2_irf("t5_2_irf", this, 40, 40), th3_erf_sig("th3_erf_sig", this), th3_erf_wgt("th3_erf_wgt", this), //CambiosPred ps_th3("ps_th3",this), rs_th3("rs_th3",this), //FinCambiosPred ps("ps",this), th("th",this) { } void PredatorAvoid::initSys() { rs_th3 = 1.0; // Predator & Predator Avoid. ps = 0; // t5_2_erf = t5_2_erf_wgt * gauss2D(t5_2_erf_sig, t5_2_erf_dia, t5_2_erf_dia); // t5_2_rf = t5_2_erf; gauss2D(th3_erf,th3_erf_sig); th3_rf = th3_erf_wgt * th3_erf; } void PredatorAvoid::simRun() { predator_f = th3_rf * th3f; //** 2D: newconv(t5_2_rf, T5_2); predator_hor = predator_f.reduce_row(); // Parcellation: horizontal comp. updateLabel(); triggerSchema(); } void PredatorAvoid::updateLabel() { ps = rs_th3*ps_th3; // ps + ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void PredatorAvoid::triggerSchema() { if (ps.elem() > th.elem()) { printf("Predator Appro \t"); float mx = NSLmax(predator_hor); if (mx != 0.0) { predator_hor_f = predator_hor/mx; // Normalize. // ASSERTED: Affect other schemas. // predator_hor_f = predator_hor_f * 1.5; //Aumenta la respuesta } } else predator_hor_f = 0; } //FinCambios SorAvoid::SorAvoid(char* str, NslModule* parent,int tectsize,int tx,int ty,int rrf) : NslModule(str,parent), th10f("th10f", this,tx,ty), th10_hor("th10_hor", this, tx), // Parcellation // KERNEL for SO Avoid Field // tm_erf("tm_erf", this, rrf*2), tm_irf("tm_irf", this, rrf*2), tm_rf("tm_rf", this, rrf*2), // tm_erf_sig("tm_erf_sig", this), tm_irf_sig("tm_irf_sig", this), // tm_erf_wgt ("tm_erf_wgt", this), tm_irf_wgt("tm_irf_wgt", this), // tm_rf_modul("tm_rf_modul", this, rrf*2), // modulated kernel. gaps("gaps", this, tectsize), // openings within obstacles. // gaps_norm("gaps_norm", this), // Normalization gapsf("gapsf", this, tectsize), psso("psso",this), rsso("rsso",this), ps("ps",this), th("th",this) { } void SorAvoid::initSys() { rsso = 1.0; // Obstacle & Obstacle Avoid ps = 0; // OBSTACLE AVOIDANCE KERNEL // tm_irf = tm_irf_wgt * gauss1D(tm_irf_sig, tm_irf_dia.elem()); // tm_rf = - tm_irf; gauss1D(tm_irf,tm_irf_sig); tm_rf = - tm_irf_wgt * tm_irf; // tm_rf_modul = tm_rf; } void SorAvoid::simRun() { th10_hor = th10f.reduce_row(); // Parcellation (from 2D to 1D). gaps = tm_rf * th10_hor; // Convolve with kernel. updateLabel(); triggerSchema(); } void SorAvoid::updateLabel() { ps = rsso*psso; // ps + ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void SorAvoid::triggerSchema() { if (ps.elem() > th.elem()) { printf("SOR avoid \t"); float mx = NSLmax(gaps*-1); if (mx != 0.0) gapsf = gaps/mx; // ASSERTED: Affect other schema } else gapsf = 0; } BumpAvoid::BumpAvoid(char* str, NslModule* parent,int tectsize) : NslModule(str,parent), baf("baf", this, tectsize), gaps("gaps", this, tectsize), gapsf("gapsf", this, tectsize), field_a("field_a", this), field_center("field_center", this), // field_side("field_side", this, tectsize), // last_orientation("last_orientation", this,tectsize), tune_tm("tune_tm", this), // TUNE GAPS field (avoid bumping). tune_tm_base("tune_tm_base", this), tune_tm_layer("tune_tm_layer", this, tectsize), safe_distance("safe_distance",this), depthY("depthY",this), depthX("depthX",this), ps("ps",this), psta("psta",this), rsta("rsta",this), th("th",this) { } void BumpAvoid::initSys() { rsta = 1.0; // Tactile & Bumping Avoid ps = 0; } void BumpAvoid::simRun() { // both should be larger than zero for no bump //Comentario después de predador /* if (depthY.elem() <= safe_distance.elem() && depthX.elem() <= safe_distance.elem()) //Bumping. ps = 0; else ps = -1.0; // Go down fast: -2.0 */ tuningField(); // TUNING the KERNEL updateLabel(); triggerSchema(); } void BumpAvoid::updateLabel() { ps = ps + rsta*psta; ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void BumpAvoid::tuningField() { if (tune_tm.elem() < 1.5) // saturate tune_tm. tune_tm = tune_tm + tune_tm_base; tune_tm_layer = tune_tm; tune_tm_layer = tune_tm_layer ^ NSLstep(-gaps); // Modulate only already active neurons. gaps = gaps - tune_tm_layer; // GAPS fiels gets tuned. gapsf = gaps; } void BumpAvoid::triggerSchema() { if (ps.elem() > th.elem()) { printf("Bump Avoid \t"); NSLoutput("Bump Avoid \t"); // every time it bumps it increases tune_tm until it reaches a saturation point. field_center = field_center +.3; // Tunes the B_A field by increasing eccentricity. // Generate Bumping Avoidance FIELD. baf.elem(floor(field_center.elem())) = field_a.elem(); } else baf = 0; } MHM::MHM(char* str, NslModule* parent, int tectsize) : NslModule(str,parent), baf("baf", this, tectsize), gapsf("gapsf", this, tectsize), prey_hor_f("prey_hor_f", this, tectsize), ip("ip", this, tectsize), // PERCEPTUAL INTEGRATION. // aip("aip", this), mhm("mhm", this, tectsize), in("in", this, tectsize), mhm_hat("mhm_hat", this, tectsize), // Predictive MHM. new_relation("new_relation", this), // temporary: new_relation learning("learning", this), // temporary: new_relation field_a("field_a", this), field_center("field_center", this), d_mhm("d_mhm", this), // Coherence over MHM. d_norm("d_norm", this), ps("ps",this), pspa("pspa",this), rspa("rspa",this), psoa("psoa",this), //CambiosPred predator_hor_f("predator_hor_f", this, tectsize), psps("psps",this), rsps("rsps",this), //FinCambiosPred rsoa("rsoa",this), psm("psm",this), rsm("rsm",this) { } void MHM::initSys() { rspa = 1.0; rsoa = 1.0; //CambiosPred rsps = 1.0; //FinCambiosPred rsm = 0.5; // psm = 0; ps = 0; if (learning.get_data() == 1) new_relation = 1; // start learning immediately } void MHM::simRun() { // Dynamics of Motor Heading Map (MHM): integrating several fields. // New fields can also be added while learning. if (new_relation.elem() == 1) { baf.elem(floor(field_center.elem())) = field_a.elem(); in = baf; // New Field "inserted" } else in = 0; // reset input (cf. antidromic) mhm_hat = mhm; // Predictive MHM. //CambiosPred //ip = gapsf + prey_hor_f + baf; // Fields over MHM. ip = gapsf + prey_hor_f + predator_hor_f; //+ baf; // (AW0501) // Fields over MHM. //FinCambiosPred //mhm = ip + in; mhm = ip; coherence(); updateLabel(); } void MHM::updateLabel() { //CambiosPredator //ps = ps + rspa*pspa + rsoa*psoa; //ps = ps + rspa*pspa + rsoa*psoa + rsps*psps; ps = ps + rspa*pspa + rsps*psps + rsm*psm; //Prey-Predator //FinCambiosPredator ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void MHM::coherence() { // Learning Dynamics: it detects an incoherence and hence a trigger // for a new schema. d_mhm = 0.03 * dist(mhm, mhm_hat); // d_mhm.nslPrint(); if (new_relation.elem() == 0 && d_mhm.elem() > d_norm.elem()) // Incoherence. new_relation.elem() = 1; // AW2000 if (new_relation.elem() == 1 && d_mhm.elem() < d_norm.elem()) // Reset. new_relation.elem() = -1; // new_relation.nslPrint(); } WTA::WTA(char* str, NslModule* parent, int tectsize) : NslModule(str,parent), mhm("mhm", this, tectsize), wta_mhm("wta_mhm", this,tectsize) { } void WTA::simRun() { wta_mhm = maxim(mhm); } Xform::Xform(char* str, NslModule* parent, int tectsize) : NslModule(str,parent), wta_mhm("wta_mhm", this, tectsize), // Predictive MHM. angle_position("angle_position", this, tectsize), gradient("gradient", this, tectsize), // Gradient of weights. angle("angle", this), // Heading (target) angle. ps("ps",this), pspa("pspa",this), //CambiosPredator psps("psps",this), rsps("rsps",this), //FinCambios rspa("rspa",this), psoa("psoa",this), rsoa("rsoa",this), psm("psm",this), rsm("rsm",this) { } void Xform::initSys() { rspa = 1.0; rsoa = 1.0; rsm = 0.5; // psm = 0; //CambiosPred rsps = 1.0; //FinCambios ps = 0; } void Xform::simRun() { // Sensory motor Transformation from retinotopic coordinates to population coding. int i; // angle_position = gradient ^ wta_mhm; wta_mhm.max(i); // angle = int (floor(angle_position.sum())); //eccentricity angle = 0; // position displacement in x if (i != 0) angle = i - wta_mhm.get_imax()/2; angle.nslPrint(); updateLabel(); } void Xform::updateLabel() { //CambiosPred //ps = ps + rspa*pspa + rsoa*psoa; //ps = ps + rspa*pspa + rsoa*psoa +rsps*psps; ps = ps + rspa*pspa + rsps*psps + rsm*psm; //Prey-Pred //FinCambiosPred ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } Forward::Forward(char* str, NslModule* parent) : NslModule(str,parent), depthY("depthY", this), // Heading (target) angle. step("step", this), out("out", this), ps("ps",this), psfo("psfo",this), rsfo("rsfo",this), //CambiosPred ps_th3("ps_th3",this), rs_th3("rs_th3",this), //FinCambios psta("psta",this), rsta("rsta",this), psdp("psdp",this), rsdp("rsdp",this), psmhm("psmhm",this), rsmhm("rsmhm",this), psbk("psbk",this), rsbk("rsbk",this), th("th",this), psm("psm",this), rsm("rsm",this) { } void Forward::initSys() { //CambiosPred rsfo = 0.4; rs_th3 = 0.6; //FinCambios rsmhm = 0.4; rsm = 0.5; // psm = 0; rsta = -1.0; rsdp = 1.0; rsbk = -1.0; ps = 0; } void Forward::simRun() { updateLabel(); triggerSchema(); } void Forward::updateLabel() { //CambiosPred //ps = ps + rsfo*psfo + rsta*psta + rsdp*psdp + rsmhm*psmhm + rsbk*psbk; //anterior //ps = ps + rsfo*psfo + rsta*psta + rsdp*psdp + rsmhm*psmhm + rsbk*psbk + rs_th3*ps_th3; ps = ps + rsfo*psfo + rsmhm*psmhm + rs_th3*ps_th3 + rsm*psm; //prey-pred NSLoutput("psfo:",psfo.elem()); NSLoutput("ps_th3:",ps_th3.elem()); NSLoutput("psmhm:", psmhm.elem()); NSLoutput("ps:", ps.elem()); //FinCambiosPred ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void Forward::triggerSchema() { out = 0; // out = step; //SIG31052001 if (ps.elem() > th.elem()) { ps = 0; //ps = -1.0; // Reset out = step; NSLoutput("forward: "); out.nslPrint(); } else ps = 0; } Sidestep::Sidestep(char* str, NslModule* parent) : NslModule(str,parent), angle("angle", this), // Heading (target) angle. out("out", this), ps("ps",this), psso("psso",this), rsso("rsso",this), psta("psta",this), rsta("rsta",this), psba("psba",this), rsba("rsba",this), psmhm("psmhm",this), rsmhm("rsmhm",this), psx("psx",this), rsx("rsx",this), th("th",this) { } void Sidestep::initSys() { rsso = 0.5; rsta = 1.0; // rsdp = -1.0; rsba = 0.5; rsmhm = 0.5; rsx = 0.5; ps = 0; } void Sidestep::simRun() { updateLabel(); triggerSchema(); } void Sidestep::updateLabel() { //ps = ps + rsso*psso + rsta*psta + rsba*psba + rsmhm*psmhm + rsx*psx; ps = ps + rsmhm*psmhm + rsx*psx; //only Prey-Pred ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void Sidestep::triggerSchema() { out = 0; if (ps.elem() > th.elem()) { ps = 0; // -1.0; // Reset out = angle; NSLoutput("sidestep: "); out.nslPrint(); } else ps=0; } Backward::Backward(char* str, NslModule* parent) : NslModule(str,parent), step("step", this), out("out", this), ps("ps",this), psta("psta",this), rsta("rsta",this), psba("psba",this), rsba("rsba",this), psfw("psfw",this), rsfw("rsfw",this), th("th",this) { } void Backward::initSys() { rsta = 1.0; rsba = 1.0; rsfw = -1.0; ps = 0; } void Backward::simRun() { updateLabel(); triggerSchema(); } void Backward::updateLabel() { ps = ps + rsta*psta + rsba*psba + rsfw*psfw; ps = NSLsigma(ps,(float)-1.0,(float)1.0,(float)-1.0,(float)1.0); } void Backward::triggerSchema() { out = 0; /* if (ps.elem() > th.elem()) { ps = -1.0; // Reset out = step; NSLoutput("backward: "); out.nslPrint(); } */} // Frog Frog::Frog(char* str, NslModule* parent,int wx,int wz,int recsize,int tectsize, int tx,int ty,int rrf) : NslModule(str,parent), visual("visual",this,wx,wz,recsize), depth("depth",this), tactile("tactile",this), feedMotivation("feedMotivation",this), preyRec("preyRec",this,recsize,tx,ty), sorRec("sorRec",this,recsize,tx,ty), predatorRec("predatorRec",this,recsize,tx,ty), //CambiosPred preyApproach("preyApproach",this,tx,ty), sorAvoid("sorAvoid",this,tectsize,tx,ty,rrf), predatorAvoid("predatorAvoid",this,tx,ty), //CambiosPred bumpAvoid("bumpAvoid",this,tectsize), mhm("mhm",this,tectsize), wta("wta",this,tectsize), xform("xform",this,tectsize), forward("forward",this), sidestep("sidestep",this), backward("backward",this), worldXZ("worldXZ",this, wx,wz), xf("xf", this), yf("yf", this), zf("zf", this), xw("xw", this), yw("yw", this), // zf("zf", this), //CambiosPred xp("xp", this), yp("yp", this), // zf("zf", this), //FinCambiosPred xb_end("xb_end", this), yb("yb", this), zb("zb", this), xb_init("xb_init", this), forward_step("forward_step", this), backward_step("backward_step", this), wangle("wangle", this), angle("angle", this), date ("date", this), dayhour ("dayhour", this), hunger("hunger",this) { makeConn(); } void Frog::makeConn() { nslRelabel(worldXZ,visual.worldXZ); nslRelabel(xb_init,depth.xb_init); nslRelabel(xb_end,depth.xb_end); nslRelabel(yb,depth.yb); nslRelabel(sidestep.out,angle); nslRelabel(forward.out,forward_step); nslRelabel(backward.out,backward_step); nslRelabel(xw,depth.xw); nslRelabel(yw,depth.yw); //FeedMotivation nslRelabel(date,feedMotivation.date); nslRelabel(dayhour,feedMotivation.dayhour); nslRelabel(hunger,feedMotivation.hunger); //EndFeedMotivation //CambiosPred nslRelabel(xp,depth.xp); nslRelabel(yp,depth.yp); nslConnect(xf,visual.xf); nslConnect(zf,visual.zf); nslConnect(xf,depth.xf); nslConnect(yf,depth.yf); //FeedMotivation /*nslConnect(hunger,feedMotivation.hunger); nslConnect(date,feedMotivation.date); nslConnect(dayhour,feedMotivation.dayhour); *///EndFeedMotivation nslConnect(visual.visual_field,preyRec.visual_field); nslConnect(visual.visual_field,sorRec.visual_field); nslConnect(depth.ps,forward.psdp); nslConnect(depth.depthY,tactile.depthY); nslConnect(depth.depthX,tactile.depthX); nslConnect(depth.depthY,sorRec.depthY); nslConnect(depth.depthX,sorRec.depthX); nslConnect(depth.depthY,bumpAvoid.depthY); nslConnect(depth.depthX,bumpAvoid.depthX); nslConnect(depth.depthY,forward.depthY); nslConnect(tactile.ps,bumpAvoid.psta); nslConnect(tactile.ps,forward.psta); nslConnect(tactile.ps,sidestep.psta); nslConnect(tactile.ps,backward.psta); //FeedMotivation nslConnect(feedMotivation.ps,preyRec.psm); nslConnect(feedMotivation.ps,mhm.psm); nslConnect(feedMotivation.ps,xform.psm); nslConnect(feedMotivation.ps,forward.psm); //EndFeedMotivation nslConnect(preyRec.t5_2f,preyApproach.t5_2f); nslConnect(preyRec.ps,preyApproach.psfo); nslConnect(preyRec.ps,forward.psfo); nslConnect(sorRec.th10f,sorAvoid.th10f); nslConnect(sorRec.ps,sorAvoid.psso); nslConnect(sorRec.ps,sidestep.psso); nslConnect(preyApproach.prey_hor_f,mhm.prey_hor_f); nslConnect(preyApproach.ps,mhm.pspa); nslConnect(preyApproach.ps,xform.pspa); nslConnect(sorAvoid.gapsf,bumpAvoid.gaps); nslConnect(sorAvoid.ps,mhm.psoa); nslConnect(sorAvoid.ps,xform.psoa); nslConnect(bumpAvoid.baf,mhm.baf); nslConnect(bumpAvoid.ps,sidestep.psba); nslConnect(bumpAvoid.ps,backward.psba); nslConnect(bumpAvoid.gapsf,mhm.gapsf); nslConnect(mhm.ps,forward.psmhm); nslConnect(mhm.ps,sidestep.psmhm); nslConnect(mhm.mhm,wta.mhm); nslConnect(wta.wta_mhm,xform.wta_mhm); nslConnect(xform.angle,sidestep.angle); nslConnect(xform.ps,sidestep.psx); nslConnect(forward.ps,backward.psfw); nslConnect(backward.ps,forward.psbk); //CambiosPred nslConnect(visual.visual_field,predatorRec.visual_field); nslConnect(predatorRec.ps,preyRec.ps_th3); nslConnect(predatorRec.th3f,predatorAvoid.th3f); nslConnect(predatorRec.ps,predatorAvoid.ps_th3); nslConnect(predatorRec.ps,forward.ps_th3); nslConnect(predatorAvoid.predator_hor_f,mhm.predator_hor_f); nslConnect(predatorAvoid.ps,mhm.psps); nslConnect(predatorAvoid.ps,xform.psps); //FinCambiosPred } void Frog::initSys() { } void Frog::endRun() { // int obj_type = 0; // 0 - prey only, 1 - predator double pi = 3.1415926535; //double edge = 3; double edge = 5; double x0 = xf.elem() - xb_init.elem(); double x2 = xb_end.elem() - xf.elem(); double yd = yf.elem() - yb.elem(); double xd = 0; // xf.elem() - xb.elem(); // frog orientation computed in these vars double x1 = 0; double y1 = 0; // frog is always oriented towards prey if (obj_type == 0) { x1 = xw.elem() - xf.elem(); // positive if prey is to the left of frog y1 = yw.elem() - yf.elem(); // positive if prey is upwards from frog } else { // frog is always oriented towards prd x1 = xp.elem() - xf.elem(); // positive if prey is to the left of frog y1 = yp.elem() - yf.elem(); // positive if prey is upwards from frog } double ty = 0; // rotation angle with respect to y axis double tx = 0; // rotation angle with respect to x axis if (x1 != 0.0) tx = atan(y1/x1); else if (y1 != 0.0) // with respect to y axis ty = atan(x1/y1); else { NSLoutput("\nBreak Epoch 0"); nslBreakEpoch(); return; } // for all this to work no scaling between local visual and global world // system coordinates is required /* no se usa actualmente la siguiente seccion ya que considera el angulo "delta" siempre igual a 0 double delta = 0; // angle.elem(); // displacement in x in local visual coord NSLoutput("frog's angle: "); angle.nslPrint(); */ double deltax = 0; double deltay = 0; /* if (x1 != 0.0) { deltax = delta/cos(tx); deltay = delta/sin(tx); NSLoutput("deltax: ",deltax); NSLoutput("deltay: ",deltay); NSLoutput("tx: ",tx); } else if (y1 != 0.0) { deltax = delta/sin(ty); deltay = delta/cos(ty); NSLoutput("deltax: ",deltax); NSLoutput("deltay: ",deltay); NSLoutput("ty: ",ty); } else { NSLoutput("\nBreak Epoch 1"); nslBreakEpoch(); return; } */ if (x1 != 0.0) { yd = deltay - y1; // deltay - xd*y1/x1; // y displacement xd = x1 + deltax; NSLoutput("xd: ",xd); NSLoutput("yd: ",yd); } else if (y1 != 0.0) { xd = deltax - x1; // yd*x1/y1; // x displacement (needs framework rotation) } else { // NSLoutput("\nAte Worm"); nslBreakEpoch(); return; } double iangle = 0; // double iangle = angle.elem()*pi/180; // double t = tan(iangle); double t = 0; // angle with respect to yd if (xd != 0.0) // AW2001: avoid 0 divide t = atan(yd/xd); // atan(y1/xd); // atan(xd/y1); // yd); NSLoutput("t: ",t); NSLoutput("sin(t): ",sin(t)); NSLoutput("cos(t): ",cos(t)); /* if (yf.elem() < yb.elem() + edge && yf.elem() > yb.elem() - edge) { forward_step = forward_step/2.0; } */ double disp = forward_step.elem() - backward_step.elem(); // overall displacement NSLoutput("disp: ", disp); // switch cos<->sin double dispx = disp*cos(t); // fabs(disp)*sin(t); double dispy = disp*sin(t); double dispx0 = dispx; double dispy0 = dispy; /* barrier if (yf.elem() > yb.elem() && (yf.elem() - dispy < yb.elem() + edge) && (xf.elem() + dispx > xb_init.elem() - edge) && (xf.elem() + dispx < xb_end.elem() + edge)) { dispy = yf.elem() - yb.elem() - edge; // maximum movement if (dispy0 != 0.0) // AW2001: avoid 0 divide dispx = dispx0*dispy/dispy0; } yf = yf - dispy; // + backward_step.elem(); if ((yf.elem() >= yb.elem() - edge) && (yf.elem() <= yb.elem() + edge)) { if ((xf.elem() + dispx < xb_init.elem()) && (xf.elem() + dispx > xb_init.elem() - edge)) dispx = xb_init.elem() - xf.elem() - edge; // maximum movement else if ((xf.elem() + dispx > xb_end.elem()) && (xf.elem() + dispx < xb_end.elem() + edge)) dispx = xf.elem() - xb_end.elem() + edge; // maximum movement } */ double x = 0, y = 0; // se calcula si se llego a la presa NSLoutput("dispx: ",dispx); NSLoutput("dispy: ",dispy); NSLoutput("xf: ",xf.elem()); NSLoutput("yf: ",yf.elem()); edge = edge/2.0; // small edges for prey and predator if (obj_type == 0) //Solo Presa { NSLoutput("xw: ",xw.elem()); NSLoutput("yw: ",yw.elem()); if ((yf.elem() - dispy > yw.elem() - edge) && (yf.elem() - dispy < yw.elem() + edge) && (xf.elem() + dispx > xw.elem() - edge) && (xf.elem() + dispx < xw.elem() + edge)) { // dispy = yf.elem() - yw.elem() - edge; // maximum movement // if (dispy0 != 0.0) // AW2001: avoid 0 divide // dispx = dispx0*dispy/dispy0; NSLoutput("\nAte Worm"); yf = yw; xf = xw; nslBreakEpoch(); return; } xf = xf + dispx; // existing yf = yf - dispy; // yf = yf - dispy; aw0301 x = xf.elem() - xw.elem(); y = yf.elem() - yw.elem(); } else //Con predador { NSLoutput("xp: ",xp.elem()); NSLoutput("yp: ",yp.elem()); //if ((yf.elem() - dispy > yp.elem() - edge) && (yf.elem() - dispy < yp.elem() + edge) && //(xf.elem() + dispx > xp.elem() - edge) && (xf.elem() + dispx < xp.elem() + edge)) if ((yf.elem() + dispy > yp.elem() - edge) && (yf.elem() + dispy < yp.elem() + edge) && (xf.elem() - dispx > xp.elem() - edge) && (xf.elem() - dispx < xp.elem() + edge)) { // dispy = yf.elem() - yp.elem() - edge; // maximum movement // if (dispy0 != 0.0) // AW2001: avoid 0 divide // dispx = dispx0*dispy/dispy0; NSLoutput("\nAte Frog"); yf = yp; xf = xp; nslBreakEpoch(); return; } xf = xf - dispx; // existing yf = yf + dispy; // yf = yf - dispy; aw0301 x = xf.elem() - xp.elem(); y = yf.elem() - yp.elem(); } // se utiliza lo siguiente para el nuevo ciclo de computo if (y != 0) { iangle = atan(x/y); wangle = iangle*180/pi; } else { iangle = pi/2; wangle = 90; } } Detour::Detour() : NslModel("Detour"), world("world",this,100,100,100), // world("world",this,200,200,200), prey("prey",this), //CambiosPred predator("predator",this), //FinCambiosPred frog("frog",this,100,100,80,80,80,80,40) // frog("frog",this,200,200,80,80,80,80,40) { makeConn(); } void Detour::makeConn() { nslConnect(world.worldXZ,frog.worldXZ); nslConnect(world.xb_init,frog.xb_init); nslConnect(world.xb_end,frog.xb_end); nslConnect(world.yb,frog.yb); nslConnect(world.zb,frog.zb); //FeedMotivation nslConnect(world.date,frog.date); nslConnect(world.dayhour,frog.dayhour); //EndFeedMotivation nslConnect(frog.xf,world.xf); nslConnect(frog.yf,world.yf); nslConnect(frog.zf,world.zf); nslConnect(frog.wangle,world.wangle); //AW99 nslConnect(prey.xw,frog.xw); nslConnect(prey.yw,frog.yw); nslConnect(prey.xw,world.xw); nslConnect(prey.yw,world.yw); nslConnect(prey.zw,world.zw); //CambiosPred nslConnect(predator.xp,frog.xp); nslConnect(predator.yp,frog.yp); nslConnect(predator.xp,world.xp); nslConnect(predator.yp,world.yp); nslConnect(predator.zp,world.zp); //FinCambiosPred } AslSchemaModel _Detour("Detour");