diff --git a/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp b/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp index 7e6b2fd..e2682a7 100644 --- a/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp +++ b/rosflight_sim/include/rosflight_sim/multirotor_forces_and_moments.hpp @@ -3,6 +3,7 @@ * * Copyright (c) 2017 Daniel Koch, James Jackson and Gary Ellingson, BYU MAGICC Lab. * Copyright (c) 2023 Brandon Sutherland, AeroVironment Inc. + * Copyright (c) 2024 Ian Reid, BYU MAGICC Lab. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -35,6 +36,7 @@ #ifndef ROSFLIGHT_SIM_MULTIROTOR_FORCES_AND_MOMENTS_H #define ROSFLIGHT_SIM_MULTIROTOR_FORCES_AND_MOMENTS_H +#include #include #include @@ -56,44 +58,45 @@ class Multirotor : public MAVForcesAndMoments private: rclcpp::Node::SharedPtr node_; Eigen::Vector3d wind_; - - double prev_time_; - - struct Rotor + + struct Prop { - double max; - std::vector F_poly; - std::vector T_poly; - double tau_up; // time constants for response - double tau_down; + // Prop thrust constant coeffs. + double CT_0; + double CT_1; + double CT_2; + + // Prop torque constant coeffs. + double CQ_0; + double CQ_1; + double CQ_2; + + double diam; }; struct Motor { - Rotor rotor; - Eigen::Vector3d position; - Eigen::Vector3d normal; - int direction; // 1 for CW -1 for CCW + double dist; + // The motor's radial angle from the body north unit vector in radians, + // for example: quad copter x frame this is 45,135,225,315, except in radians. + double radial_angle; + // The direction that the motor spins, (-1 is clockwise and 1 is counter clocwise, looking from above vehicle). + int64_t direction; + // The current command for the motor, from 0 to 1 + double command; + // Resistance of motor. + double R; + // Torque constant of motor. + double KQ; + // No load current draw of motor. + double I_0; + // Prop data + Prop prop; }; int num_rotors_; std::vector motors_; - double linear_mu_; - double angular_mu_; - std::vector ground_effect_; - - Eigen::MatrixXd rotor_position_; - Eigen::MatrixXd rotor_plane_normal_; - Eigen::VectorXd rotor_rotation_direction_; - - Eigen::MatrixXd force_allocation_matrix_; - Eigen::MatrixXd torque_allocation_matrix_; - Eigen::VectorXd desired_forces_; - Eigen::VectorXd desired_torques_; - Eigen::VectorXd actual_forces_; - Eigen::VectorXd actual_torques_; - /** * @brief Declares ROS parameters. Must be called in the constructor. */ diff --git a/rosflight_sim/params/multirotor_dynamics.yaml b/rosflight_sim/params/multirotor_dynamics.yaml index 838efcc..f23c1ea 100644 --- a/rosflight_sim/params/multirotor_dynamics.yaml +++ b/rosflight_sim/params/multirotor_dynamics.yaml @@ -4,9 +4,7 @@ /multirotor/rosflight_sil: ros__parameters: # Common Global Physical Parameters - linear_mu: 0.05 - angular_mu: 0.0005 - ground_effect: [ -55.3516, 181.8265, -203.9874, 85.3735, -7.6619 ] + rho: 1.225 # Dynamics num_rotors: 4 @@ -14,15 +12,37 @@ -0.1907, 0.205, -0.0762, -0.1907, -0.205, -0.0762, 0.1926, -0.230, -0.0762 ] + + rotor_dists: [.25, .25, .25, .25] + rotor_radial_angles: [45., 135., 225., 315.] + rotor_rotation_directions: [ -1, 1, -1, 1 ] + + # Prop Model + # Thrust + CT_0: 0.1196850 + CT_1: 0.0399995 + CT_2: -0.400139 - rotor_vector_normal: [ -0.02674078, 0.0223925, -0.99939157, - 0.02553726, 0.02375588, -0.99939157, - 0.02553726, -0.02375588, -0.99939157, - -0.02674078, -0.0223925, -0.99939157 ] + # Torque + CQ_0: 0.0097976 + CQ_1: -0.012271 + CQ_2: -0.014743 - rotor_rotation_directions: [ -1, 1, -1, 1 ] - rotor_max_thrust: 14.961 - rotor_F: [ 1.5e-5, -0.024451, 9.00225 ] - rotor_T: [ 2.22e-7,-3.51e-4, 0.12531 ] - rotor_tau_up: 0.2164 - rotor_tau_down: 0.1644 + # Diameter (m) + D_prop: 0.381 + + # Motor Specs + V_max : 24.0 # voltage for 6s battery at 4 volts per cell + R_motor : 0.042 # ohms + I_0 : 1.5 # no-load (zero torque) current (A) + KQ : 0.028937 #((1 /(KV_rpm_per_volt)) *60) / (2 *M_PI) Back-emf constant, KV in V-s/rad, Motor torque constant, KQ in N-m/A + + # Drag Model + CD: 2.05 # Coeffecient of drag for a rough cube face on into flow. + A_c: 0.04 # Crossectional Area to use in Drag. + + # Induced Drag Model + CD_induced: 0.05 + + # Ground Effect Model + ground_effect: [ -55.3516, 181.8265, -203.9874, 85.3735, -7.6619 ] diff --git a/rosflight_sim/src/multirotor_forces_and_moments.cpp b/rosflight_sim/src/multirotor_forces_and_moments.cpp index ee40072..0d821dc 100644 --- a/rosflight_sim/src/multirotor_forces_and_moments.cpp +++ b/rosflight_sim/src/multirotor_forces_and_moments.cpp @@ -32,6 +32,9 @@ * POSSIBILITY OF SUCH DAMAGE. */ +#include +#include + #include namespace rosflight_sim @@ -39,103 +42,44 @@ namespace rosflight_sim Multirotor::Multirotor(rclcpp::Node::SharedPtr node) : node_(std::move(node)) , num_rotors_(0) - , linear_mu_(0) - , angular_mu_(0) { declare_multirotor_params(); - if (!node_->get_parameter("linear_mu", linear_mu_)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'linear_mu' not defined"); - } - if (!node_->get_parameter("angular_mu", angular_mu_)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'angular_mu' not defined"); - } - if (!node_->get_parameter("ground_effect", ground_effect_)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'ground_effect' not defined"); - } - - if (!node_->get_parameter("num_rotors", num_rotors_)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'num_rotors' not defined"); - } - - std::vector rotor_positions(3 * num_rotors_); - std::vector rotor_vector_normal(3 * num_rotors_); - std::vector rotor_rotation_directions(num_rotors_); - - Rotor rotor; - if (!node_->get_parameter("rotor_positions", rotor_positions)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_positions' not defined"); - } - if (!node_->get_parameter("rotor_vector_normal", rotor_vector_normal)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_vector_normal' not defined"); - } - - if (!node_->get_parameter("rotor_rotation_directions", rotor_rotation_directions)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_rotation_directions' not defined"); - } - if (!node_->get_parameter("rotor_max_thrust", rotor.max)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_max_thrust' not defined"); - } - if (!node_->get_parameter("rotor_F", rotor.F_poly)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_F' not defined"); - } - if (!node_->get_parameter("rotor_T", rotor.T_poly)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_T' not defined"); - } - if (!node_->get_parameter("rotor_tau_up", rotor.tau_up)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_tau_up' not defined"); - } - if (!node_->get_parameter("rotor_tau_down", rotor.tau_down)) { - RCLCPP_ERROR(node_->get_logger(), "Param 'rotor_tau_down' not defined"); - } + num_rotors_ = node_->get_parameter("num_rotors").as_int(); /* Load Rotor Configuration */ motors_.resize(num_rotors_); + + Prop prop; - force_allocation_matrix_.resize(4, num_rotors_); - torque_allocation_matrix_.resize(4, num_rotors_); - for (int i = 0; i < num_rotors_; i++) { - motors_[i].rotor = rotor; - motors_[i].position.resize(3); - motors_[i].normal.resize(3); - for (int j = 0; j < 3; j++) { - motors_[i].position(j) = rotor_positions[3 * i + j]; - motors_[i].normal(j) = rotor_vector_normal[3 * i + j]; - } - motors_[i].normal.normalize(); - motors_[i].direction = (int) rotor_rotation_directions[i]; - - Eigen::Vector3d moment_from_thrust = motors_[i].position.cross(motors_[i].normal); - Eigen::Vector3d moment_from_torque = motors_[i].direction * motors_[i].normal; - - // build allocation_matrices - force_allocation_matrix_(0, i) = moment_from_thrust(0); // l - force_allocation_matrix_(1, i) = moment_from_thrust(1); // m - force_allocation_matrix_(2, i) = moment_from_thrust(2); // n - force_allocation_matrix_(3, i) = motors_[i].normal(2); // F - - torque_allocation_matrix_(0, i) = moment_from_torque(0); // l - torque_allocation_matrix_(1, i) = moment_from_torque(1); // m - torque_allocation_matrix_(2, i) = moment_from_torque(2); // n - torque_allocation_matrix_(3, i) = 0.0; // F - } - - RCLCPP_INFO_STREAM(node_->get_logger(), - "allocation matrices:\nFORCE \n" - << force_allocation_matrix_ << "\nTORQUE\n" - << torque_allocation_matrix_ << "\n"); + prop.CT_0 = node_->get_parameter("CT_0").as_double(); + prop.CT_1 = node_->get_parameter("CT_1").as_double(); + prop.CT_2 = node_->get_parameter("CT_2").as_double(); + + prop.CQ_0 = node_->get_parameter("CQ_0").as_double(); + prop.CQ_1 = node_->get_parameter("CQ_1").as_double(); + prop.CQ_2 = node_->get_parameter("CQ_2").as_double(); - // Initialize size of dynamic force and torque matrices - desired_forces_.resize(num_rotors_); - desired_torques_.resize(num_rotors_); - actual_forces_.resize(num_rotors_); - actual_torques_.resize(num_rotors_); + prop.diam = node_->get_parameter("D_prop").as_double(); + + std::vector dists = node_->get_parameter("rotor_dists").as_double_array(); + std::vector angles = node_->get_parameter("rotor_radial_angles").as_double_array(); + std::vector rotation_dirs = node_->get_parameter("rotor_rotation_directions").as_integer_array(); + + Motor motor_characteristics; + motor_characteristics.prop = prop; + motor_characteristics.R = node_->get_parameter("R_motor").as_double(); + motor_characteristics.I_0 = node_->get_parameter("I_0").as_double(); + motor_characteristics.KQ = node_->get_parameter("KQ").as_double(); + for (int i = 0; i < num_rotors_; i++) { - desired_forces_(i) = 0.0; - desired_torques_(i) = 0.0; - actual_forces_(i) = 0.0; - actual_torques_(i) = 0.0; + Motor motor = motor_characteristics; + motor.dist = dists[i]; + motor.radial_angle = angles[i] * M_PI / 180.0; + motor.direction = rotation_dirs[i]; + motor.command = 0.0; + motors_[i] = motor; } wind_ = Eigen::Vector3d::Zero(); @@ -143,83 +87,161 @@ Multirotor::Multirotor(rclcpp::Node::SharedPtr node) forces_moments_pub_ = node_->create_publisher("/forces_and_moments", 1); - prev_time_ = -1; } Multirotor::~Multirotor() = default; void Multirotor::declare_multirotor_params() { - node_->declare_parameter("mass", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("linear_mu", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("angular_mu", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("ground_effect", rclcpp::PARAMETER_DOUBLE_ARRAY); - node_->declare_parameter("num_rotors", rclcpp::PARAMETER_INTEGER); - node_->declare_parameter("rotor_positions", rclcpp::PARAMETER_DOUBLE_ARRAY); - node_->declare_parameter("rotor_vector_normal", rclcpp::PARAMETER_DOUBLE_ARRAY); + node_->declare_parameter("CT_0", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CT_1", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CT_2", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CQ_0", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CQ_1", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("CQ_2", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("KQ", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("V_max", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("R_motor", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("I_0", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("rotor_dists", rclcpp::PARAMETER_DOUBLE_ARRAY); + node_->declare_parameter("rotor_radial_angles", rclcpp::PARAMETER_DOUBLE_ARRAY); node_->declare_parameter("rotor_rotation_directions", rclcpp::PARAMETER_INTEGER_ARRAY); - node_->declare_parameter("rotor_max_thrust", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("rotor_F", rclcpp::PARAMETER_DOUBLE_ARRAY); - node_->declare_parameter("rotor_T", rclcpp::PARAMETER_DOUBLE_ARRAY); - node_->declare_parameter("rotor_tau_up", rclcpp::PARAMETER_DOUBLE); - node_->declare_parameter("rotor_tau_down", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("D_prop", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("rho", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("CD", rclcpp::PARAMETER_DOUBLE); + node_->declare_parameter("A_c", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("CD_induced", rclcpp::PARAMETER_DOUBLE); + + node_->declare_parameter("ground_effect", rclcpp::PARAMETER_DOUBLE_ARRAY); } Eigen::Matrix Multirotor::update_forces_and_torques(CurrentState x, const int act_cmds[]) { - if (prev_time_ < 0) { - prev_time_ = x.t; - return Eigen::Matrix::Zero(); - } - - double dt = x.t - prev_time_; - double pd = x.pos[2]; + // The maximum voltage of the battery. + double V_max = node_->get_parameter("V_max").as_double(); + + double rho = node_->get_parameter("rho").as_double(); // Get airspeed vector for drag force calculation (rotate wind into body frame and add to inertial velocity) Eigen::Vector3d Va = x.vel + x.rot.inverse() * wind_; - // Calculate Forces + double total_thrust = 0.0; + double total_roll_torque = 0.0; + double total_pitch_torque = 0.0; + double total_yaw_torque = 0.0; + for (int i = 0; i < num_rotors_; i++) { - // First, figure out the desired force output from passing the signal into the quadratic approximation - double signal = act_cmds[i]; - desired_forces_(i, 0) = motors_[i].rotor.F_poly[0] * signal * signal - + motors_[i].rotor.F_poly[1] * signal + motors_[i].rotor.F_poly[2]; - desired_torques_(i, 0) = motors_[i].rotor.T_poly[0] * signal * signal - + motors_[i].rotor.T_poly[1] * signal + motors_[i].rotor.T_poly[2]; - - // Then, Calculate Actual force and torque for each rotor using first-order dynamics - double tau = (desired_forces_(i, 0) > actual_forces_(i, 0)) ? motors_[i].rotor.tau_up - : motors_[i].rotor.tau_down; - double alpha = dt / (tau + dt); - actual_forces_(i, 0) = - sat((1 - alpha) * actual_forces_(i) + alpha * desired_forces_(i), motors_[i].rotor.max, 0.0); - actual_torques_(i, 0) = sat((1 - alpha) * actual_torques_(i) + alpha * desired_torques_(i), - motors_[i].rotor.max, 0.0); + + double motor_cmd = (act_cmds[i] - 1000)/1000.0; + + motors_[i].command = motor_cmd; } - // Use the allocation matrix to calculate the body-fixed force and torques - Eigen::Vector4d output_forces = force_allocation_matrix_ * actual_forces_; - Eigen::Vector4d output_torques = torque_allocation_matrix_ * actual_torques_; - Eigen::Vector4d output_forces_and_torques = output_forces + output_torques; + for (Motor motor : motors_) { + // Calculate prop_speed + // Use model with V_max + double v_in = V_max * motor.command; + + // Take true angle and only compare the portion parrallel to the body z-axis. + Eigen::Vector3d body_z; + body_z << 0.0, 0.0, 1.0; - // Calculate Ground Effect - double z = -pd; - double ground_effect = - max(ground_effect_[0] * z * z * z * z + ground_effect_[1] * z * z * z - + ground_effect_[2] * z * z + ground_effect_[3] * z + ground_effect_[4], - 0); + double Va_along_z = abs(Va.dot(body_z)); + + if (Va_along_z < 0.001) { + Va_along_z = 0.001; + } + + double a = (motor.prop.CQ_0 * (rho) * (pow((motor.prop.diam), 5.0)) / (pow((2 * M_PI), 2.0))); + double b = (motor.prop.CQ_1 * (rho) * (pow((motor.prop.diam), 4.0)) / (2 * M_PI)) * Va_along_z + + ((pow((motor.KQ), 2.0)) / (motor.R)); + double c = (motor.prop.CQ_2 * (rho) * (pow((motor.prop.diam), 3.0)) * (pow(Va_along_z, 2.0))) + - ((motor.KQ) / (motor.R)) * v_in + ((motor.KQ) * (motor.I_0)); // COULD BE OUR CULPRIT + + double prop_speed = ((-b + sqrt((pow((b), 2.0)) - (4 * a * c))) / (2 * a)); + + // Calculate the advance ratio. + double advance_ratio = (2. * M_PI * Va_along_z)/(prop_speed*motor.prop.diam); + + double CT = motor.prop.CT_2*pow(advance_ratio, 2) + motor.prop.CT_1*advance_ratio + motor.prop.CT_0; + + if (CT <= 0.0) { + CT = 0.0001; + } + + double motor_thrust = CT * (rho * pow(motor.prop.diam, 4))/(4*pow(M_PI,2)) * pow(prop_speed, 2); + + double motor_roll_torque = -motor_thrust*motor.dist*sinf(motor.radial_angle); + double motor_pitch_torque = motor_thrust*motor.dist*cosf(motor.radial_angle); + + double CQ = motor.prop.CQ_2*pow(advance_ratio, 2) + motor.prop.CQ_1*advance_ratio + motor.prop.CQ_0; + + if (CQ <= 0.0) { + CQ = 0.0001; + } + + // The torque produced by the propeller spinning. + double prop_torque = motor.direction * CQ * (rho*pow(motor.prop.diam, 5))/(4*pow(M_PI,2)) * pow(prop_speed, 2); + + total_thrust += motor_thrust; // Okay + total_roll_torque += motor_roll_torque; + total_pitch_torque += motor_pitch_torque; + total_yaw_torque += prop_torque; // Okay + } + + double cross_sectional_area = node_->get_parameter("A_c").as_double(); // m^2 + double CD = node_->get_parameter("CD").as_double(); + + // Rotate from body to inertial frame. + + Eigen::Vector3d body_forces; + body_forces << 0.0, 0.0, -total_thrust; + + double drag = 0.5 * rho * cross_sectional_area * CD * pow(Va.norm(), 2); + + Eigen::Vector3d drag_force = drag * Va / Va.norm(); + + if (Va.norm() < 0.001) { + drag_force << 0.0, 0.0, 0.0; + } + + body_forces -= drag_force; + + double CD_induced = node_->get_parameter("CD_induced").as_double(); + + Eigen::Matrix3d drag_matrix; + drag_matrix << -total_thrust*CD_induced, 0.0, 0.0, + 0.0, -total_thrust*CD_induced, 0.0, + 0.0, 0.0, 0.0; + + Eigen::Vector3d induced_drag_force = drag_matrix * x.vel; + + body_forces += induced_drag_force; + + Eigen::Vector3d body_torques; + body_torques << total_roll_torque, total_pitch_torque, -total_yaw_torque; Eigen::Matrix forces; - // Apply other forces (drag) <- follows "Quadrotors and Accelerometers - State Estimation With an Improved Dynamic - // Model" By Rob Leishman et al. - forces.block<3, 1>(0, 0) = -linear_mu_ * Va; - forces.block<3, 1>(3, 0) = -angular_mu_ * x.omega + output_forces_and_torques.block<3, 1>(0, 0); + forces.block<3,1>(0,0) = body_forces; + forces.block<3,1>(3,0) = body_torques; + + // Apply Ground Effect + std::vector ground_effect_coeffs = node_->get_parameter("ground_effect").as_double_array(); + + double ground_effect_force = max(0.0, ground_effect_coeffs[0]*pow(-x.pos.z(), 4) + + ground_effect_coeffs[1]*pow(-x.pos.z(), 3) + + ground_effect_coeffs[2]*pow(-x.pos.z(), 2) + + ground_effect_coeffs[3]*-x.pos.z() + + ground_effect_coeffs[4]); - // Apply ground effect and thrust - forces(2) += output_forces_and_torques(3) - ground_effect; + forces(2) -= ground_effect_force; geometry_msgs::msg::TwistStamped msg; diff --git a/rosflight_sim/src/rosflight_sil.cpp b/rosflight_sim/src/rosflight_sil.cpp index dfff38a..5a30aea 100644 --- a/rosflight_sim/src/rosflight_sil.cpp +++ b/rosflight_sim/src/rosflight_sil.cpp @@ -173,7 +173,7 @@ void ROSflightSIL::OnUpdate(const gazebo::common::UpdateInfo & _info) forces_ = mav_dynamics_->update_forces_and_torques(state, board_.get_outputs()); - // apply the forces and torques to the joint (apply in NWU) + // apply the forces and torques to the joint (apply in NWU) FORCES ARE IN NED AND MUST BE CONVERTED TO NWU. GazeboVector force = vec3_to_gazebo_from_eigen(NWU_to_NED * forces_.block<3, 1>(0, 0)); GazeboVector torque = vec3_to_gazebo_from_eigen(NWU_to_NED * forces_.block<3, 1>(3, 0)); link_->AddRelativeForce(force); diff --git a/rosflight_sim/xacro/multirotor.urdf.xacro b/rosflight_sim/xacro/multirotor.urdf.xacro index b83011d..a5177b0 100644 --- a/rosflight_sim/xacro/multirotor.urdf.xacro +++ b/rosflight_sim/xacro/multirotor.urdf.xacro @@ -14,11 +14,11 @@ Author: James Jackson - + - + />