From e32d487d8e0cbe36bdd02f6a3cce45ab5f2fbd5b Mon Sep 17 00:00:00 2001 From: Jonathan Date: Fri, 23 Apr 2021 13:08:54 -0300 Subject: [PATCH 1/5] new ExternalPhysicsEngine added to be use with setVehiclePose() --- .../include/physics/ExternalPhysicsEngine.hpp | 55 +++++++++++++++++++ AirLib/include/physics/PhysicsBody.hpp | 7 +++ .../multirotor/MultiRotorPhysicsBody.hpp | 15 ++++- .../Source/SimMode/SimModeWorldBase.cpp | 5 ++ .../AirSim/Source/SimMode/SimModeWorldBase.h | 2 +- 5 files changed, 82 insertions(+), 2 deletions(-) create mode 100644 AirLib/include/physics/ExternalPhysicsEngine.hpp diff --git a/AirLib/include/physics/ExternalPhysicsEngine.hpp b/AirLib/include/physics/ExternalPhysicsEngine.hpp new file mode 100644 index 0000000000..609eee0815 --- /dev/null +++ b/AirLib/include/physics/ExternalPhysicsEngine.hpp @@ -0,0 +1,55 @@ +// Copyright (c) Microsoft Corporation. All rights reserved. +// Licensed under the MIT License. + +#ifndef airsim_core_ExternalPhysicsEngine_hpp +#define airsim_core_ExternalPhysicsEngine_hpp + +#include "common/Common.hpp" +#include "physics/PhysicsEngineBase.hpp" +#include +#include +#include +#include +#include "common/CommonStructs.hpp" +#include "common/SteppableClock.hpp" +#include + +namespace msr { namespace airlib { + +class ExternalPhysicsEngine : public PhysicsEngineBase { +public: + ExternalPhysicsEngine() + { + } + + //*** Start: UpdatableState implementation ***// + virtual void resetImplementation() override + { + + } + + virtual void update() override + { + PhysicsEngineBase::update(); + + for (PhysicsBody* body_ptr : *this) { + body_ptr->updateKinematics(); + body_ptr->update(); + } + + } + virtual void reportState(StateReporter& reporter) override + { + for (PhysicsBody* body_ptr : *this) { + reporter.writeValue("ExternalPhysicsEngine",true); + reporter.writeValue("Is Grounded", body_ptr->isGrounded()); + } + //call base + UpdatableObject::reportState(reporter); + } + //*** End: UpdatableState implementation ***// + +}; + +}} //namespace +#endif diff --git a/AirLib/include/physics/PhysicsBody.hpp b/AirLib/include/physics/PhysicsBody.hpp index d2749259b3..7165e68948 100644 --- a/AirLib/include/physics/PhysicsBody.hpp +++ b/AirLib/include/physics/PhysicsBody.hpp @@ -65,6 +65,13 @@ class PhysicsBody : public UpdatableObject { kinematics_->setState(state); kinematics_->update(); } + /** + * Update kinematics without a state + */ + virtual void updateKinematics() + { + kinematics_->update(); + } public: //methods diff --git a/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp index 81902d6cfb..81ceb42493 100644 --- a/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp +++ b/AirLib/include/vehicles/multirotor/MultiRotorPhysicsBody.hpp @@ -62,11 +62,24 @@ class MultiRotorPhysicsBody : public PhysicsBody { //*** End: UpdatableState implementation ***// - //Physics engine calls this method to set next kinematics + //Fast Physics engine calls this method to set next kinematics virtual void updateKinematics(const Kinematics::State& kinematics) override { PhysicsBody::updateKinematics(kinematics); + updateSensorsAndController(); + } + + //External Physics engine calls this method to keep physics bodies updated and move rotors + virtual void updateKinematics() override + { + PhysicsBody::updateKinematics(); + + updateSensorsAndController(); + } + + void updateSensorsAndController() + { updateSensors(*params_, getKinematics(), getEnvironment()); //update controller which will update actuator control signal diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp index f325be9ace..cd87ca0f20 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.cpp @@ -1,4 +1,6 @@ #include "SimModeWorldBase.h" +#include "physics/FastPhysicsEngine.hpp" +#include "physics/ExternalPhysicsEngine.hpp" #include #include "AirBlueprintLib.h" @@ -71,6 +73,9 @@ std::unique_ptr ASimModeWorldBase::createP physics_engine->setWind(getSettings().wind); } + else if (physics_engine_name == "ExternalPhysicsEngine") { + physics_engine.reset(new msr::airlib::ExternalPhysicsEngine()); + } else { physics_engine.reset(); UAirBlueprintLib::LogMessageString("Unrecognized physics engine name: ", physics_engine_name, LogDebugLevel::Failure); diff --git a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h index 723d25fc45..2aa3a400a5 100644 --- a/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h +++ b/Unreal/Plugins/AirSim/Source/SimMode/SimModeWorldBase.h @@ -4,7 +4,7 @@ #include #include #include "api/VehicleSimApiBase.hpp" -#include "physics/FastPhysicsEngine.hpp" +#include "physics/PhysicsEngineBase.hpp" #include "physics/World.hpp" #include "physics/PhysicsWorld.hpp" #include "common/StateReporterWrapper.hpp" From eeab0792a123992b621bfbf2776db638b9e72e05 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Fri, 23 Apr 2021 14:56:35 -0300 Subject: [PATCH 2/5] added Documentation and example script for ExternalPhysicsEngine --- .../multirotor/external_physics_engine.py | 34 +++++++++++++++++++ docs/image_apis.md | 2 +- docs/settings.md | 2 +- 3 files changed, 36 insertions(+), 2 deletions(-) create mode 100644 PythonClient/multirotor/external_physics_engine.py diff --git a/PythonClient/multirotor/external_physics_engine.py b/PythonClient/multirotor/external_physics_engine.py new file mode 100644 index 0000000000..5a7e7ec072 --- /dev/null +++ b/PythonClient/multirotor/external_physics_engine.py @@ -0,0 +1,34 @@ +import setup_path +import airsim +import time + +# This example shows how to use the External Physics Engine +# It allows you to control the drone through setVehiclePose and obtain collision information. +# It is especially useful for injecting your own flight dynamics model to the AirSim drone. + +# Use Blocks environment to see the drone colliding and seeing the collision information +# in the command prompt. + +# Add this line to your settings.json before running AirSim: +# "PhysicsEngineName":"ExternalPhysicsEngine" + + +client = airsim.VehicleClient() +client.confirmConnection() +pose1 = client.simGetVehiclePose() + +pose1.orientation = airsim.to_quaternion(0.1, 0.1, 0.1) +client.simSetVehiclePose( pose1, False ) + +for i in range(900): + print(i) + pose1 = client.simGetVehiclePose() + pose1.position = pose1.position + airsim.Vector3r(0.03, 0, 0) + pose1.orientation = pose1.orientation + airsim.to_quaternion(0.1, 0.1, 0.1) + client.simSetVehiclePose( pose1, False ) + time.sleep(0.003) + collision = client.simGetCollisionInfo() + if collision.has_collided: + print(collision) + +client.reset() \ No newline at end of file diff --git a/docs/image_apis.md b/docs/image_apis.md index b83ca6a10f..9817c47b99 100644 --- a/docs/image_apis.md +++ b/docs/image_apis.md @@ -145,7 +145,7 @@ Before AirSim v1.2, cameras were accessed using ID numbers instead of names. For ## "Computer Vision" Mode -You can use AirSim in so-called "Computer Vision" mode. In this mode, physics engine is disabled and there is no vehicle, just cameras. You can move around using keyboard (use F1 to see help on keys). You can press Record button to continuously generate images. Or you can call APIs to move cameras around and take images. +You can use AirSim in so-called "Computer Vision" mode. In this mode, physics engine is disabled and there is no vehicle, just cameras (If you want to have the vehicle but without its kinematics, you can use the Multirotor mode with the Physics Engine [ExternalPhysicsEngine](settings.md##physicsenginename)). You can move around using keyboard (use F1 to see help on keys). You can press Record button to continuously generate images. Or you can call APIs to move cameras around and take images. To active this mode, edit [settings.json](settings.md) that you can find in your `Documents\AirSim` folder (or `~/Documents/AirSim` on Linux) and make sure following values exist at root level: diff --git a/docs/settings.md b/docs/settings.md index 1174db7a19..5883fa5cfa 100644 --- a/docs/settings.md +++ b/docs/settings.md @@ -425,7 +425,7 @@ To turn off the engine sound use [setting](settings.md) `"EngineSound": false`. This allows you to specify your own vehicle pawn blueprints, for example, you can replace the default car in AirSim with your own car. Your vehicle BP can reside in Content folder of your own Unreal project (i.e. outside of AirSim plugin folder). For example, if you have a car BP located in file `Content\MyCar\MySedanBP.uasset` in your project then you can set `"DefaultCar": {"PawnBP":"Class'/Game/MyCar/MySedanBP.MySedanBP_C'"}`. The `XYZ.XYZ_C` is a special notation required to specify class for BP `XYZ`. Please note that your BP must be derived from CarPawn class. By default this is not the case but you can re-parent the BP using the "Class Settings" button in toolbar in UE editor after you open the BP and then choosing "Car Pawn" for Parent Class settings in Class Options. It is also a good idea to disable "Auto Possess Player" and "Auto Possess AI" as well as set AI Controller Class to None in BP details. Please make sure your asset is included for cooking in packaging options if you are creating binary. ### PhysicsEngineName -For cars, we support only PhysX for now (regardless of value in this setting). For multirotors, we support `"FastPhysicsEngine"` only. +For cars, we support only PhysX for now (regardless of value in this setting). For multirotors, we support `"FastPhysicsEngine"` and `"ExternalPhysicsEngine"`. `"ExternalPhysicsEngine"` allows the drone to be controlled via setVehiclePose (), keeping the drone in place until the next call. It is especially useful for moving the AirSim drone using an external simulator or on a saved path. ### LocalHostIp Setting Now when connecting to remote machines you may need to pick a specific Ethernet adapter to reach those machines, for example, it might be From 444d2ee1917d13149eaf980ad64d8a8ad9d06bbc Mon Sep 17 00:00:00 2001 From: Jonathan Date: Mon, 26 Apr 2021 19:19:30 -0300 Subject: [PATCH 3/5] Error with Compiler Warning C4266 fixed --- AirLib/include/physics/DebugPhysicsBody.hpp | 7 +++++++ 1 file changed, 7 insertions(+) diff --git a/AirLib/include/physics/DebugPhysicsBody.hpp b/AirLib/include/physics/DebugPhysicsBody.hpp index 58135807bd..4af6a55465 100644 --- a/AirLib/include/physics/DebugPhysicsBody.hpp +++ b/AirLib/include/physics/DebugPhysicsBody.hpp @@ -49,6 +49,13 @@ class DebugPhysicsBody : public PhysicsBody { std::cout << " ------------------------------------------------" << std::endl; } + virtual void updateKinematics() override + { + PhysicsBody::updateKinematics(); + } + + + virtual real_T getRestitution() const override { return restitution_; From 385db8594e46f7abb20792e125467245ccbce333 Mon Sep 17 00:00:00 2001 From: jonyMarino Date: Tue, 27 Apr 2021 16:32:43 -0300 Subject: [PATCH 4/5] fix Error packaging by adding header file --- .../Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp index 801ace3271..3b1f8d2c8c 100644 --- a/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp +++ b/Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/SimModeWorldMultiRotor.cpp @@ -14,6 +14,7 @@ #include "common/ClockFactory.hpp" #include #include "vehicles/multirotor/api/MultirotorRpcLibServer.hpp" +#include "common/SteppableClock.hpp" void ASimModeWorldMultiRotor::BeginPlay() @@ -137,4 +138,4 @@ msr::airlib::VehicleApiBase* ASimModeWorldMultiRotor::getVehicleApi(const PawnSi { const auto multirotor_sim_api = static_cast(sim_api); return multirotor_sim_api->getVehicleApi(); -} \ No newline at end of file +} From 8232efe85e9c9a2c6eb27ac154d7d6d513b33180 Mon Sep 17 00:00:00 2001 From: Jonathan Date: Fri, 30 Apr 2021 14:01:25 -0300 Subject: [PATCH 5/5] style changes --- AirLib/include/physics/DebugPhysicsBody.hpp | 3 +-- AirLib/include/physics/ExternalPhysicsEngine.hpp | 8 ++++++-- PythonClient/multirotor/external_physics_engine.py | 14 +++++++------- 3 files changed, 14 insertions(+), 11 deletions(-) diff --git a/AirLib/include/physics/DebugPhysicsBody.hpp b/AirLib/include/physics/DebugPhysicsBody.hpp index 4af6a55465..a81f53137d 100644 --- a/AirLib/include/physics/DebugPhysicsBody.hpp +++ b/AirLib/include/physics/DebugPhysicsBody.hpp @@ -54,12 +54,11 @@ class DebugPhysicsBody : public PhysicsBody { PhysicsBody::updateKinematics(); } - - virtual real_T getRestitution() const override { return restitution_; } + virtual real_T getFriction() const override { return friction_; diff --git a/AirLib/include/physics/ExternalPhysicsEngine.hpp b/AirLib/include/physics/ExternalPhysicsEngine.hpp index 609eee0815..e50f9f9517 100644 --- a/AirLib/include/physics/ExternalPhysicsEngine.hpp +++ b/AirLib/include/physics/ExternalPhysicsEngine.hpp @@ -14,7 +14,10 @@ #include "common/SteppableClock.hpp" #include -namespace msr { namespace airlib { +namespace msr +{ +namespace airlib +{ class ExternalPhysicsEngine : public PhysicsEngineBase { public: @@ -51,5 +54,6 @@ class ExternalPhysicsEngine : public PhysicsEngineBase { }; -}} //namespace +} //namespace +} //namespace #endif diff --git a/PythonClient/multirotor/external_physics_engine.py b/PythonClient/multirotor/external_physics_engine.py index 5a7e7ec072..efb050b64f 100644 --- a/PythonClient/multirotor/external_physics_engine.py +++ b/PythonClient/multirotor/external_physics_engine.py @@ -15,17 +15,17 @@ client = airsim.VehicleClient() client.confirmConnection() -pose1 = client.simGetVehiclePose() +pose = client.simGetVehiclePose() -pose1.orientation = airsim.to_quaternion(0.1, 0.1, 0.1) -client.simSetVehiclePose( pose1, False ) +pose.orientation = airsim.to_quaternion(0.1, 0.1, 0.1) +client.simSetVehiclePose( pose, False ) for i in range(900): print(i) - pose1 = client.simGetVehiclePose() - pose1.position = pose1.position + airsim.Vector3r(0.03, 0, 0) - pose1.orientation = pose1.orientation + airsim.to_quaternion(0.1, 0.1, 0.1) - client.simSetVehiclePose( pose1, False ) + pose = client.simGetVehiclePose() + pose.position = pose.position + airsim.Vector3r(0.03, 0, 0) + pose.orientation = pose.orientation + airsim.to_quaternion(0.1, 0.1, 0.1) + client.simSetVehiclePose( pose, False ) time.sleep(0.003) collision = client.simGetCollisionInfo() if collision.has_collided: