diff --git a/libraries/SITL/SIM_Webots.cpp b/libraries/SITL/SIM_Webots.cpp index b3c2f87e4b..845f6aa1ca 100644 --- a/libraries/SITL/SIM_Webots.cpp +++ b/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); } diff --git a/libraries/SITL/examples/Webots/.gitignore b/libraries/SITL/examples/Webots/.gitignore new file mode 100644 index 0000000000..bee67875a3 --- /dev/null +++ b/libraries/SITL/examples/Webots/.gitignore @@ -0,0 +1,4 @@ +*.so +*.o +*.d +*.wbproj \ No newline at end of file diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD index 6730e76741..bdc2b85623 100755 Binary files a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD and b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD differ diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c index b3a8818fa9..984e2d663c 100644 --- a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.c @@ -33,6 +33,7 @@ #include #include #include +#include #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,7 +484,11 @@ 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"}; // get motor device tags diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h index 326cd475d7..9412be252d 100644 --- a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/ardupilot_SITL_QUAD.h @@ -1,9 +1,12 @@ -// #define DEBUG_MOTORS +// #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 LINEAR_THRUST +#define WIND_SIMULATION +#define VEHICLE_DRAG_FACTOR 0.001 #define ARRAY_SIZE(_arr) (sizeof(_arr) / sizeof(_arr[0])) @@ -24,7 +27,16 @@ typedef struct vector4f VECTOR4F; struct { double timestamp; VECTOR4F motors; - } state, last_state; + 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 } -}; \ No newline at end of file + { "", "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; + diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.c b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.c index 86c27888d9..742979754c 100644 --- a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.c +++ b/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); - if (vel != NULL) + linear_velocity = wb_supervisor_node_get_velocity (nodeRef); + 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 ); diff --git a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.h b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.h index 8656f3e445..4a4a6575f1 100644 --- a/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.h +++ b/libraries/SITL/examples/Webots/controllers/ardupilot_SITL_QUAD/sensors.h @@ -11,7 +11,8 @@ #include - +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); diff --git a/libraries/SITL/examples/Webots/plugins/physics/sitl_physics_env/Makefile b/libraries/SITL/examples/Webots/plugins/physics/sitl_physics_env/Makefile new file mode 100644 index 0000000000..42bf3f7207 --- /dev/null +++ b/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 diff --git a/libraries/SITL/examples/Webots/plugins/physics/sitl_physics_env/sitl_physics_env.c b/libraries/SITL/examples/Webots/plugins/physics/sitl_physics_env/sitl_physics_env.c new file mode 100644 index 0000000000..d3802e60b1 --- /dev/null +++ b/libraries/SITL/examples/Webots/plugins/physics/sitl_physics_env/sitl_physics_env.c @@ -0,0 +1,106 @@ +/* + * File: + * Date: + * Description: + * Author: + * Modifications: + */ + +#include +#include + +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); +} diff --git a/libraries/SITL/examples/Webots/quadPlus.parm b/libraries/SITL/examples/Webots/quadPlus.parm index 13b6dd55c8..25f14ee38b 100644 --- a/libraries/SITL/examples/Webots/quadPlus.parm +++ b/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 \ No newline at end of file diff --git a/libraries/SITL/examples/Webots/quadX.parm b/libraries/SITL/examples/Webots/quadX.parm index 50a68ef75a..1e5145dceb 100644 --- a/libraries/SITL/examples/Webots/quadX.parm +++ b/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 \ No newline at end of file diff --git a/libraries/SITL/examples/Webots/worlds/pyramidMapReduced2.wbt b/libraries/SITL/examples/Webots/worlds/pyramidMapReduced2.wbt index 8a7a6836d4..1661deb06a 100644 --- a/libraries/SITL/examples/Webots/worlds/pyramidMapReduced2.wbt +++ b/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 " ] + 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 { diff --git a/libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt b/libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt index 2d12469211..486c3eb789 100644 --- a/libraries/SITL/examples/Webots/worlds/webots_quadPlus.wbt +++ b/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 { - translation -0.027600999999999997 0.674307 0.005031 +DEF DEF_VEHICLE Robot { + 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 { diff --git a/libraries/SITL/examples/Webots/worlds/webots_quadX.wbt b/libraries/SITL/examples/Webots/worlds/webots_quadX.wbt index fd55a483d8..d2f2c8158b 100644 --- a/libraries/SITL/examples/Webots/worlds/webots_quadX.wbt +++ b/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 {