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 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 #ifndef INCLUDE_ROTORS_CONTROL_COMMON_H_
00022 #define INCLUDE_ROTORS_CONTROL_COMMON_H_
00023 
00024 #include <assert.h>
00025 
00026 #include <mav_msgs/conversions.h>
00027 #include <mav_msgs/default_topics.h>
00028 #include <nav_msgs/Odometry.h>
00029 
00030 #include "rotors_control/parameters.h"
00031 
00032 namespace rotors_control {
00033 
00034 // Default values.
00035 static const std::string kDefaultNamespace = "";
00036 static const std::string kDefaultCommandMotorSpeedTopic =
00037     mav_msgs::default_topics::COMMAND_ACTUATORS; // "command/motor_speed";
00038 static const std::string kDefaultCommandMultiDofJointTrajectoryTopic =
00039     mav_msgs::default_topics::COMMAND_TRAJECTORY; // "command/trajectory"
00040 static const std::string kDefaultCommandRollPitchYawrateThrustTopic =
00041     mav_msgs::default_topics::COMMAND_ROLL_PITCH_YAWRATE_THRUST;
00042     // "command/roll_pitch_yawrate_thrust"
00043 static const std::string kDefaultImuTopic =
00044     mav_msgs::default_topics::IMU; // "imu
00045 static const std::string kDefaultOdometryTopic =
00046     mav_msgs::default_topics::ODOMETRY; // "odometry"
00047 
00048 struct EigenOdometry {
00049   EigenOdometry()
00050       : position(0.0, 0.0, 0.0),
00051         orientation(Eigen::Quaterniond::Identity()),
00052         velocity(0.0, 0.0, 0.0),
00053         angular_velocity(0.0, 0.0, 0.0) {};
00054 
00055   EigenOdometry(const Eigen::Vector3d& _position,
00056                 const Eigen::Quaterniond& _orientation,
00057                 const Eigen::Vector3d& _velocity,
00058                 const Eigen::Vector3d& _angular_velocity) {
00059     position = _position;
00060     orientation = _orientation;
00061     velocity = _velocity;
00062     angular_velocity = _angular_velocity;
00063   };
00064 
00065   Eigen::Vector3d position;
00066   Eigen::Quaterniond orientation;
00067   Eigen::Vector3d velocity; // Velocity is expressed in the Body frame!
00068   Eigen::Vector3d angular_velocity;
00069 };
00070 
00071 inline void eigenOdometryFromMsg(const nav_msgs::OdometryConstPtr& msg,
00072                                  EigenOdometry* odometry) {
00073   odometry->position = mav_msgs::vector3FromPointMsg(msg->pose.pose.position);
00074   odometry->orientation = mav_msgs::quaternionFromMsg(msg->pose.pose.orientation);
00075   odometry->velocity = mav_msgs::vector3FromMsg(msg->twist.twist.linear);
00076   odometry->angular_velocity = mav_msgs::vector3FromMsg(msg->twist.twist.angular);
00077 }
00078 
00079 inline void calculateAllocationMatrix(const RotorConfiguration& rotor_configuration,
00080                                       Eigen::Matrix4Xd* allocation_matrix) {
00081   assert(allocation_matrix != nullptr);
00082   allocation_matrix->resize(4, rotor_configuration.rotors.size());
00083   unsigned int i = 0;
00084   for (const Rotor& rotor : rotor_configuration.rotors) {
00085     // Set first row of allocation matrix.
00086     (*allocation_matrix)(0, i) = sin(rotor.angle) * rotor.arm_length
00087                                  * rotor.rotor_force_constant;
00088     // Set second row of allocation matrix.
00089     (*allocation_matrix)(1, i) = -cos(rotor.angle) * rotor.arm_length
00090                                  * rotor.rotor_force_constant;
00091     // Set third row of allocation matrix.
00092     (*allocation_matrix)(2, i) = -rotor.direction * rotor.rotor_force_constant
00093                                  * rotor.rotor_moment_constant;
00094     // Set forth row of allocation matrix.
00095     (*allocation_matrix)(3, i) = rotor.rotor_force_constant;
00096     ++i;
00097   }
00098   Eigen::FullPivLU<Eigen::Matrix4Xd> lu(*allocation_matrix);
00099   // Setting the threshold for when pivots of the rank calculation should be considered nonzero.
00100   lu.setThreshold(1e-9);
00101   int rank = lu.rank();
00102   if (rank < 4) {
00103     std::cout << "The rank of the allocation matrix is " << lu.rank()
00104               << ", it should have rank 4, to have a fully controllable system,"
00105               << " check your configuration." << std::endl;
00106   }
00107 
00108 }
00109 
00110 inline void skewMatrixFromVector(Eigen::Vector3d& vector, Eigen::Matrix3d* skew_matrix) {
00111   *skew_matrix << 0, -vector.z(), vector.y(),
00112                   vector.z(), 0, -vector.x(),
00113                   -vector.y(), vector.x(), 0;
00114 }
00115 
00116 inline void vectorFromSkewMatrix(Eigen::Matrix3d& skew_matrix, Eigen::Vector3d* vector) {
00117   *vector << skew_matrix(2, 1), skew_matrix(0,2), skew_matrix(1, 0);
00118 }
00119 }
00120 
00121 #endif /* INCLUDE_ROTORS_CONTROL_COMMON_H_ */


rotors_control
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:38