common.h
Go to the documentation of this file.
1 /*
3  * Copyright 2015 Michael Burri, ASL, ETH Zurich, Switzerland
4  * Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland
5  * Copyright 2015 Helen Oleynikova, ASL, ETH Zurich, Switzerland
6  * Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland
7  *
9  * you may not use this file except in compliance with the License.
10  * You may obtain a copy of the License at
11  *
13
14  * Unless required by applicable law or agreed to in writing, software
16  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
17  * See the License for the specific language governing permissions and
18  * limitations under the License.
19  */
20
21 // Common conversion functions between geometry messages, Eigen types, and yaw.
22
23 #ifndef MAV_MSGS_COMMON_H
24 #define MAV_MSGS_COMMON_H
25
26 #include <geometry_msgs/Point.h>
27 #include <geometry_msgs/Quaternion.h>
28 #include <geometry_msgs/Vector3.h>
29 #include <Eigen/Geometry>
30
31 namespace mav_msgs {
32
33 const double kNumNanosecondsPerSecond = 1.e9;
34
37 inline double MagnitudeOfGravity(const double height,
39  // gravity calculation constants
40  const double kGravity_0 = 9.780327;
41  const double kGravity_a = 0.0053024;
42  const double kGravity_b = 0.0000058;
43  const double kGravity_c = 3.155 * 1e-7;
44
46  double sin_squared_twice_latitude =
48  return kGravity_0 * ((1 + kGravity_a * sin_squared_latitude -
49  kGravity_b * sin_squared_twice_latitude) -
50  kGravity_c * height);
51 }
52
53 inline Eigen::Vector3d vector3FromMsg(const geometry_msgs::Vector3& msg) {
54  return Eigen::Vector3d(msg.x, msg.y, msg.z);
55 }
56
57 inline Eigen::Vector3d vector3FromPointMsg(const geometry_msgs::Point& msg) {
58  return Eigen::Vector3d(msg.x, msg.y, msg.z);
59 }
60
61 inline Eigen::Quaterniond quaternionFromMsg(
62  const geometry_msgs::Quaternion& msg) {
63  // Make sure this always returns a valid Quaternion, even if the message was
64  // uninitialized or only approximately set.
65  Eigen::Quaterniond quaternion(msg.w, msg.x, msg.y, msg.z);
66  if (quaternion.norm() < std::numeric_limits<double>::epsilon()) {
67  quaternion.setIdentity();
68  } else {
69  quaternion.normalize();
70  }
71  return quaternion;
72 }
73
74 inline void vectorEigenToMsg(const Eigen::Vector3d& eigen,
75  geometry_msgs::Vector3* msg) {
76  assert(msg != NULL);
77  msg->x = eigen.x();
78  msg->y = eigen.y();
79  msg->z = eigen.z();
80 }
81
82 inline void pointEigenToMsg(const Eigen::Vector3d& eigen,
83  geometry_msgs::Point* msg) {
84  assert(msg != NULL);
85  msg->x = eigen.x();
86  msg->y = eigen.y();
87  msg->z = eigen.z();
88 }
89
90 inline void quaternionEigenToMsg(const Eigen::Quaterniond& eigen,
91  geometry_msgs::Quaternion* msg) {
92  assert(msg != NULL);
93  msg->x = eigen.x();
94  msg->y = eigen.y();
95  msg->z = eigen.z();
96  msg->w = eigen.w();
97 }
98
105 inline double yawFromQuaternion(const Eigen::Quaterniond& q) {
106  return atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
107  1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
108 }
109
110 inline Eigen::Quaterniond quaternionFromYaw(double yaw) {
111  return Eigen::Quaterniond(Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()));
112 }
113
114 inline void setQuaternionMsgFromYaw(double yaw,
115  geometry_msgs::Quaternion* msg) {
116  assert(msg != NULL);
117  Eigen::Quaterniond q_yaw = quaternionFromYaw(yaw);
118  msg->x = q_yaw.x();
119  msg->y = q_yaw.y();
120  msg->z = q_yaw.z();
121  msg->w = q_yaw.w();
122 }
123
124 inline void setAngularVelocityMsgFromYawRate(double yaw_rate,
125  geometry_msgs::Vector3* msg) {
126  assert(msg != NULL);
127  msg->x = 0.0;
128  msg->y = 0.0;
129  msg->z = yaw_rate;
130 }
131
132 inline void getEulerAnglesFromQuaternion(const Eigen::Quaternion<double>& q,
133  Eigen::Vector3d* euler_angles) {
134  {
135  assert(euler_angles != NULL);
136
137  *euler_angles << atan2(2.0 * (q.w() * q.x() + q.y() * q.z()),
138  1.0 - 2.0 * (q.x() * q.x() + q.y() * q.y())),
139  asin(2.0 * (q.w() * q.y() - q.z() * q.x())),
140  atan2(2.0 * (q.w() * q.z() + q.x() * q.y()),
141  1.0 - 2.0 * (q.y() * q.y() + q.z() * q.z()));
142  }
143 }
144
145 // Calculate the nominal rotor rates given the MAV mass, allocation matrix,
146 // angular velocity, angular acceleration, and body acceleration (normalized
147 // thrust).
148 //
149 // [torques, thrust]' = A * n^2, where
150 // torques = J * ang_acc + ang_vel x J
151 // thrust = m * norm(acc)
152 //
153 // The allocation matrix A has of a hexacopter is:
154 // A = K * B, where
155 // K = diag(l*c_T, l*c_T, c_M, c_T),
156 // [ s 1 s -s -1 -s]
157 // B = [-c 0 c c 0 -c]
158 // [-1 1 -1 1 -1 1]
159 // [ 1 1 1 1 1 1],
160 // l: arm length
161 // c_T: thrust constant
162 // c_M: moment constant
163 // s: sin(30°)
164 // c: cos(30°)
165 //
166 // The inverse can be computed computationally efficient:
167 // A^-1 \approx B^pseudo * K^-1
169  const Eigen::MatrixXd& allocation_inv, const Eigen::Vector3d& inertia,
170  double mass, const Eigen::Vector3d& angular_velocity_B,
171  const Eigen::Vector3d& angular_acceleration_B,
172  const Eigen::Vector3d& acceleration_B,
173  Eigen::VectorXd* rotor_rates_squared) {
174  const Eigen::Vector3d torque =
175  inertia.asDiagonal() * angular_acceleration_B +
176  angular_velocity_B.cross(inertia.asDiagonal() * angular_velocity_B);
177  const double thrust_force = mass * acceleration_B.norm();
178  Eigen::Vector4d input;
179  input << torque, thrust_force;
180  *rotor_rates_squared = allocation_inv * input;
181 }
182
183 inline double nanosecondsToSeconds(int64_t nanoseconds) {
184  double seconds = nanoseconds / kNumNanosecondsPerSecond;
185  return seconds;
186 }
187
188 inline int64_t secondsToNanoseconds(double seconds) {
189  int64_t nanoseconds =
190  static_cast<int64_t>(seconds * kNumNanosecondsPerSecond);
191  return nanoseconds;
192 }
193
194 } // namespace mav_msgs
195
196 #endif // MAV_MSGS_COMMON_H
void quaternionEigenToMsg(const Eigen::Quaterniond &eigen, geometry_msgs::Quaternion *msg)
Definition: common.h:90
double MagnitudeOfGravity(const double height, const double latitude_radians)
Definition: common.h:37
void setAngularVelocityMsgFromYawRate(double yaw_rate, geometry_msgs::Vector3 *msg)
Definition: common.h:124
void getSquaredRotorSpeedsFromAllocationAndState(const Eigen::MatrixXd &allocation_inv, const Eigen::Vector3d &inertia, double mass, const Eigen::Vector3d &angular_velocity_B, const Eigen::Vector3d &angular_acceleration_B, const Eigen::Vector3d &acceleration_B, Eigen::VectorXd *rotor_rates_squared)
Definition: common.h:168
Eigen::Vector3d vector3FromMsg(const geometry_msgs::Vector3 &msg)
Definition: common.h:53
void setQuaternionMsgFromYaw(double yaw, geometry_msgs::Quaternion *msg)
Definition: common.h:114
const double kNumNanosecondsPerSecond
Definition: common.h:33
void getEulerAnglesFromQuaternion(const Eigen::Quaternion< double > &q, Eigen::Vector3d *euler_angles)
Definition: common.h:132
int64_t secondsToNanoseconds(double seconds)
Definition: common.h:188
double nanosecondsToSeconds(int64_t nanoseconds)
Definition: common.h:183
double yawFromQuaternion(const Eigen::Quaterniond &q)
Extracts the yaw part from a quaternion, using RPY / euler (z-y&#39;-z&#39;&#39;) angles. RPY rotates about the f...
Definition: common.h:105
void vectorEigenToMsg(const Eigen::Vector3d &eigen, geometry_msgs::Vector3 *msg)
Definition: common.h:74
Eigen::Quaterniond quaternionFromMsg(const geometry_msgs::Quaternion &msg)
Definition: common.h:61
Eigen::Vector3d vector3FromPointMsg(const geometry_msgs::Point &msg)
Definition: common.h:57
Eigen::Quaterniond quaternionFromYaw(double yaw)
Definition: common.h:110
void pointEigenToMsg(const Eigen::Vector3d &eigen, geometry_msgs::Point *msg)
Definition: common.h:82

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 Sat Jun 15 2019 19:55:13