From f9236ab601e11e2c1045c69b85ba175f0093b39b Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sun, 2 Feb 2020 18:53:06 +0100 Subject: [PATCH 01/21] ADD CHANGELOG --- rotors_comm/CHANGELOG.rst | 3 +++ rotors_control/CHANGELOG.rst | 4 +++- rotors_description/CHANGELOG.rst | 3 +++ rotors_evaluation/CHANGELOG.rst | 3 +++ rotors_gazebo/CHANGELOG.rst | 3 +++ rotors_gazebo_plugins/CHANGELOG.rst | 3 +++ rotors_joy_interface/CHANGELOG.rst | 3 +++ rotors_simulator/CHANGELOG.rst | 3 +++ rqt_rotors/CHANGELOG.rst | 3 +++ 9 files changed, 27 insertions(+), 1 deletion(-) diff --git a/rotors_comm/CHANGELOG.rst b/rotors_comm/CHANGELOG.rst index 90998d8..a8c9481 100644 --- 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 index aec4619..002cec5 100644 --- 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 index 7de8093..c54b4f0 100644 --- 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 index 5a7f609..6be1737 100644 --- 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 index cf7c61a..efa3e64 100644 --- a/rotors_gazebo/CHANGELOG.rst +++ b/rotors_gazebo/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rotors_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.2 (20XX-XX-XX) +------------------ + 6.0.1 (2019-12-28) ------------------ * Fix issue related to "xacro.py is deprecated; please use xacro instead" diff --git a/rotors_gazebo_plugins/CHANGELOG.rst b/rotors_gazebo_plugins/CHANGELOG.rst index d5d7d09..1da3868 100644 --- a/rotors_gazebo_plugins/CHANGELOG.rst +++ b/rotors_gazebo_plugins/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rotors_gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.2 (20XX-XX-XX) +------------------ + 6.0.1 (2019-12-28) ------------------ * The package has been made compatible with ROS Melodic and Gazebo 9. The RotorS's feature/gazebo9 branch has been used as a basis. diff --git a/rotors_joy_interface/CHANGELOG.rst b/rotors_joy_interface/CHANGELOG.rst index 1ff731b..bbf5ddd 100644 --- a/rotors_joy_interface/CHANGELOG.rst +++ b/rotors_joy_interface/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rotors_joy_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.2 (20XX-XX-XX) +------------------ + 6.0.1 (2019-12-28) ------------------ diff --git a/rotors_simulator/CHANGELOG.rst b/rotors_simulator/CHANGELOG.rst index 30a39dd..918c80a 100644 --- a/rotors_simulator/CHANGELOG.rst +++ b/rotors_simulator/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rotors_simulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.2 (20XX-XX-XX) +------------------ + 6.0.1 (2019-12-28) ------------------ diff --git a/rqt_rotors/CHANGELOG.rst b/rqt_rotors/CHANGELOG.rst index 55ed8ab..7c9b4b9 100644 --- a/rqt_rotors/CHANGELOG.rst +++ b/rqt_rotors/CHANGELOG.rst @@ -2,6 +2,9 @@ Changelog for package rqt_rotors ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +6.0.2 (20XX-XX-XX) +------------------ + 6.0.1 (2019-12-28) ------------------ From f84ab70af39c2f4971c681a763cd488f729c4abc Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:17:37 +0100 Subject: [PATCH 02/21] Add hovering_example_spline.h file Add the .h file of the spline trajectory generator and the necessary file to work with it --- .../rotors_gazebo/hovering_example_spline.h | 82 +++++++++++++++++ .../include/rotors_gazebo/parameters.h | 92 +++++++++++++++++++ .../include/rotors_gazebo/parameters_ros.h | 67 ++++++++++++++ 3 files changed, 241 insertions(+) create mode 100644 rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h create mode 100644 rotors_gazebo/include/rotors_gazebo/parameters.h create mode 100644 rotors_gazebo/include/rotors_gazebo/parameters_ros.h diff --git a/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h b/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h new file mode 100644 index 0000000..5f00ca3 --- /dev/null +++ b/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h @@ -0,0 +1,82 @@ +/* + * 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 + #include + #include + #include + #include + + // package libraries + #include "rotors_gazebo/Matrix3x3.h" + #include "rotors_gazebo/Quaternion.h" + #include "rotors_gazebo/transform_datatypes.h" + + #include + #include + #include + #include + #include + #include + #include + #include + #include + + + namespace rotors_gazebo { + + class HoveringExampleSpline{ + public: + + HoveringExampleSpline(); + ~HoveringExampleSpline(); + + void TrajectoryCallback(mav_msgs::EigenDroneState* odometry, double* time_final, double* time_init); + void InitializeParams(); + template 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 new file mode 100644 index 0000000..49a17f8 --- /dev/null +++ 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 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 new file mode 100644 index 0000000..ab867d0 --- /dev/null +++ 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 + +#include "rotors_gazebo/parameters.h" + +namespace rotors_gazebo { + +template 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 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_ */ From 20bee98b2b067879f0ab958be3a44e5d74f49fe3 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:29:43 +0100 Subject: [PATCH 03/21] Add the launch file for running the Internal Model Controller --- ...razyflie2_internal_model_controller.launch | 64 +++++++++++++++++++ 1 file changed, 64 insertions(+) create mode 100644 rotors_gazebo/launch/crazyflie2_internal_model_controller.launch diff --git a/rotors_gazebo/launch/crazyflie2_internal_model_controller.launch b/rotors_gazebo/launch/crazyflie2_internal_model_controller.launch new file mode 100644 index 0000000..e3468f1 --- /dev/null +++ b/rotors_gazebo/launch/crazyflie2_internal_model_controller.launch @@ -0,0 +1,64 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From 6e0e598383431daa229bdb83ccb3d466f2beb7b7 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:30:07 +0100 Subject: [PATCH 04/21] Add launch file for running the Mellinger's controller --- .../launch/crazyflie2_hovering_example.launch | 29 ++++++++++++++++--- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/rotors_gazebo/launch/crazyflie2_hovering_example.launch b/rotors_gazebo/launch/crazyflie2_hovering_example.launch index 7b83d32..2974130 100644 --- a/rotors_gazebo/launch/crazyflie2_hovering_example.launch +++ b/rotors_gazebo/launch/crazyflie2_hovering_example.launch @@ -1,5 +1,7 @@ + @@ -11,8 +13,11 @@ - - + + + + @@ -32,16 +37,32 @@ + + - + + + + + + + + + + + + + - From 948478b4d8d8e7b0160f44d3ede2a645bad0e43d Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:34:40 +0100 Subject: [PATCH 05/21] Add internal model controller parameters --- .../crazyflie_internal_model_controller.yaml | 12 ++++++++++++ 1 file changed, 12 insertions(+) create mode 100644 rotors_gazebo/resource/crazyflie_internal_model_controller.yaml diff --git a/rotors_gazebo/resource/crazyflie_internal_model_controller.yaml b/rotors_gazebo/resource/crazyflie_internal_model_controller.yaml new file mode 100644 index 0000000..aba404e --- /dev/null +++ b/rotors_gazebo/resource/crazyflie_internal_model_controller.yaml @@ -0,0 +1,12 @@ +# Internal model controller parameters +beta_xy: {beta_x: -26.4259, beta_y: -26.3627} +beta_z: {beta_z: -27.2277} +beta_phi: {beta_phi: -0.8757} +beta_theta: {beta_theta: -0.8757} +beta_psi: {beta_psi: -14.3431} +mu_xy: {mu_x: 1, mu_y: 1} +mu_z: {mu_z: 1} +mu_phi: {mu_phi: 0.0544} +mu_theta: {mu_theta: 0.0543} +mu_psi: {mu_psi: 0.44} +U_xyz: {U_x: 1.1810, U_y: 1.1810, U_z: 4.6697} From 506877aa7b19babdb159f8cd883a789f41bce600 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:34:56 +0100 Subject: [PATCH 06/21] Add Mellinger's controller parameters --- .../crazyflie_mellinger_controller.yaml | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) create mode 100644 rotors_gazebo/resource/crazyflie_mellinger_controller.yaml diff --git a/rotors_gazebo/resource/crazyflie_mellinger_controller.yaml b/rotors_gazebo/resource/crazyflie_mellinger_controller.yaml new file mode 100644 index 0000000..9117588 --- /dev/null +++ b/rotors_gazebo/resource/crazyflie_mellinger_controller.yaml @@ -0,0 +1,28 @@ +# Mellinger controller parameters +kp_xy: {x: 0.4, y: 0.4} # Proportional gain +kd_xy: {x: 0.2, y: 0.2} # Derivative gain +ki_xy: {x: 0.05, y: 0.05} # Integral gain + +# Z Position +kp_z: {z: 1.25} # Proportional gain +kd_z: {z: 0.4} # Derivative gain +ki_z: {z: 0.05} # Integral gain + +# Attitude +kr_xy: {x: 0.4, y: 0.4} # Proportional gain +kw_xy: {x: 0.2, y: 0.2} # Derivative gain +ki_m_xy: {x: 0.05, y: 0.05} # Integral gain + +# Yaw +kr_z: {z: 1.25} # Proportional gain +kw_z: {z: 0.4} # Derivative gain +ki_m_z: {z: 0.05} # Integral gain + +# Error boundaries +i_range_m_z: {z: 1500} # Ros chapter equation 8 +i_range_m_xy: {x: 1.0, y: 1.0} +i_range_z: {z: 0.4} +i_range_xy: {x: 2.0, y: 2.0} + +# roll and pitch angular velocity +kd_omega_rp: {r: 0.002, p: 0.002} # Derivative gain From 791a41e1a7c70876de6558389745cf63ae244e1e Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:35:11 +0100 Subject: [PATCH 07/21] Add Crazyflie vehicle parameters --- rotors_gazebo/resource/crazyflie_parameters.yaml | 6 ++++++ 1 file changed, 6 insertions(+) create mode 100644 rotors_gazebo/resource/crazyflie_parameters.yaml diff --git a/rotors_gazebo/resource/crazyflie_parameters.yaml b/rotors_gazebo/resource/crazyflie_parameters.yaml new file mode 100644 index 0000000..18b085f --- /dev/null +++ b/rotors_gazebo/resource/crazyflie_parameters.yaml @@ -0,0 +1,6 @@ +# Crazyflie vehicle parameters +mass: 0.026 +inertia: {xx: 1.657171e-05, xy: 0.0, xz: 0.0, yy: 1.657171e-05, yz: 0.0, zz: 2.9261652e-05} +bf: 1.28192e-8 +bm: 5.964552e-3 +l: 0.046 From c9f71efc0ededf7725ef517b3f9e896aec2af9ca Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:35:41 +0100 Subject: [PATCH 08/21] Add Spline Trajectory Generator parameters --- rotors_gazebo/resource/spline_trajectory.yaml | 14 ++++++++++++++ 1 file changed, 14 insertions(+) create mode 100644 rotors_gazebo/resource/spline_trajectory.yaml diff --git a/rotors_gazebo/resource/spline_trajectory.yaml b/rotors_gazebo/resource/spline_trajectory.yaml new file mode 100644 index 0000000..0a7c44b --- /dev/null +++ b/rotors_gazebo/resource/spline_trajectory.yaml @@ -0,0 +1,14 @@ +# Spline trajectory parameters +time_final: {time: 10} # Remember that the trajectory generation starts after 3 seconds. This is the time_initial value +position_initial: {x: 0, y: 0, z: 0} +position_final: {x: 0, y: 0, z: 1} +velocity_initial: {x: 0, y: 0, z: 0} +velocity_final: {x: 0, y: 0, z: 0} +acceleration_initial: {x: 0, y: 0, z: 0} +acceleration_final: {x: 0, y: 0, z: 0} +orientation_initial: {roll: 0, pitch: 0, yaw: 0} +orientation_final: {roll: 0, pitch: 0, yaw: 0} +angularRate_initial: {roll: 0, pitch: 0, yaw: 0} +angularRate_final: {roll: 0, pitch: 0, yaw: 0} +angularDoubleRate_initial: {roll: 0, pitch: 0, yaw: 0} +angularDoubleRate_final: {roll: 0, pitch: 0, yaw: 0} From 331ec935a9aaf682289b810da69c109f512d2d42 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:36:35 +0100 Subject: [PATCH 09/21] Add space --- rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h | 1 + 1 file changed, 1 insertion(+) diff --git a/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h b/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h index 5f00ca3..608019e 100644 --- a/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h +++ b/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h @@ -7,6 +7,7 @@ * 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. From 334f95eb9bc526620b965c671c4627f95af7a051 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:46:09 +0100 Subject: [PATCH 10/21] Add Spline Trajectory Generator cpp file --- rotors_gazebo/src/hovering_example_spline.cpp | 466 ++++++++++++++++++ 1 file changed, 466 insertions(+) create mode 100644 rotors_gazebo/src/hovering_example_spline.cpp diff --git a/rotors_gazebo/src/hovering_example_spline.cpp b/rotors_gazebo/src/hovering_example_spline.cpp new file mode 100644 index 0000000..1ef38fc --- /dev/null +++ b/rotors_gazebo/src/hovering_example_spline.cpp @@ -0,0 +1,466 @@ +/* + * 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. + */ + +#include +#include + +#include "rotors_gazebo/Quaternion.h" +#include "rotors_gazebo/transform_datatypes.h" +#include "rotors_gazebo/parameters_ros.h" +#include +#include "rotors_gazebo/Matrix3x3.h" +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include "rotors_gazebo/hovering_example_spline.h" + +#define DELTA_1 0.5 +#define ALPHA_1 2 +#define ALPHA_2 20 +#define ALPHA_3 8 +#define ALPHA_4 12 +#define ALPHA_5 3 +#define BETA_1 30 +#define BETA_2 14 +#define BETA_3 16 +#define BETA_4 2 +#define GAMMA_1 12 +#define GAMMA_2 6 +#define SAMPLING_TIME 10e-3 /* SAMPLING CONTROLLER TIME [s] - 100Hz */ +#define START_SIMULATION_TIME 3 /* TIME GAZEBO NEEDS TO INITIALIZE THE ENVIRONMENT */ + +namespace rotors_gazebo { + + HoveringExampleSpline::HoveringExampleSpline(){ + + // Parameters initializion + a0_.setZero(); a1_.setZero(); a2_.setZero(); a3_.setZero(); a4_.setZero(); a5_.setZero(); + b0_.setZero(); b1_.setZero(); b2_.setZero(); b3_.setZero(); b4_.setZero(); b5_.setZero(); + c0_.setZero(); c1_.setZero(); c2_.setZero(); c3_.setZero(); c4_.setZero(); c5_.setZero(); + + g0_.setZero(); g1_.setZero(); g2_.setZero(); g3_.setZero(); g4_.setZero(); g5_.setZero(); + h0_.setZero(); h1_.setZero(); h2_.setZero(); h3_.setZero(); h4_.setZero(); h5_.setZero(); + + } + + HoveringExampleSpline::~HoveringExampleSpline(){} + + void HoveringExampleSpline::TrajectoryCallback(mav_msgs::EigenDroneState* odometry, double* time_final, double* time_init) { + assert(odometry); + assert(time_final); + assert(time_init); + + double time_spline; + time_spline = *time_final - *time_init; + + if (enable_parameter_computation_){ + ComputeSplineParameters(&time_final_); + enable_parameter_computation_ = false; + + ROS_DEBUG("Publishing time_final_: %f", time_final_); + } + + ROS_DEBUG("Publishing time_spline: %f", time_spline); + + // Desidered Position + double first, second, third; + first = a0_.x() * pow(time_spline, 5) + a1_.x() * pow(time_spline, 4) + a2_.x() * pow(time_spline, 3) + a3_.x() * pow(time_spline, 2) + + a4_.x() * time_spline + a5_.x(); + second = a0_.y() * pow(time_spline, 5) + a1_.y() * pow(time_spline, 4) + a2_.y() * pow(time_spline, 3) + a3_.y() * pow(time_spline, 2) + + a4_.y() * time_spline + a5_.y(); + third = a0_.z() * pow(time_spline, 5) + a1_.z() * pow(time_spline, 4) + a2_.z() * pow(time_spline, 3) + a3_.z() * pow(time_spline, 2) + + a4_.z() * time_spline + a5_.z(); + + ROS_DEBUG("Publishing position spline parameters along x: [%f, %f, %f, %f, %f, %f].", a0_.x(), a1_.x(), a2_.x(), a3_.x(), a4_.x(), a5_.x()); + ROS_DEBUG("Publishing position spline parameters along y: [%f, %f, %f, %f, %f, %f].", a0_.y(), a1_.y(), a2_.y(), a3_.y(), a4_.y(), a5_.y()); + ROS_DEBUG("Publishing position spline parameters along z: [%f, %f, %f, %f, %f, %f].", a0_.z(), a1_.z(), a2_.z(), a3_.z(), a4_.z(), a5_.z()); + + odometry->position_W = Eigen::Vector3f(first, second, third); + + ROS_DEBUG("Publishing position waypoint: [%f, %f, %f].", first, second, third); + + // Desidered Linear Velocity + first = b0_.x() * pow(time_spline, 5) + b1_.x() * pow(time_spline, 4) + b2_.x() * pow(time_spline, 3) + b3_.x() * pow(time_spline, 2) + + b4_.x() * time_spline + b5_.x(); + second = b0_.y() * pow(time_spline, 5) + b1_.y() * pow(time_spline, 4) + b2_.y() * pow(time_spline, 3) + b3_.y() * pow(time_spline, 2) + + b4_.y() * time_spline + b5_.y(); + third = b0_.z() * pow(time_spline, 5) + b1_.z() * pow(time_spline, 4) + b2_.z() * pow(time_spline, 3) + b3_.z() * pow(time_spline, 2) + + b4_.z() * time_spline + b5_.z(); + + ROS_DEBUG("Publishing velocity spline parameters along x: [%f, %f, %f, %f, %f, %f].", b0_.x(), b1_.x(), b2_.x(), b3_.x(), b4_.x(), b5_.x()); + ROS_DEBUG("Publishing velocity spline parameters along y: [%f, %f, %f, %f, %f, %f].", b0_.y(), b1_.y(), b2_.y(), b3_.y(), b4_.y(), b5_.y()); + ROS_DEBUG("Publishing velocity spline parameters along z: [%f, %f, %f, %f, %f, %f].", b0_.z(), b1_.z(), b2_.z(), b3_.z(), b4_.z(), b5_.z()); + + odometry->velocity = Eigen::Vector3f(first, second, third); + + ROS_DEBUG("Publishing velocity waypoint: [%f, %f, %f].", first, second, third); + + // Desidered Acceleration + first = c0_.x() * pow(time_spline, 5) + c1_.x() * pow(time_spline, 4) + c2_.x() * pow(time_spline, 3) + c3_.x() * pow(time_spline, 2) + + c4_.x() * time_spline + c5_.x(); + second = c0_.y() * pow(time_spline, 5) + c1_.y() * pow(time_spline, 4) + c2_.y() * pow(time_spline, 3) + c3_.y() * pow(time_spline, 2) + + c4_.y() * time_spline + c5_.y(); + third = c0_.z() * pow(time_spline, 5) + c1_.z() * pow(time_spline, 4) + c2_.z() * pow(time_spline, 3) + c3_.z() * pow(time_spline, 2) + + c4_.z() * time_spline + c5_.z(); + + odometry->acceleration = Eigen::Vector3f(first, second, third); + + ROS_DEBUG("Publishing acceleration waypoint: [%f, %f, %f].", first, second, third); + + // Desidred Attitute + rollDesRad_ = g0_.x() * pow(time_spline, 5) + g1_.x() * pow(time_spline, 4) + g2_.x() * pow(time_spline, 3) + g3_.x() * pow(time_spline, 2) + + g4_.x() * time_spline + g5_.x(); + pitchDesRad_ = g0_.y() * pow(time_spline, 5) + g1_.y() * pow(time_spline, 4) + g2_.y() * pow(time_spline, 3) + g3_.y() * pow(time_spline, 2) + + g4_.y() * time_spline + g5_.y(); + yawDesRad_ = g0_.z() * pow(time_spline, 5) + g1_.z() * pow(time_spline, 4) + g2_.z() * pow(time_spline, 3) + g3_.z() * pow(time_spline, 2) + + g4_.z() * time_spline + g5_.z(); + + // Converts radians in quaternions + double x, y, z, w; + Euler2QuaternionCommandTrajectory(&x, &y, &z, &w); + odometry->orientation_W_B = Eigen::Quaterniond(w, x, y, z); + + ROS_DEBUG("Publishing attitude waypoint: [%f, %f, %f].", rollDesRad_, pitchDesRad_, yawDesRad_); + + // Desidered Angular Velocity + first = h0_.x() * pow(time_spline, 5) + h1_.x() * pow(time_spline, 4) + h2_.x() * pow(time_spline, 3) + h3_.x() * pow(time_spline, 2) + + h4_.x() * time_spline + h5_.x(); + second = h0_.y() * pow(time_spline, 5) + h1_.y() * pow(time_spline, 4) + h2_.y() * pow(time_spline, 3) + h3_.y() * pow(time_spline, 2) + + h4_.y() * time_spline + h5_.y(); + third = h0_.z() * pow(time_spline, 5) + h1_.z() * pow(time_spline, 4) + h2_.z() * pow(time_spline, 3) + h3_.z() * pow(time_spline, 2) + + h4_.z() * time_spline + h5_.z(); + + double first_B, second_B, third_B; + first_B = first - sin(pitchDesRad_)*third; + second_B = cos(rollDesRad_)*second + sin(rollDesRad_)*cos(pitchDesRad_)*third; + third_B = -sin(rollDesRad_)*second + cos(rollDesRad_)*cos(pitchDesRad_)*third; + + odometry->angular_velocity_B = Eigen::Vector3f(first_B, second_B, third_B); + + ROS_DEBUG("Publishing angular velocity waypoint: [%f, %f, %f].", first, second, third); + + } + + void HoveringExampleSpline::ComputeSplineParameters(double* time_spline){ + assert(time_spline); + + // Parameters used for the position and its derivatives + a0_ = position_initial_; + + ROS_DEBUG("Publishing position initial: [%f, %f, %f].", position_initial_[0], position_initial_[1], position_initial_[2]); + + a1_ = velocity_initial_; + + a2_.x() = acceleration_initial_.x() * DELTA_1; + a2_.y() = acceleration_initial_.y() * DELTA_1; + a2_.z() = acceleration_initial_.z() * DELTA_1; + + ROS_DEBUG("Publishing acceleration initial: [%f, %f, %f].", acceleration_initial_[0], acceleration_initial_[1], acceleration_initial_[2]); + + a3_.x() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.x() - position_initial_.x()) - (ALPHA_3 * velocity_final_.x() + ALPHA_4 * velocity_initial_.x()) * *time_spline + - (ALPHA_5 * acceleration_final_.x() - acceleration_initial_.x()) * pow(*time_spline, 2)); + a3_.y() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.y() - position_initial_.y()) - (ALPHA_3 * velocity_final_.y() + ALPHA_4 * velocity_initial_.y()) * *time_spline + - (ALPHA_5 * acceleration_final_.y() - acceleration_initial_.y()) * pow(*time_spline, 2)); + a3_.z() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.z() - position_initial_.z()) - (ALPHA_3 * velocity_final_.z() + ALPHA_4 * velocity_initial_.z()) * *time_spline + - (ALPHA_5 * acceleration_final_.z() - acceleration_initial_.z()) * pow(*time_spline, 2)); + + a4_.x() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.x() - position_final_.x()) + (BETA_2 * velocity_final_.x() + BETA_3 * velocity_initial_.x()) * *time_spline + + (BETA_4 * acceleration_final_.x() - 2 * acceleration_initial_.x()) * pow(*time_spline, 2)); + a4_.y() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.y() - position_final_.y()) + (BETA_2 * velocity_final_.y() + BETA_3 * velocity_initial_.y()) * *time_spline + + (BETA_4 * acceleration_final_.y() - 2 * acceleration_initial_.y()) * pow(*time_spline, 2)); + a4_.z() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.z() - position_final_.z()) + (BETA_2 * velocity_final_.z() + BETA_3 * velocity_initial_.z()) * *time_spline + + (BETA_4 * acceleration_final_.z() - 2 * acceleration_initial_.z()) * pow(*time_spline, 2)); + + a5_.x() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.x() - position_initial_.x()) - GAMMA_2 * (velocity_final_.x() + velocity_initial_.x()) * *time_spline + - (acceleration_final_.x() - acceleration_initial_.x()) * pow(*time_spline, 2)); + a5_.y() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.y() - position_initial_.y()) - GAMMA_2 * (velocity_final_.y() + velocity_initial_.y()) * *time_spline + - (acceleration_final_.y() - acceleration_initial_.y()) * pow(*time_spline, 2)); + a5_.z() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.z() - position_initial_.z()) - GAMMA_2 * (velocity_final_.z() + velocity_initial_.z()) * *time_spline + - (acceleration_final_.z() - acceleration_initial_.z()) * pow(*time_spline, 2)); + //b + b0_.x() = 0; + b0_.y() = 0; + b0_.z() = 0; + + b1_.x() = 5*a0_.x(); + b1_.y() = 5*a0_.y(); + b1_.z() = 5*a0_.z(); + + b2_.x() = 4*a1_.x(); + b2_.y() = 4*a1_.y(); + b2_.z() = 4*a1_.z(); + + b3_.x() = 3*a2_.x(); + b3_.y() = 3*a2_.y(); + b3_.z() = 3*a2_.z(); + + b4_.x() = 2*a3_.x(); + b4_.y() = 2*a3_.y(); + b4_.z() = 2*a3_.z(); + + b5_ = a4_; + + //c + c0_.x() = 0; + c0_.y() = 0; + c0_.z() = 0; + + c1_.x() = 0; + c1_.y() = 0; + c1_.z() = 0; + + c2_.x() = 4*b1_.x(); + c2_.y() = 4*b1_.y(); + c2_.z() = 4*b1_.z(); + + c3_.x() = 3*b2_.x(); + c3_.y() = 3*b2_.y(); + c3_.z() = 3*b2_.z(); + + c4_.x() = 2*b3_.x(); + c4_.y() = 2*b3_.y(); + c4_.z() = 2*b3_.z(); + + c5_ = b4_; + + // Parameters used for the orientation and its derivatives + g0_ = orientation_initial_; + + g1_ = angular_velocity_initial_; + + g2_.x() = angular_acceleration_initial_.x() * DELTA_1; + g2_.y() = angular_acceleration_initial_.y() * DELTA_1; + g2_.z() = angular_acceleration_initial_.z() * DELTA_1; + + g3_.x() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.x() - orientation_initial_.x()) - (ALPHA_3 * angular_velocity_final_.x() + ALPHA_4 * + angular_velocity_initial_.x()) * *time_spline - (ALPHA_5 * angular_acceleration_final_.x() - angular_acceleration_initial_.x()) * pow(*time_spline, 2)); + g3_.y() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.y() - orientation_initial_.y()) - (ALPHA_3 * angular_velocity_final_.y() + ALPHA_4 * + angular_velocity_initial_.y()) * *time_spline - (ALPHA_5 * angular_acceleration_final_.y() - angular_acceleration_initial_.y()) * pow(*time_spline, 2)); + g3_.z() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.z() - orientation_initial_.z()) - (ALPHA_3 * angular_velocity_final_.z() + ALPHA_4 * + angular_velocity_initial_.z()) * *time_spline - (ALPHA_5 * angular_acceleration_final_.z() - angular_acceleration_initial_.z()) * pow(*time_spline, 2)); + + g4_.x() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.x() - orientation_final_.x()) + (BETA_2 * angular_velocity_final_.x() + BETA_3 * + angular_velocity_initial_.x()) * *time_spline + (BETA_4 * angular_acceleration_final_.x() - 2 * angular_acceleration_initial_.x()) * pow(*time_spline, 2)); + g4_.y() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.y() - orientation_final_.y()) + (BETA_2 * angular_velocity_final_.y() + BETA_3 * + angular_velocity_initial_.y()) * *time_spline + (BETA_4 * angular_acceleration_final_.y() - 2 * angular_acceleration_initial_.y()) * pow(*time_spline, 2)); + g4_.z() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.z() - orientation_final_.z()) + (BETA_2 * angular_velocity_final_.z() + BETA_3 * + angular_velocity_initial_.z()) * *time_spline + (BETA_4 * angular_acceleration_final_.z() - 2 * angular_acceleration_initial_.z()) * pow(*time_spline, 2)); + + g5_.x() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.x() - orientation_initial_.x()) - GAMMA_2 * (angular_velocity_final_.x() + + angular_velocity_initial_.x()) * *time_spline - (angular_acceleration_final_.x() - angular_acceleration_initial_.x()) * pow(*time_spline, 2)); + g5_.y() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.y() - orientation_initial_.y()) - GAMMA_2 * (angular_velocity_final_.y() + + angular_velocity_initial_.y()) * *time_spline - (angular_acceleration_final_.y() - angular_acceleration_initial_.y()) * pow(*time_spline, 2)); + g5_.z() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.z() - orientation_initial_.z()) - GAMMA_2 * (angular_velocity_final_.z() + + angular_velocity_initial_.z()) * *time_spline - (angular_acceleration_final_.z() - angular_acceleration_initial_.z()) * pow(*time_spline, 2)); + + //h + h0_.x() = 0; + h0_.y() = 0; + h0_.z() = 0; + + h1_.x() = 5*g0_.x(); + h1_.y() = 5*g0_.y(); + h1_.z() = 5*g0_.z(); + + h2_.x() = 4*g1_.x(); + h2_.y() = 4*g1_.y(); + h2_.z() = 4*g1_.z(); + + h3_.x() = 3*g2_.x(); + h3_.y() = 3*g2_.y(); + h3_.z() = 3*g2_.z(); + + h4_.x() = 2*g3_.x(); + h4_.y() = 2*g3_.y(); + h4_.z() = 2*g3_.z(); + + h5_ = g4_; + + } + + void HoveringExampleSpline::Euler2QuaternionCommandTrajectory(double* x, double* y, double* z, double* w) const { + assert(x); + assert(y); + assert(z); + assert(w); + + // Abbreviations for the various angular functions + double cy = cos(yawDesRad_ * 0.5); + double sy = sin(yawDesRad_ * 0.5); + double cp = cos(pitchDesRad_ * 0.5); + double sp = sin(pitchDesRad_ * 0.5); + double cr = cos(rollDesRad_ * 0.5); + double sr = sin(rollDesRad_ * 0.5); + + *w = cy * cp * cr + sy * sp * sr; + *x = cy * cp * sr - sy * sp * cr; + *y = sy * cp * sr + cy * sp * cr; + *z = sy * cp * cr - cy * sp * sr; + + ROS_DEBUG("x Trajectory: %f, y Trajectory: %f, z Trajectory: %f, w Trajectory: %f", *x, *y, *z, *w); + } + + void HoveringExampleSpline::InitializeParams(){ + + ros::NodeHandle pnh("~"); + + // Parameters reading from rosparam. + GetRosParameter(pnh, "time_final/time", time_final_, &time_final_); + GetRosParameter(pnh, "position_initial/x", position_initial_.x(), &position_initial_.x()); + GetRosParameter(pnh, "position_initial/y", position_initial_.y(), &position_initial_.y()); + GetRosParameter(pnh, "position_initial/z", position_initial_.z(), &position_initial_.z()); + + GetRosParameter(pnh, "position_final/x", position_final_.x(), &position_final_.x()); + GetRosParameter(pnh, "position_final/y", position_final_.y(), &position_final_.y()); + GetRosParameter(pnh, "position_final/z", position_final_.z(), &position_final_.z()); + + GetRosParameter(pnh, "velocity_initial/x", velocity_initial_.x(), &velocity_initial_.x()); + GetRosParameter(pnh, "velocity_initial/y", velocity_initial_.y(), &velocity_initial_.y()); + GetRosParameter(pnh, "velocity_initial/z", velocity_initial_.z(), &velocity_initial_.z()); + + GetRosParameter(pnh, "velocity_final/x", velocity_final_.x(), &velocity_final_.x()); + GetRosParameter(pnh, "velocity_final/y", velocity_final_.y(), &velocity_final_.y()); + GetRosParameter(pnh, "velocity_final/z", velocity_final_.z(), &velocity_final_.z()); + + GetRosParameter(pnh, "acceleration_initial/x", acceleration_initial_.x(), &acceleration_initial_.x()); + GetRosParameter(pnh, "acceleration_initial/y", acceleration_initial_.y(), &acceleration_initial_.y()); + GetRosParameter(pnh, "acceleration_initial/z", acceleration_initial_.z(), &acceleration_initial_.z()); + + GetRosParameter(pnh, "acceleration_final/x", acceleration_final_.x(), &acceleration_final_.x()); + GetRosParameter(pnh, "acceleration_final/y", acceleration_final_.y(), &acceleration_final_.y()); + GetRosParameter(pnh, "acceleration_final/z", acceleration_final_.z(), &acceleration_final_.z()); + + GetRosParameter(pnh, "orientation_initial/roll", orientation_initial_.x(), &orientation_initial_.x()); + GetRosParameter(pnh, "orientation_initial/pitch", orientation_initial_.y(), &orientation_initial_.y()); + GetRosParameter(pnh, "orientation_initial/yaw", orientation_initial_.z(), &orientation_initial_.z()); + + GetRosParameter(pnh, "orientation_final/roll", orientation_final_.x(), &orientation_final_.x()); + GetRosParameter(pnh, "orientation_final/pitch", orientation_final_.y(), &orientation_final_.y()); + GetRosParameter(pnh, "orientation_final/yaw", orientation_final_.z(), &orientation_final_.z()); + + GetRosParameter(pnh, "angularRate_initial/roll", angular_velocity_initial_.x(), &angular_velocity_initial_.x()); + GetRosParameter(pnh, "angularRate_initial/pitch", angular_velocity_initial_.y(), &angular_velocity_initial_.y()); + GetRosParameter(pnh, "angularRate_initial/yaw", angular_velocity_initial_.z(), &angular_velocity_initial_.z()); + + GetRosParameter(pnh, "angularRate_final/roll", angular_velocity_final_.x(), &angular_velocity_final_.x()); + GetRosParameter(pnh, "angularRate_final/pitch", angular_velocity_final_.y(), &angular_velocity_final_.y()); + GetRosParameter(pnh, "angularRate_final/yaw", angular_velocity_final_.z(), &angular_velocity_final_.z()); + + GetRosParameter(pnh, "angularDoubleRate_initial/roll", angular_acceleration_initial_.x(), &angular_acceleration_initial_.x()); + GetRosParameter(pnh, "angularDoubleRate_initial/pitch", angular_acceleration_initial_.y(), &angular_acceleration_initial_.y()); + GetRosParameter(pnh, "angularDoubleRate_initial/yaw", angular_acceleration_initial_.z(), &angular_acceleration_initial_.z()); + + GetRosParameter(pnh, "angularDoubleRate_final/roll", angular_acceleration_final_.x(), &angular_acceleration_final_.x()); + GetRosParameter(pnh, "angularDoubleRate_final/pitch", angular_acceleration_final_.y(), &angular_acceleration_final_.y()); + GetRosParameter(pnh, "angularDoubleRate_final/yaw", angular_acceleration_final_.z(), &angular_acceleration_final_.z()); + + } + + template inline void HoveringExampleSpline::GetRosParameterHovering(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; + } + } + +} + + +int main(int argc, char** argv) { + ros::init(argc, argv, "hovering_example_spline"); + ros::NodeHandle nh; + // Create a private node handle for accessing node parameters. + ros::NodeHandle nh_private("~"); + ros::Publisher trajectory_pub = + nh.advertise( + mav_msgs::default_topics::DRONE_STATE, 10); + ROS_INFO("Started hovering example with spline."); + + std_srvs::Empty srv; + bool unpaused = ros::service::call("/gazebo/unpause_physics", srv); + unsigned int i = 0; + + // Trying to unpause Gazebo for 10 seconds. + while (i <= 10 && !unpaused) { + ROS_INFO("Wait for 1 second before trying to unpause Gazebo again."); + std::this_thread::sleep_for(std::chrono::seconds(1)); + unpaused = ros::service::call("/gazebo/unpause_physics", srv); + ++i; + } + + if (!unpaused) { + ROS_FATAL("Could not wake up Gazebo."); + return -1; + } else { + ROS_INFO("Unpaused the Gazebo simulation."); + } + + // Trajectory message + mav_msgs::DroneState trajectory_msg; + trajectory_msg.header.stamp = ros::Time::now(); + mav_msgs::EigenDroneState eigen_reference; + rotors_gazebo::HoveringExampleSpline hovering_example_spline_generator; + + Eigen::Vector3f check_position_final; + hovering_example_spline_generator.GetRosParameterHovering(nh_private, "position_final/x", check_position_final.x(), &check_position_final.x()); + hovering_example_spline_generator.GetRosParameterHovering(nh_private, "position_final/y", check_position_final.y(), &check_position_final.y()); + hovering_example_spline_generator.GetRosParameterHovering(nh_private, "position_final/z", check_position_final.z(), &check_position_final.z()); + + // Wait for 5 seconds to let the Gazebo GUI show up. + if (ros::Time::now().toSec() < START_SIMULATION_TIME){ + ros::Duration(START_SIMULATION_TIME).sleep(); + hovering_example_spline_generator.InitializeParams(); + } + + double initial_time, final_time; + initial_time = START_SIMULATION_TIME; + + // Publish the trajectory values until the final values is reached + while(true){ + final_time = ros::Time::now().toSec(); + hovering_example_spline_generator.TrajectoryCallback(&eigen_reference, &final_time, &initial_time); + + mav_msgs::eigenDroneFromStateToMsg(&eigen_reference, trajectory_msg); + trajectory_pub.publish(trajectory_msg); + + ROS_DEBUG("Publishing waypoint from msg: [%f, %f, %f].", trajectory_msg.position.x, trajectory_msg.position.y, trajectory_msg.position.z); + ROS_DEBUG("Publishing waypoint: [%f, %f, %f].", eigen_reference.position_W[0], eigen_reference.position_W[1], eigen_reference.position_W[2]); + + ros::Duration(SAMPLING_TIME).sleep(); + + if(eigen_reference.position_W[0] >= check_position_final[0] && eigen_reference.position_W[1] >= check_position_final[1] && eigen_reference.position_W[2] >= check_position_final[2]) + break; + } + + ros::spin(); + + return 0; +} From 0d408f59226808218584263cec0183942a6b22b5 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:48:46 +0100 Subject: [PATCH 11/21] Update CHANGELOG --- rotors_gazebo/CHANGELOG.rst | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/rotors_gazebo/CHANGELOG.rst b/rotors_gazebo/CHANGELOG.rst index efa3e64..860af35 100644 --- a/rotors_gazebo/CHANGELOG.rst +++ b/rotors_gazebo/CHANGELOG.rst @@ -4,6 +4,10 @@ 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) ------------------ From d09e3fd4eff785963a2f9e1c5f1ec12413baa650 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:51:40 +0100 Subject: [PATCH 12/21] Update PACKAGE.XML file --- rotors_gazebo/package.xml | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/rotors_gazebo/package.xml b/rotors_gazebo/package.xml index 63131f3..dbf4900 100644 --- a/rotors_gazebo/package.xml +++ b/rotors_gazebo/package.xml @@ -1,6 +1,6 @@ rotors_gazebo - 6.0.1 + 6.0.2 The rotors_gazebo package Giuseppe Silano @@ -10,6 +10,7 @@ Emanuele Aucone Benjamin Rodriguez Luigi Iannelli + Ria Sonecha ASL 2.0 From 6023ece838710fee493a97421817ad979ff1e20c Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Sat, 8 Feb 2020 09:52:53 +0100 Subject: [PATCH 13/21] Update CMakeList file --- rotors_gazebo/CMakeLists.txt | 14 +++++++++----- 1 file changed, 9 insertions(+), 5 deletions(-) diff --git a/rotors_gazebo/CMakeLists.txt b/rotors_gazebo/CMakeLists.txt index 69bd582..26f3173 100644 --- a/rotors_gazebo/CMakeLists.txt +++ b/rotors_gazebo/CMakeLists.txt @@ -13,7 +13,7 @@ find_package(gazebo REQUIRED) if(${gazebo_VERSION_MAJOR} GREATER 2) message(STATUS "Building iris.sdf.") - + set(enable_mavlink_interface "true") set(enable_ground_truth "false") set(enable_logging "false") @@ -21,7 +21,7 @@ if(${gazebo_VERSION_MAJOR} GREATER 2) set(enable_wind "false") set(rotors_description_dir "${CMAKE_CURRENT_SOURCE_DIR}/models/rotors_description") set(scripts_dir "${CMAKE_CURRENT_SOURCE_DIR}/scripts") - + # Creates shell commands to generate .sdf file add_custom_command(OUTPUT ${CMAKE_CURRENT_SOURCE_DIR}/models/iris/iris.sdf WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} @@ -42,7 +42,7 @@ if(NO_ROS) message(STATUS "NO_ROS = true, not building waypoint publisher or hovering example, returning now.") return() endif() - + find_package(catkin REQUIRED COMPONENTS gazebo_msgs @@ -82,6 +82,10 @@ add_executable(hovering_example src/hovering_example.cpp) target_link_libraries(hovering_example ${catkin_LIBRARIES}) add_dependencies(hovering_example ${catkin_EXPORTED_TARGETS}) +add_executable(hovering_example_spline src/hovering_example_spline.cpp) +target_link_libraries(hovering_example_spline ${catkin_LIBRARIES}) +add_dependencies(hovering_example_spline ${catkin_EXPORTED_TARGETS}) + add_executable(quaternion_to_rpy src/quaternion_to_rpy.cpp) target_link_libraries(quaternion_to_rpy ${catkin_LIBRARIES}) add_dependencies(quaternion_to_rpy ${catkin_EXPORTED_TARGETS}) @@ -91,14 +95,14 @@ foreach(dir launch models resource worlds) DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/${dir}) endforeach(dir) -install(TARGETS waypoint_publisher waypoint_publisher_file hovering_example quaternion_to_rpy +install(TARGETS waypoint_publisher waypoint_publisher_file hovering_example quaternion_to_rpy hovering_example_spline ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) install( - DIRECTORY include/${PROJECT_NAME}/ + DIRECTORY include/${PROJECT_NAME}/ DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} FILES_MATCHING PATTERN "*.h" ) From a565424446ef66288c352c60d3bb44c4ef140378 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Tue, 14 Apr 2020 15:48:44 +0200 Subject: [PATCH 14/21] Update CHANGELOG and PACKAGES files --- rotors_comm/CHANGELOG.rst | 8 +++++++- rotors_comm/package.xml | 2 +- rotors_control/CHANGELOG.rst | 9 ++++++++- rotors_control/package.xml | 5 +++-- rotors_description/CHANGELOG.rst | 8 +++++++- rotors_description/package.xml | 2 +- rotors_evaluation/CHANGELOG.rst | 8 +++++++- rotors_evaluation/package.xml | 4 ++-- rotors_gazebo/CHANGELOG.rst | 14 +++++++++++++- rotors_gazebo/package.xml | 2 +- rotors_gazebo_plugins/CHANGELOG.rst | 8 +++++++- rotors_gazebo_plugins/package.xml | 2 +- rotors_joy_interface/CHANGELOG.rst | 10 +++++++++- rotors_joy_interface/package.xml | 2 +- rotors_simulator/CHANGELOG.rst | 8 +++++++- rotors_simulator/package.xml | 2 +- rqt_rotors/CHANGELOG.rst | 8 +++++++- rqt_rotors/package.xml | 2 +- 18 files changed, 84 insertions(+), 20 deletions(-) diff --git a/rotors_comm/CHANGELOG.rst b/rotors_comm/CHANGELOG.rst index a8c9481..e07aa8b 100644 --- a/rotors_comm/CHANGELOG.rst +++ b/rotors_comm/CHANGELOG.rst @@ -2,7 +2,13 @@ Changelog for package rotors_comm ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -6.0.2 (20XX-XX-XX) +6.0.4 (2020-04-14) +------------------ + +6.0.3 (2020-03-22) +------------------ + +6.0.2 (2020-02-09) ------------------ 6.0.1 (2019-12-28) diff --git a/rotors_comm/package.xml b/rotors_comm/package.xml index 45d6be0..4f69e85 100644 --- a/rotors_comm/package.xml +++ b/rotors_comm/package.xml @@ -1,6 +1,6 @@ rotors_comm - 6.0.1 + 6.0.4 RotorS specific messages and services. Giuseppe Silano diff --git a/rotors_control/CHANGELOG.rst b/rotors_control/CHANGELOG.rst index 002cec5..43ef1bb 100644 --- a/rotors_control/CHANGELOG.rst +++ b/rotors_control/CHANGELOG.rst @@ -2,12 +2,19 @@ Changelog for package rotors_control ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -6.0.2 (20XX-XX-XX) +6.0.4 (2020-04-14) ------------------ +* Contributors: Ria Sonecha, Giuseppe Silano + +6.0.3 (2020-03-22) +------------------ +* Add data saving feature +* Contributors: Giuseppe Silano 6.0.2 (2020-01-18) ------------------ * 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 6.0.1 (2019-12-28) diff --git a/rotors_control/package.xml b/rotors_control/package.xml index 2a2d967..4b0d88e 100644 --- a/rotors_control/package.xml +++ b/rotors_control/package.xml @@ -1,14 +1,15 @@ rotors_control - 6.0.1 + 6.0.4 RotorS control package Giuseppe Silano Luigi Iannelli - + Giuseppe Silano Emanuele Aucone Benjamin Rodriguez + Ria Sonecha Luigi Iannelli ASL 2.0 diff --git a/rotors_description/CHANGELOG.rst b/rotors_description/CHANGELOG.rst index c54b4f0..6e2a045 100644 --- a/rotors_description/CHANGELOG.rst +++ b/rotors_description/CHANGELOG.rst @@ -2,7 +2,13 @@ Changelog for package rotors_description ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -6.0.2 (20XX-XX-XX) +6.0.4 (2020-04-14) +------------------ + +6.0.3 (2020-03-22) +------------------ + +6.0.2 (2020-02-09) ------------------ 6.0.1 (2019-12-28) diff --git a/rotors_description/package.xml b/rotors_description/package.xml index f9b944f..934345b 100644 --- a/rotors_description/package.xml +++ b/rotors_description/package.xml @@ -1,6 +1,6 @@ rotors_description - 6.0.1 + 6.0.4 The rotors_description package provides URDF models of the AscTec multicopters. Giuseppe Silano diff --git a/rotors_evaluation/CHANGELOG.rst b/rotors_evaluation/CHANGELOG.rst index 6be1737..cda8d56 100644 --- a/rotors_evaluation/CHANGELOG.rst +++ b/rotors_evaluation/CHANGELOG.rst @@ -2,7 +2,13 @@ Changelog for package rotors_evaluation ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -6.0.2 (20XX-XX-XX) +6.0.4 (2020-04-14) +------------------ + +6.0.3 (2020-03-22) +------------------ + +6.0.2 (2020-02-09) ------------------ 6.0.1 (2019-12-28) diff --git a/rotors_evaluation/package.xml b/rotors_evaluation/package.xml index fa8707d..f06829f 100644 --- a/rotors_evaluation/package.xml +++ b/rotors_evaluation/package.xml @@ -1,12 +1,12 @@ rotors_evaluation - 6.0.1 + 6.0.4 The dataset evaluation package for the RotorS simulator. Giuseppe Silano Luigi Iannelli - + Giuseppe Silano Emanuele Aucone Benjamin Rodriguez diff --git a/rotors_gazebo/CHANGELOG.rst b/rotors_gazebo/CHANGELOG.rst index 860af35..e903245 100644 --- a/rotors_gazebo/CHANGELOG.rst +++ b/rotors_gazebo/CHANGELOG.rst @@ -2,13 +2,25 @@ Changelog for package rotors_gazebo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -6.0.2 (20XX-XX-XX) +6.0.4 (2020-04-14) ------------------ * 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.3 (2020-03-22) +------------------ +* Add data saving features in crazyflie2_hovering_example.launch +* Delete useless plots in crazyflie2_hovering_example.launch +* Contributors: Giuseppe Silano + +6.0.2 (2020-02-09) +------------------ +* Add resource file for the Crazyflie's on-board controller when the joystick interface is active +* Add lunch file for piloting the Crazyflie with the joystick +* Contributors: 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/package.xml b/rotors_gazebo/package.xml index dbf4900..4ca1a38 100644 --- a/rotors_gazebo/package.xml +++ b/rotors_gazebo/package.xml @@ -1,6 +1,6 @@ rotors_gazebo - 6.0.2 + 6.0.4 The rotors_gazebo package Giuseppe Silano diff --git a/rotors_gazebo_plugins/CHANGELOG.rst b/rotors_gazebo_plugins/CHANGELOG.rst index 1da3868..bf82db5 100644 --- a/rotors_gazebo_plugins/CHANGELOG.rst +++ b/rotors_gazebo_plugins/CHANGELOG.rst @@ -2,7 +2,13 @@ Changelog for package rotors_gazebo_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -6.0.2 (20XX-XX-XX) +6.0.4 (2020-04-14) +------------------ + +6.0.3 (2020-03-22) +------------------ + +6.0.2 (2020-02-09) ------------------ 6.0.1 (2019-12-28) diff --git a/rotors_gazebo_plugins/package.xml b/rotors_gazebo_plugins/package.xml index 890e246..bd00b2f 100644 --- a/rotors_gazebo_plugins/package.xml +++ b/rotors_gazebo_plugins/package.xml @@ -1,6 +1,6 @@ rotors_gazebo_plugins - 6.0.1 + 6.0.4 The rotors_gazebo_plugins package Giuseppe Silano diff --git a/rotors_joy_interface/CHANGELOG.rst b/rotors_joy_interface/CHANGELOG.rst index bbf5ddd..70d130e 100644 --- a/rotors_joy_interface/CHANGELOG.rst +++ b/rotors_joy_interface/CHANGELOG.rst @@ -2,9 +2,17 @@ Changelog for package rotors_joy_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -6.0.2 (20XX-XX-XX) +6.0.4 (2020-04-14) ------------------ +6.0.3 (2020-03-22) +------------------ + +6.0.2 (2019-12-28) +------------------ +* Add joystick interface for the Crazyflie +* Contributors: Giuseppe Silano + 6.0.1 (2019-12-28) ------------------ diff --git a/rotors_joy_interface/package.xml b/rotors_joy_interface/package.xml index 7ee5851..30b3eb5 100644 --- a/rotors_joy_interface/package.xml +++ b/rotors_joy_interface/package.xml @@ -1,6 +1,6 @@ rotors_joy_interface - 6.0.1 + 6.0.4 The rotors_joy_interface package to control MAVs with a joystick Giuseppe Silano diff --git a/rotors_simulator/CHANGELOG.rst b/rotors_simulator/CHANGELOG.rst index 918c80a..e03044f 100644 --- a/rotors_simulator/CHANGELOG.rst +++ b/rotors_simulator/CHANGELOG.rst @@ -2,7 +2,13 @@ Changelog for package rotors_simulator ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -6.0.2 (20XX-XX-XX) +6.0.4 (2020-04-14) +------------------ + +6.0.3 (2020-03-22) +------------------ + +6.0.2 (2020-02-09) ------------------ 6.0.1 (2019-12-28) diff --git a/rotors_simulator/package.xml b/rotors_simulator/package.xml index 8ae9319..d35b256 100644 --- a/rotors_simulator/package.xml +++ b/rotors_simulator/package.xml @@ -1,6 +1,6 @@ rotors_simulator - 6.0.1 + 6.0.4 CrazyS is an extension ROS package of RotorS aimed to modeling, developing and integrating the Crazyflie nano-quadcopter. Giuseppe Silano diff --git a/rqt_rotors/CHANGELOG.rst b/rqt_rotors/CHANGELOG.rst index 7c9b4b9..874799d 100644 --- a/rqt_rotors/CHANGELOG.rst +++ b/rqt_rotors/CHANGELOG.rst @@ -2,7 +2,13 @@ Changelog for package rqt_rotors ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ -6.0.2 (20XX-XX-XX) +6.0.4 (2020-04-14) +------------------ + +6.0.3 (2020-03-22) +------------------ + +6.0.2 (2020-02-09) ------------------ 6.0.1 (2019-12-28) diff --git a/rqt_rotors/package.xml b/rqt_rotors/package.xml index 9f1472c..8dc1c7c 100644 --- a/rqt_rotors/package.xml +++ b/rqt_rotors/package.xml @@ -1,6 +1,6 @@ rqt_rotors - 6.0.1 + 6.0.4 The rqt_rotors package Giuseppe Silano From 41fe87037048be58d977a261e945efc28c2091cc Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Tue, 14 Apr 2020 19:07:52 +0200 Subject: [PATCH 15/21] Add INDI and Mellinger's controllers --- rotors_control/CHANGELOG.rst | 2 + .../controller_parameters_mellinger.h | 101 ++++ .../internal_model_controller.h | 177 ++++++ .../rotors_control/mellinger_controller.h | 101 ++++ .../include/rotors_control/parameters.h | 26 +- .../include/rotors_control/parameters_ros.h | 56 ++ .../include/rotors_control/stabilizer_types.h | 6 +- .../src/library/internal_model_controller.cpp | 560 ++++++++++++++++++ .../src/library/mellinger_controller.cpp | 525 ++++++++++++++++ .../src/nodes/position_controller_node.cpp | 278 ++++++--- .../src/nodes/position_controller_node.h | 21 +- 11 files changed, 1771 insertions(+), 82 deletions(-) create mode 100644 rotors_control/include/rotors_control/controller_parameters_mellinger.h create mode 100644 rotors_control/include/rotors_control/internal_model_controller.h create mode 100644 rotors_control/include/rotors_control/mellinger_controller.h create mode 100644 rotors_control/src/library/internal_model_controller.cpp create mode 100644 rotors_control/src/library/mellinger_controller.cpp diff --git a/rotors_control/CHANGELOG.rst b/rotors_control/CHANGELOG.rst index 43ef1bb..442c04b 100644 --- a/rotors_control/CHANGELOG.rst +++ b/rotors_control/CHANGELOG.rst @@ -4,6 +4,8 @@ Changelog for package rotors_control 6.0.4 (2020-04-14) ------------------ +* Improvements in the include folder +* Add INDI and Mellinger's controllers * Contributors: Ria Sonecha, Giuseppe Silano 6.0.3 (2020-03-22) diff --git a/rotors_control/include/rotors_control/controller_parameters_mellinger.h b/rotors_control/include/rotors_control/controller_parameters_mellinger.h new file mode 100644 index 0000000..7a3974d --- /dev/null +++ b/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 diff --git a/rotors_control/include/rotors_control/internal_model_controller.h b/rotors_control/include/rotors_control/internal_model_controller.h new file mode 100644 index 0000000..0a9d3d4 --- /dev/null +++ b/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 +#include +#include + +#include + +#include + +#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 lambda_x_, lambda_y_, lambda_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 diff --git a/rotors_control/include/rotors_control/mellinger_controller.h b/rotors_control/include/rotors_control/mellinger_controller.h new file mode 100644 index 0000000..2e2b955 --- /dev/null +++ b/rotors_control/include/rotors_control/mellinger_controller.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 CRAZYFLIE_2_MELLINGER_CONTROLLER_H + #define CRAZYFLIE_2_MELLINGER_CONTROLLER_H + + #include + #include + #include + + #include + #include + #include + #include + #include + #include + #include + #include + #include + + #include "common.h" + #include "controller_parameters_mellinger.h" + #include "parameters.h" + #include "stabilizer_types.h" + + namespace rotors_control { + + class MellingerController{ + public: + MellingerController(); + ~MellingerController(); + void CalculateRotorVelocities(Eigen::Vector4d* rotor_velocities); + + void SetOdometryWithoutStateEstimator(const EigenOdometry& odometry); + void SetTrajectoryPoint(const mav_msgs::EigenDroneState& command_trajectory); + void SetControllerGains(); + void SetVehicleParameters(); + void SetSensorData(const sensorData_t& sensors); + + MellingerControllerParameters controller_parameters_mellinger_; + VehicleParameters vehicle_parameters_; + + private: + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + bool controller_active_; + + control_s control_t_; + + //Vehicle parameters + double bf_, m_, g_; + double l_, bm_; + double Ix_, Iy_, Iz_; + + Eigen::Vector2f kpXYPositionController_, kdXYPositionController_, kiXYPositionController_; + Eigen::Vector2f krXYPositionController_, kwXYPositionController_, ki_mXYPositionController_; + Eigen::Vector2f iRangeMXY_, iRangeXY_, kdOmegaRP_; + double kpZPositionController_, kdZPositionController_, kiZPositionController_; + double krZPositionController_, kwZPositionController_, ki_mZPositionController_; + double iRangeMZ_, iRangeZ_; + double prev_setpoint_omega_pitch_, prev_setpoint_omega_roll_; + double prev_omega_roll_, prev_omega_pitch_; + double i_error_m_x_, i_error_m_y_, i_error_m_z_; + double i_error_x_, i_error_y_, i_error_z_; + + mav_msgs::EigenDroneState command_trajectory_; + EigenOdometry odometry_; + state_t state_; + sensorData_t sensors_; + + void GetMoments(double* u_2, double* u_3, double* u_4); + void GetForce(double* u_1); + void GetThrust(Eigen::Vector3f* target_thrust); + void Quaternion2Euler(double* roll, double* pitch, double* yaw) const; + void Quaternion2EulerCommandTrajectory(double* roll, double* pitch, double* yaw) const; + void Vnormalize(Eigen::Vector3f* vectorOutput, Eigen::Vector3f vectorInput); + void Vcross(Eigen::Vector3f* vectorOutput, Eigen::Vector3f vectorA, Eigen::Vector3f vectorB); + void Vsub(Eigen::Vector3f* vectorOutput, Eigen::Vector3f vectorA, point_t vectorB); + void Vdot(double* output, Eigen::Vector3f vectorA, Eigen::Vector3f vectorB); + void Clamp(double* value, double min, double max); + void SetDroneState(); + void ControllerMellingerReset(); + + + }; + + } + #endif // CRAZYFLIE_2_MELLINGER_CONTROLLER_H diff --git a/rotors_control/include/rotors_control/parameters.h b/rotors_control/include/rotors_control/parameters.h index 0411764..cde551a 100644 --- a/rotors_control/include/rotors_control/parameters.h +++ b/rotors_control/include/rotors_control/parameters.h @@ -1,3 +1,21 @@ +/* + * 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_CONTROL_PARAMETERS_H_ #define INCLUDE_ROTORS_CONTROL_PARAMETERS_H_ @@ -76,10 +94,16 @@ class VehicleParameters { : mass_(kDefaultMass), gravity_(kDefaultGravity), inertia_(Eigen::Vector3d(kDefaultInertiaXx, kDefaultInertiaYy, - kDefaultInertiaZz).asDiagonal()) {} + kDefaultInertiaZz).asDiagonal()), + bf_(kDefaultRotorForceConstant), + bm_(kDefaultRotorMomentConstant), + armLength_(kDefaultArmLength) {} double mass_; const double gravity_; Eigen::Matrix3d inertia_; + double bm_; + double bf_; + double armLength_; RotorConfiguration rotor_configuration_; }; diff --git a/rotors_control/include/rotors_control/parameters_ros.h b/rotors_control/include/rotors_control/parameters_ros.h index ba3193b..47dd855 100644 --- a/rotors_control/include/rotors_control/parameters_ros.h +++ b/rotors_control/include/rotors_control/parameters_ros.h @@ -1,3 +1,21 @@ +/* + * 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_CONTROL_PARAMETERS_ROS_H_ #define INCLUDE_ROTORS_CONTROL_PARAMETERS_ROS_H_ @@ -72,6 +90,44 @@ inline void GetVehicleParameters(const ros::NodeHandle& nh, VehicleParameters* v &vehicle_parameters->inertia_(2, 2)); GetRotorConfiguration(nh, &vehicle_parameters->rotor_configuration_); } + +inline void GetFullVehicleParameters(const ros::NodeHandle& nh, VehicleParameters* vehicle_parameters) { + GetRosParameter(nh, "mass", + vehicle_parameters->mass_, + &vehicle_parameters->mass_); + GetRosParameter(nh, "inertia/xx", + vehicle_parameters->inertia_(0, 0), + &vehicle_parameters->inertia_(0, 0)); + GetRosParameter(nh, "inertia/xy", + vehicle_parameters->inertia_(0, 1), + &vehicle_parameters->inertia_(0, 1)); + vehicle_parameters->inertia_(1, 0) = vehicle_parameters->inertia_(0, 1); + GetRosParameter(nh, "inertia/xz", + vehicle_parameters->inertia_(0, 2), + &vehicle_parameters->inertia_(0, 2)); + vehicle_parameters->inertia_(2, 0) = vehicle_parameters->inertia_(0, 2); + GetRosParameter(nh, "inertia/yy", + vehicle_parameters->inertia_(1, 1), + &vehicle_parameters->inertia_(1, 1)); + GetRosParameter(nh, "inertia/yz", + vehicle_parameters->inertia_(1, 2), + &vehicle_parameters->inertia_(1, 2)); + vehicle_parameters->inertia_(2, 1) = vehicle_parameters->inertia_(1, 2); + GetRosParameter(nh, "inertia/zz", + vehicle_parameters->inertia_(2, 2), + &vehicle_parameters->inertia_(2, 2)); + GetRosParameter(nh, "bf", + vehicle_parameters->bf_, + &vehicle_parameters->bf_); + GetRosParameter(nh, "bm", + vehicle_parameters->bm_, + &vehicle_parameters->bm_); + GetRosParameter(nh, "l", + vehicle_parameters->armLength_, + &vehicle_parameters->armLength_); +} + + } #endif /* INCLUDE_ROTORS_CONTROL_PARAMETERS_ROS_H_ */ diff --git a/rotors_control/include/rotors_control/stabilizer_types.h b/rotors_control/include/rotors_control/stabilizer_types.h index 8eeb68e..0e037f8 100644 --- a/rotors_control/include/rotors_control/stabilizer_types.h +++ b/rotors_control/include/rotors_control/stabilizer_types.h @@ -1,6 +1,7 @@ /* - * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy + * Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy + * Copyright 2020 Ria Sonecha, MIT, USA * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -121,7 +122,8 @@ typedef struct state_s { point_t position; // m velocity_t angularVelocity; // rad/s acc_t angularAcc; // m/s^2 - velocity_t linearVelocity; //m/s + acc_t linearAcceleration; // m/s^2 + velocity_t linearVelocity; //m/s } state_t; typedef struct control_s { diff --git a/rotors_control/src/library/internal_model_controller.cpp b/rotors_control/src/library/internal_model_controller.cpp new file mode 100644 index 0000000..a119cef --- /dev/null +++ b/rotors_control/src/library/internal_model_controller.cpp @@ -0,0 +1,560 @@ +/* + * 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. + */ + +#include "rotors_control/internal_model_controller.h" +#include "rotors_control/transform_datatypes.h" +#include "rotors_control/Matrix3x3.h" +#include "rotors_control/Quaternion.h" +#include "rotors_control/stabilizer_types.h" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#include +#include + + +#define M_PI 3.14159265358979323846 /* pi */ +#define TsP 10e-3 /* Position control sampling time */ +#define TsA 5e-3 /* Attitude control sampling time */ +#define MAX_ROTOR_VELOCITY 2618 /* Max rotors velocity [rad/s] */ +#define MIN_ROTOR_VELOCITY 0 /* Min rotors velocity [rad/s] */ +#define POW_MAX_ROTOR_VELOCITY MAX_ROTOR_VELOCITY*MAX_ROTOR_VELOCITY /* Squared max rotors velocity [rad/s] */ + +using namespace std; + +namespace rotors_control { + +InternalModelController::InternalModelController() + : controller_active_(false), + e_x_(0), + e_y_(0), + e_z_(0), + dot_e_x_(0), + dot_e_y_(0), + dot_e_z_(0), + e_phi_(0), + e_theta_(0), + e_psi_(0), + dot_e_phi_(0), + dot_e_theta_(0), + dot_e_psi_(0), + bf_(0), + l_(0), + bm_(0), + m_(0), + g_(0), + Ix_(0), + Iy_(0), + Iz_(0), + beta_x_(0), + beta_y_(0), + beta_z_(0), + beta_phi_(0), + beta_theta_(0), + beta_psi_(0), + alpha_x_(0), + alpha_y_(0), + alpha_z_(0), + alpha_phi_(0), + alpha_theta_(0), + alpha_psi_(0), + mu_x_(0), + mu_y_(0), + mu_z_(0), + mu_phi_(0), + mu_theta_(0), + mu_psi_(0), + K_x_1_(0), + K_y_1_(0), + K_z_1_(0), + K_x_2_(0), + K_y_2_(0), + K_z_2_(0), + lambda_x_(0), + lambda_y_(0), + lambda_z_(0), + control_({0,0,0,0}), //roll, pitch, yaw rate, thrust + state_({0, //Position.x + 0, //Position.y + 0, //Position.z + 0, //Linear velocity x + 0, //Linear velocity y + 0, //Linear velocity z + 0, //Quaternion x + 0, //Quaternion y + 0, //Quaternion z + 0, //Quaternion w + 0, //Angular velocity x + 0, //Angular velocity y + 0}) //Angular velocity z) + { + + // Command signals initialization + command_trajectory_.position_W[0] = 0; + command_trajectory_.position_W[1] = 0; + command_trajectory_.position_W[2] = 0; + + // Timers set the outer and inner loops working frequency + timer1_ = n1_.createTimer(ros::Duration(TsA), &InternalModelController::CallbackAttitude, this, false, true); + timer2_ = n2_.createTimer(ros::Duration(TsP), &InternalModelController::CallbackPosition, this, false, true); + + +} + +//The library Destructor +InternalModelController::~InternalModelController() {} + +// The function moves the controller gains read from controller_bebop.yaml file to private variables of the class. +// These variables will be employed during the simulation +void InternalModelController::SetControllerGains(){ + + beta_x_ = controller_parameters_im_.beta_xy_.x(); + beta_y_ = controller_parameters_im_.beta_xy_.y(); + beta_z_ = controller_parameters_im_.beta_z_; + + beta_phi_ = controller_parameters_im_.beta_phi_; + beta_theta_ = controller_parameters_im_.beta_theta_; + beta_psi_ = controller_parameters_im_.beta_psi_; + + alpha_x_ = 1 - beta_x_; + alpha_y_ = 1 - beta_y_; + alpha_z_ = 1 - beta_z_; + + alpha_phi_ = 1 - beta_phi_; + alpha_theta_ = 1 - beta_theta_; + alpha_psi_ = 1 - beta_psi_; + + mu_x_ = controller_parameters_im_.mu_xy_.x(); + mu_y_ = controller_parameters_im_.mu_xy_.y(); + mu_z_ = controller_parameters_im_.mu_z_; + + mu_phi_ = controller_parameters_im_.mu_phi_; + mu_theta_ = controller_parameters_im_.mu_theta_; + mu_psi_ = controller_parameters_im_.mu_psi_; + + lambda_x_ = controller_parameters_im_.U_q_.x(); + lambda_y_ = controller_parameters_im_.U_q_.y(); + lambda_z_ = controller_parameters_im_.U_q_.z(); + + K_x_1_ = 1/mu_x_; + K_x_2_ = -2 * (beta_x_/mu_x_); + + K_y_1_ = 1/mu_y_; + K_y_2_ = -2 * (beta_y_/mu_y_); + + K_z_1_ = 1/mu_z_; + K_z_2_ = -2 * (beta_z_/mu_z_); + +} + +// As SetControllerGains, the function is used to set the vehicle parameters into private variables of the class +void InternalModelController::SetVehicleParameters(){ + + bf_ = vehicle_parameters_.bf_; + l_ = vehicle_parameters_.armLength_; + bm_ = vehicle_parameters_.bm_; + m_ = vehicle_parameters_.mass_; + g_ = vehicle_parameters_.gravity_; + Ix_ = vehicle_parameters_.inertia_(0,0); + Iy_ = vehicle_parameters_.inertia_(1,1); + Iz_ = vehicle_parameters_.inertia_(2,2); + +} + +// The functions is used to convert the drone attitude from quaternion to Euler angles +void InternalModelController::Quaternion2Euler(double* roll, double* pitch, double* yaw) const { + assert(roll); + assert(pitch); + assert(yaw); + + double x, y, z, w; + x = odometry_.orientation.x(); + y = odometry_.orientation.y(); + z = odometry_.orientation.z(); + w = odometry_.orientation.w(); + + tf::Quaternion q(x, y, z, w); + tf::Matrix3x3 m(q); + m.getRPY(*roll, *pitch, *yaw); + +} + +// When a new odometry message comes, the content of the message is stored in private variable. At the same time, the controller is going to be active. +// The attitude of the aircraft is computer (as we said before it move from quaterninon to Euler angles) and also the angular velocity is stored. +void InternalModelController::SetOdometryWithoutStateEstimator(const EigenOdometry& odometry) { + + odometry_ = odometry; + controller_active_= true; + + Quaternion2Euler(&state_.attitude.roll, &state_.attitude.pitch, &state_.attitude.yaw); + + state_.angularVelocity.x = odometry_.angular_velocity[0]; + state_.angularVelocity.y = odometry_.angular_velocity[1]; + state_.angularVelocity.z = odometry_.angular_velocity[2]; + + state_.position.x = odometry_.position[0]; + state_.position.y = odometry_.position[1]; + state_.position.z = odometry_.position[2]; + + state_.linearVelocity.x = odometry_.velocity[0]; + state_.linearVelocity.y = odometry_.velocity[1]; + state_.linearVelocity.z = odometry_.velocity[2]; + +} + +// The function sets the filter trajectory points +void InternalModelController::SetTrajectoryPoint(const mav_msgs::EigenDroneState& command_trajectory) { + + command_trajectory_ = command_trajectory; + +} + +// The function computes the propellers angular velocity +void InternalModelController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocities) { + assert(rotor_velocities); + + // The controller is inactive if a point to reach is not coming + if(!controller_active_){ + *rotor_velocities = Eigen::Vector4d::Zero(rotor_velocities->rows()); + return; + } + + double u_phi, u_theta, u_psi; + double u_x, u_y, u_z, u_Terr; + AttitudeController(&u_phi, &u_theta, &u_psi); + PosController(&control_.thrust, &control_.roll, &control_.pitch, &u_x, &u_y, &u_z, &u_Terr); + + ROS_DEBUG("Thrust: %f U_phi: %f U_theta: %f U_psi: %f", control_.thrust, u_phi, u_theta, u_psi); + + double first, second, third, fourth; + first = (1 / ( 4 * bf_ )) * control_.thrust; + second = (1 / (4 * bf_ * l_ * cos(M_PI/4) ) ) * u_phi; + third = (1 / (4 * bf_ * l_ * cos(M_PI/4) ) ) * u_theta; + fourth = (1 / ( 4 * bf_ * bm_)) * u_psi; + + double not_saturated_1, not_saturated_2, not_saturated_3, not_saturated_4; + not_saturated_1 = first - second - third - fourth; + not_saturated_2 = first - second + third + fourth; + not_saturated_3 = first + second + third - fourth; + not_saturated_4 = first + second - third + fourth; + + // The propellers velocities is limited by taking into account the physical constrains + double motorMin=not_saturated_1, motorMax=not_saturated_1, motorFix=0; + + if(not_saturated_2 < motorMin) motorMin = not_saturated_2; + if(not_saturated_2 > motorMax) motorMax = not_saturated_2; + + if(not_saturated_3 < motorMin) motorMin = not_saturated_3; + if(not_saturated_3 > motorMax) motorMax = not_saturated_3; + + if(not_saturated_4 < motorMin) motorMin = not_saturated_4; + if(not_saturated_4 > motorMax) motorMax = not_saturated_4; + + if(motorMin < MIN_ROTOR_VELOCITY) motorFix = MIN_ROTOR_VELOCITY - motorMin; + else if(motorMax > POW_MAX_ROTOR_VELOCITY) motorFix = POW_MAX_ROTOR_VELOCITY - motorMax; + + not_saturated_1 = not_saturated_1 + motorFix; + not_saturated_2 = not_saturated_2 + motorFix; + not_saturated_3 = not_saturated_3 + motorFix; + not_saturated_4 = not_saturated_4 + motorFix; + + // The values have been saturated to avoid the root square of negative values + double saturated_1, saturated_2, saturated_3, saturated_4; + if(not_saturated_1 < 0) + saturated_1 = 0; + else if(not_saturated_1 > POW_MAX_ROTOR_VELOCITY) + saturated_1 = POW_MAX_ROTOR_VELOCITY; + else + saturated_1 = not_saturated_1; + + if(not_saturated_2 < 0) + saturated_2 = 0; + else if(not_saturated_2 > POW_MAX_ROTOR_VELOCITY) + saturated_2 = POW_MAX_ROTOR_VELOCITY; + else + saturated_2 = not_saturated_2; + + if(not_saturated_3 < 0) + saturated_3 = 0; + else if(not_saturated_3 > POW_MAX_ROTOR_VELOCITY) + saturated_3 = POW_MAX_ROTOR_VELOCITY; + else + saturated_3 = not_saturated_3; + + if(not_saturated_4 < 0) + saturated_4 = 0; + else if(not_saturated_4 > POW_MAX_ROTOR_VELOCITY) + saturated_4 = POW_MAX_ROTOR_VELOCITY; + else + saturated_4 = not_saturated_4; + + double omega_1, omega_2, omega_3, omega_4; + omega_1 = sqrt(saturated_1); + omega_2 = sqrt(saturated_2); + omega_3 = sqrt(saturated_3); + omega_4 = sqrt(saturated_4); + + *rotor_velocities = Eigen::Vector4d(omega_1, omega_2, omega_3, omega_4); + + ROS_DEBUG("Omega_1: %f Omega_2: %f Omega_3: %f Omega_4: %f", omega_1, omega_2, omega_3, omega_4); + +} + +// The function computes the velocity errors +void InternalModelController::VelocityErrors(double* dot_e_x, double* dot_e_y, double* dot_e_z){ + assert(dot_e_x); + assert(dot_e_y); + assert(dot_e_z); + + double dot_x, dot_y, dot_z, theta, psi, phi; + + theta = state_.attitude.pitch; + psi = state_.attitude.yaw; + phi = state_.attitude.roll; + + dot_x = (cos(theta) * cos(psi) * state_.linearVelocity.x) + + ( ( (sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) ) ) * state_.linearVelocity.y) + + ( ( (cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) ) ) * state_.linearVelocity.z); + + dot_y = (cos(theta) * sin(psi) * state_.linearVelocity.x) + + ( ( (sin(phi) * sin(theta) * sin(psi) ) + ( cos(phi) * cos(psi) ) ) * state_.linearVelocity.y) + + ( ( (cos(phi) * sin(theta) * sin(psi) ) - ( sin(phi) * cos(psi) ) ) * state_.linearVelocity.z); + + dot_z = (-sin(theta) * state_.linearVelocity.x) + ( sin(phi) * cos(theta) * state_.linearVelocity.y) + + (cos(phi) * cos(theta) * state_.linearVelocity.z); + + *dot_e_x = command_trajectory_.velocity[0] - dot_x; + *dot_e_y = command_trajectory_.velocity[1] - dot_y; + *dot_e_z = command_trajectory_.velocity[2] - dot_z; +} + +// The function computes the position errors +void InternalModelController::PositionErrors(double* e_x, double* e_y, double* e_z){ + assert(e_x); + assert(e_y); + assert(e_z); + + *e_x = command_trajectory_.position_W[0] - state_.position.x; + *e_y = command_trajectory_.position_W[1] - state_.position.y; + *e_z = command_trajectory_.position_W[2] - state_.position.z; + + ROS_DEBUG("Position Error components: [%f %f]", command_trajectory_.position_W[2], state_.position.z); + ROS_DEBUG("Position Error: [%f %f %f]", *e_x, *e_y, *e_z); + +} + +// The function computes the position controller outputs +void InternalModelController::PosController(double* u_T, double* phi_r, double* theta_r, double* u_x, double* u_y, double* u_z, double* u_Terr){ + assert(u_T); + assert(phi_r); + assert(theta_r); + assert(u_x); + assert(u_y); + assert(u_z); + assert(u_Terr); + + //u_x computing + *u_x = ( (e_x_ * K_x_1_ * K_x_2_)/lambda_x_ ) + ( (dot_e_x_ * K_x_2_)/lambda_x_ ); + + if (*u_x > 1 || *u_x <-1) + if (*u_x > 1) + *u_x = 1; + else + *u_x = -1; + + *u_x = (*u_x * 1/2) + ( (K_x_1_/lambda_x_) * dot_e_x_ ); + + if (*u_x > 1 || *u_x <-1) + if (*u_x > 1) + *u_x = 1; + else + *u_x = -1; + + *u_x = m_ * (*u_x * lambda_x_); + + //u_y computing + *u_y = ( (e_y_ * K_y_1_ * K_y_2_)/lambda_y_ ) + ( (dot_e_y_ * K_y_2_)/lambda_y_ ); + + if (*u_y > 1 || *u_y <-1) + if (*u_y > 1) + *u_y = 1; + else + *u_y = -1; + + *u_y = (*u_y * 1/2) + ( (K_y_1_/lambda_y_) * dot_e_y_ ); + + if (*u_y > 1 || *u_y <-1) + if (*u_y > 1) + *u_y = 1; + else + *u_y = -1; + + *u_y = m_* ( *u_y * lambda_y_); + + //u_z computing + *u_z = ( (e_z_ * K_z_1_ * K_z_2_)/lambda_z_ ) + ( (dot_e_z_ * K_z_2_)/lambda_z_ ); + + if (*u_z > 1 || *u_z <-1) + if (*u_z > 1) + *u_z = 1; + else + *u_z = -1; + + *u_z = (*u_z * 1/2) + ( (K_z_1_/lambda_z_) * dot_e_z_ ); + + if (*u_z > 1 || *u_z <-1) + if (*u_z > 1) + *u_z = 1; + else + *u_z = -1; + + *u_z = m_* ( *u_z * lambda_z_); + + //u_Terr computing + *u_Terr = *u_z + (m_ * g_); + + //u_T computing + *u_T = sqrt( pow(*u_x,2) + pow(*u_y,2) + pow(*u_Terr,2) ); + + ROS_DEBUG("Position Control Signals: [%f %f %f %f]", *u_x, *u_y, *u_z, *u_T); + + + double psi_r, temp_a, temp_b; + Quaternion2EulerCommandTrajectory(&temp_a, &temp_b, &psi_r); + + *theta_r = atan( ( (*u_x * cos(psi_r) ) + ( *u_y * sin(psi_r) ) ) / *u_Terr ); + + *phi_r = atan( cos(*theta_r) * ( ( (*u_x * sin(psi_r)) - (*u_y * cos(psi_r)) ) / (*u_Terr) ) ); + +} + +//The function computes the attitude errors +void InternalModelController::AttitudeErrors(double* e_phi, double* e_theta, double* e_psi){ + assert(e_phi); + assert(e_theta); + assert(e_psi); + + double psi_r, roll_r, pitch_r; + Quaternion2EulerCommandTrajectory(&roll_r, &pitch_r, &psi_r); + + *e_phi = roll_r - state_.attitude.roll; + *e_theta = pitch_r - state_.attitude.pitch; + *e_psi = psi_r - state_.attitude.yaw; + + ROS_INFO("Angular Error: [%f %f %f]", state_.attitude.roll, state_.attitude.pitch, state_.attitude.yaw); + ROS_DEBUG("Angular Error: [%f %f %f]", *e_phi, *e_theta, *e_psi); + +} + +//The angular velocity errors +void InternalModelController::AngularVelocityErrors(double* dot_e_phi, double* dot_e_theta, double* dot_e_psi){ + assert(dot_e_phi); + assert(dot_e_theta); + assert(dot_e_psi); + + double psi_r, roll_r, pitch_r; + Quaternion2EulerCommandTrajectory(&roll_r, &pitch_r, &psi_r); + + double dot_e_phi_W_s, dot_e_theta_W_s, dot_e_psi_W_s, dot_e_phi_W_d, dot_e_theta_W_d, dot_e_psi_W_d; + + + dot_e_phi_W_s = state_.angularVelocity.x + (sin(state_.attitude.roll) * tan(state_.attitude.pitch) * state_.angularVelocity.y) + + (cos(state_.attitude.roll) * tan(state_.attitude.pitch) * state_.angularVelocity.z); + + dot_e_theta_W_s = (cos(state_.attitude.roll) * state_.angularVelocity.y) - (sin(state_.attitude.roll) * state_.angularVelocity.z); + + dot_e_psi_W_s = ( ( sin(state_.attitude.roll) * state_.angularVelocity.y) / cos(state_.attitude.pitch) ) + + ( ( cos(state_.attitude.roll) * state_.angularVelocity.z) / cos(state_.attitude.pitch) ); + + dot_e_phi_W_d = command_trajectory_.angular_velocity_B[0] + (sin(roll_r) * tan(pitch_r) * command_trajectory_.angular_velocity_B[1]) + + (cos(roll_r) * tan(pitch_r) * command_trajectory_.angular_velocity_B[2]); + + dot_e_theta_W_d = (cos(roll_r) * command_trajectory_.angular_velocity_B[1]) - (sin(roll_r) * command_trajectory_.angular_velocity_B[2]); + + dot_e_psi_W_d = ( ( sin(roll_r) * command_trajectory_.angular_velocity_B[1]) / cos(pitch_r) ) + + ( ( cos(roll_r) * command_trajectory_.angular_velocity_B[2]) / cos(pitch_r) ); + + *dot_e_phi = dot_e_phi_W_d - dot_e_phi_W_s; + *dot_e_theta = dot_e_theta_W_d - dot_e_theta_W_s; + *dot_e_psi = dot_e_psi_W_d - dot_e_psi_W_s; + +} + +void InternalModelController::Quaternion2EulerCommandTrajectory(double* roll, double* pitch, double* yaw) const { + assert(roll); + assert(pitch); + assert(yaw); + + // The estimated quaternion values + double x, y, z, w; + x = command_trajectory_.orientation_W_B.x(); + y = command_trajectory_.orientation_W_B.y(); + z = command_trajectory_.orientation_W_B.z(); + w = command_trajectory_.orientation_W_B.w(); + + tf::Quaternion q(x, y, z, w); + tf::Matrix3x3 m(q); + m.getRPY(*roll, *pitch, *yaw); + + ROS_DEBUG("Roll Trajectory: %f, Pitch Trajectory: %f, Yaw Trajectory: %f", *roll, *pitch, *yaw); + +} + +//The function computes the attitude controller outputs +void InternalModelController::AttitudeController(double* u_phi, double* u_theta, double* u_psi){ + assert(u_phi); + assert(u_theta); + assert(u_psi); + + *u_phi = Ix_ * ( ( ( (alpha_phi_/mu_phi_ ) * dot_e_phi_ ) - ( (beta_phi_/pow(mu_phi_,2) ) * e_phi_ ) ) - ( ( (Iy_ - Iz_)/(Ix_ * mu_theta_ * mu_psi_) ) * e_theta_ * e_psi_) ); + *u_theta = Iy_ * ( ( ( (alpha_theta_/mu_theta_) * dot_e_theta_) - ( (beta_theta_/pow(mu_theta_,2)) * e_theta_) ) - ( ( (Iz_ - Ix_)/(Iy_ * mu_phi_ * mu_psi_ ) ) * e_phi_ * e_psi_) ); + *u_psi = Iz_ * ( ( ( (alpha_psi_/mu_psi_ ) * dot_e_psi_ ) - ( (beta_psi_/pow(mu_psi_,2) ) * e_psi_ ) ) - ( ( (Ix_ - Iy_)/(Iz_ * mu_theta_ * mu_phi_) ) * e_theta_ * e_phi_) ); + +} + + +//The function every TsA computes the attitude and angular velocity errors. When the data storing is active, the data are saved into csv files +void InternalModelController::CallbackAttitude(const ros::TimerEvent& event){ + + AttitudeErrors(&e_phi_, &e_theta_, &e_psi_); + AngularVelocityErrors(&dot_e_phi_, &dot_e_theta_, &dot_e_psi_); +} + +//The function every TsP: +// * the next point to follow has generated (the output of the waypoint filter) +// * the output of the waypoint filter is put into the command_trajectory_ data structure +// * the EKF is used to estimate the drone attitude and linear velocity +// * the position and velocity errors are computed +// * the last part is used to store the data into csv files if the data storing is active +void InternalModelController::CallbackPosition(const ros::TimerEvent& event){ + + PositionErrors(&e_x_, &e_y_, &e_z_); + VelocityErrors(&dot_e_x_, &dot_e_y_, &dot_e_z_); + +} + + +} diff --git a/rotors_control/src/library/mellinger_controller.cpp b/rotors_control/src/library/mellinger_controller.cpp new file mode 100644 index 0000000..7fe70fd --- /dev/null +++ b/rotors_control/src/library/mellinger_controller.cpp @@ -0,0 +1,525 @@ +/* + * 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. + */ + +#include "rotors_control/transform_datatypes.h" +#include "rotors_control/Matrix3x3.h" +#include "rotors_control/Quaternion.h" +#include "rotors_control/mellinger_controller.h" +#include "rotors_control/controller_parameters_mellinger.h" + +#include +#include +#include +#include + +#include +#include + +#define SAMPLING_TIME 0.002 /* SAMPLING TIME [s] - 500Hz*/ +#define M_PI 3.14159265358979323846 /* pi [rad]*/ +#define MAX_ROTOR_VELOCITY 2618 /* Max rotors velocity [rad/s] */ +#define MIN_ROTOR_VELOCITY 0 /* Min rotors velocity [rad/s] */ +#define POW_MAX_ROTOR_VELOCITY MAX_ROTOR_VELOCITY*MAX_ROTOR_VELOCITY /* Squared max rotors velocity [rad/s] */ + +namespace rotors_control{ + + MellingerController::MellingerController() + : prev_setpoint_omega_roll_(0), + prev_setpoint_omega_pitch_(0), + prev_omega_roll_(0), + prev_omega_pitch_(0), + i_error_m_x_(0), + i_error_m_y_(0), + i_error_m_z_(0), + i_error_x_(0), + i_error_y_(0), + i_error_z_(0){ + + state_.attitude.roll = 0; // Roll + state_.attitude.pitch = 0; // Pitch + state_.attitude.yaw = 0; // Yaw + + state_.position.x = 0; // Position.x + state_.position.y = 0; // Position.y + state_.position.z = 0; // Position.z + + state_.angularVelocity.x = 0; // Angular velocity x + state_.angularVelocity.y = 0; // Angular velocity y + state_.angularVelocity.z = 0; // Angular velocity z + + state_.linearVelocity.x = 0; //Linear velocity x + state_.linearVelocity.y = 0; //Linear velocity y + state_.linearVelocity.z = 0; //Linear velocity z + + state_.attitudeQuaternion.x = 0; // Quaternion x + state_.attitudeQuaternion.y = 0; // Quaternion y + state_.attitudeQuaternion.z = 0; // Quaternion z + state_.attitudeQuaternion.w = 0; // Quaternion w + } + + MellingerController::~MellingerController() {} + + void MellingerController::CalculateRotorVelocities(Eigen::Vector4d* rotor_velocities) { + assert(rotor_velocities); + + GetForce(&control_t_.thrust); + + GetMoments(&control_t_.roll, &control_t_.pitch, &control_t_.yawRate); +/* + if (control_t_.thrust > 0) { + Clamp(&control_t_.pitch, -M_PI/6, M_PI/6); + Clamp(&control_t_.roll, -M_PI/6, M_PI/6); + Clamp(&control_t_.yawRate, -M_PI/6, M_PI/6); + + } else { + control_t_.roll = 0; + control_t_.pitch = 0; + control_t_.yawRate = 0; + + ControllerMellingerReset(); + }*/ + + ROS_DEBUG("thrust, roll, pitch, yaw: [%f, %f, %f, %f].", control_t_.thrust, control_t_.roll, control_t_.pitch, control_t_.yawRate); + + double first, second, third, fourth; + first = (1 / ( 4 * bf_ )) * control_t_.thrust; + second = (1 / (4 * bf_ * l_ * cos(M_PI/4) ) ) * control_t_.roll; + third = (1 / (4 * bf_ * l_ * cos(M_PI/4) ) ) * control_t_.pitch; + fourth = (1 / ( 4 * bf_ * bm_)) * control_t_.yawRate; + + + double not_saturated_1, not_saturated_2, not_saturated_3, not_saturated_4; + not_saturated_1 = first - second - third - fourth; + not_saturated_2 = first - second + third + fourth; + not_saturated_3 = first + second + third - fourth; + not_saturated_4 = first + second - third + fourth; + + // The propellers velocities is limited by taking into account the physical constrains + double motorMin=not_saturated_1, motorMax=not_saturated_1, motorFix=0; + + if(not_saturated_2 < motorMin) motorMin = not_saturated_2; + if(not_saturated_2 > motorMax) motorMax = not_saturated_2; + + if(not_saturated_3 < motorMin) motorMin = not_saturated_3; + if(not_saturated_3 > motorMax) motorMax = not_saturated_3; + + if(not_saturated_4 < motorMin) motorMin = not_saturated_4; + if(not_saturated_4 > motorMax) motorMax = not_saturated_4; + + if(motorMin < MIN_ROTOR_VELOCITY) motorFix = MIN_ROTOR_VELOCITY - motorMin; + else if(motorMax > POW_MAX_ROTOR_VELOCITY) motorFix = POW_MAX_ROTOR_VELOCITY - motorMax; + + not_saturated_1 = not_saturated_1 + motorFix; + not_saturated_2 = not_saturated_2 + motorFix; + not_saturated_3 = not_saturated_3 + motorFix; + not_saturated_4 = not_saturated_4 + motorFix; + + // The values have been saturated to avoid the root square of negative values + double saturated_1, saturated_2, saturated_3, saturated_4; + if(not_saturated_1 < 0) + saturated_1 = 0; + else if(not_saturated_1 > POW_MAX_ROTOR_VELOCITY) + saturated_1 = POW_MAX_ROTOR_VELOCITY; + else + saturated_1 = not_saturated_1; + + if(not_saturated_2 < 0) + saturated_2 = 0; + else if(not_saturated_2 > POW_MAX_ROTOR_VELOCITY) + saturated_2 = POW_MAX_ROTOR_VELOCITY; + else + saturated_2 = not_saturated_2; + + if(not_saturated_3 < 0) + saturated_3 = 0; + else if(not_saturated_3 > POW_MAX_ROTOR_VELOCITY) + saturated_3 = POW_MAX_ROTOR_VELOCITY; + else + saturated_3 = not_saturated_3; + + if(not_saturated_4 < 0) + saturated_4 = 0; + else if(not_saturated_4 > POW_MAX_ROTOR_VELOCITY) + saturated_4 = POW_MAX_ROTOR_VELOCITY; + else + saturated_4 = not_saturated_4; + + double omega_1, omega_2, omega_3, omega_4; + omega_1 = sqrt(saturated_1); + omega_2 = sqrt(saturated_2); + omega_3 = sqrt(saturated_3); + omega_4 = sqrt(saturated_4); + + *rotor_velocities = Eigen::Vector4d(omega_1, omega_2, omega_3, omega_4); + + ROS_DEBUG("Propellers angular velocities: [%f, %f, %f, %f].", omega_1, omega_2, omega_3, omega_4); + } + + void MellingerController::GetForce(double* u_1){ + assert(u_1); + + tf::Quaternion q(state_.attitudeQuaternion.x, state_.attitudeQuaternion.y, state_.attitudeQuaternion.z, state_.attitudeQuaternion.w); + + tfScalar d = q.length2(); + tfFullAssert(d != tfScalar(0.0)); + tfScalar s = tfScalar(2.0) / d; + tfScalar xs = q.x() * s, ys = q.y() * s, zs = q.z() * s; + tfScalar wx = q.w() * xs, wy = q.w() * ys, wz = q.w() * zs; + tfScalar xx = q.x() * xs, xy = q.x() * ys, xz = q.x() * zs; + tfScalar yy = q.y() * ys, yz = q.y() * zs, zz = q.z() * zs; + + Eigen::Vector3f z_axis, target_thrust; + z_axis = Eigen::Vector3f(xz + wy, yz - wx, tfScalar(1.0) - (xx + yy)); + GetThrust(&target_thrust); + + ROS_DEBUG("Z AXIS: [%f, %f, %f]", z_axis[0], z_axis[1], z_axis[2]); + ROS_DEBUG("THRUST: [%f, %f, %f]", target_thrust[0], target_thrust[1], target_thrust[2]); + + double tempU1; + Vdot(&tempU1, target_thrust, z_axis); + *u_1 = tempU1; + ROS_DEBUG("FORCE: [%f]", tempU1); + + } + + void MellingerController::GetMoments(double* u_2, double* u_3, double* u_4){ + assert(u_2); + assert(u_3); + assert(u_4); + + Eigen::Vector3f target_thrust, x_c_desired, z_axis_desired, x_axis_desired, y_axis_desired; + GetThrust(&target_thrust); + + Vnormalize(&z_axis_desired, target_thrust); + double rollDesired, pitchDesired, yawDesired; + Quaternion2EulerCommandTrajectory(&rollDesired, &pitchDesired, &yawDesired); + x_c_desired.x() = cos(yawDesired); + x_c_desired.y() = sin(yawDesired); + x_c_desired.z() = 0; + + Eigen::Vector3f tempVector; + Vcross(&tempVector, z_axis_desired, x_c_desired); + Vnormalize(&y_axis_desired, tempVector); + Vcross(&x_axis_desired, y_axis_desired, z_axis_desired); + + double x, y, z, w; + // Quaternions - Drone orientation + x = state_.attitudeQuaternion.x; + y = state_.attitudeQuaternion.y; + z = state_.attitudeQuaternion.z; + w = state_.attitudeQuaternion.w; + + ROS_DEBUG("y axis des: [%f, %f, %f].", y_axis_desired.x(), y_axis_desired.y(), y_axis_desired.z()); + ROS_DEBUG("z axis des: [%f, %f, %f].", z_axis_desired.x(), z_axis_desired.y(), z_axis_desired.z()); + ROS_DEBUG("QUAT: [%f, %f, %f, %f].", x, y, z, w); + + Eigen::Vector3f eR; + + eR.x() = (-1 + 2*pow(x,2) + 2*pow(y,2))*y_axis_desired.z() + z_axis_desired.y() + - 2*(x*y_axis_desired.x()*z + y*y_axis_desired.y()*z - x*y*z_axis_desired.x() + + pow(x,2)*z_axis_desired.y() + pow(z,2)*z_axis_desired.y() - y*z*z_axis_desired.z()) + + 2*w*(-(y*y_axis_desired.x()) - z*z_axis_desired.x() + x*(y_axis_desired.y() + z_axis_desired.z())); + + eR.y() = x_axis_desired.z() - z_axis_desired.x() - 2*(pow(x,2)*x_axis_desired.z() + y*(x_axis_desired.z()*y - x_axis_desired.y()*z) + - (pow(y,2) + pow(z,2))*z_axis_desired.x() + x*(-(x_axis_desired.x()*z) + y*z_axis_desired.y() + z*z_axis_desired.z()) + + w*(x*x_axis_desired.y() + z*z_axis_desired.y() - y*(x_axis_desired.x() + z_axis_desired.z()))); + + eR.z() = y_axis_desired.x() - 2*(y*(x*x_axis_desired.x() + y*y_axis_desired.x() - x*y_axis_desired.y()) + + w*(x*x_axis_desired.z() + y*y_axis_desired.z())) + 2*(-(x_axis_desired.z()*y) + + w*(x_axis_desired.x() + y_axis_desired.y()) + x*y_axis_desired.z())*z - 2*y_axis_desired.x()*pow(z,2) + + x_axis_desired.y()*(-1 + 2*pow(x,2) + 2*pow(z,2)); + + + //omega error is the opposite of how it is calculated in the paper + Eigen::Vector3f ew; + ew.x() = command_trajectory_.angular_velocity_B[0] - state_.angularVelocity.x; + ew.y() = command_trajectory_.angular_velocity_B[1] - state_.angularVelocity.y; + ew.z() = command_trajectory_.angular_velocity_B[2] - state_.angularVelocity.z; + + ROS_DEBUG("eR: [%f, %f, %f].", eR.x(), eR.y(), eR.z()); + ROS_DEBUG("ew: [%f, %f, %f].", ew.x(), ew.y(), ew.z()); + ROS_DEBUG("command trajectory angular velocity: [%f, %f, %f].", command_trajectory_.angular_velocity_B[0], command_trajectory_.angular_velocity_B[1], command_trajectory_.angular_velocity_B[2]); + ROS_DEBUG("state angular velocity: [%f, %f, %f].", state_.angularVelocity.x, state_.angularVelocity.y, state_.angularVelocity.z); + + + double err_d_roll, err_d_pitch; + /*d part initialized*/ + err_d_roll = ((command_trajectory_.angular_velocity_B[0] - prev_setpoint_omega_roll_) + - (state_.angularVelocity.x - prev_omega_roll_)) / SAMPLING_TIME ; + err_d_pitch = ((command_trajectory_.angular_velocity_B[1] - prev_setpoint_omega_pitch_) + - (state_.angularVelocity.y - prev_omega_pitch_)) / SAMPLING_TIME ; + + prev_omega_roll_ = state_.angularVelocity.x; + prev_omega_pitch_ = state_.angularVelocity.y; + prev_setpoint_omega_roll_ = command_trajectory_.angular_velocity_B[0]; + prev_setpoint_omega_pitch_ = command_trajectory_.angular_velocity_B[1]; + + i_error_m_x_ = i_error_m_x_ - (eR.x() * SAMPLING_TIME); + Clamp(&i_error_m_x_, -iRangeMXY_.x(), iRangeMXY_.x()); + i_error_m_y_ = i_error_m_y_ - (eR.y() * SAMPLING_TIME); + Clamp(&i_error_m_y_, -iRangeMXY_.y(), iRangeMXY_.y()); + i_error_m_z_ = i_error_m_z_ - (eR.z() * SAMPLING_TIME); + Clamp(&i_error_m_z_, -iRangeMZ_, iRangeMZ_); + + ROS_DEBUG("integral error: [%f, %f, %f].", i_error_m_x_, i_error_m_y_, i_error_m_z_); + ROS_DEBUG("error d: [%f, %f].", err_d_roll, err_d_pitch); + + *u_2 = -krXYPositionController_.x() * eR.x() + kwXYPositionController_.x() * ew.x() + + ki_mXYPositionController_.x() * i_error_m_x_;//+ kdOmegaRP_.x() * err_d_roll; + *u_3 = -krXYPositionController_.y() * eR.y() + kwXYPositionController_.y() * ew.y() + + ki_mXYPositionController_.y() * i_error_m_y_; //+ kdOmegaRP_.y() * err_d_pitch; + *u_4 = -krZPositionController_ * eR.z() + kwZPositionController_ * ew.z() + + ki_mZPositionController_ * i_error_m_z_; + } + + // normalize a vector (make a unit vector). + void MellingerController::Vnormalize(Eigen::Vector3f* vectorOutput, Eigen::Vector3f vectorInput) { + assert(vectorOutput); + + double a_x, a_y, a_z; + a_x = vectorInput.x(); + a_y = vectorInput.y(); + a_z = vectorInput.z(); + + double length, x, y, z; + length = sqrt((a_x * a_x) + (a_y * a_y) + (a_z * a_z)); + + x = a_x/length; + y = a_y/length; + z = a_z/length; + + *vectorOutput = Eigen::Vector3f(x, y, z); + } + + // vector cross product + void MellingerController::Vcross(Eigen::Vector3f* vectorOutput, Eigen::Vector3f vectorA, Eigen::Vector3f vectorB){ + assert(vectorOutput); + + double firstElement, secondElement, thirdElement; + firstElement = vectorA.y()*vectorB.z() - vectorA.z()*vectorB.y(); + secondElement = vectorA.z()*vectorB.x() - vectorA.x()*vectorB.z(); + thirdElement = vectorA.x()*vectorB.y() - vectorA.y()*vectorB.x(); + + *vectorOutput = Eigen::Vector3f(firstElement, secondElement, thirdElement); + } + + // subtract vectors + void MellingerController::Vsub(Eigen::Vector3f* vectorOutput, Eigen::Vector3f vectorA, point_t vectorB){ + assert(vectorOutput); + + double first, second, third; + first = vectorA.x()-vectorB.x; + second = vectorA.y()-vectorB.y; + third = vectorA.z()-vectorB.z; + *vectorOutput = Eigen::Vector3f(first, second, third); + } + + void MellingerController::Vdot(double* output, Eigen::Vector3f vectorA, Eigen::Vector3f vectorB){ + assert(output); + + *output = (vectorA.x()*vectorB.x()) + (vectorA.y()*vectorB.y()) + (vectorA.z()*vectorB.z()); + } + + void MellingerController::Clamp(double* value, double min, double max){ + assert(value); + + if (*value < min) *value = min; + if (*value > max) *value = max; + } + + void MellingerController::GetThrust(Eigen::Vector3f* target_thrust){ + assert(target_thrust); + + //in section IV of the paper error is defined as current position-desired position + //here we use desired position-current position + Eigen::Vector3f r_error, v_error; + Vsub(&r_error, command_trajectory_.position_W, state_.position); + Vsub(&v_error, command_trajectory_.velocity, state_.linearVelocity); + + //double g_vehicleMass; + //g_vehicleMass = m_ * g_; + + i_error_x_ = i_error_x_ + r_error.x() * SAMPLING_TIME ; + Clamp(&i_error_x_, -iRangeXY_.x(), iRangeXY_.x()); + i_error_y_ = i_error_y_ + r_error.y() * SAMPLING_TIME ; + Clamp(&i_error_y_, -iRangeXY_.y(), iRangeXY_.y()); + i_error_z_ = i_error_z_ + r_error.z() * SAMPLING_TIME ; + Clamp(&i_error_z_, -iRangeZ_, iRangeZ_); + + double first, second, third; + first = m_ * command_trajectory_.acceleration[0] + + kpXYPositionController_.x() * r_error.x() + kdXYPositionController_.x() * v_error.x() + + kiXYPositionController_.x() * i_error_x_; + + second = m_ * command_trajectory_.acceleration[1] + + kpXYPositionController_.y() * r_error.y() + kdXYPositionController_.y() * v_error.y() + + kiXYPositionController_.y() * i_error_y_; + + third = m_ * (command_trajectory_.acceleration[2] + g_) + + kpZPositionController_ * r_error.z() + kdZPositionController_ * v_error.z() + + kiZPositionController_ * i_error_z_; + + *target_thrust = Eigen::Vector3f(first, second, third); + ROS_DEBUG("v_error: [%f, %f, %f].", v_error[0], v_error[1], v_error[2]); + ROS_DEBUG("gravity: [%f].", g_); + ROS_DEBUG("thrust: [%f, %f, %f].", first, second, third); + + } + + // Acceleration comes from an ideal IMU sensor + void MellingerController::SetSensorData(const sensorData_t& sensors) { + + sensors_ = sensors; + + } + + void MellingerController::SetControllerGains(){ + + kpXYPositionController_ = Eigen::Vector2f(controller_parameters_mellinger_.kpXYPositionController_.x(), controller_parameters_mellinger_.kpXYPositionController_.y()); + kdXYPositionController_ = Eigen::Vector2f(controller_parameters_mellinger_.kdXYPositionController_.x(), controller_parameters_mellinger_.kdXYPositionController_.y()); + kiXYPositionController_ = Eigen::Vector2f(controller_parameters_mellinger_.kiXYPositionController_.x(), controller_parameters_mellinger_.kiXYPositionController_.y()); + + kpZPositionController_ = controller_parameters_mellinger_.kpZPositionController_; + kdZPositionController_ = controller_parameters_mellinger_.kdZPositionController_; + kiZPositionController_ = controller_parameters_mellinger_.kiZPositionController_; + + krXYPositionController_ = Eigen::Vector2f(controller_parameters_mellinger_.krXYPositionController_.x(), controller_parameters_mellinger_.krXYPositionController_.y()); + kwXYPositionController_ = Eigen::Vector2f(controller_parameters_mellinger_.kwXYPositionController_.x(), controller_parameters_mellinger_.kwXYPositionController_.y()); + ki_mXYPositionController_ = Eigen::Vector2f(controller_parameters_mellinger_.ki_mXYPositionController_.x(), controller_parameters_mellinger_.ki_mXYPositionController_.y()); + + krZPositionController_ = controller_parameters_mellinger_.krZPositionController_; + kwZPositionController_ = controller_parameters_mellinger_.kwZPositionController_; + ki_mZPositionController_ = controller_parameters_mellinger_.ki_mZPositionController_; + + iRangeMXY_ = Eigen::Vector2f(controller_parameters_mellinger_.iRangeMXY_.x(), controller_parameters_mellinger_.iRangeMXY_.y()); + iRangeXY_ = Eigen::Vector2f(controller_parameters_mellinger_.iRangeXY_.x(), controller_parameters_mellinger_.iRangeXY_.y()); + + iRangeMZ_ = controller_parameters_mellinger_.iRangeMZ_; + iRangeZ_ = controller_parameters_mellinger_.iRangeZ_; + + kdOmegaRP_ = Eigen::Vector2f(controller_parameters_mellinger_.kdOmegaRP_.x(), controller_parameters_mellinger_.kdOmegaRP_.y()); + + } + + void MellingerController::SetVehicleParameters(){ + + bf_ = vehicle_parameters_.bf_; + l_ = vehicle_parameters_.armLength_; + bm_ = vehicle_parameters_.bm_; + m_ = vehicle_parameters_.mass_; + g_ = vehicle_parameters_.gravity_; + Ix_ = vehicle_parameters_.inertia_(0,0); + Iy_ = vehicle_parameters_.inertia_(1,1); + Iz_ = vehicle_parameters_.inertia_(2,2); + } + + void MellingerController::SetOdometryWithoutStateEstimator(const EigenOdometry& odometry) { + odometry_ = odometry; + + SetDroneState(); + } + + void MellingerController::SetDroneState(){ + + // Only the position sensor is ideal, any virtual sensor or systems is available to get it + state_.position.x = odometry_.position[0]; + state_.position.y = odometry_.position[1]; + state_.position.z = odometry_.position[2]; + + state_.attitudeQuaternion.x = odometry_.orientation.x(); + state_.attitudeQuaternion.y = odometry_.orientation.y(); + state_.attitudeQuaternion.z = odometry_.orientation.z(); + state_.attitudeQuaternion.w = odometry_.orientation.w(); + + // rotating linear velocity from body to inertial frame + double theta, phi, psi; + Quaternion2Euler(&phi, &theta, &psi); + + state_.linearVelocity.x = (cos(theta) * cos(psi) * odometry_.velocity[0]) + + ( ( (sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) ) ) * odometry_.velocity[1]) + + ( ( (cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) ) ) * odometry_.velocity[2]); + + state_.linearVelocity.y = (cos(theta) * sin(psi) * odometry_.velocity[0]) + + ( ( (sin(phi) * sin(theta) * sin(psi) ) + ( cos(phi) * cos(psi) ) ) * odometry_.velocity[1]) + + ( ( (cos(phi) * sin(theta) * sin(psi) ) - ( sin(phi) * cos(psi) ) ) * odometry_.velocity[2]); + + state_.linearVelocity.z = (-sin(theta) * odometry_.velocity[0]) + ( sin(phi) * cos(theta) * odometry_.velocity[1]) + + (cos(phi) * cos(theta) * odometry_.velocity[2]); + + + ROS_DEBUG("quaternion [%f, %f, %f, %f]", odometry_.orientation.x(), odometry_.orientation.y(), odometry_.orientation.z(), odometry_.orientation.w()); + + state_.angularVelocity.x = odometry_.angular_velocity[0]; + state_.angularVelocity.y = odometry_.angular_velocity[1]; + state_.angularVelocity.z = odometry_.angular_velocity[2]; + + } + + void MellingerController::SetTrajectoryPoint(const mav_msgs::EigenDroneState& command_trajectory) { + command_trajectory_= command_trajectory; + controller_active_= true; + } + + void MellingerController::Quaternion2EulerCommandTrajectory(double* roll, double* pitch, double* yaw) const { + assert(roll); + assert(pitch); + assert(yaw); + + // The estimated quaternion values + double x, y, z, w; + x = command_trajectory_.orientation_W_B.x(); + y = command_trajectory_.orientation_W_B.y(); + z = command_trajectory_.orientation_W_B.z(); + w = command_trajectory_.orientation_W_B.w(); + + tf::Quaternion q(x, y, z, w); + tf::Matrix3x3 m(q); + m.getRPY(*roll, *pitch, *yaw); + + ROS_DEBUG("Roll Trajectory: %f, Pitch Trajectory: %f, Yaw Trajectory: %f", *roll, *pitch, *yaw); + + } + + void MellingerController::Quaternion2Euler(double* roll, double* pitch, double* yaw) const { + assert(roll); + assert(pitch); + assert(yaw); + + // The estimated quaternion values + double x, y, z, w; + x = state_.attitudeQuaternion.x; + y = state_.attitudeQuaternion.y; + z = state_.attitudeQuaternion.z; + w = state_.attitudeQuaternion.w; + + tf::Quaternion q(x, y, z, w); + tf::Matrix3x3 m(q); + m.getRPY(*roll, *pitch, *yaw); + + ROS_DEBUG("Roll: %f, Pitch: %f, Yaw: %f", *roll, *pitch, *yaw); + + } + + void MellingerController::ControllerMellingerReset(void) { + i_error_x_ = 0; + i_error_y_ = 0; + i_error_z_ = 0; + i_error_m_x_ = 0; + i_error_m_y_ = 0; + i_error_m_z_ = 0; + } +} diff --git a/rotors_control/src/nodes/position_controller_node.cpp b/rotors_control/src/nodes/position_controller_node.cpp index 2fb8cf3..0277754 100644 --- a/rotors_control/src/nodes/position_controller_node.cpp +++ b/rotors_control/src/nodes/position_controller_node.cpp @@ -1,6 +1,9 @@ /* - * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy + * Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy + * Copyright 2018 Emanuele Aucone, University of Sannio in Benevento, Italy + * Copyright 2018 Benjamin Rodriguez, MIT, USA + * Copyright 2020 Ria Sonecha, MIT, USA * * Licensed under the Apache License, Version 2.0 (the "License"); * you may not use this file except in compliance with the License. @@ -19,7 +22,7 @@ #include #include -#include +#include #include #include #include @@ -38,14 +41,41 @@ PositionControllerNode::PositionControllerNode() { ROS_INFO_ONCE("Started position controller"); - InitializeParams(); - ros::NodeHandle nh; ros::NodeHandle pnh_node("~"); - cmd_multi_dof_joint_trajectory_sub_ = nh.subscribe(mav_msgs::default_topics::COMMAND_TRAJECTORY, 1, &PositionControllerNode::MultiDofJointTrajectoryCallback, this); + // This command diplays which controller is enabled + if(pnh_node.getParam("enable_mellinger_controller", enable_mellinger_controller_)){ + ROS_INFO("Got param 'enable_mellinger_controller': %d", enable_mellinger_controller_); + } + + if(pnh_node.getParam("enable_internal_model_controller", enable_internal_model_controller_)){ + ROS_INFO("Got param 'enable_internal_model_controller': %d", enable_internal_model_controller_); + } + + InitializeParams(); + + if (!enable_mellinger_controller_ && !enable_internal_model_controller_){ - odometry_sub_ = nh.subscribe(mav_msgs::default_topics::ODOMETRY, 1, &PositionControllerNode::OdometryCallback, this); + cmd_multi_dof_joint_trajectory_sub_ = nh.subscribe(mav_msgs::default_topics::COMMAND_TRAJECTORY, 1, + &PositionControllerNode::MultiDofJointTrajectoryCallback, this); + + odometry_sub_ = nh.subscribe(mav_msgs::default_topics::ODOMETRY, 1, + &PositionControllerNode::OdometryCallback, this); + + } + else{ + + imu_ideal_sub_ = nh.subscribe(mav_msgs::default_topics::IMU, 1, + &PositionControllerNode::IMUMellingerCallback, this); + + cmd_multi_dof_joint_trajectory_spline_sub_ = nh.subscribe(mav_msgs::default_topics::DRONE_STATE, 1, + &PositionControllerNode::MultiDofJointTrajectoryMellingerCallback, this); + + odometry_sub_ = nh.subscribe(mav_msgs::default_topics::ODOMETRY, 1, + &PositionControllerNode::MellingerOdometryCallback, this); + + } motor_velocity_reference_pub_ = nh.advertise(mav_msgs::default_topics::COMMAND_ACTUATORS, 1); @@ -99,72 +129,107 @@ void PositionControllerNode::MultiDofJointTrajectoryCallback(const trajectory_ms } } +void PositionControllerNode::MultiDofJointTrajectoryMellingerCallback(const mav_msgs::DroneState& drone_state_msg){ + // Clear all pending commands. + command_timer_.stop(); + commands_.clear(); + command_waiting_times_.clear(); + + if(drone_state_msg.position.z <= 0){ + ROS_WARN_STREAM("Got MultiDOFJointTrajectoryMellinger message, but message has no points."); + return; + } + + // We can trigger the first command immediately. + mav_msgs::EigenDroneState eigen_reference; + mav_msgs::eigenDroneStateFromMsg(drone_state_msg, &eigen_reference); + if (enable_mellinger_controller_) + mellinger_controller_.SetTrajectoryPoint(eigen_reference); + else + internal_model_controller_.SetTrajectoryPoint(eigen_reference); + + if (drone_state_msg.position.z > 0) { + waypointHasBeenPublished_ = true; + if (enable_mellinger_controller_) + ROS_INFO_ONCE("PositionController got first [Mellinger] MultiDOFJointTrajectory message."); + else + ROS_INFO_ONCE("PositionController got first [InternalModel] MultiDOFJointTrajectory message."); + } +} + void PositionControllerNode::InitializeParams() { ros::NodeHandle pnh("~"); - // Parameters reading from rosparam. - GetRosParameter(pnh, "xy_gain_kp/x", - position_controller_.controller_parameters_.xy_gain_kp_.x(), - &position_controller_.controller_parameters_.xy_gain_kp_.x()); - GetRosParameter(pnh, "xy_gain_kp/y", - position_controller_.controller_parameters_.xy_gain_kp_.y(), - &position_controller_.controller_parameters_.xy_gain_kp_.y()); - GetRosParameter(pnh, "xy_gain_ki/x", - position_controller_.controller_parameters_.xy_gain_ki_.x(), - &position_controller_.controller_parameters_.xy_gain_ki_.x()); - GetRosParameter(pnh, "xy_gain_ki/y", - position_controller_.controller_parameters_.xy_gain_ki_.y(), - &position_controller_.controller_parameters_.xy_gain_ki_.y()); - - GetRosParameter(pnh, "attitude_gain_kp/phi", - position_controller_.controller_parameters_.attitude_gain_kp_.x(), - &position_controller_.controller_parameters_.attitude_gain_kp_.x()); - GetRosParameter(pnh, "attitude_gain_kp/phi", - position_controller_.controller_parameters_.attitude_gain_kp_.y(), - &position_controller_.controller_parameters_.attitude_gain_kp_.y()); - GetRosParameter(pnh, "attitude_gain_ki/theta", - position_controller_.controller_parameters_.attitude_gain_ki_.x(), - &position_controller_.controller_parameters_.attitude_gain_ki_.x()); - GetRosParameter(pnh, "attitude_gain_ki/phi", - position_controller_.controller_parameters_.attitude_gain_ki_.y(), - &position_controller_.controller_parameters_.attitude_gain_ki_.y()); - - GetRosParameter(pnh, "rate_gain_kp/p", - position_controller_.controller_parameters_.rate_gain_kp_.x(), - &position_controller_.controller_parameters_.rate_gain_kp_.x()); - GetRosParameter(pnh, "rate_gain_kp/q", - position_controller_.controller_parameters_.rate_gain_kp_.y(), - &position_controller_.controller_parameters_.rate_gain_kp_.y()); - GetRosParameter(pnh, "rate_gain_kp/r", - position_controller_.controller_parameters_.rate_gain_kp_.z(), - &position_controller_.controller_parameters_.rate_gain_kp_.z()); - GetRosParameter(pnh, "rate_gain_ki/p", - position_controller_.controller_parameters_.rate_gain_ki_.x(), - &position_controller_.controller_parameters_.rate_gain_ki_.x()); - GetRosParameter(pnh, "rate_gain_ki/q", - position_controller_.controller_parameters_.rate_gain_ki_.y(), - &position_controller_.controller_parameters_.rate_gain_ki_.y()); - GetRosParameter(pnh, "rate_gain_ki/r", - position_controller_.controller_parameters_.rate_gain_ki_.z(), - &position_controller_.controller_parameters_.rate_gain_ki_.z()); - - GetRosParameter(pnh, "yaw_gain_kp/yaw", - position_controller_.controller_parameters_.yaw_gain_kp_, - &position_controller_.controller_parameters_.yaw_gain_kp_); - GetRosParameter(pnh, "yaw_gain_ki/yaw", - position_controller_.controller_parameters_.yaw_gain_ki_, - &position_controller_.controller_parameters_.yaw_gain_ki_); - - GetRosParameter(pnh, "hovering_gain_kp/z", - position_controller_.controller_parameters_.hovering_gain_kp_, - &position_controller_.controller_parameters_.hovering_gain_kp_); - GetRosParameter(pnh, "hovering_gain_ki/z", - position_controller_.controller_parameters_.hovering_gain_ki_, - &position_controller_.controller_parameters_.hovering_gain_ki_); - GetRosParameter(pnh, "hovering_gain_kd/z", - position_controller_.controller_parameters_.hovering_gain_kd_, - &position_controller_.controller_parameters_.hovering_gain_kd_); + if(!enable_mellinger_controller_ && !enable_internal_model_controller_){ + + // Parameters reading from rosparam. + GetRosParameter(pnh, "xy_gain_kp/x", + position_controller_.controller_parameters_.xy_gain_kp_.x(), + &position_controller_.controller_parameters_.xy_gain_kp_.x()); + GetRosParameter(pnh, "xy_gain_kp/y", + position_controller_.controller_parameters_.xy_gain_kp_.y(), + &position_controller_.controller_parameters_.xy_gain_kp_.y()); + GetRosParameter(pnh, "xy_gain_ki/x", + position_controller_.controller_parameters_.xy_gain_ki_.x(), + &position_controller_.controller_parameters_.xy_gain_ki_.x()); + GetRosParameter(pnh, "xy_gain_ki/y", + position_controller_.controller_parameters_.xy_gain_ki_.y(), + &position_controller_.controller_parameters_.xy_gain_ki_.y()); + + GetRosParameter(pnh, "attitude_gain_kp/phi", + position_controller_.controller_parameters_.attitude_gain_kp_.x(), + &position_controller_.controller_parameters_.attitude_gain_kp_.x()); + GetRosParameter(pnh, "attitude_gain_kp/phi", + position_controller_.controller_parameters_.attitude_gain_kp_.y(), + &position_controller_.controller_parameters_.attitude_gain_kp_.y()); + GetRosParameter(pnh, "attitude_gain_ki/theta", + position_controller_.controller_parameters_.attitude_gain_ki_.x(), + &position_controller_.controller_parameters_.attitude_gain_ki_.x()); + GetRosParameter(pnh, "attitude_gain_ki/theta", + position_controller_.controller_parameters_.attitude_gain_ki_.y(), + &position_controller_.controller_parameters_.attitude_gain_ki_.y()); + + GetRosParameter(pnh, "rate_gain_kp/p", + position_controller_.controller_parameters_.rate_gain_kp_.x(), + &position_controller_.controller_parameters_.rate_gain_kp_.x()); + GetRosParameter(pnh, "rate_gain_kp/q", + position_controller_.controller_parameters_.rate_gain_kp_.y(), + &position_controller_.controller_parameters_.rate_gain_kp_.y()); + GetRosParameter(pnh, "rate_gain_kp/r", + position_controller_.controller_parameters_.rate_gain_kp_.z(), + &position_controller_.controller_parameters_.rate_gain_kp_.z()); + GetRosParameter(pnh, "rate_gain_ki/p", + position_controller_.controller_parameters_.rate_gain_ki_.x(), + &position_controller_.controller_parameters_.rate_gain_ki_.x()); + GetRosParameter(pnh, "rate_gain_ki/q", + position_controller_.controller_parameters_.rate_gain_ki_.y(), + &position_controller_.controller_parameters_.rate_gain_ki_.y()); + GetRosParameter(pnh, "rate_gain_ki/r", + position_controller_.controller_parameters_.rate_gain_ki_.z(), + &position_controller_.controller_parameters_.rate_gain_ki_.z()); + + GetRosParameter(pnh, "yaw_gain_kp/yaw", + position_controller_.controller_parameters_.yaw_gain_kp_, + &position_controller_.controller_parameters_.yaw_gain_kp_); + GetRosParameter(pnh, "yaw_gain_ki/yaw", + position_controller_.controller_parameters_.yaw_gain_ki_, + &position_controller_.controller_parameters_.yaw_gain_ki_); + + GetRosParameter(pnh, "hovering_gain_kp/z", + position_controller_.controller_parameters_.hovering_gain_kp_, + &position_controller_.controller_parameters_.hovering_gain_kp_); + GetRosParameter(pnh, "hovering_gain_ki/z", + position_controller_.controller_parameters_.hovering_gain_ki_, + &position_controller_.controller_parameters_.hovering_gain_ki_); + GetRosParameter(pnh, "hovering_gain_kd/z", + position_controller_.controller_parameters_.hovering_gain_kd_, + &position_controller_.controller_parameters_.hovering_gain_kd_); + + position_controller_.SetControllerGains(); + + ROS_INFO_ONCE("[Position Controller] Set controller gains and vehicle parameters"); + } position_controller_.SetControllerGains(); if (enable_state_estimator_) @@ -178,21 +243,42 @@ void PositionControllerNode::Publish(){ void PositionControllerNode::IMUCallback(const sensor_msgs::ImuConstPtr& imu_msg) { ROS_INFO_ONCE("PositionController got first imu message."); - + // Angular velocities data sensors_.gyro.x = imu_msg->angular_velocity.x; sensors_.gyro.y = imu_msg->angular_velocity.y; sensors_.gyro.z = imu_msg->angular_velocity.z; - + // Linear acceleration data sensors_.acc.x = imu_msg->linear_acceleration.x; sensors_.acc.y = imu_msg->linear_acceleration.y; sensors_.acc.z = imu_msg->linear_acceleration.z; - imu_msg_head_stamp_ = imu_msg->header.stamp; + imu_msg_head_stamp_ = imu_msg->header.stamp; } +void PositionControllerNode::IMUMellingerCallback(const sensor_msgs::ImuConstPtr& imu_msg) { + + ROS_INFO_ONCE("MellingerController got first imu message."); + + // Angular velocities data + sensors_.gyro.x = imu_msg->angular_velocity.x; + sensors_.gyro.y = imu_msg->angular_velocity.y; + sensors_.gyro.z = imu_msg->angular_velocity.z; + + // Linear acceleration data + sensors_.acc.x = imu_msg->linear_acceleration.x; + sensors_.acc.y = imu_msg->linear_acceleration.y; + sensors_.acc.z = imu_msg->linear_acceleration.z; + + imu_msg_head_stamp_ = imu_msg->header.stamp; + + position_controller_.SetSensorData(sensors_); + +} + + void PositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) { ROS_INFO_ONCE("PositionController got first odometry message."); @@ -231,7 +317,49 @@ void PositionControllerNode::OdometryCallback(const nav_msgs::OdometryConstPtr& motor_velocity_reference_pub_.publish(actuator_msg); } - + +} + +void PositionControllerNode::MellingerOdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg) { + + if(enable_mellinger_controller_) + ROS_INFO_ONCE("PositionControllerNode got first odometry message (Mellinger)."); + else + ROS_INFO_ONCE("PositionControllerNode got first odometry message (Internal Model Controller)."); + + if(waypointHasBeenPublished_){ + + //This functions allows us to put the odometry message into the odometry variable--> _position, + //_orientation,_velocit_body,_angular_velocity + EigenOdometry odometry; + eigenOdometryFromMsg(odometry_msg, &odometry); + + if(enable_mellinger_controller_) + mellinger_controller_.SetOdometryWithoutStateEstimator(odometry); + else + internal_model_controller_.SetOdometryWithoutStateEstimator(odometry); + + Eigen::Vector4d ref_rotor_velocities; + + if(enable_mellinger_controller_) + mellinger_controller_.CalculateRotorVelocities(&ref_rotor_velocities); + else + internal_model_controller_.CalculateRotorVelocities(&ref_rotor_velocities); + + //creating a new mav message. actuator_msg is used to send the velocities of the propellers. + mav_msgs::ActuatorsPtr actuator_msg(new mav_msgs::Actuators); + + //we use clear because we later want to be sure that we used the previously calculated velocity. + actuator_msg->angular_velocities.clear(); + //for all propellers, we put them into actuator_msg so they will later be used to control the crazyflie. + for (int i = 0; i < ref_rotor_velocities.size(); i++) + actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]); + actuator_msg->header.stamp = odometry_msg->header.stamp; + + motor_velocity_reference_pub_.publish(actuator_msg); + + } + } // The attitude is estimated only if the waypoint has been published @@ -271,7 +399,7 @@ void PositionControllerNode::CallbackIMUUpdate(const ros::TimerEvent& event){ for (int i = 0; i < ref_rotor_velocities.size(); i++) actuator_msg->angular_velocities.push_back(ref_rotor_velocities[i]); actuator_msg->header.stamp = imu_msg_head_stamp_; - + motor_velocity_reference_pub_.publish(actuator_msg); } @@ -283,9 +411,9 @@ void PositionControllerNode::CallbackIMUUpdate(const ros::TimerEvent& event){ int main(int argc, char** argv){ ros::init(argc, argv, "position_controller_node_with_stateEstimator"); - + ros::NodeHandle nh2; - + rotors_control::PositionControllerNode position_controller_node; ros::spin(); diff --git a/rotors_control/src/nodes/position_controller_node.h b/rotors_control/src/nodes/position_controller_node.h index 4751571..fc88011 100644 --- a/rotors_control/src/nodes/position_controller_node.h +++ b/rotors_control/src/nodes/position_controller_node.h @@ -1,5 +1,6 @@ /* - * Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy + * Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy + * Copyright 2020 Ria Sonecha, MIT, USA * Copyright 2018 Emanuele Aucone, University of Sannio in Benevento, Italy * Copyright 2018 Benjamin Rodriguez, MIT, USA * Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy @@ -26,6 +27,7 @@ #include #include +#include #include #include #include @@ -38,6 +40,8 @@ #include "rotors_control/common.h" #include "rotors_control/position_controller.h" +#include "rotors_control/mellinger_controller.h" +#include "rotors_control/internal_model_controller.h" #include "rotors_control/crazyflie_complementary_filter.h" @@ -47,16 +51,20 @@ namespace rotors_control { public: PositionControllerNode(); ~PositionControllerNode(); - + void InitializeParams(); void Publish(); private: bool waypointHasBeenPublished_ = false; - bool enable_state_estimator_; + bool enable_state_estimator_ = false; + bool enable_mellinger_controller_ = false; + bool enable_internal_model_controller_ = false; PositionController position_controller_; + MellingerController mellinger_controller_; + InternalModelController internal_model_controller_; sensorData_t sensors_; ros::Time imu_msg_head_stamp_; @@ -71,11 +79,13 @@ namespace rotors_control { void CallbackAttitudeEstimation(const ros::TimerEvent& event); void CallbackHightLevelControl(const ros::TimerEvent& event); void CallbackIMUUpdate(const ros::TimerEvent& event); - + //subscribers ros::Subscriber cmd_multi_dof_joint_trajectory_sub_; + ros::Subscriber cmd_multi_dof_joint_trajectory_spline_sub_; ros::Subscriber odometry_sub_; ros::Subscriber imu_sub_; + ros::Subscriber imu_ideal_sub_; //publisher ros::Publisher motor_velocity_reference_pub_; @@ -85,10 +95,13 @@ namespace rotors_control { ros::Timer command_timer_; void MultiDofJointTrajectoryCallback(const trajectory_msgs::MultiDOFJointTrajectoryConstPtr& trajectory_reference_msg); + void MultiDofJointTrajectoryMellingerCallback(const mav_msgs::DroneState& drone_state_msg); void OdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg); + void MellingerOdometryCallback(const nav_msgs::OdometryConstPtr& odometry_msg); void IMUCallback(const sensor_msgs::ImuConstPtr& imu_msg); + void IMUMellingerCallback(const sensor_msgs::ImuConstPtr& imu_msg); //When the Mellinger's controller is on }; } From 3ae4cef9ab8121c0c7f6f16679e49d3fbb13a35b Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Tue, 14 Apr 2020 19:17:34 +0200 Subject: [PATCH 16/21] Add ideal IMU sensor --- rotors_description/CHANGELOG.rst | 2 ++ .../urdf/component_snippets.xacro | 28 ++++++++++++++++++- rotors_description/urdf/crazyflie2.xacro | 3 +- rotors_description/urdf/crazyflie2_base.xacro | 13 ++++++--- 4 files changed, 40 insertions(+), 6 deletions(-) diff --git a/rotors_description/CHANGELOG.rst b/rotors_description/CHANGELOG.rst index 6e2a045..f5776c5 100644 --- a/rotors_description/CHANGELOG.rst +++ b/rotors_description/CHANGELOG.rst @@ -4,6 +4,8 @@ Changelog for package rotors_description 6.0.4 (2020-04-14) ------------------ +* Add ideal imu sensor for the Crazyflie +* Contributors: Ria Sonecha, Giuseppe Silano 6.0.3 (2020-03-22) ------------------ diff --git a/rotors_description/urdf/component_snippets.xacro b/rotors_description/urdf/component_snippets.xacro index a5065b3..2306fae 100644 --- a/rotors_description/urdf/component_snippets.xacro +++ b/rotors_description/urdf/component_snippets.xacro @@ -5,7 +5,10 @@ Copyright 2015 Mina Kamel, ASL, ETH Zurich, Switzerland Copyright 2015 Janosch Nikolic, ASL, ETH Zurich, Switzerland Copyright 2015 Markus Achtelik, ASL, ETH Zurich, Switzerland - Copyright 2018 Giuseppe Silano, University of Sannio in Benevento, Italy + Copyright 2020 Ria Sonecha, MIT, USA + Copyright 2018 Benjamin Rodriguez, MIT, USA + Copyright 2018 Emanuele Aucone, University of Sannio in Benevento + Copyright 2020 Giuseppe Silano, University of Sannio in Benevento, Italy Copyright 2018 Luigi Iannelli, University of Sannio in Benevento, Italy Licensed under the Apache License, Version 2.0 (the "License"); @@ -931,6 +934,29 @@ + + + + + + + + - + + + + + + - + @@ -54,4 +60,3 @@ - From 4229a3591f5674c5c227d6eff67d660daffb8654 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Tue, 14 Apr 2020 19:39:14 +0200 Subject: [PATCH 17/21] Add WARNING on MAVLink and fixed issue CMake file in rotors_control --- rotors_control/CMakeLists.txt | 24 +++++++++++++++++++++--- rotors_gazebo_plugins/CMakeLists.txt | 11 +++++------ 2 files changed, 26 insertions(+), 9 deletions(-) diff --git a/rotors_control/CMakeLists.txt b/rotors_control/CMakeLists.txt index 6833b23..5e50471 100644 --- a/rotors_control/CMakeLists.txt +++ b/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,7 +96,8 @@ 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) @@ -88,7 +105,8 @@ add_dependencies(roll_pitch_yawrate_thrust_controller_node ${catkin_EXPORTED_TAR 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} ) diff --git a/rotors_gazebo_plugins/CMakeLists.txt b/rotors_gazebo_plugins/CMakeLists.txt index d1435d8..7ca13e8 100755 --- a/rotors_gazebo_plugins/CMakeLists.txt +++ b/rotors_gazebo_plugins/CMakeLists.txt @@ -204,10 +204,10 @@ PROTOBUF_GENERATE_CPP(PROTO_SRCS PROTO_HDRS ${msgs}) add_library(mav_msgs SHARED ${PROTO_SRCS}) target_link_libraries(mav_msgs ${PROTOBUF_LIBRARY} gazebo_msgs) -# This causes mav_msgs to be linked with every created library in this file from this -# point forward. -# NOTE: This is deprecated, should be using target_link_libraries instead -link_libraries(mav_msgs) +# This causes mav_msgs to be linked with every created library in this file from this +# point forward. +# NOTE: This is deprecated, should be using target_link_libraries instead +link_libraries(mav_msgs) # To solve the issue #506 on RotorS repository # https://github.com/ethz-asl/rotors_simulator/pull/506 @@ -367,7 +367,7 @@ if (BUILD_MAVLINK_INTERFACE_PLUGIN) endif() if(NOT MAVLINK_HEADERS_FOUND) - message(FATAL_ERROR "MAVLink headers were not found. They are required for building MavlinkInterfacePlugin.") + message(WARNING "MAVLink headers were not found. They are required for building MavlinkInterfacePlugin.") endif() # Note that this library includes TWO .cpp files. @@ -520,4 +520,3 @@ if (BUILD_MAVLINK_INTERFACE) endif() #message(FATAL_ERROR "Reached EOF.") - From e5a3417ebf4dc599f015dbd277f1c7020e4d3739 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Wed, 15 Apr 2020 19:31:58 +0200 Subject: [PATCH 18/21] Some changes in the spawn file --- rotors_gazebo/CHANGELOG.rst | 1 + .../launch/spawn_mav_crazyflie.launch | 4 ++++ rotors_gazebo/worlds/basic_crazyflie.world | 22 ++++++++++++++++--- 3 files changed, 24 insertions(+), 3 deletions(-) diff --git a/rotors_gazebo/CHANGELOG.rst b/rotors_gazebo/CHANGELOG.rst index e903245..6dd9e9e 100644 --- a/rotors_gazebo/CHANGELOG.rst +++ b/rotors_gazebo/CHANGELOG.rst @@ -4,6 +4,7 @@ Changelog for package rotors_gazebo 6.0.4 (2020-04-14) ------------------ +* Add INDI and Mellinger's controllers to the spaw_mav_crazyflie.launch file * 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 diff --git a/rotors_gazebo/launch/spawn_mav_crazyflie.launch b/rotors_gazebo/launch/spawn_mav_crazyflie.launch index 5dab7df..a026f9e 100644 --- a/rotors_gazebo/launch/spawn_mav_crazyflie.launch +++ b/rotors_gazebo/launch/spawn_mav_crazyflie.launch @@ -14,11 +14,15 @@ + + + @@ -12,6 +13,7 @@ topic to a ROS topic (or vise versa). --> + EARTH_WGS84 47.3667 @@ -19,24 +21,38 @@ 500.0 0 - + + + + quick - 1000 + 10 1.3 0 0.2 - 100 + 1000 0.001 0.001 1 1000 + 6.0e-06 2.3e-05 -4.2e-05 0 0 -9.8 + + + + false + + + + + + From 0a8f721e9e0200010dbb1cad26f5644931d35f71 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Thu, 16 Apr 2020 16:43:23 +0200 Subject: [PATCH 19/21] Fix typos to run the entire control architecture --- .../src/library/internal_model_controller.cpp | 2 +- .../src/nodes/position_controller_node.cpp | 168 +++++++++++++++++- .../urdf/component_snippets.xacro | 4 +- .../launch/crazyflie2_hovering_example.launch | 34 +--- ...razyflie2_internal_model_controller.launch | 5 +- .../crazyflie2_mellinger_controller.launch | 67 +++++++ rotors_gazebo/worlds/basic_crazyflie.world | 1 + 7 files changed, 248 insertions(+), 33 deletions(-) create mode 100644 rotors_gazebo/launch/crazyflie2_mellinger_controller.launch diff --git a/rotors_control/src/library/internal_model_controller.cpp b/rotors_control/src/library/internal_model_controller.cpp index a119cef..ea8534c 100644 --- a/rotors_control/src/library/internal_model_controller.cpp +++ b/rotors_control/src/library/internal_model_controller.cpp @@ -464,7 +464,7 @@ void InternalModelController::AttitudeErrors(double* e_phi, double* e_theta, dou *e_theta = pitch_r - state_.attitude.pitch; *e_psi = psi_r - state_.attitude.yaw; - ROS_INFO("Angular Error: [%f %f %f]", state_.attitude.roll, state_.attitude.pitch, state_.attitude.yaw); + ROS_DEBUG("Angular Error: [%f %f %f]", state_.attitude.roll, state_.attitude.pitch, state_.attitude.yaw); ROS_DEBUG("Angular Error: [%f %f %f]", *e_phi, *e_theta, *e_psi); } diff --git a/rotors_control/src/nodes/position_controller_node.cpp b/rotors_control/src/nodes/position_controller_node.cpp index 0277754..48d7da7 100644 --- a/rotors_control/src/nodes/position_controller_node.cpp +++ b/rotors_control/src/nodes/position_controller_node.cpp @@ -55,6 +55,7 @@ PositionControllerNode::PositionControllerNode() { InitializeParams(); + // Topics subscribe if (!enable_mellinger_controller_ && !enable_internal_model_controller_){ cmd_multi_dof_joint_trajectory_sub_ = nh.subscribe(mav_msgs::default_topics::COMMAND_TRAJECTORY, 1, @@ -77,6 +78,7 @@ PositionControllerNode::PositionControllerNode() { } + // To publish the propellers angular velocities motor_velocity_reference_pub_ = nh.advertise(mav_msgs::default_topics::COMMAND_ACTUATORS, 1); // The subscription to the IMU topic is made only if it is available, when simulating the Crazyflie dynamics considering the also IMU values @@ -231,7 +233,171 @@ void PositionControllerNode::InitializeParams() { ROS_INFO_ONCE("[Position Controller] Set controller gains and vehicle parameters"); } - position_controller_.SetControllerGains(); + if(enable_mellinger_controller_){ + + // Parameters reading from rosparam. + GetRosParameter(pnh, "kp_xy/x", + mellinger_controller_.controller_parameters_mellinger_.kpXYPositionController_.x(), + &mellinger_controller_.controller_parameters_mellinger_.kpXYPositionController_.x()); + GetRosParameter(pnh, "kp_xy/y", + mellinger_controller_.controller_parameters_mellinger_.kpXYPositionController_.y(), + &mellinger_controller_.controller_parameters_mellinger_.kpXYPositionController_.y()); + + GetRosParameter(pnh, "kd_xy/x", + mellinger_controller_.controller_parameters_mellinger_.kdXYPositionController_.x(), + &mellinger_controller_.controller_parameters_mellinger_.kdXYPositionController_.x()); + GetRosParameter(pnh, "kd_xy/y", + mellinger_controller_.controller_parameters_mellinger_.kdXYPositionController_.y(), + &mellinger_controller_.controller_parameters_mellinger_.kdXYPositionController_.y()); + + GetRosParameter(pnh, "ki_xy/x", + mellinger_controller_.controller_parameters_mellinger_.kiXYPositionController_.x(), + &mellinger_controller_.controller_parameters_mellinger_.kiXYPositionController_.x()); + GetRosParameter(pnh, "ki_xy/y", + mellinger_controller_.controller_parameters_mellinger_.kiXYPositionController_.y(), + &mellinger_controller_.controller_parameters_mellinger_.kiXYPositionController_.y()); + + GetRosParameter(pnh, "kp_z/z", + mellinger_controller_.controller_parameters_mellinger_.kpZPositionController_, + &mellinger_controller_.controller_parameters_mellinger_.kpZPositionController_); + GetRosParameter(pnh, "kd_z/z", + mellinger_controller_.controller_parameters_mellinger_.kdZPositionController_, + &mellinger_controller_.controller_parameters_mellinger_.kdZPositionController_); + GetRosParameter(pnh, "ki_z/z", + mellinger_controller_.controller_parameters_mellinger_.kiZPositionController_, + &mellinger_controller_.controller_parameters_mellinger_.kiZPositionController_); + + GetRosParameter(pnh, "kr_xy/x", + mellinger_controller_.controller_parameters_mellinger_.krXYPositionController_.x(), + &mellinger_controller_.controller_parameters_mellinger_.krXYPositionController_.x()); + GetRosParameter(pnh, "kr_xy/y", + mellinger_controller_.controller_parameters_mellinger_.krXYPositionController_.y(), + &mellinger_controller_.controller_parameters_mellinger_.krXYPositionController_.y()); + + GetRosParameter(pnh, "kw_xy/x", + mellinger_controller_.controller_parameters_mellinger_.kwXYPositionController_.x(), + &mellinger_controller_.controller_parameters_mellinger_.kwXYPositionController_.x()); + GetRosParameter(pnh, "kw_xy/y", + mellinger_controller_.controller_parameters_mellinger_.kwXYPositionController_.y(), + &mellinger_controller_.controller_parameters_mellinger_.kwXYPositionController_.y()); + + GetRosParameter(pnh, "ki_m_xy/x", + mellinger_controller_.controller_parameters_mellinger_.ki_mXYPositionController_.x(), + &mellinger_controller_.controller_parameters_mellinger_.ki_mXYPositionController_.x()); + GetRosParameter(pnh, "ki_m_xy/y", + mellinger_controller_.controller_parameters_mellinger_.ki_mXYPositionController_.y(), + &mellinger_controller_.controller_parameters_mellinger_.ki_mXYPositionController_.y()); + + GetRosParameter(pnh, "kr_z/z", + mellinger_controller_.controller_parameters_mellinger_.krZPositionController_, + &mellinger_controller_.controller_parameters_mellinger_.krZPositionController_); + GetRosParameter(pnh, "kw_z/z", + mellinger_controller_.controller_parameters_mellinger_.kwZPositionController_, + &mellinger_controller_.controller_parameters_mellinger_.kwZPositionController_); + GetRosParameter(pnh, "ki_m_z/z", + mellinger_controller_.controller_parameters_mellinger_.ki_mZPositionController_, + &mellinger_controller_.controller_parameters_mellinger_.ki_mZPositionController_); + + GetRosParameter(pnh, "i_range_m_xy/x", + mellinger_controller_.controller_parameters_mellinger_.iRangeMXY_.x(), + &mellinger_controller_.controller_parameters_mellinger_.iRangeMXY_.x()); + GetRosParameter(pnh, "i_range_m_xy/y", + mellinger_controller_.controller_parameters_mellinger_.iRangeMXY_.y(), + &mellinger_controller_.controller_parameters_mellinger_.iRangeMXY_.y()); + + GetRosParameter(pnh, "i_range_xy/x", + mellinger_controller_.controller_parameters_mellinger_.iRangeXY_.x(), + &mellinger_controller_.controller_parameters_mellinger_.iRangeXY_.x()); + GetRosParameter(pnh, "i_range_xy/y", + mellinger_controller_.controller_parameters_mellinger_.iRangeXY_.y(), + &mellinger_controller_.controller_parameters_mellinger_.iRangeXY_.y()); + + GetRosParameter(pnh, "i_range_m_z/z", + mellinger_controller_.controller_parameters_mellinger_.iRangeMZ_, + &mellinger_controller_.controller_parameters_mellinger_.iRangeMZ_); + GetRosParameter(pnh, "i_range_z/z", + mellinger_controller_.controller_parameters_mellinger_.iRangeZ_, + &mellinger_controller_.controller_parameters_mellinger_.iRangeZ_); + + GetRosParameter(pnh, "kd_omega_rp/r", + mellinger_controller_.controller_parameters_mellinger_.kdOmegaRP_.x(), + &mellinger_controller_.controller_parameters_mellinger_.kdOmegaRP_.x()); + GetRosParameter(pnh, "kd_omega_rp/p", + mellinger_controller_.controller_parameters_mellinger_.kdOmegaRP_.y(), + &mellinger_controller_.controller_parameters_mellinger_.kdOmegaRP_.y()); + + mellinger_controller_.SetControllerGains(); + + //Analogously, the object "vehicle_parameters_" is created + GetFullVehicleParameters(pnh, &mellinger_controller_.vehicle_parameters_); + mellinger_controller_.SetVehicleParameters(); + ROS_INFO_ONCE("[Mellinger Controller] Set controller gains and vehicle parameters"); + + } + + if (enable_internal_model_controller_){ + // Read parameters from rosparam. The parameters are read by the YAML file and they + // are used to create the "controller_parameters_" object + GetRosParameter(pnh, "beta_xy/beta_x", + internal_model_controller_.controller_parameters_im_.beta_xy_.x(), + &internal_model_controller_.controller_parameters_im_.beta_xy_.x()); + GetRosParameter(pnh, "beta_xy/beta_y", + internal_model_controller_.controller_parameters_im_.beta_xy_.y(), + &internal_model_controller_.controller_parameters_im_.beta_xy_.y()); + GetRosParameter(pnh, "beta_z/beta_z", + internal_model_controller_.controller_parameters_im_.beta_z_, + &internal_model_controller_.controller_parameters_im_.beta_z_); + + GetRosParameter(pnh, "beta_phi/beta_phi", + internal_model_controller_.controller_parameters_im_.beta_phi_, + &internal_model_controller_.controller_parameters_im_.beta_phi_); + GetRosParameter(pnh, "beta_theta/beta_theta", + internal_model_controller_.controller_parameters_im_.beta_theta_, + &internal_model_controller_.controller_parameters_im_.beta_theta_); + GetRosParameter(pnh, "beta_psi/beta_psi", + internal_model_controller_.controller_parameters_im_.beta_psi_, + &internal_model_controller_.controller_parameters_im_.beta_psi_); + + GetRosParameter(pnh, "mu_xy/mu_x", + internal_model_controller_.controller_parameters_im_.mu_xy_.x(), + &internal_model_controller_.controller_parameters_im_.mu_xy_.x()); + GetRosParameter(pnh, "mu_xy/mu_y", + internal_model_controller_.controller_parameters_im_.mu_xy_.y(), + &internal_model_controller_.controller_parameters_im_.mu_xy_.y()); + GetRosParameter(pnh, "mu_z/mu_z", + internal_model_controller_.controller_parameters_im_.mu_z_, + &internal_model_controller_.controller_parameters_im_.mu_z_); + + GetRosParameter(pnh, "mu_phi/mu_phi", + internal_model_controller_.controller_parameters_im_.mu_phi_, + &internal_model_controller_.controller_parameters_im_.mu_phi_); + GetRosParameter(pnh, "mu_theta/mu_theta", + internal_model_controller_.controller_parameters_im_.mu_theta_, + &internal_model_controller_.controller_parameters_im_.mu_theta_); + GetRosParameter(pnh, "mu_psi/mu_psi", + internal_model_controller_.controller_parameters_im_.mu_psi_, + &internal_model_controller_.controller_parameters_im_.mu_psi_); + + GetRosParameter(pnh, "U_xyz/U_x", + internal_model_controller_.controller_parameters_im_.U_q_.x(), + &internal_model_controller_.controller_parameters_im_.U_q_.x()); + GetRosParameter(pnh, "U_xyz/U_y", + internal_model_controller_.controller_parameters_im_.U_q_.y(), + &internal_model_controller_.controller_parameters_im_.U_q_.y()); + GetRosParameter(pnh, "U_xyz/U_z", + internal_model_controller_.controller_parameters_im_.U_q_.z(), + &internal_model_controller_.controller_parameters_im_.U_q_.z()); + + internal_model_controller_.SetControllerGains(); + + //Analogously, the object "vehicle_parameters_" is created + GetFullVehicleParameters(pnh, &internal_model_controller_.vehicle_parameters_); + internal_model_controller_.SetVehicleParameters(); + ROS_INFO_ONCE("[Internal Model Controller] Set controller gains and vehicle parameters"); + + + } + if (enable_state_estimator_) position_controller_.crazyflie_onboard_controller_.SetControllerGains(position_controller_.controller_parameters_); diff --git a/rotors_description/urdf/component_snippets.xacro b/rotors_description/urdf/component_snippets.xacro index 2306fae..5883034 100644 --- a/rotors_description/urdf/component_snippets.xacro +++ b/rotors_description/urdf/component_snippets.xacro @@ -946,11 +946,11 @@ mass_imu_sensor="0.00001" gyroscope_noise_density="0.0" gyroscope_random_walk="0.0" - gyroscope_bias_correlation_time="0.0" + gyroscope_bias_correlation_time="1000.0" gyroscope_turn_on_bias_sigma= "0.0" accelerometer_noise_density="0.0" accelerometer_random_walk="0.0" - accelerometer_bias_correlation_time="0.0" + accelerometer_bias_correlation_time="300.0" accelerometer_turn_on_bias_sigma="0"> diff --git a/rotors_gazebo/launch/crazyflie2_hovering_example.launch b/rotors_gazebo/launch/crazyflie2_hovering_example.launch index 2974130..1d8c2c5 100644 --- a/rotors_gazebo/launch/crazyflie2_hovering_example.launch +++ b/rotors_gazebo/launch/crazyflie2_hovering_example.launch @@ -1,7 +1,5 @@ - @@ -10,12 +8,12 @@ + + + - - - @@ -37,40 +35,24 @@ - - - + - - - - - + + + - - - - - - - + - - - diff --git a/rotors_gazebo/launch/crazyflie2_internal_model_controller.launch b/rotors_gazebo/launch/crazyflie2_internal_model_controller.launch index e3468f1..8c63f6b 100644 --- a/rotors_gazebo/launch/crazyflie2_internal_model_controller.launch +++ b/rotors_gazebo/launch/crazyflie2_internal_model_controller.launch @@ -1,5 +1,7 @@ + @@ -56,9 +58,6 @@ - - - diff --git a/rotors_gazebo/launch/crazyflie2_mellinger_controller.launch b/rotors_gazebo/launch/crazyflie2_mellinger_controller.launch new file mode 100644 index 0000000..97b0c64 --- /dev/null +++ b/rotors_gazebo/launch/crazyflie2_mellinger_controller.launch @@ -0,0 +1,67 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/rotors_gazebo/worlds/basic_crazyflie.world b/rotors_gazebo/worlds/basic_crazyflie.world index 2179dd9..741a810 100644 --- a/rotors_gazebo/worlds/basic_crazyflie.world +++ b/rotors_gazebo/worlds/basic_crazyflie.world @@ -44,6 +44,7 @@ 6.0e-06 2.3e-05 -4.2e-05 0 0 -9.8 + From 12d1dc6d331f90901a3f85faa6c7c8fce6b29ffb Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Thu, 16 Apr 2020 18:32:08 +0200 Subject: [PATCH 20/21] Add Ria Sonecha as an author of the package --- rotors_description/package.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/rotors_description/package.xml b/rotors_description/package.xml index 934345b..4227d0a 100644 --- a/rotors_description/package.xml +++ b/rotors_description/package.xml @@ -10,6 +10,7 @@ Emanuele Aucone Benjamin Rodriguez Luigi Iannelli + Ria Sonecha ASL 2.0 From 0c536ffbcf0fb197dcbc3d4f7114250cb5311809 Mon Sep 17 00:00:00 2001 From: Giuseppe Silano Date: Wed, 22 Apr 2020 18:35:22 +0200 Subject: [PATCH 21/21] Changes according to #28 --- .../internal_model_controller.h | 2 +- .../src/library/internal_model_controller.cpp | 183 ++++---- .../src/nodes/position_controller_node.cpp | 14 +- .../rotors_gazebo/hovering_example_spline.h | 7 +- .../crazyflie_internal_model_controller.yaml | 14 +- .../resource/crazyflie_parameters.yaml | 6 +- rotors_gazebo/src/hovering_example_spline.cpp | 428 +++++++++++------- 7 files changed, 383 insertions(+), 271 deletions(-) diff --git a/rotors_control/include/rotors_control/internal_model_controller.h b/rotors_control/include/rotors_control/internal_model_controller.h index 0a9d3d4..3238c09 100644 --- a/rotors_control/include/rotors_control/internal_model_controller.h +++ b/rotors_control/include/rotors_control/internal_model_controller.h @@ -121,7 +121,7 @@ class InternalModelControllerParameters { double mu_x_, mu_y_, mu_z_; double mu_phi_, mu_theta_, mu_psi_; - double lambda_x_, lambda_y_, lambda_z_; + 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_; diff --git a/rotors_control/src/library/internal_model_controller.cpp b/rotors_control/src/library/internal_model_controller.cpp index ea8534c..b02f550 100644 --- a/rotors_control/src/library/internal_model_controller.cpp +++ b/rotors_control/src/library/internal_model_controller.cpp @@ -93,9 +93,9 @@ InternalModelController::InternalModelController() K_x_2_(0), K_y_2_(0), K_z_2_(0), - lambda_x_(0), - lambda_y_(0), - lambda_z_(0), + U_q_x_(0), + U_q_y_(0), + U_q_z_(0), control_({0,0,0,0}), //roll, pitch, yaw rate, thrust state_({0, //Position.x 0, //Position.y @@ -112,14 +112,19 @@ InternalModelController::InternalModelController() 0}) //Angular velocity z) { - // Command signals initialization - command_trajectory_.position_W[0] = 0; - command_trajectory_.position_W[1] = 0; - command_trajectory_.position_W[2] = 0; + // Command signals initialization - position + command_trajectory_.position_W[0] = 0; + command_trajectory_.position_W[1] = 0; + command_trajectory_.position_W[2] = 0; - // Timers set the outer and inner loops working frequency - timer1_ = n1_.createTimer(ros::Duration(TsA), &InternalModelController::CallbackAttitude, this, false, true); - timer2_ = n2_.createTimer(ros::Duration(TsP), &InternalModelController::CallbackPosition, this, false, true); + // velocity + command_trajectory_.velocity[0] = 0; + command_trajectory_.velocity[1] = 0; + command_trajectory_.velocity[2] = 0; + + // Timers set the outer and inner loops working frequency + timer1_ = n1_.createTimer(ros::Duration(TsA), &InternalModelController::CallbackAttitude, this, false, true); + timer2_ = n2_.createTimer(ros::Duration(TsP), &InternalModelController::CallbackPosition, this, false, true); } @@ -131,56 +136,60 @@ InternalModelController::~InternalModelController() {} // These variables will be employed during the simulation void InternalModelController::SetControllerGains(){ - beta_x_ = controller_parameters_im_.beta_xy_.x(); - beta_y_ = controller_parameters_im_.beta_xy_.y(); - beta_z_ = controller_parameters_im_.beta_z_; + beta_x_ = controller_parameters_im_.beta_xy_.x(); + beta_y_ = controller_parameters_im_.beta_xy_.y(); + beta_z_ = controller_parameters_im_.beta_z_; + + beta_phi_ = controller_parameters_im_.beta_phi_; + beta_theta_ = controller_parameters_im_.beta_theta_; + beta_psi_ = controller_parameters_im_.beta_psi_; - beta_phi_ = controller_parameters_im_.beta_phi_; - beta_theta_ = controller_parameters_im_.beta_theta_; - beta_psi_ = controller_parameters_im_.beta_psi_; + alpha_x_ = 1 - beta_x_; + alpha_y_ = 1 - beta_y_; + alpha_z_ = 1 - beta_z_; - alpha_x_ = 1 - beta_x_; - alpha_y_ = 1 - beta_y_; - alpha_z_ = 1 - beta_z_; + alpha_phi_ = 1 - beta_phi_; + alpha_theta_ = 1 - beta_theta_; + alpha_psi_ = 1 - beta_psi_; - alpha_phi_ = 1 - beta_phi_; - alpha_theta_ = 1 - beta_theta_; - alpha_psi_ = 1 - beta_psi_; + mu_x_ = controller_parameters_im_.mu_xy_.x(); + mu_y_ = controller_parameters_im_.mu_xy_.y(); + mu_z_ = controller_parameters_im_.mu_z_; - mu_x_ = controller_parameters_im_.mu_xy_.x(); - mu_y_ = controller_parameters_im_.mu_xy_.y(); - mu_z_ = controller_parameters_im_.mu_z_; + mu_phi_ = controller_parameters_im_.mu_phi_; + mu_theta_ = controller_parameters_im_.mu_theta_; + mu_psi_ = controller_parameters_im_.mu_psi_; - mu_phi_ = controller_parameters_im_.mu_phi_; - mu_theta_ = controller_parameters_im_.mu_theta_; - mu_psi_ = controller_parameters_im_.mu_psi_; + U_q_x_ = controller_parameters_im_.U_q_.x(); + U_q_y_ = controller_parameters_im_.U_q_.y(); + U_q_z_ = controller_parameters_im_.U_q_.z(); - lambda_x_ = controller_parameters_im_.U_q_.x(); - lambda_y_ = controller_parameters_im_.U_q_.y(); - lambda_z_ = controller_parameters_im_.U_q_.z(); + K_x_1_ = 1/mu_x_; + K_x_2_ = -2 * (beta_x_/mu_x_); - K_x_1_ = 1/mu_x_; - K_x_2_ = -2 * (beta_x_/mu_x_); + K_y_1_ = 1/mu_y_; + K_y_2_ = -2 * (beta_y_/mu_y_); - K_y_1_ = 1/mu_y_; - K_y_2_ = -2 * (beta_y_/mu_y_); + K_z_1_ = 1/mu_z_; + K_z_2_ = -2 * (beta_z_/mu_z_); - K_z_1_ = 1/mu_z_; - K_z_2_ = -2 * (beta_z_/mu_z_); + ROS_INFO_ONCE("[Internal Model Controller] Controller gains successful setup"); } // As SetControllerGains, the function is used to set the vehicle parameters into private variables of the class void InternalModelController::SetVehicleParameters(){ - bf_ = vehicle_parameters_.bf_; - l_ = vehicle_parameters_.armLength_; - bm_ = vehicle_parameters_.bm_; - m_ = vehicle_parameters_.mass_; - g_ = vehicle_parameters_.gravity_; - Ix_ = vehicle_parameters_.inertia_(0,0); - Iy_ = vehicle_parameters_.inertia_(1,1); - Iz_ = vehicle_parameters_.inertia_(2,2); + bf_ = vehicle_parameters_.bf_; + l_ = vehicle_parameters_.armLength_; + bm_ = vehicle_parameters_.bm_; + m_ = vehicle_parameters_.mass_; + g_ = vehicle_parameters_.gravity_; + Ix_ = vehicle_parameters_.inertia_(0,0); + Iy_ = vehicle_parameters_.inertia_(1,1); + Iz_ = vehicle_parameters_.inertia_(2,2); + + ROS_INFO_ONCE("[Internal Model Controller] Vehicle parameters successful setup"); } @@ -223,6 +232,10 @@ void InternalModelController::SetOdometryWithoutStateEstimator(const EigenOdomet state_.linearVelocity.y = odometry_.velocity[1]; state_.linearVelocity.z = odometry_.velocity[2]; + ROS_DEBUG("Drone position [x: %f, y: %f, z: %f]", state_.position.x, state_.position.y, state_.position.z); + ROS_DEBUG("Drone attitude [roll: %f, pitch: %f, yaw: %f]", state_.attitude.roll, state_.attitude.pitch, state_.attitude.yaw); + ROS_DEBUG("Drone linear velocity [x: %f, y: %f, z: %f]", state_.linearVelocity.x, state_.linearVelocity.y, state_.linearVelocity.z); + } // The function sets the filter trajectory points @@ -230,6 +243,16 @@ void InternalModelController::SetTrajectoryPoint(const mav_msgs::EigenDroneState command_trajectory_ = command_trajectory; + double psi_r, roll_r, pitch_r; + Quaternion2EulerCommandTrajectory(&roll_r, &pitch_r, &psi_r); + + ROS_DEBUG("Drone desired position [x_d: %f, y_d: %f, z_d: %f]", command_trajectory_.position_W[0], + command_trajectory_.position_W[1], command_trajectory_.position_W[2]); + ROS_DEBUG("Drone desired attitude [roll_d: %f, pitch_d: %f, yaw_d: %f]", roll_r, pitch_r, psi_r); + ROS_DEBUG("Drone desired linear velocity [x_d_dot: %f, y_d_dot: %f, z_d_dot: %f]", command_trajectory_.velocity[0], + command_trajectory_.velocity[1], command_trajectory_.velocity[2]); + + } // The function computes the propellers angular velocity @@ -245,6 +268,8 @@ void InternalModelController::CalculateRotorVelocities(Eigen::Vector4d* rotor_ve double u_phi, u_theta, u_psi; double u_x, u_y, u_z, u_Terr; AttitudeController(&u_phi, &u_theta, &u_psi); + // control_.roll = phi_r, desired roll angle + // control_.pitch = theta_r, desired pitch angle PosController(&control_.thrust, &control_.roll, &control_.pitch, &u_x, &u_y, &u_z, &u_Terr); ROS_DEBUG("Thrust: %f U_phi: %f U_theta: %f U_psi: %f", control_.thrust, u_phi, u_theta, u_psi); @@ -261,7 +286,7 @@ void InternalModelController::CalculateRotorVelocities(Eigen::Vector4d* rotor_ve not_saturated_3 = first + second + third - fourth; not_saturated_4 = first + second - third + fourth; - // The propellers velocities is limited by taking into account the physical constrains + // The propellers velocity is limited by taking into account the physical constrains double motorMin=not_saturated_1, motorMax=not_saturated_1, motorFix=0; if(not_saturated_2 < motorMin) motorMin = not_saturated_2; @@ -335,6 +360,8 @@ void InternalModelController::VelocityErrors(double* dot_e_x, double* dot_e_y, d psi = state_.attitude.yaw; phi = state_.attitude.roll; + // The linear and angular velocity provided as outputs by the ideal odometry sensor are expressed + // in the body-frame. Therefore, such measurements have to be rotated before computing the error dot_x = (cos(theta) * cos(psi) * state_.linearVelocity.x) + ( ( (sin(phi) * sin(theta) * cos(psi) ) - ( cos(phi) * sin(psi) ) ) * state_.linearVelocity.y) + ( ( (cos(phi) * sin(theta) * cos(psi) ) + ( sin(phi) * sin(psi) ) ) * state_.linearVelocity.z); @@ -361,7 +388,7 @@ void InternalModelController::PositionErrors(double* e_x, double* e_y, double* e *e_y = command_trajectory_.position_W[1] - state_.position.y; *e_z = command_trajectory_.position_W[2] - state_.position.z; - ROS_DEBUG("Position Error components: [%f %f]", command_trajectory_.position_W[2], state_.position.z); + ROS_DEBUG("Position Error components along the z-axis: [z_d: %f, z: %f]", command_trajectory_.position_W[2], state_.position.z); ROS_DEBUG("Position Error: [%f %f %f]", *e_x, *e_y, *e_z); } @@ -377,7 +404,7 @@ void InternalModelController::PosController(double* u_T, double* phi_r, double* assert(u_Terr); //u_x computing - *u_x = ( (e_x_ * K_x_1_ * K_x_2_)/lambda_x_ ) + ( (dot_e_x_ * K_x_2_)/lambda_x_ ); + *u_x = ( (e_x_ * K_x_1_ * K_x_2_)/U_q_x_ ) + ( (dot_e_x_ * K_x_2_)/U_q_x_ ); if (*u_x > 1 || *u_x <-1) if (*u_x > 1) @@ -385,7 +412,7 @@ void InternalModelController::PosController(double* u_T, double* phi_r, double* else *u_x = -1; - *u_x = (*u_x * 1/2) + ( (K_x_1_/lambda_x_) * dot_e_x_ ); + *u_x = (*u_x * 1/2) + ( (K_x_1_/U_q_x_) * dot_e_x_ ); if (*u_x > 1 || *u_x <-1) if (*u_x > 1) @@ -393,10 +420,10 @@ void InternalModelController::PosController(double* u_T, double* phi_r, double* else *u_x = -1; - *u_x = m_ * (*u_x * lambda_x_); + *u_x = m_ * (*u_x * U_q_x_); //u_y computing - *u_y = ( (e_y_ * K_y_1_ * K_y_2_)/lambda_y_ ) + ( (dot_e_y_ * K_y_2_)/lambda_y_ ); + *u_y = ( (e_y_ * K_y_1_ * K_y_2_)/U_q_y_ ) + ( (dot_e_y_ * K_y_2_)/U_q_y_ ); if (*u_y > 1 || *u_y <-1) if (*u_y > 1) @@ -404,7 +431,7 @@ void InternalModelController::PosController(double* u_T, double* phi_r, double* else *u_y = -1; - *u_y = (*u_y * 1/2) + ( (K_y_1_/lambda_y_) * dot_e_y_ ); + *u_y = (*u_y * 1/2) + ( (K_y_1_/U_q_y_) * dot_e_y_ ); if (*u_y > 1 || *u_y <-1) if (*u_y > 1) @@ -412,10 +439,10 @@ void InternalModelController::PosController(double* u_T, double* phi_r, double* else *u_y = -1; - *u_y = m_* ( *u_y * lambda_y_); + *u_y = m_* ( *u_y * U_q_y_); //u_z computing - *u_z = ( (e_z_ * K_z_1_ * K_z_2_)/lambda_z_ ) + ( (dot_e_z_ * K_z_2_)/lambda_z_ ); + *u_z = ( (e_z_ * K_z_1_ * K_z_2_)/U_q_z_ ) + ( (dot_e_z_ * K_z_2_)/U_q_z_ ); if (*u_z > 1 || *u_z <-1) if (*u_z > 1) @@ -423,7 +450,7 @@ void InternalModelController::PosController(double* u_T, double* phi_r, double* else *u_z = -1; - *u_z = (*u_z * 1/2) + ( (K_z_1_/lambda_z_) * dot_e_z_ ); + *u_z = (*u_z * 1/2) + ( (K_z_1_/U_q_z_) * dot_e_z_ ); if (*u_z > 1 || *u_z <-1) if (*u_z > 1) @@ -431,7 +458,7 @@ void InternalModelController::PosController(double* u_T, double* phi_r, double* else *u_z = -1; - *u_z = m_* ( *u_z * lambda_z_); + *u_z = m_* ( *u_z * U_q_z_); //u_Terr computing *u_Terr = *u_z + (m_ * g_); @@ -439,7 +466,7 @@ void InternalModelController::PosController(double* u_T, double* phi_r, double* //u_T computing *u_T = sqrt( pow(*u_x,2) + pow(*u_y,2) + pow(*u_Terr,2) ); - ROS_DEBUG("Position Control Signals: [%f %f %f %f]", *u_x, *u_y, *u_z, *u_T); + ROS_DEBUG("Position Control Signals: [%f %f %f %f %f]", *u_x, *u_y, *u_z, *u_T, *u_Terr); double psi_r, temp_a, temp_b; @@ -457,15 +484,16 @@ void InternalModelController::AttitudeErrors(double* e_phi, double* e_theta, dou assert(e_theta); assert(e_psi); - double psi_r, roll_r, pitch_r; + double roll_r, pitch_r, psi_r; Quaternion2EulerCommandTrajectory(&roll_r, &pitch_r, &psi_r); *e_phi = roll_r - state_.attitude.roll; *e_theta = pitch_r - state_.attitude.pitch; *e_psi = psi_r - state_.attitude.yaw; - ROS_DEBUG("Angular Error: [%f %f %f]", state_.attitude.roll, state_.attitude.pitch, state_.attitude.yaw); - ROS_DEBUG("Angular Error: [%f %f %f]", *e_phi, *e_theta, *e_psi); + ROS_DEBUG("Drone attitude: [roll: %f, pitch: %f, yaw: %f]", state_.attitude.roll, state_.attitude.pitch, state_.attitude.yaw); + ROS_DEBUG("Angular Error: [e_phi: %f, e_pitch: %f, e_psi: %f]", *e_phi, *e_theta, *e_psi); + ROS_DEBUG("Desired Attitude: [phi_d: %f, pitch_d: %f, psi_d: %f]", roll_r, pitch_r, psi_r); } @@ -480,27 +508,30 @@ void InternalModelController::AngularVelocityErrors(double* dot_e_phi, double* d double dot_e_phi_W_s, dot_e_theta_W_s, dot_e_psi_W_s, dot_e_phi_W_d, dot_e_theta_W_d, dot_e_psi_W_d; - + // Drone angular rate rotated from body to fixed frame dot_e_phi_W_s = state_.angularVelocity.x + (sin(state_.attitude.roll) * tan(state_.attitude.pitch) * state_.angularVelocity.y) + (cos(state_.attitude.roll) * tan(state_.attitude.pitch) * state_.angularVelocity.z); - dot_e_theta_W_s = (cos(state_.attitude.roll) * state_.angularVelocity.y) - (sin(state_.attitude.roll) * state_.angularVelocity.z); + dot_e_theta_W_s = cos(state_.attitude.roll) * state_.angularVelocity.y - sin(state_.attitude.roll) * state_.angularVelocity.z; - dot_e_psi_W_s = ( ( sin(state_.attitude.roll) * state_.angularVelocity.y) / cos(state_.attitude.pitch) ) + - ( ( cos(state_.attitude.roll) * state_.angularVelocity.z) / cos(state_.attitude.pitch) ); + dot_e_psi_W_s = ( sin(state_.attitude.roll) * state_.angularVelocity.y) / cos(state_.attitude.pitch) + + ( cos(state_.attitude.roll) * state_.angularVelocity.z) / cos(state_.attitude.pitch); + // Desired angular rate rotated from body to fixed frame dot_e_phi_W_d = command_trajectory_.angular_velocity_B[0] + (sin(roll_r) * tan(pitch_r) * command_trajectory_.angular_velocity_B[1]) + (cos(roll_r) * tan(pitch_r) * command_trajectory_.angular_velocity_B[2]); - dot_e_theta_W_d = (cos(roll_r) * command_trajectory_.angular_velocity_B[1]) - (sin(roll_r) * command_trajectory_.angular_velocity_B[2]); + dot_e_theta_W_d = cos(roll_r) * command_trajectory_.angular_velocity_B[1] - sin(roll_r) * command_trajectory_.angular_velocity_B[2]; - dot_e_psi_W_d = ( ( sin(roll_r) * command_trajectory_.angular_velocity_B[1]) / cos(pitch_r) ) + - ( ( cos(roll_r) * command_trajectory_.angular_velocity_B[2]) / cos(pitch_r) ); + dot_e_psi_W_d = ( sin(roll_r) * command_trajectory_.angular_velocity_B[1]) / cos(pitch_r) + + ( cos(roll_r) * command_trajectory_.angular_velocity_B[2]) / cos(pitch_r); *dot_e_phi = dot_e_phi_W_d - dot_e_phi_W_s; *dot_e_theta = dot_e_theta_W_d - dot_e_theta_W_s; *dot_e_psi = dot_e_psi_W_d - dot_e_psi_W_s; + ROS_DEBUG("Desired Attitude Rate: [phi_d_dot: %f, pitch_d_dot: %f, psi_d_dot: %f]", dot_e_phi_W_d, dot_e_theta_W_d, dot_e_psi_W_d); + } void InternalModelController::Quaternion2EulerCommandTrajectory(double* roll, double* pitch, double* yaw) const { @@ -529,26 +560,24 @@ void InternalModelController::AttitudeController(double* u_phi, double* u_theta, assert(u_theta); assert(u_psi); - *u_phi = Ix_ * ( ( ( (alpha_phi_/mu_phi_ ) * dot_e_phi_ ) - ( (beta_phi_/pow(mu_phi_,2) ) * e_phi_ ) ) - ( ( (Iy_ - Iz_)/(Ix_ * mu_theta_ * mu_psi_) ) * e_theta_ * e_psi_) ); - *u_theta = Iy_ * ( ( ( (alpha_theta_/mu_theta_) * dot_e_theta_) - ( (beta_theta_/pow(mu_theta_,2)) * e_theta_) ) - ( ( (Iz_ - Ix_)/(Iy_ * mu_phi_ * mu_psi_ ) ) * e_phi_ * e_psi_) ); - *u_psi = Iz_ * ( ( ( (alpha_psi_/mu_psi_ ) * dot_e_psi_ ) - ( (beta_psi_/pow(mu_psi_,2) ) * e_psi_ ) ) - ( ( (Ix_ - Iy_)/(Iz_ * mu_theta_ * mu_phi_) ) * e_theta_ * e_phi_) ); + *u_phi = Ix_ * ( ( ( (alpha_phi_/mu_phi_ ) * dot_e_phi_ ) - ( (beta_phi_/pow(mu_phi_,2) ) * e_phi_ ) ) - + ( ( (Iy_ - Iz_)/(Ix_ * mu_theta_ * mu_psi_) ) * e_theta_ * e_psi_) ); + *u_theta = Iy_ * ( ( ( (alpha_theta_/mu_theta_) * dot_e_theta_) - ( (beta_theta_/pow(mu_theta_,2)) * e_theta_) ) - + ( ( (Iz_ - Ix_)/(Iy_ * mu_phi_ * mu_psi_ ) ) * e_phi_ * e_psi_) ); + *u_psi = Iz_ * ( ( ( (alpha_psi_/mu_psi_ ) * dot_e_psi_ ) - ( (beta_psi_/pow(mu_psi_,2) ) * e_psi_ ) ) - + ( ( (Ix_ - Iy_)/(Iz_ * mu_theta_ * mu_phi_) ) * e_theta_ * e_phi_) ); } -//The function every TsA computes the attitude and angular velocity errors. When the data storing is active, the data are saved into csv files +//Every TsA this function computes the attitude and the angular rate errors void InternalModelController::CallbackAttitude(const ros::TimerEvent& event){ AttitudeErrors(&e_phi_, &e_theta_, &e_psi_); AngularVelocityErrors(&dot_e_phi_, &dot_e_theta_, &dot_e_psi_); } -//The function every TsP: -// * the next point to follow has generated (the output of the waypoint filter) -// * the output of the waypoint filter is put into the command_trajectory_ data structure -// * the EKF is used to estimate the drone attitude and linear velocity -// * the position and velocity errors are computed -// * the last part is used to store the data into csv files if the data storing is active +//Every TsP this function computes the position and velocity errors void InternalModelController::CallbackPosition(const ros::TimerEvent& event){ PositionErrors(&e_x_, &e_y_, &e_z_); diff --git a/rotors_control/src/nodes/position_controller_node.cpp b/rotors_control/src/nodes/position_controller_node.cpp index 48d7da7..3cb6ab9 100644 --- a/rotors_control/src/nodes/position_controller_node.cpp +++ b/rotors_control/src/nodes/position_controller_node.cpp @@ -150,6 +150,9 @@ void PositionControllerNode::MultiDofJointTrajectoryMellingerCallback(const mav_ else internal_model_controller_.SetTrajectoryPoint(eigen_reference); + ROS_DEBUG("Drone desired position [x_d: %f, y_d: %f, z_d: %f]", eigen_reference.position_W[0], + eigen_reference.position_W[1], eigen_reference.position_W[2]); + if (drone_state_msg.position.z > 0) { waypointHasBeenPublished_ = true; if (enable_mellinger_controller_) @@ -231,7 +234,7 @@ void PositionControllerNode::InitializeParams() { position_controller_.SetControllerGains(); ROS_INFO_ONCE("[Position Controller] Set controller gains and vehicle parameters"); - } + } if(enable_mellinger_controller_){ @@ -329,11 +332,11 @@ void PositionControllerNode::InitializeParams() { mellinger_controller_.SetControllerGains(); //Analogously, the object "vehicle_parameters_" is created - GetFullVehicleParameters(pnh, &mellinger_controller_.vehicle_parameters_); - mellinger_controller_.SetVehicleParameters(); - ROS_INFO_ONCE("[Mellinger Controller] Set controller gains and vehicle parameters"); + GetFullVehicleParameters(pnh, &mellinger_controller_.vehicle_parameters_); + mellinger_controller_.SetVehicleParameters(); + ROS_INFO_ONCE("[Mellinger Controller] Set controller gains and vehicle parameters"); - } + } if (enable_internal_model_controller_){ // Read parameters from rosparam. The parameters are read by the YAML file and they @@ -395,7 +398,6 @@ void PositionControllerNode::InitializeParams() { internal_model_controller_.SetVehicleParameters(); ROS_INFO_ONCE("[Internal Model Controller] Set controller gains and vehicle parameters"); - } if (enable_state_estimator_) diff --git a/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h b/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h index 608019e..9217a68 100644 --- a/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h +++ b/rotors_gazebo/include/rotors_gazebo/hovering_example_spline.h @@ -59,11 +59,12 @@ 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 b0_, b1_, b2_, b3_, b4_; + Eigen::Vector3f c0_, c1_, c2_, c3_; Eigen::Vector3f g0_, g1_, g2_, g3_, g4_, g5_; - Eigen::Vector3f h0_, h1_, h2_, h3_, h4_, h5_; + Eigen::Vector3f h0_, h1_, h2_, h3_, h4_; + Eigen::Vector3f i0_, i1_, i2_, i3_; // Desidred drone orientation expressed in radians double rollDesRad_, pitchDesRad_, yawDesRad_; diff --git a/rotors_gazebo/resource/crazyflie_internal_model_controller.yaml b/rotors_gazebo/resource/crazyflie_internal_model_controller.yaml index aba404e..7306200 100644 --- a/rotors_gazebo/resource/crazyflie_internal_model_controller.yaml +++ b/rotors_gazebo/resource/crazyflie_internal_model_controller.yaml @@ -1,12 +1,12 @@ # Internal model controller parameters -beta_xy: {beta_x: -26.4259, beta_y: -26.3627} -beta_z: {beta_z: -27.2277} -beta_phi: {beta_phi: -0.8757} -beta_theta: {beta_theta: -0.8757} +beta_xy: {beta_x: -16.4259, beta_y: -16.3627} +beta_z: {beta_z: -17.2277} +beta_phi: {beta_phi: -18.3431} +beta_theta: {beta_theta: -18.3431} beta_psi: {beta_psi: -14.3431} mu_xy: {mu_x: 1, mu_y: 1} mu_z: {mu_z: 1} -mu_phi: {mu_phi: 0.0544} -mu_theta: {mu_theta: 0.0543} +mu_phi: {mu_phi: 0.44} +mu_theta: {mu_theta: 0.44} mu_psi: {mu_psi: 0.44} -U_xyz: {U_x: 1.1810, U_y: 1.1810, U_z: 4.6697} +U_xyz: {U_x: 0.8299, U_y: 0.8299, U_z: 8.6697} diff --git a/rotors_gazebo/resource/crazyflie_parameters.yaml b/rotors_gazebo/resource/crazyflie_parameters.yaml index 18b085f..cf63599 100644 --- a/rotors_gazebo/resource/crazyflie_parameters.yaml +++ b/rotors_gazebo/resource/crazyflie_parameters.yaml @@ -1,6 +1,6 @@ # Crazyflie vehicle parameters -mass: 0.026 +mass: 0.027 inertia: {xx: 1.657171e-05, xy: 0.0, xz: 0.0, yy: 1.657171e-05, yz: 0.0, zz: 2.9261652e-05} -bf: 1.28192e-8 -bm: 5.964552e-3 +bf: 1.28192e-08 +bm: 5.964552e-03 l: 0.046 diff --git a/rotors_gazebo/src/hovering_example_spline.cpp b/rotors_gazebo/src/hovering_example_spline.cpp index 1ef38fc..5f2e2ca 100644 --- a/rotors_gazebo/src/hovering_example_spline.cpp +++ b/rotors_gazebo/src/hovering_example_spline.cpp @@ -56,11 +56,12 @@ namespace rotors_gazebo { // Parameters initializion a0_.setZero(); a1_.setZero(); a2_.setZero(); a3_.setZero(); a4_.setZero(); a5_.setZero(); - b0_.setZero(); b1_.setZero(); b2_.setZero(); b3_.setZero(); b4_.setZero(); b5_.setZero(); - c0_.setZero(); c1_.setZero(); c2_.setZero(); c3_.setZero(); c4_.setZero(); c5_.setZero(); + b0_.setZero(); b1_.setZero(); b2_.setZero(); b3_.setZero(); b4_.setZero(); + c0_.setZero(); c1_.setZero(); c2_.setZero(); c3_.setZero(); g0_.setZero(); g1_.setZero(); g2_.setZero(); g3_.setZero(); g4_.setZero(); g5_.setZero(); - h0_.setZero(); h1_.setZero(); h2_.setZero(); h3_.setZero(); h4_.setZero(); h5_.setZero(); + h0_.setZero(); h1_.setZero(); h2_.setZero(); h3_.setZero(); h4_.setZero(); + i0_.setZero(); i1_.setZero(); i2_.setZero(); i3_.setZero(); } @@ -83,58 +84,63 @@ namespace rotors_gazebo { ROS_DEBUG("Publishing time_spline: %f", time_spline); - // Desidered Position - double first, second, third; - first = a0_.x() * pow(time_spline, 5) + a1_.x() * pow(time_spline, 4) + a2_.x() * pow(time_spline, 3) + a3_.x() * pow(time_spline, 2) - + a4_.x() * time_spline + a5_.x(); - second = a0_.y() * pow(time_spline, 5) + a1_.y() * pow(time_spline, 4) + a2_.y() * pow(time_spline, 3) + a3_.y() * pow(time_spline, 2) - + a4_.y() * time_spline + a5_.y(); - third = a0_.z() * pow(time_spline, 5) + a1_.z() * pow(time_spline, 4) + a2_.z() * pow(time_spline, 3) + a3_.z() * pow(time_spline, 2) - + a4_.z() * time_spline + a5_.z(); + // Desired Position + double x_component, y_component, z_component; + x_component = a5_.x() * pow(time_spline, 5) + a4_.x() * pow(time_spline, 4) + a3_.x() * pow(time_spline, 3) + a2_.x() * pow(time_spline, 2) + + a1_.x() * time_spline + a0_.x(); + x_component = a5_.y() * pow(time_spline, 5) + a4_.y() * pow(time_spline, 4) + a3_.y() * pow(time_spline, 3) + a2_.y() * pow(time_spline, 2) + + a1_.y() * time_spline + a0_.y(); + z_component = a5_.z() * pow(time_spline, 5) + a4_.z() * pow(time_spline, 4) + a3_.z() * pow(time_spline, 3) + a2_.z() * pow(time_spline, 2) + + a1_.z() * time_spline + a0_.z(); - ROS_DEBUG("Publishing position spline parameters along x: [%f, %f, %f, %f, %f, %f].", a0_.x(), a1_.x(), a2_.x(), a3_.x(), a4_.x(), a5_.x()); - ROS_DEBUG("Publishing position spline parameters along y: [%f, %f, %f, %f, %f, %f].", a0_.y(), a1_.y(), a2_.y(), a3_.y(), a4_.y(), a5_.y()); - ROS_DEBUG("Publishing position spline parameters along z: [%f, %f, %f, %f, %f, %f].", a0_.z(), a1_.z(), a2_.z(), a3_.z(), a4_.z(), a5_.z()); + ROS_DEBUG("Publishing position spline parameters along x-axis: [%f, %f, %f, %f, %f, %f].", a0_.x(), a1_.x(), a2_.x(), a3_.x(), a4_.x(), a5_.x()); + ROS_DEBUG("Publishing position spline parameters along y-axis: [%f, %f, %f, %f, %f, %f].", a0_.y(), a1_.y(), a2_.y(), a3_.y(), a4_.y(), a5_.y()); + ROS_DEBUG("Publishing position spline parameters along z-axis: [%f, %f, %f, %f, %f, %f].", a0_.z(), a1_.z(), a2_.z(), a3_.z(), a4_.z(), a5_.z()); - odometry->position_W = Eigen::Vector3f(first, second, third); + odometry->position_W = Eigen::Vector3f(x_component, y_component, z_component); - ROS_DEBUG("Publishing position waypoint: [%f, %f, %f].", first, second, third); + ROS_DEBUG("Publishing position waypoint: [%f, %f, %f].", x_component, y_component, z_component); - // Desidered Linear Velocity - first = b0_.x() * pow(time_spline, 5) + b1_.x() * pow(time_spline, 4) + b2_.x() * pow(time_spline, 3) + b3_.x() * pow(time_spline, 2) - + b4_.x() * time_spline + b5_.x(); - second = b0_.y() * pow(time_spline, 5) + b1_.y() * pow(time_spline, 4) + b2_.y() * pow(time_spline, 3) + b3_.y() * pow(time_spline, 2) - + b4_.y() * time_spline + b5_.y(); - third = b0_.z() * pow(time_spline, 5) + b1_.z() * pow(time_spline, 4) + b2_.z() * pow(time_spline, 3) + b3_.z() * pow(time_spline, 2) - + b4_.z() * time_spline + b5_.z(); + // Desired Linear Velocity + x_component = b4_.x() * pow(time_spline, 4) + b3_.x() * pow(time_spline, 3) + b2_.x() * pow(time_spline, 2) + b1_.x() * time_spline + + b0_.x(); + y_component = b4_.y() * pow(time_spline, 4) + b3_.y() * pow(time_spline, 3) + b2_.y() * pow(time_spline, 2) + b1_.y() * time_spline + + b0_.y(); + z_component = b4_.z() * pow(time_spline, 4) + b3_.z() * pow(time_spline, 3) + b2_.z() * pow(time_spline, 2) + b1_.z() * time_spline + + b0_.z(); - ROS_DEBUG("Publishing velocity spline parameters along x: [%f, %f, %f, %f, %f, %f].", b0_.x(), b1_.x(), b2_.x(), b3_.x(), b4_.x(), b5_.x()); - ROS_DEBUG("Publishing velocity spline parameters along y: [%f, %f, %f, %f, %f, %f].", b0_.y(), b1_.y(), b2_.y(), b3_.y(), b4_.y(), b5_.y()); - ROS_DEBUG("Publishing velocity spline parameters along z: [%f, %f, %f, %f, %f, %f].", b0_.z(), b1_.z(), b2_.z(), b3_.z(), b4_.z(), b5_.z()); + ROS_DEBUG("Publishing velocity spline parameters along x-axis: [%f, %f, %f, %f, %f].", b0_.x(), b1_.x(), b2_.x(), b3_.x(), b4_.x()); + ROS_DEBUG("Publishing velocity spline parameters along y-axis: [%f, %f, %f, %f, %f].", b0_.y(), b1_.y(), b2_.y(), b3_.y(), b4_.y()); + ROS_DEBUG("Publishing velocity spline parameters along z-axis: [%f, %f, %f, %f, %f].", b0_.z(), b1_.z(), b2_.z(), b3_.z(), b4_.z()); - odometry->velocity = Eigen::Vector3f(first, second, third); + odometry->velocity = Eigen::Vector3f(x_component, y_component, z_component); - ROS_DEBUG("Publishing velocity waypoint: [%f, %f, %f].", first, second, third); + ROS_DEBUG("Publishing velocity waypoint: [%f, %f, %f].", x_component, y_component, z_component); - // Desidered Acceleration - first = c0_.x() * pow(time_spline, 5) + c1_.x() * pow(time_spline, 4) + c2_.x() * pow(time_spline, 3) + c3_.x() * pow(time_spline, 2) - + c4_.x() * time_spline + c5_.x(); - second = c0_.y() * pow(time_spline, 5) + c1_.y() * pow(time_spline, 4) + c2_.y() * pow(time_spline, 3) + c3_.y() * pow(time_spline, 2) - + c4_.y() * time_spline + c5_.y(); - third = c0_.z() * pow(time_spline, 5) + c1_.z() * pow(time_spline, 4) + c2_.z() * pow(time_spline, 3) + c3_.z() * pow(time_spline, 2) - + c4_.z() * time_spline + c5_.z(); + // Desired Acceleration + x_component = c3_.x() * pow(time_spline, 3) + c2_.x() * pow(time_spline, 2) + c1_.x() * time_spline + c0_.x(); + y_component = c3_.y() * pow(time_spline, 3) + c2_.y() * pow(time_spline, 2) + c1_.y() * time_spline + c0_.y(); + z_component = c3_.z() * pow(time_spline, 3) + c2_.z() * pow(time_spline, 2) + c1_.z() * time_spline + c0_.z(); - odometry->acceleration = Eigen::Vector3f(first, second, third); + ROS_DEBUG("Publishing acceleration spline parameters along y-axis: [%f, %f, %f, %f].", c0_.y(), c1_.y(), c2_.y(), c3_.y()); + ROS_DEBUG("Publishing acceleration spline parameters along x-axis: [%f, %f, %f, %f].", c0_.x(), c1_.x(), c2_.x(), c3_.x()); + ROS_DEBUG("Publishing acceleration spline parameters along z-axis: [%f, %f, %f, %f].", c0_.z(), c1_.z(), c2_.z(), c3_.z()); - ROS_DEBUG("Publishing acceleration waypoint: [%f, %f, %f].", first, second, third); + odometry->acceleration = Eigen::Vector3f(x_component, y_component, z_component); - // Desidred Attitute - rollDesRad_ = g0_.x() * pow(time_spline, 5) + g1_.x() * pow(time_spline, 4) + g2_.x() * pow(time_spline, 3) + g3_.x() * pow(time_spline, 2) - + g4_.x() * time_spline + g5_.x(); - pitchDesRad_ = g0_.y() * pow(time_spline, 5) + g1_.y() * pow(time_spline, 4) + g2_.y() * pow(time_spline, 3) + g3_.y() * pow(time_spline, 2) - + g4_.y() * time_spline + g5_.y(); - yawDesRad_ = g0_.z() * pow(time_spline, 5) + g1_.z() * pow(time_spline, 4) + g2_.z() * pow(time_spline, 3) + g3_.z() * pow(time_spline, 2) - + g4_.z() * time_spline + g5_.z(); + ROS_DEBUG("Publishing acceleration waypoint: [%f, %f, %f].", x_component, y_component, z_component); + + // Desired Attitute + rollDesRad_ = g5_.x() * pow(time_spline, 5) + g4_.x() * pow(time_spline, 4) + g3_.x() * pow(time_spline, 3) + g2_.x() * pow(time_spline, 2) + + g1_.x() * time_spline + g0_.x(); + pitchDesRad_ = g5_.y() * pow(time_spline, 5) + g4_.y() * pow(time_spline, 4) + g3_.y() * pow(time_spline, 3) + g2_.y() * pow(time_spline, 2) + + g1_.y() * time_spline + g0_.y(); + yawDesRad_ = g5_.z() * pow(time_spline, 5) + g4_.z() * pow(time_spline, 4) + g3_.z() * pow(time_spline, 3) + g2_.z() * pow(time_spline, 2) + + g1_.z() * time_spline + g0_.z(); + + ROS_DEBUG("Publishing orientation spline parameters along x-axis: [%f, %f, %f, %f, %f, %f].", g0_.x(), g1_.x(), g2_.x(), g3_.x(), g4_.x(), g5_.x()); + ROS_DEBUG("Publishing orientation spline parameters along y-axis: [%f, %f, %f, %f, %f, %f].", g0_.y(), g1_.y(), g2_.y(), g3_.y(), g4_.y(), g5_.y()); + ROS_DEBUG("Publishing orientation spline parameters along z-axis: [%f, %f, %f, %f, %f, %f].", g0_.z(), g1_.z(), g2_.z(), g3_.z(), g4_.z(), g5_.z()); // Converts radians in quaternions double x, y, z, w; @@ -143,159 +149,218 @@ namespace rotors_gazebo { ROS_DEBUG("Publishing attitude waypoint: [%f, %f, %f].", rollDesRad_, pitchDesRad_, yawDesRad_); - // Desidered Angular Velocity - first = h0_.x() * pow(time_spline, 5) + h1_.x() * pow(time_spline, 4) + h2_.x() * pow(time_spline, 3) + h3_.x() * pow(time_spline, 2) - + h4_.x() * time_spline + h5_.x(); - second = h0_.y() * pow(time_spline, 5) + h1_.y() * pow(time_spline, 4) + h2_.y() * pow(time_spline, 3) + h3_.y() * pow(time_spline, 2) - + h4_.y() * time_spline + h5_.y(); - third = h0_.z() * pow(time_spline, 5) + h1_.z() * pow(time_spline, 4) + h2_.z() * pow(time_spline, 3) + h3_.z() * pow(time_spline, 2) - + h4_.z() * time_spline + h5_.z(); + // Desired Angular Velocity + double roll_component, pitch_component, yaw_component; + roll_component = h4_.x() * pow(time_spline, 4) + h3_.x() * pow(time_spline, 3) + h2_.x() * pow(time_spline, 2) + h1_.x() * time_spline + h0_.x(); + pitch_component = h4_.y() * pow(time_spline, 4) + h3_.y() * pow(time_spline, 3) + h2_.y() * pow(time_spline, 2) + h1_.y() * time_spline + h0_.y(); + yaw_component = h4_.z() * pow(time_spline, 4) + h3_.z() * pow(time_spline, 3) + h2_.z() * pow(time_spline, 2) + h1_.z() * time_spline + h0_.z(); + + ROS_DEBUG("Publishing angular velocity waypoint: [%f, %f, %f].", roll_component, pitch_component, yaw_component); - double first_B, second_B, third_B; - first_B = first - sin(pitchDesRad_)*third; - second_B = cos(rollDesRad_)*second + sin(rollDesRad_)*cos(pitchDesRad_)*third; - third_B = -sin(rollDesRad_)*second + cos(rollDesRad_)*cos(pitchDesRad_)*third; + // The angular rates are rotated from the inertial to the body frame + // https://home.aero.polimi.it/trainelli/downloads/Bottasso_ThreeDimensionalRotations.pdf, eqs. 1.206 and 1.208 + double roll_component_B, pitch_component_B, yaw_component_B; + roll_component_B = roll_component - sin(pitchDesRad_) * yaw_component; + pitch_component_B = cos(rollDesRad_) * pitch_component + sin(rollDesRad_) * cos(pitchDesRad_) * yaw_component; + yaw_component_B = -sin(rollDesRad_) * pitch_component + cos(rollDesRad_) * cos(pitchDesRad_) * yaw_component; - odometry->angular_velocity_B = Eigen::Vector3f(first_B, second_B, third_B); + odometry->angular_velocity_B = Eigen::Vector3f(roll_component_B, pitch_component_B, yaw_component_B); - ROS_DEBUG("Publishing angular velocity waypoint: [%f, %f, %f].", first, second, third); + ROS_DEBUG("Publishing angular velocity waypoint in the body frame: [%f, %f, %f].", roll_component_B, pitch_component_B, yaw_component_B); } void HoveringExampleSpline::ComputeSplineParameters(double* time_spline){ assert(time_spline); - // Parameters used for the position and its derivatives + /* POSITION */ + // Parameters used for the position and its derivatives. The coefficient + // refers to the x, y and z-avis a0_ = position_initial_; ROS_DEBUG("Publishing position initial: [%f, %f, %f].", position_initial_[0], position_initial_[1], position_initial_[2]); + ROS_DEBUG("Content of the a0 coefficient: [%f, %f, %f].", a0_[0], a0_[1], a0_[2]); a1_ = velocity_initial_; + ROS_DEBUG("Publishing velocity initial: [%f, %f, %f].", velocity_initial_[0], velocity_initial_[1], velocity_initial_[2]); + ROS_DEBUG("Content of the a1 coefficient: [%f, %f, %f].", a1_[0], a1_[1], a1_[2]); + a2_.x() = acceleration_initial_.x() * DELTA_1; a2_.y() = acceleration_initial_.y() * DELTA_1; a2_.z() = acceleration_initial_.z() * DELTA_1; ROS_DEBUG("Publishing acceleration initial: [%f, %f, %f].", acceleration_initial_[0], acceleration_initial_[1], acceleration_initial_[2]); - - a3_.x() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.x() - position_initial_.x()) - (ALPHA_3 * velocity_final_.x() + ALPHA_4 * velocity_initial_.x()) * *time_spline - - (ALPHA_5 * acceleration_final_.x() - acceleration_initial_.x()) * pow(*time_spline, 2)); - a3_.y() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.y() - position_initial_.y()) - (ALPHA_3 * velocity_final_.y() + ALPHA_4 * velocity_initial_.y()) * *time_spline - - (ALPHA_5 * acceleration_final_.y() - acceleration_initial_.y()) * pow(*time_spline, 2)); - a3_.z() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.z() - position_initial_.z()) - (ALPHA_3 * velocity_final_.z() + ALPHA_4 * velocity_initial_.z()) * *time_spline - - (ALPHA_5 * acceleration_final_.z() - acceleration_initial_.z()) * pow(*time_spline, 2)); - - a4_.x() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.x() - position_final_.x()) + (BETA_2 * velocity_final_.x() + BETA_3 * velocity_initial_.x()) * *time_spline - + (BETA_4 * acceleration_final_.x() - 2 * acceleration_initial_.x()) * pow(*time_spline, 2)); - a4_.y() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.y() - position_final_.y()) + (BETA_2 * velocity_final_.y() + BETA_3 * velocity_initial_.y()) * *time_spline - + (BETA_4 * acceleration_final_.y() - 2 * acceleration_initial_.y()) * pow(*time_spline, 2)); - a4_.z() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.z() - position_final_.z()) + (BETA_2 * velocity_final_.z() + BETA_3 * velocity_initial_.z()) * *time_spline - + (BETA_4 * acceleration_final_.z() - 2 * acceleration_initial_.z()) * pow(*time_spline, 2)); - - a5_.x() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.x() - position_initial_.x()) - GAMMA_2 * (velocity_final_.x() + velocity_initial_.x()) * *time_spline - - (acceleration_final_.x() - acceleration_initial_.x()) * pow(*time_spline, 2)); - a5_.y() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.y() - position_initial_.y()) - GAMMA_2 * (velocity_final_.y() + velocity_initial_.y()) * *time_spline - - (acceleration_final_.y() - acceleration_initial_.y()) * pow(*time_spline, 2)); - a5_.z() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.z() - position_initial_.z()) - GAMMA_2 * (velocity_final_.z() + velocity_initial_.z()) * *time_spline - - (acceleration_final_.z() - acceleration_initial_.z()) * pow(*time_spline, 2)); - //b - b0_.x() = 0; - b0_.y() = 0; - b0_.z() = 0; - - b1_.x() = 5*a0_.x(); - b1_.y() = 5*a0_.y(); - b1_.z() = 5*a0_.z(); - - b2_.x() = 4*a1_.x(); - b2_.y() = 4*a1_.y(); - b2_.z() = 4*a1_.z(); - - b3_.x() = 3*a2_.x(); - b3_.y() = 3*a2_.y(); - b3_.z() = 3*a2_.z(); - - b4_.x() = 2*a3_.x(); - b4_.y() = 2*a3_.y(); - b4_.z() = 2*a3_.z(); - - b5_ = a4_; - - //c - c0_.x() = 0; - c0_.y() = 0; - c0_.z() = 0; - - c1_.x() = 0; - c1_.y() = 0; - c1_.z() = 0; - - c2_.x() = 4*b1_.x(); - c2_.y() = 4*b1_.y(); - c2_.z() = 4*b1_.z(); - - c3_.x() = 3*b2_.x(); - c3_.y() = 3*b2_.y(); - c3_.z() = 3*b2_.z(); - - c4_.x() = 2*b3_.x(); - c4_.y() = 2*b3_.y(); - c4_.z() = 2*b3_.z(); - - c5_ = b4_; - - // Parameters used for the orientation and its derivatives + ROS_DEBUG("Content of the a2 coefficient: [%f, %f, %f].", a2_[0], a2_[1], a2_[2]); + + a3_.x() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.x() - position_initial_.x()) - + (ALPHA_3 * velocity_final_.x() + ALPHA_4 * velocity_initial_.x()) * *time_spline - + (ALPHA_5 * acceleration_final_.x() - acceleration_initial_.x()) * pow(*time_spline, 2)); + + a3_.y() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.y() - position_initial_.y()) - + (ALPHA_3 * velocity_final_.y() + ALPHA_4 * velocity_initial_.y()) * *time_spline - + (ALPHA_5 * acceleration_final_.y() - acceleration_initial_.y()) * pow(*time_spline, 2)); + + a3_.z() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (position_final_.z() - position_initial_.z()) - + (ALPHA_3 * velocity_final_.z() + ALPHA_4 * velocity_initial_.z()) * *time_spline - + (ALPHA_5 * acceleration_final_.z() - acceleration_initial_.z()) * pow(*time_spline, 2)); + + ROS_DEBUG("Content of the a3 coefficient: [%f, %f, %f].", a3_[0], a3_[1], a3_[2]); + + a4_.x() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.x() - position_final_.x()) + + (BETA_2 * velocity_final_.x() + BETA_3 * velocity_initial_.x()) * *time_spline + + (BETA_3 * acceleration_final_.x() - BETA_4 * acceleration_initial_.x()) * pow(*time_spline, 2)); + + a4_.y() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.y() - position_final_.y()) + + (BETA_2 * velocity_final_.y() + BETA_3 * velocity_initial_.y()) * *time_spline + + (BETA_3 * acceleration_final_.y() - BETA_4 * acceleration_initial_.y()) * pow(*time_spline, 2)); + + a4_.z() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (position_initial_.z() - position_final_.z()) + + (BETA_2 * velocity_final_.z() + BETA_3 * velocity_initial_.z()) * *time_spline + + (BETA_3 * acceleration_final_.z() - BETA_4 * acceleration_initial_.z()) * pow(*time_spline, 2)); + + ROS_DEBUG("Content of the a4 coefficient: [%f, %f, %f].", a4_[0], a4_[1], a4_[2]); + + a5_.x() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.x() - position_initial_.x()) - + GAMMA_2 * (velocity_final_.x() + velocity_initial_.x()) * *time_spline - + (acceleration_final_.x() - acceleration_initial_.x()) * pow(*time_spline, 2)); + + a5_.y() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.y() - position_initial_.y()) - + GAMMA_2 * (velocity_final_.y() + velocity_initial_.y()) * *time_spline - + (acceleration_final_.y() - acceleration_initial_.y()) * pow(*time_spline, 2)); + + a5_.z() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (position_final_.z() - position_initial_.z()) - + GAMMA_2 * (velocity_final_.z() + velocity_initial_.z()) * *time_spline - + (acceleration_final_.z() - acceleration_initial_.z()) * pow(*time_spline, 2)); + + ROS_DEBUG("Content of the a5 coefficient: [%f, %f, %f].", a5_[0], a5_[1], a5_[2]); + + // The coefficients are used for having the velocity reference trajectory. The polynomial is computed + // derivating the a one. In other words, + // a5 * x^5 + a4 * x^4 + a3 * x^3 + a2 * x^2 + a1 * x + a0 + // 5 * a5 * x^4 + 4 * a4 * x^3 + 3 * a3 * x^2 + 2 * a2 * x + a1 + // b4 = 5 * a5 -- b3 = 4 * a4 -- b2 = 3 * a3 - b1 = 2 * a2 - b0 = a1 + // and so on + b4_ = 5 * a5_; + b3_ = 4 * a4_; + b2_ = 3 * a3_; + b1_ = 2 * a2_; + b0_ = a1_; + + ROS_DEBUG("Content of the b0 coefficient: [%f, %f, %f].", b0_[0], b0_[1], b0_[2]); + ROS_DEBUG("Content of the b1 coefficient: [%f, %f, %f].", b1_[0], b1_[1], b1_[2]); + ROS_DEBUG("Content of the b2 coefficient: [%f, %f, %f].", b2_[0], b2_[1], b2_[2]); + ROS_DEBUG("Content of the b3 coefficient: [%f, %f, %f].", b3_[0], b3_[1], b3_[2]); + ROS_DEBUG("Content of the b4 coefficient: [%f, %f, %f].", b4_[0], b4_[1], b4_[2]); + + // The coefficients are used for having the acceleration reference trajectory. The polynomial is computed + // derivating the b one. In other words, + // 4 * b4 * x^4 + 3 * b3 * x^3 + 2 * b2 * x^2 + b1 * x + // c3 = 4 * b4 -- c2 = 3 * b3 -- c1 = 2 * b2 - c0 = b1 + c3_ = 4 * b4_; + c2_ = 3 * b3_; + c1_ = 2 * b2_; + c0_ = b1_; + + ROS_DEBUG("Content of the c0 coefficient: [%f, %f, %f].", c0_[0], c0_[1], c0_[2]); + ROS_DEBUG("Content of the c1 coefficient: [%f, %f, %f].", c1_[0], c1_[1], c1_[2]); + ROS_DEBUG("Content of the c2 coefficient: [%f, %f, %f].", c2_[0], c2_[1], c2_[2]); + ROS_DEBUG("Content of the c3 coefficient: [%f, %f, %f].", c3_[0], c3_[1], c3_[2]); + + /* ORIENTATION */ + // Parameters used for the orientation and its derivatives. The coefficient + // refers to the ROLL (X), PITCH (Y) and YAW (Z) g0_ = orientation_initial_; + ROS_DEBUG("Publishing orientation initial: [%f, %f, %f].", orientation_initial_[0], orientation_initial_[1], orientation_initial_[2]); + ROS_DEBUG("Content of the g0 coefficient: [%f, %f, %f].", g0_[0], g0_[1], g0_[2]); + g1_ = angular_velocity_initial_; + ROS_DEBUG("Publishing angular rate initial: [%f, %f, %f].", angular_velocity_initial_[0], angular_velocity_initial_[1], angular_velocity_initial_[2]); + ROS_DEBUG("Content of the g1 coefficient: [%f, %f, %f].", g1_[0], g1_[1], g1_[2]); + g2_.x() = angular_acceleration_initial_.x() * DELTA_1; g2_.y() = angular_acceleration_initial_.y() * DELTA_1; g2_.z() = angular_acceleration_initial_.z() * DELTA_1; - g3_.x() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.x() - orientation_initial_.x()) - (ALPHA_3 * angular_velocity_final_.x() + ALPHA_4 * - angular_velocity_initial_.x()) * *time_spline - (ALPHA_5 * angular_acceleration_final_.x() - angular_acceleration_initial_.x()) * pow(*time_spline, 2)); - g3_.y() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.y() - orientation_initial_.y()) - (ALPHA_3 * angular_velocity_final_.y() + ALPHA_4 * - angular_velocity_initial_.y()) * *time_spline - (ALPHA_5 * angular_acceleration_final_.y() - angular_acceleration_initial_.y()) * pow(*time_spline, 2)); - g3_.z() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.z() - orientation_initial_.z()) - (ALPHA_3 * angular_velocity_final_.z() + ALPHA_4 * - angular_velocity_initial_.z()) * *time_spline - (ALPHA_5 * angular_acceleration_final_.z() - angular_acceleration_initial_.z()) * pow(*time_spline, 2)); - - g4_.x() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.x() - orientation_final_.x()) + (BETA_2 * angular_velocity_final_.x() + BETA_3 * - angular_velocity_initial_.x()) * *time_spline + (BETA_4 * angular_acceleration_final_.x() - 2 * angular_acceleration_initial_.x()) * pow(*time_spline, 2)); - g4_.y() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.y() - orientation_final_.y()) + (BETA_2 * angular_velocity_final_.y() + BETA_3 * - angular_velocity_initial_.y()) * *time_spline + (BETA_4 * angular_acceleration_final_.y() - 2 * angular_acceleration_initial_.y()) * pow(*time_spline, 2)); - g4_.z() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.z() - orientation_final_.z()) + (BETA_2 * angular_velocity_final_.z() + BETA_3 * - angular_velocity_initial_.z()) * *time_spline + (BETA_4 * angular_acceleration_final_.z() - 2 * angular_acceleration_initial_.z()) * pow(*time_spline, 2)); - - g5_.x() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.x() - orientation_initial_.x()) - GAMMA_2 * (angular_velocity_final_.x() + - angular_velocity_initial_.x()) * *time_spline - (angular_acceleration_final_.x() - angular_acceleration_initial_.x()) * pow(*time_spline, 2)); - g5_.y() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.y() - orientation_initial_.y()) - GAMMA_2 * (angular_velocity_final_.y() + - angular_velocity_initial_.y()) * *time_spline - (angular_acceleration_final_.y() - angular_acceleration_initial_.y()) * pow(*time_spline, 2)); - g5_.z() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.z() - orientation_initial_.z()) - GAMMA_2 * (angular_velocity_final_.z() + - angular_velocity_initial_.z()) * *time_spline - (angular_acceleration_final_.z() - angular_acceleration_initial_.z()) * pow(*time_spline, 2)); - - //h - h0_.x() = 0; - h0_.y() = 0; - h0_.z() = 0; - - h1_.x() = 5*g0_.x(); - h1_.y() = 5*g0_.y(); - h1_.z() = 5*g0_.z(); - - h2_.x() = 4*g1_.x(); - h2_.y() = 4*g1_.y(); - h2_.z() = 4*g1_.z(); - - h3_.x() = 3*g2_.x(); - h3_.y() = 3*g2_.y(); - h3_.z() = 3*g2_.z(); - - h4_.x() = 2*g3_.x(); - h4_.y() = 2*g3_.y(); - h4_.z() = 2*g3_.z(); - - h5_ = g4_; + ROS_DEBUG("Publishing angular acceleration initial: [%f, %f, %f].", angular_acceleration_initial_[0], angular_acceleration_initial_[1], + angular_acceleration_initial_[2]); + ROS_DEBUG("Content of the g2 coefficient: [%f, %f, %f].", g2_[0], g2_[1], g2_[2]); + + g3_.x() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.x() - orientation_initial_.x()) - + (ALPHA_3 * angular_velocity_final_.x() + ALPHA_4 * angular_velocity_initial_.x()) * *time_spline - + (ALPHA_5 * angular_acceleration_final_.x() - angular_acceleration_initial_.x()) * pow(*time_spline, 2)); + + g3_.y() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.y() - orientation_initial_.y()) - + (ALPHA_3 * angular_velocity_final_.y() + ALPHA_4 * angular_velocity_initial_.y()) * *time_spline - + (ALPHA_5 * angular_acceleration_final_.y() - angular_acceleration_initial_.y()) * pow(*time_spline, 2)); + + g3_.z() = (1/(ALPHA_1*pow(*time_spline,3))) * (ALPHA_2 * (orientation_final_.z() - orientation_initial_.z()) - + (ALPHA_3 * angular_velocity_final_.z() + ALPHA_4 * angular_velocity_initial_.z()) * *time_spline - + (ALPHA_5 * angular_acceleration_final_.z() - angular_acceleration_initial_.z()) * pow(*time_spline, 2)); + + ROS_DEBUG("Content of the g3 coefficient: [%f, %f, %f].", g3_[0], g3_[1], g3_[2]); + + g4_.x() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.x() - orientation_final_.x()) + + (BETA_2 * angular_velocity_final_.x() + BETA_3 * angular_velocity_initial_.x()) * *time_spline + + (BETA_3 * angular_acceleration_final_.x() - BETA_4 * angular_acceleration_initial_.x()) * pow(*time_spline, 2)); + + g4_.y() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.y() - orientation_final_.y()) + + (BETA_2 * angular_velocity_final_.y() + BETA_3 * angular_velocity_initial_.y()) * *time_spline + + (BETA_3 * angular_acceleration_final_.y() - BETA_4 * angular_acceleration_initial_.y()) * pow(*time_spline, 2)); + + g4_.z() = (1/(ALPHA_1*pow(*time_spline,4))) * (BETA_1 * (orientation_initial_.z() - orientation_final_.z()) + + (BETA_2 * angular_velocity_final_.z() + BETA_3 * angular_velocity_initial_.z()) * *time_spline + + (BETA_3 * angular_acceleration_final_.z() - BETA_4 * angular_acceleration_initial_.z()) * pow(*time_spline, 2)); + + ROS_DEBUG("Content of the g4 coefficient: [%f, %f, %f].", g4_[0], g4_[1], g4_[2]); + + g5_.x() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.x() - orientation_initial_.x()) - + GAMMA_2 * (angular_velocity_final_.x() + angular_velocity_initial_.x()) * *time_spline - + (angular_acceleration_final_.x() - angular_acceleration_initial_.x()) * pow(*time_spline, 2)); + + g5_.y() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.y() - orientation_initial_.y()) - + GAMMA_2 * (angular_velocity_final_.y() + angular_velocity_initial_.y()) * *time_spline - + (angular_acceleration_final_.y() - angular_acceleration_initial_.y()) * pow(*time_spline, 2)); + + g5_.z() = (1/(ALPHA_1*pow(*time_spline,5))) * (GAMMA_1 * (orientation_final_.z() - orientation_initial_.z()) - + GAMMA_2 * (angular_velocity_final_.z() + angular_velocity_initial_.z()) * *time_spline - + (angular_acceleration_final_.z() - angular_acceleration_initial_.z()) * pow(*time_spline, 2)); + + ROS_DEBUG("Content of the g5 coefficient: [%f, %f, %f].", g5_[0], g5_[1], g5_[2]); + + // The coefficients are used for having the angular velocity reference trajectory. The polynomial is computed + // derivating the g one. In other words, + // g5 * x^5 + g4 * x^4 + g3 * x^3 + g2 * x^2 + g1 * x + g0 + // 5 * g5 * x^4 + 4 * g4 * x^3 + 3 * g3 * x^2 + 2 * g2 * x + g1 + // h4 = 5 * g5 -- h3 = 4 * g4 -- h2 = 3 * g3 - h1 = 2 * g2 - h0 = g1 + // and so on + h4_ = 5 * g5_; + h3_ = 4 * g4_; + h2_ = 3 * g3_; + h1_ = 2 * g2_; + h0_ = g1_; + + ROS_DEBUG("Content of the h0 coefficient: [%f, %f, %f].", h0_[0], h0_[1], h0_[2]); + ROS_DEBUG("Content of the h1 coefficient: [%f, %f, %f].", h1_[0], h1_[1], h1_[2]); + ROS_DEBUG("Content of the h2 coefficient: [%f, %f, %f].", h2_[0], h2_[1], h2_[2]); + ROS_DEBUG("Content of the h3 coefficient: [%f, %f, %f].", h3_[0], h3_[1], h3_[2]); + ROS_DEBUG("Content of the h4 coefficient: [%f, %f, %f].", h4_[0], h4_[1], h4_[2]); + + // The coefficients are used for having the angular acceleration reference trajectory. The polynomial is computed + // derivating the b one. In other words, + // 4 * h4 * x^4 + 3 * h3 * x^3 + 2 * h2 * x^2 + h1 * x + // i3 = 4 * h4 -- i2 = 3 * h3 -- i1 = 2 * h2 - i0 = h1 + i3_ = 4 * h4_; + i2_ = 3 * h3_; + i1_ = 2 * h2_; + i0_ = h1_; + + ROS_DEBUG("Content of the i0 coefficient: [%f, %f, %f].", i0_[0], i0_[1], i0_[2]); + ROS_DEBUG("Content of the i1 coefficient: [%f, %f, %f].", i1_[0], i1_[1], i1_[2]); + ROS_DEBUG("Content of the i2 coefficient: [%f, %f, %f].", i2_[0], i2_[1], i2_[2]); + ROS_DEBUG("Content of the i3 coefficient: [%f, %f, %f].", i3_[0], i3_[1], i3_[2]); } @@ -424,7 +489,7 @@ int main(int argc, char** argv) { } // Trajectory message - mav_msgs::DroneState trajectory_msg; + mav_msgs::DroneState trajectory_msg, trajectory_msg_pre; trajectory_msg.header.stamp = ros::Time::now(); mav_msgs::EigenDroneState eigen_reference; rotors_gazebo::HoveringExampleSpline hovering_example_spline_generator; @@ -440,24 +505,39 @@ int main(int argc, char** argv) { hovering_example_spline_generator.InitializeParams(); } + // Reading the final time. This is stop condition for the spline generator + double end_generation_time; + hovering_example_spline_generator.GetRosParameterHovering(nh_private, "time_final/time", end_generation_time, &end_generation_time); + double initial_time, final_time; initial_time = START_SIMULATION_TIME; - // Publish the trajectory values until the final values is reached + // Publish the trajectory values until the final values is reached while(true){ final_time = ros::Time::now().toSec(); hovering_example_spline_generator.TrajectoryCallback(&eigen_reference, &final_time, &initial_time); mav_msgs::eigenDroneFromStateToMsg(&eigen_reference, trajectory_msg); - trajectory_pub.publish(trajectory_msg); + + // new message + if(eigen_reference.position_W[0] <= check_position_final[0] && + eigen_reference.position_W[1] <= check_position_final[1] && + eigen_reference.position_W[2] <= check_position_final[2]){ + trajectory_pub.publish(trajectory_msg); + trajectory_msg_pre = trajectory_msg; + } ROS_DEBUG("Publishing waypoint from msg: [%f, %f, %f].", trajectory_msg.position.x, trajectory_msg.position.y, trajectory_msg.position.z); ROS_DEBUG("Publishing waypoint: [%f, %f, %f].", eigen_reference.position_W[0], eigen_reference.position_W[1], eigen_reference.position_W[2]); ros::Duration(SAMPLING_TIME).sleep(); - if(eigen_reference.position_W[0] >= check_position_final[0] && eigen_reference.position_W[1] >= check_position_final[1] && eigen_reference.position_W[2] >= check_position_final[2]) - break; + // Hold the message until the simulation ends + if(eigen_reference.position_W[0] > check_position_final[0] && + eigen_reference.position_W[1] > check_position_final[1] && + eigen_reference.position_W[2] > check_position_final[2]) + trajectory_pub.publish(trajectory_msg_pre); + } ros::spin();