rotors_gazebo_plugins: fw_parameters.h Source File

Go to the documentation of this file.
00001 /*
00002  * Copyright 2017 Pavel Vechersky, ASL, ETH Zurich, Switzerland
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009 
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  */
00016 
00017 #ifndef ROTORS_GAZEBO_PLUGINS_FW_PARAMETERS_H_
00018 #define ROTORS_GAZEBO_PLUGINS_FW_PARAMETERS_H_
00019 
00020 #include <Eigen/Dense>
00021 #include <yaml-cpp/yaml.h>
00022 
00023 namespace gazebo {
00024 
00025 // Forward declaration.
00026 struct ControlSurface;
00027 
00028 // Default vehicle parameters (Techpod model)
00029 static constexpr double kDefaultWingSpan = 2.59;
00030 static constexpr double kDefaultWingSurface = 0.47;
00031 static constexpr double kDefaultChordLength = 0.18;
00032 static constexpr double kDefaultThrustInclination = 0.0;
00033 
00034 // Default aerodynamic parameter values (Techpod model)
00035 static constexpr double kDefaultAlphaMax = 0.27;
00036 static constexpr double kDefaultAlphaMin = -0.27;
00037 
00038 static const Eigen::Vector3d kDefaultCDragAlpha =
00039     Eigen::Vector3d(0.1360, -0.6737, 5.4546);
00040 static const Eigen::Vector3d kDefaultCDragBeta =
00041     Eigen::Vector3d(0.0195, 0.0, -0.3842);
00042 static const Eigen::Vector3d kDefaultCDragDeltaAil =
00043     Eigen::Vector3d(0.0195, 1.4205e-4, 7.5037e-6);
00044 static const Eigen::Vector3d kDefaultCDragDeltaFlp =
00045     Eigen::Vector3d(0.0195, 2.7395e-4, 1.23e-5);
00046 
00047 static const Eigen::Vector2d kDefaultCSideForceBeta =
00048     Eigen::Vector2d(0.0, -0.3073);
00049 
00050 static const Eigen::Vector4d kDefaultCLiftAlpha =
00051     Eigen::Vector4d(0.2127, 10.8060, -46.8324, 60.6017);
00052 static const Eigen::Vector2d kDefaultCLiftDeltaAil =
00053     Eigen::Vector2d(0.3304, 0.0048);
00054 static const Eigen::Vector2d kDefaultCLiftDeltaFlp =
00055     Eigen::Vector2d(0.3304, 0.0073);
00056 
00057 static const Eigen::Vector2d kDefaultCRollMomentBeta =
00058     Eigen::Vector2d(0.0, -0.0154);
00059 static const Eigen::Vector2d kDefaultCRollMomentP =
00060     Eigen::Vector2d(0.0, -0.1647);
00061 static const Eigen::Vector2d kDefaultCRollMomentR =
00062     Eigen::Vector2d(0.0, 0.0117);
00063 static const Eigen::Vector2d kDefaultCRollMomentDeltaAil =
00064     Eigen::Vector2d(0.0, 0.0570);
00065 static const Eigen::Vector2d kDefaultCRollMomentDeltaFlp =
00066     Eigen::Vector2d(0.0, 0.001);
00067 
00068 static const Eigen::Vector2d kDefaultCPitchMomentAlpha =
00069     Eigen::Vector2d(0.0435, -2.9690);
00070 static const Eigen::Vector2d kDefaultCPitchMomentQ =
00071     Eigen::Vector2d(-0.1173, -106.1541);
00072 static const Eigen::Vector2d kDefaultCPitchMomentDeltaElv =
00073     Eigen::Vector2d(-0.1173, -6.1308);
00074 
00075 static const Eigen::Vector2d kDefaultCYawMomentBeta =
00076     Eigen::Vector2d(0.0, 0.0430);
00077 static const Eigen::Vector2d kDefaultCYawMomentR =
00078     Eigen::Vector2d(0.0, -0.0827);
00079 static const Eigen::Vector2d kDefaultCYawMomentDeltaRud =
00080     Eigen::Vector2d(0.0, 0.06);
00081 
00082 static const Eigen::Vector3d kDefaultCThrust =
00083     Eigen::Vector3d(0.0, 14.7217, 0.0);
00084 
00085 // Default values for fixed-wing controls (Techpod model)
00086 static constexpr double kDefaultControlSurfaceDeflectionMin =
00087     -20.0 * M_PI / 180.0;
00088 static constexpr double kDefaultControlSurfaceDeflectionMax =
00089     20.0 * M_PI / 180.0;
00090 
00091 static constexpr int kDefaultAileronLeftChannel = 4;
00092 static constexpr int kDefaultAileronRightChannel = 0;
00093 static constexpr int kDefaultElevatorChannel = 1;
00094 static constexpr int kDefaultFlapChannel = 2;
00095 static constexpr int kDefaultRudderChannel = 3;
00096 static constexpr int kDefaultThrottleChannel = 5;
00097 
00100 inline void YAMLReadControlSurface(const YAML::Node& node,
00101                                    const std::string& name,
00102                                    ControlSurface& surface);
00103 
00106 template <typename Derived>
00107 inline void YAMLReadEigenVector(const YAML::Node& node,
00108                                 const std::string& name,
00109                                 Eigen::MatrixBase<Derived>& value);
00110 
00112 template <typename T>
00113 inline void YAMLReadParam(const YAML::Node& node,
00114                           const std::string& name,
00115                           T& value);
00116 
00118 #define READ_CONTROL_SURFACE(node, item) \
00119     YAMLReadControlSurface(node, #item, item);
00120 #define READ_EIGEN_VECTOR(node, item) YAMLReadEigenVector(node, #item, item);
00121 #define READ_PARAM(node, item) YAMLReadParam(node, #item, item);
00122 
00123 struct FWAerodynamicParameters {
00124   EIGEN_MAKE_ALIGNED_OPERATOR_NEW
00125 
00126   FWAerodynamicParameters()
00127       : alpha_max(kDefaultAlphaMax),
00128         alpha_min(kDefaultAlphaMin),
00129         c_drag_alpha(kDefaultCDragAlpha),
00130         c_drag_beta(kDefaultCDragBeta),
00131         c_drag_delta_ail(kDefaultCDragDeltaAil),
00132         c_drag_delta_flp(kDefaultCDragDeltaFlp),
00133         c_side_force_beta(kDefaultCSideForceBeta),
00134         c_lift_alpha(kDefaultCLiftAlpha),
00135         c_lift_delta_ail(kDefaultCLiftDeltaAil),
00136         c_lift_delta_flp(kDefaultCLiftDeltaFlp),
00137         c_roll_moment_beta(kDefaultCRollMomentBeta),
00138         c_roll_moment_p(kDefaultCRollMomentP),
00139         c_roll_moment_r(kDefaultCRollMomentR),
00140         c_roll_moment_delta_ail(kDefaultCRollMomentDeltaAil),
00141         c_roll_moment_delta_flp(kDefaultCRollMomentDeltaFlp),
00142         c_pitch_moment_alpha(kDefaultCPitchMomentAlpha),
00143         c_pitch_moment_q(kDefaultCPitchMomentQ),
00144         c_pitch_moment_delta_elv(kDefaultCPitchMomentDeltaElv),
00145         c_yaw_moment_beta(kDefaultCYawMomentBeta),
00146         c_yaw_moment_r(kDefaultCYawMomentR),
00147         c_yaw_moment_delta_rud(kDefaultCYawMomentDeltaRud),
00148         c_thrust(kDefaultCThrust) {}
00149 
00150   double alpha_max;
00151   double alpha_min;
00152 
00153   Eigen::Vector3d c_drag_alpha;
00154   Eigen::Vector3d c_drag_beta;
00155   Eigen::Vector3d c_drag_delta_ail;
00156   Eigen::Vector3d c_drag_delta_flp;
00157 
00158   Eigen::Vector2d c_side_force_beta;
00159 
00160   Eigen::Vector4d c_lift_alpha;
00161   Eigen::Vector2d c_lift_delta_ail;
00162   Eigen::Vector2d c_lift_delta_flp;
00163 
00164   Eigen::Vector2d c_roll_moment_beta;
00165   Eigen::Vector2d c_roll_moment_p;
00166   Eigen::Vector2d c_roll_moment_r;
00167   Eigen::Vector2d c_roll_moment_delta_ail;
00168   Eigen::Vector2d c_roll_moment_delta_flp;
00169 
00170   Eigen::Vector2d c_pitch_moment_alpha;
00171   Eigen::Vector2d c_pitch_moment_q;
00172   Eigen::Vector2d c_pitch_moment_delta_elv;
00173 
00174   Eigen::Vector2d c_yaw_moment_beta;
00175   Eigen::Vector2d c_yaw_moment_r;
00176   Eigen::Vector2d c_yaw_moment_delta_rud;
00177 
00178   Eigen::Vector3d c_thrust;
00179 
00180   void LoadAeroParamsYAML(const std::string& yaml_path) {
00181     const YAML::Node node = YAML::LoadFile(yaml_path);
00182 
00183     READ_PARAM(node, alpha_max);
00184     READ_PARAM(node, alpha_min);
00185 
00186     READ_EIGEN_VECTOR(node, c_drag_alpha);
00187     READ_EIGEN_VECTOR(node, c_drag_beta);
00188     READ_EIGEN_VECTOR(node, c_drag_delta_ail);
00189     READ_EIGEN_VECTOR(node, c_drag_delta_flp);
00190 
00191     READ_EIGEN_VECTOR(node, c_side_force_beta);
00192 
00193     READ_EIGEN_VECTOR(node, c_lift_alpha);
00194     READ_EIGEN_VECTOR(node, c_lift_delta_ail);
00195     READ_EIGEN_VECTOR(node, c_lift_delta_flp);
00196 
00197     READ_EIGEN_VECTOR(node, c_roll_moment_beta);
00198     READ_EIGEN_VECTOR(node, c_roll_moment_p);
00199     READ_EIGEN_VECTOR(node, c_roll_moment_r);
00200     READ_EIGEN_VECTOR(node, c_roll_moment_delta_ail);
00201     READ_EIGEN_VECTOR(node, c_roll_moment_delta_flp);
00202 
00203     READ_EIGEN_VECTOR(node, c_pitch_moment_alpha);
00204     READ_EIGEN_VECTOR(node, c_pitch_moment_q);
00205     READ_EIGEN_VECTOR(node, c_pitch_moment_delta_elv);
00206 
00207     READ_EIGEN_VECTOR(node, c_yaw_moment_beta);
00208     READ_EIGEN_VECTOR(node, c_yaw_moment_r);
00209     READ_EIGEN_VECTOR(node, c_yaw_moment_delta_rud);
00210 
00211     READ_EIGEN_VECTOR(node, c_thrust);
00212   }
00213 };
00214 
00215 struct ControlSurface {
00216   ControlSurface(int cs_channel,
00217                  double defl_min = kDefaultControlSurfaceDeflectionMin,
00218                  double defl_max = kDefaultControlSurfaceDeflectionMax)
00219       : channel(cs_channel),
00220         deflection_min(defl_min),
00221         deflection_max(defl_max) {}
00222 
00223   int channel;
00224 
00225   double deflection_min;
00226   double deflection_max;
00227 
00228   void LoadControlSurfaceNode(const YAML::Node& node) {
00229     READ_PARAM(node, channel);
00230     READ_PARAM(node, deflection_min);
00231     READ_PARAM(node, deflection_max);
00232   }
00233 };
00234 
00235 struct FWVehicleParameters {
00236   FWVehicleParameters()
00237       : wing_span(kDefaultWingSpan),
00238         wing_surface(kDefaultWingSurface),
00239         chord_length(kDefaultChordLength),
00240         thrust_inclination(kDefaultThrustInclination),
00241         throttle_channel(kDefaultThrottleChannel),
00242         aileron_left(kDefaultAileronLeftChannel),
00243         aileron_right(kDefaultAileronRightChannel),
00244         elevator(kDefaultElevatorChannel),
00245         flap(kDefaultFlapChannel),
00246         rudder(kDefaultRudderChannel) {}
00247 
00248   double wing_span;
00249   double wing_surface;
00250   double chord_length;
00251   double thrust_inclination;
00252 
00253   int throttle_channel;
00254 
00255   ControlSurface aileron_left;
00256   ControlSurface aileron_right;
00257   ControlSurface elevator;
00258   ControlSurface flap;
00259   ControlSurface rudder;
00260 
00261   void LoadVehicleParamsYAML(const std::string& yaml_path) {
00262     const YAML::Node node = YAML::LoadFile(yaml_path);
00263 
00264     READ_PARAM(node, wing_span);
00265     READ_PARAM(node, wing_surface);
00266     READ_PARAM(node, chord_length);
00267     READ_PARAM(node, thrust_inclination);
00268 
00269     READ_PARAM(node, throttle_channel);
00270 
00271     READ_CONTROL_SURFACE(node, aileron_left);
00272     READ_CONTROL_SURFACE(node, aileron_right);
00273     READ_CONTROL_SURFACE(node, elevator);
00274     READ_CONTROL_SURFACE(node, flap);
00275     READ_CONTROL_SURFACE(node, rudder);
00276   }
00277 };
00278 
00279 inline void YAMLReadControlSurface(const YAML::Node& node,
00280                                    const std::string& name,
00281                                    ControlSurface& surface) {
00282   const YAML::Node surface_node = node[name];
00283   surface.LoadControlSurfaceNode(surface_node);
00284 }
00285 
00286 template <typename Derived>
00287 inline void YAMLReadEigenVector(const YAML::Node& node,
00288                                 const std::string& name,
00289                                 Eigen::MatrixBase<Derived>& value) {
00290   std::vector<typename Derived::RealScalar> vec =
00291       node[name].as<std::vector<typename Derived::RealScalar>>();
00292   assert(vec.size() == Derived::SizeAtCompileTime);
00293   value = Eigen::Map<Derived>(&vec[0], vec.size());
00294 }
00295 
00296 template <typename T>
00297 inline void YAMLReadParam(const YAML::Node& node,
00298                           const std::string& name,
00299                           T& value) {
00300   value = node[name].as<T>();
00301 }
00302 
00303 }
00304 
00305 #endif /* ROTORS_GAZEBO_PLUGINS_FW_PARAMETERS_H_ */