Merge pull request #4066 from jonyMarino/pr/set_kinematics_for_external_physics

add simSetKinematics API

Author: Jonathan

AirLib/include/api/RpcLibClientBase.hpp

@@ -128,6 +128,7 @@ namespace airlib
 
         bool simCreateVoxelGrid(const Vector3r& position, const int& x_size, const int& y_size, const int& z_size, const float& res, const std::string& output_file);
         msr::airlib::Kinematics::State simGetGroundTruthKinematics(const std::string& vehicle_name = "") const;
+        void simSetKinematics(const Kinematics::State& state, bool ignore_collision, const std::string& vehicle_name = "");
         msr::airlib::Environment::State simGetGroundTruthEnvironment(const std::string& vehicle_name = "") const;
         std::vector<std::string> simSwapTextures(const std::string& tags, int tex_id = 0, int component_id = 0, int material_id = 0);
 

AirLib/include/api/VehicleSimApiBase.hpp

@@ -54,6 +54,7 @@ namespace airlib
         virtual Pose getPose() const = 0;
         virtual void setPose(const Pose& pose, bool ignore_collision) = 0;
         virtual const Kinematics::State* getGroundTruthKinematics() const = 0;
+        virtual void setKinematics(const Kinematics::State& state, bool ignore_collision) = 0;
         virtual const msr::airlib::Environment* getGroundTruthEnvironment() const = 0;
 
         virtual CollisionInfo getCollisionInfo() const = 0;

AirLib/src/api/RpcLibClientBase.cpp

@@ -246,6 +246,11 @@ __pragma(warning(disable : 4239))
             pimpl_->client.call("simSetVehiclePose", RpcLibAdaptorsBase::Pose(pose), ignore_collision, vehicle_name);
         }
 
+        void RpcLibClientBase::simSetKinematics(const Kinematics::State& state, bool ignore_collision, const std::string& vehicle_name)
+        {
+            pimpl_->client.call("simSetKinematics", RpcLibAdaptorsBase::KinematicsState(state), ignore_collision, vehicle_name);
+        }
+
         void RpcLibClientBase::simSetTraceLine(const std::vector<float>& color_rgba, float thickness, const std::string& vehicle_name)
         {
             pimpl_->client.call("simSetTraceLine", color_rgba, thickness, vehicle_name);

AirLib/src/api/RpcLibServerBase.cpp

@@ -381,6 +381,10 @@ namespace airlib
             return RpcLibAdaptorsBase::KinematicsState(result);
         });
 
+        pimpl_->server.bind("simSetKinematics", [&](const RpcLibAdaptorsBase::KinematicsState& state, bool ignore_collision, const std::string& vehicle_name) {
+            getVehicleSimApi(vehicle_name)->setKinematics(state.to(), ignore_collision);
+        });
+
         pimpl_->server.bind("simGetGroundTruthEnvironment", [&](const std::string& vehicle_name) -> RpcLibAdaptorsBase::EnvironmentState {
             const Environment::State& result = (*getVehicleSimApi(vehicle_name)->getGroundTruthEnvironment()).getState();
             return RpcLibAdaptorsBase::EnvironmentState(result);

PythonClient/airsim/client.py

@@ -683,6 +683,19 @@ def simGetGroundTruthKinematics(self, vehicle_name = ''):
         return KinematicsState.from_msgpack(kinematics_state)
     simGetGroundTruthKinematics.__annotations__ = {'return': KinematicsState}
 
+    def simSetKinematics(self, state, ignore_collision, vehicle_name = ''):
+        """
+        Set the kinematics state of the vehicle
+
+        If you don't want to change position (or orientation) then just set components of position (or orientation) to floating point nan values
+
+        Args:
+            state (KinematicsState): Desired Pose pf the vehicle
+            ignore_collision (bool): Whether to ignore any collision or not
+            vehicle_name (str, optional): Name of the vehicle to move
+        """
+        self.client.call('simSetKinematics', state, ignore_collision, vehicle_name)
+
     def simGetGroundTruthEnvironment(self, vehicle_name = ''):
         """
         Get ground truth environment state

Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.cpp

@@ -226,6 +226,12 @@ const msr::airlib::Kinematics::State* PawnSimApi::getGroundTruthKinematics() con
     return &kinematics_->getState();
 }
 
+void PawnSimApi::setKinematics(const Kinematics::State& state, bool ignore_collision)
+{
+    unused(ignore_collision);
+    return kinematics_->setState(state);
+}
+
 const msr::airlib::Environment* PawnSimApi::getGroundTruthEnvironment() const
 {
     return environment_.get();

Unity/AirLibWrapper/AirsimWrapper/Source/PawnSimApi.h

@@ -85,6 +85,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
     virtual void updateRenderedState(float dt) override;
     virtual void updateRendering(float dt) override;
     virtual const msr::airlib::Kinematics::State* getGroundTruthKinematics() const override;
+    virtual void setKinematics(const Kinematics::State& state, bool ignore_collision) override;
     virtual const msr::airlib::Environment* getGroundTruthEnvironment() const override;
     virtual std::string getRecordFileLine(bool is_header_line) const override;
     virtual void reportState(msr::airlib::StateReporter& reporter) override;

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp

@@ -125,6 +125,14 @@ void MultirotorPawnSimApi::setPose(const Pose& pose, bool ignore_collision)
     pending_pose_status_ = PendingPoseStatus::RenderStatePending;
 }
 
+void MultirotorPawnSimApi::setKinematics(const msr::airlib::Kinematics::State& state, bool ignore_collision)
+{
+    PawnSimApi::setKinematics(state, ignore_collision);
+
+    msr::airlib::Pose pose(state.pose.position, state.pose.orientation);
+    setPose(pose, ignore_collision);
+}
+
 //*** Start: UpdatableState implementation ***//
 void MultirotorPawnSimApi::resetImplementation()
 {

Unity/AirLibWrapper/AirsimWrapper/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h

@@ -30,6 +30,7 @@ class MultirotorPawnSimApi : public PawnSimApi
     virtual void reportState(StateReporter& reporter) override;
     virtual UpdatableObject* getPhysicsBody() override;
     virtual void setPose(const Pose& pose, bool ignore_collision) override;
+    virtual void setKinematics(const msr::airlib::Kinematics::State& state, bool ignore_collision) override;
 
     msr::airlib::MultirotorApiBase* getVehicleApi() const
     {

Unity/build.cmd

@@ -1,3 +1,4 @@
+@echo off
 REM //---------- copy binaries and include for MavLinkCom in deps ----------
 msbuild AirLibWrapper\AirsimWrapper.sln  /target:Clean /target:Build  /property:Configuration=Release /property:Platform=x64
 if ERRORLEVEL 1 goto :buildfailed

Unreal/Plugins/AirSim/Source/PawnSimApi.cpp

@@ -528,6 +528,13 @@ const msr::airlib::Kinematics::State* PawnSimApi::getGroundTruthKinematics() con
 {
     return &kinematics_->getState();
 }
+
+void PawnSimApi::setKinematics(const Kinematics::State& state, bool ignore_collision)
+{
+    unused(ignore_collision);
+
+    return kinematics_->setState(state);
+}
 const msr::airlib::Environment* PawnSimApi::getGroundTruthEnvironment() const
 {
     return environment_.get();

Unreal/Plugins/AirSim/Source/PawnSimApi.h

@@ -37,6 +37,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
     typedef msr::airlib::AirSimSettings::VehicleSetting VehicleSetting;
     typedef msr::airlib::ImageCaptureBase ImageCaptureBase;
     typedef msr::airlib::DetectionInfo DetectionInfo;
+    typedef msr::airlib::Kinematics Kinematics;
 
     struct Params
     {
@@ -92,6 +93,7 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
     virtual void updateRenderedState(float dt) override;
     virtual void updateRendering(float dt) override;
     virtual const msr::airlib::Kinematics::State* getGroundTruthKinematics() const override;
+    virtual void setKinematics(const msr::airlib::Kinematics::State& state, bool ignore_collision) override;
     virtual const msr::airlib::Environment* getGroundTruthEnvironment() const override;
     virtual std::string getRecordFileLine(bool is_header_line) const override;
     virtual void reportState(msr::airlib::StateReporter& reporter) override;
@@ -149,7 +151,6 @@ class PawnSimApi : public msr::airlib::VehicleSimApiBase
 
 private: //vars
     typedef msr::airlib::AirSimSettings AirSimSettings;
-    typedef msr::airlib::Kinematics Kinematics;
     typedef msr::airlib::Environment Environment;
 
     Params params_;

Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.cpp

@@ -139,6 +139,16 @@ void MultirotorPawnSimApi::setPose(const Pose& pose, bool ignore_collision)
     pending_pose_status_ = PendingPoseStatus::RenderPending;
 }
 
+void MultirotorPawnSimApi::setKinematics(const Kinematics::State& state, bool ignore_collision)
+{
+    multirotor_physics_body_->lock();
+    multirotor_physics_body_->updateKinematics(state);
+    multirotor_physics_body_->setGrounded(false);
+    multirotor_physics_body_->unlock();
+    pending_pose_collisions_ = ignore_collision;
+    pending_pose_status_ = PendingPoseStatus::RenderPending;
+}
+
 //*** Start: UpdatableState implementation ***//
 void MultirotorPawnSimApi::resetImplementation()
 {

Unreal/Plugins/AirSim/Source/Vehicles/Multirotor/MultirotorPawnSimApi.h

@@ -43,6 +43,7 @@ class MultirotorPawnSimApi : public PawnSimApi
     virtual UpdatableObject* getPhysicsBody() override;
 
     virtual void setPose(const Pose& pose, bool ignore_collision) override;
+    virtual void setKinematics(const Kinematics::State& state, bool ignore_collision) override;
     virtual void pawnTick(float dt) override;
 
     msr::airlib::MultirotorApiBase* getVehicleApi() const