Get moment of inertia from simulator

PX4 · Jun 25, 2024 · c233640 · c233640
1 parent ea8e9d1
commit c233640
Show file tree

Hide file tree

Showing 2 changed files with 9 additions and 9 deletions.
diff --git a/models/monocopter/monocopter.sdf.jinja b/models/monocopter/monocopter.sdf.jinja
@@ -226,16 +226,16 @@
       <child>gps::link</child>
       <parent>base_link</parent>
     </joint>
-    <!-- <plugin name="left_wing" filename="libLiftDragPlugin.so">
-      <a0>0.05984281113</a0>
+    <plugin name="left_wing" filename="libLiftDragPlugin.so">
+      <a0>0.0</a0>
       <cla>4.752798721</cla>
       <cda>0.6417112299</cda>
       <cma>0.0</cma>
       <alpha_stall>0.3391428111</alpha_stall>
       <cla_stall>-3.85</cla_stall>
       <cda_stall>-0.9233984055</cda_stall>
       <cma_stall>0</cma_stall>
-      <cp>-0.05 0.45 0.05</cp>
+      <cp>-0.05 0.25 0.05</cp>
       <area>0.6</area>
       <air_density>1.2041</air_density>
       <forward>1 0 0</forward>
@@ -248,13 +248,13 @@
       <control_joint_rad_to_cl>-0.3</control_joint_rad_to_cl>
       <robotNamespace></robotNamespace>
       <windSubTopic>world_wind</windSubTopic>
-    </plugin> -->
+    </plugin>
     <plugin name='dynamics' filename='libgazebo_monocopter_dyanmics_plugin.so'>
         <linkName>base_link</linkName>
-        <cp>-0.05 0.45 0.05</cp>
+        <!-- <cp>-0.05 0.45 0.05</cp>
         <cla>4.752798721</cla>
         <cda>0.6417112299</cda>
-        <cma>0.0</cma>
+        <cma>0.0</cma> -->
         <control_joint_name>
           left_elevon_joint
         </control_joint_name>

diff --git a/src/gazebo_monocopter_dynamics_plugin.cpp b/src/gazebo_monocopter_dynamics_plugin.cpp
@@ -114,10 +114,10 @@ void MonocopterDynamicsPlugin::OnUpdate(const common::UpdateInfo&){
     ignition::math::Vector3d linear_velocity = ignitionFromGazeboMath(link_->GetRelativeLinearVel());
     ignition::math::Vector3d angular_velocity = ignitionFromGazeboMath(link_->GetRelativeAngularVel());
   #endif
-  ///TODO: Get moment of inertia
-  ignition::math::Matrix3d I_;
+  /// Get moment of inertia
+  ignition::math::Matrix3d I_ = link_->GetInertial()->MOI();
 
-  ignition::math::Vector3d torque_coriolis = angular_velocity.Cross(I_ * angular_velocity);
+  ignition::math::Vector3d torque_coriolis = -angular_velocity.Cross(I_ * angular_velocity);
 
   ///TODO: Compute motor torque
   ignition::math::Vector3d torque_motor;