2 ////////////////////////////////////////////////////////////////////////////////
3 // This program is free software; you can redistribute it and/or //
4 // modify it under the terms of the GNU General Public License //
5 // version 2 as published by the Free Software Foundation. //
7 // This program is distributed in the hope that it will be useful, but //
8 // WITHOUT ANY WARRANTY; without even the implied warranty of //
9 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU //
10 // General Public License for more details. //
12 // Written and (C) by François Fleuret //
13 // Contact <francois.fleuret@epfl.ch> for comments & bug reports //
14 ////////////////////////////////////////////////////////////////////////////////
16 #include "manipulator.h"
18 static const scalar_t force_break = 200;
19 static const scalar_t force_max = 50;
20 static const scalar_t hand_speed = 5;
22 Manipulator::Manipulator(Task *task) : _touching_polygon(0), _current_polygon(0),
23 _grab_relative_x(0), _grab_relative_y(0),
25 _hand_x(0), _hand_y(0),
26 _force_x(0), _force_y(0),
27 _grab(false), _release(false),
28 _current_action(ACTION_GRAB) {
29 Map::init(NB_DISCRETE_PARAMETERS + NB_CONTINUOUS_MAPS * continuous_map_size);
30 _xmax = task->width();
31 _ymax = task->height();
32 for(int m = - continuous_map_size; m <= continuous_map_size; m++)
33 _kernel_continuous_maps[m + continuous_map_size] = 2 * exp(-sq(m)/sq(continuous_map_size/9)) - 1;
36 int Manipulator::nb_actions() { return NB_ACTIONS; }
38 void Manipulator::do_action(int action) {
39 scalar_t prev_hand_x =_hand_x, prev_hand_y = _hand_y;
43 case ACTION_MOVE_RIGHT:
44 _hand_x += hand_speed;
45 if(_hand_x >= _xmax) {
46 _hand_x = prev_hand_x - hand_speed;
47 _current_action = ACTION_MOVE_LEFT;
50 case ACTION_MOVE_LEFT:
51 _hand_x -= hand_speed;
53 _hand_x = prev_hand_x + hand_speed;
54 _current_action = ACTION_MOVE_RIGHT;
57 case ACTION_MOVE_DOWN:
58 _hand_y += hand_speed;
59 if(_hand_y >= _ymax) {
60 _hand_y = prev_hand_y - hand_speed;
61 _current_action = ACTION_MOVE_UP;
65 _hand_y -= hand_speed;
67 _hand_y = prev_hand_y + hand_speed;
68 _current_action = ACTION_MOVE_DOWN;
78 cerr << "Unknown action number (" << action << ")" << endl;
83 void Manipulator::update_map() {
84 parameters[FEEL_TOUCHING_OBJECT] = (_touching_polygon ? +1 : -1);
85 parameters[FEEL_GRABBING_OBJECT] = (_current_polygon ? +1 : -1);
87 int px = int(_hand_x * continuous_map_size / _xmax),
88 py = int(_hand_y * continuous_map_size / _ymax),
89 fx = int((continuous_map_size - 1) * (0.5 + 0.5 * _force_x / force_max)),
90 fy = int((continuous_map_size - 1) * (0.5 + 0.5 * _force_y / force_max));
92 ASSERT(fx >=0 && fx < continuous_map_size && fy >= 0 && fy < continuous_map_size,
93 "Discrete force sensations out of bounds");
95 for(int m = 0; m < continuous_map_size; m++) {
96 parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_X * continuous_map_size + m] =
97 _kernel_continuous_maps[px - m + continuous_map_size];
98 parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_Y * continuous_map_size + m] =
99 _kernel_continuous_maps[py - m + continuous_map_size];
100 parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_X * continuous_map_size + m] =
101 _kernel_continuous_maps[fx - m + continuous_map_size];
102 parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_Y * continuous_map_size + m] =
103 _kernel_continuous_maps[fy - m + continuous_map_size];
107 void Manipulator::update(scalar_t dt, Universe *universe) {
108 _touching_polygon = universe->pick_polygon(_hand_x, _hand_y);
110 if(!_current_polygon && _grab) {
111 _current_polygon = _touching_polygon;
112 if(_current_polygon) {
113 _grab_relative_x = _current_polygon->relative_x(_hand_x, _hand_y);
114 _grab_relative_y = _current_polygon->relative_y(_hand_x, _hand_y);
118 if(_release) _current_polygon = 0;
120 _grab = false; _release = false;
122 if(_current_polygon) {
123 scalar_t xf = _current_polygon->absolute_x(_grab_relative_x, _grab_relative_y);
124 scalar_t yf = _current_polygon->absolute_y(_grab_relative_x, _grab_relative_y);
125 _force_x = (_hand_x - xf) * 10;
126 _force_y = (_hand_y - yf) * 10;
128 scalar_t f = sqrt(sq(_force_x) + sq(_force_y));
130 if(f >= force_break) {
131 _current_polygon = false;
136 _force_x *= (force_max/f);
137 _force_y *= (force_max/f);
139 _current_polygon->set_speed(0, 0, 0);
140 _current_polygon->apply_force(dt, xf, yf, _force_x, _force_y);
149 int Manipulator::random_action() {
150 const scalar_t proba_to_grab_when_touching = 0.1;
151 const scalar_t proba_to_change_motion = 0.025;
153 if(_current_polygon) {
154 if(drand48() < 0.01) _current_action = ACTION_RELEASE;
155 else if(_current_action == ACTION_RELEASE || _current_action == ACTION_GRAB ||
156 drand48() < proba_to_change_motion) {
157 switch(int(drand48() * 4)) {
159 _current_action = ACTION_MOVE_UP;
162 _current_action = ACTION_MOVE_RIGHT;
165 _current_action = ACTION_MOVE_DOWN;
168 _current_action = ACTION_MOVE_LEFT;
175 if(_touching_polygon && drand48() < proba_to_grab_when_touching) _current_action = ACTION_GRAB;
176 else if(_current_action == ACTION_RELEASE || _current_action == ACTION_GRAB ||
177 drand48() < proba_to_change_motion) {
178 switch(int(drand48() * 4)) {
180 _current_action = ACTION_MOVE_UP;
183 _current_action = ACTION_MOVE_RIGHT;
186 _current_action = ACTION_MOVE_DOWN;
189 _current_action = ACTION_MOVE_LEFT;
197 return _current_action;
200 int Manipulator::parameter_width() {
201 return 80 + continuous_map_size * sqsize + 1;
204 int Manipulator::parameter_height() {
205 return 6 * sqsize + 2;
208 void Manipulator::draw_parameters(int x0, int y0, SimpleWindow *window) {
211 int wtext = parameter_width() - continuous_map_size * sqsize - 1;
213 window->color(0.8, 0.8, 0.8);
214 window->fill_rectangle(x0 + wtext, y0, continuous_map_size * sqsize + 1, 4 * sqsize + 1);
215 window->draw_text("x-position", x0 + 8, y0 + 0 * sqsize + sqsize - 4);
216 window->draw_text("y-position", x0 + 8, y0 + 1 * sqsize + sqsize - 4);
217 window->draw_text( "x-force", x0 + 8, y0 + 2 * sqsize + sqsize - 4);
218 window->draw_text( "y-force", x0 + 8, y0 + 3 * sqsize + sqsize - 4);
220 window->fill_rectangle(x0 + wtext + 0, y0 + 4 * sqsize, sqsize + 1, sqsize + 1);
221 window->draw_text("grabbing", x0 + 8, y0 + 4 * sqsize + sqsize - 4);
223 window->fill_rectangle(x0 + wtext + 0, y0 + 5 * sqsize, sqsize + 1, sqsize + 1);
224 window->draw_text("touching", x0 + 8, y0 + 5 * sqsize + sqsize - 4);
226 s = parameters[FEEL_GRABBING_OBJECT];
227 window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
228 window->fill_rectangle(x0 + wtext + 1, y0 + 4 * sqsize + 1, sqsize - 1, sqsize - 1);
230 s = parameters[FEEL_TOUCHING_OBJECT];
231 window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
232 window->fill_rectangle(x0 + wtext + 1, y0 + 5 * sqsize + 1, sqsize - 1, sqsize - 1);
234 for(int m = 0; m < continuous_map_size; m++) {
235 s = parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_X * continuous_map_size + m];
236 window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
237 window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 0 * sqsize + 1,
240 s = parameters[NB_DISCRETE_PARAMETERS + MAP_LOCATION_Y * continuous_map_size + m];
241 window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
242 window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 1 * sqsize + 1,
245 s = parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_X * continuous_map_size + m];
246 window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
247 window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 2 * sqsize + 1,
250 s = parameters[NB_DISCRETE_PARAMETERS + MAP_RESISTANCE_Y * continuous_map_size + m];
251 window->color(0.5 * (1+s), 0.5 * (1+s), 0.5 * (1+s));
252 window->fill_rectangle(x0 + wtext + m * sqsize + 1, y0 + 3 * sqsize + 1,
257 void Manipulator::draw_on_universe(SimpleWindow *window) {
258 int xc = int(_hand_x), yc = int(_hand_y);
261 window->color(1.0, 1.0, 1.0);
263 window->draw_line(xc - delta, yc, xc + delta, yc);
264 window->draw_line(xc, yc - delta, xc, yc + delta);
266 if(_current_polygon) {
267 window->draw_line(xc - delta, yc - delta, xc + delta, yc - delta);
268 window->draw_line(xc + delta, yc - delta, xc + delta, yc + delta);
269 window->draw_line(xc + delta, yc + delta, xc - delta, yc + delta);
270 window->draw_line(xc - delta, yc + delta, xc - delta, yc - delta);
273 if(_touching_polygon) {
274 window->draw_line(xc - (delta + 2), yc - (delta + 2), xc + (delta + 2), yc - (delta + 2));
275 window->draw_line(xc + (delta + 2), yc - (delta + 2), xc + (delta + 2), yc + (delta + 2));
276 window->draw_line(xc + (delta + 2), yc + (delta + 2), xc - (delta + 2), yc + (delta + 2));
277 window->draw_line(xc - (delta + 2), yc + (delta + 2), xc - (delta + 2), yc - (delta + 2));
281 // void Manipulator::force_grab(Universe *universe, scalar_t x, scalar_t y) {
284 // _current_polygon = universe->pick_polygon(_hand_x, _hand_y);
285 // if(_current_polygon) {
286 // _grab_relative_x = (_hand_x - _current_polygon->_center_x) * cos(_current_polygon->_theta)
287 // - (_hand_y - _current_polygon->_center_y) * sin(_current_polygon->_theta);
288 // _grab_relative_y = + (_hand_x - _current_polygon->_center_x) * sin(_current_polygon->_theta)
289 // + (_hand_y - _current_polygon->_center_y) * cos(_current_polygon->_theta);
293 void Manipulator::force_move(scalar_t x, scalar_t y) {
294 scalar_t prev_hand_x =_hand_x, prev_hand_y = _hand_y;
297 if(_hand_x < 0 || _hand_x >= _xmax) _hand_x = prev_hand_x;
298 if(_hand_y < 0 || _hand_y >= _ymax) _hand_y = prev_hand_y;
301 void Manipulator::force_release() {
302 _current_polygon = 0;