Revision 0c8d22a5 src/server/eyelid_motion_generator.cpp
| src/server/eyelid_motion_generator.cpp | ||
|---|---|---|
| 25 | 25 |
* Excellence Initiative. |
| 26 | 26 |
*/ |
| 27 | 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 |
|
| 28 |
#include "humotion/server/eyelid_motion_generator.h" |
|
| 29 |
#include "humotion/server/server.h" |
|
| 30 |
|
|
| 31 |
// using namespace boost; |
|
| 32 |
// using namespace std; |
|
| 33 |
// using namespace humotion; |
|
| 34 |
// using namespace humotion::server; |
|
| 35 |
|
|
| 36 |
using humotion::server::EyelidMotionGenerator; |
|
| 37 |
|
|
| 38 |
// saccade detection threshold in deg/s |
|
| 39 |
const float EyelidMotionGenerator::SACCADE_SPEED_THRESHOLD = 15.0; // deg/s |
|
| 40 |
// eyeblink duration: |
|
| 41 |
const float EyelidMotionGenerator::EYEBLINK_DURATION_MS = 150.0; // ms |
|
| 42 |
// periodic eyeblinks: every 2..10s: |
|
| 43 |
const float EyelidMotionGenerator::EYEBLINK_EYERY_MS_MIN = 2000.0; // ms |
|
| 44 |
const float EyelidMotionGenerator::EYEBLINK_EYERY_MS_MAX = 10000.0; // ms |
|
| 45 |
// how many seconds do we block further blinks after each eyeblink? |
|
| 46 |
const float EyelidMotionGenerator::EYEBLINK_BLOCKING_TIME = 1000.0; // ms |
|
| 45 | 47 |
|
| 46 | 48 |
//! constructor |
| 47 |
EyelidMotionGenerator::EyelidMotionGenerator(JointInterface *j) : EyeMotionGenerator(j){
|
|
| 49 |
EyelidMotionGenerator::EyelidMotionGenerator(JointInterface *j) : EyeMotionGenerator(j) {
|
|
| 48 | 50 |
saccade_blink_active = false; |
| 49 | 51 |
saccade_blink_requested = false; |
| 50 | 52 |
eyelid_closed[LEFT] = false; |
| 51 | 53 |
eyelid_closed[RIGHT] = false; |
| 52 | 54 |
|
| 53 |
eyeblink_blocked_timeout = get_system_time(); |
|
| 55 |
eyeblink_blocked_timeout = boost::get_system_time();
|
|
| 54 | 56 |
} |
| 55 | 57 |
|
| 56 | 58 |
//! destructor |
| 57 |
EyelidMotionGenerator::~EyelidMotionGenerator(){
|
|
| 59 |
EyelidMotionGenerator::~EyelidMotionGenerator() {
|
|
| 58 | 60 |
} |
| 59 | 61 |
|
| 60 | 62 |
|
| 61 | 63 |
//! calculate joint targets |
| 62 | 64 |
//! \TODO: make up motion twice as slow as down motion |
| 63 |
void EyelidMotionGenerator::calculate_targets(){
|
|
| 64 |
//printf("> humotion: calculating eyelid targets\n");
|
|
| 65 |
void EyelidMotionGenerator::calculate_targets() {
|
|
| 66 |
// printf("> humotion: calculating eyelid targets\n");
|
|
| 65 | 67 |
|
| 66 |
//fetch current angles:
|
|
| 68 |
// fetch current angles
|
|
| 67 | 69 |
float eye_tilt_now = get_current_position(JointInterface::ID_EYES_BOTH_UD); |
| 68 | 70 |
|
| 69 |
//calculate left eyelid targets:
|
|
| 71 |
// calculate left eyelid targets
|
|
| 70 | 72 |
float eyelid_upper_left_target = eye_tilt_now + requested_gaze_state.eyelid_opening_upper; |
| 71 | 73 |
float eyelid_lower_left_target = eye_tilt_now - requested_gaze_state.eyelid_opening_lower; |
| 72 | 74 |
|
| 73 |
//calculate right eyelid targets:
|
|
| 75 |
// calculate right eyelid targets
|
|
| 74 | 76 |
float eyelid_upper_right_target = eye_tilt_now + requested_gaze_state.eyelid_opening_upper; |
| 75 | 77 |
float eyelid_lower_right_target = eye_tilt_now - requested_gaze_state.eyelid_opening_lower; |
| 76 | 78 |
|
| 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(JointInterface::ID_EYES_LEFT_LID_UPPER, eyelid_upper_left_target, 0.0); |
|
| 85 |
joint_interface->set_target(JointInterface::ID_EYES_LEFT_LID_LOWER, eyelid_lower_left_target, 0.0); |
|
| 86 |
joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LID_UPPER, eyelid_upper_right_target, 0.0); |
|
| 87 |
joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LID_LOWER, eyelid_lower_right_target, 0.0); |
|
| 88 |
|
|
| 89 |
//check for saccade |
|
| 79 |
// limit target angles |
|
| 80 |
eyelid_upper_left_target = limit_target(JointInterface::ID_EYES_LEFT_LID_UPPER, |
|
| 81 |
eyelid_upper_left_target); |
|
| 82 |
eyelid_lower_left_target = limit_target(JointInterface::ID_EYES_LEFT_LID_LOWER, |
|
| 83 |
eyelid_lower_left_target); |
|
| 84 |
eyelid_upper_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_UPPER, |
|
| 85 |
eyelid_upper_right_target); |
|
| 86 |
eyelid_lower_right_target = limit_target(JointInterface::ID_EYES_RIGHT_LID_LOWER, |
|
| 87 |
eyelid_lower_right_target); |
|
| 88 |
|
|
| 89 |
// (temporarily) store the target |
|
| 90 |
joint_interface->set_target(JointInterface::ID_EYES_LEFT_LID_UPPER, |
|
| 91 |
eyelid_upper_left_target, 0.0); |
|
| 92 |
joint_interface->set_target(JointInterface::ID_EYES_LEFT_LID_LOWER, |
|
| 93 |
eyelid_lower_left_target, 0.0); |
|
| 94 |
joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LID_UPPER, |
|
| 95 |
eyelid_upper_right_target, 0.0); |
|
| 96 |
joint_interface->set_target(JointInterface::ID_EYES_RIGHT_LID_LOWER, |
|
| 97 |
eyelid_lower_right_target, 0.0); |
|
| 98 |
|
|
| 99 |
// check for saccade |
|
| 90 | 100 |
check_for_saccade(); |
| 91 | 101 |
|
| 92 |
//is there a blink request? |
|
| 93 |
start_external_eyeblinks(requested_gaze_state.eyeblink_request_left, requested_gaze_state.eyeblink_request_right); |
|
| 102 |
// is there a blink request? |
|
| 103 |
start_external_eyeblinks(requested_gaze_state.eyeblink_request_left, |
|
| 104 |
requested_gaze_state.eyeblink_request_right); |
|
| 94 | 105 |
|
| 95 |
//do we have a saccade & do we want to exec an eyeblink? |
|
| 106 |
// do we have a saccade & do we want to exec an eyeblink?
|
|
| 96 | 107 |
process_saccadic_eyeblinks(); |
| 97 | 108 |
|
| 98 |
//execute for periodic eyeblinks every ? ms |
|
| 109 |
// execute for periodic eyeblinks every ? ms
|
|
| 99 | 110 |
process_periodic_eyeblinks(); |
| 100 | 111 |
|
| 101 |
//close eyes again after a given timeout |
|
| 112 |
// close eyes again after a given timeout
|
|
| 102 | 113 |
handle_eyeblink_timeout(); |
| 103 | 114 |
|
| 104 |
//eyeblinks override position target (if necessary) |
|
| 115 |
// eyeblinks override position target (if necessary)
|
|
| 105 | 116 |
override_lids_for_eyeblink(); |
| 106 | 117 |
} |
| 107 | 118 |
|
| 108 | 119 |
//! 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 |
if ((duration_left != 0) || (duration_right != 0)){
|
|
| 112 |
eyeblink_blocked_timeout = get_system_time() - posix_time::seconds(100); |
|
| 120 |
void EyelidMotionGenerator::start_external_eyeblinks(int duration_left, int duration_right) {
|
|
| 121 |
// manual eyeblinks will ALWAYs get executed as we use |
|
| 122 |
// a negative block timeout (=timeout has already passed) |
|
| 123 |
if ((duration_left != 0) || (duration_right != 0)) {
|
|
| 124 |
eyeblink_blocked_timeout = boost::get_system_time() - boost::posix_time::seconds(100); |
|
| 113 | 125 |
} |
| 114 | 126 |
|
| 115 | 127 |
|
| 116 |
if (duration_left == 0){
|
|
| 117 |
//nothing to do |
|
| 118 |
}else if (duration_left < 0){
|
|
| 119 |
//infinite sleep -> close |
|
| 128 |
if (duration_left == 0) {
|
|
| 129 |
// nothing to do
|
|
| 130 |
} else if (duration_left < 0) {
|
|
| 131 |
// infinite sleep -> close
|
|
| 120 | 132 |
eyelid_closed[LEFT] = true; |
| 121 |
}else{
|
|
| 122 |
//timeout sleep |
|
| 133 |
} else {
|
|
| 134 |
// timeout sleep
|
|
| 123 | 135 |
start_eyeblink(LEFT, duration_left); |
| 124 | 136 |
eyelid_closed[LEFT] = false; |
| 125 | 137 |
} |
| 126 | 138 |
|
| 127 |
if (duration_right == 0){
|
|
| 128 |
//nothing to do |
|
| 129 |
}else if (duration_right < 0){
|
|
| 130 |
//infinite sleep -> close |
|
| 139 |
if (duration_right == 0) {
|
|
| 140 |
// nothing to do
|
|
| 141 |
} else if (duration_right < 0) {
|
|
| 142 |
// infinite sleep -> close
|
|
| 131 | 143 |
eyelid_closed[RIGHT] = true; |
| 132 |
}else{
|
|
| 133 |
//timeout sleep |
|
| 144 |
} else {
|
|
| 145 |
// timeout sleep
|
|
| 134 | 146 |
start_eyeblink(RIGHT, duration_right); |
| 135 | 147 |
eyelid_closed[RIGHT] = false; |
| 136 | 148 |
} |
| ... | ... | |
| 138 | 150 |
|
| 139 | 151 |
//! process saccadic blink requests |
| 140 | 152 |
//! -> 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");
|
|
| 153 |
void EyelidMotionGenerator::process_saccadic_eyeblinks() {
|
|
| 154 |
if (saccade_blink_requested) {
|
|
| 155 |
// every n-th's saccade requests an eyeblink
|
|
| 156 |
// here: use 0.3 even though humans use bigger prob value (but that looks stupid on robot)
|
|
| 157 |
if ((std::rand()%3) == 0) {
|
|
| 158 |
// printf("> saccadic eyeblink:\n");
|
|
| 147 | 159 |
start_eyeblink(LEFT); |
| 148 | 160 |
start_eyeblink(RIGHT); |
| 149 | 161 |
} |
| ... | ... | |
| 153 | 165 |
|
| 154 | 166 |
//! process periodic blink requests |
| 155 | 167 |
//! -> 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); |
|
| 168 |
void EyelidMotionGenerator::process_periodic_eyeblinks() {
|
|
| 169 |
if (eyeblink_active[LEFT] || eyeblink_active[RIGHT]) {
|
|
| 170 |
// calculate next timeout for a new periodic eyeblink |
|
| 171 |
int milliseconds_to_next_blink = EYEBLINK_EYERY_MS_MIN |
|
| 172 |
+ (std::rand() % static_cast<int>(EYEBLINK_EYERY_MS_MAX-EYEBLINK_EYERY_MS_MIN)); |
|
| 173 |
periodic_blink_start_time = boost::get_system_time() |
|
| 174 |
+ boost::posix_time::milliseconds(milliseconds_to_next_blink); |
|
| 161 | 175 |
} |
| 162 | 176 |
|
| 163 |
if (get_system_time() > periodic_blink_start_time){
|
|
| 164 |
///printf("> periodic eyeblink:\n");
|
|
| 177 |
if (boost::get_system_time() > periodic_blink_start_time) {
|
|
| 178 |
// printf("> periodic eyeblink:\n");
|
|
| 165 | 179 |
start_eyeblink(LEFT); |
| 166 | 180 |
start_eyeblink(RIGHT); |
| 167 | 181 |
} |
| ... | ... | |
| 169 | 183 |
|
| 170 | 184 |
//! handle eyeblink timeouts. |
| 171 | 185 |
//! 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(); |
|
| 186 |
void EyelidMotionGenerator::handle_eyeblink_timeout() {
|
|
| 187 |
boost::system_time now = boost::get_system_time();
|
|
| 174 | 188 |
|
| 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]){
|
|
| 189 |
// take care of re-opening eye timeout
|
|
| 190 |
for (int i=LEFT; i <= RIGHT; i++) {
|
|
| 191 |
if (eyeblink_active[i]) {
|
|
| 192 |
if (now >= eyeblink_timeout[i]) {
|
|
| 179 | 193 |
eyeblink_active[i] = false; |
| 180 | 194 |
} |
| 181 | 195 |
} |
| 182 | 196 |
} |
| 183 | 197 |
|
| 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); |
|
| 198 |
// take care of blocking time |
|
| 199 |
if (eyeblink_active[LEFT] || eyeblink_active[RIGHT]) {
|
|
| 200 |
eyeblink_blocked_timeout = boost::get_system_time() |
|
| 201 |
+ boost::posix_time::milliseconds(EYEBLINK_BLOCKING_TIME); |
|
| 187 | 202 |
} |
| 188 |
|
|
| 189 |
|
|
| 190 | 203 |
} |
| 191 | 204 |
|
| 192 | 205 |
//! override eyelid positions |
| 193 | 206 |
//! 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]){
|
|
| 207 |
void EyelidMotionGenerator::override_lids_for_eyeblink() {
|
|
| 208 |
// close the requested eyelids
|
|
| 209 |
if (eyeblink_active[LEFT] || eyelid_closed[LEFT]) {
|
|
| 197 | 210 |
close_eyelid(JointInterface::ID_EYES_LEFT_LID_UPPER); |
| 198 | 211 |
close_eyelid(JointInterface::ID_EYES_LEFT_LID_LOWER); |
| 199 | 212 |
} |
| 200 |
if(eyeblink_active[RIGHT] || eyelid_closed[RIGHT]){
|
|
| 213 |
if (eyeblink_active[RIGHT] || eyelid_closed[RIGHT]) {
|
|
| 201 | 214 |
close_eyelid(JointInterface::ID_EYES_RIGHT_LID_UPPER); |
| 202 | 215 |
close_eyelid(JointInterface::ID_EYES_RIGHT_LID_LOWER); |
| 203 | 216 |
} |
| ... | ... | |
| 206 | 219 |
|
| 207 | 220 |
|
| 208 | 221 |
//! overwrite the intermediate target to close the given eyelid: |
| 209 |
void EyelidMotionGenerator::close_eyelid(int joint_id){
|
|
| 222 |
void EyelidMotionGenerator::close_eyelid(int joint_id) {
|
|
| 210 | 223 |
float value = 0.0; |
| 211 | 224 |
|
| 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){
|
|
| 225 |
// set upper to minimal allowed value (=as closed as possible) + a small offset
|
|
| 226 |
// set lower to the same value (instead of max) in order to avoid collisions
|
|
| 227 |
switch (joint_id) {
|
|
| 215 | 228 |
default: |
| 216 |
//no eyelid -> return |
|
| 229 |
// no eyelid -> return
|
|
| 217 | 230 |
return; |
| 218 | 231 |
|
| 219 | 232 |
case(JointInterface::ID_EYES_LEFT_LID_UPPER): |
| 220 | 233 |
case(JointInterface::ID_EYES_LEFT_LID_LOWER): |
| 221 |
//use the upper value + 10 deg as close state: |
|
| 234 |
// use the upper value + 10 deg as close state:
|
|
| 222 | 235 |
value = joint_interface->get_joint_min(JointInterface::ID_EYES_LEFT_LID_UPPER) + 10.0; |
| 223 |
//overwrite last target_ |
|
| 236 |
// overwrite last target_
|
|
| 224 | 237 |
joint_interface->set_target(joint_id, value, 0.0); |
| 225 | 238 |
break; |
| 226 | 239 |
|
| 227 | 240 |
case(JointInterface::ID_EYES_RIGHT_LID_UPPER): |
| 228 | 241 |
case(JointInterface::ID_EYES_RIGHT_LID_LOWER): |
| 229 |
//use the upper value + 10 deg as close state: |
|
| 242 |
// use the upper value + 10 deg as close state:
|
|
| 230 | 243 |
value = joint_interface->get_joint_min(JointInterface::ID_EYES_RIGHT_LID_UPPER) + 10.0; |
| 231 |
//overwrite last target_ |
|
| 244 |
// overwrite last target_
|
|
| 232 | 245 |
joint_interface->set_target(joint_id, value, 0.0); |
| 233 | 246 |
break; |
| 234 | 247 |
} |
| ... | ... | |
| 237 | 250 |
//! start an eyeblink request |
| 238 | 251 |
//! this will actually set a flag and a timeout for reopening the given eyelid pair again |
| 239 | 252 |
//! \param int id of side (LEFT or RIGHT) |
| 240 |
void EyelidMotionGenerator::start_eyeblink(int side, int duration){
|
|
| 241 |
//cout << "BLOCKED UNTIL " << eyeblink_blocked_timeout << "\n"; |
|
| 242 |
if (get_system_time() < eyeblink_blocked_timeout){
|
|
| 243 |
//return if we are still in the block time |
|
| 253 |
void EyelidMotionGenerator::start_eyeblink(int side, int duration) {
|
|
| 254 |
// cout << "BLOCKED UNTIL " << eyeblink_blocked_timeout << "\n";
|
|
| 255 |
if (boost::get_system_time() < eyeblink_blocked_timeout) {
|
|
| 256 |
// return if we are still in the block time
|
|
| 244 | 257 |
return; |
| 245 | 258 |
} |
| 246 |
//request for n ms eyeblink: |
|
| 247 |
eyeblink_timeout[side] = get_system_time() + posix_time::milliseconds(duration);
|
|
| 259 |
// request for n ms eyeblink:
|
|
| 260 |
eyeblink_timeout[side] = boost::get_system_time() + boost::posix_time::milliseconds(duration);
|
|
| 248 | 261 |
|
| 249 | 262 |
eyeblink_active[side] = true; |
| 250 | 263 |
} |
| 251 | 264 |
|
| 252 | 265 |
//! 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 |
|
| 266 |
void EyelidMotionGenerator::check_for_saccade() {
|
|
| 267 |
if (get_eye_saccade_active()) {
|
|
| 268 |
// this is a saccade, check if not already in saccade:
|
|
| 269 |
if (!saccade_blink_active) {
|
|
| 270 |
// this is a new saccade! restart blink timer
|
|
| 258 | 271 |
saccade_blink_requested = true; |
| 259 | 272 |
saccade_blink_active = true; |
| 260 | 273 |
} |
| 261 |
}else{
|
|
| 274 |
} else {
|
|
| 262 | 275 |
saccade_blink_active = false; |
| 263 | 276 |
} |
| 264 | 277 |
} |
| 265 | 278 |
|
| 266 | 279 |
|
| 267 | 280 |
//! 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()){
|
|
| 281 |
void EyelidMotionGenerator::publish_targets() {
|
|
| 282 |
// publish values if there is an active gaze input within the last timerange
|
|
| 283 |
if (gaze_target_input_active()) {
|
|
| 271 | 284 |
joint_interface->publish_target(JointInterface::ID_EYES_LEFT_LID_UPPER); |
| 272 | 285 |
joint_interface->publish_target(JointInterface::ID_EYES_LEFT_LID_LOWER); |
| 273 | 286 |
joint_interface->publish_target(JointInterface::ID_EYES_RIGHT_LID_UPPER); |
Also available in: Unified diff