Commit 3d8a86ef authored by Tobias's avatar Tobias
Browse files

introduced centralized controller as Controller state 5

parent 14ed5e1a
......@@ -171,6 +171,7 @@ private slots:
void on_stateButton_2_clicked();
void on_stateButton_3_clicked();
void on_stateButton_4_clicked();
void on_stateButton_5_clicked();
void on_set_setpoint_button_distr_clicked();
......
......@@ -1396,7 +1396,15 @@ void MainWindow::on_stateButton_4_clicked()
ROS_INFO("Controller State button 4 pressed in GUI");
}
void MainWindow::on_stateButton_5_clicked()
{
CustomButton msg_custom_button;
msg_custom_button.button_index = 5;
msg_custom_button.command_code = 0;//(ui->studentField_3->text()).toFloat();
this->distributedStateButtonPublisher.publish(msg_custom_button);
ROS_INFO("Controller State button 5 pressed in GUI");
}
Setpoint MainWindow::correctSetpointBox(Setpoint setpoint, CrazyflieContext context)
{
......
......@@ -2382,6 +2382,19 @@
</property>
</widget>
</item>
<item row="15" column="6">
<widget class="QPushButton" name="stateButton_5">
<property name="sizePolicy">
<sizepolicy hsizetype="MinimumExpanding" vsizetype="MinimumExpanding">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string>LQG, no Ovlp</string>
</property>
</widget>
</item>
</layout>
</item>
</layout>
......
......@@ -200,25 +200,40 @@ std::vector<float> gainMatrixThrust = { 0.00, 0.00, 0.19, 0.00, 0.00, 0.08
// Set point for the load, used for the distributed controller
std::vector<float> load_setpoint = {0.0 , 0.0 , 0.0 , 0.0}; // The setpoints for (x,y,z) position in that order
// augmented equilibirium state vector of the copter-load system
Eigen::Matrix<float,57,1> augmented_state_eq;
// initialize estimated state
Eigen::Matrix<float,57,1> overlapping_estimator_state_prev;
// initialize measurement vector
Eigen::Matrix<float,24,1> y_to_controller;
// output matrix C
Eigen::Matrix<float,24,57> outputMatrix;
Eigen::Matrix<float,24,33> outputMatrix_centralized;
// augmented equilibirium state vector of the copter-load system
Eigen::Matrix<float,33,1> state_eq;
Eigen::Matrix<float,33,1> estimator_state_prev;
// augmented equilibirium state vector of the copter-load system
Eigen::Matrix<float,57,1> augmented_state_eq;
// initialize estimated state
Eigen::Matrix<float,57,1> overlapping_estimator_state_prev;
// gains
struct overlapping_controller_struct
{
Eigen::Matrix<float,57,24> Gamma;
Eigen::Matrix<float,57,57> Phi;
Eigen::Matrix<float,12,57> K;
Eigen::Matrix<float,12,57> K;
};
overlapping_controller_struct overlappingController;
// centralized controller
struct centralized_controller_struct
{
Eigen::Matrix<float,33,24> Gamma;
Eigen::Matrix<float,33,33> Phi;
Eigen::Matrix<float,12,33> K;
};
centralized_controller_struct centralized_controller;
// ROS Publisher for debugging variables
ros::Publisher debugPublisher;
......@@ -319,4 +334,5 @@ void get_y_from_CFData( Controller::Request &request ) ;
Eigen::MatrixXf readMatrix(std::string filename);
std::string getcwd_string( void );
void distributed_control_command( Controller::Request &request, Controller::Response &response );
void centralized_control_command( Controller::Request &request, Controller::Response &response);
std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw );
\ No newline at end of file
......@@ -12,11 +12,11 @@ motorPoly : [5.484560e-4, 1.032633e-6, 2.130295e-11]
takeOffinitial : 0.05
takeOffDistance : 0.6
landingDistance : 0.1
durationTakeOff : 3.0
durationLanding : 3.0
durationTakeOff : 2.0
durationLanding : 2.0
takeOffDistance_load : 0.1
landingDistance_load : 0.1
durationLanding_load : 5.0
durationLanding_load : 2.0
# equilateral triangle for copter formation
triangle_sidelength : 0.4
......
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0.23 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 -0.23 0 0 0 0
0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 -0.19919 0 0 0
0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 -0.115 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0.19919 0.115 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 1 0 0 0 0 0 0 0 0.19919 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 1 0 0 0 0 0 0 -0.115 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 -0.19919 0.115 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0
\ No newline at end of file
0.61803 0 0 0 -0.00026152 0 0 -0.00026541 0 0 0 -1.4303e-06 0 0 0 0 0 0 0 1.4303e-06 0 0 0 0
0 0.61803 0 0.00026251 0 0 0.00026503 0 0 0 0 0 0 0 2.5701e-06 0 0 0 0 0 0 0 2.5701e-06 0
0 0 0.61803 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0.0014523 0 0.040397 0 0 0.050739 0 0 5.1747e-06 0 -0.011267 0 0 -0.00092505 0 0 5.1747e-06 0 -0.011267 0 0 -0.00092505 0
0.001453 0 0 0 -0.0031643 0 0 -0.086621 -4.7147e-06 -9.6448e-06 0 -0.011206 -0.0023108 0 0.033772 -0.0078635 -4.7147e-06 9.6448e-06 0 0.011206 -0.0023108 0 -0.033772 -0.0078635
0 0.2416 0 0.091635 0 0 0.10118 0 0 -3.2128e-05 0 -0.035207 0 0 -0.025659 0 0 -3.2128e-05 0 -0.035207 0 0 -0.025659 0
0.24162 0 0 0 -0.031001 0 0 -0.24791 -7.4329e-05 -4.1973e-06 0 0.0046133 0.015045 0 0.13445 0.023017 -7.4329e-05 4.1973e-06 0 -0.0046133 0.015045 0 -0.13445 0.023017
0 0 0 0 -2.7686e-06 0 0 2.2642e-06 0.61803 0 0 0 -0.00026186 0 -4.4423e-06 -0.00026557 0 0 0 0 0 0 4.4423e-06 -3.1122e-06
0 0 0 0 -3.9213e-06 0 2.5701e-06 4.6343e-06 0 0.61803 0 0.00026254 1.4562e-06 0 0.00026071 2.4861e-06 0 0 0 0 1.4562e-06 0 6.8891e-06 2.4861e-06
0 0 0 0 0 0 0 0 0 0 0.61803 0 0 0 0 0 0 0 0 0 0 0 0 0
1.0584e-06 5.1747e-06 0 -0.011267 0.036544 0 -0.00092505 -0.03448 -9.2343e-06 0.0014431 0 0.029215 -0.012945 0 0.083521 -0.0078212 -9.2343e-06 1.4339e-05 0 -8.4817e-05 -0.012945 0 -0.033707 -0.0078212
0 0 0 0 0.010726 0 0 -0.0090262 0.0014532 -5.3069e-06 0 0.0035465 -0.039029 0 0.0087848 -0.049708 6.1066e-06 5.3069e-06 0 -0.0035465 0.012635 0 -0.0087848 0.0019559
3.7701e-05 -3.2128e-05 0 -0.035207 0.11973 0 -0.025659 -0.21114 -5.9967e-05 0.24158 0 0.082595 -0.036298 0 0.29752 -0.011442 -5.9967e-05 -1.8253e-05 0 -0.026167 -0.036298 0 -0.222 -0.011442
-6.8931e-06 0 0 0 0.070208 0 0 -0.10009 0.24156 0 0 0.01621 -0.09931 0 0.11375 -0.10067 -6.7975e-05 0 0 -0.01621 0.027531 0 -0.11375 0.026175
0 0 0 0 -2.7686e-06 0 0 2.2642e-06 0 0 0 0 0 0 -4.4423e-06 -3.1122e-06 0.61803 0 0 0 -0.00026186 0 4.4423e-06 -0.00026557
0 0 0 0 3.9213e-06 0 2.5701e-06 -4.6343e-06 0 0 0 0 -1.4562e-06 0 6.8891e-06 -2.4861e-06 0 0.61803 0 0.00026254 -1.4562e-06 0 0.00026071 -2.4861e-06
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.61803 0 0 0 0 0
-1.0584e-06 5.1747e-06 0 -0.011267 -0.036544 0 -0.00092505 0.03448 9.2343e-06 1.4339e-05 0 -8.4817e-05 0.012945 0 -0.033707 0.0078212 9.2343e-06 0.0014431 0 0.029215 0.012945 0 0.083521 0.0078212
0 0 0 0 0.010726 0 0 -0.0090262 6.1066e-06 -5.3069e-06 0 0.0035465 0.012635 0 0.0087848 0.0019559 0.0014532 5.3069e-06 0 -0.0035465 -0.039029 0 -0.0087848 -0.049708
-3.7701e-05 -3.2128e-05 0 -0.035207 -0.11973 0 -0.025659 0.21114 5.9967e-05 -1.8253e-05 0 -0.026167 0.036298 0 -0.222 0.011442 5.9967e-05 0.24158 0 0.082595 0.036298 0 0.29752 0.011442
-6.8931e-06 0 0 0 0.070208 0 0 -0.10009 -6.7975e-05 0 0 0.01621 0.027531 0 0.11375 0.026175 0.24156 0 0 -0.01621 -0.09931 0 -0.11375 -0.10067
0 -2.544e-06 0 0.011012 0 0 -0.0050638 0 0 -2.544e-06 0 0.011012 0 0 -0.0050638 0 0 -2.544e-06 0 0.011012 0 0 -0.0050638 0
1.5503e-06 0 0 0 0.019714 0 0 -0.016111 0 -3.7407e-06 0 -0.0016783 0.0084354 0 0.010358 -0.0065422 0 3.7407e-06 0 0.0016783 0.0084354 0 -0.010358 -0.0065422
0 0 0 0 0 0.22669 0 0 0 0 0 0 0 0.22669 0 0 0 0 0 0 0 0.22669 0 0
0 1.4956e-05 0 0.026322 0 0 0.0062638 0 0 1.4956e-05 0 0.026322 0 0 0.0062638 0 0 1.4956e-05 0 0.026322 0 0 0.0062638 0
-1.1942e-06 0 0 0 0.061018 0 0 -0.070798 -3.6573e-05 -3.7255e-06 0 0.0022167 0.016702 0 0.070809 0.0036908 -3.6573e-05 3.7255e-06 0 -0.0022167 0.016702 0 -0.070809 0.0036908
0 0 0 0 0 5.7523 0 0 0 0 0 0 0 5.7523 0 0 0 0 0 0 0 5.7523 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 1.8668 0 0 0 0 0 0 0 -1.8668 0 0
0 0 0 0 0 -2.1556 0 0 0 0 0 0 0 1.0778 0 0 0 0 0 0 0 1.0778 0 0
1.0062e-05 0 0 0 0.13774 0 0 -0.11581 -2.6484e-05 -2.3305e-05 0 -0.056463 -0.047995 0 0.11114 -0.011239 -2.6484e-05 2.3305e-05 0 0.056463 -0.047995 0 -0.11114 -0.011239
0 0 0 0 0 0 0 0 0 0 0 0 0 50.5997 0 0 0 0 0 0 0 -50.5997 0 0
0 0 0 0 0 -58.4275 0 0 0 0 0 0 0 29.2138 0 0 0 0 0 0 0 29.2138 0 0
0.00011723 0 0 0 0.46124 0 0 -0.63883 -0.00017814 -5.2964e-05 0 -0.090248 -0.13895 0 0.60089 -0.041938 -0.00017814 5.2964e-05 0 0.090248 -0.13895 0 -0.60089 -0.041938
\ No newline at end of file
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -8.9494 0 0 -0.89908 0 2.0354 0 0 0.18776 0
-17.6272 0 0 0 -21.0598 0 -4.858 0.92982 -2.6367 0 -14.9794 8.5718 -2.0449 0.89132 0.92982 2.6367 0 14.9794 8.5718 2.0449 0.89132 0 7.1596 0 0 4.687 0 0 0 -2.5669 0 0 -2.2401
0 -14.9712 0 -4.882 0 -2.7453 0 0 -0.46314 0 -0.82757 0 -0.28499 0 0 -0.46314 0 -0.82757 0 -0.28499 0 -9.2135 0 0 -5.8331 0 0 0 0 0 0 0 0
0 0 -9.5125 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -8.9494 0 0 -0.89908 -1.7627 -1.0177 0 -0.1626 -0.093879 0
0.88503 0 0 0 4.8328 0 0.67088 -15.536 1.2265 0 5.2419 -8.0632 0.87499 -3.1928 -1.028 -1.2265 0 -5.2419 -4.0088 -0.87499 -0.73252 0 9.9943 0 0 6.1474 0 0 0 0.67067 0 0 0.51935
-2.6514 -0.46314 0 -0.82757 -16.1082 -0.28499 -2.1162 1.2675 -17.4425 0 -19.4413 8.7825 -4.6953 1.0778 1.2675 2.0081 0 13.7318 8.7825 1.665 1.0778 -9.2135 -1.3308 0 -5.8331 -0.65212 0 0 0 -1.5888 0 0 -1.3786
0 0 0 0 0 0 0 0 0 -9.5125 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -8.9494 0 0 -0.89908 1.7627 -1.0177 0 0.1626 -0.093879 0
0.88503 0 0 0 4.8328 0 0.67088 -1.028 1.2265 0 5.2419 -4.0088 0.87499 -0.73252 -15.536 -1.2265 0 -5.2419 -8.0632 -0.87499 -3.1928 0 9.9943 0 0 6.1474 0 0 0 0.67067 0 0 0.51935
2.6514 -0.46314 0 -0.82757 16.1082 -0.28499 2.1162 -1.2675 2.0081 0 13.7318 -8.7825 1.665 -1.0778 -1.2675 -17.4425 0 -19.4413 -8.7825 -4.6953 -1.0778 -9.2135 1.3308 0 -5.8331 0.65212 0 0 0 1.5888 0 0 1.3786
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -9.5125 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
\ No newline at end of file
0.20569 0 0 0 -0.21086 0 -0.04858 0.0092982 -0.026367 0 -0.14979 0.085718 -0.020449 0.0089132 0.0092982 0.026367 0 0.14979 0.085718 0.020449 0.0089132 0 0.071857 0 0 0.04687 0 0 0 -0.02561 0 0 -0.022401
0 0.23225 0 -0.049083 0 -0.027453 0 0 -0.0046314 0 -0.008277 0 -0.0028499 0 0 -0.0046314 0 -0.008277 0 -0.0028499 0 -0.092397 0 0 -0.058331 0 0 0 0 0 0 0 0
0 0 0.28684 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 -0.0014523 0 0.95443 0 0.01 0 0 -5.1747e-06 0 0.0060958 0 0 0 0 -5.1747e-06 0 0.0060958 0 0 0 -0.017864 0 0 0 0 0 0 0 0 0 0 0
-0.001453 0 0 0 0.95511 0 0.01 4.7147e-06 9.6448e-06 0 -0.011283 -0.0050871 0 0 4.7147e-06 -9.6448e-06 0 0.011283 -0.0050871 0 0 0 0.0077858 0 0 0 0 0 0 -0.0042679 0 0 0
0 0.00018634 0 -0.20739 0 1 0 0 3.2128e-05 0 -0.034967 0 0 0 0 3.2128e-05 0 -0.034967 0 0 0 -0.021221 0 0 0 0 0 0 0 0 0 0 0
0.00015939 0 0 0 -0.1775 0 1 7.4329e-05 4.1973e-06 0 -0.0063615 -0.08284 0 0 7.4329e-05 -4.1973e-06 0 0.0063615 -0.08284 0 0 0 0.00091178 0 0 0 0 0 0 0.012428 0 0 0
0.0088503 0 0 0 0.048328 0 0.0067088 0.22661 0.012265 0 0.052421 -0.080895 0.0087499 -0.031928 -0.01028 -0.012265 0 -0.052421 -0.040089 -0.0087499 -0.0073252 0 0.10021 0 0 0.061474 0 0 0 0.0066775 0 0 0.0051935
-0.026514 -0.0046314 0 -0.008277 -0.16108 -0.0028499 -0.021162 0.012675 0.20754 0 -0.19468 0.087827 -0.046953 0.010778 0.012675 0.020081 0 0.13731 0.087827 0.01665 0.010778 -0.092397 -0.013307 0 -0.058331 -0.0065212 0 0 0 -0.015835 0 0 -0.013786
0 0 0 0 0 0 0 0 0 0.28684 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
-1.0584e-06 -5.1747e-06 0 0.0060958 0.0010321 0 0 9.2343e-06 -0.0014431 0 0.94363 -0.010383 0.01 0 9.2343e-06 -1.4339e-05 0 0.016896 -0.010383 0 0 -0.017864 -0.010654 0 0 0 0 0 0 -0.0055464 0 0 0
0 0 0 0 0.00084986 0 0 -0.0014532 5.3069e-06 0 -0.0061657 0.95563 0 0.01 -6.1066e-06 -5.3069e-06 0 0.0061657 0.0072954 0 0 0 0.015668 0 0 0 0 0 0 -0.0040894 0 0 0
-3.7701e-05 3.2128e-05 0 -0.034967 0.042349 0 0 5.9967e-05 0.00020021 0 -0.22478 -0.067897 1 0 5.9967e-05 1.8253e-05 0 -0.017575 -0.067897 0 0 -0.021221 -0.047134 0 0 0 0 0 0 -0.014222 0 0 0
6.8931e-06 0 0 0 -0.0073987 0 0 0.00022219 0 0 -0.0018094 -0.24744 0 1 6.7975e-05 0 0 0.0018094 -0.075018 0 0 0 0.0015708 0 0 0 0 0 0 -0.017945 0 0 0
0.0088503 0 0 0 0.048328 0 0.0067088 -0.01028 0.012265 0 0.052421 -0.040089 0.0087499 -0.0073252 0.22661 -0.012265 0 -0.052421 -0.080895 -0.0087499 -0.031928 0 0.10021 0 0 0.061474 0 0 0 0.0066775 0 0 0.0051935
0.026514 -0.0046314 0 -0.008277 0.16108 -0.0028499 0.021162 -0.012675 0.020081 0 0.13731 -0.087827 0.01665 -0.010778 -0.012675 0.20754 0 -0.19468 -0.087827 -0.046953 -0.010778 -0.092397 0.013307 0 -0.058331 0.0065212 0 0 0 0.015835 0 0 0.013786
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.28684 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
1.0584e-06 -5.1747e-06 0 0.0060958 -0.0010321 0 0 -9.2343e-06 -1.4339e-05 0 0.016896 0.010383 0 0 -9.2343e-06 -0.0014431 0 0.94363 0.010383 0.01 0 -0.017864 0.010654 0 0 0 0 0 0 0.0055464 0 0 0
0 0 0 0 0.00084986 0 0 -6.1066e-06 5.3069e-06 0 -0.0061657 0.0072954 0 0 -0.0014532 -5.3069e-06 0 0.0061657 0.95563 0 0.01 0 0.015668 0 0 0 0 0 0 -0.0040894 0 0 0
3.7701e-05 3.2128e-05 0 -0.034967 -0.042349 0 0 -5.9967e-05 1.8253e-05 0 -0.017575 0.067897 0 0 -5.9967e-05 0.00020021 0 -0.22478 0.067897 1 0 -0.021221 0.047134 0 0 0 0 0 0 0.014222 0 0 0
6.8931e-06 0 0 0 -0.0073987 0 0 6.7975e-05 0 0 -0.0018094 -0.075018 0 0 0.00022219 0 0 0.0018094 -0.24744 0 1 0 0.0015708 0 0 0 0 0 0 -0.017945 0 0 0
0 2.544e-06 0 -0.002974 0 0 0 0 2.544e-06 0 -0.002974 0 0 0 0 2.544e-06 0 -0.002974 0 0 0 0.96696 0 0 0.01 0 0 0 0 0 0 0 0
-1.5503e-06 0 0 0 0.0018018 0 0 0 3.7407e-06 0 -0.0043399 0.00094658 0 0 0 -3.7407e-06 0 0.0043399 0.00094658 0 0 0 0.96341 0 0 0.01 0 0 0 -0.0032628 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.31993 0 0 0.01 0 0 0 0 0 0
0 -1.4956e-05 0 0.016407 0 0 0 0 -1.4956e-05 0 0.016407 0 0 0 0 -1.4956e-05 0 0.016407 0 0 0 -0.078965 0 0 1 0 0 0 0 0 0 0 0
1.1942e-06 0 0 0 -0.001119 0 0 3.6573e-05 3.7255e-06 0 -0.0049281 -0.040739 0 0 3.6573e-05 -3.7255e-06 0 0.0049281 -0.040739 0 0 0 -0.094421 0 0 1 0 0 0 -0.0093097 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -19.4576 0 0 0.77891 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.25632 0 0 0.01 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.25632 0 0 0.01 0
-1.0062e-05 0 0 0 0.010962 0 0 2.6484e-05 2.3305e-05 0 -0.02734 -0.029617 0 0 2.6484e-05 -2.3305e-05 0 0.02734 -0.029617 0 0 0 -0.041744 0 0 0 0 0 0 0.93479 0 0 0.01
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -22.7937 0 0 0.75682 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -22.7937 0 0 0.75682 0
-0.00011723 0 0 0 0.13224 0 0 0.00017814 5.2964e-05 0 -0.063897 -0.20096 0 0 0.00017814 -5.2964e-05 0 0.063897 -0.20096 0 0 0 -0.18334 0 0 0 0 0 0 -0.174 0 0 1
\ No newline at end of file
1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0.23 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 -0.23 0 0 0 0
0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 -0.19919 0 0 0
0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 -0.115 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0.19919 0.115 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 1 0 0 0 0 0 0 0 0.19919 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 1 0 0 0 0 0 0 -0.115 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 1 0 0 0 -0.19919 0.115 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.5 0 0 0 0 0 0 0 0 0 0 0 0 0 0
\ No newline at end of file
0.61803 0 0 0 -0.00026152 0 0 -0.00026541 0 0 0 -1.4303e-06 0 0 0 0 0 0 0 1.4303e-06 0 0 0 0
0 0.61803 0 0.00026251 0 0 0.00026503 0 0 0 0 0 0 0 2.5701e-06 0 0 0 0 0 0 0 2.5701e-06 0
0 0 0.61803 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0.0014523 0 0.040397 0 0 0.050739 0 0 5.1747e-06 0 -0.011267 0 0 -0.00092505 0 0 5.1747e-06 0 -0.011267 0 0 -0.00092505 0
0.001453 0 0 0 -0.0031643 0 0 -0.086621 -4.7147e-06 -9.6448e-06 0 -0.011206 -0.0023108 0 0.033772 -0.0078635 -4.7147e-06 9.6448e-06 0 0.011206 -0.0023108 0 -0.033772 -0.0078635
0 0.2416 0 0.091635 0 0 0.10118 0 0 -3.2128e-05 0 -0.035207 0 0 -0.025659 0 0 -3.2128e-05 0 -0.035207 0 0 -0.025659 0
0.24162 0 0 0 -0.031001 0 0 -0.24791 -7.4329e-05 -4.1973e-06 0 0.0046133 0.015045 0 0.13445 0.023017 -7.4329e-05 4.1973e-06 0 -0.0046133 0.015045 0 -0.13445 0.023017
0 0 0 0 -2.7686e-06 0 0 2.2642e-06 0.61803 0 0 0 -0.00026186 0 -4.4423e-06 -0.00026557 0 0 0 0 0 0 4.4423e-06 -3.1122e-06
0 0 0 0 -3.9213e-06 0 2.5701e-06 4.6343e-06 0 0.61803 0 0.00026254 1.4562e-06 0 0.00026071 2.4861e-06 0 0 0 0 1.4562e-06 0 6.8891e-06 2.4861e-06
0 0 0 0 0 0 0 0 0 0 0.61803 0 0 0 0 0 0 0 0 0 0 0 0 0
1.0584e-06 5.1747e-06 0 -0.011267 0.036544 0 -0.00092505 -0.03448 -9.2343e-06 0.0014431 0 0.029215 -0.012945 0 0.083521 -0.0078212 -9.2343e-06 1.4339e-05 0 -8.4817e-05 -0.012945 0 -0.033707 -0.0078212
0 0 0 0 0.010726 0 0 -0.0090262 0.0014532 -5.3069e-06 0 0.0035465 -0.039029 0 0.0087848 -0.049708 6.1066e-06 5.3069e-06 0 -0.0035465 0.012635 0 -0.0087848 0.0019559
3.7701e-05 -3.2128e-05 0 -0.035207 0.11973 0 -0.025659 -0.21114 -5.9967e-05 0.24158 0 0.082595 -0.036298 0 0.29752 -0.011442 -5.9967e-05 -1.8253e-05 0 -0.026167 -0.036298 0 -0.222 -0.011442
-6.8931e-06 0 0 0 0.070208 0 0 -0.10009 0.24156 0 0 0.01621 -0.09931 0 0.11375 -0.10067 -6.7975e-05 0 0 -0.01621 0.027531 0 -0.11375 0.026175
0 0 0 0 -2.7686e-06 0 0 2.2642e-06 0 0 0 0 0 0 -4.4423e-06 -3.1122e-06 0.61803 0 0 0 -0.00026186 0 4.4423e-06 -0.00026557
0 0 0 0 3.9213e-06 0 2.5701e-06 -4.6343e-06 0 0 0 0 -1.4562e-06 0 6.8891e-06 -2.4861e-06 0 0.61803 0 0.00026254 -1.4562e-06 0 0.00026071 -2.4861e-06
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.61803 0 0 0 0 0
-1.0584e-06 5.1747e-06 0 -0.011267 -0.036544 0 -0.00092505 0.03448 9.2343e-06 1.4339e-05 0 -8.4817e-05 0.012945 0 -0.033707 0.0078212 9.2343e-06 0.0014431 0 0.029215 0.012945 0 0.083521 0.0078212
0 0 0 0 0.010726 0 0 -0.0090262 6.1066e-06 -5.3069e-06 0 0.0035465 0.012635 0 0.0087848 0.0019559 0.0014532 5.3069e-06 0 -0.0035465 -0.039029 0 -0.0087848 -0.049708
-3.7701e-05 -3.2128e-05 0 -0.035207 -0.11973 0 -0.025659 0.21114 5.9967e-05 -1.8253e-05 0 -0.026167 0.036298 0 -0.222 0.011442 5.9967e-05 0.24158 0 0.082595 0.036298 0 0.29752 0.011442
-6.8931e-06 0 0 0 0.070208 0 0 -0.10009 -6.7975e-05 0 0 0.01621 0.027531 0 0.11375 0.026175 0.24156 0 0 -0.01621 -0.09931 0 -0.11375 -0.10067
0 -2.544e-06 0 0.011012 0 0 -0.0050638 0 0 -2.544e-06 0 0.011012 0 0 -0.0050638 0 0 -2.544e-06 0 0.011012 0 0 -0.0050638 0
1.5503e-06 0 0 0 0.019714 0 0 -0.016111 0 -3.7407e-06 0 -0.0016783 0.0084354 0 0.010358 -0.0065422 0 3.7407e-06 0 0.0016783 0.0084354 0 -0.010358 -0.0065422
0 0 0 0 0 0.22669 0 0 0 0 0 0 0 0.22669 0 0 0 0 0 0 0 0.22669 0 0
0 1.4956e-05 0 0.026322 0 0 0.0062638 0 0 1.4956e-05 0 0.026322 0 0 0.0062638 0 0 1.4956e-05 0 0.026322 0 0 0.0062638 0
-1.1942e-06 0 0 0 0.061018 0 0 -0.070798 -3.6573e-05 -3.7255e-06 0 0.0022167 0.016702 0 0.070809 0.0036908 -3.6573e-05 3.7255e-06 0 -0.0022167 0.016702 0 -0.070809 0.0036908
0 0 0 0 0 5.7523 0 0 0 0 0 0 0 5.7523 0 0 0 0 0 0 0 5.7523 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 1.8668 0 0 0 0 0 0 0 -1.8668 0 0
0 0 0 0 0 -2.1556 0 0 0 0 0 0 0 1.0778 0 0 0 0 0 0 0 1.0778 0 0
1.0062e-05 0 0 0 0.13774 0 0 -0.11581 -2.6484e-05 -2.3305e-05 0 -0.056463 -0.047995 0 0.11114 -0.011239 -2.6484e-05 2.3305e-05 0 0.056463 -0.047995 0 -0.11114 -0.011239
0 0 0 0 0 0 0 0 0 0 0 0 0 50.5997 0 0 0 0 0 0 0 -50.5997 0 0
0 0 0 0 0 -58.4275 0 0 0 0 0 0 0 29.2138 0 0 0 0 0 0 0 29.2138 0 0
0.00011723 0 0 0 0.46124 0 0 -0.63883 -0.00017814 -5.2964e-05 0 -0.090248 -0.13895 0 0.60089 -0.041938 -0.00017814 5.2964e-05 0 0.090248 -0.13895 0 -0.60089 -0.041938
\ No newline at end of file
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -8.9494 0 0 -0.89908 0 2.0354 0 0 0.18776 0
-17.6272 0 0 0 -21.0598 0 -4.858 0.92982 -2.6367 0 -14.9794 8.5718 -2.0449 0.89132 0.92982 2.6367 0 14.9794 8.5718 2.0449 0.89132 0 7.1596 0 0 4.687 0 0 0 -2.5669 0 0 -2.2401
0 -14.9712 0 -4.882 0 -2.7453 0 0 -0.46314 0 -0.82757 0 -0.28499 0 0 -0.46314 0 -0.82757 0 -0.28499 0 -9.2135 0 0 -5.8331 0 0 0 0 0 0 0 0
0 0 -9.5125 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -8.9494 0 0 -0.89908 -1.7627 -1.0177 0 -0.1626 -0.093879 0
0.88503 0 0 0 4.8328 0 0.67088 -15.536 1.2265 0 5.2419 -8.0632 0.87499 -3.1928 -1.028 -1.2265 0 -5.2419 -4.0088 -0.87499 -0.73252 0 9.9943 0 0 6.1474 0 0 0 0.67067 0 0 0.51935
-2.6514 -0.46314 0 -0.82757 -16.1082 -0.28499 -2.1162 1.2675 -17.4425 0 -19.4413 8.7825 -4.6953 1.0778 1.2675 2.0081 0 13.7318 8.7825 1.665 1.0778 -9.2135 -1.3308 0 -5.8331 -0.65212 0 0 0 -1.5888 0 0 -1.3786
0 0 0 0 0 0 0 0 0 -9.5125 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -8.9494 0 0 -0.89908 1.7627 -1.0177 0 0.1626 -0.093879 0
0.88503 0 0 0 4.8328 0 0.67088 -1.028 1.2265 0 5.2419 -4.0088 0.87499 -0.73252 -15.536 -1.2265 0 -5.2419 -8.0632 -0.87499 -3.1928 0 9.9943 0 0 6.1474 0 0 0 0.67067 0 0 0.51935
2.6514 -0.46314 0 -0.82757 16.1082 -0.28499 2.1162 -1.2675 2.0081 0 13.7318 -8.7825 1.665 -1.0778 -1.2675 -17.4425 0 -19.4413 -8.7825 -4.6953 -1.0778 -9.2135 1.3308 0 -5.8331 0.65212 0 0 0 1.5888 0 0 1.3786
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -9.5125 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
\ No newline at end of file
0.20569 0 0 0 -0.21086 0 -0.04858 0.0092982 -0.026367 0 -0.14979 0.085718 -0.020449 0.0089132 0.0092982 0.026367 0 0.14979 0.085718 0.020449 0.0089132 0 0.071857 0 0 0.04687 0 0 0 -0.02561 0 0 -0.022401
0 0.23225 0 -0.049083 0 -0.027453 0 0 -0.0046314 0 -0.008277 0 -0.0028499 0 0 -0.0046314 0 -0.008277 0 -0.0028499 0 -0.092397 0 0 -0.058331 0 0 0 0 0 0 0 0
0 0 0.28684 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 -0.0014523 0 0.95443 0 0.01 0 0 -5.1747e-06 0 0.0060958 0 0 0 0 -5.1747e-06 0 0.0060958 0 0 0 -0.017864 0 0 0 0 0 0 0 0 0 0 0
-0.001453 0 0 0 0.95511 0 0.01 4.7147e-06 9.6448e-06 0 -0.011283 -0.0050871 0 0 4.7147e-06 -9.6448e-06 0 0.011283 -0.0050871 0 0 0 0.0077858 0 0 0 0 0 0 -0.0042679 0 0 0
0 0.00018634 0 -0.20739 0 1 0 0 3.2128e-05 0 -0.034967 0 0 0 0 3.2128e-05 0 -0.034967 0 0 0 -0.021221 0 0 0 0 0 0 0 0 0 0 0
0.00015939 0 0 0 -0.1775 0 1 7.4329e-05 4.1973e-06 0 -0.0063615 -0.08284 0 0 7.4329e-05 -4.1973e-06 0 0.0063615 -0.08284 0 0 0 0.00091178 0 0 0 0 0 0 0.012428 0 0 0
0.0088503 0 0 0 0.048328 0 0.0067088 0.22661 0.012265 0 0.052421 -0.080895 0.0087499 -0.031928 -0.01028 -0.012265 0 -0.052421 -0.040089 -0.0087499 -0.0073252 0 0.10021 0 0 0.061474 0 0 0 0.0066775 0 0 0.0051935
-0.026514 -0.0046314 0 -0.008277 -0.16108 -0.0028499 -0.021162 0.012675 0.20754 0 -0.19468 0.087827 -0.046953 0.010778 0.012675 0.020081 0 0.13731 0.087827 0.01665 0.010778 -0.092397 -0.013307 0 -0.058331 -0.0065212 0 0 0 -0.015835 0 0 -0.013786
0 0 0 0 0 0 0 0 0 0.28684 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
-1.0584e-06 -5.1747e-06 0 0.0060958 0.0010321 0 0 9.2343e-06 -0.0014431 0 0.94363 -0.010383 0.01 0 9.2343e-06 -1.4339e-05 0 0.016896 -0.010383 0 0 -0.017864 -0.010654 0 0 0 0 0 0 -0.0055464 0 0 0
0 0 0 0 0.00084986 0 0 -0.0014532 5.3069e-06 0 -0.0061657 0.95563 0 0.01 -6.1066e-06 -5.3069e-06 0 0.0061657 0.0072954 0 0 0 0.015668 0 0 0 0 0 0 -0.0040894 0 0 0
-3.7701e-05 3.2128e-05 0 -0.034967 0.042349 0 0 5.9967e-05 0.00020021 0 -0.22478 -0.067897 1 0 5.9967e-05 1.8253e-05 0 -0.017575 -0.067897 0 0 -0.021221 -0.047134 0 0 0 0 0 0 -0.014222 0 0 0
6.8931e-06 0 0 0 -0.0073987 0 0 0.00022219 0 0 -0.0018094 -0.24744 0 1 6.7975e-05 0 0 0.0018094 -0.075018 0 0 0 0.0015708 0 0 0 0 0 0 -0.017945 0 0 0
0.0088503 0 0 0 0.048328 0 0.0067088 -0.01028 0.012265 0 0.052421 -0.040089 0.0087499 -0.0073252 0.22661 -0.012265 0 -0.052421 -0.080895 -0.0087499 -0.031928 0 0.10021 0 0 0.061474 0 0 0 0.0066775 0 0 0.0051935
0.026514 -0.0046314 0 -0.008277 0.16108 -0.0028499 0.021162 -0.012675 0.020081 0 0.13731 -0.087827 0.01665 -0.010778 -0.012675 0.20754 0 -0.19468 -0.087827 -0.046953 -0.010778 -0.092397 0.013307 0 -0.058331 0.0065212 0 0 0 0.015835 0 0 0.013786
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.28684 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
1.0584e-06 -5.1747e-06 0 0.0060958 -0.0010321 0 0 -9.2343e-06 -1.4339e-05 0 0.016896 0.010383 0 0 -9.2343e-06 -0.0014431 0 0.94363 0.010383 0.01 0 -0.017864 0.010654 0 0 0 0 0 0 0.0055464 0 0 0
0 0 0 0 0.00084986 0 0 -6.1066e-06 5.3069e-06 0 -0.0061657 0.0072954 0 0 -0.0014532 -5.3069e-06 0 0.0061657 0.95563 0 0.01 0 0.015668 0 0 0 0 0 0 -0.0040894 0 0 0
3.7701e-05 3.2128e-05 0 -0.034967 -0.042349 0 0 -5.9967e-05 1.8253e-05 0 -0.017575 0.067897 0 0 -5.9967e-05 0.00020021 0 -0.22478 0.067897 1 0 -0.021221 0.047134 0 0 0 0 0 0 0.014222 0 0 0
6.8931e-06 0 0 0 -0.0073987 0 0 6.7975e-05 0 0 -0.0018094 -0.075018 0 0 0.00022219 0 0 0.0018094 -0.24744 0 1 0 0.0015708 0 0 0 0 0 0 -0.017945 0 0 0
0 2.544e-06 0 -0.002974 0 0 0 0 2.544e-06 0 -0.002974 0 0 0 0 2.544e-06 0 -0.002974 0 0 0 0.96696 0 0 0.01 0 0 0 0 0 0 0 0
-1.5503e-06 0 0 0 0.0018018 0 0 0 3.7407e-06 0 -0.0043399 0.00094658 0 0 0 -3.7407e-06 0 0.0043399 0.00094658 0 0 0 0.96341 0 0 0.01 0 0 0 -0.0032628 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.31993 0 0 0.01 0 0 0 0 0 0
0 -1.4956e-05 0 0.016407 0 0 0 0 -1.4956e-05 0 0.016407 0 0 0 0 -1.4956e-05 0 0.016407 0 0 0 -0.078965 0 0 1 0 0 0 0 0 0 0 0
1.1942e-06 0 0 0 -0.001119 0 0 3.6573e-05 3.7255e-06 0 -0.0049281 -0.040739 0 0 3.6573e-05 -3.7255e-06 0 0.0049281 -0.040739 0 0 0 -0.094421 0 0 1 0 0 0 -0.0093097 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -19.4576 0 0 0.77891 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.25632 0 0 0.01 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.25632 0 0 0.01 0
-1.0062e-05 0 0 0 0.010962 0 0 2.6484e-05 2.3305e-05 0 -0.02734 -0.029617 0 0 2.6484e-05 -2.3305e-05 0 0.02734 -0.029617 0 0 0 -0.041744 0 0 0 0 0 0 0.93479 0 0 0.01
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -22.7937 0 0 0.75682 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -22.7937 0 0 0.75682 0
-0.00011723 0 0 0 0.13224 0 0 0.00017814 5.2964e-05 0 -0.063897 -0.20096 0 0 0.00017814 -5.2964e-05 0 0.063897 -0.20096 0 0 0 -0.18334 0 0 0 0 0 0 -0.174 0 0 1
\ No newline at end of file
......@@ -339,6 +339,43 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
}
break;
}
case 5:
{
// --------------------------------------
// Centralized LQG controller copter and load
// --------------------------------------
if (outer_loop_counter == 0) {
// distributed controller was enabled
ROS_INFO("[STUDENT CONTROLLER] Centralized LQG enabled");
// load z-setpoint is current load z-position
load_setpoint[0] = request.otherCrazyflies[3].x;
load_setpoint[1] = request.otherCrazyflies[3].y;
load_setpoint[2] = request.otherCrazyflies[3].z;
}
// Distributed Controller runs at 100Hz --> downsampling by factor 2
if (outer_loop_counter % 2 == 0)
{
// set equilibrium load position to current load set point
state_eq(21,0) = load_setpoint[0];
state_eq(22,0) = load_setpoint[1];
state_eq(23,0) = load_setpoint[2];
// update measurement vector using vicon Data
get_y_from_CFData( request );
}
// compute centralized controller commands
//centralized_control_command( request, response );
// compute agent's lqr command for the setpoint
agent_lqr_command( request , response );
break;
}
}
outer_loop_counter += 1;
return true;
......@@ -410,6 +447,70 @@ void distributed_control_command( Controller::Request &request, Controller::Resp
}
void centralized_control_command( Controller::Request &request, Controller::Response &response)
{
// Control output
Eigen::Matrix<float,12,1> u = centralized_controller.K * estimator_state_prev;
// Estimator update
estimator_state_prev = centralized_controller.Phi * estimator_state_prev + centralized_controller.Gamma * y_to_controller;
// extract motor command from u:
float thrustSum = 0.0;
float outRoll = 0.0;
float outPitch = 0.0;
float outYaw = 0.0;
//if ( request.ownCrazyflie.crazyflieName == request.otherCrazyflies[0].crazyflieName )
if (my_agentID == 1 )
{
// Controller for crazyflie 01
thrustSum = u(0,0);
outRoll = u(1,0);
outPitch = u(2,0);
outYaw = u(3,0);
}
//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[1].crazyflieName )
else if (my_agentID == 2 )
{
// Controller for crazyflie 02
thrustSum = u(4,0);
outRoll = u(5,0);
outPitch = u(6,0);
outYaw = u(7,0);
}
//else if (request.ownCrazyflie.crazyflieName == request.otherCrazyflies[2].crazyflieName )
else if (my_agentID == 3 )
{
// Controller for crazyflie 03
thrustSum = u(8,0);
outRoll = u(9,0);
outPitch = u(10,0);
outYaw = u(11,0);
}
if (outer_loop_counter % 50 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] u thrust sum computed = " << thrustSum );
}
// PREPARE AND RETURN THE VARIABLE "response"
response.controlOutput.roll = outRoll;
response.controlOutput.pitch = outPitch;
response.controlOutput.yaw = outYaw;
response.controlOutput.motorCmd1 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd2 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd3 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
response.controlOutput.motorCmd4 = computeMotorPolyBackward(thrustSum/4 + gravity_force_copterloadsystem);
// choosing the Crazyflie onBoard controller type.
// it can either be Motor, Rate or Angle based
// response.controlOutput.onboardControllerType = MOTOR_MODE;
response.controlOutput.onboardControllerType = RATE_MODE;
// response.controlOutput.onboardControllerType = ANGLE_MODE;
}
void get_y_from_CFData( Controller::Request &request )
{
// get load orientation for transformation to world frame
......@@ -928,7 +1029,9 @@ void ControllerStateReceivedCallback(const CustomButton& commandReceived)
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 2 received in controller. ");
ROS_INFO("[STUDENT CONTROLLER] Controller State: enable distributed Controller. ");
// load_setpoint = viconData.load_pose ;
augmented_state_eq(6,0) = PI/2; // cable angle theta1_eq = pi/2
augmented_state_eq(25,0) = PI/2; // cable angle theta2_eq = pi/2
augmented_state_eq(44,0) = PI/2; // cable angle theta3_eq = pi/2
controller_state = custom_button_index;
outer_loop_counter = 0;
break;
......@@ -976,6 +1079,20 @@ void ControllerStateReceivedCallback(const CustomButton& commandReceived)
}
break;
}
// > State 5: copter-load system controller , centralized
case 5:
{
// Let the user know that this part of the code was triggered
ROS_INFO("[STUDENT CONTROLLER] Button 5 received in controller. ");
ROS_INFO("[STUDENT CONTROLLER] Controller State: centralized controller for copter-load system ");
augmented_state_eq(6,0) = PI/2; // cable angle theta1_eq = pi/2
augmented_state_eq(13,0) = PI/2; // cable angle theta2_eq = pi/2
augmented_state_eq(20,0) = PI/2; // cable angle theta3_eq = pi/2
controller_state = custom_button_index;
outer_loop_counter = 0;
break;
}
default:
{
......@@ -1344,25 +1461,40 @@ int main(int argc, char* argv[]) {
// get current working directory
std::string folderpath = getcwd_string();
folderpath = folderpath.substr(0, folderpath.size()-5);
folderpath.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/distr_data/");
folderpath.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/distr_data/");
// get current working directory for centralized controller
std::string folderpath_c = getcwd_string();
folderpath_c = folderpath_c.substr(0, folderpath_c.size()-5);
folderpath_c.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/distr_data/centralized/");
// READ MATRICES FROM DATA FOLDER
std::string filename_K = folderpath;
std::string filename_Gamma = folderpath;
std::string filename_Phi = folderpath;
std::string filename_C = folderpath;
std::string filename_K = folderpath;
std::string filename_Gamma = folderpath;
std::string filename_Phi = folderpath;
std::string filename_C = folderpath;
overlappingController.K = readMatrix( filename_K.append("K.txt") );
overlappingController.Gamma = readMatrix( filename_Gamma.append("Gamma.txt") );
overlappingController.Phi = readMatrix( filename_Phi.append("Phi.txt" ) );
outputMatrix = readMatrix( filename_C.append( "C.txt" ) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] outputMatrix = " << outputMatrix);
filename_K = folderpath_c;
filename_Gamma = folderpath_c;
filename_Phi = folderpath_c;
filename_C = folderpath_c;
centralized_controller.K = readMatrix( filename_K.append("K.txt") );
centralized_controller.Gamma = readMatrix( filename_Gamma.append("Gamma.txt") );
centralized_controller.Phi = readMatrix( filename_Phi.append("Phi.txt" ) );
outputMatrix_centralized = readMatrix( filename_C.append( "C.txt" ) );
//ROS_INFO_STREAM("[STUDENT CONTROLLER] outputMatrix = " << outputMatrix);
// Initialize some state vector of the output feedback controller
augmented_state_eq.setZero();
augmented_state_eq(6,0) = PI/2; // cable angle theta1_eq = pi/2
augmented_state_eq(25,0) = PI/2; // cable angle theta2_eq = pi/2
augmented_state_eq(44,0) = PI/2; // cable angle theta3_eq = pi/2
overlapping_estimator_state_prev.setZero();
state_eq.setZero();
estimator_state_prev.setZero();
// set measurement vector to zero
y_to_controller.setZero();
// Print out some information to the user.
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment