From 1a44f0cfe16c9de1f27b9f4cef5ea50d67dc1ebb Mon Sep 17 00:00:00 2001
From: Ezzat Elokda <elokdae@ethz.ch>
Date: Mon, 31 Oct 2022 20:30:44 +0100
Subject: [PATCH] Implemented MPC controller for tutorial controller
---
.../forms/tutorialcontrollertab.ui | 1413 +++++++----------
.../include/tutorialcontrollertab.h | 16 +-
.../src/tutorialcontrollertab.cpp | 132 +-
.../include/nodes/TutorialControllerService.h | 130 +-
.../dfall_pkg/param/TutorialController.yaml | 27 +-
.../src/nodes/TutorialControllerService.cpp | 751 +++++++--
6 files changed, 1360 insertions(+), 1109 deletions(-)
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tutorialcontrollertab.ui b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tutorialcontrollertab.ui
index 45743c6f..49b90d6a 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tutorialcontrollertab.ui
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/forms/tutorialcontrollertab.ui
@@ -19,8 +19,8 @@
<string>Form</string>
</property>
<layout class="QGridLayout" name="gridLayout">
- <item row="12" column="0" rowspan="2">
- <layout class="QGridLayout" name="gridLayout_3">
+ <item row="9" column="0">
+ <layout class="QGridLayout" name="gridLayout_2">
<property name="leftMargin">
<number>0</number>
</property>
@@ -33,11 +33,8 @@
<property name="bottomMargin">
<number>0</number>
</property>
- <property name="horizontalSpacing">
- <number>20</number>
- </property>
- <item row="0" column="1">
- <widget class="QPushButton" name="custom_button_2">
+ <item row="3" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_x">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -46,74 +43,28 @@
</property>
<property name="maximumSize">
<size>
- <width>240</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
- <property name="text">
- <string>Button 2</string>
- </property>
- </widget>
- </item>
- <item row="0" column="3">
- <widget class="QPushButton" name="custom_button_4">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>240</width>
- <height>60</height>
- </size>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
</property>
<property name="text">
- <string>Button 4</string>
- </property>
- </widget>
- </item>
- <item row="1" column="0">
- <widget class="QLineEdit" name="lineEdit_custom_1">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>240</width>
- <height>60</height>
- </size>
+ <string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QPushButton" name="custom_button_3">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>240</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Button 3</string>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="0" column="0">
- <widget class="QPushButton" name="custom_button_1">
+ <item row="4" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -122,17 +73,22 @@
</property>
<property name="maximumSize">
<size>
- <width>240</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
- <property name="text">
- <string>Button 1</string>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
</widget>
</item>
- <item row="0" column="5">
- <spacer name="horizontalSpacer">
+ <item row="3" column="7">
+ <spacer name="horizontalSpacer_3">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
@@ -144,163 +100,22 @@
</property>
</spacer>
</item>
- <item row="0" column="4">
- <widget class="QPushButton" name="custom_button_5">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>240</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Button 5</string>
- </property>
- </widget>
- </item>
- <item row="1" column="1">
- <widget class="QLineEdit" name="lineEdit_custom_2">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>240</width>
- <height>60</height>
- </size>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLineEdit" name="lineEdit_custom_3">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>240</width>
- <height>60</height>
- </size>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QLineEdit" name="lineEdit_custom_4">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>240</width>
- <height>60</height>
- </size>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QLineEdit" name="lineEdit_custom_5">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
+ <item row="4" column="1">
+ <widget class="QLabel" name="label_row_y">
<property name="maximumSize">
<size>
- <width>240</width>
+ <width>16777215</width>
<height>60</height>
</size>
</property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- </layout>
- </item>
- <item row="9" column="0">
- <layout class="QGridLayout" name="gridLayout_2">
- <property name="leftMargin">
- <number>0</number>
- </property>
- <property name="topMargin">
- <number>0</number>
- </property>
- <property name="rightMargin">
- <number>0</number>
- </property>
- <property name="bottomMargin">
- <number>0</number>
- </property>
- <item row="0" column="0">
- <widget class="QLabel" name="label_measured_title">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
<property name="text">
- <string>Measured</string>
+ <string>y [m]</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="3" column="2">
- <widget class="QLineEdit" name="lineEdit_error_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
<item row="1" column="0">
<layout class="QHBoxLayout" name="horizontalLayout_5">
<property name="spacing">
@@ -384,44 +199,84 @@
</property>
</widget>
</item>
- <item row="5" column="1">
- <widget class="QLabel" name="label_row_z">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
+ <item row="1" column="4">
+ <widget class="QLabel" name="label_new_title_line2">
<property name="text">
- <string>z [m]</string>
+ <string>Setpoint</string>
</property>
<property name="alignment">
<set>Qt::AlignCenter</set>
</property>
</widget>
</item>
- <item row="6" column="1">
- <widget class="QLabel" name="label_row_yaw">
+ <item row="3" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="maximumSize">
<size>
- <width>16777215</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
<property name="text">
- <string>yaw [deg]</string>
+ <string>xx.xx</string>
</property>
<property name="alignment">
- <set>Qt::AlignCenter</set>
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="3" column="6">
- <layout class="QHBoxLayout" name="horizontalLayout">
- <item>
- <widget class="QPushButton" name="x_increment_minus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <item row="0" column="6">
+ <widget class="QLabel" name="label_2">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Increment</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <widget class="QLabel" name="label_measured_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>Measured</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="4" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item>
+ <widget class="QPushButton" name="y_increment_minus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
@@ -438,7 +293,7 @@
</widget>
</item>
<item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_x">
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_y">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -465,7 +320,13 @@
</widget>
</item>
<item>
- <widget class="QPushButton" name="x_increment_plus_button">
+ <widget class="QPushButton" name="y_increment_plus_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
<property name="maximumSize">
<size>
<width>60</width>
@@ -479,108 +340,8 @@
</item>
</layout>
</item>
- <item row="3" column="5">
- <spacer name="horizontalSpacer_2">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
- </property>
- <property name="sizeType">
- <enum>QSizePolicy::Fixed</enum>
- </property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>20</height>
- </size>
- </property>
- </spacer>
- </item>
- <item row="4" column="1">
- <widget class="QLabel" name="label_row_y">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>y [m]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="7" column="1">
- <widget class="QLabel" name="label_row_pitch">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>pitch [deg]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="8" column="1">
- <widget class="QLabel" name="label_row_roll">
- <property name="maximumSize">
- <size>
- <width>16777215</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>roll [deg]</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="2">
- <widget class="QLabel" name="label_error_title_line2">
- <property name="text">
- <string>meas-ref</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="3">
- <widget class="QLabel" name="label_current_title_2">
- <property name="text">
- <string>Setpoint</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="0" column="2">
- <widget class="QLabel" name="label_error_title">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Error</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="7" column="3">
- <widget class="QPushButton" name="default_setpoint_button">
+ <item row="4" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -593,39 +354,24 @@
<height>60</height>
</size>
</property>
- <property name="text">
- <string>Default</string>
- </property>
- </widget>
- </item>
- <item row="0" column="4">
- <widget class="QLabel" name="label_new_title">
<property name="font">
<font>
- <weight>75</weight>
- <bold>true</bold>
+ <family>Courier</family>
</font>
</property>
<property name="text">
- <string>New</string>
+ <string>xx.xx</string>
</property>
<property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="4">
- <widget class="QLabel" name="label_new_title_line2">
- <property name="text">
- <string>Setpoint</string>
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
- <item row="4" column="2">
- <widget class="QLineEdit" name="lineEdit_error_y">
+ <item row="5" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_z">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -654,8 +400,42 @@
</property>
</widget>
</item>
- <item row="5" column="2">
- <widget class="QLineEdit" name="lineEdit_error_z">
+ <item row="3" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <item>
+ <widget class="QLineEdit" name="lineEdit_measured_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ </layout>
+ </item>
+ <item row="5" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -673,44 +453,24 @@
<family>Courier</family>
</font>
</property>
- <property name="text">
- <string>xx.xx</string>
- </property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
</widget>
</item>
- <item row="6" column="2">
- <widget class="QLineEdit" name="lineEdit_error_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
+ <item row="0" column="2">
+ <widget class="QLabel" name="label_error_title">
<property name="font">
<font>
- <family>Courier</family>
+ <weight>75</weight>
+ <bold>true</bold>
</font>
</property>
<property name="text">
- <string>xx.xx</string>
+ <string>Error</string>
</property>
<property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
+ <set>Qt::AlignCenter</set>
</property>
</widget>
</item>
@@ -730,271 +490,10 @@
</property>
</widget>
</item>
- <item row="4" column="3">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="5" column="3">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="6" column="3">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="3" column="3">
- <widget class="QLineEdit" name="lineEdit_setpoint_current_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- <item row="0" column="6">
- <widget class="QLabel" name="label_2">
- <property name="font">
- <font>
- <weight>75</weight>
- <bold>true</bold>
- </font>
- </property>
- <property name="text">
- <string>Increment</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="6" column="4">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="7" column="4">
- <widget class="QPushButton" name="set_setpoint_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>Set New</string>
- </property>
- </widget>
- </item>
- <item row="4" column="4">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_y">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="5" column="4">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_z">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="3" column="4">
- <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- </widget>
- </item>
- <item row="1" column="6">
- <widget class="QLabel" name="label">
- <property name="text">
- <string>Setpoint</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignCenter</set>
- </property>
- </widget>
- </item>
- <item row="4" column="6">
- <layout class="QHBoxLayout" name="horizontalLayout_2">
+ <item row="5" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout_3">
<item>
- <widget class="QPushButton" name="y_increment_minus_button">
+ <widget class="QPushButton" name="z_increment_minus_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -1013,7 +512,7 @@
</widget>
</item>
<item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_y">
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_z">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -1040,7 +539,7 @@
</widget>
</item>
<item>
- <widget class="QPushButton" name="y_increment_plus_button">
+ <widget class="QPushButton" name="z_increment_plus_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -1060,38 +559,19 @@
</item>
</layout>
</item>
- <item row="6" column="6">
- <layout class="QHBoxLayout" name="horizontalLayout_4">
- <item>
- <widget class="QPushButton" name="yaw_increment_minus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>-</string>
- </property>
- </widget>
- </item>
+ <item row="4" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_7">
<item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_yaw">
+ <widget class="QLineEdit" name="lineEdit_measured_y">
<property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="maximumSize">
<size>
- <width>140</width>
+ <width>180</width>
<height>60</height>
</size>
</property>
@@ -1101,38 +581,54 @@
</font>
</property>
<property name="text">
- <string>15</string>
+ <string>xx.xx</string>
</property>
<property name="alignment">
<set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
</property>
- </widget>
- </item>
- <item>
- <widget class="QPushButton" name="yaw_increment_plus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>60</width>
- <height>60</height>
- </size>
- </property>
- <property name="text">
- <string>+</string>
+ <property name="readOnly">
+ <bool>true</bool>
</property>
</widget>
</item>
</layout>
</item>
- <item row="5" column="6">
- <layout class="QHBoxLayout" name="horizontalLayout_3">
+ <item row="5" column="1">
+ <widget class="QLabel" name="label_row_z">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>z [m]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="0" column="4">
+ <widget class="QLabel" name="label_new_title">
+ <property name="font">
+ <font>
+ <weight>75</weight>
+ <bold>true</bold>
+ </font>
+ </property>
+ <property name="text">
+ <string>New</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="6">
+ <layout class="QHBoxLayout" name="horizontalLayout">
<item>
- <widget class="QPushButton" name="z_increment_minus_button">
+ <widget class="QPushButton" name="x_increment_minus_button">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -1151,7 +647,7 @@
</widget>
</item>
<item>
- <widget class="QLineEdit" name="lineEdit_setpoint_increment_z">
+ <widget class="QLineEdit" name="lineEdit_setpoint_increment_x">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
<horstretch>0</horstretch>
@@ -1178,13 +674,7 @@
</widget>
</item>
<item>
- <widget class="QPushButton" name="z_increment_plus_button">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="Fixed">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
+ <widget class="QPushButton" name="x_increment_plus_button">
<property name="maximumSize">
<size>
<width>60</width>
@@ -1198,23 +688,84 @@
</item>
</layout>
</item>
- <item row="3" column="7">
- <spacer name="horizontalSpacer_3">
- <property name="orientation">
- <enum>Qt::Horizontal</enum>
+ <item row="4" column="2">
+ <widget class="QLineEdit" name="lineEdit_error_y">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
</property>
- <property name="sizeHint" stdset="0">
+ <property name="maximumSize">
<size>
- <width>40</width>
- <height>20</height>
+ <width>180</width>
+ <height>60</height>
</size>
</property>
- </spacer>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
</item>
- <item row="3" column="0">
- <layout class="QHBoxLayout" name="horizontalLayout_6">
+ <item row="1" column="6">
+ <widget class="QLabel" name="label">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="3" column="4">
+ <widget class="QLineEdit" name="lineEdit_setpoint_new_x">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="1" column="2">
+ <widget class="QLabel" name="label_error_title_line2">
+ <property name="text">
+ <string>meas-ref</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_8">
<item>
- <widget class="QLineEdit" name="lineEdit_measured_x">
+ <widget class="QLineEdit" name="lineEdit_measured_z">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -1245,10 +796,104 @@
</item>
</layout>
</item>
- <item row="4" column="0">
- <layout class="QHBoxLayout" name="horizontalLayout_7">
+ <item row="3" column="5">
+ <spacer name="horizontalSpacer_2">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Fixed</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>20</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ <item row="1" column="3">
+ <widget class="QLabel" name="label_current_title_2">
+ <property name="text">
+ <string>Setpoint</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="5" column="3">
+ <widget class="QLineEdit" name="lineEdit_setpoint_current_z">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="font">
+ <font>
+ <family>Courier</family>
+ </font>
+ </property>
+ <property name="text">
+ <string>xx.xx</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
+ </property>
+ <property name="readOnly">
+ <bool>true</bool>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="3">
+ <widget class="QPushButton" name="default_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Default</string>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="4">
+ <widget class="QPushButton" name="set_setpoint_button">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>180</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Set New</string>
+ </property>
+ </widget>
+ </item>
+ <item row="8" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_10">
<item>
- <widget class="QLineEdit" name="lineEdit_measured_y">
+ <widget class="QLineEdit" name="lineEdit_measured_yaw">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -1279,10 +924,26 @@
</item>
</layout>
</item>
- <item row="5" column="0">
- <layout class="QHBoxLayout" name="horizontalLayout_8">
+ <item row="8" column="1">
+ <widget class="QLabel" name="label_row_yaw">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>yaw [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
+ </item>
+ <item row="6" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_11">
<item>
- <widget class="QLineEdit" name="lineEdit_measured_z">
+ <widget class="QLineEdit" name="lineEdit_measured_roll">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
@@ -1347,81 +1008,48 @@
</item>
</layout>
</item>
- <item row="6" column="0">
- <layout class="QHBoxLayout" name="horizontalLayout_10">
- <item>
- <widget class="QLineEdit" name="lineEdit_measured_yaw">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- </layout>
+ <item row="6" column="1">
+ <widget class="QLabel" name="label_row_roll">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>roll [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
</item>
- <item row="8" column="0">
- <layout class="QHBoxLayout" name="horizontalLayout_11">
- <item>
- <widget class="QLineEdit" name="lineEdit_measured_roll">
- <property name="sizePolicy">
- <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
- <horstretch>0</horstretch>
- <verstretch>0</verstretch>
- </sizepolicy>
- </property>
- <property name="maximumSize">
- <size>
- <width>180</width>
- <height>60</height>
- </size>
- </property>
- <property name="font">
- <font>
- <family>Courier</family>
- </font>
- </property>
- <property name="text">
- <string>xx.xx</string>
- </property>
- <property name="alignment">
- <set>Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter</set>
- </property>
- <property name="readOnly">
- <bool>true</bool>
- </property>
- </widget>
- </item>
- </layout>
+ <item row="7" column="1">
+ <widget class="QLabel" name="label_row_pitch">
+ <property name="maximumSize">
+ <size>
+ <width>16777215</width>
+ <height>60</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>pitch [deg]</string>
+ </property>
+ <property name="alignment">
+ <set>Qt::AlignCenter</set>
+ </property>
+ </widget>
</item>
</layout>
</item>
- <item row="14" column="0">
+ <item row="13" column="0">
<spacer name="verticalSpacer">
<property name="orientation">
<enum>Qt::Vertical</enum>
</property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Expanding</enum>
+ </property>
<property name="sizeHint" stdset="0">
<size>
<width>20</width>
@@ -1430,24 +1058,201 @@
</property>
</spacer>
</item>
- <item row="11" column="0">
- <spacer name="verticalSpacer_2">
+ <item row="10" column="0">
+ <widget class="Line" name="line">
<property name="orientation">
- <enum>Qt::Vertical</enum>
+ <enum>Qt::Horizontal</enum>
</property>
- <property name="sizeType">
- <enum>QSizePolicy::Fixed</enum>
+ </widget>
+ </item>
+ <item row="0" column="0">
+ <layout class="QHBoxLayout" name="horizontalLayout_12">
+ <property name="spacing">
+ <number>0</number>
</property>
- <property name="sizeHint" stdset="0">
- <size>
- <width>20</width>
- <height>20</height>
- </size>
+ <property name="sizeConstraint">
+ <enum>QLayout::SetDefaultConstraint</enum>
</property>
- </spacer>
+ <item>
+ <widget class="QFrame" name="red_frame_setup_left">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>80</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="green_frame_setup_left">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>80</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(115, 210, 22);</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QPushButton" name="custom_button_1">
+ <property name="sizePolicy">
+ <sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
+ <horstretch>0</horstretch>
+ <verstretch>0</verstretch>
+ </sizepolicy>
+ </property>
+ <property name="minimumSize">
+ <size>
+ <width>400</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>400</width>
+ <height>80</height>
+ </size>
+ </property>
+ <property name="text">
+ <string>Setup MPC Optimization</string>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="green_frame_setup_right">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>80</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color: rgb(115, 210, 22);</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <widget class="QFrame" name="red_frame_setup_right">
+ <property name="minimumSize">
+ <size>
+ <width>20</width>
+ <height>40</height>
+ </size>
+ </property>
+ <property name="maximumSize">
+ <size>
+ <width>20</width>
+ <height>80</height>
+ </size>
+ </property>
+ <property name="styleSheet">
+ <string notr="true">background-color:red;</string>
+ </property>
+ <property name="frameShape">
+ <enum>QFrame::StyledPanel</enum>
+ </property>
+ <property name="frameShadow">
+ <enum>QFrame::Raised</enum>
+ </property>
+ </widget>
+ </item>
+ <item>
+ <spacer name="horizontalSpacer_4">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeType">
+ <enum>QSizePolicy::Expanding</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>0</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
</item>
- <item row="10" column="0">
- <widget class="Line" name="line">
+ <item row="11" column="0" rowspan="2">
+ <layout class="QGridLayout" name="gridLayout_3">
+ <property name="leftMargin">
+ <number>0</number>
+ </property>
+ <property name="topMargin">
+ <number>0</number>
+ </property>
+ <property name="rightMargin">
+ <number>0</number>
+ </property>
+ <property name="bottomMargin">
+ <number>0</number>
+ </property>
+ <property name="horizontalSpacing">
+ <number>20</number>
+ </property>
+ <property name="verticalSpacing">
+ <number>0</number>
+ </property>
+ <item row="0" column="1">
+ <spacer name="horizontalSpacer">
+ <property name="orientation">
+ <enum>Qt::Horizontal</enum>
+ </property>
+ <property name="sizeHint" stdset="0">
+ <size>
+ <width>40</width>
+ <height>20</height>
+ </size>
+ </property>
+ </spacer>
+ </item>
+ </layout>
+ </item>
+ <item row="1" column="0">
+ <widget class="Line" name="line_2">
<property name="orientation">
<enum>Qt::Horizontal</enum>
</property>
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tutorialcontrollertab.h b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tutorialcontrollertab.h
index 94e6a5c1..daac3680 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tutorialcontrollertab.h
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/include/tutorialcontrollertab.h
@@ -99,7 +99,6 @@ private slots:
void on_lineEdit_setpoint_new_x_returnPressed();
void on_lineEdit_setpoint_new_y_returnPressed();
void on_lineEdit_setpoint_new_z_returnPressed();
- void on_lineEdit_setpoint_new_yaw_returnPressed();
void on_set_setpoint_button_clicked();
@@ -111,14 +110,8 @@ private slots:
void on_y_increment_minus_button_clicked();
void on_z_increment_plus_button_clicked();
void on_z_increment_minus_button_clicked();
- void on_yaw_increment_plus_button_clicked();
- void on_yaw_increment_minus_button_clicked();
void on_custom_button_1_clicked();
- void on_custom_button_2_clicked();
- void on_custom_button_3_clicked();
- void on_custom_button_4_clicked();
- void on_custom_button_5_clicked();
@@ -151,6 +144,10 @@ private:
// > For being notified when the setpoint is changed
ros::Subscriber setpointChangedSubscriber;
+ // SUBSCRIBER
+ // > For being notified when the optimization setup status is changed
+ ros::Subscriber optimizationSetupStatusChangedSubscriber;
+
// PUBLISHER
// > For notifying that a custom button is pressed
ros::Publisher customButtonPublisher;
@@ -166,8 +163,11 @@ private:
// For receiving message that the setpoint was changed
void setpointChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+ // For receiving message that the optimization setup status was changed
+ void optimizationSetupStatusChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint);
+
// Publish a message when a custom button is pressed
- void publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer);
+ void publish_custom_button_command(int button_index);
// Fill the header for a message
void fillSetpointMessageHeader( dfall_pkg::SetpointWithHeader & msg );
diff --git a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tutorialcontrollertab.cpp b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tutorialcontrollertab.cpp
index de3c81c0..b18b12d4 100644
--- a/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tutorialcontrollertab.cpp
+++ b/dfall_ws/src/dfall_pkg/GUI_Qt/flyingAgentGUI/src/tutorialcontrollertab.cpp
@@ -47,6 +47,10 @@ TutorialControllerTab::TutorialControllerTab(QWidget *parent) :
ui->red_frame_position_left->setVisible(false);
ui->red_frame_position_right->setVisible(false);
+ // Hide the two green frames that are used to indcate when MPC optimization problem is successfully set up
+ ui->green_frame_setup_left->setVisible(false);
+ ui->green_frame_setup_right->setVisible(false);
+
#ifdef CATKIN_MAKE
@@ -81,6 +85,13 @@ TutorialControllerTab::TutorialControllerTab(QWidget *parent) :
setpointChangedSubscriber = nodeHandle_for_this_gui.subscribe("TutorialControllerService/SetpointChanged", 1, &TutorialControllerTab::setpointChangedCallback, this);
}
+ // SUBSCRIBE TO OPTIMIZATION SETUP STATUS CHANGES
+ // Only if this is an agent GUI
+ if (m_type == TYPE_AGENT)
+ {
+ optimizationSetupStatusChangedSubscriber = nodeHandle_for_this_gui.subscribe("TutorialControllerService/OptimizationSetupStatusChanged", 1, &TutorialControllerTab::optimizationSetupStatusChangedCallback, this);
+ }
+
// CREATE THE CUSTOM BUTTON PRESSED PUBLISHER
customButtonPublisher = nodeHandle_for_this_gui.advertise<dfall_pkg::CustomButtonWithHeader>("TutorialControllerService/CustomButtonPressed", 1);
@@ -133,7 +144,7 @@ TutorialControllerTab::~TutorialControllerTab()
#ifdef CATKIN_MAKE
-void TutorialControllerTab::publish_custom_button_command(int button_index , QLineEdit * lineEdit_pointer)
+void TutorialControllerTab::publish_custom_button_command(int button_index)
{
// Initialise the message as a local variable
dfall_pkg::CustomButtonWithHeader msg;
@@ -141,15 +152,7 @@ void TutorialControllerTab::publish_custom_button_command(int button_index , QLi
fillCustomButtonMessageHeader( msg );
// Fill in the button index
msg.button_index = button_index;
- // Get the line edit data, as a float if possible
- bool isValidFloat = false;
- float lineEdit_as_float = (lineEdit_pointer->text()).toFloat(&isValidFloat);
- // Fill in the data
- if (isValidFloat)
- msg.float_data = lineEdit_as_float;
- else
- msg.string_data = (lineEdit_pointer->text()).toStdString();
- // Publish the setpoint
+ // Publish the button press
this->customButtonPublisher.publish(msg);
// Inform the user about the change
ROS_INFO_STREAM("[TUTORIAL CONTROLLER TAB GUI] button " << button_index << " clicked.");
@@ -160,35 +163,7 @@ void TutorialControllerTab::publish_custom_button_command(int button_index , QLi
void TutorialControllerTab::on_custom_button_1_clicked()
{
#ifdef CATKIN_MAKE
- publish_custom_button_command(1,ui->lineEdit_custom_1);
-#endif
-}
-
-void TutorialControllerTab::on_custom_button_2_clicked()
-{
-#ifdef CATKIN_MAKE
- publish_custom_button_command(2,ui->lineEdit_custom_2);
-#endif
-}
-
-void TutorialControllerTab::on_custom_button_3_clicked()
-{
-#ifdef CATKIN_MAKE
- publish_custom_button_command(3,ui->lineEdit_custom_3);
-#endif
-}
-
-void TutorialControllerTab::on_custom_button_4_clicked()
-{
-#ifdef CATKIN_MAKE
- publish_custom_button_command(4,ui->lineEdit_custom_4);
-#endif
-}
-
-void TutorialControllerTab::on_custom_button_5_clicked()
-{
-#ifdef CATKIN_MAKE
- publish_custom_button_command(5,ui->lineEdit_custom_5);
+ publish_custom_button_command(1);
#endif
}
@@ -231,8 +206,6 @@ void TutorialControllerTab::setMeasuredPose(float x , float y , float z , float
float error_y = y - (ui->lineEdit_setpoint_current_y->text() ).toFloat();
float error_z = z - (ui->lineEdit_setpoint_current_z->text() ).toFloat();
- float error_yaw_deg = yaw * RAD2DEG - (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
-
// UPDATE THE ERROR COLUMN
if (error_x < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_error_x->setText(qstr + QString::number( error_x, 'f', 3));
@@ -241,9 +214,6 @@ void TutorialControllerTab::setMeasuredPose(float x , float y , float z , float
if (error_z < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_error_z->setText(qstr + QString::number( error_z, 'f', 3));
- if (error_yaw_deg < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_error_yaw->setText(qstr + QString::number( error_yaw_deg , 'f', 1));
-
// Ensure the red frames are not visible
if ( ui->red_frame_position_left->isVisible() )
ui->red_frame_position_left->setVisible(false);
@@ -275,7 +245,6 @@ void TutorialControllerTab::poseDataUnavailableSlot()
ui->lineEdit_error_x->setText("xx.xx");
ui->lineEdit_error_y->setText("xx.xx");
ui->lineEdit_error_z->setText("xx.xx");
- ui->lineEdit_error_yaw->setText("xx.xx");
}
@@ -329,15 +298,26 @@ void TutorialControllerTab::setpointChangedCallback(const dfall_pkg::SetpointWit
ui->lineEdit_setpoint_current_y->setText(qstr + QString::number( y, 'f', 3));
if (z < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_setpoint_current_z->setText(qstr + QString::number( z, 'f', 3));
-
- if (yaw < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_setpoint_current_yaw->setText(qstr + QString::number( yaw * RAD2DEG, 'f', 1));
}
#endif
+// *** OPTIMIZATION SETUP STATUS CHANGED CALLBACK
+#ifdef CATKIN_MAKE
+void TutorialControllerTab::optimizationSetupStatusChangedCallback(const dfall_pkg::SetpointWithHeader& newSetpoint)
+{
+ // USING SETPOINT X TO COMMUNICATE OPTIMIZATION SUCCESSFUL FLAG
+ bool optimization_setup_successful = (newSetpoint.x == 1.0);
+ // COTNROL THE VISIBILITY OF THE STATUS INDICATORS
+ ui->green_frame_setup_left->setVisible(optimization_setup_successful);
+ ui->green_frame_setup_right->setVisible(optimization_setup_successful);
+
+ ui->red_frame_setup_left->setVisible(!optimization_setup_successful);
+ ui->red_frame_setup_right->setVisible(!optimization_setup_successful);
+}
+#endif
@@ -402,11 +382,6 @@ void TutorialControllerTab::on_lineEdit_setpoint_new_z_returnPressed()
ui->set_setpoint_button->animateClick();
}
-void TutorialControllerTab::on_lineEdit_setpoint_new_yaw_returnPressed()
-{
- ui->set_setpoint_button->animateClick();
-}
-
void TutorialControllerTab::on_set_setpoint_button_clicked()
{
@@ -429,11 +404,6 @@ void TutorialControllerTab::on_set_setpoint_button_clicked()
z = (ui->lineEdit_setpoint_new_z->text()).toFloat();
else
z = (ui->lineEdit_setpoint_current_z->text()).toFloat();
- // > For yaws
- if(!ui->lineEdit_setpoint_new_yaw->text().isEmpty())
- yaw = (ui->lineEdit_setpoint_new_yaw->text()).toFloat();
- else
- yaw = (ui->lineEdit_setpoint_current_yaw->text()).toFloat();
// Call the function to publish the setpoint
publishSetpoint(x,y,z,yaw);
@@ -561,39 +531,6 @@ void TutorialControllerTab::on_z_increment_minus_button_clicked()
}
}
-void TutorialControllerTab::on_yaw_increment_plus_button_clicked()
-{
- // Only need to do something if the field is not empty
- if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
- {
- // Call the function to increment the setpoint
- increment_setpoint_by(0.0,0.0,0.0, (ui->lineEdit_setpoint_increment_yaw->text()).toFloat() );
- }
- else
- {
- #ifdef CATKIN_MAKE
- // Inform the user that nothing can be done
- ROS_INFO_STREAM("[TUTORIAL CONTROLLER GUI] Increment yaw setpoint clicked but field is empty");
- #endif
- }
-}
-void TutorialControllerTab::on_yaw_increment_minus_button_clicked()
-{
- // Only need to do something if the field is not empty
- if(!ui->lineEdit_setpoint_increment_yaw->text().isEmpty())
- {
- // Call the function to increment the setpoint
- increment_setpoint_by(0.0,0.0,0.0, -(ui->lineEdit_setpoint_increment_yaw->text()).toFloat() );
- }
- else
- {
- #ifdef CATKIN_MAKE
- // Inform the user that nothing can be done
- ROS_INFO_STREAM("[TUTORIAL CONTROLLER GUI] Increment yaw setpoint clicked but field is empty");
- #endif
- }
-}
-
void TutorialControllerTab::increment_setpoint_by(float x_inc, float y_inc, float z_inc, float yaw_inc_degrees)
{
@@ -607,7 +544,7 @@ void TutorialControllerTab::increment_setpoint_by(float x_inc, float y_inc, floa
(ui->lineEdit_setpoint_current_x->text() ).toFloat() + x_inc,
(ui->lineEdit_setpoint_current_y->text() ).toFloat() + y_inc,
(ui->lineEdit_setpoint_current_z->text() ).toFloat() + z_inc,
- (ui->lineEdit_setpoint_current_yaw->text()).toFloat() + yaw_inc_degrees
+ 0.0
);
}
else if (m_type == TYPE_COORDINATOR)
@@ -622,15 +559,12 @@ void TutorialControllerTab::increment_setpoint_by(float x_inc, float y_inc, floa
// > For x
if(!ui->lineEdit_setpoint_new_x->text().isEmpty())
x = (ui->lineEdit_setpoint_new_x->text()).toFloat();
- // > For x
+ // > For y
if(!ui->lineEdit_setpoint_new_y->text().isEmpty())
y = (ui->lineEdit_setpoint_new_y->text()).toFloat();
- // > For x
+ // > For z
if(!ui->lineEdit_setpoint_new_z->text().isEmpty())
z = (ui->lineEdit_setpoint_new_z->text()).toFloat();
- // > For x
- if(!ui->lineEdit_setpoint_new_yaw->text().isEmpty())
- yaw = (ui->lineEdit_setpoint_new_yaw->text()).toFloat();
// Add the increment to this
float x_new = x + x_inc;
@@ -649,9 +583,6 @@ void TutorialControllerTab::increment_setpoint_by(float x_inc, float y_inc, floa
if (z_new < 0.0f) qstr = ""; else qstr = "+";
ui->lineEdit_setpoint_new_z->setText(qstr + QString::number( z_new, 'f', 3));
- if (yaw_new < 0.0f) qstr = ""; else qstr = "+";
- ui->lineEdit_setpoint_new_yaw->setText(qstr + QString::number( yaw_new, 'f', 3));
-
// Call the function to publish the setpoint
publishSetpoint(x_new,y_new,z_new,yaw_new);
@@ -738,7 +669,6 @@ void TutorialControllerTab::setAgentIDsToCoordinate(QVector<int> agentIDs , bool
ui->lineEdit_setpoint_current_x->setText("xx.xx");
ui->lineEdit_setpoint_current_y->setText("xx.xx");
ui->lineEdit_setpoint_current_z->setText("xx.xx");
- ui->lineEdit_setpoint_current_yaw->setText("xx.xx");
}
#endif
diff --git a/dfall_ws/src/dfall_pkg/include/nodes/TutorialControllerService.h b/dfall_ws/src/dfall_pkg/include/nodes/TutorialControllerService.h
index 0e247368..464ebe17 100644
--- a/dfall_ws/src/dfall_pkg/include/nodes/TutorialControllerService.h
+++ b/dfall_ws/src/dfall_pkg/include/nodes/TutorialControllerService.h
@@ -80,13 +80,17 @@
// Need for having a ROS "bag" to store data for post-analysis
//#include <rosbag/bag.h>
+// Include Eigen for matrix operations
+#include "Eigen/Dense"
+#include "Eigen/Sparse"
-
+// Include OSQP optimization platform
+#include "osqp.h"
// Namespacing the package
using namespace dfall_pkg;
-
+using namespace Eigen;
@@ -134,7 +138,11 @@ using namespace dfall_pkg;
-
+// *** CONSTANTS ***
+const float const_gravity = 9.81;
+const int const_num_states = 8; // not controlling yaw with MPC
+const int const_num_inputs = 3; // not controlling yaw with MPC
+const int const_num_states_plus_inputs = const_num_states + const_num_inputs;
@@ -161,46 +169,104 @@ std::string m_namespace_to_coordinator_parameter_service;
-// VARAIBLES FOR VALUES LOADED FROM THE YAML FILE
+// *** VARAIBLES FOR VALUES LOADED FROM THE YAML FILE ***
// > the mass of the crazyflie, in [grams]
float yaml_cf_mass_in_grams = 30.0;
// The weight of the Crazyflie in Newtons, i.e., mg
-float m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81 / 1000.0;
+float m_cf_weight_in_newtons = yaml_cf_mass_in_grams * const_gravity / 1000.0;
// > the frequency at which the controller is running
float yaml_control_frequency = 200.0;
-// > the default setpoint, the ordering is (x,y,z,yaw),
-// with units [meters,meters,meters,radians]
-std::vector<float> yaml_default_setpoint = {0.0,0.0,0.4,0.0};
+// > the default setpoint, the ordering is (x, y, z),
+// with units [meters, meters, meters]
+std::vector<float> yaml_default_setpoint = {0.0, 0.0, 0.4};
+// > the MPC prediction horizon, in discrete time steps
+int yaml_prediction_horizon = 100;
+// > the MPC state cost matrix diagonal entries, the ordering is (x, y, z, xdot, ydot, zdot, roll, pitch)
+std::vector<float> yaml_state_cost_diagonals = {1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0, 1.0};
+// > the MPC input cost matrix diagonal entries, the ordering is (totalThrust, rollRate, pitchRate)
+std::vector<float> yaml_input_cost_diagonals = {1.0, 1.0, 1.0};
-// The location error of the Crazyflie at the "previous" time step
-float m_previous_stateErrorInertial[9];
+// > the MPC state constraints, the ordering is (x, y, z, xdot, ydot, zdot, roll, pitch)
+std::vector<float> yaml_min_state_constraints = {-2.0, -2.0, 0.1, -100, -100, -100, -0.5236, -0.5236};
+std::vector<float> yaml_max_state_constraints = {2.0, 2.0, 3.0, 100, 100, 100, 0.5236, 0.5236};
-// The setpoint to be tracked, the ordering is (x,y,z,yaw),
-// with units [meters,meters,meters,radians]
-std::vector<float> m_setpoint{0.0,0.0,0.4,0.0};
+// > the MPC input constraints, the ordering is (totalThrust, rollRate, pitchRate)
+std::vector<float> yaml_min_input_constraints = {0.1597, -1.5708, -1.5708};
+std::vector<float> yaml_max_input_constraints = {0.4791, 1.5708, 1.5708};
-// The LQR Controller parameters for "LQR_RATE_MODE"
-std::vector<float> m_gainMatrixRollRate = { 0.00,-3.00, 0.00, 0.00,-1.00, 0.00, 5.00, 0.00, 0.00};
-std::vector<float> m_gainMatrixPitchRate = { 3.00, 0.00, 0.00, 1.00, 0.00, 0.00, 0.00, 5.00, 0.00};
+
+// *** LQR CONTROLLER PARAMETERS FOR BACKUP CONTROLLER ***
+std::vector<float> m_gainMatrixRollRate = { 0.00,-4.00, 0.00, 0.00,-1.00, 0.00, 5.00, 0.00, 0.00};
+std::vector<float> m_gainMatrixPitchRate = { 4.00, 0.00, 0.00, 1.00, 0.00, 0.00, 0.00, 5.00, 0.00};
std::vector<float> m_gainMatrixYawRate = { 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 0.00, 2.00};
-std::vector<float> m_gainMatrixThrust = { 0.00, 0.00, 0.30, 0.00, 0.00, 0.20, 0.00, 0.00, 0.00};
+std::vector<float> m_gainMatrixThrust = { 0.00, 0.00, 0.50, 0.00, 0.00, 0.25, 0.00, 0.00, 0.00};
+
+
+
+// *** VARIABLES USED FOR THE MPC OPTIMIZATION PROBLEM WITH OSQP ***
+// flag to indicate if the MPC optimization problem was setup successfully with OSQP
+bool m_mpc_optimization_setup_success = false;
+
+// the state cost matrix
+MatrixXf m_state_cost_matrix;
+
+// the current state vector
+MatrixXf m_current_state_vector = MatrixXf::Zero(const_num_states, 1);
+
+// the state setpoint to be tracked
+MatrixXf m_state_setpoint_vector = MatrixXf::Zero(const_num_states, 1);
+
+// the OSQP linear cost vector as Eigen matrix
+MatrixXf m_osqp_q_vector;
+
+// the OSQP linear cost vector for one time step as Eigen matrix
+MatrixXf m_osqp_q_vector_for_one_time_step;
+
+// the OSQP linear cost vector as c_float array
+c_float* m_osqp_q_runtime_cfloat;
+
+// the OSQP inequality constraint lower bound vector as c_float array
+c_float* m_osqp_l_runtime_cfloat;
+
+// the OSQP inequality constraint upper bound vector as c_float array
+c_float* m_osqp_u_runtime_cfloat;
+
+// the osqp workspace
+OSQPWorkspace* m_osqp_work;
+// the input vector to apply
+MatrixXf m_mpc_input_vector_to_apply = MatrixXf::Zero(const_num_inputs, 1);
+
+
+
+// *** MISCELLANEOUS ***
+// The location error of the Crazyflie at the "previous" time step
+float m_previous_stateErrorInertial[9];
+
+// The setpoint to be tracked, the ordering is (x, y, z, yaw),
+// with units [meters, meters, meters, radians]
+std::vector<float> m_setpoint{0.0, 0.0, 0.4, 0.0};
// ROS Publisher for debugging variables
ros::Publisher m_debugPublisher;
-// ROS Publisher for inform the network about
-// changes to the setpoin
+// ROS Publisher to inform the network about
+// changes to the setpoint
ros::Publisher m_setpointChangedPublisher;
+// ROS Publisher to inform the network about changes to the optimization setup status
+ros::Publisher m_optimizationSetupStatusChangedPublisher;
+
+
+
@@ -248,4 +314,28 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
// FOR LOADING THE YAML PARAMETERS
void timerCallback_initial_load_yaml(const ros::TimerEvent&);
void isReadyTutorialControllerYamlCallback(const IntWithHeader & msg);
-void fetchTutorialControllerYamlParameters(ros::NodeHandle& nodeHandle);
\ No newline at end of file
+void fetchTutorialControllerYamlParameters(ros::NodeHandle& nodeHandle);
+
+
+
+// *** SETUP THE MPC OPTIMIZATION PROBLEM WITH OSQP ***
+void setup_mpc_optimization();
+
+// *** CHANGE THE MPC OPTIMIZATION SETPOINT ***
+void change_mpc_optimization_setpoint();
+
+// *** UPDATE INITIAL CONDITION AND SOLVE OPTIMIZATION ***
+bool update_initial_condition_and_solve_mpc_optimization();
+
+// *** HELPER FUNCTIONS ***
+// Converts Eigen Dense matrix to CSC format used in OSQP
+csc* eigen2csc(const MatrixXf& eigen_dense_mat);
+
+// COMMUNICATE OPTIMIZATION SETUP STATUS TO GUI
+void send_mpc_optimization_setup_status_to_gui();
+
+// OSQP extended cleanup
+void osqp_extended_cleanup();
+
+// OSQP data cleanup
+void osqp_cleanup_data(OSQPData* data);
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/param/TutorialController.yaml b/dfall_ws/src/dfall_pkg/param/TutorialController.yaml
index 199a54ec..753c23ef 100644
--- a/dfall_ws/src/dfall_pkg/param/TutorialController.yaml
+++ b/dfall_ws/src/dfall_pkg/param/TutorialController.yaml
@@ -1,9 +1,26 @@
-# Mass of the crazyflie (32g based on measuring a crazyflie 2.1 with flowdeck and battery with kitchen scale)
+# Mass of the Crazyflie
mass : 30
-# Frequency of the controller, in hertz (should be 200Hz for in-class and 50Hz for at-home)
+# Frequency of the controller, in hertz
control_frequency : 200
-# The default setpoint, the ordering is (x,y,z,yaw),
-# with unit [meters,meters,meters,radians]
-default_setpoint : [0.0, 0.0, 0.4, 0.0]
\ No newline at end of file
+# The default setpoint, the ordering is (x, y, z),
+# with unit [meters, meters, meters]
+default_setpoint : [0.0, 0.0, 0.4]
+
+# MPC prediction horizon, in discrete time steps
+prediction_horizon: 100
+
+# MPC state cost matrix diagonal entries, the ordering is (x, y, z, xdot, ydot, zdot, roll, pitch)
+state_cost_diagonals: [10.0, 10.0, 40.0, 1.0, 1.0, 1.0, 1.0, 1.0]
+
+# MPC input cost matrix diagonal entries, the ordering is (totalThrust, rollRate, pitchRate)
+input_cost_diagonals: [1.0, 1.0, 1.0]
+
+# MPC state constraints, the ordering is (x, y, z, xdot, ydot, zdot, roll, pitch)
+min_state_constraints: [-2.0, -2.0, 0.1, -100, -100, -100, -0.5236, -0.5236]
+max_state_constraints: [2.0, 2.0, 3.0, 100, 100, 100, 0.5236, 0.5236]
+
+# MPC input constraints, the ordering is (totalThrust, rollRate, pitchRate)
+min_input_constraints: [0.1597, -1.5708, -1.5708]
+max_input_constraints: [0.4791, 1.5708, 1.5708]
\ No newline at end of file
diff --git a/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService.cpp b/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService.cpp
index a2967aa9..318f3e34 100644
--- a/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService.cpp
+++ b/dfall_ws/src/dfall_pkg/src/nodes/TutorialControllerService.cpp
@@ -36,20 +36,6 @@
// INCLUDE THE HEADER
#include "nodes/TutorialControllerService.h"
-
-// ---------------------------------------------------------------------------------------------------
-// CCCC L A SSSS SSSS V V A RRRR III A BBBB L EEEEE SSSS
-// C L A A S S V V A A R R I A A B B L E S
-// C L A A SSS SSS V V A A RRRR I A A BBBB L EEE SSS
-// C L AAAAA S S V V AAAAA R R I AAAAA B B L E S
-// CCCC LLLLL A A SSSS SSSS V A A R R III A A BBBB LLLLL EEEEE SSSS
-// ---------------------------------------------------------------------------------------------------
-
-//DECLARE YOUR CLASS VARIABLES BELOW HERE
-
-
-
-
// ----------------------------------------------------------------------------------
// FFFFF U U N N CCCC TTTTT III OOO N N
// F U U NN N C T I O O NN N
@@ -65,6 +51,447 @@
// ----------------------------------------------------------------------------------
+// *** SETUP THE MPC OPTIMIZATION PROBLEM WITH OSQP ***
+void setup_mpc_optimization()
+{
+ try
+ {
+ // *** COMPUTE THE NUMBER OF DECISION VARIABLES ***
+ int num_state_decision_variables = (yaml_prediction_horizon + 1) * const_num_states;
+ int num_input_decision_variables = yaml_prediction_horizon * const_num_inputs;
+
+ int num_decision_variables = num_state_decision_variables + num_input_decision_variables;
+
+
+
+ // *** SPECIFY THE STATE COST MATRIX Q AND THE INPUT COST MATRIX R ***
+ m_state_cost_matrix = MatrixXf::Zero(const_num_states, const_num_states);
+ for (int i = 0; i < const_num_states; i++)
+ m_state_cost_matrix(i, i) = yaml_state_cost_diagonals[i];
+
+ MatrixXf input_cost_matrix = MatrixXf::Zero(const_num_inputs, const_num_inputs);
+ for (int i = 0; i < const_num_inputs; i++)
+ input_cost_matrix(i, i) = yaml_input_cost_diagonals[i];
+
+
+
+ // *** SPECIFY THE A, B DISCRETE LTI DYNAMICS MATRICES ***
+ MatrixXf A_dynamics_matrix = MatrixXf::Identity(const_num_states, const_num_states);
+ A_dynamics_matrix.block(0, 3, 3, 3) = 1.0 / yaml_control_frequency * MatrixXf::Identity(3, 3);
+ A_dynamics_matrix(0, 7) = const_gravity / (2.0 * yaml_control_frequency * yaml_control_frequency);
+ A_dynamics_matrix(1, 6) = -const_gravity / (2.0 * yaml_control_frequency * yaml_control_frequency);
+ A_dynamics_matrix(3, 7) = const_gravity / yaml_control_frequency;
+ A_dynamics_matrix(4, 6) = -const_gravity / yaml_control_frequency;
+
+ MatrixXf B_dynamics_matrix = MatrixXf::Zero(const_num_states, const_num_inputs);
+ B_dynamics_matrix(0, 2) = const_gravity / (6.0 * yaml_control_frequency * yaml_control_frequency * yaml_control_frequency);
+ B_dynamics_matrix(1, 1) = -const_gravity / (6.0 * yaml_control_frequency * yaml_control_frequency * yaml_control_frequency);
+ B_dynamics_matrix(2, 0) = 1.0 / (2.0 * yaml_cf_mass_in_grams * yaml_control_frequency * yaml_control_frequency);
+ B_dynamics_matrix(3, 2) = const_gravity / (2.0 * yaml_control_frequency * yaml_control_frequency);
+ B_dynamics_matrix(4, 1) = -const_gravity / (2.0 * yaml_control_frequency * yaml_control_frequency);
+ B_dynamics_matrix(5, 0) = 1.0 / (yaml_cf_mass_in_grams * yaml_control_frequency);
+ B_dynamics_matrix.bottomRightCorner(2, 2) = 1.0 / yaml_control_frequency * MatrixXf::Identity(2, 2);
+
+
+
+ // *** BUILD THE OSQP QUADRATIC COST MATRIX P ***
+ // order of decision variables: (state_0, input_0, ... , state_{T-1}, input_{T-1}, state_T)
+ MatrixXf osqp_P_matrix = MatrixXf::Zero(num_decision_variables, num_decision_variables);
+
+ // add the Q and R block diagonals
+ for (int i = 0; i < yaml_prediction_horizon; i++)
+ {
+ osqp_P_matrix.block(i * const_num_states_plus_inputs, i * const_num_states_plus_inputs, const_num_states, const_num_states) = m_state_cost_matrix;
+ osqp_P_matrix.block(i * const_num_states_plus_inputs + const_num_states, i * const_num_states_plus_inputs + const_num_states, const_num_inputs, const_num_inputs) = input_cost_matrix;
+ }
+
+ // add another Q block diagonal for the terminal state constraint
+ osqp_P_matrix.bottomRightCorner(const_num_states, const_num_states) = m_state_cost_matrix;
+
+
+
+ // *** BUILD THE OSQP LINEAR COST VECTOR q ***
+ // this is where we specify the state setpoint to the optimization
+ m_osqp_q_vector = MatrixXf::Zero(num_decision_variables, 1);
+
+ // build the OSQP q vector for one time step
+ m_osqp_q_vector_for_one_time_step = -m_state_cost_matrix * m_state_setpoint_vector;
+
+ // fill in all block entries of the OSQP q vector
+ for (int i = 0; i <= yaml_prediction_horizon; i++)
+ m_osqp_q_vector.middleRows(i * const_num_states_plus_inputs, const_num_states) = m_osqp_q_vector_for_one_time_step;
+
+
+
+ // *** BUILD THE OSQP EQUALITY CONSTRAINTS MATRIX FOR THE DYNAMICS ***
+ MatrixXf osqp_A_equality_matrix = MatrixXf::Zero(num_state_decision_variables, num_decision_variables);
+
+ // fill in the identity for setting the initial state
+ osqp_A_equality_matrix.topLeftCorner(const_num_states, const_num_states) = MatrixXf::Identity(const_num_states, const_num_states);
+
+ // build the [A, B, -I] block for the dynamics constraint 0 = A * x_i + B * u_i - I * x_{i+1}
+ MatrixXf ABI_matrix = MatrixXf::Zero(const_num_states, const_num_states_plus_inputs + const_num_states);
+ ABI_matrix.leftCols(const_num_states) = A_dynamics_matrix;
+ ABI_matrix.middleCols(const_num_states, const_num_inputs) = B_dynamics_matrix;
+ ABI_matrix.rightCols(const_num_states) = -MatrixXf::Identity(const_num_states, const_num_states);
+
+ // fill in all the dynamics blocks
+ for (int i = 0; i < yaml_prediction_horizon; i++)
+ osqp_A_equality_matrix.block((i + 1) * const_num_states, i * const_num_states_plus_inputs, const_num_states, const_num_states_plus_inputs + const_num_states) = ABI_matrix;
+
+ // set the OSQP LHS and RHS inequality vectors to 0 to get equality
+ MatrixXf osqp_l_equality_vector = MatrixXf::Zero(num_state_decision_variables, 1);
+ MatrixXf osqp_u_equality_vector = MatrixXf::Zero(num_state_decision_variables, 1);
+
+
+
+ // *** BUILD THE OSQP INEQUALITY CONSTRAINTS MARTIX FOR THE STATE AND INPUT BOUNDS ***
+ MatrixXf osqp_A_inequality_matrix = MatrixXf::Identity(num_decision_variables, num_decision_variables);
+
+ // build the OSQP bound vectors l and u for one time step
+ MatrixXf osqp_l_inequality_vector_for_one_time_step = MatrixXf::Zero(const_num_states_plus_inputs, 1);
+ MatrixXf osqp_u_inequality_vector_for_one_time_step = MatrixXf::Zero(const_num_states_plus_inputs, 1);
+
+ // state constraints for one time step
+ for (int i = 0; i < const_num_states; i++)
+ {
+ osqp_l_inequality_vector_for_one_time_step(i) = yaml_min_state_constraints[i];
+ osqp_u_inequality_vector_for_one_time_step(i) = yaml_max_state_constraints[i];
+ }
+ // input constraints for one time step
+ for (int i = 0; i < const_num_inputs; i++)
+ {
+ osqp_l_inequality_vector_for_one_time_step(const_num_states + i) = yaml_min_input_constraints[i];
+ osqp_u_inequality_vector_for_one_time_step(const_num_states + i) = yaml_max_input_constraints[i];
+ }
+ // subtract feedforward equilibrium thrust from total thrust bounds
+ osqp_l_inequality_vector_for_one_time_step(const_num_states) -= m_cf_weight_in_newtons;
+ osqp_u_inequality_vector_for_one_time_step(const_num_states) -= m_cf_weight_in_newtons;
+
+ // build the OSQP bound vectors l and u for all time steps
+ MatrixXf osqp_l_inequality_vector = MatrixXf::Zero(num_decision_variables, 1);
+ MatrixXf osqp_u_inequality_vector = MatrixXf::Zero(num_decision_variables, 1);
+ for (int i = 0; i < yaml_prediction_horizon; i++)
+ {
+ osqp_l_inequality_vector.middleRows(i * const_num_states_plus_inputs, const_num_states_plus_inputs) = osqp_l_inequality_vector_for_one_time_step;
+ osqp_u_inequality_vector.middleRows(i * const_num_states_plus_inputs, const_num_states_plus_inputs) = osqp_u_inequality_vector_for_one_time_step;
+ }
+
+ // add the terminal state constraints
+ osqp_l_inequality_vector.bottomRows(const_num_states) = osqp_l_inequality_vector_for_one_time_step.topRows(const_num_states);
+ osqp_u_inequality_vector.bottomRows(const_num_states) = osqp_u_inequality_vector_for_one_time_step.topRows(const_num_states);
+
+
+
+ // *** CONCATENATE THE OSQP INEQUALITY AND EQUALITY CONSTRAINTS ***
+ MatrixXf osqp_A_matrix = MatrixXf::Zero(osqp_A_equality_matrix.rows() + osqp_A_inequality_matrix.rows(), num_decision_variables);
+ MatrixXf osqp_l_vector = MatrixXf::Zero(osqp_l_equality_vector.rows() + osqp_l_inequality_vector.rows(), 1);
+ MatrixXf osqp_u_vector = MatrixXf::Zero(osqp_u_equality_vector.rows() + osqp_u_inequality_vector.rows(), 1);
+
+ // equality constraints
+ osqp_A_matrix.topRows(osqp_A_equality_matrix.rows()) = osqp_A_equality_matrix;
+ osqp_l_vector.topRows(osqp_l_equality_vector.rows()) = osqp_l_equality_vector;
+ osqp_u_vector.topRows(osqp_u_equality_vector.rows()) = osqp_u_equality_vector;
+
+ // inequality constraints
+ osqp_A_matrix.bottomRows(osqp_A_inequality_matrix.rows()) = osqp_A_inequality_matrix;
+ osqp_l_vector.bottomRows(osqp_l_inequality_vector.rows()) = osqp_l_inequality_vector;
+ osqp_u_vector.bottomRows(osqp_u_inequality_vector.rows()) = osqp_u_inequality_vector;
+
+
+
+ // *** OSQP MODEL SETUP ***
+ // follows 'Setup and solve' example (https://osqp.org/docs/examples/setup-and-solve.html)
+
+ // first make sure any previosuly setup models are deallocated from memory
+ osqp_extended_cleanup();
+
+ // convert Eigen matrices to CSC format
+ csc* osqp_P_csc = eigen2csc(osqp_P_matrix);
+ csc* osqp_A_csc = eigen2csc(osqp_A_matrix);
+
+ // convert Eigen vectors to c_float arrays
+ // this copy will be used in the OSQPData data structure, which will be destroyed after setup is complete
+ c_float* osqp_q_cfloat = (c_float*) c_malloc(m_osqp_q_vector.rows() * sizeof(c_float));
+ c_float* osqp_l_cfloat = (c_float*) c_malloc(osqp_l_vector.rows() * sizeof(c_float));
+ c_float* osqp_u_cfloat = (c_float*) c_malloc(osqp_u_vector.rows() * sizeof(c_float));
+
+ // this copy will be used during runtime to change the initial states and the tracking setpoints
+ m_osqp_q_runtime_cfloat = (c_float*) c_malloc(m_osqp_q_vector.rows() * sizeof(c_float));
+ m_osqp_l_runtime_cfloat = (c_float*) c_malloc(osqp_l_vector.rows() * sizeof(c_float));
+ m_osqp_u_runtime_cfloat = (c_float*) c_malloc(osqp_u_vector.rows() * sizeof(c_float));
+
+ // the following syntax copies Eigen vectors to c_float arrays
+ Matrix<c_float, Dynamic, Dynamic>::Map(osqp_q_cfloat, m_osqp_q_vector.rows(), m_osqp_q_vector.cols()) = m_osqp_q_vector.cast<c_float>();
+ Matrix<c_float, Dynamic, Dynamic>::Map(m_osqp_q_runtime_cfloat, m_osqp_q_vector.rows(), m_osqp_q_vector.cols()) = m_osqp_q_vector.cast<c_float>();
+
+ Matrix<c_float, Dynamic, Dynamic>::Map(osqp_l_cfloat, osqp_l_vector.rows(), osqp_l_vector.cols()) = osqp_l_vector.cast<c_float>();
+ Matrix<c_float, Dynamic, Dynamic>::Map(m_osqp_l_runtime_cfloat, osqp_l_vector.rows(), osqp_l_vector.cols()) = osqp_l_vector.cast<c_float>();
+
+ Matrix<c_float, Dynamic, Dynamic>::Map(osqp_u_cfloat, osqp_u_vector.rows(), osqp_u_vector.cols()) = osqp_u_vector.cast<c_float>();
+ Matrix<c_float, Dynamic, Dynamic>::Map(m_osqp_u_runtime_cfloat, osqp_u_vector.rows(), osqp_u_vector.cols()) = osqp_u_vector.cast<c_float>();
+
+ // populate OSQP model data
+ OSQPData* osqp_data = (OSQPData*) c_malloc(sizeof(OSQPData));
+ osqp_data->n = osqp_A_matrix.cols();
+ osqp_data->m = osqp_A_matrix.rows();
+ osqp_data->P = osqp_P_csc;
+ osqp_data->q = osqp_q_cfloat;
+ osqp_data->A = osqp_A_csc;
+ osqp_data->l = osqp_l_cfloat;
+ osqp_data->u = osqp_u_cfloat;
+
+ // OSQP solver settings: define as default, then change settings as desired
+ OSQPSettings* osqp_settings = (OSQPSettings*) c_malloc(sizeof(OSQPSettings));
+ osqp_set_default_settings(osqp_settings);
+ // d_osqp_settings->verbose = true;
+
+ // setup OSQP workspace
+ m_osqp_work = osqp_setup(osqp_data, osqp_settings);
+
+ // clear data and settings after setting up to allow subseqeuent setups
+ osqp_cleanup_data(osqp_data);
+ c_free(osqp_settings);
+
+ // set the MPC optimization setup success flag
+ if (!m_osqp_work)
+ {
+ m_mpc_optimization_setup_success = false;
+
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization setup with OSQP failed");
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization must be (re-)setup");
+
+ // communicate optimization setup status to GUI
+ send_mpc_optimization_setup_status_to_gui();
+
+ return;
+ }
+
+ // setup successful flag
+ m_mpc_optimization_setup_success = true;
+
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization setup with OSQP successful");
+
+ // communicate optimization setup status to GUI
+ send_mpc_optimization_setup_status_to_gui();
+ }
+
+ catch(std::exception& e)
+ {
+ m_mpc_optimization_setup_success = false;
+
+ ROS_INFO_STREAM("[TUTORIAL CONTROLLER] MPC optimization setup with OSQP exception, error message: " << e.what());
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization must be (re-)setup");
+
+ // communicate optimization setup status to GUI
+ send_mpc_optimization_setup_status_to_gui();
+ }
+ catch(...)
+ {
+ m_mpc_optimization_setup_success = false;
+
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization setup with OSQP unknown exception");
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization must be (re-)setup");
+
+ // communicate optimization setup status to GUI
+ send_mpc_optimization_setup_status_to_gui();
+ }
+}
+
+
+
+// *** CHANGE THE MPC OPTIMIZATION SETPOINT ***
+void change_mpc_optimization_setpoint()
+{
+ try
+ {
+ // *** UPDATE THE OSQP LINEAR COST VECTOR q ***
+ // update the OSQP q vector for one time step
+ m_osqp_q_vector_for_one_time_step = -m_state_cost_matrix * m_state_setpoint_vector;
+
+ // fill in the block entries of the OSQP q vector
+ for (int i = 0; i <= yaml_prediction_horizon; i++)
+ m_osqp_q_vector.middleRows(i * const_num_states_plus_inputs, const_num_states) = m_osqp_q_vector_for_one_time_step;
+
+ // convert Eigen vector to c_float array
+ Matrix<c_float, Dynamic, Dynamic>::Map(m_osqp_q_runtime_cfloat, m_osqp_q_vector.rows(), m_osqp_q_vector.cols()) = m_osqp_q_vector.cast<c_float>();
+
+ // update OSQP linear cost
+ osqp_update_lin_cost(m_osqp_work, m_osqp_q_runtime_cfloat);
+
+
+
+ // *** INFORM THE USER ***
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization setpoint updated with OSQP successfully");
+ }
+
+ catch(std::exception& e)
+ {
+ m_mpc_optimization_setup_success = false;
+
+ ROS_INFO_STREAM("[TUTORIAL CONTROLLER] MPC optimization setpoint update with OSQP exception, error message: " << e.what());
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization must be (re-)setup");
+
+ // communicate optimization setup status to GUI
+ send_mpc_optimization_setup_status_to_gui();
+ }
+ catch(...)
+ {
+ m_mpc_optimization_setup_success = false;
+
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization setpoint update with OSQP unknown exception");
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization must be (re-)setup");
+
+ // communicate optimization setup status to GUI
+ send_mpc_optimization_setup_status_to_gui();
+ }
+}
+
+// *** UPDATE INITIAL CONDITION AND SOLVE OPTIMIZATION ***
+bool update_initial_condition_and_solve_mpc_optimization()
+{
+ try
+ {
+ // *** UPDATE THE EQUALITY CONSTRAINT VECTORS FOR THE INITIAL STATE ***
+ // the initial state equality constraint is the first constraint defined
+ for (int i = 0; i < const_num_states; i++)
+ {
+ m_osqp_l_runtime_cfloat[i] = m_current_state_vector(i);
+ m_osqp_u_runtime_cfloat[i] = m_current_state_vector(i);
+ }
+
+ // update the OSQP constraint bounds
+ osqp_update_bounds(m_osqp_work, m_osqp_l_runtime_cfloat, m_osqp_u_runtime_cfloat);
+
+
+
+ // *** SOLVE OPTIMIZATION ***
+ osqp_solve(m_osqp_work);
+
+
+
+ // *** GET SOLUTION IF SUCCESSFULLY SOLVED ***
+ // status_val > 0 means that an optimal solution was found
+ // refer to OSQP "constants.h" for the detailed status codes
+ if (m_osqp_work->info->status_val > 0)
+ {
+ // get the MPC input to apply, which is the optimal input for the first time step
+ for (int i = 0; i < const_num_inputs; i++)
+ m_mpc_input_vector_to_apply(i) = m_osqp_work->solution->x[const_num_states + i];
+
+
+ // inform the user
+ ROS_INFO_STREAM("[TUTORIAL CONTROLLER] MPC optimal solution found with OSQP, status: " << m_osqp_work->info->status);
+ ROS_INFO_STREAM("Thrust: " << m_mpc_input_vector_to_apply(0));
+ ROS_INFO_STREAM("Roll Rate: " << m_mpc_input_vector_to_apply(1));
+ ROS_INFO_STREAM("Pitch Rate: " << m_mpc_input_vector_to_apply(2));
+ ROS_INFO_STREAM("Objective: " << m_osqp_work->info->obj_val);
+ ROS_INFO_STREAM("Runtime: " << m_osqp_work->info->run_time);
+
+ return true;
+ }
+
+
+ // we arrive here if optimal solution was not found
+ ROS_INFO_STREAM("[TUTORIAL CONTROLLER] MPC failed to find optimal solution with OSQP, status: " << m_osqp_work->info->status);
+
+ return false;
+ }
+
+ catch(std::exception& e)
+ {
+ m_mpc_optimization_setup_success = false;
+
+ ROS_INFO_STREAM("[TUTORIAL CONTROLLER] MPC optimization solution with OSQP exception, error message: " << e.what());
+ ROS_INFO("[TUTORIAL CONTROLLER] MPC optimization must be (re-)setup");
+
+ // communicate optimization setup status to GUI
+ send_mpc_optimization_setup_status_to_gui();
+ }
+ catch(...)
+ {
+ m_mpc_optimization_setup_success = false;
+
+ ROS_INFO("[MPC CONTROLLER] MPC optimization solution with OSQP unknown exception");
+ ROS_INFO("[MPC CONTROLLER] MPC optimization must be (re-)setup");
+
+ // communicate optimization setup status to GUI
+ send_mpc_optimization_setup_status_to_gui();
+ }
+}
+
+
+// *** HELPER FUNCTIONS ***
+
+// Convert Eigen Dense matrix to CSC format used in OSQP
+csc* eigen2csc(const MatrixXf& eigen_dense_mat)
+{
+ // First convert Eigen Dense matrix to Eigen Sparse
+ SparseMatrix<float> eigen_sparse_mat = eigen_dense_mat.sparseView();
+
+ // Second convert Eigen Sparse matrix to TRIPLET format, because it is not as mind bending as CSC format
+ csc* trip_mat = csc_spalloc(eigen_sparse_mat.rows(), eigen_sparse_mat.cols(), eigen_sparse_mat.nonZeros(), 1, 1);
+ trip_mat->nz = eigen_sparse_mat.nonZeros();
+ // The following code was found under 'Iterating over the nonzero coefficients' section here: https://eigen.tuxfamily.org/dox/group__TutorialSparse.html
+ int i = 0;
+ for (int j = 0; j < eigen_sparse_mat.outerSize(); j++)
+ for (SparseMatrix<float>::InnerIterator it(eigen_sparse_mat, j); it; ++it)
+ {
+ trip_mat->x[i] = it.value(); // Value
+ trip_mat->i[i] = it.row(); // Row index
+ trip_mat->p[i] = it.col(); // Column index
+
+ i++;
+ }
+
+ // Third convert TRIPLET matrix to CSC format, using OSQP built in function
+ csc* csc_mat = triplet_to_csc(trip_mat, OSQP_NULL);
+
+ // triplet_to_csc makes new copy of matrix, so intermediate TRIPLET matrix must be de-allocated
+ csc_spfree(trip_mat);
+
+ return csc_mat;
+}
+
+
+// COMMUNICATE OPTIMIZATION SETUP STATUS TO GUI
+void send_mpc_optimization_setup_status_to_gui()
+{
+ // Use x field of Setpoint msg data structure because too lazy to define new message type
+ SetpointWithHeader msg;
+ msg.x = 1.0 * m_mpc_optimization_setup_success;
+
+ // Publish the message
+ m_optimizationSetupStatusChangedPublisher.publish(msg);
+}
+
+
+// OSQP EXTENDED CLEANUP
+// frees any data structures to which memory was allocated
+void osqp_extended_cleanup()
+{
+ osqp_cleanup(m_osqp_work);
+ c_free(m_osqp_q_runtime_cfloat);
+ c_free(m_osqp_l_runtime_cfloat);
+ c_free(m_osqp_u_runtime_cfloat);
+}
+
+
+// OSQP DATA CLEANUP
+// frees all the memory allocated to the OSQPData data structure
+void osqp_cleanup_data(OSQPData* data)
+{
+ if (!data)
+ return;
+
+ csc_spfree(data->P);
+ csc_spfree(data->A);
+ c_free(data->q);
+ c_free(data->l);
+ c_free(data->u);
+
+ c_free(data);
+}
@@ -193,7 +620,6 @@
//
//
//
-// This function WILL NEED TO BE edited for successful completion of the classroom exercise
bool calculateControlOutput(Controller::Request &request, Controller::Response &response)
{
@@ -210,6 +636,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
stateErrorInertial[1] = request.ownCrazyflie.y - m_setpoint[1];
stateErrorInertial[2] = request.ownCrazyflie.z - m_setpoint[2];
+
+
// Compute an estimate of the velocity
// > Via finite differences if receiveing
// Motion Capture data
@@ -245,10 +673,12 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
{
ROS_ERROR_STREAM("[TUTORIAL CONTROLLER] Received a request.ownCrazyflie with unrecognised type, request.ownCrazyflie.type = " << request.ownCrazyflie.type );
}
+
// Fill in the roll and pitch angle measurements directly
stateErrorInertial[6] = request.ownCrazyflie.roll;
stateErrorInertial[7] = request.ownCrazyflie.pitch;
+
// Fill in the yaw angle error
// > This error should be "unwrapped" to be in the range
@@ -256,12 +686,26 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// > First, get the yaw error into a local variable
float yawError = request.ownCrazyflie.yaw - m_setpoint[3];
// > Second, "unwrap" the yaw error to the interval ( -pi , pi )
- while(yawError > PI) {yawError -= 2 * PI;}
- while(yawError < -PI) {yawError += 2 * PI;}
+ while (yawError > PI) {yawError -= 2 * PI;}
+ while (yawError < -PI) {yawError += 2 * PI;}
// > Third, put the "yawError" into the "stateError" variable
stateErrorInertial[8] = yawError;
+
+ // FILL IN THE STATE FOR THE MPC OPTIMIZATION
+ // note that MPC controller does not control yaw
+ m_current_state_vector(0) = request.ownCrazyflie.x;
+ m_current_state_vector(1) = request.ownCrazyflie.y;
+ m_current_state_vector(2) = request.ownCrazyflie.z;
+ m_current_state_vector(3) = stateErrorInertial[3];
+ m_current_state_vector(4) = stateErrorInertial[4];
+ m_current_state_vector(5) = stateErrorInertial[5];
+ m_current_state_vector(6) = request.ownCrazyflie.roll;
+ m_current_state_vector(7) = request.ownCrazyflie.pitch;
+
+
+
// CONVERSION INTO BODY FRAME
// Conver the state erorr from the Inertial frame into the Body frame
// > Note: the function "convertIntoBodyFrame" is implemented in this file
@@ -274,139 +718,106 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// SAVE THE STATE ERROR TO BE USED NEXT TIME THIS FUNCTION IS CALLED
// > as we have already used previous error we can now update it update it
- for(int i = 0; i < 9; ++i)
- {
+ for (int i = 0; i < 9; i++)
m_previous_stateErrorInertial[i] = stateErrorInertial[i];
- }
+ // INITIALIZE CONTROLLER COMMANDS
+ float totalThrust_command;
+ float rollRate_command;
+ float pitchRate_command;
+ float yawRate_command;
+
+ // YAW CONTROL
+ // controlled by backup LQR controller and not MPC
+ yawRate_command = 0.0;
+ for (int i = 0; i < 9; i++)
+ yawRate_command -= m_gainMatrixYawRate[i] * stateErrorBody[i];
- // **********************
- // Y Y A W W
- // Y Y A A W W
- // Y A A W W
- // Y AAAAA W W W
- // Y A A W W
- //
- // YAW CONTROLLER
-
- // Instantiate the local variable for the yaw rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float yawRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the yaw rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- yawRate_forResponse -= m_gainMatrixYawRate[i] * stateErrorBody[i];
- }
-
- // Put the computed yaw rate into the "response" variable
- // > The "controller mode" specifies that it is an
- // angular rate setpoint
- response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
- response.controlOutput.yawControllerSetpoint = yawRate_forResponse;
-
-
+
- // **************************************
- // BBBB OOO DDDD Y Y ZZZZZ
- // B B O O D D Y Y Z
- // BBBB O O D D Y Z
- // B B O O D D Y Z
- // BBBB OOO DDDD Y ZZZZZ
- //
- // ALITUDE CONTROLLER (i.e., z-controller)
+ // IF MPC OPTIMIZATION IS SETUP SUCCESSFULLY, ATTEMPT TO USE MPC CONTROL
+ bool use_mpc_controller = false;
+ if (m_mpc_optimization_setup_success)
+ use_mpc_controller = update_initial_condition_and_solve_mpc_optimization();
- // Instantiate the local variable for the thrust adjustment that will be
- // requested from the Crazyflie's on-baord "inner-loop" controller
- float thrustAdjustment = 0;
+ // if MPC optimization successfully found a solution, use solution
+ if (use_mpc_controller)
+ {
+ totalThrust_command = m_mpc_input_vector_to_apply(0) + m_cf_weight_in_newtons;
+ rollRate_command = m_mpc_input_vector_to_apply(1);
+ pitchRate_command = m_mpc_input_vector_to_apply(2);
+ }
- // Perform the "-Kx" LQR computation for the thrust adjustment to respoond with
- for(int i = 0; i < 9; ++i)
+ // else, use backup controller
+ else
{
- thrustAdjustment -= m_gainMatrixThrust[i] * stateErrorBody[i];
+ // BODY Z CONTROL
+ // backup LQR controller
+ totalThrust_command = 0.0;
+ for (int i = 0; i < 9; i++)
+ totalThrust_command -= m_gainMatrixThrust[i] * stateErrorBody[i];
+
+ // add feed-forward term
+ totalThrust_command += m_cf_weight_in_newtons;
+
+
+ // BODY X CONTROL
+ // backup LQR controller
+ pitchRate_command = 0.0;
+ for(int i = 0; i < 9; i++)
+ pitchRate_command -= m_gainMatrixPitchRate[i] * stateErrorBody[i];
+
+ // BODY Y CONTROL
+ // backup LQR controller
+ rollRate_command = 0.0;
+ for(int i = 0; i < 9; i++)
+ rollRate_command -= m_gainMatrixRollRate[i] * stateErrorBody[i];
}
+
+
- // Put the computed thrust adjustment into the "response" variable.
- // > On top of the thrust adjustment, we must add the feed-forward thrust
- // to counter-act gravity.
- // > NOTE: remember that the thrust is commanded per motor, so you sohuld
- // consider whether the "thrustAdjustment" computed by your
- // controller needed to be divided by 4 or not.
- // > NOTE: the "m_cf_weight_in_newtons" value is the total thrust required
- // as feed-forward. Assuming the the Crazyflie is symmetric, this
- // value is divided by four.
- float feed_forward_thrust_per_motor = m_cf_weight_in_newtons / 4.0;
+ // *** SET THE REPONSE VARIABLE WITH THE INPUT COMMANDS ***
+
+ // THRUST COMMANDS FOR Z
// > NOTE: the function "newtons2cmd_for_crazyflie" converts the input argument
// from Newtons to the 16-bit command expected by the Crazyflie.
- response.controlOutput.motorCmd1 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd2 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd3 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
- response.controlOutput.motorCmd4 = newtons2cmd_for_crazyflie(thrustAdjustment + feed_forward_thrust_per_motor);
-
+ float perMotorThrust_command = totalThrust_command / 4.0;
+ response.controlOutput.motorCmd1 = newtons2cmd_for_crazyflie(perMotorThrust_command);
+ response.controlOutput.motorCmd2 = newtons2cmd_for_crazyflie(perMotorThrust_command);
+ response.controlOutput.motorCmd3 = newtons2cmd_for_crazyflie(perMotorThrust_command);
+ response.controlOutput.motorCmd4 = newtons2cmd_for_crazyflie(perMotorThrust_command);
+
// Set the onboard z-controller to be OFF
// > This is because we commands the motor thrusts and
// hence an "inner" control loop is NOT needed onboard
// to control the z-height
response.controlOutput.zControllerMode = CF_ONBOARD_CONTROLLER_MODE_OFF;
-
-
- // **************************************
- // BBBB OOO DDDD Y Y X X
- // B B O O D D Y Y X X
- // BBBB O O D D Y X
- // B B O O D D Y X X
- // BBBB OOO DDDD Y X X
- //
- // BODY FRAME X CONTROLLER
-
- // Instantiate the local variable for the pitch rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float pitchRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the pitch rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- pitchRate_forResponse -= m_gainMatrixPitchRate[i] * stateErrorBody[i];
- }
+ // PITCH RATE COMMAND FOR X
// Put the computed pitch rate into the "response" variable
// > The "controller mode" specifies that it is an
// angular rate setpoint
response.controlOutput.xControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
- response.controlOutput.xControllerSetpoint = pitchRate_forResponse;
-
-
-
-
- // **************************************
- // BBBB OOO DDDD Y Y Y Y
- // B B O O D D Y Y Y Y
- // BBBB O O D D Y Y
- // B B O O D D Y Y
- // BBBB OOO DDDD Y Y
- //
- // BODY FRAME Y CONTROLLER
-
- // Instantiate the local variable for the roll rate that will be requested
- // from the Crazyflie's on-baord "inner-loop" controller
- float rollRate_forResponse = 0;
-
- // Perform the "-Kx" LQR computation for the roll rate to respoond with
- for(int i = 0; i < 9; ++i)
- {
- rollRate_forResponse -= m_gainMatrixRollRate[i] * stateErrorBody[i];
- }
+ response.controlOutput.xControllerSetpoint = pitchRate_command;
+
+ // ROLL RATE COMMAND FOR Y
// Put the computed roll rate into the "response" variable
// > The "controller mode" specifies that it is an
// angular rate setpoint
response.controlOutput.yControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
- response.controlOutput.yControllerSetpoint = rollRate_forResponse;
+ response.controlOutput.yControllerSetpoint = rollRate_command;
+ // YAW RATE COMMAND FOR YAW
+ // Put the computed yaw rate into the "response" variable
+ // > The "controller mode" specifies that it is an
+ // angular rate setpoint
+ response.controlOutput.yawControllerMode = CF_ONBOARD_CONTROLLER_MODE_ANGULAR_RATE;
+ response.controlOutput.yawControllerSetpoint = yawRate_command;
@@ -441,10 +852,10 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
// The "DebugMsg" type has 10 properties from "value_1" to "value_10", all of
// type "float64" that you can fill in with data you would like to plot in
// real-time.
- debugMsg.value_1 = thrustAdjustment;
- debugMsg.value_2 = rollRate_forResponse;
- debugMsg.value_3 = pitchRate_forResponse;
- debugMsg.value_4 = yawRate_forResponse;
+ debugMsg.value_1 = totalThrust_command - m_cf_weight_in_newtons;
+ debugMsg.value_2 = rollRate_command;
+ debugMsg.value_3 = pitchRate_command;
+ debugMsg.value_4 = yawRate_command;
// ......................
// debugMsg.value_10 = your_variable_name;
@@ -578,7 +989,7 @@ void requestSetpointChangeCallback(const SetpointWithHeader& newSetpoint)
yaml_default_setpoint[0],
yaml_default_setpoint[1],
yaml_default_setpoint[2],
- yaml_default_setpoint[3]
+ 0.0
);
}
else
@@ -605,6 +1016,16 @@ void setNewSetpoint(float x, float y, float z, float yaw)
m_setpoint[2] = z;
m_setpoint[3] = yaw;
+ // Put the new setpoint in the state setpoint
+ m_state_setpoint_vector(0) = x;
+ m_state_setpoint_vector(1) = y;
+ m_state_setpoint_vector(2) = z;
+
+ // Update MPC optimization setpoint
+ if (m_mpc_optimization_setup_success)
+ change_mpc_optimization_setpoint();
+
+
// Publish the change so that the network is updated
// (mainly the "flying agent GUI" is interested in
// displaying this change to the user)
@@ -671,32 +1092,13 @@ void customCommandReceivedCallback(const CustomButtonWithHeader& commandReceived
switch(custom_button_index)
{
- // > FOR CUSTOM BUTTON 1
+ // > FOR CUSTOM BUTTON 1 - SETUP OPTIMIZATION
case 1:
{
// Let the user know that this part of the code was triggered
ROS_INFO_STREAM("[TUTORIAL CONTROLLER] Button 1 received in controller, with message.float_data = " << float_data );
// Code here to respond to custom button 1
-
- break;
- }
-
- // > FOR CUSTOM BUTTON 2
- case 2:
- {
- // Let the user know that this part of the code was triggered
- ROS_INFO_STREAM("[TUTORIAL CONTROLLER] Button 2 received in controller, with message.float_data = " << float_data );
- // Code here to respond to custom button 2
-
- break;
- }
-
- // > FOR CUSTOM BUTTON 3
- case 3:
- {
- // Let the user know that this part of the code was triggered
- ROS_INFO_STREAM("[TUTORIAL CONTROLLER] Button 3 received in controller, with message.float_data = " << float_data );
- // Code here to respond to custom button 3
+ setup_mpc_optimization();
break;
}
@@ -824,21 +1226,33 @@ void fetchTutorialControllerYamlParameters(ros::NodeHandle& nodeHandle)
// GET THE PARAMETERS:
- // > The mass of the crazyflie
+ // The mass of the crazyflie
yaml_cf_mass_in_grams = getParameterFloat(nodeHandle_for_paramaters , "mass");
- // Display one of the YAML parameters to debug if it is working correctly
- //ROS_INFO_STREAM("DEBUGGING: mass leaded from loacl file = " << yaml_cf_mass_in_grams );
-
- // > The frequency at which the "computeControlOutput" is being called, as determined
- // by the frequency at which the Vicon system provides position and attitude data
+ // The frequency at which the "computeControlOutput" is being called, as determined
+ // by the frequency at which the Vicon system provides position and attitude data
yaml_control_frequency = getParameterFloat(nodeHandle_for_paramaters, "control_frequency");
- // The default setpoint, the ordering is (x,y,z,yaw),
- // with unit [meters,meters,meters,radians]
- getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 4);
+ // The default setpoint, the ordering is (x, y, z, yaw),
+ // with unit [meters, meters, meters, radians]
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "default_setpoint", yaml_default_setpoint, 3);
+ // The MPC prediction horizon, in discrete time steps
+ yaml_prediction_horizon = getParameterInt(nodeHandle_for_paramaters , "prediction_horizon");
+ // The MPC state cost matrix diagonal entries, the ordering is (x, y, z, xdot, ydot, zdot, roll, pitch, yaw)
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "state_cost_diagonals", yaml_state_cost_diagonals, const_num_states);
+
+ // The MPC input cost matrix diagonal entries, the ordering is (totalThrust, rollRate, pitchRate, yawRate)
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "input_cost_diagonals", yaml_input_cost_diagonals, const_num_inputs);
+
+ // The MPC state constraints, the ordering is (x, y, z, xdot, ydot, zdot, roll, pitch, yaw)
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "min_state_constraints", yaml_min_state_constraints, const_num_states);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "max_state_constraints", yaml_max_state_constraints, const_num_states);
+
+ // The MPC input constraints, the ordering is (totalThrust, rollRate, pitchRate, yawRate)
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "min_input_constraints", yaml_min_input_constraints, const_num_inputs);
+ getParameterFloatVectorKnownLength(nodeHandle_for_paramaters, "max_input_constraints", yaml_max_input_constraints, const_num_inputs);
// > DEBUGGING: Print out one of the parameters that was loaded to
// debug if the fetching of parameters worked correctly
@@ -850,7 +1264,7 @@ void fetchTutorialControllerYamlParameters(ros::NodeHandle& nodeHandle)
// > Compute the feed-forward force that we need to counteract
// gravity (i.e., mg) in units of [Newtons]
- m_cf_weight_in_newtons = yaml_cf_mass_in_grams * 9.81/1000.0;
+ m_cf_weight_in_newtons = yaml_cf_mass_in_grams * const_gravity / 1000.0;
// DEBUGGING: Print out one of the computed quantities
ROS_INFO_STREAM("[TUTORIAL CONTROLLER] DEBUGGING: thus the weight of this agent in [Newtons] = " << m_cf_weight_in_newtons);
@@ -871,6 +1285,9 @@ void fetchTutorialControllerYamlParameters(ros::NodeHandle& nodeHandle)
// This function does NOT need to be edited
int main(int argc, char* argv[]) {
+ // Initialize variables that cannot be initialized in header file
+ m_state_setpoint_vector(2) = 0.4;
+
// Starting the ROS-node
ros::init(argc, argv, "TutorialControllerService");
@@ -957,22 +1374,6 @@ int main(int argc, char* argv[]) {
- // GIVE YAML VARIABLES AN INITIAL VALUE
- // This can be done either here or as part of declaring the
- // variables in the header file
-
- // > the mass of the crazyflie, in [grams]
- yaml_cf_mass_in_grams = 25.0;
- // > the frequency at which the controller is running
- yaml_control_frequency = 200.0;
- // > the default setpoint, the ordering is (x,y,z,yaw),
- // with units [meters,meters,meters,radians]
- yaml_default_setpoint[0] = 0.0;
- yaml_default_setpoint[1] = 0.0;
- yaml_default_setpoint[2] = 0.4;
- yaml_default_setpoint[3] = 0.0;
-
-
// FETCH ANY PARAMETERS REQUIRED FROM THE "PARAMETER SERVICES"
// The yaml files for the controllers are not added to
@@ -1030,6 +1431,14 @@ int main(int argc, char* argv[]) {
// field that displays the current setpoint for this controller.
m_setpointChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("SetpointChanged", 1);
+ // Instantiate the class variable "m_optimizationSetupStatusChangedPublisher" to
+ // be a "ros::Publisher". This variable advertises under the name
+ // "OptimizationSetupStatusChanged" and is a message with the structure defined
+ // in the file "SetpointWithHeader.msg" (located in the "msg" folder).
+ // This publisher is used by the "flying agent GUI" to update the
+ // optimization setup status indicators.
+ m_optimizationSetupStatusChangedPublisher = nodeHandle.advertise<SetpointWithHeader>("OptimizationSetupStatusChanged", 1);
+
// Instantiate the local variable "getCurrentSetpointService" to be
// a "ros::ServiceServer" type variable that advertises the service
// called "GetCurrentSetpoint". This service has the input-output
--
GitLab