[gazebo] Add actuator dynamics and bebop model #2219

tomvand · 2018-01-23T16:27:51Z

New:

Added actuator dynamics (low-pass filter) and spin-up torques to gazebo fdm
Added a simple bebop model and two example airframe files (not added to conf since they should be drop-in replacements for the existing jsbsim airframes). The dynamics of the model should be more-or-less correct, the camera will be tweaked in a later PR.

Fixed:

Actuator torques now get their value from NPS_ACTUATOR_TORQUES instead of _THRUSTS
Updated documentation

Flies ok with PID control Does not fly ok with INDI, the required G1 R is 0.159 instead of 0.0022 (estimated by flying in adaptive mode). The other parameters are close to those specified in the airframe file.

Fixed two bugs: 1. gazebo_actuators.torques was set to NPS_ACTUATOR_THRUSTS 2. spinup torque direction is now also controlled by motor mixing

gautierhattenberger · 2018-01-23T20:51:02Z

conf/airframes/examples/bebop2_indi_gazebo.xml

+    <module name="ins" type="extended"/>
+  </firmware>
+
+  <modules main_freq="512">


Can you move the modules from this node to the firmware node ? This section is supposed to be removed in future version.

Sure, fixed.

kirkscheper

Most of it looks good, I am just not ok with there being separate airframes for gazebo and jbsim for every vehicle. Having the ardrone one was to highlight an example of using gazebo but don't think we need one for every airframe. It is possible to have the defines in the airframe and then select the simulation by changing the line in the nps target...

Additionally, I don't think that the actuator dynamics should be defined in the airframe. I don't think those values should change with the particular airframe but with the vehicle type. Like this, it will result in lots of frivolous copy pasting.

In the case that you don't want the low and high pass values to be used, you can have a define like NOS_NO_ACTUATOR_DYNAMICS or something which will disable them. Additionally, airframes where the dynamics are not defined can have some default time constants which 'disable' the filters like NPS_ACTUATOR_TIME_CONSTANTS = {dt/2, dt/2, dt/2, dt/2} (some constant for whatever the dt is...).

#lessmaintenance

kirkscheper · 2018-01-24T09:38:41Z

conf/airframes/examples/bebop2_indi_gazebo.xml

@@ -0,0 +1,215 @@
+<!DOCTYPE airframe SYSTEM "../airframe.dtd">
+
+<airframe name="bebop2_indi">


Is it really necessary to have a separate airframe just for gazebo? I still think that this is a dangerous trend, we will end up with duplicate files for all airframes.

Nope. The gazebo airframes were provided only as an example, so it's fine by me to copy the necessary defines back to the original airframes, delete the gazebo ones and let the user change the fdm as you suggest.

kirkscheper · 2018-01-24T09:39:25Z

conf/airframes/examples/bebop_gazebo.xml

+
+<airframe name="bebop">
+
+  <firmware name="rotorcraft">


See what I mean...

kirkscheper · 2018-01-24T09:44:02Z

conf/airframes/examples/bebop_gazebo.xml

+    <define name="ACTUATOR_THRUSTS" value="2.09, 2.09, 2.09, 2.09" type="float[]"/>
+    <define name="ACTUATOR_TORQUES" value="0.0250, 0.0250, 0.0250, 0.0250" type="float[]"/>
+    <define name="ACTUATOR_TIME_CONSTANTS" value="0.02, 0.02, 0.02, 0.02" type="float[]"/>
+    <define name="ACTUATOR_MAX_ANGULAR_MOMENTUM" value="0.19, 0.19, 0.19, 0.19" type="float[]"/>


Can't these be added to the sdf model instead. I would image that they are vehicle specific and not airframe specific. Not sure if you want these to be easily configurable.

Alternatively, they could be settings in a header file stored with the simulation model which will be included in the gazebo file.

Agreed, the sdf might be a better place for this. Since the sdf is already read at the start of the simulation, I'll see if I can get these values from there instead

SDF does not accept elements that are not defined in the spec, instead it shows a warning and the element is not parsed.

Instead, I have now moved the gazebo defines to xml files in conf/simulator/gazebo/airframes that should be included from the main airframe file. Let me know what you think.

kirkscheper · 2018-01-24T09:46:56Z

sw/airborne/filters/high_pass_filter.h

+}
+
+/** Update first order high pass filter state with a new value.
+ *


@param descriptors?

kirkscheper · 2018-01-24T09:50:35Z

sw/simulator/nps/nps_fdm_gazebo.cpp

@@ -102,10 +104,13 @@ struct gazebo_actuators_t {
  string names[NPS_COMMANDS_NB];
  double thrusts[NPS_COMMANDS_NB];
  double torques[NPS_COMMANDS_NB];
+  struct FirstOrderLowPass lowpass[NPS_COMMANDS_NB];
+  struct FirstOrderHighPass highpass[NPS_COMMANDS_NB];
+  float max_ang_momentum[NPS_COMMANDS_NB];


float or double?

kirkscheper · 2018-01-24T09:50:47Z

sw/simulator/nps/nps_fdm_gazebo.cpp

@@ -375,6 +396,7 @@ static void init_gazebo(void)

  for (uint8_t i = 0; i < NPS_COMMANDS_NB; i++) {
    gazebo_actuators.torques[i] = -fabs(gazebo_actuators.torques[i]) * yaw_coef[i] / fabs(yaw_coef[i]);
+    gazebo_actuators.max_ang_momentum[i] = -fabs(gazebo_actuators.max_ang_momentum[i]) * yaw_coef[i] / fabs(yaw_coef[i]);


if float you need fabsf

kirkscheper · 2018-01-24T09:52:32Z

sw/simulator/nps/nps_fdm_gazebo.cpp

+    double udot = update_first_order_high_pass(&gazebo_actuators.highpass[i],
+        sp);
+    double spinup_torque = gazebo_actuators.max_ang_momentum[i] /
+    (2 * sqrtf(u > 0.05 ? u : 0.05)) * udot;


indentation

kirkscheper · 2018-01-24T09:52:49Z

sw/simulator/nps/nps_fdm_gazebo.cpp

+#ifdef NPS_ACTUATOR_MAX_ANGULAR_MOMENTUM
+    // Spinup torque
+    double udot = update_first_order_high_pass(&gazebo_actuators.highpass[i],
+        sp);


This sp can fit on the line above :p

Try telling eclipse that :P

Should prevent duplicate actuator defines spread accross all airframe files.

Moved to firmware targets so it will be disabled in gazebo. Is this actually working in jsbsim??

tomvand · 2018-01-29T10:27:49Z

Minor issues are fixed now. Moved the gazebo-specific defines to shared .xml files in conf/simulator/gazebo/airframes. I've removed the new _gazebo aiframes but left the existing ardrone2_gazebo.xml and ardrone2_gazebo_cyberzoo.xml in place for now, let me know if these should be removed as well.

kirkscheper · 2018-01-29T10:29:42Z

conf/airframes/examples/bebop2_indi.xml

@@ -140,7 +146,7 @@
    <define name="G1_R" value="0.0022"/>

    <!-- For the bebop2 we need to filter the roll rate due to the dampers -->
-    <define name="FILTER_ROLL_RATE" value="TRUE"/>
+    <!-- FILTER_ROLL_RATE moved to firmware targets -->


Why has this been moved to nps target only? Not sure if that's a good idea. Why does this have to be turned off for Gazebo?

Setting FILTER_ROLL_RATE to TRUE causes oscillations around the roll axis. I thought this was a gazebo problem, but I've just found out this also happens in jsbsim so it might actually be a problem with this filter instead.

Could you please try to replicate this as follows:

Start a flight with the examples/bebop2_indi airframe and rotorcraft_basic flight plan (from master). The flight will be simulated using jsbsim, not gazebo.

Follow the flight plan until the standby block.

Check the PFD in GCS, does the drone oscillate around the roll axis?

Disable the ROLL_FILTER, does the drone remain stable now?

@EwoudSmeur I guess you implemented the jbsim indi stuff, can you check this?

Discussed with @EwoudSmeur , enabling the roll filter could indeed cause these oscillations. Use of the filter on the real bebop2 is also unnecessary when the dampers are locked (which I've been told is usually the case). How about setting the roll rate filter to off by default, with a comment hinting at its use when flying with unlocked dampers (see latest commit)?

kirkscheper · 2018-01-29T10:32:10Z

conf/simulator/gazebo/airframes/bebop.xml

+    <define name="ACTUATOR_TIME_CONSTANTS" value="0.02, 0.02, 0.02, 0.02" type="float[]"/>
+    <define name="ACTUATOR_MAX_ANGULAR_MOMENTUM" value="0.19, 0.19, 0.19, 0.19" type="float[]"/>
+    <define name="GAZEBO_AC_NAME" value="bebop" type="string"/>
+    <define name="BYPASS_AHRS" value="1"/>


is it required to bypass the ahrs? If not I would leave this out of the default settings.

Yes, AHRS is not supported yet. I tried removing the bypass, but this results in an incorrect heading estimate.

Ok, just make sure to update this once it is working.

kirkscheper · 2018-01-29T10:33:15Z

conf/simulator/gazebo/airframes/bebop.xml

+    <define name="GAZEBO_AC_NAME" value="bebop" type="string"/>
+    <define name="BYPASS_AHRS" value="1"/>
+  </section>
+</airframe>


you are missing some line endings in these files. Add a new line a the end of the file. You can also set this in Eclipse to be done automatically.

kirkscheper · 2018-01-29T10:36:54Z

sw/simulator/nps/nps_fdm_gazebo.cpp

+    // Spinup torque
+    double udot = update_first_order_high_pass(&gazebo_actuators.highpass[i], sp);
+    double spinup_torque = gazebo_actuators.max_ang_momentum[i] /
+        (2 * sqrtf(u > 0.05 ? u : 0.05)) * udot;


should use sqrt for doubles, not sqrtf. Also can you augment 2 * to 2. * (saves a background conversion)

kirkscheper · 2018-01-29T10:40:04Z

sw/simulator/nps/nps_fdm_gazebo.cpp

-struct gazebo_actuators_t gazebo_actuators = {NPS_ACTUATOR_NAMES, NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_THRUSTS};
-
+struct gazebo_actuators_t gazebo_actuators = { NPS_ACTUATOR_NAMES,
+    NPS_ACTUATOR_THRUSTS, NPS_ACTUATOR_TORQUES, { }, { }, { } };


Can you write { 0 } here to explicitly initialise the arrays? I guess it already works like this but since you only set these values inside an ifdef I think it would be better to explicitly show the default values here.

No, this causes a warning that not all fields of the filters are initialized.

Caution: causes oscillations around roll axis in gazebo

* Add gazebo model for Parrot Bebop 1 * Add first-order high pass filter * First implementation of actuator dynamics and spinup torque Flies ok with PID control Does not fly ok with INDI, the required G1 R is 0.159 instead of 0.0022 (estimated by flying in adaptive mode). The other parameters are close to those specified in the airframe file. * FIX incorrect yaw behavior Fixed two bugs: 1. gazebo_actuators.torques was set to NPS_ACTUATOR_THRUSTS 2. spinup torque direction is now also controlled by motor mixing * Clean up debug code and file logger * Clean up gazebo example airframes * Minor cleanup * FIX warnings about missing initializer in fdm_gazebo * Update documentation * Remove modules section, move to firmware * Minor fixes to fdm_gazebo and high_pass_filter * Move actuator dynamics to included airframe files Should prevent duplicate actuator defines spread accross all airframe files. * Fix FILTER_ROLL_RATE in bebop2_indi airframe Moved to firmware targets so it will be disabled in gazebo. Is this actually working in jsbsim?? * Update documentation * Fix newlines * Minor fixes in nps_fdm_gazebo.cpp * Re-enable filter roll rate Caution: causes oscillations around roll axis in gazebo * Disable roll filter and cleanup

tomvand added 9 commits January 18, 2018 15:43

Add gazebo model for Parrot Bebop 1

758c53b

Add first-order high pass filter

3add91c

First implementation of actuator dynamics and spinup torque

2b861c9

Flies ok with PID control Does not fly ok with INDI, the required G1 R is 0.159 instead of 0.0022 (estimated by flying in adaptive mode). The other parameters are close to those specified in the airframe file.

FIX incorrect yaw behavior

ab3a43f

Fixed two bugs: 1. gazebo_actuators.torques was set to NPS_ACTUATOR_THRUSTS 2. spinup torque direction is now also controlled by motor mixing

Clean up debug code and file logger

48c2737

Clean up gazebo example airframes

017f6e0

Minor cleanup

32cde54

FIX warnings about missing initializer in fdm_gazebo

c7740bc

Update documentation

47efa88

gautierhattenberger reviewed Jan 23, 2018

View reviewed changes

Remove modules section, move to firmware

3bd4b71

kirkscheper requested changes Jan 24, 2018

View reviewed changes

tomvand added 4 commits January 25, 2018 16:18

Minor fixes to fdm_gazebo and high_pass_filter

d8647a9

Move actuator dynamics to included airframe files

b47a9a1

Should prevent duplicate actuator defines spread accross all airframe files.

Fix FILTER_ROLL_RATE in bebop2_indi airframe

e8e2c08

Moved to firmware targets so it will be disabled in gazebo. Is this actually working in jsbsim??

Update documentation

dcb1c21

kirkscheper reviewed Jan 29, 2018

View reviewed changes

tomvand added 4 commits January 29, 2018 11:57

Fix newlines

942c735

Minor fixes in nps_fdm_gazebo.cpp

a6157a1

Re-enable filter roll rate

21b8da5

Caution: causes oscillations around roll axis in gazebo

Disable roll filter and cleanup

05d4ab3

kirkscheper approved these changes Jan 30, 2018

View reviewed changes

kirkscheper merged commit cfabb51 into paparazzi:master Jan 30, 2018

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

[gazebo] Add actuator dynamics and bebop model #2219

[gazebo] Add actuator dynamics and bebop model #2219

tomvand commented Jan 23, 2018

gautierhattenberger Jan 23, 2018

tomvand Jan 24, 2018

kirkscheper left a comment

kirkscheper Jan 24, 2018

tomvand Jan 24, 2018

kirkscheper Jan 24, 2018

kirkscheper Jan 24, 2018

tomvand Jan 24, 2018

tomvand Jan 29, 2018

kirkscheper Jan 24, 2018

kirkscheper Jan 24, 2018

kirkscheper Jan 24, 2018

kirkscheper Jan 24, 2018

kirkscheper Jan 24, 2018

tomvand Jan 24, 2018

tomvand commented Jan 29, 2018

kirkscheper Jan 29, 2018

tomvand Jan 29, 2018

kirkscheper Jan 29, 2018

tomvand Jan 30, 2018

kirkscheper Jan 29, 2018

tomvand Jan 29, 2018

kirkscheper Jan 29, 2018

kirkscheper Jan 29, 2018

kirkscheper Jan 29, 2018

kirkscheper Jan 29, 2018

tomvand Jan 29, 2018

		@@ -0,0 +1,215 @@
		<!DOCTYPE airframe SYSTEM "../airframe.dtd">

		<airframe name="bebop2_indi">

[gazebo] Add actuator dynamics and bebop model #2219

[gazebo] Add actuator dynamics and bebop model #2219

Conversation

tomvand commented Jan 23, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

kirkscheper left a comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

tomvand commented Jan 29, 2018

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment

Choose a reason for hiding this comment