Add payload lowering

Author: Jackie Chan

client/python/example_user_scripts/payload.py

@@ -70,7 +70,7 @@ async def main():
 
         # Command the drone to move up in NED coordinate system at 1 m/s for 3 seconds
         move_up_task = await drone.move_by_velocity_async(
-            v_north=0.0, v_east=0.0, v_down=-1.0, duration=3.0
+            v_north=0.0, v_east=0.0, v_down=-1.0, duration=2.0
         )
         projectairsim_log().info("Move-Up invoked")
 
@@ -108,27 +108,42 @@ async def main():
         await move_north_task
         projectairsim_log().info("Move-North completed")
 
-        # Command the drone to move down in NED coordinate system at 1 m/s for 3.5 seconds
+        # Command the drone to move down in NED coordinate system at 1 m/s for 3.0 seconds
         move_down_task = await drone.move_by_velocity_async(
             v_north=0.0, v_east=0.0, v_down=1.0, duration=3.0
         )
         projectairsim_log().info("Move-Down invoked")
         await move_down_task
         projectairsim_log().info("Move-Down completed")
 
+        await asyncio.sleep(1)
+
+        # Command the drone to lower payload
+        projectairsim_log().info("Lowering payload")
+        success = drone.lower_payload(15)
+        projectairsim_log().info(f"Payload was successfully lowered: {success}")
+
         # Command the drone to detach payload
         projectairsim_log().info("Detaching payload")
         success = drone.detach_payload()
         projectairsim_log().info(f"Payload was successfully detached: {success}")
 
-        # Command the drone to move north in NED coordinate system at 1 m/s for 1 second
+        # Command the drone to move north-east in NED coordinate system at 1 m/s for 1 second
         move_north_task = await drone.move_by_velocity_async(
-            v_north=1.0, v_east=1.0, v_down=0.0, duration=1.0
+            v_north=1.0, v_east=1.0, v_down=0.0, duration=2.0
         )
         projectairsim_log().info("Move-North invoked")
         await move_north_task
         projectairsim_log().info("Move-North completed")
 
+        # Command the drone to move down in NED coordinate system at 2 m/s for 3 seconds
+        move_down_task = await drone.move_by_velocity_async(
+            v_north=0.0, v_east=0.0, v_down=2.0, duration=2.0
+        )
+        projectairsim_log().info("Move-Down invoked")
+        await move_down_task
+        projectairsim_log().info("Move-Down completed")
+
         projectairsim_log().info("land_async: starting")
         land_task = await drone.land_async()
         await land_task

client/python/example_user_scripts/sim_config/payload_actor.jsonc

@@ -29,7 +29,8 @@
           "visual": {
             "geometry": {
               "type": "unreal_mesh",
-              "name": "/Drone/Quadrotor1"
+              "name": "/Engine/BasicShapes/Cube",
+              "scale": "0.5 1.0 0.5"
             }
           }
         }

client/python/projectairsim/src/projectairsim/drone.py

@@ -831,7 +831,12 @@ async def rotate_to_yaw_async(
             asyncio.Task: An awaitable task wrapping the async coroutine
         """
 
-        params: Dict = {"yaw": yaw, "timeout_sec": timeout_sec, "margin": margin, "yaw_rate": yaw_rate}
+        params: Dict = {
+            "yaw": yaw,
+            "timeout_sec": timeout_sec,
+            "margin": margin,
+            "yaw_rate": yaw_rate,
+        }
 
         # print("params: {}".format(params))
         req: Dict = {
@@ -1527,7 +1532,7 @@ def set_external_force(self, ext_force: List[float]) -> bool:
 
     def attach_payload(self, payload_id: str) -> bool:
         """
-        Attach a payload actor in the scene below the drone. The position offset between 
+        Attach a payload actor in the scene below the drone. The position offset between
         the drone and payload at the time of this call will be saved.
 
         Args:
@@ -1557,4 +1562,29 @@ def detach_payload(self) -> bool:
             "version": 1.0,
         }
         success = self.client.request(detach_payload_req)
-        return success
+        return success
+
+    def lower_payload(self, dz: float, v_down: float = 1.0) -> bool:
+        """
+        Lowers payload towards the ground. Assumes payload is already attached
+        and user will call detach_payload() subsequently.
+
+        TODO: make it work with raising payloads too
+
+        Args:
+            dz (float): how much to lower the payload by from the resting
+                        position under the drone (dz >= 0). If dz exceeds the
+                        distance from the resting position to the ground, the payload
+                        will be lowered until it makes contact with the ground.
+            v_down (float): how fast to lower the payload by (v_down > 0)
+
+        Returns:
+            bool: True if payload is lowered successfully
+        """
+        lower_payload_req: Dict = {
+            "method": f"{self.world_parent_topic}/LowerPayloadActor",
+            "params": {"drone_name": self.name, "dz": dz, "vz": v_down},
+            "version": 1.0,
+        }
+        success = self.client.request(lower_payload_req)
+        return success

client/python/projectairsim/src/projectairsim/payload_actor.py

@@ -7,6 +7,7 @@
 from projectairsim.utils import projectairsim_log
 from typing import Dict
 
+
 class PayloadActor(object):
     def __init__(self, client: ProjectAirSimClient, world: World, name: str):
         """ProjectAirSim Payload Actor Interface

core_sim/include/core_sim/scene.hpp

@@ -117,6 +117,9 @@ class Scene {
       const std::vector<float>& vel_x_lin, const std::vector<float>& vel_y_lin,
       const std::vector<float>& vel_z_lin);
 
+  bool LowerPayloadActorServiceMethod(const std::string& drone_name,
+                                      const float dz, const float vz);
+
   void RegisterServiceMethod(const ServiceMethod& method,
                              MethodHandler method_handler);
 

core_sim/src/actor/payload_actor.cpp

@@ -153,6 +153,10 @@ void PayloadActor::SetPoseOffset(const Pose& offset) {
   static_cast<PayloadActor::Impl*>(pimpl_.get())->SetPoseOffset(offset);
 }
 
+const Pose& PayloadActor::GetPoseOffset() const {
+  return static_cast<PayloadActor::Impl*>(pimpl_.get())->GetPoseOffset();
+}
+
 void PayloadActor::UpdateKinematics(const Kinematics& kinematics) {
   static_cast<PayloadActor::Impl*>(pimpl_.get())->UpdateKinematics(kinematics);
 }
@@ -313,9 +317,8 @@ const Kinematics& PayloadActor::Impl::ComputeAttachedKinematics(
                   kinematics.pose.position.y() + payload_offset_.position.y(),
                   kinematics.pose.position.z() + payload_offset_.position.z());
 
-  return Kinematics(
-      Pose(new_pos, kinematics.pose.orientation * payload_offset_.orientation),
-      kinematics.twist, kinematics.accels);
+  return Kinematics(Pose(new_pos, payload_offset_.orientation),
+                    kinematics.twist, kinematics.accels);
 }
 
 const bool PayloadActor::Impl::InAttachedState() const {
@@ -336,9 +339,14 @@ void PayloadActor::Impl::UpdateEnvironment() {
 }
 
 void PayloadActor::Impl::SetPoseOffset(const Pose& offset) {
+  std::lock_guard<std::mutex> lock(update_lock_);
   payload_offset_ = offset;
 }
 
+const Pose& PayloadActor::Impl::GetPoseOffset() const {
+  return payload_offset_;
+}
+
 bool PayloadActor::Impl::GetStartLanded() const { return start_landed_; }
 
 void PayloadActor::Impl::SetStartLanded(bool start_landed) {

core_sim/src/scene.cpp

@@ -179,6 +179,9 @@ class Scene::Impl : public ComponentWithTopicsAndServiceMethods {
   Vector3 GetWindVelocity();
   void UpdateWindVelocity();
 
+  bool LowerPayloadActorServiceMethod(const std::string& drone_name,
+                                      const float dz, const float vz);
+
   std::string CallBackAfter(int t_secs);
 
   void RegisterServiceMethod(const ServiceMethod& method,
@@ -380,6 +383,11 @@ const Vector3 Scene::GetWindVelocity() const {
   return pimpl_->GetWindVelocity();
 }
 
+bool Scene::LowerPayloadActorServiceMethod(const std::string& drone_name,
+                                           const float dz, const float vz) {
+  return pimpl_->LowerPayloadActorServiceMethod(drone_name, dz, vz);
+}
+
 void Scene::AddNEDTrajectory(
     const std::string& traj_name, const std::vector<float>& time,
     const std::vector<float>& pose_x, const std::vector<float>& pose_y,
@@ -505,6 +513,44 @@ void Scene::Impl::UpdateWindVelocity() {
   }
 }
 
+bool Scene::Impl::LowerPayloadActorServiceMethod(const std::string& drone_name,
+                                                 const float dz,
+                                                 const float vz) {
+  TimeSec start = SimClock::Get()->NowSimSec();
+  TimeSec curr_time = start;
+  TimeSec timeout = 180;  // 3 minutes seems reasonable
+
+  auto& actors = GetActors();
+  int actor_idx = GetActorIndex(drone_name);
+  auto& actor_ref = actors[actor_idx];
+  auto& actor = static_cast<Robot&>(actor_ref.get());
+  PayloadActor* payload_actor = actor.GetPayloadActor();
+
+  if (!payload_actor) {
+    logger_.LogError(name_, "Drone '%hs' is not attached to a PayloadActor.",
+                     drone_name.c_str());
+    throw Error("Drone is not attached to a PayloadActor.");
+  } else {
+    Vector3 payload_pos = payload_actor->GetPoseOffset().position;
+    float z = payload_pos.z();
+    float z_final = z + dz;
+
+    while (z < z_final) {
+      auto payload_collision_info = payload_actor->GetCollisionInfo();
+      if (payload_collision_info.has_collided) break;
+
+      z = z + vz * (SimClock::Get()->NowSimSec() - curr_time);
+      Vector3 new_pos(payload_pos.x(), payload_pos.y(), z);
+      payload_actor->SetPoseOffset(
+          Pose(new_pos, payload_actor->GetPoseOffset().orientation));
+
+      if (curr_time - start > timeout) return false;
+      curr_time = SimClock::Get()->NowSimSec();
+    }  // TODO: split into robot.cpp?
+  }
+  return true;
+}
+
 void Scene::Impl::SetCallbackPhysicsStart(
     const std::function<void()>& callback) {
   physics_start_callback_ = callback;
@@ -917,6 +963,12 @@ void Scene::Impl::RegisterServiceMethods() {
   auto get_wind_vel_handler =
       get_wind_vel.CreateMethodHandler(&Scene::Impl::GetWindVelocity, *this);
   RegisterServiceMethod(get_wind_vel, get_wind_vel_handler);
+
+  auto lower_payload_actor =
+      ServiceMethod("LowerPayloadActor", {"drone_name", "dz", "vz"});
+  auto lower_payload_actor_handler = lower_payload_actor.CreateMethodHandler(
+      &Scene::Impl::LowerPayloadActorServiceMethod, *this);
+  RegisterServiceMethod(lower_payload_actor, lower_payload_actor_handler);
 }
 
 void Scene::Impl::UnregisterAllServiceMethods() {