SITL: adding wind simulation in Webots

libraries/SITL/SIM_Webots.cpp

@@ -364,7 +364,6 @@ void Webots::output_rover(const struct sitl_input &input)
     const int len = snprintf(buf, sizeof(buf)-1, "{\"rover\": [%f, %f], \"wnd\": [%f, %f, %f, %f]}\n",
              motor1, motor2,
              input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
-    //printf("rover motors m1: %f m2: %f\n", steer_angle, speed_ms);
     
     buf[len] = 0;
 
@@ -400,7 +399,6 @@ void Webots::output_quad(const struct sitl_input &input)
              m_front, m_right, m_back, m_left,
              input.wind.speed, wind_ef.x, wind_ef.y, wind_ef.z);
     buf[len] = 0;
-
     sim_sock->send(buf, len);
 }
 

libraries/SITL/examples/Webots/.gitignore

@@ -0,0 +1,4 @@
+*.so
+*.o
+*.d
+*.wbproj

libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c

@@ -33,6 +33,7 @@
 #include <sys/types.h>
 #include <webots/robot.h>
 #include <webots/supervisor.h>
+#include <webots/emitter.h>
 #include "ardupilot_SITL_QUAD.h"
 #include "sockets.h"
 #include "sensors.h"
@@ -49,12 +50,13 @@ static WbDeviceTag compass;
 static WbDeviceTag gps;
 static WbDeviceTag camera;
 static WbDeviceTag inertialUnit;
+static WbDeviceTag emitter;
 static WbNodeRef world_info;
 
 static const double *northDirection;
 static double v[MOTOR_NUM];
 int port;
-
+float drafFactor = VEHICLE_DRAG_FACTOR;
 
 static int timestep;
 
@@ -192,6 +194,41 @@ void update_controls()
   #endif
 
 
+  #ifdef WIND_SIMULATION
+  /*
+    Drag: Fd = ½ ρ Cd A v²
+
+    Fd is drag force in Newtons
+    ρ is the density of air in kg/m³
+    Cd is the drag coefficient
+    A is the cross section of our quad in m³ in the direction of movement
+    v is the velocity in m/s
+  */
+  if (northDirection[0] == 1)
+  {
+    wind_webots_axis.x =  state.wind.x - linear_velocity[0];
+    wind_webots_axis.z = -state.wind.y - linear_velocity[2];   // "-state.wind.y" as angle 90 wind is from EAST.
+    wind_webots_axis.y =  state.wind.z - linear_velocity[1];
+  }
+  else
+  { // as in pyramids and any open map street world.
+    wind_webots_axis.x =  state.wind.y - linear_velocity[0]; // always add "linear_velocity" as there is no axis transformation here.
+    wind_webots_axis.z = -state.wind.x - linear_velocity[2];
+    wind_webots_axis.y =  state.wind.z - linear_velocity[1];
+  }
+
+  wind_webots_axis.x = drafFactor * wind_webots_axis.x * abs(wind_webots_axis.x);
+  wind_webots_axis.z = drafFactor * wind_webots_axis.z * abs(wind_webots_axis.z);
+  wind_webots_axis.y = drafFactor * wind_webots_axis.y * abs(wind_webots_axis.y);
+
+  wb_emitter_send(emitter, &wind_webots_axis, sizeof(VECTOR4F));
+  
+  #ifdef DEBUG_WIND
+  printf("wind sitl: %f %f %f %f\n",state.wind.w, state.wind.x, state.wind.y, state.wind.z);
+  printf("wind ctrl: (drafFactor) %f %f %f %f %f\n",drafFactor, wind_webots_axis.w, wind_webots_axis.x, wind_webots_axis.y, wind_webots_axis.z);
+  #endif
+
+  #endif
 }
 
 
@@ -363,6 +400,15 @@ void initialize (int argc, char *argv[])
           if (argc > i+1 )
           {
             port = atoi (argv[i+1]);
+            printf("socket port %d\n",port);
+          }
+        }
+        else if (strcmp (argv[i],"-df")==0)
+        {
+          if (argc > i+1 )
+          {
+            drafFactor = atof (argv[i+1]);
+            printf("drag Factor %f\n",drafFactor);
           }
         }
     }
@@ -373,7 +419,7 @@ void initialize (int argc, char *argv[])
   /* necessary to initialize webots stuff */
   wb_robot_init();
   
-
+  // Get WorldInfo Node.
   WbNodeRef root, node;
   WbFieldRef children, field;
   int n, i;
@@ -390,14 +436,13 @@ void initialize (int argc, char *argv[])
     }
   }
 
-  printf("\n");
   node = wb_supervisor_field_get_mf_node(children, 0);
   field = wb_supervisor_node_get_field(node, "northDirection");
   northDirection = wb_supervisor_field_get_sf_vec3f(field);
   
   if (northDirection[0] == 1)
   {
-    printf ("Axis Default Directions");
+    printf ("Axis Default Directions\n");
   }
 
   printf("WorldInfo.northDirection = %g %g %g\n\n", northDirection[0], northDirection[1], northDirection[2]);
@@ -405,6 +450,10 @@ void initialize (int argc, char *argv[])
 
 
   
+  // get Self Node
+  self_node = wb_supervisor_node_get_self();
+
+  
   // keybaard
   timestep = (int)wb_robot_get_basic_time_step();
   wb_keyboard_enable(timestep);
@@ -435,6 +484,10 @@ void initialize (int argc, char *argv[])
   camera = wb_robot_get_device("camera1");
    wb_camera_enable(camera, timestep);
 
+  #ifdef WIND_SIMULATION
+  // emitter
+  emitter = wb_robot_get_device("emitter_plugin");
+  #endif
   
   const char *MOTOR_NAMES[] = {"motor1", "motor2", "motor3", "motor4"};
   

libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h

@@ -1,9 +1,12 @@
 // #define DEBUG_MOTORS 
+// #define DEBUG_WIND    
 // #define DEBUG_SENSORS   
- #define DEBUG_USE_KB 
+// #define DEBUG_USE_KB 
 // #define DEBUG_INPUT_DATA
 // #define LINEAR_THRUST
+#define WIND_SIMULATION
 
+#define VEHICLE_DRAG_FACTOR 0.001
 
 #define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0]))
 
@@ -24,6 +27,15 @@ typedef struct vector4f VECTOR4F;
 struct {
         double timestamp;
         VECTOR4F motors;
+        VECTOR4F wind; 
+        /*
+         struct {
+        float speed;      // m/s
+        float direction;  // degrees 0..360
+        float turbulence;
+        float dir_z;	  //degrees -90..90
+        } wind;
+        */
        } state, last_state;
 
 
@@ -35,7 +47,15 @@ struct keytable {
         void *ptr;
         enum data_type type;
 
-} keytable[1] = {
+} keytable[2] = {
         //{ "", "timestamp", &state.timestamp, DATA_DOUBLE },
-        { "", "engines",    &state.motors, DATA_VECTOR4F }
+        { "", "eng",    &state.motors, DATA_VECTOR4F },
+        { "", "wnd",    &state.wind, DATA_VECTOR4F }
 };
+
+/*
+        w: wind speed
+        x , y, z: wind direction.
+*/
+VECTOR4F __attribute__((packed, aligned(1)))  wind_webots_axis;
+

libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.c

@@ -133,16 +133,16 @@ void getGyro (const WbDeviceTag gyro, const double *northDirection, char *buf)
 
 void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection,  char * buf)
 {
-    const double * vel = wb_supervisor_node_get_velocity (nodeRef);
+    linear_velocity = wb_supervisor_node_get_velocity (nodeRef);
-    if (vel != NULL)
+    if (linear_velocity != NULL)
     {
       if (northDirection[0] == 1)
       {
-        sprintf (buf,"[%f, %f, %f]", vel[0], vel[2], vel[1]);
+        sprintf (buf,"[%f, %f, %f]", linear_velocity[0], linear_velocity[2], linear_velocity[1]);
       }
       else
       {
-        sprintf (buf,"[%f, %f, %f]",  vel[2], -vel[0], vel[1]);
+        sprintf (buf,"[%f, %f, %f]",  linear_velocity[2], -linear_velocity[0], linear_velocity[1]);
       } 
     }
     
@@ -186,7 +186,7 @@ void getAllSensors (char *buf, const double *northDirection, WbDeviceTag gyro, W
         getCompass(compass, northDirection, compass_buf);
         getGPS(gps, northDirection, gps_buf);
         getInertia (inertial_unit, northDirection, inertial_buf);
-        getLinearVelocity(wb_supervisor_node_get_self(), northDirection, linear_velocity_buf);
+        getLinearVelocity(self_node, northDirection, linear_velocity_buf);
 
         sprintf (buf,"{\"ts\": %s,\"vehicle.imu\": {\"av\": %s,\"la\": %s,\"mf\": %s,\"vehicle.gps\":{%s},\"vehicle.velocity\":{\"wlv\": %s},\"vehicle.pose\":{%s,%s}}\r\n"
                                   , szTime,                                  gyro_buf,                    acc_buf,                 compass_buf,               gps_buf,                                  linear_velocity_buf,               gps_buf, inertial_buf );

libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.h

@@ -11,7 +11,8 @@
 #include <webots/camera.h>
 
 
-
+WbNodeRef self_node;
+double *linear_velocity;
 
 void getInertia (const WbDeviceTag inertialUnit, const double *northDirection, char *buf);
 void getLinearVelocity (WbNodeRef nodeRef, const double *northDirection, char * buf);

libraries/SITL/examples/Webots/plugins/physics/sitl_physics_env/Makefile

@@ -0,0 +1,74 @@
+# Copyright 1996-2019 Cyberbotics Ltd.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+### Generic Makefile.include for Webots controllers, physics plugins, robot
+### window libraries, remote control libraries and other libraries
+### to be used with GNU make
+###
+### Platforms: Windows, macOS, Linux
+### Languages: C, C++
+###
+### Authors: Olivier Michel, Yvan Bourquin, Fabien Rohrer
+###          Edmund Ronald, Sergei Poskriakov
+###
+###-----------------------------------------------------------------------------
+###
+### This file is meant to be included from the Makefile files located in the
+### Webots projects subdirectories. It is possible to set a number of variables
+### to customize the build process, i.e., add source files, compilation flags,
+### include paths, libraries, etc. These variables should be set in your local
+### Makefile just before including this Makefile.include. This Makefile.include
+### should never be modified.
+###
+### Here is a description of the variables you may set in your local Makefile:
+###
+### ---- C Sources ----
+### if your program uses several C source files:
+### C_SOURCES = my_plugin.c my_clever_algo.c my_graphics.c
+###
+### ---- C++ Sources ----
+### if your program uses several C++ source files:
+### CXX_SOURCES = my_plugin.cc my_clever_algo.cpp my_graphics.c++
+###
+### ---- Compilation options ----
+### if special compilation flags are necessary:
+### CFLAGS = -Wno-unused-result
+###
+### ---- Linked libraries ----
+### if your program needs additional libraries:
+### INCLUDE = -I"/my_library_path/include"
+### LIBRARIES = -L"/path/to/my/library" -lmy_library -lmy_other_library
+###
+### ---- Linking options ----
+### if special linking flags are needed:
+### LFLAGS = -s
+###
+### ---- Webots included libraries ----
+### if you want to use the Webots C API in your C++ controller program:
+### USE_C_API = true
+### if you want to link with the Qt framework embedded in Webots:
+### QT = core gui widgets network
+###
+### ---- Debug mode ----
+### if you want to display the gcc command line for compilation and link, as
+### well as the rm command details used for cleaning:
+### VERBOSE = 1
+###
+###-----------------------------------------------------------------------------
+
+### Do not modify: this includes Webots global Makefile.include
+space :=
+space +=
+WEBOTS_HOME_PATH=$(subst $(space),\ ,$(strip $(subst \,/,$(WEBOTS_HOME))))
+include $(WEBOTS_HOME_PATH)/resources/Makefile.include

libraries/SITL/examples/Webots/plugins/physics/sitl_physics_env/sitl_physics_env.c

@@ -0,0 +1,106 @@
+/*
+ * File:
+ * Date:
+ * Description:
+ * Author:
+ * Modifications:
+ */
+
+#include <ode/ode.h>
+#include <plugins/physics.h>
+
+dBodyID vehicleID; 
+
+static pthread_mutex_t mutex; // needed to run with multi-threaded version of ODE
+
+/*
+ * Note: This plugin will become operational only after it was compiled and associated with the current world (.wbt).
+ * To associate this plugin with the world follow these steps:
+ *  1. In the Scene Tree, expand the "WorldInfo" node and select its "physics" field
+ *  2. Then hit the [Select] button at the bottom of the Scene Tree
+ *  3. In the list choose the name of this plugin (same as this file without the extention)
+ *  4. Then save the .wbt by hitting the "Save" button in the toolbar of the 3D view
+ *  5. Then reload the world: the plugin should now load and execute with the current simulation
+ */
+
+void webots_physics_init() {
+  pthread_mutex_init(&mutex, NULL);
+  
+  vehicleID = dWebotsGetBodyFromDEF("DEF_VEHICLE");
+  if (vehicleID == NULL)
+  {
+     dWebotsConsolePrintf("WARNING: NO DEF_VEHICLE OBJECT TO CONTROL no wind calculations will be applied.\n");
+     return ;
+  }
+  else
+  {
+    dWebotsConsolePrintf("DEF_VEHICLE object found.\n");
+     
+  }
+  /*
+   * Get ODE object from the .wbt model, e.g.
+   *   dBodyID body1 = dWebotsGetBodyFromDEF("MY_ROBOT");
+   *   dBodyID body2 = dWebotsGetBodyFromDEF("MY_SOLID");
+   *   dGeomID geom2 = dWebotsGetGeomFromDEF("MY_SOLID");
+   * If an object is not found in the .wbt world, the function returns NULL.
+   * Your code should correcly handle the NULL cases because otherwise a segmentation fault will crash Webots.
+   *
+   * This function is also often used to add joints to the simulation, e.g.
+   *   dWorldID world = dBodyGetWorld(body1);
+   *   pthread_mutex_lock(&mutex);
+   *   dJointID joint = dJointCreateBall(world, 0);
+   *   dJointAttach(joint, body1, body2);
+   *   pthread_mutex_unlock(&mutex);
+   *   ...
+   */
+}
+
+void webots_physics_step() {
+  /*
+   * Do here what needs to be done at every time step, e.g. add forces to bodies
+   *   dBodyAddForce(body1, f[0], f[1], f[2]);
+   *   ...
+   */
+        
+   int dataSize;
+   if (vehicleID != NULL)
+   {
+      const char *data = (const char *)dWebotsReceive(&dataSize);
+      if (dataSize > 0)
+      {
+        float wind[4];
+        memcpy (&wind[0],data,sizeof(float)*4);
+#if 0        
+        dWebotsConsolePrintf("wind: %f %f %f %f \n", wind[0],wind[1],wind[2],wind[3]);
+#endif        
+        dBodyAddForce(vehicleID, wind[1], wind[2], wind[3]);
+      }
+     
+   }
+}
+
+int webots_physics_collide(dGeomID g1, dGeomID g2) {
+  /*
+   * This function needs to be implemented if you want to overide Webots collision detection.
+   * It must return 1 if the collision was handled and 0 otherwise.
+   * Note that contact joints should be added to the contact_joint_group which can change over the time, e.g.
+   *   n = dCollide(g1, g2, MAX_CONTACTS, &contact[0].geom, sizeof(dContact));
+   *   dJointGroupID contact_joint_group = dWebotsGetContactJointGroup();
+   *   dWorldID world = dBodyGetWorld(body1);
+   *   ...
+   *   pthread_mutex_lock(&mutex);
+   *   dJointCreateContact(world, contact_joint_group, &contact[i])
+   *   dJointAttach(contact_joint, body1, body2);
+   *   pthread_mutex_unlock(&mutex);
+   *   ...
+   */
+  return 0;
+}
+
+void webots_physics_cleanup() {
+  /*
+   * Here you need to free any memory you allocated in above, close files, etc.
+   * You do not need to free any ODE object, they will be freed by Webots.
+   */
+  pthread_mutex_destroy(&mutex);
+}

libraries/SITL/examples/Webots/quadPlus.parm

@@ -25,3 +25,7 @@ EK2_LOG_MASK     7
 LOG_BITMASK      978943   
 LOG_MAV_BUFSIZE  32
 LOG_REPLAY       1
+WPNAV_SPEED      800
+SIM_WIND_DIR     90.0
+SIM_WIND_SPD     6.0
+SIM_WIND_TURB    0.0

libraries/SITL/examples/Webots/quadX.parm

@@ -25,3 +25,6 @@ EK2_LOG_MASK     7
 LOG_BITMASK      978943   
 LOG_MAV_BUFSIZE  32
 LOG_REPLAY       1
+SIM_WIND_DIR     90.0
+SIM_WIND_SPD     5.0
+SIM_WIND_TURB    0.0

libraries/SITL/examples/Webots/worlds/pyramidMapReduced2.wbt

@@ -4,6 +4,7 @@ WorldInfo {
     "World generated using the Open Street Map to Webots importer"
     "Author: David Mansolino <david.mansolino@epfl.ch>"
   ]
+  physics "sitl_physics_env"
   basicTimeStep 2
   FPS 5
   optimalThreadCount 4
@@ -34,9 +35,13 @@ Floor {
     metalness 0
   }
 }
-Robot {
+DEF DEF_VEHICLE Robot {
   rotation -1.6967771456756733e-10 1 2.4151813868919206e-10 -0.7512383045779282
   children [
+    Emitter {
+      name "emitter_plugin"
+      description "commuicates with physics plugin"
+    }
     Shape {
       appearance Appearance {
         material Material {
@@ -343,7 +348,7 @@ Robot {
   }
   rotationStep 0.261799
   controller "ardupilot_SITL_QUAD"
-  controllerArgs "-p 5599"
+  controllerArgs "-p 5599 -df 0.02"
   supervisor TRUE
 }
 Road {

libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt

@@ -1,11 +1,18 @@
 #VRML_SIM R2019b utf8
 WorldInfo {
   gravity 0 -9.80665 0
+  physics "sitl_physics_env"
   basicTimeStep 2
   FPS 15
   optimalThreadCount 4
   randomSeed 52
 }
+Viewpoint {
+  orientation 0.5542589845891367 0.8247263718920818 0.11235385844706426 5.801159266912386
+  position -4.295082945651872 3.093288102690911 9.827811912728198
+  follow "quad_plus_sitl"
+  followOrientation TRUE
+}
 DogHouse {
   translation 34.82 0.76 -24.56
   name "dog house(1)"
@@ -18,12 +25,6 @@ DogHouse {
   translation 185.42 0.77 48.97
   name "dog house(5)"
 }
-Viewpoint {
-  orientation 0.9206416250609748 0.36103346961097993 0.14857264898272837 5.442484201093071
-  position -1.9234783002250637 6.507213279340571 5.620669046678885
-  follow "quad_plus_sitl"
-  followOrientation TRUE
-}
 Background {
   skyColor [
     0.15 0.5 1
@@ -47,10 +48,14 @@ Solid {
   ]
   name "solid(1)"
 }
-Robot {
+DEF DEF_VEHICLE Robot {
-  translation -0.027600999999999997 0.674307 0.005031
+  translation -0.027601 0.674307 0.005031
   rotation -0.012461000916064287 0.999885073506054 -0.008635000634797779 -0.22761130717958622
   children [
+    Emitter {
+      name "emitter_plugin"
+      description "commuicates with physics plugin"
+    }
     Shape {
       appearance Appearance {
         material Material {
@@ -356,7 +361,7 @@ Robot {
   }
   rotationStep 0.261799
   controller "ardupilot_SITL_QUAD"
-  controllerArgs "-p 5599"
+  controllerArgs "-p 5599 -df 0.01"
   supervisor TRUE
 }
 DirectionalLight {

libraries/SITL/examples/Webots/worlds/webots_quadX.wbt

@@ -1,6 +1,7 @@
 #VRML_SIM R2019b utf8
 WorldInfo {
   gravity 0 -9.80665 0
+  physics "sitl_physics_env"
   basicTimeStep 2
   FPS 25
   optimalThreadCount 4
@@ -47,10 +48,14 @@ Solid {
   ]
   name "solid(1)"
 }
-Robot {
+DEF DEF_VEHICLE Robot {
   translation -0.027601 0.674307 0.005031
   rotation 0 1 0 0.785388
   children [
+    Emitter {
+      name "emitter_plugin"
+      description "commuicates with physics plugin"
+    }
     Shape {
       appearance Appearance {
         material Material {
@@ -362,7 +367,7 @@ Robot {
   }
   rotationStep 0.261799
   controller "ardupilot_SITL_QUAD"
-  controllerArgs "-p 5599"
+  controllerArgs "-p 5599  -df 0.01"
   supervisor TRUE
 }
 DirectionalLight {