[GAZEBO_9] Add Mellinger's and Silano et. al controllers

rotors_comm/CHANGELOG.rst

@@ -2,6 +2,15 @@
 Changelog for package rotors_comm
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+5.0.4 (2020-04-14)
+------------------
+
+5.0.3 (2020-03-22)
+------------------
+
+5.0.2 (2020-02-09)
+------------------
+
 5.0.1 (2019-12-28)
 ------------------
 

rotors_comm/package.xml

@@ -1,6 +1,6 @@
 <package>
   <name>rotors_comm</name>
-  <version>5.0.1</version>
+  <version>5.0.4</version>
   <description>RotorS specific messages and services.</description>
 
   <maintainer email="giuseppe.silano@unisannio.it">Giuseppe Silano</maintainer>

rotors_control/CHANGELOG.rst

@@ -2,9 +2,21 @@
 Changelog for package rotors_control
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
-5.0.2 (2020-01-18)
+5.0.4 (2020-04-14)
+------------------
+* Improvements in the include folder
+* Add INDI and Mellinger's controllers
+* Contributors: Ria Sonecha, Giuseppe Silano
+
+5.0.3 (2020-03-22)
+------------------
+* Add data saving feature
+* Contributors: Giuseppe Silano
+
+5.0.2 (2020-02-09)
 ------------------
 * Fix typo in the position_controller_node with the enable_state_estimator variable #24
+* Add RollPitchYawRateThrust controller library for piloting the Crazyflie using the joystick #30
 * Contributors: Giuseppe Silano
 
 5.0.1 (2019-12-28)

rotors_control/CMakeLists.txt

@@ -18,7 +18,9 @@ find_package(
 
 catkin_package(
   INCLUDE_DIRS include ${Eigen_INCLUDE_DIRS}
-  LIBRARIES lee_position_controller position_controller crazyflie_onboard_controller roll_pitch_yawrate_thrust_controller sensfusion6 crazyflie_complementary_filter
+  LIBRARIES lee_position_controller position_controller crazyflie_onboard_controller
+    roll_pitch_yawrate_thrust_controller sensfusion6 crazyflie_complementary_filter
+    mellinger_controller internal_model_controller
   CATKIN_DEPENDS geometry_msgs mav_msgs nav_msgs roscpp sensor_msgs
   DEPENDS Eigen
 )
@@ -54,12 +56,26 @@ add_library(sensfusion6
   src/library/sensfusion6.cpp
 )
 
+add_library(mellinger_controller
+  src/library/mellinger_controller.cpp
+)
+
+add_library(internal_model_controller
+  src/library/internal_model_controller.cpp
+)
+
 target_link_libraries(lee_position_controller ${catkin_LIBRARIES})
 add_dependencies(lee_position_controller ${catkin_EXPORTED_TARGETS})
 
 target_link_libraries(position_controller ${catkin_LIBRARIES})
 add_dependencies(position_controller ${catkin_EXPORTED_TARGETS})
 
+target_link_libraries(mellinger_controller ${catkin_LIBRARIES})
+add_dependencies(mellinger_controller ${catkin_EXPORTED_TARGETS})
+
+target_link_libraries(internal_model_controller ${catkin_LIBRARIES})
+add_dependencies(internal_model_controller ${catkin_EXPORTED_TARGETS})
+
 target_link_libraries(roll_pitch_yawrate_thrust_controller ${catkin_LIBRARIES})
 add_dependencies(roll_pitch_yawrate_thrust_controller ${catkin_EXPORTED_TARGETS})
 
@@ -80,15 +96,17 @@ target_link_libraries(lee_position_controller_node
 add_executable(position_controller_node src/nodes/position_controller_node.cpp)
 add_dependencies(position_controller_node ${catkin_EXPORTED_TARGETS})
 target_link_libraries(position_controller_node
-  position_controller crazyflie_complementary_filter crazyflie_onboard_controller sensfusion6 ${catkin_LIBRARIES})
+  position_controller crazyflie_complementary_filter crazyflie_onboard_controller
+  sensfusion6 mellinger_controller internal_model_controller ${catkin_LIBRARIES})
 
 add_executable(roll_pitch_yawrate_thrust_controller_node
   src/nodes/roll_pitch_yawrate_thrust_controller_node.cpp)
 add_dependencies(roll_pitch_yawrate_thrust_controller_node ${catkin_EXPORTED_TARGETS})
 target_link_libraries(roll_pitch_yawrate_thrust_controller_node
   roll_pitch_yawrate_thrust_controller ${catkin_LIBRARIES})
 
-install(TARGETS lee_position_controller position_controller crazyflie_onboard_controller roll_pitch_yawrate_thrust_controller
+install(TARGETS lee_position_controller position_controller crazyflie_onboard_controller
+  roll_pitch_yawrate_thrust_controller mellinger_controller internal_model_controller
   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
 )
 

rotors_control/include/rotors_control/controller_parameters_mellinger.h

@@ -0,0 +1,101 @@
+/*
+ * 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 CONTROLLER_PARAMETERS_MELLINGER_H
+#define CONTROLLER_PARAMETERS_MELLINGER_H
+
+// Default values for the position controller of the Crazyflie2. XYController [x,y], AttitudeController [phi,theta]
+//RateController [p,q,r], YawController[yaw], HoveringController[z]
+static const Eigen::Vector2f kpXYDefaultPositionController = Eigen::Vector2f(0.4, 0.4);
+static const Eigen::Vector2f kdXYDefaultPositionController = Eigen::Vector2f(0.2, 0.2);
+static const Eigen::Vector2f kiXYDefaultPositionController = Eigen::Vector2f(0.05, 0.05);
+
+static const double kpZDefaultPositionController = 1.25;
+static const double kdZDefaultPositionController = 0.4;
+static const double kiZDefaultPositionController = 0.05;
+
+static const Eigen::Vector2f krXYDefaultPositionController = Eigen::Vector2f(70000, 70000);
+static const Eigen::Vector2f kwXYDefaultPositionController = Eigen::Vector2f(20000, 20000);
+static const Eigen::Vector2f ki_mXYDefaultPositionController = Eigen::Vector2f(0.0, 0.0);
+
+static const double krZDefaultPositionController = 60000;
+static const double kwZDefaultPositionController = 12000;
+static const double ki_mZDefaultPositionController = 500;
+
+static const Eigen::Vector2f iRangeMXY = Eigen::Vector2f(1.0, 1.0);
+static const Eigen::Vector2f iRangeXY = Eigen::Vector2f(2.0, 2.0);
+
+static const double iRangeMZ = 1500;
+static const double iRangeZ = 0.4;
+
+static const Eigen::Vector2f kdOmegaRP = Eigen::Vector2f(200, 200);
+
+
+namespace rotors_control {
+
+	class MellingerControllerParameters {
+	 public:
+	  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+	  MellingerControllerParameters()
+	      : kpXYPositionController_(kpXYDefaultPositionController),
+					kdXYPositionController_(kdXYDefaultPositionController),
+					kiXYPositionController_(kiXYDefaultPositionController),
+					kpZPositionController_(kpZDefaultPositionController),
+					kdZPositionController_(kdZDefaultPositionController),
+					kiZPositionController_(kiZDefaultPositionController),
+					krXYPositionController_(krXYDefaultPositionController),
+					kwXYPositionController_(kwXYDefaultPositionController),
+					ki_mXYPositionController_(ki_mXYDefaultPositionController),
+					krZPositionController_(krZDefaultPositionController),
+					kwZPositionController_(kwZDefaultPositionController),
+					ki_mZPositionController_(ki_mZDefaultPositionController),
+					iRangeMXY_(iRangeMXY),
+					iRangeXY_(iRangeXY),
+					iRangeMZ_(iRangeMZ),
+					iRangeZ_(iRangeZ),
+					kdOmegaRP_(kdOmegaRP){
+	  }
+
+
+		Eigen::Vector2f kpXYPositionController_;
+		Eigen::Vector2f kdXYPositionController_;
+		Eigen::Vector2f kiXYPositionController_;
+
+		double kpZPositionController_;
+		double kdZPositionController_;
+		double kiZPositionController_;
+
+		Eigen::Vector2f krXYPositionController_;
+		Eigen::Vector2f kwXYPositionController_;
+		Eigen::Vector2f ki_mXYPositionController_;
+
+		double krZPositionController_;
+		double kwZPositionController_;
+		double ki_mZPositionController_;
+
+		Eigen::Vector2f iRangeMXY_;
+		Eigen::Vector2f iRangeXY_;
+
+		double iRangeMZ_;
+		double iRangeZ_;
+
+		Eigen::Vector2f kdOmegaRP_;
+	};
+
+}
+
+#endif // CONTROLLER_PARAMETERS_MELLINGER_H

rotors_control/include/rotors_control/internal_model_controller.h

@@ -0,0 +1,177 @@
+/*
+ * 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 INTERNAL_MODEL_CONTROLLER_H
+#define INTERNAL_MODEL_CONTROLLER_H
+
+#include <mav_msgs/conversions.h>
+#include <mav_msgs/eigen_mav_msgs.h>
+#include <mav_msgs/DroneState.h>
+
+#include <string>
+
+#include <ros/time.h>
+
+#include "stabilizer_types.h"
+#include "parameters.h"
+#include "common.h"
+
+using namespace std;
+
+namespace rotors_control {
+
+// Default values for the Parrot Bebop controller. For more information about the control architecture, please take a look
+// at the publications page into the Wiki section.
+static const Eigen::Vector2d kPDefaultXYController = Eigen::Vector2d(-26.4259, -26.3627);
+static const double kPDefaultAltitudeController = -27.2277;
+
+static const double kPDefaultRollController = -1.7514;
+static const double kPDefaultPitchController = -1.7513;
+static const double kPDefaultYawRateController = -14.3431;
+
+static const Eigen::Vector2d MuDefaultXYController = Eigen::Vector2d(1, 1);
+static const double MuDefaultAltitudeController = 1;
+
+static const double MuDefaultRollController = 0.0544;
+static const double MuDefaultPitchController = 0.0543;
+static const double MuDefaultYawRateController = 0.44;
+
+static const Eigen::Vector3d UqDefaultXYZ = Eigen::Vector3d(1.1810, 1.1810, 4.6697);
+
+class InternalModelControllerParameters {
+ public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  InternalModelControllerParameters()
+      : beta_xy_(kPDefaultXYController),
+        beta_z_(kPDefaultAltitudeController),
+        beta_phi_(kPDefaultRollController),
+        beta_theta_(kPDefaultPitchController),
+        beta_psi_(kPDefaultYawRateController),
+        mu_xy_(MuDefaultXYController),
+        mu_z_(MuDefaultAltitudeController),
+        mu_theta_(MuDefaultPitchController),
+        mu_phi_(MuDefaultRollController),
+        mu_psi_(MuDefaultYawRateController),
+	    U_q_(UqDefaultXYZ){
+  }
+
+  Eigen::Vector2d beta_xy_;
+  double beta_z_;
+
+  double beta_phi_;
+  double beta_theta_;
+  double beta_psi_;
+
+  Eigen::Vector2d mu_xy_;
+  double mu_z_;
+
+  double mu_phi_;
+  double mu_theta_;
+  double mu_psi_;
+
+  Eigen::Vector3d U_q_;
+};
+
+    class InternalModelController{
+        public:
+            InternalModelController();
+            ~InternalModelController();
+            void CalculateRotorVelocities(Eigen::Vector4d* rotor_velocities);
+
+            void SetOdometryWithoutStateEstimator(const EigenOdometry& odometry);
+            void SetTrajectoryPoint(const mav_msgs::EigenDroneState& command_trajectory);
+            void SetControllerGains();
+            void SetVehicleParameters();
+
+            InternalModelControllerParameters controller_parameters_im_;
+            VehicleParameters vehicle_parameters_;
+
+            EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+        private:
+            //Boolean variables to active/unactive the controller and the data storage
+            bool controller_active_;
+
+            //Gazebo Message for attitude and position
+            ros::NodeHandle clientHandlePosition_;
+            ros::ServiceClient clientPosition_;
+
+            ros::NodeHandle clientHandleAttitude_;
+            ros::ServiceClient clientAttitude_;
+
+            //Controller gains
+            double beta_x_, beta_y_, beta_z_;
+            double beta_phi_, beta_theta_, beta_psi_;
+
+            double alpha_x_, alpha_y_, alpha_z_;
+            double alpha_phi_, alpha_theta_, alpha_psi_;
+
+            double mu_x_, mu_y_, mu_z_;
+            double mu_phi_, mu_theta_, mu_psi_;
+
+	          double U_q_x_, U_q_y_, U_q_z_;
+	          double K_x_1_, K_x_2_;
+	          double K_y_1_, K_y_2_;
+	          double K_z_1_, K_z_2_;
+
+            //Position and linear velocity errors
+            double e_x_;
+            double e_y_;
+            double e_z_;
+            double dot_e_x_;
+            double dot_e_y_;
+            double dot_e_z_;
+
+            //Attitude and angular velocity errors
+            double e_phi_;
+            double e_theta_;
+            double e_psi_;
+            double dot_e_phi_;
+            double dot_e_theta_;
+            double dot_e_psi_;
+
+            //Vehicle parameters
+            double bf_, m_, g_;
+            double l_, bm_;
+            double Ix_, Iy_, Iz_;
+
+            ros::NodeHandle n1_;
+            ros::NodeHandle n2_;
+            ros::Timer timer1_;
+            ros::Timer timer2_;
+
+            //Callback functions to compute the errors among axis and angles
+            void CallbackAttitude(const ros::TimerEvent& event);
+            void CallbackPosition(const ros::TimerEvent& event);
+
+	          state_t state_;
+            control_t control_;
+            mav_msgs::EigenDroneState command_trajectory_;
+            EigenOdometry odometry_;
+
+            void SetOdometryEstimated();
+            void Quaternion2Euler(double* roll, double* pitch, double* yaw) const;
+            void AttitudeController(double* u_phi, double* u_theta, double* u_psi);
+            void AngularVelocityErrors(double* dot_e_phi_, double* dot_e_theta_, double* dot_e_psi_);
+            void AttitudeErrors(double* e_phi_, double* e_theta_, double* e_psi_);
+            void PosController(double* u_T, double* phi_r, double* theta_r, double* u_x, double* u_y, double* u_z, double* u_Terr);
+            void PositionErrors(double* e_x, double* e_y, double* e_z);
+            void VelocityErrors(double* dot_e_x, double* dot_e_y, double* dot_e_z);
+            void Quaternion2EulerCommandTrajectory(double* roll, double* pitch, double* yaw) const;
+
+    };
+
+}
+#endif // INTERNAL_MODEL_CONTROLLER_H