quadrotor_aerodynamics.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_quadrotor_model/quadrotor_aerodynamics.h>
00030 #include <hector_quadrotor_model/helpers.h>
00031 
00032 #include <ros/node_handle.h>
00033 #include <boost/array.hpp>
00034 
00035 //extern "C" {
00036 //  #include "quadrotorDrag/quadrotorDrag.h"
00037 //  #include "quadrotorDrag/quadrotorDrag_initialize.h"
00038 //}
00039 
00040 #include <boost/array.hpp>
00041 #include <Eigen/Geometry>
00042 
00043 #include "matlab_helpers.h"
00044 
00045 namespace hector_quadrotor_model {
00046 
00047 struct DragParameters
00048 {
00049     real_T C_wxy;
00050     real_T C_wz;
00051     real_T C_mxy;
00052     real_T C_mz;
00053 
00054     DragParameters()
00055       : C_wxy(0.0)
00056       , C_wz(0.0)
00057       , C_mxy(0.0)
00058       , C_mz(0.0)
00059     {}
00060 };
00061 
00062 // extern void quadrotorDrag(const real_T uin[6], const DragParameters parameter, real_T dt, real_T y[6]);
00063 struct QuadrotorAerodynamics::DragModel {
00064   DragParameters parameters_;
00065   boost::array<real_T,6> u;
00066   boost::array<real_T,6> y;
00067 };
00068 
00069 QuadrotorAerodynamics::QuadrotorAerodynamics()
00070 {
00071   // initialize drag model
00072   // quadrotorDrag_initialize();
00073   drag_model_ = new DragModel;
00074 }
00075 
00076 QuadrotorAerodynamics::~QuadrotorAerodynamics()
00077 {
00078   delete drag_model_;
00079 }
00080 
00081 /*
00082  * quadrotorDrag.c
00083  *
00084  * Code generation for function 'quadrotorDrag'
00085  *
00086  * C source code generated on: Sun Nov  3 13:34:38 2013
00087  *
00088  */
00089 
00090 /* Include files */
00091 //#include "rt_nonfinite.h"
00092 //#include "quadrotorDrag.h"
00093 
00094 /* Type Definitions */
00095 
00096 /* Named Constants */
00097 
00098 /* Variable Declarations */
00099 
00100 /* Variable Definitions */
00101 
00102 /* Function Definitions */
00103 void quadrotorDrag(const real_T uin[6], const DragParameters parameter, real_T
00104                    dt, real_T y[6])
00105 {
00106   int32_T i;
00107   real_T absoluteVelocity;
00108   real_T absoluteAngularVelocity;
00109 
00110   /*  initialize vectors */
00111   for (i = 0; i < 6; i++) {
00112     y[i] = 0.0;
00113   }
00114 
00115   /*  Input variables */
00116   /*  Constants */
00117   /*  temporarily used vector */
00118   absoluteVelocity = sqrt((rt_powd_snf(uin[0], 2.0) + rt_powd_snf(uin[1], 2.0))
00119     + rt_powd_snf(uin[2], 2.0));
00120   absoluteAngularVelocity = sqrt((rt_powd_snf(uin[3], 2.0) + rt_powd_snf(uin[4],
00121     2.0)) + rt_powd_snf(uin[5], 2.0));
00122 
00123   /*  system outputs */
00124   /*  calculate drag force */
00125   y[0] = parameter.C_wxy * absoluteVelocity * uin[0];
00126   y[1] = parameter.C_wxy * absoluteVelocity * uin[1];
00127   y[2] = parameter.C_wz * absoluteVelocity * uin[2];
00128 
00129   /*  calculate draq torque */
00130   y[3] = parameter.C_mxy * absoluteAngularVelocity * uin[3];
00131   y[4] = parameter.C_mxy * absoluteAngularVelocity * uin[4];
00132   y[5] = parameter.C_mz * absoluteAngularVelocity * uin[5];
00133 }
00134 
00135 /* End of code generation (quadrotorDrag.c) */
00136 
00137 inline void QuadrotorAerodynamics::f(const double uin[10], double dt, double y[14]) const
00138 {
00139   quadrotorDrag(uin, drag_model_->parameters_, dt, y);
00140 }
00141 
00142 bool QuadrotorAerodynamics::configure(const ros::NodeHandle &param)
00143 {
00144   // get model parameters
00145   if (!param.getParam("C_wxy", drag_model_->parameters_.C_wxy)) return false;
00146   if (!param.getParam("C_wz",  drag_model_->parameters_.C_wz)) return false;
00147   if (!param.getParam("C_mxy", drag_model_->parameters_.C_mxy)) return false;
00148   if (!param.getParam("C_mz",  drag_model_->parameters_.C_mz)) return false;
00149 
00150 #ifndef NDEBUG
00151   std::cout << "Loaded the following quadrotor drag model parameters from namespace " << param.getNamespace() << ":" << std::endl;
00152   std::cout << "C_wxy = " << drag_model_->parameters_.C_wxy << std::endl;
00153   std::cout << "C_wz = "  << drag_model_->parameters_.C_wz << std::endl;
00154   std::cout << "C_mxy = " << drag_model_->parameters_.C_mxy << std::endl;
00155   std::cout << "C_mz = "  << drag_model_->parameters_.C_mz << std::endl;
00156 #endif
00157 
00158   reset();
00159   return true;
00160 }
00161 
00162 void QuadrotorAerodynamics::reset()
00163 {
00164   boost::mutex::scoped_lock lock(mutex_);
00165   drag_model_->u.assign(0.0);
00166   drag_model_->y.assign(0.0);
00167 
00168   twist_ = geometry_msgs::Twist();
00169   wind_ = geometry_msgs::Vector3();
00170   wrench_ = geometry_msgs::Wrench();
00171 }
00172 
00173 void QuadrotorAerodynamics::setOrientation(const geometry_msgs::Quaternion& orientation)
00174 {
00175   boost::mutex::scoped_lock lock(mutex_);
00176   orientation_ = orientation;
00177 }
00178 
00179 void QuadrotorAerodynamics::setTwist(const geometry_msgs::Twist& twist)
00180 {
00181   boost::mutex::scoped_lock lock(mutex_);
00182   twist_ = twist;
00183 }
00184 
00185 void QuadrotorAerodynamics::setBodyTwist(const geometry_msgs::Twist& body_twist)
00186 {
00187   boost::mutex::scoped_lock lock(mutex_);
00188   Eigen::Quaterniond orientation(orientation_.w, orientation_.x, orientation_.y, orientation_.z);
00189   Eigen::Matrix<double,3,3> inverse_rotation_matrix(orientation.inverse().toRotationMatrix());
00190 
00191   Eigen::Vector3d body_linear(body_twist.linear.x, body_twist.linear.y, body_twist.linear.z);
00192   Eigen::Vector3d world_linear(inverse_rotation_matrix * body_linear);
00193   twist_.linear.x = world_linear.x();
00194   twist_.linear.y = world_linear.y();
00195   twist_.linear.z = world_linear.z();
00196 
00197   Eigen::Vector3d body_angular(body_twist.angular.x, body_twist.angular.y, body_twist.angular.z);
00198   Eigen::Vector3d world_angular(inverse_rotation_matrix * body_angular);
00199   twist_.angular.x = world_angular.x();
00200   twist_.angular.y = world_angular.y();
00201   twist_.angular.z = world_angular.z();
00202 }
00203 
00204 void QuadrotorAerodynamics::setWind(const geometry_msgs::Vector3& wind)
00205 {
00206   boost::mutex::scoped_lock lock(mutex_);
00207   wind_ = wind;
00208 }
00209 
00210 void QuadrotorAerodynamics::update(double dt)
00211 {
00212   if (dt <= 0.0) return;
00213   boost::mutex::scoped_lock lock(mutex_);
00214 
00215   // fill input vector u for drag model
00216   drag_model_->u[0] =  (twist_.linear.x - wind_.x);
00217   drag_model_->u[1] = -(twist_.linear.y - wind_.y);
00218   drag_model_->u[2] = -(twist_.linear.z - wind_.z);
00219   drag_model_->u[3] =  twist_.angular.x;
00220   drag_model_->u[4] = -twist_.angular.y;
00221   drag_model_->u[5] = -twist_.angular.z;
00222 
00223   // We limit the input velocities to +-100. Required for numeric stability in case of collisions,
00224   // where velocities returned by Gazebo can be very high.
00225   limit(drag_model_->u, -100.0, 100.0);
00226 
00227   // convert input to body coordinates
00228   Eigen::Quaterniond orientation(orientation_.w, orientation_.x, orientation_.y, orientation_.z);
00229   Eigen::Matrix<double,3,3> rotation_matrix(orientation.toRotationMatrix());
00230   Eigen::Map<Eigen::Vector3d> linear(&(drag_model_->u[0]));
00231   Eigen::Map<Eigen::Vector3d> angular(&(drag_model_->u[3]));
00232   linear  = rotation_matrix * linear;
00233   angular = rotation_matrix * angular;
00234 
00235   ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.twist:  " << PrintVector<double>(drag_model_->u.begin(), drag_model_->u.begin() + 6));
00236   checknan(drag_model_->u, "drag model input");
00237 
00238   // update drag model
00239   f(drag_model_->u.data(), dt, drag_model_->y.data());
00240 
00241   ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.force:  " << PrintVector<double>(drag_model_->y.begin() + 0, drag_model_->y.begin() + 3));
00242   ROS_DEBUG_STREAM_NAMED("quadrotor_aerodynamics", "aerodynamics.torque: " << PrintVector<double>(drag_model_->y.begin() + 3, drag_model_->y.begin() + 6));
00243   checknan(drag_model_->y, "drag model output");
00244 
00245   // drag_model_ gives us inverted vectors!
00246   wrench_.force.x  = -( drag_model_->y[0]);
00247   wrench_.force.y  = -(-drag_model_->y[1]);
00248   wrench_.force.z  = -(-drag_model_->y[2]);
00249   wrench_.torque.x = -( drag_model_->y[3]);
00250   wrench_.torque.y = -(-drag_model_->y[4]);
00251   wrench_.torque.z = -(-drag_model_->y[5]);
00252 }
00253 
00254 } // namespace hector_quadrotor_model


hector_quadrotor_model
Author(s): Johannes Meyer , Alexander Sendobry
autogenerated on Thu Jun 6 2019 21:32:59