Statistics
| Branch: | Tag: | Revision:

humotion / examples / yarp_icub / src / icub_faceinterface.cpp @ f95312df

History | View | Annotate | Download (8.58 KB)

1 6a2d467f 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 "humotion_yarp_icub/icub_faceinterface.h"
29
30 a5cc61d4 sschulz
#include <boost/format.hpp>
31 07e68eb7 sschulz
#include <boost/lexical_cast.hpp>
32 a5cc61d4 sschulz
33 6a2d467f Simon Schulz
#include <algorithm>
34
#include <string>
35
36
using yarp::os::Network;
37
using yarp::os::Bottle;
38
using std::cout;
39
using std::cerr;
40
using std::string;
41
42
//! constructor
43
iCubFaceInterface::iCubFaceInterface(std::string _scope) {
44
    scope = _scope;
45
46
    // attach to facial expressions:
47
    std::string emotion_scope = scope + "/face/raw/in";
48
    cout << "opening connection to '"<< emotion_scope << "'\n";
49
50 07e68eb7 sschulz
    bool init_ok = true;
51 6a2d467f Simon Schulz
    for (int i = 0; i < 4; i++) {
52
        // strange, if we use one output port only the first command is executed?! flushing issues?
53 07e68eb7 sschulz
        std::string emotion_port_out = "/emotionwriter" + boost::lexical_cast<std::string>(i);
54 6a2d467f Simon Schulz
        if (!emotion_port[i].open(emotion_port_out.c_str())) {
55
            cerr << "ERROR: failed to open emotion port '" << emotion_port_out << "'\n";
56 07e68eb7 sschulz
            init_ok = false;
57 6a2d467f Simon Schulz
        }
58
        if (!Network::connect(emotion_port_out.c_str(), emotion_scope.c_str())) {
59
            cerr << "ERROR: failed to connect to emotion port '" << emotion_port_out << "'\n";
60 07e68eb7 sschulz
            init_ok = false;
61 6a2d467f Simon Schulz
        }
62
    }
63 07e68eb7 sschulz
64
    if (!init_ok) {
65
        cerr << "ERROR: failed to set up emotion component\n";
66
        cerr << "       please make sure that the faceExpressions yarpdev is started\n";
67
        cerr << "       (e.g.  yarpdev --name /icub/face/raw --device serial ... is running)\n";
68
        exit(EXIT_FAILURE);
69
    }
70 6a2d467f Simon Schulz
}
71
72
//! destructor
73
iCubFaceInterface::~iCubFaceInterface() {
74
}
75
76
//! special command to set eyelid angle
77
//! \param angle in degrees
78
void iCubFaceInterface::set_eyelid_angle(float angle) {
79
    if (emotion_port[0].getOutputCount() > 0) {
80
        // try to set the value based on the upper one
81
        // some guesses from the sim: S30 = 0° / S40 = 10°
82
        int opening = (25.0 + 0.8*angle);
83
        opening = std::min(48, std::max(24, opening));
84
85
        if (opening == lid_opening_previous) {
86
            // no update necessary
87
            return;
88
        }
89
90
        lid_angle = angle;
91
        lid_opening_previous = opening;
92
93
        char buf[20];
94
        snprintf(buf, sizeof(buf), "S%2d", opening);
95
96
        // cout << "SETTING EYELID '" << buf << "' (" << angle << " -> " << opening << "\n";
97
        Bottle &cmd = emotion_port[0].prepare();
98
        cmd.clear();
99
        cmd.addString(buf);
100
        emotion_port[0].writeStrict();
101
    } else {
102
        cerr << "ERROR: no icub emotion output\n";
103
        exit(EXIT_FAILURE);
104
    }
105
}
106
107
//! special command to set the eyebrow angle
108
//! \param id {0=left, 1=right)
109
//! \param angle in degrees
110
void iCubFaceInterface::set_eyebrow_angle(int id, float *target_angle) {
111
    int port_id;
112
    if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW) {
113
        port_id = 1;
114
    } else {
115
        port_id = 2;
116
    }
117
118
    if (emotion_port[port_id].getOutputCount() > 0) {
119
        double angle = target_angle[id];
120
        int icub_val = 0;
121
122
        // swap rotation direction for eyebrow
123
        if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW) {
124
            angle = -angle;
125
        }
126
127
        // convert to icub representation
128
        if (angle < -20) {
129
            icub_val = 1;
130
        } else if (angle < 10) {
131
            icub_val = 2;
132
        } else if (angle < 20) {
133
            icub_val = 4;
134
        } else {
135
            icub_val = 8;
136
        }
137
138
        // make sure to update only on new values
139
        if (icub_val == target_angle_previous[id]) {
140
                // no updata necessary
141
                return;
142
        }
143
144
        // store actual value
145
        target_angle_previous[id] = icub_val;
146
147
148
        std::string cmd_s;
149
        if (id == iCubJointInterface::ICUB_ID_EYES_LEFT_BROW) {
150 07e68eb7 sschulz
            cmd_s = "L0" + boost::lexical_cast<std::string>(icub_val);
151 6a2d467f Simon Schulz
        } else {
152 07e68eb7 sschulz
            cmd_s = "R0" + boost::lexical_cast<std::string>(icub_val);
153 6a2d467f Simon Schulz
        }
154
155
        cout << "SETTING EYEBROW " << id << " (" << angle << " -> " << cmd_s << ")\n";
156
157
        Bottle &cmd = emotion_port[port_id].prepare();
158
        cmd.clear();
159
        cmd.addString(cmd_s);
160
        emotion_port[port_id].writeStrict();
161
    } else {
162
        cerr << "ERROR: no icub emotion output\n";
163
        exit(EXIT_FAILURE);
164
    }
165
}
166
167
void iCubFaceInterface::set_mouth(float *target_angle) {
168
    // convert from 6DOF mouth displacement to icub leds:
169
    int led_value = 0;
170
171
    // fetch center opening
172
    double center_opening = target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_LOWER] -
173
            target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_UPPER];
174
    bool mouth_open = (center_opening > 15.0) ? true : false;
175
176
    // side of mouth high or low?
177
    double center_avg = (target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_LOWER] +
178
            target_angle[iCubJointInterface::ICUB_ID_LIP_CENTER_UPPER])/2.0;
179
    double left_avg   = (target_angle[iCubJointInterface::ICUB_ID_LIP_LEFT_LOWER] +
180
            target_angle[iCubJointInterface::ICUB_ID_LIP_LEFT_UPPER])/2.0;
181
    double right_avg  = (target_angle[iCubJointInterface::ICUB_ID_LIP_RIGHT_LOWER] +
182
            target_angle[iCubJointInterface::ICUB_ID_LIP_RIGHT_UPPER])/2.0;
183
184
    // happy, neutral or sad?
185
    double diff_l = center_avg - left_avg;
186
    double diff_r = center_avg - right_avg;
187
    double diff   = (diff_l+diff_r)/2.0;
188
189
    if (diff > 2.0) {
190
        if (mouth_open) {
191
            led_value = 0x14;
192
        } else {
193
            if (diff > 2.6) {
194
                led_value = 0x0A;
195
            } else {
196
                led_value = 0x0B;
197
            }
198
        }
199
    } else if (diff < -3.0) {
200
        if (mouth_open) {
201
            led_value = 0x06;
202
        } else {
203
            led_value = 0x18;
204
        }
205
    } else if (diff < -2.0) {
206
        if (mouth_open) {
207
            led_value = 0x04;  // 0x25;
208
        } else {
209
            led_value = 0x08;
210
        }
211
    } else {
212
        if (mouth_open) {
213
            led_value = 0x16;
214
        } else {
215
            led_value = 0x08;
216
        }
217
    }
218
219
220
    if (led_value == previous_mouth_state) {
221
        // no update necessary
222
        return;
223
    }
224
225
    previous_mouth_state = led_value;
226
227
    // convert to string
228
    char buf[10];
229
    snprintf(buf, sizeof(buf), "M%02X", led_value);
230
231
    /*
232
    cout << "sending mouth " << buf << "\n";
233
    cout << boost::format("  mouth angles: %3.2f %3.2f %3.2f\n")
234
            % target_angle[ICUB_ID_LIP_LEFT_UPPER]
235
            % target_angle[ICUB_ID_LIP_CENTER_UPPER]
236
            % target_angle[ICUB_ID_LIP_RIGHT_UPPER];
237
    cout << boost::format("  mouth         %3.2f %3.2f %3.2f\n")
238
            % target_angle[ICUB_ID_LIP_LEFT_LOWER]
239
            % target_angle[ICUB_ID_LIP_CENTER_LOWER]
240
            % target_angle[ICUB_ID_LIP_RIGHT_LOWER];
241
    cout << boost::format("  mouth  open=%3.2f diff=%3.2f\n")
242
            % center_opening
243
            % diff;
244
    */
245
246
    // add mouth
247
    Bottle &cmd = emotion_port[3].prepare();
248
    cmd.clear();
249
    cmd.addString(buf);
250
    emotion_port[3].writeStrict();
251
252
/*
253
    //store joint values which we do not handle on icub here:
254
    double timestamp = get_timestamp_ms();
255
    JointInterface::store_incoming_position(ID_LIP_LEFT_UPPER,   target_angle[ICUB_ID_LIP_LEFT_UPPER], timestamp);
256
    JointInterface::store_incoming_position(ID_LIP_LEFT_LOWER,   target_angle[ICUB_ID_LIP_LEFT_LOWER], timestamp);
257
    JointInterface::store_incoming_position(ID_LIP_CENTER_UPPER, target_angle[ICUB_ID_LIP_CENTER_UPPER], timestamp);
258
    JointInterface::store_incoming_position(ID_LIP_CENTER_LOWER, target_angle[ICUB_ID_LIP_CENTER_LOWER], timestamp);
259
    JointInterface::store_incoming_position(ID_LIP_RIGHT_UPPER,  target_angle[ICUB_ID_LIP_RIGHT_UPPER], timestamp);
260
    JointInterface::store_incoming_position(ID_LIP_RIGHT_LOWER,  target_angle[ICUB_ID_LIP_RIGHT_LOWER], timestamp);
261
*/
262
}