common.h
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 Markus Achtelik, ASL, ETH Zurich, Switzerland
00005  * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland
00006  * Copyright 2015 Mina Kamel, 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 // Common conversion functions between geometry messages, Eigen types, and yaw.
00022 
00023 #ifndef MAV_MSGS_COMMON_H
00024 #define MAV_MSGS_COMMON_H
00025 
00026 #include <geometry_msgs/Point.h>
00027 #include <geometry_msgs/Quaternion.h>
00028 #include <geometry_msgs/Vector3.h>
00029 #include <Eigen/Geometry>
00030 
00031 namespace mav_msgs {
00032 
00033 const double kNumNanosecondsPerSecond = 1.e9;
00034 
00037 inline double MagnitudeOfGravity(const double height,
00038                                  const double latitude_radians) {
00039   // gravity calculation constants
00040   const double kGravity_0 = 9.780327;
00041   const double kGravity_a = 0.0053024;
00042   const double kGravity_b = 0.0000058;
00043   const double kGravity_c = 3.155 * 1e-7;
00044 
00045   double sin_squared_latitude = sin(latitude_radians) * sin(latitude_radians);
00046   double sin_squared_twice_latitude =
00047       sin(2 * latitude_radians) * sin(2 * latitude_radians);
00048   return kGravity_0 * ((1 + kGravity_a * sin_squared_latitude -
00049                         kGravity_b * sin_squared_twice_latitude) -
00050                        kGravity_c * height);
00051 }
00052 
00053 inline Eigen::Vector3d vector3FromMsg(const geometry_msgs::Vector3& msg) {
00054   return Eigen::Vector3d(msg.x, msg.y, msg.z);
00055 }
00056 
00057 inline Eigen::Vector3d vector3FromPointMsg(const geometry_msgs::Point& msg) {
00058   return Eigen::Vector3d(msg.x, msg.y, msg.z);
00059 }
00060 
00061 inline Eigen::Quaterniond quaternionFromMsg(
00062     const geometry_msgs::Quaternion& msg) {
00063   // Make sure this always returns a valid Quaternion, even if the message was
00064   // uninitialized or only approximately set.
00065   Eigen::Quaterniond quaternion(msg.w, msg.x, msg.y, msg.z);
00066   if (quaternion.norm() < std::numeric_limits<double>::epsilon()) {
00067     quaternion.setIdentity();
00068   } else {
00069     quaternion.normalize();
00070   }
00071   return quaternion;
00072 }
00073 
00074 inline void vectorEigenToMsg(const Eigen::Vector3d& eigen,
00075                              geometry_msgs::Vector3* msg) {
00076   assert(msg != NULL);
00077   msg->x = eigen.x();
00078   msg->y = eigen.y();
00079   msg->z = eigen.z();
00080 }
00081 
00082 inline void pointEigenToMsg(const Eigen::Vector3d& eigen,
00083                             geometry_msgs::Point* msg) {
00084   assert(msg != NULL);
00085   msg->x = eigen.x();
00086   msg->y = eigen.y();
00087   msg->z = eigen.z();
00088 }
00089 
00090 inline void quaternionEigenToMsg(const Eigen::Quaterniond& eigen,
00091                                  geometry_msgs::Quaternion* msg) {
00092   assert(msg != NULL);
00093   msg->x = eigen.x();
00094   msg->y = eigen.y();
00095   msg->z = eigen.z();
00096   msg->w = eigen.w();
00097 }
00098 
00105 inline double yawFromQuaternion(const Eigen::Quaterniond& q) {
00106   return atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
00107                1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
00108 }
00109 
00110 inline Eigen::Quaterniond quaternionFromYaw(double yaw) {
00111   return Eigen::Quaterniond(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
00112 }
00113 
00114 inline void setQuaternionMsgFromYaw(double yaw,
00115                                     geometry_msgs::Quaternion* msg) {
00116   assert(msg != NULL);
00117   Eigen::Quaterniond q_yaw = quaternionFromYaw(yaw);
00118   msg->x = q_yaw.x();
00119   msg->y = q_yaw.y();
00120   msg->z = q_yaw.z();
00121   msg->w = q_yaw.w();
00122 }
00123 
00124 inline void setAngularVelocityMsgFromYawRate(double yaw_rate,
00125                                              geometry_msgs::Vector3* msg) {
00126   assert(msg != NULL);
00127   msg->x = 0.0;
00128   msg->y = 0.0;
00129   msg->z = yaw_rate;
00130 }
00131 
00132 inline void getEulerAnglesFromQuaternion(const Eigen::Quaternion<double>& q,
00133                                          Eigen::Vector3d* euler_angles) {
00134   {
00135     assert(euler_angles != NULL);
00136 
00137     *euler_angles << atan2(2.0 * (q.w() * q.x() + q.y() * q.z()),
00138                            1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())),
00139         asin(2.0 * (q.w() * q.y() - q.z() * q.x())),
00140         atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
00141               1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
00142   }
00143 }
00144 
00145 // Calculate the nominal rotor rates given the MAV mass, allocation matrix,
00146 // angular velocity, angular acceleration, and body acceleration (normalized
00147 // thrust).
00148 //
00149 // [torques, thrust]' = A * n^2, where
00150 // torques = J * ang_acc + ang_vel x J
00151 // thrust = m * norm(acc)
00152 //
00153 // The allocation matrix A has of a hexacopter is:
00154 // A = K * B, where
00155 // K = diag(l*c_T, l*c_T, c_M, c_T),
00156 //     [ s  1  s -s -1 -s]
00157 // B = [-c  0  c  c  0 -c]
00158 //     [-1  1 -1  1 -1  1]
00159 //     [ 1  1  1  1  1  1],
00160 // l: arm length
00161 // c_T: thrust constant
00162 // c_M: moment constant
00163 // s: sin(30°)
00164 // c: cos(30°)
00165 //
00166 // The inverse can be computed computationally efficient:
00167 // A^-1 \approx B^pseudo * K^-1
00168 inline void getSquaredRotorSpeedsFromAllocationAndState(
00169     const Eigen::MatrixXd& allocation_inv, const Eigen::Vector3d& inertia,
00170     double mass, const Eigen::Vector3d& angular_velocity_B,
00171     const Eigen::Vector3d& angular_acceleration_B,
00172     const Eigen::Vector3d& acceleration_B,
00173     Eigen::VectorXd* rotor_rates_squared) {
00174   const Eigen::Vector3d torque =
00175       inertia.asDiagonal() * angular_acceleration_B +
00176       angular_velocity_B.cross(inertia.asDiagonal() * angular_velocity_B);
00177   const double thrust_force = mass * acceleration_B.norm();
00178   Eigen::Vector4d input;
00179   input << torque, thrust_force;
00180   *rotor_rates_squared = allocation_inv * input;
00181 }
00182 
00183 inline double nanosecondsToSeconds(int64_t nanoseconds) {
00184   double seconds = nanoseconds / kNumNanosecondsPerSecond;
00185   return seconds;
00186 }
00187 
00188 inline int64_t secondsToNanoseconds(double seconds) {
00189   int64_t nanoseconds =
00190       static_cast<int64_t>(seconds * kNumNanosecondsPerSecond);
00191   return nanoseconds;
00192 }
00193 
00194 }  // namespace mav_msgs
00195 
00196 #endif  // MAV_MSGS_COMMON_H


mav_msgs
Author(s): Simon Lynen, Markus Achtelik, Pascal Gohl, Sammy Omari, Michael Burri, Fadri Furrer, Helen Oleynikova, Mina Kamel, Karen Bodie, Rik Bähnemann
autogenerated on Fri Jun 14 2019 19:31:56