added setSimSetObjectPose #1161

AirLib/include/api/RpcLibClientBase.hpp

@@ -37,7 +37,8 @@ class RpcLibClientBase {
     void simContinueForTime(double seconds);
 
     Pose simGetObjectPose(const std::string& object_name) const;
-
+    bool simSetObjectPose(const std::string& object_name, const Pose& pose, bool teleport = true);
+
     //task management APIs
     void cancelLastTask(const std::string& vehicle_name = "");
     virtual RpcLibClientBase* waitOnLastTask(bool* task_result = nullptr, float timeout_sec = Utils::nan<float>());

AirLib/include/api/WorldSimApiBase.hpp

@@ -26,6 +26,8 @@ class WorldSimApiBase {
         const std::string& message_param = "", unsigned char severity = 0) = 0;
 
     virtual Pose getObjectPose(const std::string& object_name) const = 0;
+    virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) = 0;
+
 };
 
 

AirLib/src/api/RpcLibClientBase.cpp

@@ -213,6 +213,10 @@ msr::airlib::Pose RpcLibClientBase::simGetObjectPose(const std::string& object_n
 {
     return pimpl_->client.call("simGetObjectPose", object_name).as<RpcLibAdapatorsBase::Pose>().to();
 }
+bool RpcLibClientBase::simSetObjectPose(const std::string& object_name, const msr::airlib::Pose& pose, bool teleport)
+{
+    return pimpl_->client.call("simSetObjectPose", object_name, RpcLibAdapatorsBase::Pose(pose), teleport).as<bool>();
+}
 
 CameraInfo RpcLibClientBase::simGetCameraInfo(const std::string& camera_name, const std::string& vehicle_name) const
 {

AirLib/src/api/RpcLibServerBase.cpp

@@ -148,6 +148,9 @@ RpcLibServerBase::RpcLibServerBase(ApiProvider* api_provider, const std::string&
         const auto& pose = getWorldSimApi()->getObjectPose(object_name); 
         return RpcLibAdapatorsBase::Pose(pose);
     });
+    pimpl_->server.bind("simSetObjectPose", [&](const std::string& object_name, const RpcLibAdapatorsBase::Pose& pose, bool teleport) -> bool {
+        return getWorldSimApi()->setObjectPose(object_name, pose.to(), teleport);
+    });
 
     pimpl_->server.bind("simGetGroundTruthKinematics", [&](const std::string& vehicle_name) -> RpcLibAdapatorsBase::KinematicsState {
         const Kinematics::State& result = *getVehicleSimApi(vehicle_name)->getGroundTruthKinematics();

PythonClient/airsim/client.py

@@ -104,6 +104,8 @@ def simGetVehiclePose(self, vehicle_name = ''):
     def simGetObjectPose(self, object_name):
         pose = self.client.call('simGetObjectPose', object_name)
         return Pose.from_msgpack(pose)
+    def simSetObjectPose(self, object_name, pose, teleport = True):
+        return self.client.call('simSetObjectPose', object_name, pose, teleport)
 
     def simSetSegmentationObjectID(self, mesh_name, object_id, is_name_regex = False):
         return self.client.call('simSetSegmentationObjectID', mesh_name, object_id, is_name_regex)

PythonClient/airsim/types.py

@@ -47,6 +47,9 @@ def __init__(self, x_val = np.float32(0), y_val = np.float32(0), z_val = np.floa
         self.y_val = y_val
         self.z_val = z_val
 
+    def __add__(self, other):
+        return Vector3r(self.x_val + other.x_val, self.y_val + other.y_val, self.z_val + other.z_val)
+
 
 class Quaternionr(MsgpackMixin):
     w_val = np.float32(0)

PythonClient/computer_vision/objects.py

@@ -9,9 +9,64 @@
 client = airsim.VehicleClient()
 client.confirmConnection()
 
-#Go to object in Unreal Editor, click on it and then look for Tags property. 
-#Add a tag "MyObject" (without quotes), save and the query using following line
-#see more about tags here: https://answers.unrealengine.com/questions/543807/whats-the-difference-between-tag-and-tag.html
-pose = client.simGetObjectPose("MyObject");
-print("Position: %s, Orientation: %s" % (pprint.pformat(pose.position),
-    pprint.pformat(pose.orientation)))
+# objects can be named in two ways:
+# 1. In UE Editor, select and object and change its name to something else. Note that you must *change* its name because
+#    default name is auto-generated and varies from run-to-run.
+# 2. OR you can do this: In UE Editor select the object and then go to "Actor" section, click down arrow to see "Tags" property and add a tag there.
+#
+# The simGetObjectPose and simSetObjectPose uses first object that has specified name OR tag.
+# more info: https://answers.unrealengine.com/questions/543807/whats-the-difference-between-tag-and-tag.html
+#            https://answers.unrealengine.com/revisions/790629.html
+
+# below works in Blocks environment
+
+#------------------------------------ Get current pose ------------------------------------------------
+
+# search object by name: 
+pose1 = client.simGetObjectPose("OrangeBall");
+print("OrangeBall - Position: %s, Orientation: %s" % (pprint.pformat(pose1.position),
+    pprint.pformat(pose1.orientation)))
+
+# search another object by tag
+pose2 = client.simGetObjectPose("PulsingCone");
+print("PulsingCone - Position: %s, Orientation: %s" % (pprint.pformat(pose2.position),
+    pprint.pformat(pose2.orientation)))
+
+# search non-existent object
+pose3 = client.simGetObjectPose("Non-Existent"); # should return nan pose
+print("Non-Existent - Position: %s, Orientation: %s" % (pprint.pformat(pose3.position),
+    pprint.pformat(pose3.orientation)))
+
+
+#------------------------------------ Set new pose ------------------------------------------------
+
+# here we move with teleport enabled so collisions are ignored
+pose1.position = pose1.position + airsim.Vector3r(-2, -2, -2)
+success = client.simSetObjectPose("OrangeBall", pose1, True);
+airsim.wait_key("OrangeBall moved. Success: %i" % (success))
+
+# here we move with teleport enabled so collisions are not ignored
+pose2.position = pose2.position + airsim.Vector3r(3, 3, -2)
+success = client.simSetObjectPose("PulsingCone", pose2, False);
+airsim.wait_key("PulsingCone moved. Success: %i" % (success))
+
+# move non-existent object
+success = client.simSetObjectPose("Non-Existent", pose2); # should return nan pose
+airsim.wait_key("Non-Existent moved. Success: %i" % (success))
+
+#------------------------------------ Get new pose ------------------------------------------------
+
+
+pose1 = client.simGetObjectPose("OrangeBall");
+print("OrangeBall - Position: %s, Orientation: %s" % (pprint.pformat(pose1.position),
+    pprint.pformat(pose1.orientation)))
+
+# search another object by tag
+pose2 = client.simGetObjectPose("PulsingCone");
+print("PulsingCone - Position: %s, Orientation: %s" % (pprint.pformat(pose2.position),
+    pprint.pformat(pose2.orientation)))
+
+# search non-existent object
+pose3 = client.simGetObjectPose("Non-Existent"); # should return nan pose
+print("Non-Existent - Position: %s, Orientation: %s" % (pprint.pformat(pose3.position),
+    pprint.pformat(pose3.orientation)))

PythonClient/computer_vision/setup_path.py

@@ -35,18 +35,18 @@ def getParentDir():
     @staticmethod
     def addAirSimModulePath():
         # if airsim module is installed then don't do anything else
-        import pkgutil
-        airsim_loader = pkgutil.find_loader('airsim')
-        if airsim_loader is not None:
-            return
-
-        grand_parent = SetupPath.getParentDir()
-        if grand_parent !=  '':
-            airsim_path = os.path.join(grand_parent, 'airsim')
+        #import pkgutil
+        #airsim_loader = pkgutil.find_loader('airsim')
+        #if airsim_loader is not None:
+        #    return
+
+        parent = SetupPath.getParentDir()
+        if parent !=  '':
+            airsim_path = os.path.join(parent, 'airsim')
             client_path = os.path.join(airsim_path, 'client.py')
             if os.path.exists(client_path):
-                sys.path.insert(0, grand_parent)
+                sys.path.insert(0, parent)
         else:
-            logging.warning("airsim module not found in grand parent folder. Using default installed module (if any).")
+            logging.warning("airsim module not found in parent folder. Using installed package (pip install airsim).")
 
 SetupPath.addAirSimModulePath()

PythonClient/multirotor/setup_path.py

@@ -35,18 +35,18 @@ def getParentDir():
     @staticmethod
     def addAirSimModulePath():
         # if airsim module is installed then don't do anything else
-        import pkgutil
-        airsim_loader = pkgutil.find_loader('airsim')
-        if airsim_loader is not None:
-            return
-
-        grand_parent = SetupPath.getParentDir()
-        if grand_parent !=  '':
-            airsim_path = os.path.join(grand_parent, 'airsim')
+        #import pkgutil
+        #airsim_loader = pkgutil.find_loader('airsim')
+        #if airsim_loader is not None:
+        #    return
+
+        parent = SetupPath.getParentDir()
+        if parent !=  '':
+            airsim_path = os.path.join(parent, 'airsim')
             client_path = os.path.join(airsim_path, 'client.py')
             if os.path.exists(client_path):
-                sys.path.insert(0, grand_parent)
+                sys.path.insert(0, parent)
         else:
-            logging.warning("airsim module not found in grand parent folder. Using default installed module (if any).")
+            logging.warning("airsim module not found in parent folder. Using installed package (pip install airsim).")
 
 SetupPath.addAirSimModulePath()

Unreal/Plugins/AirSim/Source/WorldSimApi.cpp

@@ -56,9 +56,27 @@ WorldSimApi::Pose WorldSimApi::getObjectPose(const std::string& object_name) con
     Pose result;
     UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &result]() {
         AActor* actor = UAirBlueprintLib::FindActor<AActor>(simmode_, FString(object_name.c_str()));
-        result = actor ? simmode_->getGlobalNedTransform().toLocalNed(actor->GetActorTransform())
+        result = actor ? simmode_->getGlobalNedTransform().toGlobalNed(FTransform(actor->GetActorRotation(), actor->GetActorLocation()))
             : Pose::nanPose();
     }, true);
     return result;
 }
 
+bool WorldSimApi::setObjectPose(const std::string& object_name, const WorldSimApi::Pose& pose, bool teleport)
+{
+    bool result;
+    UAirBlueprintLib::RunCommandOnGameThread([this, &object_name, &pose, teleport, &result]() {
+        FTransform actor_transform = simmode_->getGlobalNedTransform().fromGlobalNed(pose);
+        AActor* actor = UAirBlueprintLib::FindActor<AActor>(simmode_, FString(object_name.c_str()));
+        if (actor) {
+            if (teleport) 
+                result = actor->SetActorLocationAndRotation(actor_transform.GetLocation(), actor_transform.GetRotation(), false, nullptr, ETeleportType::TeleportPhysics);
+            else
+                result = actor->SetActorLocationAndRotation(actor_transform.GetLocation(), actor_transform.GetRotation(), true);
+        }
+        else
+            result = false;
+    }, true);
+    return result;
+}
+

Unreal/Plugins/AirSim/Source/WorldSimApi.h

@@ -25,6 +25,7 @@ class WorldSimApi : public msr::airlib::WorldSimApiBase {
         const std::string& message_param = "", unsigned char severity = 0) override;
 
     virtual Pose getObjectPose(const std::string& object_name) const override;
+    virtual bool setObjectPose(const std::string& object_name, const Pose& pose, bool teleport) override;
 
 private:
     ASimModeBase* simmode_;