lee_position_controller.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2015 Fadri Furrer, ASL, ETH Zurich, Switzerland
00003  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
00004  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
00007  *
00008  * Licensed under the Apache License, Version 2.0 (the "License");
00009  * you may not use this file except in compliance with the License.
00010  * You may obtain a copy of the License at
00011  *
00012  *     http://www.apache.org/licenses/LICENSE-2.0
00013 
00014  * Unless required by applicable law or agreed to in writing, software
00015  * distributed under the License is distributed on an "AS IS" BASIS,
00016  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00017  * See the License for the specific language governing permissions and
00018  * limitations under the License.
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   // To make the tuning independent of the inertia matrix we divide here.
00036   normalized_attitude_gain_ = controller_parameters_.attitude_gain_.transpose()
00037       * vehicle_parameters_.inertia_.inverse();
00038   // To make the tuning independent of the inertia matrix we divide here.
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   // Calculate the pseude-inverse A^{ \dagger} and then multiply by the inertia matrix I.
00048   // A^{ \dagger} = A^T*(A*A^T)^{-1}
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   // Return 0 velocities on all rotors, until the first command is received.
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   // Project thrust onto body z axis.
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   // Transform velocity to world frame.
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 // Implementation from the T. Lee et al. paper
00114 // Control of complex maneuvers for a quadrotor UAV using geometric methods on SE(3)
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   // Get the desired rotation matrix.
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   // Angle error according to lee et al.
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   // TODO(burrimi) include angular rate references at some point.
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); // we don't need the inertia matrix here
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