From cfabb51eedcb6447c3485e6c8cdd18423897c862 Mon Sep 17 00:00:00 2001 From: Tom van Dijk Date: Tue, 30 Jan 2018 16:39:18 +0100 Subject: [PATCH] [gazebo] Add actuator dynamics and bebop model (#2219) * 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 --- conf/airframes/examples/ardrone2_gazebo.xml | 4 +- .../examples/ardrone2_gazebo_cyberzoo.xml | 6 +- conf/airframes/examples/bebop.xml | 8 +- conf/airframes/examples/bebop2_indi.xml | 15 +- conf/modules/fdm_gazebo.xml | 94 ++++--- conf/simulator/gazebo/airframes/ardrone2.xml | 10 + conf/simulator/gazebo/airframes/bebop.xml | 12 + conf/simulator/gazebo/models/bebop/bebop.sdf | 264 ++++++++++++++++++ .../gazebo/models/bebop/model.config | 15 + sw/airborne/filters/high_pass_filter.h | 57 ++++ sw/simulator/nps/nps_fdm_gazebo.cpp | 53 +++- 11 files changed, 477 insertions(+), 61 deletions(-) create mode 100644 conf/simulator/gazebo/airframes/ardrone2.xml create mode 100644 conf/simulator/gazebo/airframes/bebop.xml create mode 100644 conf/simulator/gazebo/models/bebop/bebop.sdf create mode 100644 conf/simulator/gazebo/models/bebop/model.config diff --git a/conf/airframes/examples/ardrone2_gazebo.xml b/conf/airframes/examples/ardrone2_gazebo.xml index 3f17fa6942..74745dea83 100644 --- a/conf/airframes/examples/ardrone2_gazebo.xml +++ b/conf/airframes/examples/ardrone2_gazebo.xml @@ -191,12 +191,10 @@
- - -
+
diff --git a/conf/airframes/examples/ardrone2_gazebo_cyberzoo.xml b/conf/airframes/examples/ardrone2_gazebo_cyberzoo.xml index 4f48760e60..e4021fca7c 100644 --- a/conf/airframes/examples/ardrone2_gazebo_cyberzoo.xml +++ b/conf/airframes/examples/ardrone2_gazebo_cyberzoo.xml @@ -194,14 +194,10 @@
- - - - -
+
diff --git a/conf/airframes/examples/bebop.xml b/conf/airframes/examples/bebop.xml index ecc9c1d1e4..7c4546d892 100644 --- a/conf/airframes/examples/bebop.xml +++ b/conf/airframes/examples/bebop.xml @@ -33,9 +33,7 @@ - - - + @@ -53,8 +51,7 @@ - - + @@ -196,6 +193,7 @@
+
diff --git a/conf/airframes/examples/bebop2_indi.xml b/conf/airframes/examples/bebop2_indi.xml index eff63f1e44..1cc2e52e09 100644 --- a/conf/airframes/examples/bebop2_indi.xml +++ b/conf/airframes/examples/bebop2_indi.xml @@ -10,8 +10,8 @@ - - + + @@ -27,9 +27,7 @@ - - - + @@ -37,8 +35,8 @@ - - + + @@ -140,7 +138,7 @@ - + @@ -195,6 +193,7 @@
+
diff --git a/conf/modules/fdm_gazebo.xml b/conf/modules/fdm_gazebo.xml index 8b6a95064d..1ab82a68c6 100644 --- a/conf/modules/fdm_gazebo.xml +++ b/conf/modules/fdm_gazebo.xml @@ -50,50 +50,76 @@ <module name="fdm" type="gazebo"/> </target> @endcode - 2. Add actuator thrusts and torques to the SIMULATOR section: + 2. Include the gazebo defines for the vehicle: @code{.xml} <section name="SIMULATOR" prefix="NPS_"> - <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/> - <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/> - <define name="ACTUATOR_TORQUES" value="0.155, -0.155, 0.155, -0.155" type="float[]"/> - ... - <section> - @endcode - The thrusts and torques are expressed in SI units (N, Nm) and should - be in the same order as the ACTUATOR_NAMES. - 3. In the same section, bypass the AHRS and INS as these are not - supported yet: - @code{.xml} - <section name="SIMULATOR" prefix="NPS_"> - ... - <define name="BYPASS_AHRS" value="1"/> - <define name="BYPASS_INS" value="1"/> ... </section> + <include href="conf/simulator/gazebo/airframes/ardrone2.xml"/> @endcode - 4. If required, enable video thread simulation: - @code{.xml} - <section name="SIMULATOR" prefix="NPS_"> - ... - <define name="SIMULATE_VIDEO" value="1"/> - ... - </section> - @endcode - 5. If required, set the aircraft model in the airframe file: - @code{.xml} - <section name="SIMULATOR" prefix="NPS_"> - ... - <define name="GAZEBO_AC_NAME" value="my_uav"/> - </section> - @endcode - 6. Make sure all included modules work with nps. At the moment, most of + - If conf/simulator/gazebo/airframes does not contain an xml for the + vehicle model, it should be created with the following contents: + 1. Actuator thrusts and torques + @code{.xml} + <!DOCTYPE airframe SYSTEM "../../../airframes/airframe.dtd"> + + <airframe> + <section name="SIMULATOR" prefix="NPS_"> + <define name="ACTUATOR_THRUSTS" value="1.55, 1.55, 1.55, 1.55" type="float[]"/> + <define name="ACTUATOR_TORQUES" value="0.155, 0.155, 0.155, 0.155" type="float[]"/> + ... + </section> + </airframe> + @endcode + The thrusts and torques are expressed in SI units (N, Nm) and should + be in the same order as the ACTUATOR_NAMES defined in the airframe file. + The torque direction is determined automatically from the motor mixing. + 2. (Optional) Add actuator dynamics to the SIMULATOR section: + @code{.xml} + <section name="SIMULATOR" prefix="NPS_"> + ... + <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[]"/> + ... + </section> + @endcode + Actuator time constants can be provided without specifying the + actuator's maximum angular momentum. If the maximum angular momentum + is provided as well, it is used to calculate the rotor spin-up torque. + 3. In the same section, bypass the AHRS and INS as these are not + supported yet: + @code{.xml} + <section name="SIMULATOR" prefix="NPS_"> + ... + <define name="BYPASS_AHRS" value="1"/> + <define name="BYPASS_INS" value="1"/> + ... + </section> + @endcode + + 4. If required, enable video thread simulation: + @code{.xml} + <section name="SIMULATOR" prefix="NPS_"> + ... + <define name="SIMULATE_VIDEO" value="1"/> + ... + </section> + @endcode + 5. Set the aircraft model in the xml file: + @code{.xml} + <section name="SIMULATOR" prefix="NPS_"> + ... + <define name="GAZEBO_AC_NAME" value="my_uav"/> + </section> + @endcode + 3. Make sure all included modules work with nps. At the moment, most of the modules that depend on video_thread are only built when ap is selected as the target. To fix this, add nps to the target attribute in the module xml, e.g.: @code{.xml} <makefile target="ap|nps"> @endcode - 4. If required, set the simulation environment in the flight plan file: + 4. The simulation environment is set in the flight plan file: @code{.xml} <flight_plan ...> <header> @@ -101,7 +127,7 @@ #define NPS_GAZEBO_WORLD "my.world" </header> ... - <flight_plan> + </flight_plan> @endcode diff --git a/conf/simulator/gazebo/airframes/ardrone2.xml b/conf/simulator/gazebo/airframes/ardrone2.xml new file mode 100644 index 0000000000..f884a7d247 --- /dev/null +++ b/conf/simulator/gazebo/airframes/ardrone2.xml @@ -0,0 +1,10 @@ + + + +
+ + + + +
+ diff --git a/conf/simulator/gazebo/airframes/bebop.xml b/conf/simulator/gazebo/airframes/bebop.xml new file mode 100644 index 0000000000..a38082e168 --- /dev/null +++ b/conf/simulator/gazebo/airframes/bebop.xml @@ -0,0 +1,12 @@ + + + +
+ + + + + + +
+ diff --git a/conf/simulator/gazebo/models/bebop/bebop.sdf b/conf/simulator/gazebo/models/bebop/bebop.sdf new file mode 100644 index 0000000000..76130818a2 --- /dev/null +++ b/conf/simulator/gazebo/models/bebop/bebop.sdf @@ -0,0 +1,264 @@ + + + + 0 0 .1 0 0 0 + + + + 0.001 + + + + 0.38905 + + 0.000906 + 0.001242 + 0.002054 + 0. + 0.000014 + 0. + + + + + + + 0.4 0.4 0.05 + + + + + + + + 0.15 0.05 0.05 + + + + + + + collision + + + + + + + 0.077 0.095 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.05 + 0.02 + + + + 0.25 0.85 0.95 1 + 0.25 0.85 0.95 1 + + + + + + chassis + nw_motor + + + + -0.077 -0.095 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.05 + 0.02 + + + + 0.1 0.1 0.1 1 + 0.1 0.1 0.1 1 + + + + + + chassis + se_motor + + + + 0.077 -0.095 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.05 + 0.02 + + + + 0.25 0.85 0.95 1 + 0.25 0.85 0.95 1 + + + + + + chassis + ne_motor + + + + -0.077 0.095 0 0 0 0 + + 0.01 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + + + 0.05 + 0.02 + + + + 0.1 0.1 0.1 1 + 0.1 0.1 0.1 1 + + + + + + chassis + sw_motor + + + + + + + 0.15 0 0 0 0 0 + + 0.001 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + 30.0 + + 1.3962634 + + 1280 + 720 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + + + chassis + front_camera + + + + 0 0 -.03 0 1.57 0 + + 0.001 + + 0.0001 + 0.0001 + 0.0001 + 0 + 0 + 0 + + + + 30.0 + + 1.3962634 + + 320 + 240 + R8G8B8 + + + 0.02 + 300 + + + gaussian + + 0.0 + 0.007 + + + + + + + chassis + bottom_camera + + + diff --git a/conf/simulator/gazebo/models/bebop/model.config b/conf/simulator/gazebo/models/bebop/model.config new file mode 100644 index 0000000000..b2302efbdb --- /dev/null +++ b/conf/simulator/gazebo/models/bebop/model.config @@ -0,0 +1,15 @@ + + + Bebop (Paparazzi) + 1.0 + bebop.sdf + + + Tom van Dijk + tomvand@users.noreply.github.com + + + + Simple Bebop model for use with Paparazzi's NPS (http://wiki.paparazziuav.org). + + diff --git a/sw/airborne/filters/high_pass_filter.h b/sw/airborne/filters/high_pass_filter.h index cd04d37e5b..01af146492 100644 --- a/sw/airborne/filters/high_pass_filter.h +++ b/sw/airborne/filters/high_pass_filter.h @@ -30,6 +30,63 @@ #include "std.h" #include "math/pprz_algebra_int.h" +/** First order high pass filter structure. + * + * using bilinear z transform + */ +struct FirstOrderHighPass +{ + float time_const; + float last_in; + float last_out; +}; + +/** Init first order high pass filter. + * + * Laplace transform in continuous time: + * tau*s + * H(s) = --------- + * 1 + tau*s + * + * @param filter first order high pass filter structure + * @param tau time constant of the first order high pass filter + * @param sample_time sampling period of the signal + * @param value initial value of the filter + */ +static inline void init_first_order_high_pass(struct FirstOrderHighPass *filter, + float tau, float sample_time, float value) +{ + filter->last_in = value; + filter->last_out = value; + filter->time_const = 2.0f * tau / sample_time; +} + +/** Update first order high pass filter state with a new value. + * + * @param filter first order high pass filter structure + * @param value new input value of the filter + * @return new filtered value + */ +static inline float update_first_order_high_pass( + struct FirstOrderHighPass *filter, float value) +{ + float out = (filter->time_const * (value - filter->last_in) + + (filter->time_const - 1.0f) * filter->last_out) + / (1.0f + filter->time_const); + filter->last_in = value; + filter->last_out = out; + return out; +} + +/** Get current value of the first order high pass filter. + * + * @param filter first order high pass filter structure + * @return current value of the filter + */ +static inline float get_first_order_high_pass(struct FirstOrderHighPass *filter) +{ + return filter->last_out; +} /** Fourth order filter structure. * diff --git a/sw/simulator/nps/nps_fdm_gazebo.cpp b/sw/simulator/nps/nps_fdm_gazebo.cpp index fae20da7c8..8581e31ab7 100644 --- a/sw/simulator/nps/nps_fdm_gazebo.cpp +++ b/sw/simulator/nps/nps_fdm_gazebo.cpp @@ -56,6 +56,8 @@ extern "C" { #include "math/pprz_isa.h" #include "math/pprz_algebra_double.h" +#include "filters/low_pass_filter.h" +#include "filters/high_pass_filter.h" #include "subsystems/actuators/motor_mixing_types.h" } @@ -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]; + double max_ang_momentum[NPS_COMMANDS_NB]; }; -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, { }, { }, { } }; #if NPS_SIMULATE_LASER_RANGE_ARRAY extern "C" { @@ -208,7 +213,7 @@ inline struct DoubleVect3 to_pprz_ltp(gazebo::math::Vector3 xyz) // External functions, interface with Paparazzi's NPS as declared in nps_fdm.h /** - * Set JSBsim specific fields that are not used for Gazebo. + * Initialize actuator dynamics, set unused fields in fdm * @param dt */ void nps_fdm_init(double dt) @@ -216,6 +221,22 @@ void nps_fdm_init(double dt) fdm.init_dt = dt; // JSBsim specific fdm.curr_dt = dt; // JSBsim specific fdm.nan_count = 0; // JSBsim specific + +#ifdef NPS_ACTUATOR_TIME_CONSTANTS + // Set up low-pass filter to simulate delayed actuator response + const float tau[NPS_COMMANDS_NB] = NPS_ACTUATOR_TIME_CONSTANTS; + for(int i=0; i 0.05 ? u : 0.05)) * udot; + torque += spinup_torque; +#endif + + // Apply force and torque to gazebo model gazebo::physics::LinkPtr link = model->GetLink(gazebo_actuators.names[i]); link->AddRelativeForce(gazebo::math::Vector3(0, 0, thrust)); link->AddRelativeTorque(gazebo::math::Vector3(0, 0, torque));