2 // Written and (C) by Francois Fleuret
3 // Contact <francois.fleuret@idiap.ch> for comments & bug reports
5 #include "manipulator.h"
7 static const scalar_t force_break = 200;
8 static const scalar_t force_max = 50;
9 static const scalar_t hand_speed = 5;
11 Manipulator::Manipulator(Task *task) : _touching_polygon(0), _current_polygon(0),
12 _grab_relative_x(0), _grab_relative_y(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;
25 int Manipulator::nb_actions() { return NB_ACTIONS; }
27 void Manipulator::do_action(int action) {
28 scalar_t prev_hand_x =_hand_x, prev_hand_y = _hand_y;
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;
39 case ACTION_MOVE_LEFT:
40 _hand_x -= hand_speed;
42 _hand_x = prev_hand_x + hand_speed;
43 _current_action = ACTION_MOVE_RIGHT;
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;
54 _hand_y -= hand_speed;
56 _hand_y = prev_hand_y + hand_speed;
57 _current_action = ACTION_MOVE_DOWN;
67 cerr << "Unknown action number (" << action << ")" << endl;
72 void Manipulator::update_map() {
73 parameters[FEEL_TOUCHING_OBJECT] = (_touching_polygon ? +1 : -1);
74 parameters[FEEL_GRABBING_OBJECT] = (_current_polygon ? +1 : -1);
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));
81 ASSERT(fx >=0 && fx < continuous_map_size && fy >= 0 && fy < continuous_map_size,
82 "Discrete force sensations out of bounds");
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];
96 void Manipulator::update(scalar_t dt, Universe *universe) {
97 _touching_polygon = universe->pick_polygon(_hand_x, _hand_y);
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);
107 if(_release) _current_polygon = 0;
109 _grab = false; _release = false;
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;
117 scalar_t f = sqrt(sq(_force_x) + sq(_force_y));
119 if(f >= force_break) {
120 _current_polygon = 0;
125 _force_x *= (force_max/f);
126 _force_y *= (force_max/f);
128 _current_polygon->set_speed(0, 0, 0);
129 _current_polygon->apply_force(dt, xf, yf, _force_x, _force_y);
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;
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)) {
148 _current_action = ACTION_MOVE_UP;
151 _current_action = ACTION_MOVE_RIGHT;
154 _current_action = ACTION_MOVE_DOWN;
157 _current_action = ACTION_MOVE_LEFT;
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)) {
169 _current_action = ACTION_MOVE_UP;
172 _current_action = ACTION_MOVE_RIGHT;
175 _current_action = ACTION_MOVE_DOWN;
178 _current_action = ACTION_MOVE_LEFT;
186 return _current_action;
189 int Manipulator::parameter_width() {
190 return 80 + continuous_map_size * sqsize + 1;
193 int Manipulator::parameter_height() {
194 return 6 * sqsize + 2;
197 void Manipulator::draw_parameters(int x0, int y0, SimpleWindow *window) {
200 int wtext = parameter_width() - continuous_map_size * sqsize - 1;
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);
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);
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);
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);
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);
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,
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,
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,
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,
246 void Manipulator::draw_on_universe(SimpleWindow *window) {
247 int xc = int(_hand_x), yc = int(_hand_y);
250 window->color(0.25, 0.25, 0.25);
252 window->draw_line(xc - delta, yc, xc + delta, yc);
253 window->draw_line(xc, yc - delta, xc, yc + delta);
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);
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));
270 // void Manipulator::force_grab(Universe *universe, scalar_t x, scalar_t 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);
282 void Manipulator::force_move(scalar_t x, scalar_t y) {
283 scalar_t prev_hand_x =_hand_x, prev_hand_y = _hand_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;
290 void Manipulator::force_release() {
291 _current_polygon = 0;