From 95de6166a21a9f5572a9d0097c093bc40d5c854d Mon Sep 17 00:00:00 2001 From: Jaeyoung-Lim Date: Thu, 22 Jul 2021 14:20:49 +0200 Subject: [PATCH] Add style checks Add stylecheck in github actions --- .github/workflows/check_style.yml | 29 ++ Makefile | 2 + Tools/check_code_format.sh | 10 + Tools/fix_code_style.sh | 21 ++ include/aero_parameters.h | 258 ++++++++---------- include/data_driven_dynamics_plugin.h | 9 +- include/parametric_dynamics_model.h | 65 ++--- src/data_driven_dynamics_plugin.cpp | 87 +++--- src/parametric_dynamics_model.cpp | 108 ++++---- .../gazebo_aerodynamics_plugin_test.cpp | 14 +- 10 files changed, 306 insertions(+), 297 deletions(-) create mode 100644 .github/workflows/check_style.yml create mode 100755 Tools/check_code_format.sh create mode 100755 Tools/fix_code_style.sh diff --git a/.github/workflows/check_style.yml b/.github/workflows/check_style.yml new file mode 100644 index 00000000..e267fe35 --- /dev/null +++ b/.github/workflows/check_style.yml @@ -0,0 +1,29 @@ +name: Style Checks + +on: + push: + branches: + - master + pull_request: + branches: + - '*' + +jobs: + build: + runs-on: ubuntu-latest + strategy: + fail-fast: false + matrix: + container: + - 'px4io/px4-dev-simulation-focal:2020-09-14' + container: ${{ matrix.container }} + steps: + - uses: actions/checkout@v1 + - name: submodule update + run: git submodule update --init --recursive + - name: Install clang-format + run: apt update && apt install -y clang-format-6.0 + - name: Check Code format + working-directory: Tools + run: | + ./check_code_format.sh .. diff --git a/Makefile b/Makefile index 4a2820e6..3d4d600e 100644 --- a/Makefile +++ b/Makefile @@ -32,3 +32,5 @@ docker-run: estimate-model: python3 Tools/parametric_model/generate_parametric_model.py --model ${model} --config ${config} --data_selection ${data_selection} ${log} +format: + Tools/fix_code_style.sh . \ No newline at end of file diff --git a/Tools/check_code_format.sh b/Tools/check_code_format.sh new file mode 100755 index 00000000..7ac8c29d --- /dev/null +++ b/Tools/check_code_format.sh @@ -0,0 +1,10 @@ +#!/bin/bash + +# Fix style recursively in all the repository +sh fix_code_style.sh .. + +# Print the diff with the remote branch (empty if no diff) +git --no-pager diff -U0 --color + +# Check if there are changes, and failed +if ! git diff-index --quiet HEAD --; then echo "Code style check failed, please run clang-format (e.g. with scripts/fix_code_style.sh)"; exit 1; fi diff --git a/Tools/fix_code_style.sh b/Tools/fix_code_style.sh new file mode 100755 index 00000000..c85f84ee --- /dev/null +++ b/Tools/fix_code_style.sh @@ -0,0 +1,21 @@ +#!/bin/bash + +STYLE="google" + +if [ "$#" -eq 0 ]; then + echo "Usage: $0 " + echo "" + echo "ERROR: At least one source file or one directory must be provided!" + + exit 1 +fi + +for arg in "$@" +do + if [ -f $arg ]; then + clang-format-6.0 -i -style='{BasedOnStyle: google, ColumnLimit: 120}' $arg + elif [ -d $arg ]; then + find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs clang-format-6.0 -i -style='{BasedOnStyle: google, ColumnLimit: 120}' + find $arg -iname '*.h' -o -iname '*.cpp' -o -iname '*.hpp' | xargs chmod 644 + fi +done diff --git a/include/aero_parameters.h b/include/aero_parameters.h index bda1d2db..cd3153d0 100644 --- a/include/aero_parameters.h +++ b/include/aero_parameters.h @@ -17,8 +17,8 @@ #ifndef ROTORS_GAZEBO_PLUGINS_FW_PARAMETERS_H_ #define ROTORS_GAZEBO_PLUGINS_FW_PARAMETERS_H_ -#include #include +#include #include #include @@ -37,58 +37,36 @@ static constexpr double kDefaultThrustInclination = 0.0; static constexpr double kDefaultAlphaMax = 0.27; static constexpr double kDefaultAlphaMin = -0.27; -static const Eigen::Vector3d kDefaultCDragAlpha = - Eigen::Vector3d(0.1360, -0.6737, 5.4546); -static const Eigen::Vector3d kDefaultCDragBeta = - Eigen::Vector3d(0.0195, 0.0, -0.3842); -static const Eigen::Vector3d kDefaultCDragDeltaAil = - Eigen::Vector3d(0.0195, 1.4205e-4, 7.5037e-6); -static const Eigen::Vector3d kDefaultCDragDeltaFlp = - Eigen::Vector3d(0.0195, 2.7395e-4, 1.23e-5); - -static const Eigen::Vector2d kDefaultCSideForceBeta = - Eigen::Vector2d(0.0, -0.3073); - -static const Eigen::Vector4d kDefaultCLiftAlpha = - Eigen::Vector4d(0.2127, 10.8060, -46.8324, 60.6017); -static const Eigen::Vector2d kDefaultCLiftDeltaAil = - Eigen::Vector2d(0.3304, 0.0048); -static const Eigen::Vector2d kDefaultCLiftDeltaFlp = - Eigen::Vector2d(0.3304, 0.0073); - -static const Eigen::Vector2d kDefaultCRollMomentBeta = - Eigen::Vector2d(0.0, -0.0154); -static const Eigen::Vector2d kDefaultCRollMomentP = - Eigen::Vector2d(0.0, -0.1647); -static const Eigen::Vector2d kDefaultCRollMomentR = - Eigen::Vector2d(0.0, 0.0117); -static const Eigen::Vector2d kDefaultCRollMomentDeltaAil = - Eigen::Vector2d(0.0, 0.0570); -static const Eigen::Vector2d kDefaultCRollMomentDeltaFlp = - Eigen::Vector2d(0.0, 0.001); - -static const Eigen::Vector2d kDefaultCPitchMomentAlpha = - Eigen::Vector2d(0.0435, -2.9690); -static const Eigen::Vector2d kDefaultCPitchMomentQ = - Eigen::Vector2d(-0.1173, -106.1541); -static const Eigen::Vector2d kDefaultCPitchMomentDeltaElv = - Eigen::Vector2d(-0.1173, -6.1308); - -static const Eigen::Vector2d kDefaultCYawMomentBeta = - Eigen::Vector2d(0.0, 0.0430); -static const Eigen::Vector2d kDefaultCYawMomentR = - Eigen::Vector2d(0.0, -0.0827); -static const Eigen::Vector2d kDefaultCYawMomentDeltaRud = - Eigen::Vector2d(0.0, 0.06); - -static const Eigen::Vector3d kDefaultCThrust = - Eigen::Vector3d(0.0, 14.7217, 0.0); +static const Eigen::Vector3d kDefaultCDragAlpha = Eigen::Vector3d(0.1360, -0.6737, 5.4546); +static const Eigen::Vector3d kDefaultCDragBeta = Eigen::Vector3d(0.0195, 0.0, -0.3842); +static const Eigen::Vector3d kDefaultCDragDeltaAil = Eigen::Vector3d(0.0195, 1.4205e-4, 7.5037e-6); +static const Eigen::Vector3d kDefaultCDragDeltaFlp = Eigen::Vector3d(0.0195, 2.7395e-4, 1.23e-5); + +static const Eigen::Vector2d kDefaultCSideForceBeta = Eigen::Vector2d(0.0, -0.3073); + +static const Eigen::Vector4d kDefaultCLiftAlpha = Eigen::Vector4d(0.2127, 10.8060, -46.8324, 60.6017); +static const Eigen::Vector2d kDefaultCLiftDeltaAil = Eigen::Vector2d(0.3304, 0.0048); +static const Eigen::Vector2d kDefaultCLiftDeltaFlp = Eigen::Vector2d(0.3304, 0.0073); + +static const Eigen::Vector2d kDefaultCRollMomentBeta = Eigen::Vector2d(0.0, -0.0154); +static const Eigen::Vector2d kDefaultCRollMomentP = Eigen::Vector2d(0.0, -0.1647); +static const Eigen::Vector2d kDefaultCRollMomentR = Eigen::Vector2d(0.0, 0.0117); +static const Eigen::Vector2d kDefaultCRollMomentDeltaAil = Eigen::Vector2d(0.0, 0.0570); +static const Eigen::Vector2d kDefaultCRollMomentDeltaFlp = Eigen::Vector2d(0.0, 0.001); + +static const Eigen::Vector2d kDefaultCPitchMomentAlpha = Eigen::Vector2d(0.0435, -2.9690); +static const Eigen::Vector2d kDefaultCPitchMomentQ = Eigen::Vector2d(-0.1173, -106.1541); +static const Eigen::Vector2d kDefaultCPitchMomentDeltaElv = Eigen::Vector2d(-0.1173, -6.1308); + +static const Eigen::Vector2d kDefaultCYawMomentBeta = Eigen::Vector2d(0.0, 0.0430); +static const Eigen::Vector2d kDefaultCYawMomentR = Eigen::Vector2d(0.0, -0.0827); +static const Eigen::Vector2d kDefaultCYawMomentDeltaRud = Eigen::Vector2d(0.0, 0.06); + +static const Eigen::Vector3d kDefaultCThrust = Eigen::Vector3d(0.0, 14.7217, 0.0); // Default values for fixed-wing controls (Techpod model) -static constexpr double kDefaultControlSurfaceDeflectionMin = - -20.0 * M_PI / 180.0; -static constexpr double kDefaultControlSurfaceDeflectionMax = - 20.0 * M_PI / 180.0; +static constexpr double kDefaultControlSurfaceDeflectionMin = -20.0 * M_PI / 180.0; +static constexpr double kDefaultControlSurfaceDeflectionMax = 20.0 * M_PI / 180.0; static constexpr int kDefaultAileronLeftChannel = 0; static constexpr int kDefaultAileronRightChannel = 0; @@ -99,36 +77,29 @@ static constexpr int kDefaultThrottleChannel = 3; /// \brief Wrapper function for extracting control surface parameters from a /// YAML node. -inline void YAMLReadControlSurface(const YAML::Node& node, - const std::string& name, - ControlSurface& surface); +inline void YAMLReadControlSurface(const YAML::Node& node, const std::string& name, ControlSurface& surface); /// \brief This function reads a vector from a YAML node and converts it into /// a vector of type Eigen. template -inline void YAMLReadEigenVector(const YAML::Node& node, - const std::string& name, - Eigen::MatrixBase& value); +inline void YAMLReadEigenVector(const YAML::Node& node, const std::string& name, Eigen::MatrixBase& value); /// \brief This function reads a parameter from a YAML node. template -inline void YAMLReadParam(const YAML::Node& node, - const std::string& name, - T& value); +inline void YAMLReadParam(const YAML::Node& node, const std::string& name, T& value); /// \brief Macros to reduce copies of names. -#define READ_CONTROL_SURFACE(node, item) \ - YAMLReadControlSurface(node, #item, item); +#define READ_CONTROL_SURFACE(node, item) YAMLReadControlSurface(node, #item, item); #define READ_EIGEN_VECTOR(node, item) YAMLReadEigenVector(node, #item, item); #define READ_PARAM(node, item) YAMLReadParam(node, #item, item); struct RotorParameters { - Eigen::Vector3d position{Eigen::Vector3d::Zero()}; - Eigen::Vector3d rotor_axis{Eigen::Vector3d(0.0, 0.0, 1.0)}; - double diameter{1.0}; - double vertical_rot_drag_lin{0.07444735702448266}; - double vertical_rot_thrust_lin{-0.0017229667485354344}; - double vertical_rot_thrust_quad{4.0095427586089745}; + Eigen::Vector3d position{Eigen::Vector3d::Zero()}; + Eigen::Vector3d rotor_axis{Eigen::Vector3d(0.0, 0.0, 1.0)}; + double diameter{1.0}; + double vertical_rot_drag_lin{0.07444735702448266}; + double vertical_rot_thrust_lin{-0.0017229667485354344}; + double vertical_rot_thrust_quad{4.0095427586089745}; }; struct FWAerodynamicParameters { EIGEN_MAKE_ALIGNED_OPERATOR_NEW @@ -193,26 +164,25 @@ struct FWAerodynamicParameters { Eigen::Vector3d c_thrust; void LoadAeroParamsYAML(const std::string& yaml_path) { - const YAML::Node node = YAML::LoadFile(yaml_path); - gzdbg << yaml_path < -inline void YAMLReadEigenVector(const YAML::Node& node, - const std::string& name, - Eigen::MatrixBase& value) { - std::vector vec = - node[name].as>(); +inline void YAMLReadEigenVector(const YAML::Node& node, const std::string& name, Eigen::MatrixBase& value) { + std::vector vec = node[name].as>(); assert(vec.size() == Derived::SizeAtCompileTime); value = Eigen::Map(&vec[0], vec.size()); } template -inline void YAMLReadParam(const YAML::Node& node, - const std::string& name, - T& value) { +inline void YAMLReadParam(const YAML::Node& node, const std::string& name, T& value) { value = node[name].as(); } -} +} // namespace gazebo #endif /* ROTORS_GAZEBO_PLUGINS_FW_PARAMETERS_H_ */ diff --git a/include/data_driven_dynamics_plugin.h b/include/data_driven_dynamics_plugin.h index 7a35b3e7..27aa16d0 100644 --- a/include/data_driven_dynamics_plugin.h +++ b/include/data_driven_dynamics_plugin.h @@ -21,10 +21,10 @@ #include #include -#include -#include #include #include +#include +#include #include #include "CommandMotorSpeed.pb.h" @@ -56,12 +56,11 @@ class DataDrivenDynamicsPlugin : public ModelPlugin { /// \brief Calculates the forces and moments to be applied to the /// fixed-wing body. - void UpdateForcesAndMoments(Eigen::Vector3d &forces, Eigen::Vector3d &moments); + void UpdateForcesAndMoments(Eigen::Vector3d& forces, Eigen::Vector3d& moments); double NormalizedInputToAngle(const ControlSurface& surface, double input); private: - /// \brief Flag to indicate that gazebo_mavlink_interface plugin handles /// routing of actuation data (instead of gazebo_ros_interface_plugin) bool use_gazebo_mavlink_interface_; @@ -141,4 +140,4 @@ class DataDrivenDynamicsPlugin : public ModelPlugin { } // namespace gazebo -#endif // ROTORS_GAZEBO_PLUGINS_FW_DYNAMICS_PLUGIN_H +#endif // ROTORS_GAZEBO_PLUGINS_FW_DYNAMICS_PLUGIN_H diff --git a/include/parametric_dynamics_model.h b/include/parametric_dynamics_model.h index 7d4c5494..866a94d0 100644 --- a/include/parametric_dynamics_model.h +++ b/include/parametric_dynamics_model.h @@ -30,37 +30,40 @@ static constexpr double kMinAirSpeedThresh = 0.1; namespace gazebo { class ParametricDynamicsModel { - public: - ParametricDynamicsModel(); - virtual ~ParametricDynamicsModel(); - void setState(const ignition::math::Vector3d &B_air_speed_W_B, const ignition::math::Vector3d &B_angular_velocity_W_B, - double delta_aileron_left, double delta_aileron_right, double delta_elevator, double delta_flap, double delta_rudder, const Eigen::VectorXd actuator_inputs); - void LoadAeroParams(std::string aero_params_yaml) { - aero_params_->LoadAeroParamsYAML(aero_params_yaml); - }; - void LoadVehicleParams(std::string vehicle_params_yaml) { - vehicle_params_->LoadVehicleParamsYAML(vehicle_params_yaml); - }; - Eigen::Vector3d getForce() {return force_;}; - Eigen::Vector3d getMoment() {return moment_;}; - std::shared_ptr getAeroParams(){return aero_params_;}; - std::shared_ptr getVehicleParams(){return vehicle_params_;}; - inline static Eigen::Vector3d ignition2eigen(const ignition::math::Vector3d &vec) { - return Eigen::Vector3d(vec.X(), vec.Y(), vec.Z()); - }; + public: + ParametricDynamicsModel(); + virtual ~ParametricDynamicsModel(); + void setState(const ignition::math::Vector3d &B_air_speed_W_B, const ignition::math::Vector3d &B_angular_velocity_W_B, + double delta_aileron_left, double delta_aileron_right, double delta_elevator, double delta_flap, + double delta_rudder, const Eigen::VectorXd actuator_inputs); + void LoadAeroParams(std::string aero_params_yaml) { aero_params_->LoadAeroParamsYAML(aero_params_yaml); }; + void LoadVehicleParams(std::string vehicle_params_yaml) { + vehicle_params_->LoadVehicleParamsYAML(vehicle_params_yaml); + }; + Eigen::Vector3d getForce() { return force_; }; + Eigen::Vector3d getMoment() { return moment_; }; + std::shared_ptr getAeroParams() { return aero_params_; }; + std::shared_ptr getVehicleParams() { return vehicle_params_; }; + inline static Eigen::Vector3d ignition2eigen(const ignition::math::Vector3d &vec) { + return Eigen::Vector3d(vec.X(), vec.Y(), vec.Z()); + }; - private: - void computeTotalRotorWrench(const Eigen::Vector3d airspeed, const Eigen::VectorXd &actuator_inputs, Eigen::Vector3d &rotor_force, Eigen::Vector3d &rotor_moment); Eigen::Vector3d computeRotorForce(const Eigen::Vector3d airspeed, const double actuator_input, const RotorParameters &rotor_params); - Eigen::Vector3d computeRotorMoment(const Eigen::Vector3d airspeed, const double actuator_input, const RotorParameters &rotor_params, Eigen::Vector3d rotor_force); - Eigen::Vector3d force_{Eigen::Vector3d::Zero()}; - Eigen::Vector3d moment_{Eigen::Vector3d::Zero()}; - /// \brief Throttle input, in range from 0 to 1. - double throttle_{0.0}; - /// \brief The aerodynamic properties of the aircraft. - std::shared_ptr aero_params_; + private: + void computeTotalRotorWrench(const Eigen::Vector3d airspeed, const Eigen::VectorXd &actuator_inputs, + Eigen::Vector3d &rotor_force, Eigen::Vector3d &rotor_moment); + Eigen::Vector3d computeRotorForce(const Eigen::Vector3d airspeed, const double actuator_input, + const RotorParameters &rotor_params); + Eigen::Vector3d computeRotorMoment(const Eigen::Vector3d airspeed, const double actuator_input, + const RotorParameters &rotor_params, Eigen::Vector3d rotor_force); + Eigen::Vector3d force_{Eigen::Vector3d::Zero()}; + Eigen::Vector3d moment_{Eigen::Vector3d::Zero()}; + /// \brief Throttle input, in range from 0 to 1. + double throttle_{0.0}; + /// \brief The aerodynamic properties of the aircraft. + std::shared_ptr aero_params_; - /// \brief The physical properties of the aircraft. - std::shared_ptr vehicle_params_; + /// \brief The physical properties of the aircraft. + std::shared_ptr vehicle_params_; }; -} -#endif // PARAMETRIC_DYNAMICS_MODEL_H +} // namespace gazebo +#endif // PARAMETRIC_DYNAMICS_MODEL_H diff --git a/src/data_driven_dynamics_plugin.cpp b/src/data_driven_dynamics_plugin.cpp index 140eac91..8b702442 100644 --- a/src/data_driven_dynamics_plugin.cpp +++ b/src/data_driven_dynamics_plugin.cpp @@ -19,19 +19,13 @@ namespace gazebo { -DataDrivenDynamicsPlugin::DataDrivenDynamicsPlugin() - : ModelPlugin(), - node_handle_(0), - W_wind_speed_W_B_(0, 0, 0) { - actuator_inputs_ = Eigen::VectorXd::Zero(num_input_channels); +DataDrivenDynamicsPlugin::DataDrivenDynamicsPlugin() : ModelPlugin(), node_handle_(0), W_wind_speed_W_B_(0, 0, 0) { + actuator_inputs_ = Eigen::VectorXd::Zero(num_input_channels); } -DataDrivenDynamicsPlugin::~DataDrivenDynamicsPlugin() { -} - -void DataDrivenDynamicsPlugin::Load(physics::ModelPtr _model, - sdf::ElementPtr _sdf) { +DataDrivenDynamicsPlugin::~DataDrivenDynamicsPlugin() {} +void DataDrivenDynamicsPlugin::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf) { // Store the pointer to the model. model_ = _model; world_ = model_->GetWorld(); @@ -59,34 +53,31 @@ void DataDrivenDynamicsPlugin::Load(physics::ModelPtr _model, // Get the pointer to the link. link_ = model_->GetLink(link_name); if (link_ == NULL) { - gzthrow("[gazebo_fw_dynamics_plugin] Couldn't find specified link \"" - << link_name << "\"."); + gzthrow("[gazebo_fw_dynamics_plugin] Couldn't find specified link \"" << link_name << "\"."); } parametric_model_ = std::make_shared(); // Get the path to fixed-wing aerodynamics parameters YAML file. If not // provided, default Techpod parameters are used. if (_sdf->HasElement("aeroParamsYAML")) { - std::string aero_params_yaml = - _sdf->GetElement("aeroParamsYAML")->Get(); + std::string aero_params_yaml = _sdf->GetElement("aeroParamsYAML")->Get(); parametric_model_->LoadAeroParams(aero_params_yaml); } else { gzwarn << "[gazebo_fw_dynamics_plugin] No aerodynamic paramaters YAML file" - << " specified, using default Techpod parameters.\n"; + << " specified, using default Techpod parameters.\n"; } // Get the path to fixed-wing vehicle parameters YAML file. If not provided, // default Techpod parameters are used. if (_sdf->HasElement("vehicleParamsYAML")) { - std::string vehicle_params_yaml = - _sdf->GetElement("vehicleParamsYAML")->Get(); + std::string vehicle_params_yaml = _sdf->GetElement("vehicleParamsYAML")->Get(); vehicle_params_.LoadVehicleParamsYAML(vehicle_params_yaml); parametric_model_->LoadVehicleParams(vehicle_params_yaml); } else { gzwarn << "[gazebo_fw_dynamics_plugin] No vehicle paramaters YAML file" - << " specified, using default Techpod parameters.\n"; + << " specified, using default Techpod parameters.\n"; } std::string actuator_sub_topic_ = "/gazebo/command/motor_speed"; @@ -95,12 +86,13 @@ void DataDrivenDynamicsPlugin::Load(physics::ModelPtr _model, } wind_speed_sub_ = node_handle_->Subscribe("~/world_wind", &DataDrivenDynamicsPlugin::WindVelocityCallback, this); - actuators_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + actuator_sub_topic_, &DataDrivenDynamicsPlugin::ActuatorsCallback, this); + actuators_sub_ = node_handle_->Subscribe("~/" + model_->GetName() + actuator_sub_topic_, + &DataDrivenDynamicsPlugin::ActuatorsCallback, this); // Listen to the update event. This event is broadcast every // simulation iteration. - this->updateConnection_ = event::Events::ConnectWorldUpdateBegin( - boost::bind(&DataDrivenDynamicsPlugin::OnUpdate, this, _1)); + this->updateConnection_ = + event::Events::ConnectWorldUpdateBegin(boost::bind(&DataDrivenDynamicsPlugin::OnUpdate, this, _1)); } void DataDrivenDynamicsPlugin::OnUpdate(const common::UpdateInfo& _info) { @@ -108,29 +100,28 @@ void DataDrivenDynamicsPlugin::OnUpdate(const common::UpdateInfo& _info) { UpdateForcesAndMoments(forces, moments); - ignition::math::Vector3d forces_msg = ignition::math::Vector3d (forces[0], forces[1], forces[2]); - ignition::math::Vector3d moments_msg = ignition::math::Vector3d (moments[0], moments[1], moments[2]); + ignition::math::Vector3d forces_msg = ignition::math::Vector3d(forces[0], forces[1], forces[2]); + ignition::math::Vector3d moments_msg = ignition::math::Vector3d(moments[0], moments[1], moments[2]); link_->AddRelativeForce(forces_msg); link_->AddRelativeTorque(moments_msg); } -void DataDrivenDynamicsPlugin::UpdateForcesAndMoments(Eigen::Vector3d &forces, Eigen::Vector3d &moments) { +void DataDrivenDynamicsPlugin::UpdateForcesAndMoments(Eigen::Vector3d& forces, Eigen::Vector3d& moments) { // Express the air speed and angular velocity in the body frame. // B denotes body frame and W world frame ... e.g., W_rot_W_B denotes // rotation of B wrt. W expressed in W. ignition::math::Quaterniond W_rot_W_B = link_->WorldPose().Rot(); - ignition::math::Vector3d B_air_speed_W_B = W_rot_W_B.RotateVectorReverse( - link_->WorldLinearVel() - W_wind_speed_W_B_); + ignition::math::Vector3d B_air_speed_W_B = W_rot_W_B.RotateVectorReverse(link_->WorldLinearVel() - W_wind_speed_W_B_); ignition::math::Vector3d B_angular_velocity_W_B = link_->RelativeAngularVel(); - parametric_model_->setState(B_air_speed_W_B, B_angular_velocity_W_B, delta_aileron_left_, - delta_aileron_right_, delta_elevator_, delta_flap_, delta_rudder_, actuator_inputs_); + parametric_model_->setState(B_air_speed_W_B, B_angular_velocity_W_B, delta_aileron_left_, delta_aileron_right_, + delta_elevator_, delta_flap_, delta_rudder_, actuator_inputs_); const Eigen::Vector3d parametric_force = parametric_model_->getForce(); const Eigen::Vector3d parametric_moment = parametric_model_->getMoment(); - //TODO: Implement Residual dynamics prediction + // TODO: Implement Residual dynamics prediction const Eigen::Vector3d residual_force{Eigen::Vector3d::Zero()}; const Eigen::Vector3d residual_moment{Eigen::Vector3d::Zero()}; @@ -143,26 +134,22 @@ void DataDrivenDynamicsPlugin::UpdateForcesAndMoments(Eigen::Vector3d &forces, E moments << moments_B[0], -moments_B[1], -moments_B[2]; } -double DataDrivenDynamicsPlugin::NormalizedInputToAngle( - const ControlSurface& surface, double input) { +double DataDrivenDynamicsPlugin::NormalizedInputToAngle(const ControlSurface& surface, double input) { return (surface.deflection_max + surface.deflection_min) * 0.5 + - (surface.deflection_max - surface.deflection_min) * 0.5 * input; + (surface.deflection_max - surface.deflection_min) * 0.5 * input; } -void DataDrivenDynamicsPlugin::ActuatorsCallback(CommandMotorSpeedPtr &actuators_msg) { - - //TODO: Get channel information from yml file - delta_aileron_left_ = -NormalizedInputToAngle(vehicle_params_.aileron_left, - static_cast(actuators_msg->motor_speed(5))); - delta_aileron_right_ = -NormalizedInputToAngle(vehicle_params_.aileron_right, - static_cast(actuators_msg->motor_speed(6))); - delta_elevator_ = -NormalizedInputToAngle(vehicle_params_.elevator, - static_cast(actuators_msg->motor_speed(7))); - delta_flap_ = NormalizedInputToAngle(vehicle_params_.flap, - static_cast(actuators_msg->motor_speed(3))); - delta_rudder_ = NormalizedInputToAngle(vehicle_params_.rudder, - static_cast(actuators_msg->motor_speed(2))); - //TODO: Throttle is set to zero since force is applied outside of this plugin +void DataDrivenDynamicsPlugin::ActuatorsCallback(CommandMotorSpeedPtr& actuators_msg) { + // TODO: Get channel information from yml file + delta_aileron_left_ = + -NormalizedInputToAngle(vehicle_params_.aileron_left, static_cast(actuators_msg->motor_speed(5))); + delta_aileron_right_ = + -NormalizedInputToAngle(vehicle_params_.aileron_right, static_cast(actuators_msg->motor_speed(6))); + delta_elevator_ = + -NormalizedInputToAngle(vehicle_params_.elevator, static_cast(actuators_msg->motor_speed(7))); + delta_flap_ = NormalizedInputToAngle(vehicle_params_.flap, static_cast(actuators_msg->motor_speed(3))); + delta_rudder_ = NormalizedInputToAngle(vehicle_params_.rudder, static_cast(actuators_msg->motor_speed(2))); + // TODO: Throttle is set to zero since force is applied outside of this plugin // throttle_ = actuators_msg->normalized(vehicle_params_.throttle_channel); throttle_ = 0.0; @@ -171,11 +158,9 @@ void DataDrivenDynamicsPlugin::ActuatorsCallback(CommandMotorSpeedPtr &actuators } } -void DataDrivenDynamicsPlugin::WindVelocityCallback( - WindPtr& msg) { - ignition::math::Vector3d wind_vel_ = ignition::math::Vector3d(msg->velocity().x(), - msg->velocity().y(), - msg->velocity().z()); +void DataDrivenDynamicsPlugin::WindVelocityCallback(WindPtr& msg) { + ignition::math::Vector3d wind_vel_ = + ignition::math::Vector3d(msg->velocity().x(), msg->velocity().y(), msg->velocity().z()); } GZ_REGISTER_MODEL_PLUGIN(DataDrivenDynamicsPlugin); diff --git a/src/parametric_dynamics_model.cpp b/src/parametric_dynamics_model.cpp index 6d5d9d36..cf5c8d1a 100644 --- a/src/parametric_dynamics_model.cpp +++ b/src/parametric_dynamics_model.cpp @@ -20,15 +20,16 @@ namespace gazebo { ParametricDynamicsModel::ParametricDynamicsModel() { - aero_params_ = std::make_shared(); - vehicle_params_ = std::make_shared(); - + aero_params_ = std::make_shared(); + vehicle_params_ = std::make_shared(); } ParametricDynamicsModel::~ParametricDynamicsModel() {} -void ParametricDynamicsModel::setState(const ignition::math::Vector3d &B_air_speed_W_B, const ignition::math::Vector3d &B_angular_velocity_W_B, - double delta_aileron_left, double delta_aileron_right, double delta_elevator, double delta_flap, double delta_rudder, const Eigen::VectorXd actuator_inputs) { +void ParametricDynamicsModel::setState(const ignition::math::Vector3d &B_air_speed_W_B, + const ignition::math::Vector3d &B_angular_velocity_W_B, + double delta_aileron_left, double delta_aileron_right, double delta_elevator, + double delta_flap, double delta_rudder, const Eigen::VectorXd actuator_inputs) { // Traditionally, fixed-wing aerodynamics use NED (North-East-Down) frame, // but since our model's body frame is in North-West-Up frame we rotate the // linear and angular velocities by 180 degrees around the X axis. @@ -49,63 +50,68 @@ void ParametricDynamicsModel::setState(const ignition::math::Vector3d &B_air_spe moment_ = moment_rotor_B; } -void ParametricDynamicsModel::computeTotalRotorWrench(const Eigen::Vector3d airspeed, const Eigen::VectorXd &actuator_inputs, Eigen::Vector3d &rotor_force, Eigen::Vector3d &rotor_moment) { - - rotor_force = Eigen::Vector3d::Zero(); - rotor_moment = Eigen::Vector3d::Zero(); - - for (size_t i = 0; i < aero_params_->rotor_parameters_.size(); i++) { - Eigen::Vector3d single_rotor_force =computeRotorForce(airspeed, actuator_inputs[i], aero_params_->rotor_parameters_[i]); - Eigen::Vector3d single_rotor_moment =computeRotorMoment(airspeed, actuator_inputs[i], aero_params_->rotor_parameters_[i], single_rotor_force); - rotor_force+=single_rotor_force; - rotor_moment+=single_rotor_moment; - } +void ParametricDynamicsModel::computeTotalRotorWrench(const Eigen::Vector3d airspeed, + const Eigen::VectorXd &actuator_inputs, + Eigen::Vector3d &rotor_force, Eigen::Vector3d &rotor_moment) { + rotor_force = Eigen::Vector3d::Zero(); + rotor_moment = Eigen::Vector3d::Zero(); + + for (size_t i = 0; i < aero_params_->rotor_parameters_.size(); i++) { + Eigen::Vector3d single_rotor_force = + computeRotorForce(airspeed, actuator_inputs[i], aero_params_->rotor_parameters_[i]); + Eigen::Vector3d single_rotor_moment = + computeRotorMoment(airspeed, actuator_inputs[i], aero_params_->rotor_parameters_[i], single_rotor_force); + rotor_force += single_rotor_force; + rotor_moment += single_rotor_moment; + } } -Eigen::Vector3d ParametricDynamicsModel::computeRotorForce(const Eigen::Vector3d airspeed, const double actuator_input, const RotorParameters &rotor_params) { +Eigen::Vector3d ParametricDynamicsModel::computeRotorForce(const Eigen::Vector3d airspeed, const double actuator_input, + const RotorParameters &rotor_params) { + if (!std::isfinite(actuator_input)) return Eigen::Vector3d::Zero(); + // Thrust force computation + const double prop_diameter = rotor_params.diameter; + const double thrust_lin = rotor_params.vertical_rot_thrust_lin; + const double thrust_quad = rotor_params.vertical_rot_thrust_quad; - if (!std::isfinite(actuator_input)) return Eigen::Vector3d::Zero(); - // Thrust force computation - const double prop_diameter = rotor_params.diameter; - const double thrust_lin = rotor_params.vertical_rot_thrust_lin; - const double thrust_quad = rotor_params.vertical_rot_thrust_quad; + Eigen::Vector3d rotor_axis = (rotor_params.rotor_axis).normalized(); - Eigen::Vector3d rotor_axis = (rotor_params.rotor_axis).normalized(); + Eigen::Vector3d v_airspeed_parallel_to_rotor_axis = airspeed.dot(rotor_axis) * rotor_axis; + Eigen::Vector3d v_airspeed_vertical_to_rotor_axis = airspeed - v_airspeed_parallel_to_rotor_axis; - Eigen::Vector3d v_airspeed_parallel_to_rotor_axis = airspeed.dot(rotor_axis) * rotor_axis; - Eigen::Vector3d v_airspeed_vertical_to_rotor_axis = airspeed - v_airspeed_parallel_to_rotor_axis; + /// TODO: Compensate for angular rates + Eigen::Vector3d rotor_thrust = ((thrust_lin * v_airspeed_parallel_to_rotor_axis.norm() * actuator_input + + thrust_quad * std::pow(actuator_input, 2) * prop_diameter)) * + kAirDensity * std::pow(prop_diameter, 3) * rotor_axis; + Eigen::Vector3d rotor_drag = Eigen::Vector3d::Zero(); - /// TODO: Compensate for angular rates - Eigen::Vector3d rotor_thrust = ((thrust_lin * v_airspeed_parallel_to_rotor_axis.norm() * actuator_input - + thrust_quad * std::pow(actuator_input, 2) * prop_diameter) ) * kAirDensity * std::pow(prop_diameter, 3) * rotor_axis; - Eigen::Vector3d rotor_drag = Eigen::Vector3d::Zero(); - - return rotor_thrust + rotor_drag; + return rotor_thrust + rotor_drag; } -Eigen::Vector3d ParametricDynamicsModel::computeRotorMoment(const Eigen::Vector3d airspeed, const double actuator_input, const RotorParameters &rotor_params, Eigen::Vector3d rotor_force) { - - // Thrust force computation - // const double prop_diameter = rotor_params.diameter; - // const double c_m_leaver_quad = rotor_params.c_m_leaver_quad; - // const double c_m_leaver_lin = rotor_params.c_m_leaver_lin; - // const double c_m_drag_z_quad = rotor_params.c_m_drag_z_quad; - // const double c_m_drag_z_lin = rotor_params.c_m_drag_z_lin; - // const double c_m_rolling = rotor_params.c_m_rolling; +Eigen::Vector3d ParametricDynamicsModel::computeRotorMoment(const Eigen::Vector3d airspeed, const double actuator_input, + const RotorParameters &rotor_params, + Eigen::Vector3d rotor_force) { + // Thrust force computation + // const double prop_diameter = rotor_params.diameter; + // const double c_m_leaver_quad = rotor_params.c_m_leaver_quad; + // const double c_m_leaver_lin = rotor_params.c_m_leaver_lin; + // const double c_m_drag_z_quad = rotor_params.c_m_drag_z_quad; + // const double c_m_drag_z_lin = rotor_params.c_m_drag_z_lin; + // const double c_m_rolling = rotor_params.c_m_rolling; - Eigen::Vector3d rotor_axis = (rotor_params.rotor_axis).normalized(); - Eigen::Vector3d rotor_position = rotor_params.position; + Eigen::Vector3d rotor_axis = (rotor_params.rotor_axis).normalized(); + Eigen::Vector3d rotor_position = rotor_params.position; - Eigen::Vector3d v_airspeed_parallel_to_rotor_axis = airspeed.dot(rotor_axis) * rotor_axis; - Eigen::Vector3d v_airspeed_vertical_to_rotor_axis = airspeed - v_airspeed_parallel_to_rotor_axis; + Eigen::Vector3d v_airspeed_parallel_to_rotor_axis = airspeed.dot(rotor_axis) * rotor_axis; + Eigen::Vector3d v_airspeed_vertical_to_rotor_axis = airspeed - v_airspeed_parallel_to_rotor_axis; - // Eigen::Vector3d moment_drag = Eigen::Vector3d::Zero(); - // Eigen::Vector3d moment_rolling = Eigen::Vector3d::Zero(); - // return moment_drag + moment_rolling; + // Eigen::Vector3d moment_drag = Eigen::Vector3d::Zero(); + // Eigen::Vector3d moment_rolling = Eigen::Vector3d::Zero(); + // return moment_drag + moment_rolling; - /// TODO: Using rotor forces to calculate moment generation as a wip patch - Eigen::Vector3d rotor_moment = rotor_position.cross(rotor_force); + /// TODO: Using rotor forces to calculate moment generation as a wip patch + Eigen::Vector3d rotor_moment = rotor_position.cross(rotor_force); - return rotor_moment; -} + return rotor_moment; } +} // namespace gazebo diff --git a/unit_tests/gazebo_aerodynamics_plugin_test.cpp b/unit_tests/gazebo_aerodynamics_plugin_test.cpp index 20e7654c..aaa42dcd 100644 --- a/unit_tests/gazebo_aerodynamics_plugin_test.cpp +++ b/unit_tests/gazebo_aerodynamics_plugin_test.cpp @@ -6,23 +6,17 @@ * public API of the class and therefore not declared in the header * of the plugin. */ namespace detail { -ignition::math::Vector3d ThreeAxisRot( - double r11, double r12, double r21, double r31, double r32); +ignition::math::Vector3d ThreeAxisRot(double r11, double r12, double r21, double r31, double r32); double NormalizeAbout(double _angle, double reference); double ShortestAngularDistance(double _from, double _to); -ignition::math::Vector3d QtoZXY( - const ignition::math::Quaterniond &_q); -} - +ignition::math::Vector3d QtoZXY(const ignition::math::Quaterniond &_q); +} // namespace detail //////////////////////////////////////////// /// ThreeAxisRot /////////////////////////// //////////////////////////////////////////// -TEST(Sample, Test1) { - ASSERT_EQ(true, true); -} - +TEST(Sample, Test1) { ASSERT_EQ(true, true); }