Add Wind simulation (#2867)

* Add Wind force calculation to PhysicsEngine

Use velocity wrt wind to calculate drag forces

* Add Settings field for Wind, simSetWind API

* Add documentation, example script for setWind API

* [Unity] Set Wind API implementation

AirLib/include/api/RpcLibClientBase.hpp

@@ -118,6 +118,8 @@ class RpcLibClientBase {
     void stopRecording();
     bool isRecording();
 
+    void simSetWind(const Vector3r& wind) const;
+
 protected:
     void* getClient();
     const void* getClient() const;

AirLib/include/api/WorldSimApiBase.hpp

@@ -71,6 +71,8 @@ class WorldSimApiBase {
     virtual void startRecording() = 0;
     virtual void stopRecording() = 0;
     virtual bool isRecording() const = 0;
+
+    virtual void setWind(const Vector3r& wind) const = 0;
 };
 
 

AirLib/include/common/AirSimSettings.hpp

@@ -362,6 +362,7 @@ struct AirSimSettings {
     float speed_unit_factor =  1.0f;
     std::string speed_unit_label = "m\\s";
     std::map<std::string, std::unique_ptr<SensorSetting>> sensor_defaults;
+    Vector3r wind = Vector3r::Zero();
 
 public: //methods
     static AirSimSettings& singleton()
@@ -1064,6 +1065,14 @@ struct AirSimSettings {
                 tod_setting.move_sun = tod_settings_json.getBool("MoveSun", tod_setting.move_sun);
             }
         }
+
+        {
+            // Wind Settings
+            Settings child_json;
+            if (settings_json.getChild("Wind", child_json)) {
+                wind = createVectorSetting(child_json, wind);
+            }
+        }
     }
 
     static void loadDefaultCameraSetting(const Settings& settings_json, CameraSetting& camera_defaults)

AirLib/include/physics/FastPhysicsEngine.hpp

@@ -18,8 +18,8 @@ namespace msr { namespace airlib {
 
 class FastPhysicsEngine : public PhysicsEngineBase {
 public:
-    FastPhysicsEngine(bool enable_ground_lock = true)
-        : enable_ground_lock_(enable_ground_lock)
+    FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero())
+        : enable_ground_lock_(enable_ground_lock), wind_(wind)
     { 
     }
 
@@ -59,6 +59,12 @@ class FastPhysicsEngine : public PhysicsEngineBase {
     }
     //*** End: UpdatableState implementation ***//
 
+    // Set Wind, for API and Settings implementation
+    void setWind(const Vector3r& wind) override
+    {
+        wind_ = wind;
+    }
+
 private:
     void initPhysicsBody(PhysicsBody* body_ptr)
     {
@@ -76,7 +82,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
 
         //first compute the response as if there was no collision
         //this is necessary to take in to account forces and torques generated by body
-        getNextKinematicsNoCollision(dt, body, current, next, next_wrench);
+        getNextKinematicsNoCollision(dt, body, current, next, next_wrench, wind_);
 
         //if there is collision, see if we need collision response
         const CollisionInfo collision_info = body.getCollisionInfo();
@@ -255,9 +261,9 @@ class FastPhysicsEngine : public PhysicsEngineBase {
     }
 
     static Wrench getDragWrench(const PhysicsBody& body, const Quaternionr& orientation, 
-        const Vector3r& linear_vel, const Vector3r& angular_vel_body)
+        const Vector3r& linear_vel, const Vector3r& angular_vel_body, const Vector3r& wind_world)
     {
-        //add linear drag due to velocity we had since last dt seconds
+        //add linear drag due to velocity we had since last dt seconds + wind
         //drag vector magnitude is proportional to v^2, direction opposite of velocity
         //total drag is b*v + c*v*v but we ignore the first term as b << c (pg 44, Classical Mechanics, John Taylor)
         //To find the drag force, we find the magnitude in the body frame and unit vector direction in world frame
@@ -268,9 +274,13 @@ class FastPhysicsEngine : public PhysicsEngineBase {
         Wrench wrench = Wrench::zero();
         const real_T air_density = body.getEnvironment().getState().air_density;
 
+        // Use relative velocity of the body wrt wind
+        const Vector3r relative_vel = linear_vel - wind_world;
+        const Vector3r linear_vel_body = VectorMath::transformToBodyFrame(relative_vel, orientation);
+
         for (uint vi = 0; vi < body.dragVertexCount(); ++vi) {
             const auto& vertex = body.getDragVertex(vi);
-            const Vector3r vel_vertex = VectorMath::transformToBodyFrame(linear_vel, orientation) + angular_vel_body.cross(vertex.getPosition());
+            const Vector3r vel_vertex = linear_vel_body + angular_vel_body.cross(vertex.getPosition());
             const real_T vel_comp = vertex.getNormal().dot(vel_vertex);
             //if vel_comp is -ve then we cull the face. If velocity too low then drag is not generated
             if (vel_comp > kDragMinVelocity) {
@@ -312,7 +322,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
     }
 
     static void getNextKinematicsNoCollision(TTimeDelta dt, PhysicsBody& body, const Kinematics::State& current, 
-        Kinematics::State& next, Wrench& next_wrench)
+        Kinematics::State& next, Wrench& next_wrench, const Vector3r& wind)
     {
         const real_T dt_real = static_cast<real_T>(dt);
 
@@ -338,13 +348,13 @@ class FastPhysicsEngine : public PhysicsEngineBase {
             next.accelerations.linear = Vector3r::Zero();
         }
         else {
-            //add linear drag due to velocity we had since last dt seconds
+            //add linear drag due to velocity we had since last dt seconds + wind
             //drag vector magnitude is proportional to v^2, direction opposite of velocity
             //total drag is b*v + c*v*v but we ignore the first term as b << c (pg 44, Classical Mechanics, John Taylor)
             //To find the drag force, we find the magnitude in the body frame and unit vector direction in world frame
             avg_linear = current.twist.linear + current.accelerations.linear * (0.5f * dt_real);
             avg_angular = current.twist.angular + current.accelerations.angular * (0.5f * dt_real);
-            const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular);
+            const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular, wind);
 
             next_wrench = body_wrench + drag_wrench;
 
@@ -449,6 +459,7 @@ class FastPhysicsEngine : public PhysicsEngineBase {
     std::stringstream debug_string_;
     bool enable_ground_lock_;
     TTimePoint last_message_time;
+    Vector3r wind_;
 };
 
 }} //namespace

AirLib/include/physics/PhysicsEngineBase.hpp

@@ -44,6 +44,8 @@ class PhysicsEngineBase : public UpdatableObject {
     virtual void erase_remove(TUpdatableObjectPtr obj) { 
         members_.erase(std::remove(members_.begin(), members_.end(), obj), members_.end()); }
 
+    virtual void setWind(const Vector3r& wind) {};
+
 private:
     MembersContainer members_;
 };

AirLib/src/api/RpcLibClientBase.cpp

@@ -444,6 +444,12 @@ bool RpcLibClientBase::isRecording()
     return pimpl_->client.call("isRecording").as<bool>();
 }
 
+void RpcLibClientBase::simSetWind(const Vector3r& wind) const
+{
+    RpcLibAdapatorsBase::Vector3r conv_wind(wind);
+    pimpl_->client.call("simSetWind", conv_wind);
+}
+
 void* RpcLibClientBase::getClient()
 {
     return &pimpl_->client;

AirLib/src/api/RpcLibServerBase.cpp

@@ -370,6 +370,10 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         return getWorldSimApi()->isRecording();
     });
 
+    pimpl_->server.bind("simSetWind", [&](const RpcLibAdapatorsBase::Vector3r& wind) -> void {
+        getWorldSimApi()->setWind(wind.to());
+    });
+
     //if we don't suppress then server will bomb out for exceptions raised by any method
     pimpl_->server.suppress_exceptions(true);
 }

PythonClient/airsim/client.py

@@ -781,6 +781,15 @@ def isRecording(self):
         """
         return self.client.call('isRecording')
 
+    def simSetWind(self, wind):
+        """
+        Set simulated wind, in World frame, NED direction, m/s
+
+        Args:
+            wind (Vector3r): Wind, in World frame, NED direction, in m/s 
+        """
+        self.client.call('simSetWind', wind)
+
 # -----------------------------------  Multirotor APIs ---------------------------------------------
 class MultirotorClient(VehicleClient, object):
     def __init__(self, ip = "", port = 41451, timeout_value = 3600):

PythonClient/multirotor/set_wind.py

@@ -0,0 +1,35 @@
+import setup_path
+import airsim
+import time
+
+client = airsim.MultirotorClient()
+client.confirmConnection()
+client.enableApiControl(True)
+
+client.armDisarm(True)
+
+print("Setting wind to 10m/s in forward direction") # NED
+wind = airsim.Vector3r(10, 0, 0)
+client.simSetWind(wind)
+
+# Takeoff or hover
+landed = client.getMultirotorState().landed_state
+if landed == airsim.LandedState.Landed:
+    print("taking off...")
+    client.takeoffAsync().join()
+else:
+    print("already flying...")
+    client.hoverAsync().join()
+
+time.sleep(5)
+
+print("Setting wind to 15m/s towards right") # NED
+wind = airsim.Vector3r(0, 15, 0)
+client.simSetWind(wind)
+
+time.sleep(5)
+
+# Set wind to 0
+print("Resetting wind to 0")
+wind = airsim.Vector3r(0, 0, 0)
+client.simSetWind(wind)

Unity/AirLibWrapper/AirsimWrapper/Source/SimMode/SimModeBase.cpp

@@ -86,6 +86,13 @@ void SimModeBase::setTimeOfDay(bool is_enabled, const std::string& start_datetim
         //throw std::domain_error("setTimeOfDay is not implemented by SimMode");
 }
 
+void SimModeBase::setWind(const msr::airlib::Vector3r& wind) const
+{
+    // should be overridden by derived class
+    unused(wind);
+    throw std::domain_error("setWind is not implemented by SimMode");
+}
+
 std::unique_ptr<msr::airlib::ApiServerBase> SimModeBase::createApiServer() const
 {
 	//this will be the case when compilation with RPCLIB is disabled or simmode doesn't support APIs

Unity/AirLibWrapper/AirsimWrapper/Source/SimMode/SimModeBase.h

@@ -46,6 +46,7 @@ class SimModeBase
 	virtual bool isPaused() const;
 	virtual void pause(bool is_paused);
 	virtual void continueForTime(double seconds);
+    virtual void setWind(const msr::airlib::Vector3r& wind) const;
 	void startApiServer();
 	void stopApiServer();
 	bool isApiServerStarted();

Unity/AirLibWrapper/AirsimWrapper/Source/SimMode/SimModeWorldBase.cpp

@@ -94,6 +94,11 @@ void SimModeWorldBase::continueForTime(double seconds)
 	physics_world_->continueForTime(seconds);
 }
 
+void SimModeWorldBase::setWind(const msr::airlib::Vector3r& wind) const
+{
+    physics_engine_->setWind(wind);
+}
+
 void SimModeWorldBase::updateDebugReport(msr::airlib::StateReporterWrapper& debug_reporter)
 {
 	unused(debug_reporter);

Unity/AirLibWrapper/AirsimWrapper/Source/SimMode/SimModeWorldBase.h

@@ -36,6 +36,8 @@ class SimModeWorldBase : public SimModeBase
 	virtual void pause(bool is_paused) override;
 	virtual void continueForTime(double seconds) override;
 
+    virtual void setWind(const msr::airlib::Vector3r& wind) const override;
+
 private:
 	std::unique_ptr<msr::airlib::PhysicsWorld> physics_world_;
 	PhysicsEngineBase* physics_engine_;

Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp

@@ -180,4 +180,9 @@ bool WorldSimApi::isRecording() const
     return false;
 }
 
+void WorldSimApi::setWind(const Vector3r& wind) const
+{
+    simmode_->setWind(wind);
+};
+
 #pragma endregion

Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h

@@ -61,6 +61,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase
     virtual void stopRecording() override;
     virtual bool isRecording() const override;
 
+    virtual void setWind(const Vector3r& wind) const override;
+
 private:
 	SimModeBase * simmode_;
 	std::string vehicle_name_;

Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp

@@ -269,6 +269,13 @@ void ASimModeBase::continueForTime(double seconds)
     throw std::domain_error("continueForTime is not implemented by SimMode");
 }
 
+void ASimModeBase::setWind(const msr::airlib::Vector3r& wind) const
+{
+    // should be overridden by derived class
+    unused(wind);
+    throw std::domain_error("setWind not implemented by SimMode");
+}
+
 std::unique_ptr<msr::airlib::ApiServerBase> ASimModeBase::createApiServer() const
 {
     //this will be the case when compilation with RPCLIB is disabled or simmode doesn't support APIs

Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h

@@ -62,6 +62,8 @@ class AIRSIM_API ASimModeBase : public AActor
     virtual void pause(bool is_paused);
     virtual void continueForTime(double seconds);
 
+    virtual void setWind(const msr::airlib::Vector3r& wind) const;
+
     virtual void setTimeOfDay(bool is_enabled, const std::string& start_datetime, bool is_start_datetime_dst,
         float celestial_clock_speed, float update_interval_secs, bool move_sun);
 

Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp

@@ -62,6 +62,8 @@ std::unique_ptr<ASimModeWorldBase::PhysicsEngineBase> ASimModeWorldBase::createP
         else {
             physics_engine.reset(new msr::airlib::FastPhysicsEngine());
         }
+
+        physics_engine->setWind(getSettings().wind);
     }
     else {
         physics_engine.reset();
@@ -98,6 +100,11 @@ void ASimModeWorldBase::continueForTime(double seconds)
     UGameplayStatics::SetGamePaused(this->GetWorld(), true);
 }
 
+void ASimModeWorldBase::setWind(const msr::airlib::Vector3r& wind) const
+{
+    physics_engine_->setWind(wind);
+}
+
 void ASimModeWorldBase::updateDebugReport(msr::airlib::StateReporterWrapper& debug_reporter)
 {
     unused(debug_reporter);

Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h

@@ -31,6 +31,8 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase
     virtual void pause(bool is_paused) override;
     virtual void continueForTime(double seconds) override;
 
+    virtual void setWind(const msr::airlib::Vector3r& wind) const override;
+
 protected:
     void startAsyncUpdator();
     void stopAsyncUpdator();

Unreal/Plugins/AirSim/Source/WorldSimApi.cpp

@@ -479,3 +479,9 @@ bool WorldSimApi::isRecording() const
 {
     return simmode_->isRecording();
 }
+
+
+void WorldSimApi::setWind(const Vector3r& wind) const
+{
+    simmode_->setWind(wind);
+}

Unreal/Plugins/AirSim/Source/WorldSimApi.h

@@ -64,6 +64,8 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase {
     virtual void stopRecording() override;
     virtual bool isRecording() const override;
 
+    virtual void setWind(const Vector3r& wind) const override;
+
 private:
     AActor* createNewActor(const FActorSpawnParameters& spawn_params, const FTransform& actor_transform, const Vector3r& scale, UStaticMesh* static_mesh);
     void spawnPlayer();

docs/apis.md

@@ -193,6 +193,20 @@ This API works alongwith toggling Recording using R button, therefore if it's en
 
 Note that this will only save the data as specfied in the settings. For full freedom in storing data such as certain sensor information, or in a different format or layout, use the other APIs to fetch the data and save as desired.
 
+### Wind API
+
+Wind can be changed during simulation using `simSetWind()`. Wind is specified in World frame, NED direction and m/s values
+
+E.g. To set 20m/s wind in North (forward) direction -
+
+```python
+# Set wind to (20,0,0) in NED (forward direction)
+wind = airsim.Vector3r(20, 0, 0)
+client.simSetWind(wind)
+```
+
+Also see example script in [set_wind.py](https://github.com/Microsoft/AirSim/blob/master/PythonClient/multirotor/set_wind.py)
+
 ### Lidar APIs
 AirSim offers API to retrieve point cloud data from Lidar sensors on vehicles. You can set the number of channels, points per second, horizontal and vertical FOV, etc parameters in [settings.json](settings.md).
 

docs/settings.md

@@ -45,6 +45,7 @@ Below are complete list of settings available along with their default values. I
   "PhysicsEngineName": "",
   "SpeedUnitFactor": 1.0,
   "SpeedUnitLabel": "m/s",
+  "Wind": { "X": 0, "Y": 0, "Z": 0 },
   "Recording": {
     "RecordOnMove": false,
     "RecordInterval": 0.05,
@@ -222,6 +223,10 @@ The `InitMethod` determines how object IDs are initialized at startup to generat
 
  If `MeshNamingMethod` is "" or "OwnerName" then we use mesh's owner name to generate random hash as object IDs. If it is "StaticMeshName" then we use static mesh's name to generate random hash as object IDs. Note that it is not possible to tell individual instances of the same static mesh apart this way, but the names are often more intuitive.
 
+## Wind Settings
+
+This setting specifies the wind speed in World frame, in NED direction. Values are in m/s. By default, speed is 0, i.e. no wind.
+
 ## Camera Settings
 The `CameraDefaults` element at root level specifies defaults used for all cameras. These defaults can be overridden for individual camera in `Cameras` element inside `Vehicles` as described later.