From 6186fc7e3412e0ea373c83cd6995c523895a7fa8 Mon Sep 17 00:00:00 2001
From: HarukiKozukapenguin <kozukozupenguin@g.ecc.u-tokyo.ac.jp>
Date: Fri, 29 Nov 2024 21:00:41 +0900
Subject: [PATCH] [aerial_model] print inertia when launching

---
 aerial_robot_model/src/model/base_model/robot_model.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/aerial_robot_model/src/model/base_model/robot_model.cpp b/aerial_robot_model/src/model/base_model/robot_model.cpp
index 880d6b04b..20eb8f488 100644
--- a/aerial_robot_model/src/model/base_model/robot_model.cpp
+++ b/aerial_robot_model/src/model/base_model/robot_model.cpp
@@ -363,6 +363,9 @@ namespace aerial_robot_model {
     ROS_INFO_STREAM_ONCE("[aerial_robot_model] robot mass is " << mass_);
 
     setInertia((cog.Inverse() * link_inertia).getRotationalInertia());
+    Eigen::Matrix3d print_inertia = getInertia<Eigen::Matrix3d>();
+    ROS_INFO_STREAM_ONCE("[aerial_robot_model] robot inertia is \n" << print_inertia);
+
     setCog2Baselink(cog.Inverse() * f_baselink);
 
     /* thrust point based on COG */