Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
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
00035 static const std::string kDefaultNamespace = "";
00036 static const std::string kDefaultCommandMotorSpeedTopic =
00037 mav_msgs::default_topics::COMMAND_ACTUATORS;
00038 static const std::string kDefaultCommandMultiDofJointTrajectoryTopic =
00039 mav_msgs::default_topics::COMMAND_TRAJECTORY;
00040 static const std::string kDefaultCommandRollPitchYawrateThrustTopic =
00041 mav_msgs::default_topics::COMMAND_ROLL_PITCH_YAWRATE_THRUST;
00042
00043 static const std::string kDefaultImuTopic =
00044 mav_msgs::default_topics::IMU;
00045 static const std::string kDefaultOdometryTopic =
00046 mav_msgs::default_topics::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;
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
00086 (*allocation_matrix)(0, i) = sin(rotor.angle) * rotor.arm_length
00087 * rotor.rotor_force_constant;
00088
00089 (*allocation_matrix)(1, i) = -cos(rotor.angle) * rotor.arm_length
00090 * rotor.rotor_force_constant;
00091
00092 (*allocation_matrix)(2, i) = -rotor.direction * rotor.rotor_force_constant
00093 * rotor.rotor_moment_constant;
00094
00095 (*allocation_matrix)(3, i) = rotor.rotor_force_constant;
00096 ++i;
00097 }
00098 Eigen::FullPivLU<Eigen::Matrix4Xd> lu(*allocation_matrix);
00099
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
rotors_control
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:38