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

Author: rajat2004

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