Commit 7f2e2079 authored by Nicolas Peslerbe's avatar Nicolas Peslerbe

Hugo update

parent 5fa423b9
......@@ -31,7 +31,7 @@
// ******** SETTINGS PARAMETERS
#define USE_SUPERVISOR false
#define FLOCK_SIZE 5
#define NEIGHBOR_BOUND
#define NEIGHBOR_BOUND
#define TIME_STEP 64 // [ms] Length of time step
......@@ -54,6 +54,11 @@ static int robot_id_u, robot_id;
static void reset(){
wb_robot_init();
// Initialize dead reckoning variables at (0,0,0)
double dr_x = 0;
double dr_y = 0;
double dr_theta = 0;
robot_name=(char*) wb_robot_get_name();
sscanf(robot_name,"epuck%d",&robot_id_u);
robot_id = robot_id_u%FLOCK_SIZE;
......@@ -69,13 +74,20 @@ int main(){
reset(); // Resetting the robot
float new_speed[2] = {0,0};
float vel_migr[2] = {5,0};
for(;;){
Measurement* list = rangeAndBearing_getRelativeRobotPositions(robot_name);
infrared_updateList(list);
laplace_compute(list, new_speed);
// realign robot in direction of the migratory urge
dead_reckoning(&dr_x, &dr_y, &dr_theta, vel_migr);
// compute new speed
laplace_compute(list, new_speed, vel_migr);
// apply speed to motors
motors_applySpeed(new_speed);
wb_robot_step(TIME_STEP);
}
}
void dead_reckoning(double* dr_x, double* dr_y, double* dr_theta, double* vel_mig){
// update dead reckoning via motor steps since last call
e_do_dr();
// retrieve new position
e_get_dr_position(&dr_x, &dr_y, &dr_theta);
// compute new velocity to realign according to the migratory urge
vel_mig -> V0 + dr_theta
}
/**************************************************/
/* Particle Swarm Optimization */
/* Header file */
/* */
/* Author: Jim Pugh */
/* Last Modified: 25.10.2012 */
/* */
/**************************************************/
#define FONT "Arial"
#define NB_SENSOR 8
#define DATASIZE 2*NB_SENSOR+6
#define SWARMSIZE 10
// Functions
double* pso(int,int,double,double,double,double,double,int,int,int); // Run particle swarm optimization
void fitness(double[][DATASIZE],double[],int[][SWARMSIZE]); // Fitness function for particle evolution
double rnd(void); // Generate random number in [0,1]
void findPerformance(double[][DATASIZE],double[],double[],char,int,int[][SWARMSIZE]); // Find the current performance of the swarm
void updateLocalPerf(double[][DATASIZE],double[],double[][DATASIZE],double[],double[]); // Update the best performance of a single particle
void copyParticle(double[],double[]); // Copy value of one particle to another
void updateNBPerf(double[][DATASIZE],double[],double[][DATASIZE],double[],int[][SWARMSIZE]); // Update the best performance of a particle neighborhood
int mod(int,int); // Modulus function
double s(double); // S-function to transform [-infinity,infinity] to [0,1]
double bestResult(double[][DATASIZE],double[],double[]); // Find the best result in a swarm
......@@ -101,8 +101,8 @@ void printMatrix(char* name, float* matrix, int m, int n){
}
}
void laplace_compute(Measurement* objectsInSurround, float* velTot){
void laplace_compute(Measurement* objectsInSurround, float* velTot, float* weights){
// Compute neighborhood by range and bearing (contains neighbors robots and obstacles)
// X contains relative positions of each neighbors (size = (num_neighbors * 2)
int numberOfObjects = 0;
......@@ -129,7 +129,7 @@ void laplace_compute(Measurement* objectsInSurround, float* velTot){
}
printMatrix("incidenceMatrix", incidenceMatrix, numberOfObjects+1, numberOfObjects);
int obstacleInSurround = 0;
{
Measurement* nextObject = objectsInSurround;
......@@ -137,9 +137,9 @@ void laplace_compute(Measurement* objectsInSurround, float* velTot){
while(nextObject){
if (nextObject->robotID != -1)
weightsMatrix[j*numberOfObjects+j] = 1;
weightsMatrix[j*numberOfObjects+j] = fabs(weights[0]);
else{
weightsMatrix[j*numberOfObjects+j] = -2; // more importance to avoid obstacle
weightsMatrix[j*numberOfObjects+j] = -fabs(weights[1]);
obstacleInSurround = 1;
}
nextObject = nextObject->next;
......@@ -149,7 +149,7 @@ void laplace_compute(Measurement* objectsInSurround, float* velTot){
printMatrix("weightsMatrix", weightsMatrix, numberOfObjects, numberOfObjects);
float alpha = (obstacleInSurround ? ALPHA_OBST : ALPHA_FREE);
float incidenceMatrixT[numberOfObjects * (numberOfObjects+1)];
transpose(incidenceMatrix, incidenceMatrixT, numberOfObjects+1, numberOfObjects);
printMatrix("incidenceMatrixT", incidenceMatrixT, numberOfObjects, (numberOfObjects+1));
......@@ -159,12 +159,12 @@ void laplace_compute(Measurement* objectsInSurround, float* velTot){
multiply(incidenceMatrix, weightsMatrix, IW, numberOfObjects+1, numberOfObjects, numberOfObjects);
printMatrix("IW", IW, numberOfObjects+1, numberOfObjects);
float L[(numberOfObjects+1) * (numberOfObjects+1)];
multiply(IW, incidenceMatrixT, L, numberOfObjects+1, numberOfObjects, numberOfObjects+1);
printMatrix("L", L, numberOfObjects+1, numberOfObjects+1);
float vel_graph[2] = {0, 0};
// Compute velocity of current robot with v = -L(x-B)
{
......@@ -181,6 +181,7 @@ void laplace_compute(Measurement* objectsInSurround, float* velTot){
float vel_migr[2] = {5, 0};
velTot[X] = alpha * vel_graph[X] + (1-alpha) * vel_migr[X]; // tot velocity is a weighted sum of graph and migration velocities
velTot[Y] = alpha * vel_graph[Y] + (1-alpha) * vel_migr[Y];
}
......@@ -215,4 +216,4 @@ int main(){
return 0;
}
#endif
\ No newline at end of file
#endif
/********************************************************************************/
/* File: motors.c */
/* Date: Dec-19 */
/* Description: Motors lib - DIS Project */
/* */
/* Author: Hugo Aguettaz, Marie-Joe Stoeri & Nicolas Peslerbe */
/********************************************************************************/
#include "inc/motors.h"
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/differential_wheels.h>
#include <math.h>
#define KU 4
#define KW 4
#define R 0.041/2 //radius of a wheel
#define L 0.053/2 //half distance between wheels
static WbDeviceTag left_motor;
static WbDeviceTag right_motor;
void motors_init(){
left_motor = wb_robot_get_device("left wheel motor");
right_motor = wb_robot_get_device("right wheel motor");
wb_motor_set_position(left_motor, INFINITY);
wb_motor_set_position(right_motor, INFINITY);
}
// function to transform speed of the center of mass of the robot to left and right wheels speeds
void motors_transformSpeed(float* speed, float* v_right, float* v_left){
linearSpeed = KU * sqrt(pow(speed[X],2)+pow(speed[Y],2)) * cos(atan2(speed[Y],speed[X]));
rotationalSpeed = KW * atan2(speed[Y],speed[X]);
v_right* = (linearSpeed + L * rotationalSpeed)/R;
v_left* = (linearSpeed - L * rotationalSpeed)/R;
}
void motors_applySpeed(float* speed){
float* v_right;
float* v_left;
motors_transformSpeed(speed, v_right, v_left);
wb_motor_set_position(left_motor, v_left);
wb_motor_set_position(right_motor, v_right);
}
\ No newline at end of file
#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <webots/robot.h>
#include <webots/motor.h>
#include <webots/differential_wheels.h>
#include <webots/distance_sensor.h>
#include <webots/light_sensor.h>
#include <webots/emitter.h>
#include <webots/receiver.h>
#define MAX_SPEED 1000.0 // Maximum speed of wheels in each direction
#define MAX_SPEED_WEB 6.28 // Maximum speed webots
#define MAX_ACC 1000.0 // Maximum amount speed can change in 128 ms
#define NB_SENSOR 8 // Number of proximity sensors
#define DATASIZE 2 // Number of elements in particle
// Fitness definitions
#define MAX_DIFF (2*MAX_SPEED) // Maximum difference between wheel speeds
#define MAX_SENS 4096.0 // Maximum sensor value
WbDeviceTag ds[NB_SENSOR];
WbDeviceTag emitter;
WbDeviceTag rec;
WbDeviceTag left_motor, right_motor;
double good_w[DATASIZE] = {-11.15, -16.93, -8.20, -18.11, -17.99, 8.55, -8.89, 3.52, 29.74,
-7.48, 5.61, 11.16, -9.54, 4.58, 1.41, 2.09, 26.50, 23.11,
-3.44, -3.78, 23.20, 8.41};
int braiten;
void reset(void) {
char text[4];
int i;
text[1]='s';
text[3]='\0';
for (i=0;i<NB_SENSOR;i++) {
text[0]='p';
text[2]='0'+i;
ds[i] = wb_robot_get_device(text); // distance sensors
}
emitter = wb_robot_get_device("emitter_epuck");
rec = wb_robot_get_device("receiver_epuck");
left_motor = wb_robot_get_device("left wheel motor");
right_motor = wb_robot_get_device("right wheel motor");
wb_motor_set_position(left_motor, INFINITY);
wb_motor_set_position(right_motor, INFINITY);
}
// Generate random number from 0 to 1
double rnd() {
return (double)rand()/RAND_MAX;
}
// Generate Gaussian random number with 0 mean and 1 std
double gauss(void) {
double x1, x2, w;
do {
x1 = 2.0 * rnd() - 1.0;
x2 = 2.0 * rnd() - 1.0;
w = x1*x1 + x2*x2;
} while (w >= 1.0);
w = sqrt((-2.0 * log(w))/w);
return(x1*w);
}
// S-function to transform v variable to [0,1]
double s(double v) {
if (v > 5)
return 1.0;
else if (v < -5)
return 0.0;
else
return 1.0/(1.0 + exp(-1*v));
}
// Find the fitness for obstacle avoidance of the passed controller
double fitfunc(double weights[DATASIZE],int its) {
float speed[2] = {0,0} // Wheel speeds
float old_speed[2]; // Previous wheel speeds (for recursion)
//int left_encoder,right_encoder;
double ds_value[NB_SENSOR];
int i,j;
// Fitness variables
double fit_speed; // Speed aspect of fitness
double fit_diff; // Speed difference between wheels aspect of fitness
double fit_sens; // Proximity sensing aspect of fitness
double sens_val[NB_SENSOR]; // Average values for each proximity sensor
double fitness; // Fitness of controller
// Initially no fitness measurements
fit_speed = 0.0;
fit_diff = 0.0;
for (i=0;i<NB_SENSOR;i++) {
sens_val[i] = 0.0;
}
fit_sens = 0.0;
old_left = 0.0;
old_right = 0.0;
// Evaluate fitness repeatedly
for (j=0;j<its;j++) {
if (braiten) j--; // Loop forever
ds_value[0] = (double) wb_distance_sensor_get_value(ds[0]);
ds_value[1] = (double) wb_distance_sensor_get_value(ds[1]);
ds_value[2] = (double) wb_distance_sensor_get_value(ds[2]);
ds_value[3] = (double) wb_distance_sensor_get_value(ds[3]);
ds_value[4] = (double) wb_distance_sensor_get_value(ds[4]);
ds_value[5] = (double) wb_distance_sensor_get_value(ds[5]);
ds_value[6] = (double) wb_distance_sensor_get_value(ds[6]);
ds_value[7] = (double) wb_distance_sensor_get_value(ds[7]);
// Compute the robot velocity according to laplacian method
Measurement* list = rangeAndBearing_getRelativeRobotPositions(robot_name);
infrared_updateList(list);
laplace_compute(list, speed);
// Make sure we don't accelerate too fast
if (left_speed - old_left > MAX_ACC) left_speed = old_left+MAX_ACC;
if (left_speed - old_left < -MAX_ACC) left_speed = old_left-MAX_ACC;
if (right_speed - old_right > MAX_ACC) left_speed = old_right+MAX_ACC;
if (right_speed - old_right < -MAX_ACC) left_speed = old_right-MAX_ACC;
// Make sure speeds are within bounds
if (left_speed > MAX_SPEED) left_speed = MAX_SPEED;
if (left_speed < -1.0*MAX_SPEED) left_speed = -1.0*MAX_SPEED;
if (right_speed > MAX_SPEED) right_speed = MAX_SPEED;
if (right_speed < -1.0*MAX_SPEED) right_speed = -1.0*MAX_SPEED;
// Set new old speeds
old_speed[0] = speed[0];
old_speed[1] = speed[1];
/*left_encoder = wb_differential_wheels_get_left_encoder();
right_encoder = wb_differential_wheels_get_right_encoder();
if (left_encoder>9000) wb_differential_wheels_set_encoders(0,right_encoder);
if (right_encoder>1000) wb_differential_wheels_set_encoders(left_encoder,0);*/
// Set the motor speeds
//wb_differential_wheels_set_speed((int)left_speed,(int)right_speed);
float msl_w = left_speed*MAX_SPEED_WEB/1000;
float msr_w = right_speed*MAX_SPEED_WEB/1000;
wb_motor_set_velocity(left_motor, (int)msl_w);
wb_motor_set_velocity(right_motor, (int)msr_w);
wb_robot_step(128); // run one step
// Get current fitness value
// Average speed
fit_speed += (fabs(left_speed) + fabs(right_speed))/(2.0*MAX_SPEED);
// Difference in speed
fit_diff += fabs(left_speed - right_speed)/MAX_DIFF;
// Sensor values
for (i=0;i<NB_SENSOR;i++) {
sens_val[i] += ds_value[i]/MAX_SENS;
}
}
// Find most active sensor
for (i=0;i<NB_SENSOR;i++) {
if (sens_val[i] > fit_sens) fit_sens = sens_val[i];
}
// Average values over all steps
fit_speed /= its;
fit_diff /= its;
fit_sens /= its;
// Better fitness should be higher
fitness = fit_speed*(1.0 - sqrt(fit_diff))*(1.0 - fit_sens);
return fitness;
}
//-------------MAIN------------//
int main() {
double buffer[255];
double *rbuffer;
double fit;
int i;
wb_robot_init();
reset();
for(i=0;i<NB_SENSOR;i++) {
wb_distance_sensor_enable(ds[i],64);
}
wb_receiver_enable(rec,32);
//wb_differential_wheels_enable_encoders(64);
braiten = 0; // Don't run forever
wb_robot_step(64);
while (1) {
// Wait for data
while (wb_receiver_get_queue_length(rec) == 0) {
wb_robot_step(64);
}
rbuffer = (double *)wb_receiver_get_data(rec);
// Check for pre-programmed avoidance behavior
if (rbuffer[DATASIZE] == -1.0) {
braiten = 1;
fitfunc(good_w,100);
// Otherwise, run provided controller
} else {
fit = fitfunc(rbuffer,rbuffer[DATASIZE]);
buffer[0] = fit;
wb_emitter_send(emitter,(void *)buffer,sizeof(double));
}
wb_receiver_next_packet(rec);
}
return 0;
}
This diff is collapsed.
This diff is collapsed.
Webots Project File version R2019b
perspectives: 000000ff00000000fd0000000300000000000001de00000433fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e006500540072006500650100000019000003f10000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e02000003cd0000007d000001de0000043300000001000002a900000430fc0200000001fb0000001400540065007800740045006400690074006f00720100000000000004300000008b00ffffff000000030000078000000433fc0100000001fb0000000e0043006f006e0073006f006c00650000000000000007800000008700ffffff000004d50000043000000004000000040000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000100000005120100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000000c0000001120100000002010000000201
perspectives: 000000ff00000000fd0000000300000000000001de00000433fc0100000002fc00000000ffffffff0000000000fffffffc0200000001fb00000012005300630065006e006500540072006500650100000019000003f10000000000000000fb0000001a0044006f00630075006d0065006e0074006100740069006f006e02000003cd0000007d000001de00000433000000010000029b000002cbfc0200000001fb0000001400540065007800740045006400690074006f00720100000000000002cb0000008b00ffffff0000000300000a0000000253fc0100000001fb0000000e0043006f006e0073006f006c0065010000000000000a000000008700ffffff00000763000002cb00000004000000040000000100000008fc00000000
simulationViewPerspectives: 000000ff000000010000000200000100000006530100000002010000000101
sceneTreePerspectives: 000000ff0000000100000002000002f3000000000100000002010000000201
maximizedDockId: -1
centralWidgetVisible: 1
projectionMode: PERSPECTIVE
renderingMode: PLAIN
selectionDisabled: 0
viewpointLocked: 0
orthographicViewHeight: 1
textFiles: 2 "../TP/DIS_19-20_lab02/controllers/obstacles/obstacles.c" "../TP/DIS_19-20_lab02/controllers/braiten_sln/braiten_sln.c" "controllers/ctrl1/ctrl1.c"
renderingDevicePerspectives: epuck2:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
textFiles: 2 "../TP/DIS_19-20_lab02/controllers/braiten_sln/braiten_sln.c" "controllers/ctrl1/ctrl1.c" "controllers/ctrl1/infrared.c"
renderingDevicePerspectives: epuck6:camera;1;1;0;0
renderingDevicePerspectives: epuck5:camera;1;1;0;0
renderingDevicePerspectives: epuck1:camera;1;1;0;0
renderingDevicePerspectives: epuck7:camera;1;1;0;0
renderingDevicePerspectives: epuck9:camera;1;1;0;0
renderingDevicePerspectives: epuck2:camera;1;1;0;0
renderingDevicePerspectives: epuck4:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
renderingDevicePerspectives: epuck3:camera;1;1;0;0
renderingDevicePerspectives: epuck6:camera;1;1;0;0
renderingDevicePerspectives: epuck0:camera;1;1;0;0
renderingDevicePerspectives: epuck8:camera;1;1;0;0
#VRML_SIM R2019a utf8
#VRML_SIM R2019b utf8
WorldInfo {
info [
"Description"
......@@ -221,83 +221,63 @@ DEF long_rock_11 Solid {
size 0.2 0.2 0.01
}
}
DEF SUPERVISOR Robot {
name "super0"
controller ""
supervisor TRUE
children [
DEF emitter Emitter {
channel 1
}
]
}
DEF SUPERVISOR Robot {
name "super1"
controller ""
supervisor TRUE
children [
DEF emitter Emitter {
channel 2
}
]
}
DEF epuck0 E-puck {
translation -0.1 0 0
rotation 0 1 0 1.57
name "epuck0"
controller "flock_controller"
controller "ctrl1"
}
DEF epuck1 E-puck {
translation -0.1 0 -0.1
rotation 0 1 0 1.57
name "epuck1"
controller "flock_controller"
controller "ctrl1"
}
DEF epuck2 E-puck {
translation -0.1 0 0.1
rotation 0 1 0 1.57
name "epuck2"
controller "flock_controller"
controller "ctrl1"
}
DEF epuck3 E-puck {
translation -0.1 0 -0.2
rotation 0 1 0 1.57
name "epuck3"
controller "flock_controller"
controller "ctrl1"
}
DEF epuck4 E-puck {
translation -0.1 0 0.2
rotation 0 1 0 1.57
name "epuck4"
controller "flock_controller"
controller "ctrl1"
}
DEF epuck5 E-puck {
translation -1.9 0 0
rotation 0 1 0 -1.57
name "epuck5"
controller "flock_controller"
controller "ctrl1"
}
DEF epuck6 E-puck {
translation -1.9 0 -0.1
rotation 0 1 0 -1.57
name "epuck6"
controller "flock_controller"
controller "ctrl1"
}
DEF epuck7 E-puck {
translation -1.9 0 0.1
rotation 0 1 0 -1.57
name "epuck7"
controller "flock_controller"
controller "ctrl1"
}
DEF epuck8 E-puck {
translation -1.9 0 -0.2
rotation 0 1 0 -1.57
name "epuck8"
controller "flock_controller"
controller "ctrl1"
}
DEF epuck9 E-puck {
translation -1.9 0 0.2
rotation 0 1 0 -1.57
name "epuck9"
controller "flock_controller"
controller "ctrl1"
}
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