From b947545113b9d83a43dee887fc751ed7abd2e70e Mon Sep 17 00:00:00 2001 From: p3jawors Date: Tue, 20 Oct 2020 15:35:24 -0400 Subject: [PATCH] Cherrypicked payload branches from old fork - updated to work with latest version of airsim - made necessary changes to Unreal/Plugins/Source that were not required for the last version --- AirLib/include/api/RpcLibClientBase.hpp | 2 ++ AirLib/include/api/WorldSimApiBase.hpp | 1 + AirLib/include/common/AirSimSettings.hpp | 11 ++++++- AirLib/include/physics/FastPhysicsEngine.hpp | 30 ++++++++++++++----- AirLib/include/physics/PhysicsEngineBase.hpp | 1 + AirLib/src/api/RpcLibClientBase.cpp | 6 +++- AirLib/src/api/RpcLibServerBase.cpp | 5 +++- PythonClient/airsim/client.py | 14 +++++++-- .../AirSim/Source/SimMode/SimModeBase.cpp | 7 +++++ .../AirSim/Source/SimMode/SimModeBase.h | 1 + .../Source/SimMode/SimModeWorldBase.cpp | 6 ++++ .../AirSim/Source/SimMode/SimModeWorldBase.h | 1 + Unreal/Plugins/AirSim/Source/WorldSimApi.cpp | 5 ++++ Unreal/Plugins/AirSim/Source/WorldSimApi.h | 1 + 14 files changed, 78 insertions(+), 13 deletions(-) diff --git a/AirLib/include/api/RpcLibClientBase.hpp b/AirLib/include/api/RpcLibClientBase.hpp index fd90824524..23bc36a754 100644 --- a/AirLib/include/api/RpcLibClientBase.hpp +++ b/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 listVehicles(); std::string getSettingsString() const; diff --git a/AirLib/include/api/WorldSimApiBase.hpp b/AirLib/include/api/WorldSimApiBase.hpp index 833e590d7d..3f2c27642b 100644 --- a/AirLib/include/api/WorldSimApiBase.hpp +++ b/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 listVehicles() const = 0; virtual std::string getSettingsString() const = 0; diff --git a/AirLib/include/common/AirSimSettings.hpp b/AirLib/include/common/AirSimSettings.hpp index 7977813ad4..08d8c618a2 100644 --- a/AirLib/include/common/AirSimSettings.hpp +++ b/AirLib/include/common/AirSimSettings.hpp @@ -414,6 +414,7 @@ namespace airlib std::string speed_unit_label = "m\\s"; std::map> sensor_defaults; Vector3r wind = Vector3r::Zero(); + Vector3r ext_force = Vector3r::Zero(); CameraSettingMap external_cameras; std::string settings_text_ = ""; @@ -1203,6 +1204,15 @@ 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) @@ -1212,7 +1222,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) { diff --git a/AirLib/include/physics/FastPhysicsEngine.hpp b/AirLib/include/physics/FastPhysicsEngine.hpp index 39ee9c48f2..80d56322c7 100644 --- a/AirLib/include/physics/FastPhysicsEngine.hpp +++ b/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,12 @@ 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 +94,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 +267,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 +332,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(dt); @@ -354,9 +363,13 @@ namespace airlib //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, wind); + const Wrench drag_wrench = getDragWrench(body, current.pose.orientation, avg_linear, avg_angular, wind);//, ext_force); + + // 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; + 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 +472,7 @@ namespace airlib bool enable_ground_lock_; TTimePoint last_message_time; Vector3r wind_; + Vector3r ext_force_; }; } } //namespace diff --git a/AirLib/include/physics/PhysicsEngineBase.hpp b/AirLib/include/physics/PhysicsEngineBase.hpp index 9aa95939e9..5586e8cdf7 100644 --- a/AirLib/include/physics/PhysicsEngineBase.hpp +++ b/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 diff --git a/AirLib/src/api/RpcLibClientBase.cpp b/AirLib/src/api/RpcLibClientBase.cpp index 891c5155d6..0cc4ed1cca 100644 --- a/AirLib/src/api/RpcLibClientBase.cpp +++ b/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 RpcLibClientBase::listVehicles() { return pimpl_->client.call("listVehicles").as>(); diff --git a/AirLib/src/api/RpcLibServerBase.cpp b/AirLib/src/api/RpcLibServerBase.cpp index 0710bd8d5a..77d9cf324f 100644 --- a/AirLib/src/api/RpcLibServerBase.cpp +++ b/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 { return getWorldSimApi()->listVehicles(); }); diff --git a/PythonClient/airsim/client.py b/PythonClient/airsim/client.py index 4f139ac552..f9cfd2256c 100644 --- a/PythonClient/airsim/client.py +++ b/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) \ No newline at end of file + return CarControls.from_msgpack(controls_raw) diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp index 6edb123860..47febf38c4 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.cpp +++ b/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 ASimModeBase::createApiServer() const { //this will be the case when compilation with RPCLIB is disabled or simmode doesn't support APIs diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h index 943cece855..f21144a611 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeBase.h +++ b/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); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index 176cb610f3..f86fe850cd 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -74,6 +74,7 @@ std::unique_ptr 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); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index bf54f2b5d7..37be47fedb 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/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(); diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp index 11e7b6cf97..7387f52a9b 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.cpp @@ -727,6 +727,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 WorldSimApi::listVehicles() const { std::vector vehicle_names; diff --git a/Unreal/Plugins/AirSim/Source/WorldSimApi.h b/Unreal/Plugins/AirSim/Source/WorldSimApi.h index a65a8187ef..4b3c0e1f83 100644 --- a/Unreal/Plugins/AirSim/Source/WorldSimApi.h +++ b/Unreal/Plugins/AirSim/Source/WorldSimApi.h @@ -76,6 +76,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 listVehicles() const override;