WIP Update dynamics plugin

PX4 · Jul 3, 2024 · d11439e · d11439e
1 parent 78df22b
commit d11439e
Show file tree

Hide file tree

Showing 2 changed files with 21 additions and 28 deletions.
diff --git a/models/monocopter/monocopter.sdf.jinja b/models/monocopter/monocopter.sdf.jinja
@@ -8,20 +8,21 @@
         <pose>0 0 0 0 0 0</pose>
         <mass>1.5</mass>
         <inertia>
-          <ixx>0.197563</ixx>
-          <ixy>0</ixy>
-          <ixz>0</ixz>
-          <iyy>0.1458929</iyy>
-          <iyz>0</iyz>
-          <izz>0.1477</izz>
+          <ixx>0.014186812.85</ixx>
+          <ixy>0.00224544875</ixy>
+          <ixz>-0.061034406</ixz>
+          <iyy>0.00108135429</iyy>
+          <iyz>-0.00011539117</iyz>
+          <izz>0.01517241974</izz>
         </inertia>
       </inertial>
       <collision name='base_link_collision'>
         <pose>0 0 -0.07 0 0 0</pose>
         <geometry>
-          <box>
-            <size>0.47 0.47 0.11</size>
-          </box>
+          <cylinder>
+            <length>0.1</length>
+            <radius>0.5</radius>
+          </cylinder>
         </geometry>
         <surface>
           <contact>
@@ -235,8 +236,8 @@
       <cla_stall>-3.85</cla_stall>
       <cda_stall>-0.9233984055</cda_stall>
       <cma_stall>0</cma_stall>
-      <cp>0.0 0.0 0.0</cp>
-      <area>0.6</area>
+      <cp>-0.05 0.0 0.0</cp>
+      <area>0.02</area>
       <air_density>1.2041</air_density>
       <forward>1 0 0</forward>
       <upward>0 0 1</upward>
@@ -249,16 +250,9 @@
       <robotNamespace></robotNamespace>
       <windSubTopic>world_wind</windSubTopic>
     </plugin>
-    <plugin name='dynamics' filename='libgazebo_monocopter_dyanmics_plugin.so'>
-        <linkName>base_link</linkName>
-        <!-- <cp>-0.05 0.45 0.05</cp>
-        <cla>4.752798721</cla>
-        <cda>0.6417112299</cda>
-        <cma>0.0</cma> -->
-        <control_joint_name>
-          left_elevon_joint
-        </control_joint_name>
-        <control_joint_rad_to_cl>-0.3</control_joint_rad_to_cl>
+    <plugin name='dynamics' filename='libgazebo_monocopter_dynamics_plugin.so'>
+      <robotNamespace></robotNamespace>
+      <link_name>base_link</link_name>
     </plugin>
     <plugin name='puller' filename='libgazebo_motor_model.so'>
       <robotNamespace></robotNamespace>

diff --git a/src/gazebo_monocopter_dynamics_plugin.cpp b/src/gazebo_monocopter_dynamics_plugin.cpp
@@ -80,10 +80,7 @@ void MonocopterDynamicsPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _s
   if (_sdf->HasElement("motorNumber"))
     motor_number_ = _sdf->GetElement("motorNumber")->Get<int>();
   else
-    gzerr << "[gazebo_catapult_plugin] Please specify a motorNumber.\n";
-
-  getSdfParam<double>(_sdf, "force", force_magnitude_, force_magnitude_);
-  getSdfParam<double>(_sdf, "duration", launch_duration_, launch_duration_);
+    gzerr << "[gazebo_monocopter_dynamics_plugin] Please specify a motorNumber.\n";
 
   // Listen to the update event. This event is broadcast every simulation iteration.
   _updateConnection = event::Events::ConnectWorldUpdateBegin(boost::bind(&MonocopterDynamicsPlugin::OnUpdate, this, _1));
@@ -102,7 +99,7 @@ void MonocopterDynamicsPlugin::OnUpdate(const common::UpdateInfo&){
   ///TODO: Compute force induced by aerodynamics
   ignition::math::Vector3d force_aerodynamic;
   total_force = force_motor + force_aerodynamic;
-  this->link_->AddRelativeForce(total_force);
+  // this->link_->AddRelativeForce(total_force);
 
   /// Compute moments
   ignition::math::Vector3d total_torque;
@@ -117,14 +114,16 @@ void MonocopterDynamicsPlugin::OnUpdate(const common::UpdateInfo&){
   /// 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);
+  std::cout << "angular velocity: " << angular_velocity.X() << ", " << angular_velocity.Y() << ", " << angular_velocity.Z() << std::endl;
+  std::cout << " - torque_coriolis: " << torque_coriolis.X() << ", " << torque_coriolis.Y() << ", " << torque_coriolis.Z() << std::endl;
 
   ///TODO: Compute motor torque
   ignition::math::Vector3d torque_motor;
   ///TODO: Compute moment induced by aerodynamics
   ignition::math::Vector3d torque_aerodynamic;
 
-  total_torque = torque_coriolis + torque_motor + torque_aerodynamic;
+  total_torque = torque_motor - torque_coriolis;
   this->link_->AddRelativeTorque(total_torque);
 }