Working but need to fix payload offset

Author: Jackie Chan

client/python/example_user_scripts/payload.py

@@ -8,14 +8,17 @@
 
 from projectairsim import ProjectAirSimClient, World, PayloadActor, Drone
 from projectairsim.utils import projectairsim_log
-from typing import List
+from projectairsim.image_utils import ImageDisplay
 
 
 # Async main function to wrap async drone commands
 async def main():
     # Create a Project AirSim client
     client = ProjectAirSimClient()
 
+    # Initialize an ImageDisplay object to display camera sub-windows
+    image_display = ImageDisplay()
+
     try:
         # Connect to simulation environment
         client.connect()
@@ -27,15 +30,126 @@ async def main():
         drone = Drone(client, world, "Drone1")
 
         # Create PayloadActor object
-        payload = PayloadActor(client, world, "Payload") # spawn like drone in config and watch it fall
+        payload = PayloadActor(
+            client, world, "Payload"
+        )  # spawn like drone in config and watch it fall
+
+        # ------------------------------------------------------------------------------
+
+        # Subscribe to chase camera sensor as a client-side pop-up window
+        chase_cam_window = "ChaseCam"
+        image_display.add_chase_cam(chase_cam_window)
+        client.subscribe(
+            drone.sensors["Chase"]["scene_camera"],
+            lambda _, chase: image_display.receive(chase, chase_cam_window),
+        )
+
+        # Subscribe to the downward-facing camera sensor's RGB and Depth images
+        rgb_name = "RGB-Image"
+        image_display.add_image(rgb_name, subwin_idx=0)
+        client.subscribe(
+            drone.sensors["DownCamera"]["scene_camera"],
+            lambda _, rgb: image_display.receive(rgb, rgb_name),
+        )
+
+        image_display.start()
+
+        # ------------------------------------------------------------------------------
+
+        # Set the drone to be ready to fly
+        drone.enable_api_control()
+        drone.arm()
+
+        # ------------------------------------------------------------------------------
+
+        projectairsim_log().info("takeoff_async: starting")
+        takeoff_task = await drone.takeoff_async()
+
+        await takeoff_task
+        projectairsim_log().info("takeoff_async: completed")
+
+        # 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
+        )
+        projectairsim_log().info("Move-Up invoked")
+
+        await move_up_task
+        projectairsim_log().info("Move-Up completed")
+
+        # Command the drone to move to the payload position
+        payload_pos = payload.get_kinematics()["pose"]["position"]
+
+        projectairsim_log().info("Moving to payload position")
+        move_to_payload_task = await drone.move_to_position_async(
+            north=payload_pos["x"], east=payload_pos["y"], down=-3, velocity=1.0
+        )
+        await move_to_payload_task
+        projectairsim_log().info("Moved to payload")
+
+        # Command the drone to attach payload
+        projectairsim_log().info("Attaching payload")
+        success = drone.attach_payload(payload.name)
+        projectairsim_log().info(f"Payload was successfully attached: {success}")
+
+        # Command the drone to move up in NED coordinate system at 1 m/s for 2 seconds
+        move_up_task = await drone.move_by_velocity_async(
+            v_north=0.0, v_east=0.0, v_down=-1.0, duration=2.0
+        )
+        projectairsim_log().info("Move-Up invoked")
+        await move_up_task
+        projectairsim_log().info("Move-Up completed")
+
+        # Command the drone to move north in NED coordinate system at 1 m/s for 2 seconds
+        move_north_task = await drone.move_by_velocity_async(
+            v_north=1.0, v_east=1.0, v_down=0.0, duration=4.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 1 m/s for 3.5 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")
+
+        # 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
+        move_north_task = await drone.move_by_velocity_async(
+            v_north=1.0, v_east=1.0, v_down=0.0, duration=1.0
+        )
+        projectairsim_log().info("Move-North invoked")
+        await move_north_task
+        projectairsim_log().info("Move-North completed")
+
+        projectairsim_log().info("land_async: starting")
+        land_task = await drone.land_async()
+        await land_task
+        projectairsim_log().info("land_async: completed")
+
+        # ------------------------------------------------------------------------------
+
+        # Shut down the drone
+        drone.disarm()
+        drone.disable_api_control()
 
+        # ------------------------------------------------------------------------------
 
+    # logs exception on the console
     except Exception as err:
         projectairsim_log().error(f"Exception occurred: {err}", exc_info=True)
 
     finally:
         # Always disconnect from the simulation environment to allow next connection
         client.disconnect()
+        image_display.stop()
 
 
 if __name__ == "__main__":

client/python/example_user_scripts/sim_config/payload_actor.jsonc

@@ -3,7 +3,7 @@
         {
           "name": "Frame",
           "inertial": {
-            "mass": 1.0,
+            "mass": 0.2,
             "inertia": {
               "type": "geometry",
               "geometry": {

client/python/example_user_scripts/sim_config/scene_payload_actor.jsonc

@@ -16,7 +16,7 @@
             "type": "payload_actor",
             "name": "Payload",
             "origin": {
-            "xyz": "1.0 8.0 -5.0",
+            "xyz": "5.0 8.0 -4.0",
             "rpy-deg": "0 0 0"
         },
         "payload-actor-config": "payload_actor.jsonc"

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

@@ -1527,7 +1527,8 @@ 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 to a position 0.1m below the drone.
+        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:
             payload_id (str): the id of the payload actor to attach
@@ -1541,4 +1542,19 @@ def attach_payload(self, payload_id: str) -> bool:
             "version": 1.0,
         }
         success = self.client.request(attach_payload_req)
+        return success
+
+    def detach_payload(self) -> bool:
+        """
+        Detaches the payload actor from the drone.
+
+        Returns:
+            bool: True if payload is detached successfully
+        """
+        detach_payload_req: Dict = {
+            "method": f"{self.world_parent_topic}/DetachPayloadActor",
+            "params": {"drone_name": self.name},
+            "version": 1.0,
+        }
+        success = self.client.request(detach_payload_req)
         return success

core_sim/include/core_sim/actor/payload_actor.hpp

@@ -48,6 +48,8 @@ class PayloadActor : public Actor {
   void UpdateKinematics(const Kinematics& kinematics);
   const Environment& GetEnvironment() const;
   void UpdateEnvironment();
+  const Pose& GetPoseOffset() const;
+  void SetPoseOffset(const Pose& offset);
 
   const CollisionInfo& GetCollisionInfo() const;
   void UpdateCollisionInfo(const CollisionInfo& collision_info);
@@ -60,6 +62,9 @@ class PayloadActor : public Actor {
 
   const Wrench& GetDragFaceWrench() const;
   void SetDragFaceWrench(const Wrench& drag_wrench);
+  const Matrix3x3& GetInertiaMatrix() const;
+  const float GetMass() const;
+  void SetMass(const float mass);
 
   void BeginUpdate();
   void EndUpdate();
@@ -76,6 +81,10 @@ class PayloadActor : public Actor {
 
   void Load(ConfigJson config_json) override;
 
+  const Kinematics& ComputeAttachedKinematics(
+      const Kinematics& kinematics) const;
+  void ComputeInertiaMatrix();
+
   class Impl;
   class Loader;
 };

core_sim/include/core_sim/actor/robot.hpp

@@ -90,6 +90,8 @@ class Robot : public Actor {
   void SetController(std::unique_ptr<IController> controller);
 
   void SetPayloadActor(PayloadActor& payload_actor);
+  PayloadActor* GetPayloadActor() const;
+  void ClearPayloadActor();
 
   void PublishRobotPose(const PoseMessage& pose);
 
@@ -155,8 +157,6 @@ class Robot : public Actor {
 
   void Load(ConfigJson config_json) override;
 
-  const Kinematics& ComputeAttachedKinematics() const;
-
   class Impl;
   class Loader;
 };