humotion / src / server / eye_motion_generator.cpp @ caf7373f
History | View | Annotate | Download (5.199 KB)
1 |
/*
|
---|---|
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/eye_motion_generator.h" |
29 |
#include "server/server.h" |
30 |
|
31 |
using namespace std; |
32 |
using namespace humotion; |
33 |
using namespace humotion::server; |
34 |
|
35 |
//saccade detection threshold in deg/s
|
36 |
const float EyeMotionGenerator::SACCADE_SPEED_THRESHOLD = 15.0; |
37 |
|
38 |
//! constructor
|
39 |
EyeMotionGenerator::EyeMotionGenerator(JointInterface *j) : GazeMotionGenerator(j, 3, 1.0/Server::MOTION_UPDATERATE){ |
40 |
} |
41 |
|
42 |
|
43 |
//! destructor
|
44 |
EyeMotionGenerator::~EyeMotionGenerator(){ |
45 |
} |
46 |
|
47 |
//! set up an eyemotion profile
|
48 |
//! this will use speed and acceleration calc formulas from literature:
|
49 |
//! \param dof id of joint
|
50 |
//! \param target angle
|
51 |
//! \param current angle
|
52 |
void EyeMotionGenerator::setup_eyemotion(int dof, float target, float now){ |
53 |
//get distance to target:
|
54 |
float distance_abs = fabs(target - now);
|
55 |
//get max speed: factor can be found in encyc britannica: "linear.... being 300° per second for 10° and 500° per second for 30°" (max=700)
|
56 |
float max_speed = fmin(700.0, 10.0*distance_abs + 200.0); |
57 |
//max accel: use data from ""Speed and Accuracy of Saccadic Eye Movements: Characteristics of Impulse Variability in the Oculomotor System"
|
58 |
// http://www-personal.umich.edu/~kornblum/files/journal_exp_psych_HPP_15-3.pdf
|
59 |
// gathered from table2
|
60 |
float max_accel = fmin(80000.0, 1526.53*distance_abs + 10245.4); |
61 |
|
62 |
//feed reflexxes api with data
|
63 |
reflexxes_set_input(dof, target, max_speed, max_accel); |
64 |
} |
65 |
|
66 |
//! calculate joint targets
|
67 |
void EyeMotionGenerator::calculate_targets(){
|
68 |
//use the target values for a faster response
|
69 |
float neck_pan_now = joint_interface->get_target_position(JointInterface::ID_NECK_PAN);
|
70 |
float neck_tilt_now = joint_interface->get_target_position(JointInterface::ID_NECK_TILT);
|
71 |
|
72 |
//calculate target angles for the eyes:
|
73 |
//right eye is dominant -> direct output
|
74 |
float eye_pan_r_target = (requested_gaze_state.pan + requested_gaze_state.vergence/2.0) - (neck_pan_now); |
75 |
|
76 |
//left eye is non dominant -> filtered output: TODO: activate low pass filtered output
|
77 |
//FIXME: USE LOWPASS FILTER HERE --> output_angle[angle_names::EYE_PAN_L] + 0.1 * (eye_pan_l_target - output_angle[angle_names::EYE_PAN_L]) etc;
|
78 |
float eye_pan_l_target = (requested_gaze_state.pan - requested_gaze_state.vergence/2.0) - (neck_pan_now); |
79 |
|
80 |
//tilt:
|
81 |
float eye_tilt_target = requested_gaze_state.tilt - (neck_tilt_now);
|
82 |
|
83 |
if (0){ |
84 |
eye_pan_r_target = 0.0; |
85 |
eye_pan_l_target = 0.0; |
86 |
eye_tilt_target = 0.0; |
87 |
} |
88 |
|
89 |
//check and take care of limits
|
90 |
eye_pan_l_target = limit_target(JointInterface::ID_EYES_LEFT_LR, eye_pan_l_target); |
91 |
eye_pan_r_target = limit_target(JointInterface::ID_EYES_RIGHT_LR, eye_pan_r_target); |
92 |
eye_tilt_target = limit_target(JointInterface::ID_EYES_BOTH_UD, eye_tilt_target); |
93 |
|
94 |
//fetch current angles:
|
95 |
float eye_pan_l_now = get_current_position(JointInterface::ID_EYES_LEFT_LR);
|
96 |
float eye_pan_r_now = get_current_position(JointInterface::ID_EYES_RIGHT_LR);
|
97 |
float eye_tilt_now = get_current_position(JointInterface::ID_EYES_BOTH_UD);
|
98 |
|
99 |
//pass paramaters to reflexxes api:
|
100 |
setup_eyemotion(0, eye_pan_l_target, eye_pan_l_now);
|
101 |
setup_eyemotion(1, eye_pan_r_target, eye_pan_r_now);
|
102 |
setup_eyemotion(2, eye_tilt_target, eye_tilt_now);
|
103 |
|
104 |
//call reflexxes to handle profile calculation:
|
105 |
reflexxes_calculate_profile(); |
106 |
|
107 |
//tell the joint about the new values:
|
108 |
joint_interface->set_target_position(JointInterface::ID_EYES_LEFT_LR, reflexxes_position_output->NewPositionVector->VecData[0]);
|
109 |
joint_interface->set_target_position(JointInterface::ID_EYES_RIGHT_LR, reflexxes_position_output->NewPositionVector->VecData[1]);
|
110 |
joint_interface->set_target_position(JointInterface::ID_EYES_BOTH_UD, reflexxes_position_output->NewPositionVector->VecData[2]);
|
111 |
} |
112 |
|
113 |
|
114 |
//! pubish the calculated targets to the joint subsystem
|
115 |
void EyeMotionGenerator::publish_targets(){
|
116 |
//publish values if there is an active gaze input within the last timerange
|
117 |
if (gaze_target_input_active()){
|
118 |
joint_interface->publish_target_position(JointInterface::ID_EYES_LEFT_LR); |
119 |
joint_interface->publish_target_position(JointInterface::ID_EYES_RIGHT_LR); |
120 |
joint_interface->publish_target_position(JointInterface::ID_EYES_BOTH_UD); |
121 |
} |
122 |
} |