Gazebo debug - DO NOT MERGE

bmb_control/scripts/rqt_plot.sh

@@ -0,0 +1 @@
+rqt_plot /aircraft_state/pose/position/z /aircraft_state/twist/linear/x:z /control_inputs/elevator_angle:propeller_force /state_command/pitch:speed

bmb_description/urdf/aris.urdf.xacro

@@ -68,9 +68,8 @@
         </visual>
         <inertial>
             <mass value="3.3"/>
-            <inertia ixx="12.327" ixy="-3.07" ixz="0.739"
-                     iyy="1.711" iyz="-2.789" izz="12.659"/>
-            <origin xyz="-0.12195 0.00111 0.06595" rpy="0 0 0"/>
+            <inertia ixx="12.327" ixy="0" ixz="0"
+                     iyy="1.711" iyz="0" izz="12.659"/>
         </inertial>
         <collision>
             <geometry>

bmb_gazebo/src/ARISControlPlugin.cpp

@@ -21,7 +21,7 @@
 using namespace gazebo;
 
 static const auto COM_OFFSET =
-    bmbToIgnitionVector3(Vector3<double>{-0.12195, 0.00111, 0.06595});
+    bmbToIgnitionVector3(Vector3<double>{}); // -0.12195, 0.00111, 0.06595});
 
 ARISControlPlugin::~ARISControlPlugin() {
 #if GAZEBO_MAJOR_VERSION >= 8
@@ -88,11 +88,11 @@ void ARISControlPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) {
       "control_inputs", 1, &ARISControlPlugin::controlInputsCallback, this);
 
   // initialize linear velocity to 10m/s
-  base_link->SetLinearVel(bmbToIgnitionVector3(Vector3<double>{10}));
+  // base_link->SetLinearVel(bmbToIgnitionVector3(Vector3<double>{10}));
 
   // set camera to follow the model
   // TODO: get this working
-  gui::Events::follow(_model->GetName());
+  // gui::Events::follow(_model->GetName());
 
   ROS_INFO("ARIS ready to fly. The force will be with you");
 }

bmb_world_model/src/AppliedLoads.cpp

@@ -11,24 +11,19 @@
 #include <cmath>
 
 // aerodynamic slope constants
-static constexpr Wrench<double> BODY_M_WRENCH{
-    -0.320883207023485, 0, -1.39627650007691, 0, -0.250343385330279, 0};
-static constexpr Wrench<double> AILERON_M_WRENCH{
-    -0.0331320958688885, 0,
-    0.131535586088028,   0.0547822725108043,
-    0.0125388081950779,  0.00886500122201903};
+static constexpr Wrench<double> BODY_M_WRENCH{0, 0, -1.39627650007691};
+static constexpr Wrench<double> AILERON_M_WRENCH{0, 0, 0, 0.0547822725108043};
 static constexpr Wrench<double> ELEVATOR_M_WRENCH{
-    -0.0238646243046933, 0, 0.0803509547078008, 0, 0.0842508724650606, 0};
-static constexpr Wrench<double> RUDDER_M_WRENCH{0, 0, 0,
-                                                0, 0, 0.000279918236359769};
+    0, 0, 0, 0, 0.0842508724650606, 0};
+static constexpr Wrench<double> RUDDER_M_WRENCH{};
 
 // aerodynamic offset constants
-static constexpr Wrench<double> BODY_B_WRENCH{
-    -0.0595573697856826, 0, -0.204721453757253, 0, 0.0015675980775375, 0};
+static constexpr Wrench<double> BODY_B_WRENCH{-0.0595573697856826, 0,
+                                              -0.204721453757253};
 
 // propeller constants
-static constexpr double THRUST_TORQUE_RATIO_PROPELLER = 1;
-static constexpr Vector3<double> L_FRONT_PROPELLER{0, 0, 0};
+static constexpr double THRUST_TORQUE_RATIO_PROPELLER = 0;
+static constexpr Vector3<double> L_FRONT_PROPELLER{};
 
 using bmb_utilities::saturation;