Extend Multicoptor Control system to include nested model inertial params

src/systems/multicopter_control/MulticopterVelocityControl.cc

@@ -36,6 +36,7 @@
 #include "ignition/gazebo/components/Gravity.hh"
 #include "ignition/gazebo/components/Inertial.hh"
 #include "ignition/gazebo/components/Link.hh"
+#include "ignition/gazebo/components/Model.hh"
 #include "ignition/gazebo/components/ParentEntity.hh"
 #include "ignition/gazebo/components/World.hh"
 #include "ignition/gazebo/Link.hh"
@@ -92,20 +93,7 @@ void MulticopterVelocityControl::Configure(const Entity &_entity,
   VehicleParameters vehicleParams;
 
   math::Inertiald vehicleInertial;
-  // Compute the vehicle's moment of inertia and mass assuming that all the
-  // links in the model belong to the vehicle.
-  for (const Entity &link :
-       _ecm.ChildrenByComponents(this->model.Entity(), components::Link()))
-  {
-    auto inertial = _ecm.Component<components::Inertial>(link);
-    if (nullptr == inertial)
-    {
-      ignerr << "Could not find inertial component on on link "
-             << this->comLinkName << std::endl;
-      return;
-    }
-    vehicleInertial += inertial->Data();
-  }
+  vehicleInertial = this->VehicleInertial(_ecm, this->model.Entity());
 
   vehicleParams.mass = vehicleInertial.MassMatrix().Mass();
   vehicleParams.inertia = math::eigen3::convert(vehicleInertial.Moi());
@@ -318,6 +306,33 @@ void MulticopterVelocityControl::Configure(const Entity &_entity,
   this->initialized = true;
 }
 
+//////////////////////////////////////////////////
+math::Inertiald MulticopterVelocityControl::VehicleInertial(
+    const EntityComponentManager &_ecm, Entity _entity)
+{
+  math::Inertiald vehicleInertial;
+
+  for (const Entity &link :
+       _ecm.ChildrenByComponents(_entity, components::Link()))
+  {
+    auto inertial = _ecm.Component<components::Inertial>(link);
+    if (nullptr == inertial)
+    {
+      ignerr << "Could not find inertial component on link "
+             << this->comLinkName << std::endl;
+      return vehicleInertial;
+    }
+    vehicleInertial += inertial->Data();
+  }
+
+  for (const Entity &modelEnt :
+       _ecm.ChildrenByComponents(_entity, components::Model()))
+  {
+    vehicleInertial += this->VehicleInertial(_ecm, modelEnt);
+  }
+  return vehicleInertial;
+}
+
 //////////////////////////////////////////////////
 void MulticopterVelocityControl::PreUpdate(
     const ignition::gazebo::UpdateInfo &_info,

src/systems/multicopter_control/MulticopterVelocityControl.hh

@@ -185,6 +185,13 @@ namespace systems
                  ignition::gazebo::EntityComponentManager &_ecm,
                  const Eigen::VectorXd &_vels);
 
+    /// \brief Get the vehicle inertial from child links and nested models
+    /// \param[in] _ecm Immutable reference to the EntityComponentManager
+    /// \param[in] _entity Model entity to get inertial for
+    private: math::Inertiald VehicleInertial(
+                 const ignition::gazebo::EntityComponentManager &_ecm,
+                 Entity _entity);
+
     /// \brief Model interface
     private: Model model{kNullEntity};
 

test/integration/multicopter.cc

@@ -34,6 +34,7 @@
 #include "ignition/gazebo/components/Link.hh"
 #include "ignition/gazebo/components/Name.hh"
 #include "ignition/gazebo/components/Model.hh"
+#include "ignition/gazebo/components/Pose.hh"
 
 #include "ignition/gazebo/Model.hh"
 #include "ignition/gazebo/Server.hh"
@@ -317,3 +318,65 @@ TEST_F(MulticopterTest,
       });
   server->Run(true, 10, false);
 }
+
+/////////////////////////////////////////////////
+TEST_F(MulticopterTest,
+       IGN_UTILS_TEST_DISABLED_ON_WIN32(MulticopterVelocityControlNestedModel))
+{
+  // test that the drone is able to take off when carrying a payload
+  // (nexted model) with extra mass.
+
+  // Start server
+  auto server =
+      this->StartServer("/test/worlds/quadcopter_velocity_control_nested.sdf");
+
+  test::Relay testSystem;
+  transport::Node node;
+  auto cmdVel = node.Advertise<msgs::Twist>("/X3/gazebo/command/twist");
+
+  // Add the system
+  server->AddSystem(testSystem.systemPtr);
+  server->Run(true, 1, false);
+
+  // get pose of drone in post update
+  math::Pose3d x3Pose;
+  testSystem.OnPostUpdate(
+      [&](const gazebo::UpdateInfo &,
+          const gazebo::EntityComponentManager &_ecm)
+      {
+          auto x3Ent = _ecm.EntityByComponents(
+              components::Model(), components::Name("X3"));
+          ASSERT_NE(kNullEntity, x3Ent);
+
+          auto poseComp = _ecm.Component<components::Pose>(x3Ent);
+          if (poseComp)
+             x3Pose = poseComp->Data();
+      });
+
+  server->Run(true, 100, false);
+
+  // check initial z pos
+  double initialZ = x3Pose.Pos().Z();
+  EXPECT_GT(0.1, initialZ);
+
+  // run for a few interations and verify drone is still on the ground
+  server->Run(true, 100, false);
+  EXPECT_NEAR(initialZ, x3Pose.Pos().Z(), 1e-3);
+
+  // send linear z vel for drone to take off
+  msgs::Twist msg;
+  msgs::Set(msg.mutable_linear(), math::Vector3d(0, 0, 5));
+  cmdVel.Publish(msg);
+
+  // verify drone continues to fly higher over the duration of 1 second
+  double zHeight = x3Pose.Pos().Z();
+  for (unsigned int i = 0; i < 10; ++i)
+  {
+    server->Run(true, 100, false);
+    EXPECT_LT(zHeight, x3Pose.Pos().Z());
+    zHeight = x3Pose.Pos().Z();
+  }
+
+  // one last check to verify drone is at least 5 meters off the ground
+  EXPECT_LT(5.0, x3Pose.Pos().Z());
+}