gsilano · gsilano · Apr 22, 2020 · Feb 2, 2020 · Feb 8, 2020 · Feb 8, 2020
diff --git a/rotors_comm/CHANGELOG.rst b/rotors_comm/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package rotors_comm
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.2 (20XX-XX-XX)
+------------------
+
 6.0.1 (2019-12-28)
 ------------------
 

diff --git a/rotors_control/CHANGELOG.rst b/rotors_control/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package rotors_control
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.2 (20XX-XX-XX)
+------------------
+
 6.0.2 (2020-01-18)
 ------------------
 * Fix typo in the position_controller_node with the enable_state_estimator variable #24
@@ -46,4 +49,3 @@ Changelog for package rotors_control
 * added Crazyflie 2.0 position controller. The lower level controller is the same of the Crazyflie 2.0 firmware (released 2018.01.01)
 * started from the 2.1.1 (2017-04-27) release version of RotorS
 * Contributors: Giuseppe Silano, Emanuele Aucone, Benjamin Rodriguez, Luigi Iannelli
-
diff --git a/rotors_description/CHANGELOG.rst b/rotors_description/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package rotors_description
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.2 (20XX-XX-XX)
+------------------
+
 6.0.1 (2019-12-28)
 ------------------
 * Deleted pi constant in the component_snippest file.

diff --git a/rotors_evaluation/CHANGELOG.rst b/rotors_evaluation/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package rotors_evaluation
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.2 (20XX-XX-XX)
+------------------
+
 6.0.1 (2019-12-28)
 ------------------
 

diff --git a/rotors_gazebo/CHANGELOG.rst b/rotors_gazebo/CHANGELOG.rst
@@ -2,6 +2,13 @@
 Changelog for package rotors_gazebo
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+6.0.2 (20XX-XX-XX)
+------------------
+* Add spline trajectory generator
+* Add launch files to run the Internal Model and Mellinger's controllers
+* Add resource files for the above controllers and trajectory generator
+* Contributors: Ria Sonecha, Giuseppe Silano
+
 6.0.1 (2019-12-28)
 ------------------
 * Fix issue related to "xacro.py is deprecated; please use xacro instead"

diff --git a/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h b/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h
@@ -0,0 +1,83 @@
+/*
+ * Copyright 2020 Ria Sonecha, MIT, USA
+ * Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+ #ifndef HOVERING_EXAMPLE_SPLINE_H
+ #define HOVERING_EXAMPLE_SPLINE_H
+
+ #include <boost/bind.hpp>
+ #include <Eigen/Eigen>
+ #include <Eigen/Core>
+ #include <math.h>
+ #include <stdio.h>
+
+ // package libraries
+ #include "rotors_gazebo/Matrix3x3.h"
+ #include "rotors_gazebo/Quaternion.h"
+ #include "rotors_gazebo/transform_datatypes.h"
+
+ #include <nav_msgs/Odometry.h>
+ #include <mav_msgs/conversions.h>
+ #include <mav_msgs/default_topics.h>
+ #include <sensor_msgs/Imu.h>
+ #include <ros/callback_queue.h>
+ #include <ros/ros.h>
+ #include <ros/time.h>
+ #include <mav_msgs/DroneState.h>
+ #include <mav_msgs/eigen_mav_msgs.h>
+
+
+ namespace rotors_gazebo {
+
+ class HoveringExampleSpline{
+ public:
+
+ HoveringExampleSpline();
+ ~HoveringExampleSpline();
+
+ void TrajectoryCallback(mav_msgs::EigenDroneState* odometry, double* time_final, double* time_init);
+ void InitializeParams();
+ template<typename T> inline void GetRosParameterHovering(const ros::NodeHandle& nh,
+ const std::string& key,
+ const T& default_value,
+ T* value);
+
+ private:
+ bool enable_parameter_computation_ = true;
+ //polynomial coefficients
+ Eigen::Vector3f a0_, a1_, a2_, a3_, a4_, a5_;
+ Eigen::Vector3f b0_, b1_, b2_, b3_, b4_, b5_;
+ Eigen::Vector3f c0_, c1_, c2_, c3_, c4_, c5_;
+
+ Eigen::Vector3f g0_, g1_, g2_, g3_, g4_, g5_;
+ Eigen::Vector3f h0_, h1_, h2_, h3_, h4_, h5_;
+
+ // Desidred drone orientation expressed in radians
+ double rollDesRad_, pitchDesRad_, yawDesRad_;
+
+ // Parameters used for the trajectory generation
+ double time_final_;
+ Eigen::Vector3f position_initial_, position_final_, velocity_initial_, velocity_final_, acceleration_initial_, acceleration_final_;
+ Eigen::Vector3f orientation_initial_, orientation_final_, angular_velocity_initial_, angular_velocity_final_, angular_acceleration_initial_, angular_acceleration_final_;
+
+ void ComputeSplineParameters(double* time_spline);
+
+ void Euler2QuaternionCommandTrajectory(double* x, double* y, double* z, double* w) const;
+
+ };
+
+ }
+ #endif // HOVERING_EXAMPLE_SPLINE_H
diff --git a/rotors_gazebo/include/rotors_gazebo/parameters.h b/rotors_gazebo/include/rotors_gazebo/parameters.h
@@ -0,0 +1,92 @@
+/*
+ * Copyright 2020 Ria Sonecha, Massachusetts Institute of Technology in Cambridge, MA, USA
+ * Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef INCLUDE_ROTORS_GAZEBO_PARAMETERS_H_
+#define INCLUDE_ROTORS_GAZEBO_PARAMETERS_H_
+
+namespace rotors_gazebo {
+// Default values for the Asctec Firefly rotor configuration.
+static constexpr double kDefaultRotor0Angle = 0.52359877559;
+static constexpr double kDefaultRotor1Angle = 1.57079632679;
+static constexpr double kDefaultRotor2Angle = 2.61799387799;
+static constexpr double kDefaultRotor3Angle = -2.61799387799;
+static constexpr double kDefaultRotor4Angle = -1.57079632679;
+static constexpr double kDefaultRotor5Angle = -0.52359877559;
+
+// Default vehicle parameters for Asctec Firefly.
+static constexpr double kDefaultMass = 1.56779;
+static constexpr double kDefaultArmLength = 0.215;
+static constexpr double kDefaultInertiaXx = 0.0347563;
+static constexpr double kDefaultInertiaYy = 0.0458929;
+static constexpr double kDefaultInertiaZz = 0.0977;
+static constexpr double kDefaultRotorForceConstant = 8.54858e-6;
+static constexpr double kDefaultRotorMomentConstant = 1.6e-2;
+
+// Default physics parameters.
+static constexpr double kDefaultGravity = 9.81;
+
+struct Rotor {
+ Rotor()
+ : angle(0.0),
+ arm_length(kDefaultArmLength),
+ rotor_force_constant(kDefaultRotorForceConstant),
+ rotor_moment_constant(kDefaultRotorMomentConstant),
+ direction(1) {}
+ Rotor(double _angle, double _arm_length,
+ double _rotor_force_constant, double _rotor_moment_constant,
+ int _direction)
+ : angle(_angle),
+ arm_length(_arm_length),
+ rotor_force_constant(_rotor_force_constant),
+ rotor_moment_constant(_rotor_moment_constant),
+ direction(_direction) {}
+ double angle;
+ double arm_length;
+ double rotor_force_constant;
+ double rotor_moment_constant;
+ int direction;
+};
+
+struct RotorConfiguration {
+ RotorConfiguration() {
+ // Rotor configuration of Asctec Firefly.
+ rotors.push_back(
+ Rotor(kDefaultRotor0Angle, kDefaultArmLength, kDefaultRotorForceConstant,
+ kDefaultRotorMomentConstant, 1));
+ rotors.push_back(
+ Rotor(kDefaultRotor1Angle, kDefaultArmLength, kDefaultRotorForceConstant,
+ kDefaultRotorMomentConstant, -1));
+ rotors.push_back(
+ Rotor(kDefaultRotor2Angle, kDefaultArmLength, kDefaultRotorForceConstant,
+ kDefaultRotorMomentConstant, 1));
+ rotors.push_back(
+ Rotor(kDefaultRotor3Angle, kDefaultArmLength, kDefaultRotorForceConstant,
+ kDefaultRotorMomentConstant, -1));
+ rotors.push_back(
+ Rotor(kDefaultRotor4Angle, kDefaultArmLength, kDefaultRotorForceConstant,
+ kDefaultRotorMomentConstant, 1));
+ rotors.push_back(
+ Rotor(kDefaultRotor5Angle, kDefaultArmLength, kDefaultRotorForceConstant,
+ kDefaultRotorMomentConstant, -1));
+ }
+ std::vector<Rotor> rotors;
+};
+
+
+}
+
+#endif /* INCLUDE_ROTORS_GAZEBO_PARAMETERS_H_ */
diff --git a/rotors_gazebo/include/rotors_gazebo/parameters_ros.h b/rotors_gazebo/include/rotors_gazebo/parameters_ros.h
@@ -0,0 +1,67 @@
+/*
+ * Copyright 2020 Ria Sonecha, Massachusetts Institute of Technology in Cambridge, MA, USA
+ * Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ * http://www.apache.org/licenses/LICENSE-2.0
+
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef INCLUDE_ROTORS_GAZEBO_PARAMETERS_ROS_H_
+#define INCLUDE_ROTORS_GAZEBO_PARAMETERS_ROS_H_
+
+#include <ros/ros.h>
+
+#include "rotors_gazebo/parameters.h"
+
+namespace rotors_gazebo {
+
+template<typename T> inline void GetRosParameter(const ros::NodeHandle& nh,
+ const std::string& key,
+ const T& default_value,
+ T* value) {
+ ROS_ASSERT(value != nullptr);
+ bool have_parameter = nh.getParam(key, *value);
+ if (!have_parameter) {
+ ROS_WARN_STREAM("[rosparam]: could not find parameter " << nh.getNamespace()
+ << "/" << key << ", setting to default: " << default_value);
+ *value = default_value;
+ }
+}
+
+inline void GetRotorConfiguration(const ros::NodeHandle& nh,
+ RotorConfiguration* rotor_configuration) {
+ std::map<std::string, double> single_rotor;
+ std::string rotor_configuration_string = "rotor_configuration/";
+ unsigned int i = 0;
+ while (nh.getParam(rotor_configuration_string + std::to_string(i), single_rotor)) {
+ if (i == 0) {
+ rotor_configuration->rotors.clear();
+ }
+ Rotor rotor;
+ nh.getParam(rotor_configuration_string + std::to_string(i) + "/angle",
+ rotor.angle);
+ nh.getParam(rotor_configuration_string + std::to_string(i) + "/arm_length",
+ rotor.arm_length);
+ nh.getParam(rotor_configuration_string + std::to_string(i) + "/rotor_force_constant",
+ rotor.rotor_force_constant);
+ nh.getParam(rotor_configuration_string + std::to_string(i) + "/rotor_moment_constant",
+ rotor.rotor_moment_constant);
+ nh.getParam(rotor_configuration_string + std::to_string(i) + "/direction",
+ rotor.direction);
+ rotor_configuration->rotors.push_back(rotor);
+ ++i;
+ }
+}
+
+}
+
+#endif /* INCLUDE_ROTORS_GAZEBO_PARAMETERS_ROS_H_ */
diff --git a/rotors_gazebo/launch/crazyflie2_hovering_example.launch b/rotors_gazebo/launch/crazyflie2_hovering_example.launch
@@ -1,5 +1,7 @@
 <launch>
  <arg name="mav_name" default="crazyflie2"/>
+ <!-- Pay attention that the world sampling time is line with the one used in the
+ controller algorithm-->
  <arg name="world_name" default="basic"/>
  <arg name="enable_logging" default="false" />
  <arg name="enable_ground_truth" default="true" />
@@ -11,8 +13,11 @@
  <!-- The following line causes gzmsg and gzerr messages to be printed to the console
  (even when Gazebo is started through roslaunch) -->
  <arg name="verbose" default="false"/>
-
- <!-- The following lines simulate the world in Gazebo. The physic engine properties 
+ <!-- Enables the Position Controller disabling the Mellinger's one -->
+ <arg name="enable_position_controller" default="true"/>
+ <arg name="enable_mellinger_controller" default="false"/>
+
+ <!-- The following lines simulate the world in Gazebo. The physic engine properties
  are set up in the file "basic_crazyflie.world" file -->
  <env name="GAZEBO_MODEL_PATH" value="${GAZEBO_MODEL_PATH}:$(find rotors_gazebo)/models"/>
  <env name="GAZEBO_RESOURCE_PATH" value="${GAZEBO_RESOURCE_PATH}:$(find rotors_gazebo)/models"/>
@@ -32,16 +37,32 @@
  <arg name="enable_logging" value="$(arg enable_logging)" />
  <arg name="enable_ground_truth" value="$(arg enable_ground_truth)" />
  <arg name="enable_state_estimator" value="$(arg enable_state_estimator)" />
+ <!-- The enable signal is passed as input to the spawn_mav_crazyflie file. This allows to read The
+ value assumed by the variable (true/false) in the position_controller_node file -->
+ <arg unless="$(arg enable_position_controller)" name="enable_mellinger_controller" default="true"/>
  <arg name="log_file" value="$(arg log_file)"/>
  </include>
- <!-- The Crazyflie position controller -->
+ <!-- The Crazyflie controller -->
  <node name="position_controller_node" pkg="rotors_control" type="position_controller_node" output="screen">
  <param name="enable_state_estimator" value="$(arg enable_state_estimator)" />
+ <!-- Enabling/Disabling Mellinger's controller-->
+ <param unless="$(arg enable_mellinger_controller)" name="enable_position_controller" value="false" />
+ <param if="$(arg enable_mellinger_controller)" name="enable_mellinger_controller" value="true" />
+ <!-- Crazyflie file parameters used within the Position and Mellinger controllers. For more details on how the crazyflie2
+ controller architecture, see what is reported at this page https://www.bitcraze.io/2020/02/out-of-control/ -->
+ <rosparam command="load" file="$(find rotors_gazebo)/resource/crazyflie_parameters.yaml" />
  <rosparam unless="$(arg enable_state_estimator)" command="load" file="$(find rotors_gazebo)/resource/controller_$(arg mav_name).yaml" />
  <rosparam if="$(arg enable_state_estimator)" command="load" file="$(find rotors_gazebo)/resource/controller_$(arg mav_name)_with_stateEstimator.yaml" />
  <rosparam command="load" file="$(find rotors_gazebo)/resource/$(arg mav_name).yaml" />
+ <!-- Loading Mellinger's parameters -->
+ <rosparam if="$(arg enable_mellinger_controller)" command="load" file="$(find rotors_gazebo)/resource/crazyflie_mellinger_controller.yaml" />
+ </node>
+ <!-- Enable/Disable the trajectory generator - If the position_controller is activated, the hovering_example will be executed,
+ otherwise the spline generator and the Mellinger's controller will be run-->
+ <node unless="$(arg enable_mellinger_controller)" name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen" />
+ <node if="$(arg enable_mellinger_controller)" name="hovering_example_spline" pkg="rotors_gazebo" type="hovering_example_spline" output="screen" >
+ <rosparam command="load" file="$(find rotors_gazebo)/resource/spline_trajectory.yaml" />
  </node>
- <node name="hovering_example" pkg="rotors_gazebo" type="hovering_example" output="screen" />
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" />
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" />
  <node name="quaternion_to_rpy" pkg="rotors_gazebo" type="quaternion_to_rpy" output="screen" >