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(); + ROS_INFO_STREAM_ONCE("[aerial_robot_model] robot inertia is \n" << print_inertia); + setCog2Baselink(cog.Inverse() * f_baselink); /* thrust point based on COG */