Commit 24ed4153 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Working version, to be fitted a bit, with not too bad PSO

parent 39b71e8e
......@@ -5,6 +5,6 @@
void infrared_getSensorsValues(double* intensity);
void infrared_init();
Measurement* infrared_updateList(Measurement* measurementList, const double* weights, double* frontSpeedRatio);
Measurement* infrared_updateList(Measurement* measurementList, const double* weights);
#endif
......@@ -10,6 +10,6 @@ void rob_main_loop(Measurement** list,
double* old_speed,
int* applied_speed,
const double* infrared_objects_w,
const double laplace_divider,
double* velocityRatio);
#endif
\ No newline at end of file
double weight_migration,
const double laplace_divider);
#endif
......@@ -50,7 +50,7 @@ void infrared_init(){
}
void infrared_getSensorsValues(double* intensity){
int i;
int i;
#ifdef IS_REAL_ROBOT
double sensorMean[NB_SENSORS];
......@@ -73,38 +73,35 @@ void infrared_getSensorsValues(double* intensity){
#else
double maxIntensity = 0;
int argMaxIntensity;
for(i=0;i<NB_SENSORS;i++)
{
intensity[i] = wb_distance_sensor_get_value(ds[i]);
if (intensity[i] > maxIntensity) {
maxIntensity = intensity[i];
argMaxIntensity = i;
}
}
#endif
}
Measurement* infrared_updateList(Measurement* measurementList, const double* weights, double* frontSpeedRatio){
Measurement* infrared_updateList(Measurement* measurementList, const double* weights){
double intensity[NB_SENSORS];
infrared_getSensorsValues(intensity);
infrared_getSensorsValues(intensity);
int i;
int fakeObjectAdded =0;
for(i=0;i<NB_SENSORS;i++)
{
if(utilities_debug()) printf("sensor %d intensity at orientation %f = %f\n", i, ps_orientations[i], intensity[i]);
if (intensity[i] > DISTANCE_THRESHOLD){
double distance = 1/intensity[i]*10 ;
if(distance < 0.01) distance = 0.01;
if(distance < 0.03) distance = 0.03;
double theta = ps_orientations[i];
if(i==0 || i==1 || i==2 || i==5 || i==6 || i== 7) *frontSpeedRatio = 0.8;
if(i == 2) {
//utilities_minImm(frontSpeedRatio,1);
//utilities_minImm(frontSpeedRatio,1);
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
......@@ -116,36 +113,32 @@ Measurement* infrared_updateList(Measurement* measurementList, const double* wei
temp->relativePosition[DISTANCE] = 0.03; // relative x pos
temp->relativePosition[THETA] = 3.64; // relative y pos
temp->life = 3;
temp->life = 3;
temp->weight = weights[8];
measurementList = temp;
fakeObjectAdded= 1;
}
if(i == 5) {
//utilities_minImm(frontSpeedRatio,1);
Measurement* temp = malloc(sizeof(Measurement));
temp->next = measurementList;
//utilities_minImm(frontSpeedRatio,1);
Measurement* temp = malloc(sizeof(Measurement));
temp->robotID = -1;
temp->next = measurementList;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->robotID = -1;
temp->relativePosition[DISTANCE] = 0.03; // relative x pos
temp->relativePosition[THETA] = 2.64; // relative y pos
temp->life = 3;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->weight = weights[9];
temp->relativePosition[DISTANCE] = 0.03; // relative x pos
temp->relativePosition[THETA] = 2.64; // relative y pos
temp->life = 3;
measurementList = temp;
fakeObjectAdded= 1;
temp->weight = weights[9];
}
measurementList = temp;
}
Measurement* temp = malloc(sizeof(Measurement));
......@@ -160,13 +153,7 @@ Measurement* infrared_updateList(Measurement* measurementList, const double* wei
temp->relativePosition[THETA] = theta; // relative y pos
temp->weight = weights[i];
temp->life = 1;
// //temp->weight = ps_weights[i]*2;
/*if(temp->relativePosition[THETA] > 0 && temp->relativePosition[THETA] < 3.1415926 )
temp->weight = 35;
else
temp->weight = 20;*/
temp->life = 1;
if(utilities_debug()) printf("Object detected: distance = %f, theta = %f\n", temp->relativePosition[DISTANCE], temp->relativePosition[THETA]);
......
......@@ -75,8 +75,6 @@ void laplace_compute(Measurement* objectsInSurround, double* updatedSpeedRefRob,
}
utilities_printMatrix("incidenceMatrix", incidenceMatrix, numberOfObjects+1, numberOfObjects);
int obstacleInSurround = 0;
{
Measurement* nextObject = objectsInSurround;
int j = 0;
......@@ -86,7 +84,6 @@ void laplace_compute(Measurement* objectsInSurround, double* updatedSpeedRefRob,
weightsMatrix[j*numberOfObjects+j] = fabs(nextObject->weight);
else{
weightsMatrix[j*numberOfObjects+j] = -fabs(nextObject->weight);
obstacleInSurround = 1;
}
nextObject = nextObject->next;
j++;
......@@ -134,8 +131,8 @@ void laplace_compute(Measurement* objectsInSurround, double* updatedSpeedRefRob,
i++;
}
}
updatedSpeedRefRob[X] = velLaplaceRefRob[X]/12;
updatedSpeedRefRob[Y] = velLaplaceRefRob[Y]/12;
updatedSpeedRefRob[X] = velLaplaceRefRob[X]/laplace_divider;
updatedSpeedRefRob[Y] = velLaplaceRefRob[Y]/laplace_divider;
//if(utilities_getRobotId() == 8) printf("%f,%f\n", updatedSpeedRefRob[X], updatedSpeedRefRob[Y]);
......
......@@ -33,9 +33,6 @@
#define ROBOT_TO_MONITOR -1
#define TIME_STEP 64 // [ms] Length of time step
#define EPS 0.03
#define MIGR_SPEED 0.05
#define SPEED_CORR 0.05
#define MAX_ACC 0.03
// ********
#define NB_SENSORS 8 // Number of distance sensors
......@@ -61,17 +58,23 @@ void rob_main_reset(){
motors_init();
}
Measurement* rob_main_addMigrationWeight(Measurement* list, double* my_position, double weight_migration){
Measurement* temp = malloc(sizeof(Measurement));
temp->next = list;
void rob_main_migratory(double* my_speed, double* my_position, double laplacianWeight, double frontSpeedRatio){
//realign migration
if(frontSpeedRatio == 1.){
if (my_position[Y] > EPS) my_speed[Y] -= SPEED_CORR;
if (my_position[Y] < -EPS) my_speed[Y] += SPEED_CORR;
}
my_speed[X] = my_speed[X] + MIGR_SPEED ;
temp->robotID = 500;
temp->biasPosition[X] = 0;
temp->biasPosition[Y] = 0;
temp->relativePosition[DISTANCE] = 0.10; // relative x pos
temp->relativePosition[THETA] = -my_position[2]; // relative y pos
temp->life = 1;
temp->weight = fabs(weight_migration);
// add constant migration to the other side
return temp;
}
void rob_main_loop(Measurement** list,
......@@ -80,17 +83,20 @@ void rob_main_loop(Measurement** list,
double* old_speed,
int* applied_speed,
const double* infrared_objects_w,
const double laplace_divider,
double* velocityRatio){
double weight_migration,
const double laplace_divider){
numberPrint++;
if(utilities_debug()) printf("___________________\n");
if(utilities_debug()) printf("Robot %d\n",utilities_getRobotId());
int lastTheta = my_position[2];
deadReckoning_updateSelfPosition(my_position, applied_speed);
int deltaTheta = my_position[2] - lastTheta;
Measurement* element = *list;
Measurement* previousElement = NULL;
while(element != NULL){
element->life--;
if(element->life == 0){
......@@ -107,28 +113,15 @@ void rob_main_loop(Measurement** list,
else element = previousElement->next;
}
*list = rob_main_addMigrationWeight(*list, my_position, weight_migration);
*list = rangeAndBearing_getRelativeRobotPositions(*list);
*list = infrared_updateList(*list, infrared_objects_w, velocityRatio);
*list = infrared_updateList(*list, infrared_objects_w);
double my_speed_tmp[2];
// compute new speed according to laplace method in initial referential
laplace_compute(*list, my_speed, laplace_divider);
double laplacianWeight = (0.4-exp(-sqrtf(my_speed[X]*my_speed[X]+my_speed[Y]*my_speed[Y])*20)*0.4);
if(utilities_debug()) printf("Before migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
utilities_transformReferential(my_speed, my_speed_tmp, -my_position[2]); //from Rt to R0
// add migration urge and realignment in intial referential
if(utilities_debug()) printf("Velocity ratio = %f / Laplacian Weight: %f\n", *velocityRatio, laplacianWeight);
rob_main_migratory(my_speed_tmp, my_position, laplacianWeight, *velocityRatio);
// change speed in inital referential to current referential
utilities_transformReferential(my_speed_tmp, my_speed, my_position[2]); //from R0 to Rt
if(utilities_debug()) printf("After migr x = %f, y = %f\n", my_speed[X], my_speed[Y]);
//my_speed[X] = velocityRatio * my_speed[X] + (1-velocityRatio) * old_speed[X];
if(utilities_debug()) printf("Speed x = %f, y = %f\n", my_speed[X], my_speed[Y]);
if(my_speed[X] - old_speed[X] > MAX_ACC) my_speed[X] = old_speed[X] + MAX_ACC;
if(my_speed[X] - old_speed[X] < -MAX_ACC) my_speed[X] = old_speed[X] - MAX_ACC;
......@@ -138,14 +131,8 @@ void rob_main_loop(Measurement** list,
old_speed[X] = my_speed[X];
old_speed[Y] = my_speed[Y];
if(*velocityRatio < 1.) *velocityRatio += 0.05;
if(utilities_debug()) printf("After readapt x = %f, y = %f\n", my_speed[X], my_speed[Y]);
if(utilities_debug()) printf("After speed readapt x = %f, y = %f\n", my_speed[X], my_speed[Y]);
// if(utilities_getRobotId() == 8) printf("%f,%f\n", my_speed[X], my_speed[Y]);
//if(utilities_getRobotId() == 8) printf("time step: %d\n", numberPrint);
// apply speed to motors
motors_applySpeed(my_speed, applied_speed);
......@@ -156,4 +143,4 @@ void rob_main_loop(Measurement** list,
int i;
for(i=0;i<20000;i++); //wait 10ms;
#endif
}
\ No newline at end of file
}
No preview for this file type
......@@ -12,8 +12,9 @@
#include "../commonSrc/inc/measurements.h"
#include <stdlib.h>
const double ps_weights[] = {20, 10, 5, 5, 5, 5, 15, 30, 10, 15};
const double ps_weights[] = {15, 15, 15, 0, 0, 35, 35, 35, 10, 15};
const double laplacian_divider = 12;
const double weight_migration = 5;
int main(){
......@@ -22,8 +23,6 @@ int main(){
double my_speed[2] = {0,0};
double old_speed[2] = {0,0};
int applied_speed[2] = {0,0};
double velocityRatio = 1;
Measurement* list = NULL;
for(;;){
rob_main_loop( &list,
......@@ -32,8 +31,8 @@ int main(){
old_speed,
applied_speed,
ps_weights,
laplacian_divider,
&velocityRatio);
weight_migration,
laplacian_divider);
}
}
......@@ -16,7 +16,7 @@
#define NB_SENSOR 8 // Number of proximity sensors
#define NB_WEIGHT 11
#define DATASIZE 2*NB_WEIGHT+6 // Number of elements in particle
#define DATASIZE NB_WEIGHT // Number of elements in particle
// Fitness definitions
#define MAX_DIFF (2*MAX_SPEED) // Maximum difference between wheel speeds
......@@ -26,7 +26,8 @@ WbDeviceTag emitter;
WbDeviceTag rec;
const double ps_weights[NB_WEIGHT] = {20., 10., 5., 5., 5., 5., 15., 30., 10., 15.};
const double ps_weights[] = {15, 15, 15, 0, 0, 35, 35, 35, 10, 15};
const double laplacian_divider = 12;
void reset_sup_com(void) {
emitter = wb_robot_get_device("emitter_epuck");
......@@ -83,12 +84,11 @@ double fitfunc(const double* infra_obj_w,int its) {
double my_speed[2] = {0,0};
double old_speed[2] = {0,0};
int applied_speed[2] = {0,0};
double velocityRatio = 1;
Measurement* list = NULL;
const double laplacian_divider = fabs(infra_obj_w[20]);
//const double laplacian_divider = 12;
//const double weight_migration = 5;
const double weight_migration = fabs(infra_obj_w[10]);
// Evaluate fitness repeatedly
for (j=0;j<its;j++) {
......@@ -104,8 +104,8 @@ double fitfunc(const double* infra_obj_w,int its) {
old_speed,
applied_speed,
infra_obj_w,
laplacian_divider,
&velocityRatio);
weight_migration,
laplacian_divider);
// Get current fitness value
// Average speed
fit_speed += (abs(applied_speed[0]) + abs(applied_speed[1]))/MAX_DIFF;
......
......@@ -9,7 +9,7 @@
#define FONT "Arial"
#define NB_WEIGHT 11
#define DATASIZE 2*NB_WEIGHT+6
#define DATASIZE NB_WEIGHT
#define SWARMSIZE 10
// Functions
......
......@@ -132,6 +132,11 @@ int main() {
printf("Performance: %.3f\n",fit);
endfit += fit/10; // average over the 10 runs
}
for (i = 0; i < NB_WEIGHT; i++){
printf("Weight: %d %.3f\n",i, (double) bestw[i]);
}
printf("Average performance: %.3f\n",endfit);
......
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff0000000100000127000001f4fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000001f40000008b00ffffff00000003000007800000023afc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000008700ffffff00000657000001f400000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff0000000100000002000001180000032a0100000002010000000101
sceneTreePerspectives: 000000ff000000010000000200000177000000000100000002010000000201
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff00000001000001270000020dfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000020d000000a200ffffff000000030000073d0000023afc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff000006100000020d00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff0000000100000002000001180000032a0100000006010000000101
sceneTreePerspectives: 000000ff000000010000000200000177000000000100000006010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 1 "controllers/ctrl1/ctrl1.c" "controllers/pso_epuck/pso_epuck.c"
renderingDevicePerspectives: epuck4:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck6:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck7:camera;1;0.820513;0;0
textFiles: 0 "controllers/ctrl1/ctrl1.c" "controllers/pso_epuck/pso_epuck.c"
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck5:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck7:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck3:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck4:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck8:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck9:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck6:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck5:camera;1;0.820513;0;0
......@@ -7,5 +7,5 @@ centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c" "controllers/ctrl1/inc/utilities.h"
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck0:camera;1;1;0;0
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff000000010000027a00000262fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000002620000008b00ffffff0000000300000780000001ccfc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000008700ffffff000005040000026200000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000002010000000201
perspectives: 000000ff00000000fd00000003000000000000000000000000fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000027a0000027bfc0200000001fb0000001400540065007800740045006400690074006f007201000000160000027b000000a200ffffff000000030000073d000001ccfc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff000004bd0000027b00000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff00000001000000020000016b000007750100000006010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000201
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 0 "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
Webots Project File version R2019b
perspectives: 000000ff00000000fd00000003000000000000037a00000495fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000008700ffffff000000010000030500000368fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000003680000008b00ffffff0000000300000780000000c6fc0100000001fb0000000e0043006f006e0073006f006c00650100000000000007800000008700ffffff000004790000036800000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff0000000100000002000000fa000000c50100000002010000000100
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fa0100000002010000000200
perspectives: 000000ff00000000fd00000003000000000000037a00000495fc0100000001fb0000001a0044006f00630075006d0065006e0074006100740069006f006e0000000000ffffffff0000005400ffffff000000010000030500000381fc0200000001fb0000001400540065007800740045006400690074006f0072010000001600000381000000a200ffffff000000030000073d000000c6fc0100000001fb0000000e0043006f006e0073006f006c006501000000000000073d0000005400ffffff000004320000038100000001000000020000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000118000000c50100000006010000000100
sceneTreePerspectives: 000000ff0000000100000002000000c0000000fc0100000006010000000200
maximizedDockId: -1
centralWidgetVisible: 1
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 1 "controllers/pso_epuck/pso_epuck.c" "controllers/pso_osb_sup/pso_obs_sup.c"
textFiles: 0 "controllers/pso_epuck/pso_epuck.c" "controllers/pso_osb_sup/pso_obs_sup.c"
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck3:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck4:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck0:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck1:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck2:camera;1;0.820513;0;0
renderingDevicePerspectives: epuck4:camera;1;0.820513;0;0
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment