diff --git a/bmb_control/scripts/rqt_plot.sh b/bmb_control/scripts/rqt_plot.sh new file mode 100755 index 00000000..8fe806a0 --- /dev/null +++ b/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 diff --git a/bmb_description/urdf/aris.urdf.xacro b/bmb_description/urdf/aris.urdf.xacro index 610da6de..5ac8c7d4 100644 --- a/bmb_description/urdf/aris.urdf.xacro +++ b/bmb_description/urdf/aris.urdf.xacro @@ -68,9 +68,8 @@ - - + diff --git a/bmb_gazebo/src/ARISControlPlugin.cpp b/bmb_gazebo/src/ARISControlPlugin.cpp index 9bea8551..9fd24eca 100644 --- a/bmb_gazebo/src/ARISControlPlugin.cpp +++ b/bmb_gazebo/src/ARISControlPlugin.cpp @@ -21,7 +21,7 @@ using namespace gazebo; static const auto COM_OFFSET = - bmbToIgnitionVector3(Vector3{-0.12195, 0.00111, 0.06595}); + bmbToIgnitionVector3(Vector3{}); // -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{10})); + // base_link->SetLinearVel(bmbToIgnitionVector3(Vector3{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"); } diff --git a/bmb_world_model/src/AppliedLoads.cpp b/bmb_world_model/src/AppliedLoads.cpp index 0a9c2324..bd362111 100644 --- a/bmb_world_model/src/AppliedLoads.cpp +++ b/bmb_world_model/src/AppliedLoads.cpp @@ -11,24 +11,19 @@ #include // aerodynamic slope constants -static constexpr Wrench BODY_M_WRENCH{ - -0.320883207023485, 0, -1.39627650007691, 0, -0.250343385330279, 0}; -static constexpr Wrench AILERON_M_WRENCH{ - -0.0331320958688885, 0, - 0.131535586088028, 0.0547822725108043, - 0.0125388081950779, 0.00886500122201903}; +static constexpr Wrench BODY_M_WRENCH{0, 0, -1.39627650007691}; +static constexpr Wrench AILERON_M_WRENCH{0, 0, 0, 0.0547822725108043}; static constexpr Wrench ELEVATOR_M_WRENCH{ - -0.0238646243046933, 0, 0.0803509547078008, 0, 0.0842508724650606, 0}; -static constexpr Wrench RUDDER_M_WRENCH{0, 0, 0, - 0, 0, 0.000279918236359769}; + 0, 0, 0, 0, 0.0842508724650606, 0}; +static constexpr Wrench RUDDER_M_WRENCH{}; // aerodynamic offset constants -static constexpr Wrench BODY_B_WRENCH{ - -0.0595573697856826, 0, -0.204721453757253, 0, 0.0015675980775375, 0}; +static constexpr Wrench BODY_B_WRENCH{-0.0595573697856826, 0, + -0.204721453757253}; // propeller constants -static constexpr double THRUST_TORQUE_RATIO_PROPELLER = 1; -static constexpr Vector3 L_FRONT_PROPELLER{0, 0, 0}; +static constexpr double THRUST_TORQUE_RATIO_PROPELLER = 0; +static constexpr Vector3 L_FRONT_PROPELLER{}; using bmb_utilities::saturation;