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/lee_position_controller.h"
00022
00023 namespace rotors_control {
00024
00025 LeePositionController::LeePositionController()
00026 : initialized_params_(false),
00027 controller_active_(false) {
00028 InitializeParameters();
00029 }
00030
00031 LeePositionController::~LeePositionController() {}
00032
00033 void LeePositionController::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 LeePositionController::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 acceleration;
00067 ComputeDesiredAcceleration(&acceleration);
00068
00069 Eigen::Vector3d angular_acceleration;
00070 ComputeDesiredAngularAcc(acceleration, &angular_acceleration);
00071
00072
00073 double thrust = -vehicle_parameters_.mass_ * acceleration.dot(odometry_.orientation.toRotationMatrix().col(2));
00074
00075 Eigen::Vector4d angular_acceleration_thrust;
00076 angular_acceleration_thrust.block<3, 1>(0, 0) = angular_acceleration;
00077 angular_acceleration_thrust(3) = thrust;
00078
00079 *rotor_velocities = angular_acc_to_rotor_velocities_ * angular_acceleration_thrust;
00080 *rotor_velocities = rotor_velocities->cwiseMax(Eigen::VectorXd::Zero(rotor_velocities->rows()));
00081 *rotor_velocities = rotor_velocities->cwiseSqrt();
00082 }
00083
00084 void LeePositionController::SetOdometry(const EigenOdometry& odometry) {
00085 odometry_ = odometry;
00086 }
00087
00088 void LeePositionController::SetTrajectoryPoint(
00089 const mav_msgs::EigenTrajectoryPoint& command_trajectory) {
00090 command_trajectory_ = command_trajectory;
00091 controller_active_ = true;
00092 }
00093
00094 void LeePositionController::ComputeDesiredAcceleration(Eigen::Vector3d* acceleration) const {
00095 assert(acceleration);
00096
00097 Eigen::Vector3d position_error;
00098 position_error = odometry_.position - command_trajectory_.position_W;
00099
00100
00101 const Eigen::Matrix3d R_W_I = odometry_.orientation.toRotationMatrix();
00102 Eigen::Vector3d velocity_W = R_W_I * odometry_.velocity;
00103 Eigen::Vector3d velocity_error;
00104 velocity_error = velocity_W - command_trajectory_.velocity_W;
00105
00106 Eigen::Vector3d e_3(Eigen::Vector3d::UnitZ());
00107
00108 *acceleration = (position_error.cwiseProduct(controller_parameters_.position_gain_)
00109 + velocity_error.cwiseProduct(controller_parameters_.velocity_gain_)) / vehicle_parameters_.mass_
00110 - vehicle_parameters_.gravity_ * e_3 - command_trajectory_.acceleration_W;
00111 }
00112
00113
00114
00115 void LeePositionController::ComputeDesiredAngularAcc(const Eigen::Vector3d& acceleration,
00116 Eigen::Vector3d* angular_acceleration) const {
00117 assert(angular_acceleration);
00118
00119 Eigen::Matrix3d R = odometry_.orientation.toRotationMatrix();
00120
00121
00122 Eigen::Vector3d b1_des;
00123 double yaw = command_trajectory_.getYaw();
00124 b1_des << cos(yaw), sin(yaw), 0;
00125
00126 Eigen::Vector3d b3_des;
00127 b3_des = -acceleration / acceleration.norm();
00128
00129 Eigen::Vector3d b2_des;
00130 b2_des = b3_des.cross(b1_des);
00131 b2_des.normalize();
00132
00133 Eigen::Matrix3d R_des;
00134 R_des.col(0) = b2_des.cross(b3_des);
00135 R_des.col(1) = b2_des;
00136 R_des.col(2) = b3_des;
00137
00138
00139 Eigen::Matrix3d angle_error_matrix = 0.5 * (R_des.transpose() * R - R.transpose() * R_des);
00140 Eigen::Vector3d angle_error;
00141 vectorFromSkewMatrix(angle_error_matrix, &angle_error);
00142
00143
00144 Eigen::Vector3d angular_rate_des(Eigen::Vector3d::Zero());
00145 angular_rate_des[2] = command_trajectory_.getYawRate();
00146
00147 Eigen::Vector3d angular_rate_error = odometry_.angular_velocity - R_des.transpose() * R * angular_rate_des;
00148
00149 *angular_acceleration = -1 * angle_error.cwiseProduct(normalized_attitude_gain_)
00150 - angular_rate_error.cwiseProduct(normalized_angular_rate_gain_)
00151 + odometry_.angular_velocity.cross(odometry_.angular_velocity);
00152 }
00153 }
rotors_control
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:38