To receive notifications about scheduled maintenance, please subscribe to the mailing-list gitlab-operations@sympa.ethz.ch. You can subscribe to the mailing-list at https://sympa.ethz.ch

Commit 04a9ac7a authored by Tobias's avatar Tobias
Browse files

now the load is measured instead of link angles...

parent b0901cef
......@@ -327,7 +327,7 @@ void turn_motors_off( Controller::Response &response );
void agent_lqr_command( Controller::Request &request, Controller::Response &response );
void get_y_from_CFData( Controller::Request &request ) ;
void get_y_from_CFData_withLoad( Controller::Request &request ) ;
Eigen::MatrixXf readMatrix(std::string filename);
std::string getcwd_string( void );
void distributed_control_command( Controller::Request &request, Controller::Response &response );
......
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 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 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 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 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
\ No newline at end of file
0.27015 0 0 0 -0.0011927 0 0 0 0 -1.9301e-05 0.0001904 0 0 0 0 1.9301e-05 0.0001904 0 0 0.00029356 0 0 0 0.00010748
0 0.27015 0 0.0012084 0 0 0 0 0 -0.00019494 0 0 0 0 0 -0.00019494 0 0 -0.00030204 0 0 0 0 0
0 0 0.27016 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0.0047619 0 0.27345 0 0 0 0.0002123 0 -0.038966 0 0 0 0.0002123 0 -0.038966 0 0 -0.14617 0 0 0 0 0
0.004762 0 0 0 -0.27236 0 0.00019237 -2.9032e-05 0 0.0054342 0.035372 0 0.00019237 2.9032e-05 0 -0.0054342 0.035372 0 0 0.15447 0 0 0 0.078889
0 0.23905 0 0.20202 0 0 0 -0.00042616 0 -0.082263 0 0 0 -0.00042616 0 -0.082263 0 0 -0.011192 0 0 0 0 0
0.23916 0 0 0 -0.32531 0 -0.000563 -2.5672e-06 0 0.12883 0.13176 0 -0.000563 2.5672e-06 0 -0.12883 0.13176 0 0 0.043234 0 0 0 0.21561
0 0 0 0 0.00018903 0 0.27015 0 0 4.8712e-06 -0.0012053 0 0 0 0 -4.8712e-06 0.00019804 0 0 0.00030173 0 0 0 -6.6939e-05
0 0 0 -0.00019494 1.6718e-05 0 0 0.27015 0 0.0011838 -3.2762e-06 0 0 0 0 -0.00017026 -3.2762e-06 0 -0.00030204 -1.3246e-05 0 0 0 7.12e-05
0 0 0 0 0 0 0 0 0.27016 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
3.1272e-06 0.0002123 0 -0.038966 0.00081557 0 -1.7786e-05 0.0047377 0 0.27854 -0.0028899 0 -1.7786e-05 0.00023649 0 -0.044047 -0.0028899 0 -0.14617 0.0069008 0 0 0 0.069189
0.00017827 0 0 0 0.03237 0 0.00477 -3.9121e-05 0 0.0070961 -0.27123 0 0.00022038 3.9121e-05 0 -0.0070961 0.041191 0 0 0.14864 0 0 0 -0.017452
0.00014166 -0.00042616 0 -0.082263 -0.14338 0 -0.000214 0.23894 0 0.37076 0.051445 0 -0.000214 -0.00031994 0 -0.251 0.051445 0 -0.011192 0.054184 0 0 0 0.30408
-0.00035343 0 0 0 -0.046939 0 0.23889 -9.7127e-05 0 0.14353 -0.15446 0 -0.00058094 9.7127e-05 0 -0.14353 0.12983 0 0 0.055467 0 0 0 0.18835
0 0 0 0 0.00018903 0 0 0 0 4.8712e-06 0.00019804 0 0.27015 0 0 -4.8712e-06 -0.0012053 0 0 0.00030173 0 0 0 -6.6939e-05
0 0 0 -0.00019494 -1.6718e-05 0 0 0 0 -0.00017026 3.2762e-06 0 0 0.27015 0 0.0011838 3.2762e-06 0 -0.00030204 1.3246e-05 0 0 0 -7.12e-05
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.27016 0 0 0 0 0 0 0 0 0
-3.1272e-06 0.0002123 0 -0.038966 -0.00081557 0 1.7786e-05 0.00023649 0 -0.044047 0.0028899 0 1.7786e-05 0.0047377 0 0.27854 0.0028899 0 -0.14617 -0.0069008 0 0 0 -0.069189
0.00017827 0 0 0 0.03237 0 0.00022038 -3.9121e-05 0 0.0070961 0.041191 0 0.00477 3.9121e-05 0 -0.0070961 -0.27123 0 0 0.14864 0 0 0 -0.017452
-0.00014166 -0.00042616 0 -0.082263 0.14338 0 0.000214 -0.00031994 0 -0.251 -0.051445 0 0.000214 0.23894 0 0.37076 -0.051445 0 -0.011192 -0.054184 0 0 0 -0.30408
-0.00035343 0 0 0 -0.046939 0 -0.00058094 -9.7127e-05 0 0.14353 0.12983 0 0.23889 9.7127e-05 0 -0.14353 -0.15446 0 0 0.055467 0 0 0 0.18835
0 -0.00030316 0 0.097301 0 0 0 -0.00030316 0 0.097301 0 0 0 -0.00030316 0 0.097301 0 0 0.17023 0 0 0 0 0
0.00029407 0 0 0 0.095564 0 0.00030258 -1.4559e-05 0 0.0027689 0.097466 0 0.00030258 1.4559e-05 0 -0.0027689 0.097466 0 0 0.17203 0 0 0 0.0061146
0 0 0 0 0 0.11831 0 0 0 0 0 0.11831 0 0 0 0 0 0.11831 0 0 0.11831 0 0 0
0 1.231e-06 0 0.11961 0 0 0 1.231e-06 0 0.11961 0 0 0 1.231e-06 0 0.11961 0 0 0.1064 0 0 0 0 0
4.4778e-05 0 0 0 0.056939 0 -8.0606e-05 -4.0373e-05 0 0.070159 0.14283 0 -8.0606e-05 4.0373e-05 0 -0.070159 0.14283 0 0 0.12772 0 0 0 0.1038
0 0 0 0 0 0.11526 0 0 0 0 0 0.11526 0 0 0 0 0 0.11526 0 0 0.11526 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0.053299 0 0 0 0 0 -0.053299 0 0 0 0.26758 0 0
0 0 0 0 0 -0.061544 0 0 0 0 0 0.030772 0 0 0 0 0 0.030772 0 0 0 0 0.26758 0
0.00010565 0 0 0 0.030033 0 -7.017e-05 6.2761e-05 0 -0.020379 -0.017486 0 -7.017e-05 -6.2761e-05 0 0.020379 -0.017486 0 0 0.0066714 0 0 0 0.29613
0 0 0 0 0 0 0 0 0 0 0 0.051229 0 0 0 0 0 -0.051229 0 0 0 0.25719 0 0
0 0 0 0 0 -0.059154 0 0 0 0 0 0.029577 0 0 0 0 0 0.029577 0 0 0 0 0.25719 0
0.00044551 0 0 0 -0.32643 0 -0.000646 -0.00031267 0 0.41522 0.10455 0 -0.000646 0.00031267 0 -0.41522 0.10455 0 0 0.15681 0 0 0 1.0025
\ 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 -0.53522 0 0 -0.57714 0 0.48944 0 0 0.49956 0
-7.3464 0 0 0 -5.7493 0 -1.5428 1.2902 -2.9948 0 -5.7279 3.8058 -1.1048 0.61372 1.2902 2.9948 0 5.7279 3.8058 1.1048 0.61372 0 0.15406 0 0 0.34031 0 0 0 -0.20797 0 0 -0.3667
0 -4.084 0 0.73176 0 -0.32802 0 0 -0.24088 0 0.32292 0 -0.036128 0 0 -0.24088 0 0.32292 0 -0.036128 0 -0.17845 0 0 -0.38538 0 0 0 0 0 0 0 0
0 0 -0.31573 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.53522 0 0 -0.57714 -0.42387 -0.24472 0 -0.43263 -0.24978 0
1.241 0 0 0 1.9762 0 0.39208 -4.6799 1.2927 0 1.5348 -0.28731 0.38936 -0.54975 -0.83683 -1.2927 0 -1.5348 -0.69615 -0.38936 -0.25786 0 0.18892 0 0 0.40338 0 0 0 0.072182 0 0 0.11638
-2.99 -0.24088 0 0.32292 -5.4968 -0.036128 -1.0828 1.3301 -6.7269 0 -4.1551 2.9913 -1.2968 0.55805 1.3301 2.4021 0 5.2098 2.9913 0.93264 0.55805 -0.17845 -0.015867 0 -0.38538 -0.027857 0 0 0 -0.12977 0 0 -0.22393
0 0 0 0 0 0 0 0 0 -0.31573 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.53522 0 0 -0.57714 0.42387 -0.24472 0 0.43263 -0.24978 0
1.241 0 0 0 1.9762 0 0.39208 -0.83683 1.2927 0 1.5348 -0.69615 0.38936 -0.25786 -4.6799 -1.2927 0 -1.5348 -0.28731 -0.38936 -0.54975 0 0.18892 0 0 0.40338 0 0 0 0.072182 0 0 0.11638
2.99 -0.24088 0 0.32292 5.4968 -0.036128 1.0828 -1.3301 2.4021 0 5.2098 -2.9913 0.93264 -0.55805 -1.3301 -6.7269 0 -4.1551 -2.9913 -1.2968 -0.55805 -0.17845 0.015867 0 -0.38538 0.027857 0 0 0 0.12977 0 0 0.22393
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.31573 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
\ No newline at end of file
0.65638 0 0 0 -0.058089 0 -0.015428 0.012902 -0.029948 0 -0.057269 0.038153 -0.011048 0.0061372 0.012902 0.029948 0 0.057269 0.038153 0.011048 0.0061372 0 0.002059 0 0 0.0034031 0 0 0 -0.0018767 0 0 -0.003667
0 0.68901 0 0.0067134 0 -0.0032802 0 0 -0.0024096 0 0.0033267 0 -0.00036128 0 0 -0.0024096 0 0.0033267 0 -0.00036128 0 -0.002301 0 0 -0.0038538 0 0 0 0 0 0 0 0
0 0 0.72669 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
0 -0.0047619 0 0.86327 0 0.01 0 0 -0.0002123 0 0.019483 0 0 0 0 -0.0002123 0 0.019483 0 0 0 -0.049358 0 0 0 0 0 0 0 0 0 0 0
-0.004762 0 0 0 0.86382 0 0.01 -0.00019237 2.9032e-05 0 -0.0027171 0.017686 0 0 -0.00019237 -2.9032e-05 0 0.0027171 0.017686 0 0 0 0.047146 0 0 0 0 0 0 -0.0059465 0 0 0
0 0.0027335 0 -0.21199 0 1 0 0 0.00042616 0 -0.024268 0 0 0 0 0.00042616 0 -0.024268 0 0 0 -0.026305 0 0 0 0 0 0 0 0 0 0 0
0.0026188 0 0 0 -0.2007 0 1 0.000563 2.5672e-06 0 -0.0012463 -0.035989 0 0 0.000563 -2.5672e-06 0 0.0012463 -0.035989 0 0 0 0.018553 0 0 0 0 0 0 -0.059164 0 0 0
0.012409 0 0 0 0.019856 0 0.0039208 0.68305 0.012927 0 0.015346 -0.0034758 0.0038936 -0.0054975 -0.0083692 -0.012927 0 -0.015346 -0.0068625 -0.0038936 -0.0025786 0 0.0024058 0 0 0.0040338 0 0 0 0.00063139 0 0 0.0011638
-0.0299 -0.0024096 0 0.0033267 -0.054959 -0.00036128 -0.010828 0.013301 0.66258 0 -0.042143 0.029912 -0.012968 0.0055805 0.013301 0.02402 0 0.052183 0.029912 0.0093264 0.0055805 -0.002301 -0.00015559 0 -0.0038538 -0.00027857 0 0 0 -0.0011038 0 0 -0.0022393
0 0 0 0 0 0 0 0 0 0.72669 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
-3.1272e-06 -0.0002123 0 0.019483 0.00040779 0 0 1.7786e-05 -0.0047377 0 0.86073 -0.0014449 0.01 0 1.7786e-05 -0.00023649 0 0.022024 -0.0014449 0 0 -0.049358 -0.0019367 0 0 0 0 0 0 -0.0057874 0 0 0
-0.00017827 0 0 0 0.016185 0 0 -0.00477 3.9121e-05 0 -0.0035481 0.86439 0 0.01 -0.00022038 -3.9121e-05 0 0.0035481 0.020596 0 0 0 0.049032 0 0 0 0 0 0 -0.013621 0 0 0
-0.00014166 0.00042616 0 -0.024268 0.016365 0 0 0.000214 0.0028398 0 -0.22011 -0.018304 1 0 0.000214 0.00031994 0 -0.016155 -0.018304 0 0 -0.026305 -0.013697 0 0 0 0 0 0 -0.13542 0 0 0
0.00035343 0 0 0 -0.015928 0 0 0.0028883 9.7127e-05 0 -0.0085937 -0.22468 0 1 0.00058094 -9.7127e-05 0 0.0085937 -0.036957 0 0 0 0.016104 0 0 0 0 0 0 -0.12321 0 0 0
0.012409 0 0 0 0.019856 0 0.0039208 -0.0083692 0.012927 0 0.015346 -0.0068625 0.0038936 -0.0025786 0.68305 -0.012927 0 -0.015346 -0.0034758 -0.0038936 -0.0054975 0 0.0024058 0 0 0.0040338 0 0 0 0.00063139 0 0 0.0011638
0.0299 -0.0024096 0 0.0033267 0.054959 -0.00036128 0.010828 -0.013301 0.02402 0 0.052183 -0.029912 0.0093264 -0.0055805 -0.013301 0.66258 0 -0.042143 -0.029912 -0.012968 -0.0055805 -0.002301 0.00015559 0 -0.0038538 0.00027857 0 0 0 0.0011038 0 0 0.0022393
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.72669 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0
3.1272e-06 -0.0002123 0 0.019483 -0.00040779 0 0 -1.7786e-05 -0.00023649 0 0.022024 0.0014449 0 0 -1.7786e-05 -0.0047377 0 0.86073 0.0014449 0.01 0 -0.049358 0.0019367 0 0 0 0 0 0 0.0057874 0 0 0
-0.00017827 0 0 0 0.016185 0 0 -0.00022038 3.9121e-05 0 -0.0035481 0.020596 0 0 -0.00477 -3.9121e-05 0 0.0035481 0.86439 0 0.01 0 0.049032 0 0 0 0 0 0 -0.013621 0 0 0
0.00014166 0.00042616 0 -0.024268 -0.016365 0 0 -0.000214 0.00031994 0 -0.016155 0.018304 0 0 -0.000214 0.0028398 0 -0.22011 0.018304 1 0 -0.026305 0.013697 0 0 0 0 0 0 0.13542 0 0 0
0.00035343 0 0 0 -0.015928 0 0 0.00058094 9.7127e-05 0 -0.0085937 -0.036957 0 0 0.0028883 -9.7127e-05 0 0.0085937 -0.22468 0 1 0 0.016104 0 0 0 0 0 0 -0.12321 0 0 0
0 0.00030316 0 -0.048651 0 0 0 0 0.00030316 0 -0.048651 0 0 0 0 0.00030316 0 -0.048651 0 0 0 0.53787 0 0 0.01 0 0 0 0 0 0 0 0
-0.00029407 0 0 0 0.047782 0 0 -0.00030258 1.4559e-05 0 -0.0013844 0.048733 0 0 -0.00030258 -1.4559e-05 0 0.0013844 0.048733 0 0 0 0.53748 0 0 0.01 0 0 0 -0.0045741 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.52676 0 0 0.01 0 0 0 0 0 0
0 -1.231e-06 0 -0.027106 0 0 0 0 -1.231e-06 0 -0.027106 0 0 0 0 -1.231e-06 0 -0.027106 0 0 0 -0.46524 0 0 1 0 0 0 0 0 0 0 0
-4.4778e-05 0 0 0 0.03224 0 0 8.0606e-05 4.0373e-05 0 -0.0034951 0.020482 0 0 8.0606e-05 -4.0373e-05 0 0.0034951 0.020482 0 0 0 -0.47032 0 0 1 0 0 0 -0.056096 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.59264 0 0 0.85808 0 0 0 0 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0.71118 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.71118 0 0 0.01 0
-0.00010565 0 0 0 0.015017 0 0 7.017e-05 -6.2761e-05 0 0.010189 -0.0087432 0 0 7.017e-05 6.2761e-05 0 -0.010189 -0.0087432 0 0 0 -0.0017317 0 0 0 0 0 0 0.68483 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.91151 0 0 0.35299 0 0
0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 0 -0.91151 0 0 0.35299 0
-0.00044551 0 0 0 0.057821 0 0 0.000646 0.00031267 0 -0.016189 -0.058244 0 0 0.000646 -0.00031267 0 0.016189 -0.058244 0 0 0 -0.039473 0 0 0 0 0 0 -0.73797 0 0 1
\ No newline at end of file
......@@ -394,7 +394,8 @@ bool calculateControlOutput(Controller::Request &request, Controller::Response &
if (outer_loop_counter % 2 == 0)
{
// update measurement vector using vicon Data
get_y_from_CFData( request );
//get_y_from_CFData( request );
get_y_from_CFData_withLoad( request );
}
......@@ -645,6 +646,108 @@ void get_y_from_CFData( Controller::Request &request )
}
}
void get_y_from_CFData_withLoad( Controller::Request &request )
{
std::vector<float> P1_world_eq;
P1_world_eq = transformation_frame_load_to_world( attachmentPoint.P1, 0.0 , 0.0 , load_setpoint[3] );
std::vector<float> P2_world_eq;
P2_world_eq = transformation_frame_load_to_world( attachmentPoint.P2, 0.0 , 0.0 , load_setpoint[3] );
std::vector<float> P3_world_eq;
P3_world_eq = transformation_frame_load_to_world( attachmentPoint.P3, 0.0 , 0.0 , load_setpoint[3] );
// y1 = [CF01.x , CF01.y , CF01.z, CF01.roll, CF01.pitch, CF01.yaw , len_cable*phi1 , len_cable*theta1 ]
float yaw = request.otherCrazyflies[0].yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
Eigen::Matrix<float,24,1> y_meas;
y_meas(0,0) = request.otherCrazyflies[0].roll;
y_meas(1,0) = request.otherCrazyflies[0].pitch;
y_meas(2,0) = yaw;
y_meas(3,0) = request.otherCrazyflies[0].x;
y_meas(4,0) = request.otherCrazyflies[0].y;
y_meas(5,0) = request.otherCrazyflies[0].z;
// y2 = [CF02.x , CF02.y , CF02.z, CF02.roll, CF02.pitch, CF02.yaw , len_cable*phi2 , len_cable*theta2 ]
yaw = request.otherCrazyflies[1].yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
y_meas(6,0) = request.otherCrazyflies[1].roll;
y_meas(7,0) = request.otherCrazyflies[1].pitch;
y_meas(8,0) = yaw;
y_meas(9,0) = request.otherCrazyflies[1].x;
y_meas(10,0) = request.otherCrazyflies[1].y;
y_meas(11,0) = request.otherCrazyflies[1].z;
// y3 = [CF03.x , CF03.y , CF03.z, CF03.roll, CF03.pitch, CF03.yaw , len_cable*phi3 , len_cable*theta3 ]
yaw = request.otherCrazyflies[2].yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
y_meas(12,0) = request.otherCrazyflies[2].roll;
y_meas(13,0) = request.otherCrazyflies[2].pitch;
y_meas(14,0) = yaw;
y_meas(15,0) = request.otherCrazyflies[2].x;
y_meas(16,0) = request.otherCrazyflies[2].y;
y_meas(17,0) = request.otherCrazyflies[2].z;
y_meas(18,0) = request.otherCrazyflies[3].x;
y_meas(19,0) = request.otherCrazyflies[3].y;
y_meas(20,0) = request.otherCrazyflies[3].z;
y_meas(21,0) = request.otherCrazyflies[3].roll;
y_meas(22,0) = request.otherCrazyflies[3].pitch;
yaw = request.otherCrazyflies[3].yaw;
while(yaw > PI) {yaw -= 2 * PI;}
while(yaw < -PI) {yaw += 2 * PI;}
y_meas(23,0) = yaw;
// compute measurement difference measured vs. set point
// y_meas - C * xi_equiliburium
//Eigen::Matrix<float,24,1> y_eq = outputMatrix * augmented_state_eq;
Eigen::Matrix<float,24,1> y_eq;
y_eq.setZero();
y_eq(3,0) = load_setpoint[0] + P1_world_eq[0]; // equilibrium x-pose copter 1
y_eq(4,0) = load_setpoint[1] + P1_world_eq[1]; // equilibrium y-pose copter 1
y_eq(5,0) = load_setpoint[2] + P1_world_eq[2] + len_cable; // equilibrium z-pose copter 1
y_eq(9,0) = load_setpoint[0] + P2_world_eq[0]; // equilibrium x-pose copter 2
y_eq(10,0) = load_setpoint[1] + P2_world_eq[1]; // equilibrium y-pose copter 2
y_eq(11,0) = load_setpoint[2] + P2_world_eq[2] + len_cable; // equilibrium z-pose copter 2
y_eq(15,0) = load_setpoint[0] + P3_world_eq[0]; // equilibrium x-pose copter 3
y_eq(16,0) = load_setpoint[1] + P3_world_eq[1]; // equilibrium y-pose copter 3
y_eq(17,0) = load_setpoint[2] + P3_world_eq[2] + len_cable; // equilibrium z-pose copter 3
y_eq(18,0) = load_setpoint[0]; // equilibrium x-pose load
y_eq(19,0) = load_setpoint[1]; // equilibrium y-pose load
y_eq(20,0) = load_setpoint[2]; // equilibrium z-pose load
y_eq(23,0) = load_setpoint[3]; // equilibrium yaw load
// compute y w.r.t. equilibrium position, passed to controller
y_to_controller = y_meas - y_eq;
// Debugging
if (outer_loop_counter % 50 == 0)
{
ROS_INFO_STREAM("[STUDENT CONTROLLER] Agent : " << my_agentID);
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_measured^T = " << y_meas.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_eq^T = " << y_eq.transpose() );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter1 = " << y_to_controller(3,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter1 = " << y_to_controller(4,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter1 = " << y_to_controller(5,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter2 = " << y_to_controller(11,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter2 = " << y_to_controller(12,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter2 = " << y_to_controller(13,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] x-error for copter3 = " << y_to_controller(19,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y-error for copter3 = " << y_to_controller(20,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] z-error for copter3 = " << y_to_controller(21,0) );
ROS_INFO_STREAM("[STUDENT CONTROLLER] y_to_controller^T = " << y_to_controller.transpose() );
}
}
std::vector<float> transformation_frame_load_to_world(std::vector<float> P_load , float roll, float pitch, float yaw )
......@@ -1459,7 +1562,7 @@ int main(int argc, char* argv[]) {
// 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/");
folderpath_c.append("/work/D-FaLL-System/pps_ws/src/d_fall_pps/param/distr_data/centralized_withLoad/");
// READ MATRICES FROM DATA FOLDER
std::string filename_K = folderpath;
......
Markdown is supported
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