Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021 #include "rotors_control/roll_pitch_yawrate_thrust_controller.h"
00022
00023 namespace rotors_control {
00024
00025 RollPitchYawrateThrustController::RollPitchYawrateThrustController()
00026 : initialized_params_(false),
00027 controller_active_(false) {
00028 InitializeParameters();
00029 }
00030
00031 RollPitchYawrateThrustController::~RollPitchYawrateThrustController() {}
00032
00033 void RollPitchYawrateThrustController::InitializeParameters() {
00034 calculateAllocationMatrix(vehicle_parameters_.rotor_configuration_, &(controller_parameters_.allocation_matrix_));
00035
00036 normalized_attitude_gain_ = controller_parameters_.attitude_gain_.transpose()
00037 * vehicle_parameters_.inertia_.inverse();
00038
00039 normalized_angular_rate_gain_ = controller_parameters_.angular_rate_gain_.transpose()
00040 * vehicle_parameters_.inertia_.inverse();
00041
00042 Eigen::Matrix4d I;
00043 I.setZero();
00044 I.block<3, 3>(0, 0) = vehicle_parameters_.inertia_;
00045 I(3, 3) = 1;
00046 angular_acc_to_rotor_velocities_.resize(vehicle_parameters_.rotor_configuration_.rotors.size(), 4);
00047
00048
00049 angular_acc_to_rotor_velocities_ = controller_parameters_.allocation_matrix_.transpose()
00050 * (controller_parameters_.allocation_matrix_
00051 * controller_parameters_.allocation_matrix_.transpose()).inverse() * I;
00052 initialized_params_ = true;
00053 }
00054
00055 void RollPitchYawrateThrustController::CalculateRotorVelocities(Eigen::VectorXd* rotor_velocities) const {
00056 assert(rotor_velocities);
00057 assert(initialized_params_);
00058
00059 rotor_velocities->resize(vehicle_parameters_.rotor_configuration_.rotors.size());
00060
00061 if (!controller_active_) {
00062 *rotor_velocities = Eigen::VectorXd::Zero(rotor_velocities->rows());
00063 return;
00064 }
00065
00066 Eigen::Vector3d angular_acceleration;
00067 ComputeDesiredAngularAcc(&angular_acceleration);
00068
00069 Eigen::Vector4d angular_acceleration_thrust;
00070 angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
00071 angular_acceleration_thrust(3) = roll_pitch_yawrate_thrust_.thrust.z();
00072
00073 *rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
00074 *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
00075 *rotor_velocities = rotor_velocities->cwiseSqrt();
00076 }
00077
00078 void RollPitchYawrateThrustController::SetOdometry(const EigenOdometry& odometry) {
00079 odometry_ = odometry;
00080 }
00081
00082 void RollPitchYawrateThrustController::SetRollPitchYawrateThrust(
00083 const mav_msgs::EigenRollPitchYawrateThrust& roll_pitch_yawrate_thrust) {
00084 roll_pitch_yawrate_thrust_ = roll_pitch_yawrate_thrust;
00085 controller_active_ = true;
00086 }
00087
00088
00089
00090 void RollPitchYawrateThrustController::ComputeDesiredAngularAcc(Eigen::Vector3d* angular_acceleration) const {
00091 assert(angular_acceleration);
00092
00093 Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();
00094 double yaw = atan2(R(1, 0), R(0, 0));
00095
00096
00097 Eigen::Matrix3d R_des;
00098 R_des = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ())
00099 * Eigen::AngleAxisd(roll_pitch_yawrate_thrust_.roll, Eigen::Vector3d::UnitX())
00100 * Eigen::AngleAxisd(roll_pitch_yawrate_thrust_.pitch, Eigen::Vector3d::UnitY());
00101
00102
00103 Eigen::Matrix3d angle_error_matrix = 0.5 * (R_des.transpose() * R - R.transpose() * R_des);
00104 Eigen::Vector3d angle_error;
00105 vectorFromSkewMatrix(angle_error_matrix, &angle_error);
00106
00107
00108 Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
00109 angular_rate_des[2] = roll_pitch_yawrate_thrust_.yaw_rate;
00110
00111 Eigen::Vector3d angular_rate_error = odometry_.angular_velocity - R_des.transpose() * R * angular_rate_des;
00112
00113 *angular_acceleration = -1 * angle_error.cwiseProduct(normalized_attitude_gain_)
00114 - angular_rate_error.cwiseProduct(normalized_angular_rate_gain_)
00115 + odometry_.angular_velocity.cross(odometry_.angular_velocity);
00116 }
00117 }
rotors_control
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:38