Skip to content
This repository

HTTPS clone URL

Subversion checkout URL

You can clone with HTTPS or Subversion.

Download ZIP

Hopefully fix force loading from file. Made it more robust at least. #2

Merged
merged 1 commit into from about 2 years ago

2 participants

Dan ssketch
Dan

Refactor force loading from file. BelugaBoundaryControlLaw's constructor now takes two arguments: the x force file and the y force file. Add BelugaBoundaryControlLaw::loadForceFiles. Add tests.

Dan Swain Refactor force loading from file. BelugaBoundaryControlLaw's construc…
…tor now takes two arguments: the x force file and the y force file. Add BelugaBoundaryControlLaw::loadForceFiles. Add tests.
37acd20
ssketch ssketch merged commit a814229 into from
ssketch ssketch closed this
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment

Showing 1 unique commit by 1 author.

Apr 26, 2012
Dan Swain Refactor force loading from file. BelugaBoundaryControlLaw's construc…
…tor now takes two arguments: the x force file and the y force file. Add BelugaBoundaryControlLaw::loadForceFiles. Add tests.
37acd20
This page is out of date. Refresh to see the latest.
95 src/BelugaControl.cpp
... ... @@ -1,4 +1,5 @@
1 1 #include <math.h>
  2 +#include <iostream>
2 3 #include <fstream>
3 4 #ifndef MT_CLAMP
4 5 #define MT_CLAMP(x, a, b) (((x) > (b)) ? (b) : (((x) < (a)) ? (a) : (x)))
@@ -23,9 +24,11 @@ BelugaLowLevelControlLaw* belugaLowLevelControlLawFactory(unsigned int bot_num,
23 24 }
24 25
25 26 BelugaBoundaryControlLaw* belugaBoundaryControlLawFactory(unsigned int bot_num,
26   - unsigned int law_num)
  27 + unsigned int law_num,
  28 + const char* force_file_name_x,
  29 + const char* force_file_name_y)
27 30 {
28   - return new BelugaBoundaryControlLaw();
  31 + return new BelugaBoundaryControlLaw(force_file_name_x, force_file_name_y);
29 32 }
30 33
31 34
@@ -205,30 +208,82 @@ mt_dVector_t BelugaLowLevelControlLaw::doControl(const mt_dVector_t& state,
205 208 return u;
206 209 }
207 210
208   -BelugaBoundaryControlLaw::BelugaBoundaryControlLaw()
  211 +BelugaBoundaryControlLaw::BelugaBoundaryControlLaw(const char* force_file_name_x,
  212 + const char* force_file_name_y)
209 213 : mt_ControlLaw(3 /* # control inputs */,
210 214 1 /* # parameters */),
211 215 m_bActive(true),
212 216 m_dGain(1e-6) // must be calibrated for robots in tank
213 217 {
  218 + if(!loadForceFiles(force_file_name_x, force_file_name_y))
  219 + {
  220 + std::cerr << "BelugaBoundaryControlLaw Error: Unable to load force files. Controller will be inactive." << std::endl;
  221 + m_bActive = false;
  222 + }
  223 +}
  224 +
  225 +bool BelugaBoundaryControlLaw::loadForceFiles(const char* force_file_name_x,
  226 + const char* force_file_name_y)
  227 +{
  228 + if(!force_file_name_x)
  229 + {
  230 + std::cerr << "BelugaBoundaryControlLaw::loadForceFiles Error: No X input file name" << std::endl;
  231 + return false;
  232 + }
  233 + if(!force_file_name_y)
  234 + {
  235 + std::cerr << "BelugaBoundaryControlLaw::loadForceFiles Error: No Y input file name" << std::endl;
  236 + return false;
  237 + }
  238 +
214 239 /* read boundary forces from files and store in arrays */
215   - std::ifstream forcesX("Boundary_ForcesX.txt");
216   - std::ifstream forcesY("Boundary_ForcesY.txt");
217   -
218   - while (forcesX.good())
219   - {
220   - for (unsigned int j = 0; j < 2*n+1; j++)
221   - {
222   - for (unsigned int i = 0; i < 2*n+1; i++)
223   - {
224   - forcesX >> FX[i][j];
225   - forcesY >> FY[i][j];
226   - }
227   - }
228   - }
  240 + std::ifstream forcesX(force_file_name_x);
  241 + std::ifstream forcesY(force_file_name_y);
  242 +
  243 + bool load_ok = true;
  244 + if(!forcesX || !forcesX.good())
  245 + {
  246 + std::cerr << "BelugaBoundaryControlLaw::loadForceFiles Error: Could not load X file '" <<
  247 + force_file_name_x << "'" << std::endl;
  248 + load_ok = false;
  249 + }
  250 + if(!forcesY || !forcesY.good())
  251 + {
  252 + std::cerr << "BelugaBoundaryControlLaw::loadForceFiles Error: Could not load Y file '" <<
  253 + force_file_name_y << "'" << std::endl;
  254 + load_ok = false;
  255 + }
  256 + if(!load_ok)
  257 + {
  258 + return false;
  259 + }
  260 +
  261 + double f = 0;
  262 + unsigned int n_loaded = 0;
  263 + for (unsigned int j = 0; forcesX.good() && j < 2*N_FORCE_GRID+1; j++)
  264 + {
  265 + for (unsigned int i = 0; forcesY.good() && i < 2*N_FORCE_GRID+1; i++)
  266 + {
  267 + forcesX >> f;
  268 + FX[i][j] = f;
  269 + forcesY >> f;
  270 + FY[i][j] = f;
  271 + n_loaded++;
  272 + }
  273 + }
229 274
230 275 forcesX.close();
231 276 forcesY.close();
  277 +
  278 + unsigned int n_expected = (2*N_FORCE_GRID + 1)*(2*N_FORCE_GRID + 1);
  279 + if(n_loaded < n_expected)
  280 + {
  281 + std::cerr << "BelugaBoundaryControlLaw::loadForceFiles Error: Only loaded " <<
  282 + n_loaded << " data points. Expected " << n_expected << std::endl;
  283 + return false;
  284 + }
  285 +
  286 + return true;
232 287 }
233 288
234 289 mt_dVector_t BelugaBoundaryControlLaw::doControl(const mt_dVector_t& state,
@@ -255,9 +310,9 @@ mt_dVector_t BelugaBoundaryControlLaw::doControl(const mt_dVector_t& state,
255 310
256 311 /* get boundary force corresponding to grid-point closest to robot */
257 312 double rmax = 1.3*DEFAULT_TANK_RADIUS;
258   - double step = rmax/n;
259   - unsigned int i = MT_CLAMP((floor(x/step + 0.5) + n),0,2*n);
260   - unsigned int j = MT_CLAMP((floor(y/step + 0.5) + n),0,2*n);
  313 + double step = rmax/N_FORCE_GRID;
  314 + unsigned int i = MT_CLAMP((floor(x/step + 0.5) + N_FORCE_GRID),0,2*N_FORCE_GRID);
  315 + unsigned int j = MT_CLAMP((floor(y/step + 0.5) + N_FORCE_GRID),0,2*N_FORCE_GRID);
261 316 double fx = FX[i][j];
262 317 double fy = FY[i][j];
263 318
14 src/BelugaControl.h
@@ -44,7 +44,7 @@ class BelugaLowLevelControlLaw : public mt_ControlLaw
44 44 class BelugaBoundaryControlLaw : public mt_ControlLaw
45 45 {
46 46 public:
47   - BelugaBoundaryControlLaw();
  47 + BelugaBoundaryControlLaw(const char* force_file_name_x, const char* force_file_name_y);
48 48
49 49 mt_dVector_t doControl(const mt_dVector_t& state,
50 50 const mt_dVector_t& u_in);
@@ -53,12 +53,14 @@ class BelugaBoundaryControlLaw : public mt_ControlLaw
53 53
54 54 double m_dGain;
55 55
  56 + bool loadForceFiles(const char* force_file_name_x, const char* force_file_name_y);
  57 +
56 58 protected:
57 59 bool m_bActive;
58 60
59   - static const unsigned int n = 20; // from MATLAB gridding_window.m code
60   - double FX[2*n+1][2*n+1];
61   - double FY[2*n+1][2*n+1];
  61 + static const unsigned int N_FORCE_GRID = 20; // from MATLAB gridding_window.m code
  62 + double FX[2*N_FORCE_GRID+1][2*N_FORCE_GRID+1];
  63 + double FY[2*N_FORCE_GRID+1][2*N_FORCE_GRID+1];
62 64
63 65 static std::string s_sName;
64 66 };
@@ -68,7 +70,9 @@ BelugaWaypointControlLaw* belugaWaypointControlLawFactory(unsigned int bot_num,
68 70 BelugaLowLevelControlLaw* belugaLowLevelControlLawFactory(unsigned int bot_num,
69 71 unsigned int law_num);
70 72 BelugaBoundaryControlLaw* belugaBoundaryControlLawFactory(unsigned int bot_num,
71   - unsigned int law_num);
  73 + unsigned int law_num,
  74 + const char* force_file_name_x,
  75 + const char* force_file_name_y);
72 76
73 77
74 78 #endif // BELUGA_CONTROL_H
19 test/test_Controller.cpp
@@ -5,6 +5,7 @@
5 5
6 6 int testBelugaWaypointControlLaw()
7 7 {
  8 +
8 9 BelugaWaypointControlLaw control_law;
9 10 control_law.doActivate(true);
10 11
@@ -79,7 +80,23 @@ int testBelugaLowLevelControlLaw()
79 80
80 81 int testBelugaBoundaryControlLaw()
81 82 {
82   - BelugaBoundaryControlLaw control_law;
  83 + const char* bad_file_x = "bad_file.txt";
  84 + const char* bad_file_y = "bad_file.txt";
  85 + const char* good_file_x = "../src/Boundary_ForcesX.txt";
  86 + const char* good_file_y = "../src/Boundary_ForcesY.txt";
  87 +
  88 + BelugaBoundaryControlLaw control_law(good_file_x, good_file_y);
  89 +
  90 + /* should fail to load bad files */
  91 + DO_TEST("Checking for failure on loading bad files (x)",
  92 + control_law.loadForceFiles(bad_file_x, good_file_y),
  93 + "Didn't return false for " << bad_file_x);
  94 + DO_TEST("Checking for failure on loading bad files (y)",
  95 + control_law.loadForceFiles(good_file_x, bad_file_y),
  96 + "Didn't return false for " << bad_file_y);
  97 + DO_TEST("Checking for success on loading good files",
  98 + !control_law.loadForceFiles(good_file_x, good_file_y),
  99 + "Unable to load files " << good_file_x << " and " << good_file_y);
83 100
84 101 std::string err_msg;
85 102

Tip: You can add notes to lines in a file. Hover to the left of a line to make a note

Something went wrong with that request. Please try again.