Merge pull request #8 from CodexLabsLLC/payload

Added function to apply external forces (simple payload) microsoft#4519

Author: xxEoD2242

AirLib/include/api/RpcLibClientBase.hpp

@@ -161,6 +161,8 @@ namespace airlib
         bool isRecording();
 
         void simSetWind(const Vector3r& wind) const;
+        void simSetExtForce(const Vector3r& ext_force) const;
+
         vector<string> listVehicles();
 
         std::string getSettingsString() const;

AirLib/include/api/WorldSimApiBase.hpp

@@ -86,6 +86,7 @@ namespace airlib
         virtual bool isRecording() const = 0;
 
         virtual void setWind(const Vector3r& wind) const = 0;
+        virtual void setExtForce(const Vector3r& ext_force) const = 0;
         virtual vector<string> listVehicles() const = 0;
 
         virtual std::string getSettingsString() const = 0;

AirLib/include/common/AirSimSettings.hpp

@@ -419,6 +419,7 @@ namespace airlib
         std::string speed_unit_label = "m\\s";
         std::map<std::string, std::shared_ptr<SensorSetting>> sensor_defaults;
         Vector3r wind = Vector3r::Zero();
+        Vector3r ext_force = Vector3r::Zero();
         CameraSettingMap external_cameras;
 
         std::string settings_text_ = "";
@@ -1208,6 +1209,13 @@ namespace airlib
                     wind = createVectorSetting(child_json, wind);
                 }
             }
+            {
+                // External Force Settings
+                Settings child_json;
+                if (settings_json.getChild("ExternalForce", child_json)) {
+                    ext_force = createVectorSetting(child_json, ext_force);
+                }
+            }
         }
 
         static void loadDefaultCameraSetting(const Settings& settings_json, CameraSetting& camera_defaults)
@@ -1217,7 +1225,6 @@ namespace airlib
                 camera_defaults = createCameraSetting(child_json, camera_defaults);
             }
         }
-
         static void loadCameraDirectorSetting(const Settings& settings_json,
                                               CameraDirectorSetting& camera_director, const std::string& simmode_name)
         {

AirLib/include/physics/FastPhysicsEngine.hpp

@@ -22,8 +22,8 @@ namespace airlib
     class FastPhysicsEngine : public PhysicsEngineBase
     {
     public:
-        FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero())
-            : enable_ground_lock_(enable_ground_lock), wind_(wind)
+        FastPhysicsEngine(bool enable_ground_lock = true, Vector3r wind = Vector3r::Zero(), Vector3r ext_force = Vector3r::Zero())
+            : enable_ground_lock_(enable_ground_lock), wind_(wind), ext_force_(ext_force)
         {
             setName("FastPhysicsEngine");
         }
@@ -69,6 +69,11 @@ namespace airlib
         {
             wind_ = wind;
         }
+        // Set External Force
+        void setExtForce(const Vector3r& ext_force) override
+        {
+            ext_force_ = ext_force;
+        }
 
     private:
         void initPhysicsBody(PhysicsBody* body_ptr)
@@ -88,7 +93,7 @@ namespace airlib
 
             //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, wind_);
+            getNextKinematicsNoCollision(dt, body, current, next, next_wrench, wind_, ext_force_);
 
             //if there is collision, see if we need collision response
             const CollisionInfo collision_info = body.getCollisionInfo();
@@ -261,8 +266,11 @@ namespace airlib
             }
         }
 
-        static Wrench getDragWrench(const PhysicsBody& body, const Quaternionr& orientation,
-                                    const Vector3r& linear_vel, const Vector3r& angular_vel_body, const Vector3r& wind_world)
+        static Wrench getDragWrench(const PhysicsBody& body,
+                                    const Quaternionr& orientation,
+                                    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 + wind
             //drag vector magnitude is proportional to v^2, direction opposite of velocity
@@ -323,7 +331,7 @@ namespace airlib
         }
 
         static void getNextKinematicsNoCollision(TTimeDelta dt, PhysicsBody& body, const Kinematics::State& current,
-                                                 Kinematics::State& next, Wrench& next_wrench, const Vector3r& wind)
+                                                 Kinematics::State& next, Wrench& next_wrench, const Vector3r& wind, const Vector3r& ext_force)
         {
             const real_T dt_real = static_cast<real_T>(dt);
 
@@ -356,7 +364,11 @@ namespace airlib
                 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, wind);
 
-                next_wrench = body_wrench + drag_wrench;
+                // ext_force is defined in world space
+                Wrench ext_force_wrench = Wrench::zero();
+                ext_force_wrench.force = ext_force;
+
+                next_wrench = body_wrench + drag_wrench + ext_force_wrench;
 
                 //Utils::log(Utils::stringf("B-WRN %s: ", VectorMath::toString(body_wrench.force).c_str()));
                 //Utils::log(Utils::stringf("D-WRN %s: ", VectorMath::toString(drag_wrench.force).c_str()));
@@ -459,6 +471,7 @@ namespace airlib
         bool enable_ground_lock_;
         TTimePoint last_message_time;
         Vector3r wind_;
+        Vector3r ext_force_;
     };
 }
 } //namespace

AirLib/include/physics/PhysicsEngineBase.hpp

@@ -28,6 +28,7 @@ namespace airlib
         }
 
         virtual void setWind(const Vector3r& wind) { unused(wind); };
+        virtual void setExtForce(const Vector3r& ext_force) { unused(ext_force); };
     };
 }
 } //namespace

AirLib/src/api/RpcLibClientBase.cpp

@@ -620,7 +620,11 @@ __pragma(warning(disable : 4239))
             RpcLibAdaptorsBase::Vector3r conv_wind(wind);
             pimpl_->client.call("simSetWind", conv_wind);
         }
-
+        void RpcLibClientBase::simSetExtForce(const Vector3r& ext_force) const
+        {
+            RpcLibAdaptorsBase::Vector3r conv_ext_force(ext_force);
+            pimpl_->client.call("simSetExtForce", conv_ext_force);
+        }
         vector<string> RpcLibClientBase::listVehicles()
         {
             return pimpl_->client.call("listVehicles").as<vector<string>>();

AirLib/src/api/RpcLibServerBase.cpp

@@ -278,7 +278,6 @@ namespace airlib
             const auto& response = getWorldSimApi()->getDetections(type, CameraDetails(camera_name, vehicle_name, external));
             return RpcLibAdaptorsBase::DetectionInfo::from(response);
         });
-
         pimpl_->server.bind("reset", [&]() -> void {
             //Exit if already resetting.
             static bool resetInProgress;
@@ -501,6 +500,10 @@ namespace airlib
             getWorldSimApi()->setWind(wind.to());
         });
 
+        pimpl_->server.bind("simSetExtForce", [&](const RpcLibAdaptorsBase::Vector3r& ext_force) -> void {
+            getWorldSimApi()->setExtForce(ext_force.to());
+        });
+
         pimpl_->server.bind("listVehicles", [&]() -> vector<string> {
             return getWorldSimApi()->listVehicles();
         });

PythonClient/airsim/client.py

@@ -1113,7 +1113,17 @@ def getSettingsString(self):
         """
         return self.client.call('getSettingsString')
 
-#----------------------------------- Multirotor APIs ---------------------------------------------
+    def simSetExtForce(self, ext_force):
+        """
+        Set arbitrary external forces, in World frame, NED direction. Can be used
+        for implementing simple payloads.
+
+        Args:
+            ext_force (Vector3r): Force, in World frame, NED direction, in N
+        """
+        self.client.call('simSetExtForce', ext_force)
+
+# -----------------------------------  Multirotor APIs ---------------------------------------------
 class MultirotorClient(VehicleClient, object):
     def __init__(self, ip = "", port = 41451, timeout_value = 3600):
         super(MultirotorClient, self).__init__(ip, port, timeout_value)
@@ -1618,4 +1628,4 @@ def getCarControls(self, vehicle_name=''):
             CarControls:
         """
         controls_raw = self.client.call('getCarControls', vehicle_name)
-        return CarControls.from_msgpack(controls_raw)
+        return CarControls.from_msgpack(controls_raw)

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

@@ -107,6 +107,13 @@ std::unique_ptr<msr::airlib::ApiServerBase> SimModeBase::createApiServer() const
     return nullptr;
 }
 
+void SimModeBase::setExtForce(const msr::airlib::Vector3r& ext_force) const
+{
+    // should be overridden by derived class
+    unused(ext_force);
+    throw std::domain_error("setExtForce not implemented by SimMode");
+}
+
 void SimModeBase::setupClockSpeed()
 {
     //default setup - this should be overridden in derived modes as needed

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

@@ -48,6 +48,7 @@ class SimModeBase
     virtual void continueForTime(double seconds);
     virtual void continueForFrames(uint32_t frames);
     virtual void setWind(const msr::airlib::Vector3r& wind) const;
+    virtual void setExtForce(const msr::airlib::Vector3r& ext_force) const;
     void startApiServer();
     void stopApiServer();
     bool isApiServerStarted();

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

@@ -67,6 +67,8 @@ std::unique_ptr<SimModeWorldBase::PhysicsEngineBase> SimModeWorldBase::createPhy
         else {
             physics_engine.reset(new msr::airlib::FastPhysicsEngine());
         }
+
+        physics_engine->setExtForce(getSettings().ext_force);
     }
     else if (physics_engine_name == "ExternalPhysicsEngine") {
         physics_engine.reset(new msr::airlib::ExternalPhysicsEngine());
@@ -98,6 +100,11 @@ void SimModeWorldBase::setWind(const msr::airlib::Vector3r& wind) const
     physics_engine_->setWind(wind);
 }
 
+void SimModeWorldBase::setExtForce(const msr::airlib::Vector3r& ext_force) const
+{
+    physics_engine_->setExtForce(ext_force);
+}
+
 void SimModeWorldBase::updateDebugReport(msr::airlib::StateReporterWrapper& debug_reporter)
 {
     unused(debug_reporter);

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

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

Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.cpp

@@ -279,6 +279,11 @@ bool WorldSimApi::testLineOfSightBetweenPoints(const msr::airlib::GeoPoint& poin
     return false;
 }
 
+void WorldSimApi::setExtForce(const msr::airlib::Vector3r& ext_force) const
+{
+    simmode_->setExtForce(ext_force);
+}
+
 std::vector<msr::airlib::GeoPoint> WorldSimApi::getWorldExtents() const
 {
     std::vector<msr::airlib::GeoPoint> result;

Unity/AirLibWrapper/AirsimWrapper/Source/WorldSimApi.h

@@ -21,6 +21,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase
     virtual std::string spawnObject(const std::string& object_name, const std::string& load_component, const Pose& pose, const Vector3r& scale, bool physics_enabled, bool is_blueprint) override { return ""; };
     virtual bool destroyObject(const std::string& object_name) override { return false; };
     virtual std::vector<std::string> listAssets() const override;
+    virtual void setExtForce(const Vector3r& ext_force) const override;
 
     virtual bool isPaused() const override;
     virtual void reset() override;

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

@@ -312,6 +312,13 @@ void ASimModeBase::setWind(const msr::airlib::Vector3r& wind) const
     throw std::domain_error("setWind not implemented by SimMode");
 }
 
+void ASimModeBase::setExtForce(const msr::airlib::Vector3r& ext_force) const
+{
+    // should be overridden by derived class
+    unused(ext_force);
+    throw std::domain_error("setExtForce 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

@@ -63,6 +63,7 @@ class AIRSIM_API ASimModeBase : public AActor
     virtual void continueForFrames(uint32_t frames);
 
     virtual void setWind(const msr::airlib::Vector3r& wind) const;
+    virtual void setExtForce(const msr::airlib::Vector3r& ext_force) 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

@@ -74,6 +74,7 @@ std::unique_ptr<ASimModeWorldBase::PhysicsEngineBase> ASimModeWorldBase::createP
         }
 
         physics_engine->setWind(getSettings().wind);
+        physics_engine->setExtForce(getSettings().ext_force);
     }
     else if (physics_engine_name == "ExternalPhysicsEngine") {
         physics_engine.reset(new msr::airlib::ExternalPhysicsEngine());
@@ -136,6 +137,11 @@ void ASimModeWorldBase::setWind(const msr::airlib::Vector3r& wind) const
     physics_engine_->setWind(wind);
 }
 
+void ASimModeWorldBase::setExtForce(const msr::airlib::Vector3r& ext_force) const
+{
+    physics_engine_->setExtForce(ext_force);
+}
+
 void ASimModeWorldBase::updateDebugReport(msr::airlib::StateReporterWrapper& debug_reporter)
 {
     unused(debug_reporter);

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

@@ -34,6 +34,7 @@ class AIRSIM_API ASimModeWorldBase : public ASimModeBase
     virtual void continueForFrames(uint32_t frames) override;
 
     virtual void setWind(const msr::airlib::Vector3r& wind) const override;
+    virtual void setExtForce(const msr::airlib::Vector3r& ext_force) const override;
 
 protected:
     void startAsyncUpdator();

Unreal/Plugins/AirSim/Source/WorldSimApi.cpp

@@ -719,6 +719,11 @@ void WorldSimApi::setWind(const Vector3r& wind) const
     simmode_->setWind(wind);
 }
 
+void WorldSimApi::setExtForce(const Vector3r& ext_force) const
+{
+    simmode_->setExtForce(ext_force);
+}
+
 std::vector<std::string> WorldSimApi::listVehicles() const
 {
     std::vector<std::string> vehicle_names;

Unreal/Plugins/AirSim/Source/WorldSimApi.h

@@ -75,6 +75,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase
     virtual bool isRecording() const override;
 
     virtual void setWind(const Vector3r& wind) const override;
+    virtual void setExtForce(const Vector3r& ext_force) const override;
     virtual bool createVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file) override;
     virtual std::vector<std::string> listVehicles() const override;