manipulator.cc

   1
   2 // Written and (C) by Francois Fleuret
   3 // Contact <francois.fleuret@idiap.ch> for comments & bug reports
   4
   5 #include "manipulator.h"
   6
   7 static const scalar_t force_break = 200;
   8 static const scalar_t force_max = 50;
   9 static const scalar_t hand_speed = 5;
  10
  11 Manipulator::Manipulator(Task *task) : _touching_polygon(0), _current_polygon(0),
  12                                        _grab_relative_x(0), _grab_relative_y(0),
  13                                        _xmax(0), _ymax(0),
  14                                        _hand_x(0), _hand_y(0),
  15                                        _force_x(0), _force_y(0),
  16                                        _grab(false), _release(false),
  17                                        _current_action(ACTION_GRAB) {
  18   Map::init(NB_DISCRETE_PARAMETERS + NB_CONTINUOUS_MAPS * continuous_map_size);
  19   _xmax = task->width();
  20   _ymax = task->height();
  21   for(int m = - continuous_map_size; m <= continuous_map_size; m++)
  22     _kernel_continuous_maps[m + continuous_map_size] = 2 * exp(-sq(m)/sq(continuous_map_size/9)) - 1;
  23 }
  24
  25 int Manipulator::nb_actions() { return NB_ACTIONS; }
  26
  27 void Manipulator::do_action(int action) {
  28   scalar_t prev_hand_x  =_hand_x, prev_hand_y = _hand_y;
  29   switch(action) {
  30   case ACTION_IDLE:
  31     break;
  32   case ACTION_MOVE_RIGHT:
  33     _hand_x += hand_speed;
  34     if(_hand_x >= _xmax) {
  35       _hand_x = prev_hand_x - hand_speed;
  36       _current_action = ACTION_MOVE_LEFT;
  37     }
  38     break;
  39   case ACTION_MOVE_LEFT:
  40     _hand_x -= hand_speed;
  41     if(_hand_x < 0) {
  42       _hand_x = prev_hand_x + hand_speed;
  43       _current_action = ACTION_MOVE_RIGHT;
  44     }
  45     break;
  46   case ACTION_MOVE_DOWN:
  47     _hand_y += hand_speed;
  48     if(_hand_y >= _ymax) {
  49       _hand_y = prev_hand_y - hand_speed;
  50       _current_action = ACTION_MOVE_UP;
  51     }
  52     break;
  53   case ACTION_MOVE_UP:
  54     _hand_y -= hand_speed;
  55     if(_hand_y < 0) {
  56       _hand_y = prev_hand_y + hand_speed;
  57       _current_action = ACTION_MOVE_DOWN;
  58     }
  59     break;
  60   case ACTION_GRAB:
  61     _grab = true;
  62     break;
  63   case ACTION_RELEASE:
  64     _release = true;
  65     break;
  66   default:
  67     cerr << "Unknown action number (" << action << ")" << endl;
  68     abort();
  69   }
  70 }
  71
  72 void Manipulator::update_map() {
  73   parameters[FEEL_TOUCHING_OBJECT] = (_touching_polygon ? +1 : -1);
  74   parameters[FEEL_GRABBING_OBJECT] = (_current_polygon ? +1 : -1);
  75
  76   int px = int(_hand_x * continuous_map_size / _xmax),
  77     py = int(_hand_y * continuous_map_size / _ymax),
  78     fx = int((continuous_map_size - 1) * (0.5 + 0.5 * _force_x / force_max)),
  79     fy = int((continuous_map_size - 1) * (0.5 + 0.5 * _force_y / force_max));
  80
  81   ASSERT(fx >=0 && fx < continuous_map_size && fy >= 0 && fy < continuous_map_size,
  82          "Discrete force sensations out of bounds");
  83
  84   for(int m = 0; m < continuous_map_size; m++) {
  85     parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_X * continuous_map_size + m] =
  86       _kernel_continuous_maps[px - m + continuous_map_size];
  87     parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_Y * continuous_map_size + m] =
  88       _kernel_continuous_maps[py - m + continuous_map_size];
  89     parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_X * continuous_map_size + m] =
  90       _kernel_continuous_maps[fx - m + continuous_map_size];
  91     parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_Y * continuous_map_size + m] =
  92       _kernel_continuous_maps[fy - m + continuous_map_size];
  93   }
  94 }
  95
  96 void Manipulator::update(scalar_t dt, Universe *universe) {
  97   _touching_polygon = universe->pick_polygon(_hand_x, _hand_y);
  98
  99   if(!_current_polygon && _grab) {
 100     _current_polygon = _touching_polygon;
 101     if(_current_polygon) {
 102       _grab_relative_x = _current_polygon->relative_x(_hand_x, _hand_y);
 103       _grab_relative_y = _current_polygon->relative_y(_hand_x, _hand_y);
 104     }
 105   }
 106
 107   if(_release) _current_polygon = 0;
 108
 109   _grab = false; _release = false;
 110
 111   if(_current_polygon) {
 112     scalar_t xf = _current_polygon->absolute_x(_grab_relative_x, _grab_relative_y);
 113     scalar_t yf = _current_polygon->absolute_y(_grab_relative_x, _grab_relative_y);
 114     _force_x = (_hand_x - xf) * 10;
 115     _force_y = (_hand_y - yf) * 10;
 116
 117     scalar_t f = sqrt(sq(_force_x) + sq(_force_y));
 118
 119     if(f >= force_break) {
 120       _current_polygon = 0;
 121       _force_x = 0;
 122       _force_y = 0;
 123     } else {
 124       if(f >= force_max) {
 125         _force_x *= (force_max/f);
 126         _force_y *= (force_max/f);
 127       }
 128       _current_polygon->set_speed(0, 0, 0);
 129       _current_polygon->apply_force(dt, xf, yf, _force_x, _force_y);
 130     }
 131
 132   } else {
 133     _force_x = 0;
 134     _force_y = 0;
 135   }
 136 }
 137
 138 int Manipulator::random_action() {
 139   const scalar_t proba_to_grab_when_touching = 0.1;
 140   const scalar_t proba_to_change_motion = 0.025;
 141
 142   if(_current_polygon) {
 143     if(drand48() < 0.01) _current_action = ACTION_RELEASE;
 144     else if(_current_action == ACTION_RELEASE || _current_action == ACTION_GRAB ||
 145             drand48() < proba_to_change_motion) {
 146       switch(int(drand48() * 4)) {
 147       case 0:
 148         _current_action = ACTION_MOVE_UP;
 149         break;
 150       case 1:
 151         _current_action = ACTION_MOVE_RIGHT;
 152         break;
 153       case 2:
 154         _current_action = ACTION_MOVE_DOWN;
 155         break;
 156       case 3:
 157         _current_action = ACTION_MOVE_LEFT;
 158         break;
 159       default:
 160         abort();
 161       }
 162     }
 163   } else {
 164     if(_touching_polygon && drand48() < proba_to_grab_when_touching) _current_action = ACTION_GRAB;
 165     else if(_current_action == ACTION_RELEASE || _current_action == ACTION_GRAB ||
 166             drand48() < proba_to_change_motion) {
 167       switch(int(drand48() * 4)) {
 168       case 0:
 169         _current_action = ACTION_MOVE_UP;
 170         break;
 171       case 1:
 172         _current_action = ACTION_MOVE_RIGHT;
 173         break;
 174       case 2:
 175         _current_action = ACTION_MOVE_DOWN;
 176         break;
 177       case 3:
 178         _current_action = ACTION_MOVE_LEFT;
 179         break;
 180       default:
 181         abort();
 182       }
 183     }
 184   }
 185
 186   return _current_action;
 187 }
 188
 189 int Manipulator::parameter_width() {
 190   return 80 + continuous_map_size * sqsize + 1;
 191 }
 192
 193 int Manipulator::parameter_height() {
 194   return 6 * sqsize + 2;
 195 }
 196
 197 void Manipulator::draw_parameters(int x0, int y0, SimpleWindow *window) {
 198   scalar_t s;
 199
 200   int wtext = parameter_width() - continuous_map_size * sqsize - 1;
 201
 202   window->color(0.8, 0.8, 0.8);
 203   window->fill_rectangle(x0 + wtext, y0, continuous_map_size * sqsize + 1, 4 * sqsize + 1);
 204   window->draw_text("x-position", x0 + 8, y0 + 0 * sqsize + sqsize - 4);
 205   window->draw_text("y-position", x0 + 8, y0 + 1 * sqsize + sqsize - 4);
 206   window->draw_text(   "x-force", x0 + 8, y0 + 2 * sqsize + sqsize - 4);
 207   window->draw_text(   "y-force", x0 + 8, y0 + 3 * sqsize + sqsize - 4);
 208
 209   window->fill_rectangle(x0 + wtext + 0, y0 + 4 * sqsize, sqsize + 1, sqsize + 1);
 210   window->draw_text("grabbing", x0 + 8, y0 + 4 * sqsize + sqsize - 4);
 211
 212   window->fill_rectangle(x0 + wtext + 0, y0 + 5 * sqsize, sqsize + 1, sqsize + 1);
 213   window->draw_text("touching", x0 + 8, y0 + 5 * sqsize + sqsize - 4);
 214
 215   s = parameters[FEEL_GRABBING_OBJECT];
 216   window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 217   window->fill_rectangle(x0 + wtext + 1, y0 + 4 * sqsize + 1, sqsize - 1, sqsize - 1);
 218
 219   s = parameters[FEEL_TOUCHING_OBJECT];
 220   window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 221   window->fill_rectangle(x0 + wtext + 1, y0 + 5 * sqsize + 1, sqsize - 1, sqsize - 1);
 222
 223   for(int m = 0; m < continuous_map_size; m++) {
 224     s = parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_X * continuous_map_size + m];
 225     window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 226     window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 0 * sqsize + 1,
 227                            sqsize-1, sqsize-1);
 228
 229     s = parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_Y * continuous_map_size + m];
 230     window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 231     window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 1 * sqsize + 1,
 232                            sqsize-1, sqsize-1);
 233
 234     s = parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_X * continuous_map_size + m];
 235     window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 236     window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 2 * sqsize + 1,
 237                            sqsize-1, sqsize-1);
 238
 239     s = parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_Y * continuous_map_size + m];
 240     window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
 241     window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 3 * sqsize + 1,
 242                            sqsize-1, sqsize-1);
 243   }
 244 }
 245
 246 void Manipulator::draw_on_universe(SimpleWindow *window) {
 247   int xc = int(_hand_x), yc = int(_hand_y);
 248   const int delta = 5;
 249
 250   window->color(0.25, 0.25, 0.25);
 251
 252   window->draw_line(xc - delta, yc, xc + delta, yc);
 253   window->draw_line(xc, yc - delta, xc, yc + delta);
 254
 255   if(_current_polygon) {
 256     window->draw_line(xc - delta, yc - delta, xc + delta, yc - delta);
 257     window->draw_line(xc + delta, yc - delta, xc + delta, yc + delta);
 258     window->draw_line(xc + delta, yc + delta, xc - delta, yc + delta);
 259     window->draw_line(xc - delta, yc + delta, xc - delta, yc - delta);
 260   }
 261
 262   if(_touching_polygon) {
 263     window->draw_line(xc - (delta + 2), yc - (delta + 2), xc + (delta + 2), yc - (delta + 2));
 264     window->draw_line(xc + (delta + 2), yc - (delta + 2), xc + (delta + 2), yc + (delta + 2));
 265     window->draw_line(xc + (delta + 2), yc + (delta + 2), xc - (delta + 2), yc + (delta + 2));
 266     window->draw_line(xc - (delta + 2), yc + (delta + 2), xc - (delta + 2), yc - (delta + 2));
 267   }
 268 }
 269
 270 // void Manipulator::force_grab(Universe *universe, scalar_t x, scalar_t y) {
 271 //   _hand_x = x;
 272 //   _hand_y = y;
 273 //   _current_polygon = universe->pick_polygon(_hand_x, _hand_y);
 274 //   if(_current_polygon) {
 275 //     _grab_relative_x = (_hand_x - _current_polygon->_center_x) * cos(_current_polygon->_theta)
 276 //       - (_hand_y - _current_polygon->_center_y) * sin(_current_polygon->_theta);
 277 //     _grab_relative_y = + (_hand_x - _current_polygon->_center_x) * sin(_current_polygon->_theta)
 278 //       + (_hand_y - _current_polygon->_center_y) * cos(_current_polygon->_theta);
 279 //   }
 280 // }
 281
 282 void Manipulator::force_move(scalar_t x, scalar_t y) {
 283   scalar_t prev_hand_x  =_hand_x, prev_hand_y = _hand_y;
 284   _hand_x = x;
 285   _hand_y = y;
 286   if(_hand_x < 0 || _hand_x >= _xmax) _hand_x = prev_hand_x;
 287   if(_hand_y < 0 || _hand_y >= _ymax) _hand_y = prev_hand_y;
 288 }
 289
 290 void Manipulator::force_release() {
 291   _current_polygon = 0;
 292   _force_x = 0.0;
 293   _force_y = 0.0;
 294 }