humotion / src / server / joint_interface.cpp @ 60dfadc5
History | View | Annotate | Download (7.643 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/joint_interface.h" |
||
29 | #include "server/controller.h" |
||
30 | |||
31 | using namespace std; |
||
32 | using namespace humotion; |
||
33 | using namespace humotion::server; |
||
34 | using namespace boost; |
||
35 | |||
36 | |||
37 | //! constructor
|
||
38 | JointInterface::JointInterface(){ |
||
39 | framerate = 50.0; |
||
40 | mouth_enabled = false;
|
||
41 | gaze_enabled = false;
|
||
42 | } |
||
43 | |||
44 | //! destructor
|
||
45 | JointInterface::~JointInterface(){ |
||
46 | } |
||
47 | |||
48 | //! set joint target position
|
||
49 | //! \param joint_id of joint
|
||
50 | //! \param float position
|
||
51 | void JointInterface::set_target_position(int joint_id, float position){ |
||
52 | assert(joint_id < JOINT_ID_ENUM_SIZE); |
||
53 | |||
54 | //update current value
|
||
55 | joint_target[joint_id] = position; |
||
56 | } |
||
57 | |||
58 | //! fetch target position
|
||
59 | //! \param joint_id of joint
|
||
60 | float JointInterface::get_target_position(int joint_id){ |
||
61 | assert(joint_id < JOINT_ID_ENUM_SIZE); |
||
62 | return joint_target[joint_id];
|
||
63 | } |
||
64 | |||
65 | //! incoming position data
|
||
66 | //! \param joint name
|
||
67 | //! \param position
|
||
68 | //! \param timestamp when the position was measured
|
||
69 | void JointInterface::store_incoming_position(int joint_id, float position, double timestamp){ |
||
70 | //lock the tsd_list for this access. by doing this we assure
|
||
71 | //that no other thread accessing this element can diturb the
|
||
72 | //following atomic instructions:
|
||
73 | mutex::scoped_lock scoped_lock(joint_ts_position_map_access_mutex); |
||
74 | |||
75 | //printf("> humotion: incoming joint position for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,position,timestamp);
|
||
76 | joint_ts_position_map[joint_id].insert(timestamp, position); |
||
77 | |||
78 | incoming_position_count++; |
||
79 | } |
||
80 | |||
81 | //! return incoming position data count & clear this counter
|
||
82 | //! this can be used as a keep alive status check
|
||
83 | //! \return number of incoming joint positions since the last call
|
||
84 | unsigned int JointInterface::get_and_clear_incoming_position_count(){ |
||
85 | unsigned int i = incoming_position_count; |
||
86 | incoming_position_count = 0;
|
||
87 | return i;
|
||
88 | } |
||
89 | |||
90 | //! incoming speed data
|
||
91 | //! \param joint name
|
||
92 | //! \param speed
|
||
93 | //! \param timestamp when the position was measured
|
||
94 | void JointInterface::store_incoming_speed(int joint_id, float speed, double timestamp){ |
||
95 | //lock the tsd_list for this access. by doing this we assure
|
||
96 | //that no other thread accessing this element can diturb the
|
||
97 | //following atomic instructions:
|
||
98 | mutex::scoped_lock scoped_lock(joint_ts_speed_map_access_mutex); |
||
99 | |||
100 | //printf("> humotion: incoming joint speed for joint id 0x%02X = %4.2f (ts=%.2f)\n",joint_id,speed,timestamp);
|
||
101 | joint_ts_speed_map[joint_id].insert(timestamp, speed); |
||
102 | } |
||
103 | |||
104 | //! return the timestamped float for the given joints position
|
||
105 | TimestampedList JointInterface::get_ts_position(int joint_id){
|
||
106 | //lock the tsd_list for this access. by doing this we assure
|
||
107 | 4e9221c4 | Simon Schulz | //that no other thread accessing this element can disturb the
|
108 | 8c6c1163 | Simon Schulz | //following atomic instructions:
|
109 | mutex::scoped_lock scoped_lock(joint_ts_position_map_access_mutex); |
||
110 | |||
111 | //search map:
|
||
112 | joint_tsl_map_t::iterator it = joint_ts_position_map.find(joint_id); |
||
113 | |||
114 | if (it == joint_ts_position_map.end()){
|
||
115 | printf("> humotion: no ts_position for joint id 0x%02X found\n", joint_id);
|
||
116 | return TimestampedList();
|
||
117 | } |
||
118 | |||
119 | //ok fine, we found our value:
|
||
120 | return it->second;
|
||
121 | } |
||
122 | |||
123 | //! return the timestamped float for the given joints speed
|
||
124 | TimestampedList JointInterface::get_ts_speed(int joint_id){
|
||
125 | //lock the tsd_list for this access. by doing this we assure
|
||
126 | //that no other thread accessing this element can diturb the
|
||
127 | //following atomic instructions:
|
||
128 | mutex::scoped_lock scoped_lock(joint_ts_speed_map_access_mutex); |
||
129 | |||
130 | //search map:
|
||
131 | joint_tsl_map_t::iterator it = joint_ts_speed_map.find(joint_id); |
||
132 | |||
133 | if (it == joint_ts_speed_map.end()){
|
||
134 | printf("> humotion: no ts_speed for joint id 0x%02X found\n", joint_id);
|
||
135 | return TimestampedList();
|
||
136 | } |
||
137 | |||
138 | //ok fine, we found our value:
|
||
139 | return it->second;
|
||
140 | } |
||
141 | |||
142 | //! set framerate
|
||
143 | void JointInterface::set_framerate(float f){ |
||
144 | framerate = f; |
||
145 | } |
||
146 | |||
147 | //! enable all mouth joints
|
||
148 | void JointInterface::enable_mouth_joints(){
|
||
149 | //already enabled? skip this
|
||
150 | if (mouth_enabled){
|
||
151 | return;
|
||
152 | } |
||
153 | |||
154 | printf("> humotion: ENABLING MOUTH JOINTS\n");
|
||
155 | enable_joint(ID_LIP_LEFT_UPPER); |
||
156 | enable_joint(ID_LIP_LEFT_LOWER); |
||
157 | enable_joint(ID_LIP_CENTER_UPPER); |
||
158 | enable_joint(ID_LIP_CENTER_LOWER); |
||
159 | enable_joint(ID_LIP_RIGHT_UPPER); |
||
160 | enable_joint(ID_LIP_RIGHT_LOWER); |
||
161 | mouth_enabled = true;
|
||
162 | } |
||
163 | |||
164 | |||
165 | //! disable all mouth joints
|
||
166 | void JointInterface::disable_mouth_joints(){
|
||
167 | //already disabled? skip this
|
||
168 | if (!mouth_enabled){
|
||
169 | return;
|
||
170 | } |
||
171 | |||
172 | printf("> humotion: DISABLING MOUTH JOINTS\n");
|
||
173 | disable_joint(ID_LIP_LEFT_UPPER); |
||
174 | disable_joint(ID_LIP_LEFT_LOWER); |
||
175 | disable_joint(ID_LIP_CENTER_UPPER); |
||
176 | disable_joint(ID_LIP_CENTER_LOWER); |
||
177 | disable_joint(ID_LIP_RIGHT_UPPER); |
||
178 | disable_joint(ID_LIP_RIGHT_LOWER); |
||
179 | mouth_enabled = false;
|
||
180 | } |
||
181 | |||
182 | //! enable all gaze joints
|
||
183 | void JointInterface::enable_gaze_joints(){
|
||
184 | //already enabled? skip this
|
||
185 | if (gaze_enabled){
|
||
186 | return;
|
||
187 | } |
||
188 | |||
189 | printf("> humotion: ENABLING GAZE JOINTS\n");
|
||
190 | enable_joint(ID_EYES_LEFT_LR); |
||
191 | enable_joint(ID_EYES_RIGHT_LR); |
||
192 | enable_joint(ID_EYES_BOTH_UD); |
||
193 | |||
194 | enable_joint(ID_EYES_LEFT_LID_UPPER); |
||
195 | enable_joint(ID_EYES_LEFT_LID_LOWER); |
||
196 | enable_joint(ID_EYES_RIGHT_LID_UPPER); |
||
197 | enable_joint(ID_EYES_RIGHT_LID_LOWER); |
||
198 | |||
199 | enable_joint(ID_EYES_LEFT_BROW); |
||
200 | enable_joint(ID_EYES_RIGHT_BROW); |
||
201 | |||
202 | enable_joint(ID_NECK_PAN); |
||
203 | enable_joint(ID_NECK_TILT); |
||
204 | enable_joint(ID_NECK_ROLL); |
||
205 | |||
206 | gaze_enabled = true;
|
||
207 | } |
||
208 | |||
209 | //! disable all gaze joints
|
||
210 | void JointInterface::disable_gaze_joints(){
|
||
211 | //already disabled? skip this
|
||
212 | if (!gaze_enabled){
|
||
213 | return;
|
||
214 | } |
||
215 | |||
216 | printf("> humotion: DISABLING GAZE JOINTS\n");
|
||
217 | disable_joint(ID_EYES_LEFT_LR); |
||
218 | disable_joint(ID_EYES_RIGHT_LR); |
||
219 | disable_joint(ID_EYES_BOTH_UD); |
||
220 | |||
221 | disable_joint(ID_EYES_LEFT_LID_UPPER); |
||
222 | disable_joint(ID_EYES_LEFT_LID_LOWER); |
||
223 | disable_joint(ID_EYES_RIGHT_LID_UPPER); |
||
224 | disable_joint(ID_EYES_RIGHT_LID_LOWER); |
||
225 | |||
226 | disable_joint(ID_EYES_LEFT_BROW); |
||
227 | disable_joint(ID_EYES_RIGHT_BROW); |
||
228 | |||
229 | disable_joint(ID_NECK_PAN); |
||
230 | disable_joint(ID_NECK_TILT); |
||
231 | disable_joint(ID_NECK_ROLL); |
||
232 | |||
233 | gaze_enabled = false;
|
||
234 | } |
||
235 | |||
236 | //! fetch maximum allowable joint position
|
||
237 | //! \return max position
|
||
238 | float JointInterface::get_joint_max(int joint_id){ |
||
239 | assert((joint_id > 0) && (joint_id < JOINT_ID_ENUM_SIZE));
|
||
240 | return joint_max[joint_id];
|
||
241 | } |
||
242 | |||
243 | //! fetch minimum allowable joint position
|
||
244 | //! \return min position
|
||
245 | float JointInterface::get_joint_min(int joint_id){ |
||
246 | assert((joint_id > 0) && (joint_id < JOINT_ID_ENUM_SIZE));
|
||
247 | return joint_min[joint_id];
|
||
248 | } |
||
249 | |||
250 | //! check if joint position map is empty
|
||
251 | //! \return true if empty
|
||
252 | bool JointInterface::get_joint_position_map_empty(){
|
||
253 | return (joint_ts_position_map.empty());
|
||
254 | } |