humotion / src / server / eyelid_motion_generator.cpp @ 8db9ba4f
History | View | Annotate | Download (10.318 KB)
1 | 8c6c1163 | Simon Schulz | /*
|
---|---|---|---|
2 | * This file is part of humotion
|
||
3 | *
|
||
4 | * Copyright(c) sschulz <AT> techfak.uni-bielefeld.de
|
||
5 | * http://opensource.cit-ec.de/projects/humotion
|
||
6 | *
|
||
7 | * This file may be licensed under the terms of the
|
||
8 | * GNU Lesser General Public License Version 3 (the ``LGPL''),
|
||
9 | * or (at your option) any later version.
|
||
10 | *
|
||
11 | * Software distributed under the License is distributed
|
||
12 | * on an ``AS IS'' basis, WITHOUT WARRANTY OF ANY KIND, either
|
||
13 | * express or implied. See the LGPL for the specific language
|
||
14 | * governing rights and limitations.
|
||
15 | *
|
||
16 | * You should have received a copy of the LGPL along with this
|
||
17 | * program. If not, go to http://www.gnu.org/licenses/lgpl.html
|
||
18 | * or write to the Free Software Foundation, Inc.,
|
||
19 | * 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
|
||
20 | *
|
||
21 | * The development of this software was supported by the
|
||
22 | * Excellence Cluster EXC 277 Cognitive Interaction Technology.
|
||
23 | * The Excellence Cluster EXC 277 is a grant of the Deutsche
|
||
24 | * Forschungsgemeinschaft (DFG) in the context of the German
|
||
25 | * Excellence Initiative.
|
||
26 | */
|
||
27 | |||
28 | #include "server/eyelid_motion_generator.h" |
||
29 | #include "server/server.h" |
||
30 | |||
31 | using namespace boost; |
||
32 | using namespace std; |
||
33 | using namespace humotion; |
||
34 | using namespace humotion::server; |
||
35 | |||
36 | //saccade detection threshold in deg/s
|
||
37 | const float EyelidMotionGenerator::SACCADE_SPEED_THRESHOLD = 15.0; //deg/s |
||
38 | //eyeblink duration:
|
||
39 | const float EyelidMotionGenerator::EYEBLINK_DURATION_MS = 150.0;//ms |
||
40 | //periodic eyeblinks: every 2..10s:
|
||
41 | const float EyelidMotionGenerator::EYEBLINK_EYERY_MS_MIN = 2000.0; //ms |
||
42 | const float EyelidMotionGenerator::EYEBLINK_EYERY_MS_MAX = 10000.0; //ms |
||
43 | //how many seconds do we block further blinks after each eyeblink?
|
||
44 | const float EyelidMotionGenerator::EYEBLINK_BLOCKING_TIME = 1000.0; //ms |
||
45 | |||
46 | //! constructor
|
||
47 | EyelidMotionGenerator::EyelidMotionGenerator(JointInterface *j) : EyeMotionGenerator(j){ |
||
48 | saccade_blink_active = false;
|
||
49 | saccade_blink_requested = false;
|
||
50 | eyelid_closed[LEFT] = false;
|
||
51 | eyelid_closed[RIGHT] = false;
|
||
52 | 40dc3e35 | Simon Schulz | |
53 | eyeblink_blocked_timeout = get_system_time(); |
||
54 | 8c6c1163 | Simon Schulz | } |
55 | |||
56 | //! destructor
|
||
57 | EyelidMotionGenerator::~EyelidMotionGenerator(){ |
||
58 | } |
||
59 | |||
60 | |||
61 | //! calculate joint targets
|
||
62 | //! \TODO: make up motion twice as slow as down motion
|
||
63 | void EyelidMotionGenerator::calculate_targets(){
|
||
64 | //printf("> humotion: calculating eyelid targets\n");
|
||
65 | |||
66 | //fetch current angles:
|
||
67 | float eye_tilt_now = get_current_position(JointInterface::ID_EYES_BOTH_UD);
|
||
68 | |||
69 | //calculate left eyelid targets:
|
||
70 | float eyelid_upper_left_target = eye_tilt_now + requested_gaze_state.eyelid_opening_upper;
|
||
71 | float eyelid_lower_left_target = eye_tilt_now - requested_gaze_state.eyelid_opening_lower;
|
||
72 | |||
73 | //calculate right eyelid targets:
|
||
74 | float eyelid_upper_right_target = eye_tilt_now + requested_gaze_state.eyelid_opening_upper;
|
||
75 | float eyelid_lower_right_target = eye_tilt_now - requested_gaze_state.eyelid_opening_lower;
|
||
76 | |||
77 | //limit target angles:
|
||
78 | eyelid_upper_left_target = limit_target(JointInterface::ID_EYES_LEFT_LID_UPPER, eyelid_upper_left_target); |
||
79 | eyelid_lower_left_target = limit_target(JointInterface::ID_EYES_LEFT_LID_LOWER, eyelid_lower_left_target); |
||
80 | eyelid_upper_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_UPPER, eyelid_upper_right_target); |
||
81 | eyelid_lower_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_LOWER, eyelid_lower_right_target); |
||
82 | |||
83 | //(temporarily) store the target
|
||
84 | joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LID_UPPER, eyelid_upper_left_target); |
||
85 | joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LID_LOWER, eyelid_lower_left_target); |
||
86 | joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LID_UPPER, eyelid_upper_right_target); |
||
87 | joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LID_LOWER, eyelid_lower_right_target); |
||
88 | |||
89 | //check for saccade
|
||
90 | check_for_saccade(); |
||
91 | |||
92 | //is there a blink request?
|
||
93 | start_external_eyeblinks(requested_gaze_state.eyeblink_request_left, requested_gaze_state.eyeblink_request_right); |
||
94 | |||
95 | //do we have a saccade & do we want to exec an eyeblink?
|
||
96 | process_saccadic_eyeblinks(); |
||
97 | |||
98 | //execute for periodic eyeblinks every ? ms
|
||
99 | process_periodic_eyeblinks(); |
||
100 | |||
101 | //close eyes again after a given timeout
|
||
102 | handle_eyeblink_timeout(); |
||
103 | |||
104 | //eyeblinks override position target (if necessary)
|
||
105 | override_lids_for_eyeblink(); |
||
106 | } |
||
107 | |||
108 | //! process any external blink requests
|
||
109 | void EyelidMotionGenerator::start_external_eyeblinks(int duration_left, int duration_right){ |
||
110 | //manual eyeblinks will ALWAYs get executed as we use a negative block timeout (=timout has already passed)
|
||
111 | 9c7ef5fc | Simon Schulz | if ((duration_left != 0) || (duration_right != 0)){ |
112 | eyeblink_blocked_timeout = get_system_time() - posix_time::seconds(100);
|
||
113 | } |
||
114 | |||
115 | 8c6c1163 | Simon Schulz | |
116 | if (duration_left == 0){ |
||
117 | //nothing to do
|
||
118 | }else if (duration_left < 0){ |
||
119 | //infinite sleep -> close
|
||
120 | eyelid_closed[LEFT] = true;
|
||
121 | }else{
|
||
122 | //timeout sleep
|
||
123 | start_eyeblink(LEFT, duration_left); |
||
124 | eyelid_closed[LEFT] = false;
|
||
125 | } |
||
126 | |||
127 | if (duration_right == 0){ |
||
128 | //nothing to do
|
||
129 | }else if (duration_right < 0){ |
||
130 | //infinite sleep -> close
|
||
131 | eyelid_closed[RIGHT] = true;
|
||
132 | }else{
|
||
133 | //timeout sleep
|
||
134 | start_eyeblink(RIGHT, duration_right); |
||
135 | eyelid_closed[RIGHT] = false;
|
||
136 | } |
||
137 | } |
||
138 | |||
139 | //! process saccadic blink requests
|
||
140 | //! -> when we do a saccade, the chances to blink are much higher
|
||
141 | void EyelidMotionGenerator::process_saccadic_eyeblinks(){
|
||
142 | if (saccade_blink_requested){
|
||
143 | //every n-th's saccade requests an eyeblink
|
||
144 | //here: use 0.3 even though humans use bigger prob value (but that looks stupid on robot)
|
||
145 | if ((rand()%3) == 0){ |
||
146 | ///printf("> saccadic eyeblink:\n");
|
||
147 | start_eyeblink(LEFT); |
||
148 | start_eyeblink(RIGHT); |
||
149 | } |
||
150 | saccade_blink_requested = false;
|
||
151 | } |
||
152 | } |
||
153 | |||
154 | //! process periodic blink requests
|
||
155 | //! -> we want to have an eyeblink every n...m seconds
|
||
156 | void EyelidMotionGenerator::process_periodic_eyeblinks(){
|
||
157 | if((eyeblink_active[LEFT]) || eyeblink_active[RIGHT]){
|
||
158 | //calculate next timeout for a new periodic eyeblink
|
||
159 | int milliseconds_to_next_blink = EYEBLINK_EYERY_MS_MIN + (rand()%(int)(EYEBLINK_EYERY_MS_MAX-EYEBLINK_EYERY_MS_MIN)); |
||
160 | periodic_blink_start_time = get_system_time() + posix_time::milliseconds(milliseconds_to_next_blink); |
||
161 | } |
||
162 | |||
163 | if (get_system_time() > periodic_blink_start_time){
|
||
164 | ///printf("> periodic eyeblink:\n");
|
||
165 | start_eyeblink(LEFT); |
||
166 | start_eyeblink(RIGHT); |
||
167 | } |
||
168 | } |
||
169 | |||
170 | //! handle eyeblink timeouts.
|
||
171 | //! this function will actually re-open the eyes after a predefined time
|
||
172 | void EyelidMotionGenerator::handle_eyeblink_timeout(){
|
||
173 | boost::system_time now = get_system_time(); |
||
174 | |||
175 | //take care of re-opening eye timeout:
|
||
176 | for(int i=LEFT; i<=RIGHT; i++){ |
||
177 | if (eyeblink_active[i]){
|
||
178 | if (now >= eyeblink_timeout[i]){
|
||
179 | eyeblink_active[i] = false;
|
||
180 | } |
||
181 | } |
||
182 | } |
||
183 | |||
184 | //take care of blocking time
|
||
185 | if (eyeblink_active[LEFT] || eyeblink_active[RIGHT]){
|
||
186 | eyeblink_blocked_timeout = get_system_time() + posix_time::milliseconds(EYEBLINK_BLOCKING_TIME); |
||
187 | } |
||
188 | |||
189 | |||
190 | } |
||
191 | |||
192 | //! override eyelid positions
|
||
193 | //! this will override (=close) the eyelids for all possible eyeblink requests
|
||
194 | void EyelidMotionGenerator::override_lids_for_eyeblink(){
|
||
195 | //close the requested eyelids
|
||
196 | if (eyeblink_active[LEFT] || eyelid_closed[LEFT]){
|
||
197 | close_eyelid(JointInterface::ID_EYES_LEFT_LID_UPPER); |
||
198 | close_eyelid(JointInterface::ID_EYES_LEFT_LID_LOWER); |
||
199 | } |
||
200 | if(eyeblink_active[RIGHT] || eyelid_closed[RIGHT]){
|
||
201 | close_eyelid(JointInterface::ID_EYES_RIGHT_LID_UPPER); |
||
202 | close_eyelid(JointInterface::ID_EYES_RIGHT_LID_LOWER); |
||
203 | } |
||
204 | } |
||
205 | |||
206 | |||
207 | |||
208 | //! overwrite the intermediate target to close the given eyelid:
|
||
209 | void EyelidMotionGenerator::close_eyelid(int joint_id){ |
||
210 | float value = 0.0; |
||
211 | |||
212 | //set upper to minimal allowed value (=as closed as possible) + a small offset
|
||
213 | //set lower to the same value (instead of max) in order to avoid collisions
|
||
214 | switch(joint_id){
|
||
215 | default:
|
||
216 | //no eyelid -> return
|
||
217 | return;
|
||
218 | |||
219 | case(JointInterface::ID_EYES_LEFT_LID_UPPER):
|
||
220 | case(JointInterface::ID_EYES_LEFT_LID_LOWER):
|
||
221 | //use the upper value + 10 deg as close state:
|
||
222 | value = joint_interface->get_joint_min(JointInterface::ID_EYES_LEFT_LID_UPPER) + 10.0; |
||
223 | //overwrite last target_
|
||
224 | joint_interface->set_target_position(joint_id, value); |
||
225 | break;
|
||
226 | |||
227 | case(JointInterface::ID_EYES_RIGHT_LID_UPPER):
|
||
228 | case(JointInterface::ID_EYES_RIGHT_LID_LOWER):
|
||
229 | //use the upper value + 10 deg as close state:
|
||
230 | value = joint_interface->get_joint_min(JointInterface::ID_EYES_RIGHT_LID_UPPER) + 10.0; |
||
231 | //overwrite last target_
|
||
232 | joint_interface->set_target_position(joint_id, value); |
||
233 | break;
|
||
234 | } |
||
235 | } |
||
236 | |||
237 | //! start an eyeblink request
|
||
238 | //! this will actually set a flag and a timeout for reopening the given eyelid pair again
|
||
239 | //! \param int id of side (LEFT or RIGHT)
|
||
240 | void EyelidMotionGenerator::start_eyeblink(int side, int duration){ |
||
241 | 40dc3e35 | Simon Schulz | //cout << "BLOCKED UNTIL " << eyeblink_blocked_timeout << "\n";
|
242 | 8c6c1163 | Simon Schulz | if (get_system_time() < eyeblink_blocked_timeout){
|
243 | //return if we are still in the block time
|
||
244 | return;
|
||
245 | } |
||
246 | //request for n ms eyeblink:
|
||
247 | eyeblink_timeout[side] = get_system_time() + posix_time::milliseconds(duration); |
||
248 | |||
249 | eyeblink_active[side] = true;
|
||
250 | } |
||
251 | |||
252 | //! check if there is an ongoing saccade:
|
||
253 | void EyelidMotionGenerator::check_for_saccade(){
|
||
254 | if (get_eye_saccade_active()){
|
||
255 | //this is a saccade, check if not already in saccade:
|
||
256 | if (!saccade_blink_active){
|
||
257 | //this is a new saccade! restart blink timer
|
||
258 | saccade_blink_requested = true;
|
||
259 | saccade_blink_active = true;
|
||
260 | } |
||
261 | }else{
|
||
262 | saccade_blink_active = false;
|
||
263 | } |
||
264 | } |
||
265 | |||
266 | |||
267 | //! publish targets to motor boards:
|
||
268 | void EyelidMotionGenerator::publish_targets(){
|
||
269 | //publish values if there is an active gaze input within the last timerange
|
||
270 | if (gaze_target_input_active()){
|
||
271 | joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_LID_UPPER); |
||
272 | joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_LID_LOWER); |
||
273 | joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_LID_UPPER); |
||
274 | joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_LID_LOWER); |
||
275 | } |
||
276 | } |