Fawkes API  Fawkes Development Version
 All Classes Namespaces Functions Variables Typedefs Enumerations Enumerator Friends Groups Pages
naostiffness_config_plugin.cpp
1 
2 /***************************************************************************
3  * naostiffness_config_plugin.cpp - Config plugin for the nao joint stiffnesses
4  *
5  * Created: Tue Apr 7 15:15:15 2009
6  * Copyright 2009 Tobias Kellner
7  *
8  ****************************************************************************/
9 
10 /* This program is free software; you can redistribute it and/or modify
11  * it under the terms of the GNU General Public License as published by
12  * the Free Software Foundation; either version 2 of the License, or
13  * (at your option) any later version.
14  *
15  * This program is distributed in the hope that it will be useful,
16  * but WITHOUT ANY WARRANTY; without even the implied warranty of
17  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
18  * GNU Library General Public License for more details.
19  *
20  * Read the full text in the LICENSE.GPL file in the doc directory.
21  */
22 
23 #include "naostiffness_config_plugin.h"
24 
25 #include <config/config.h>
26 
27 using namespace fawkes;
28 using std::string;
29 using sigc::mem_fun;
30 using sigc::compose;
31 using Gtk::SpinButton;
32 using Gtk::CheckButton;
33 using Gtk::ComboBox;
34 
35 
36 #define STIFFNESS_CFG_PATH "/naomotion/stiffness"
37 
38 /** @class NaoStiffnessConfigDialog naostiffness_config_plugin.h
39  * Config dialog of the config editor plugin for the nao joint stiffnesses.
40  * @author Tobias Kellner
41  */
42 
43 /** Constructor.
44  * Allows to construct a dialog by means of get_widget_derived(...).
45  * @param cobject base object pointer
46  * @param builder Gtk builder
47  */
49  const Glib::RefPtr<Gtk::Builder> &builder)
50  : Gtk::Dialog(cobject)
51 {
52  builder->get_widget("hy", __hy);
53  builder->get_widget("hp", __hp);
54 
55  builder->get_widget("lsp", __lsp);
56  builder->get_widget("rsp", __rsp);
57  builder->get_widget("lsr", __lsr);
58  builder->get_widget("rsr", __rsr);
59  builder->get_widget("ley", __ley);
60  builder->get_widget("rey", __rey);
61  builder->get_widget("ler", __ler);
62  builder->get_widget("rer", __rer);
63 
64  builder->get_widget("lhyp", __lhyp);
65  builder->get_widget("rhyp", __rhyp);
66  builder->get_widget("lhr", __lhr);
67  builder->get_widget("rhr", __rhr);
68  builder->get_widget("lhp", __lhp);
69  builder->get_widget("rhp", __rhp);
70  builder->get_widget("lkp", __lkp);
71  builder->get_widget("rkp", __rkp);
72  builder->get_widget("lar", __lar);
73  builder->get_widget("rar", __rar);
74  builder->get_widget("lap", __lap);
75  builder->get_widget("rap", __rap);
76 
77  builder->get_widget("checkbutton_default", __def);
78  builder->get_widget("checkbutton_lock", __lck);
79 
80  __lck->signal_toggled().connect(mem_fun(*this, &NaoStiffnessConfigDialog::on_checkbutton_lock_toggled));
82 
83  builder->get_widget("combobox_behaviour", __bhv);
84  __bhv->set_active(0);
85  __bhv->get_active()->get_value(0, __cur_bhv);
86  __bhv->signal_changed().connect(mem_fun(*this, &NaoStiffnessConfigDialog::on_combobox_behaviour_changed));
87 }
88 
89 /** Destructor. */
91 {
92 }
93 
94 /** Set joint stiffness values in the dialog
95  * @param vals structure containing the stiffness values
96  */
98 {
99  __hy->set_value(vals.hy);
100  __hp->set_value(vals.hp);
101 
102  __lsp->set_value(vals.lsp);
103  __rsp->set_value(vals.rsp);
104  __lsr->set_value(vals.lsr);
105  __rsr->set_value(vals.rsr);
106  __ley->set_value(vals.ley);
107  __rey->set_value(vals.rey);
108  __ler->set_value(vals.ler);
109  __rer->set_value(vals.rer);
110 
111  __lhyp->set_value(vals.lhyp);
112  __rhyp->set_value(vals.rhyp);
113  __lhr->set_value(vals.lhr);
114  __rhr->set_value(vals.rhr);
115  __lhp->set_value(vals.lhp);
116  __rhp->set_value(vals.rhp);
117  __lkp->set_value(vals.lkp);
118  __rkp->set_value(vals.rkp);
119  __lar->set_value(vals.lar);
120  __rar->set_value(vals.rar);
121  __lap->set_value(vals.lap);
122  __rap->set_value(vals.rap);
123 
124  __lck->set_active(
125  __lsp->get_value() == __rsp->get_value() &&
126  __lsr->get_value() == __rsr->get_value() &&
127  __ley->get_value() == __rey->get_value() &&
128  __ler->get_value() == __rer->get_value() &&
129  __lhyp->get_value() == __rhyp->get_value() &&
130  __lhr->get_value() == __rhr->get_value() &&
131  __lhp->get_value() == __rhp->get_value() &&
132  __lkp->get_value() == __rkp->get_value() &&
133  __lar->get_value() == __rar->get_value() &&
134  __lap->get_value() == __rap->get_value()
135  );
136 }
137 
138 /** Get joint stiffness values from the dialog
139  * @param vals structure the stiffness values get written to
140  */
142 {
143  vals.hy = __hy ->get_value();
144  vals.hp = __hp->get_value();
145 
146  vals.lsp = __lsp->get_value();
147  vals.rsp = __rsp->get_value();
148  vals.lsr = __lsr->get_value();
149  vals.rsr = __rsr->get_value();
150  vals.ley = __ley->get_value();
151  vals.rey = __rey->get_value();
152  vals.ler = __ler->get_value();
153  vals.rer = __rer->get_value();
154 
155  vals.lhyp = __lhyp->get_value();
156  vals.rhyp = __rhyp->get_value();
157  vals.lhr = __lhr->get_value();
158  vals.rhr = __rhr->get_value();
159  vals.lhp = __lhp->get_value();
160  vals.rhp = __rhp->get_value();
161  vals.lkp = __lkp->get_value();
162  vals.rkp = __rkp->get_value();
163  vals.lar = __lar->get_value();
164  vals.rar = __rar->get_value();
165  vals.lap = __lap->get_value();
166  vals.rap = __rap->get_value();
167 }
168 
169 /** Lock checkbox toggled handler. */
171 {
172  bool locked = __lck->get_active();
173 
174  __rsp->set_sensitive(!locked);
175  __rsr->set_sensitive(!locked);
176  __rey->set_sensitive(!locked);
177  __rer->set_sensitive(!locked);
178 
179  __rhyp->set_sensitive(!locked);
180  __rhr->set_sensitive(!locked);
181  __rhp->set_sensitive(!locked);
182  __rkp->set_sensitive(!locked);
183  __rar->set_sensitive(!locked);
184  __rap->set_sensitive(!locked);
185 
186  if (locked)
187  {
188  __connections.push_back(__lsp->signal_value_changed().connect(compose(mem_fun(*__rsp, &SpinButton::set_value), mem_fun(*__lsp, &SpinButton::get_value))));
189  __connections.push_back(__lsr->signal_value_changed().connect(compose(mem_fun(*__rsr, &SpinButton::set_value), mem_fun(*__lsr, &SpinButton::get_value))));
190  __connections.push_back(__ley->signal_value_changed().connect(compose(mem_fun(*__rey, &SpinButton::set_value), mem_fun(*__ley, &SpinButton::get_value))));
191  __connections.push_back(__ler->signal_value_changed().connect(compose(mem_fun(*__rer, &SpinButton::set_value), mem_fun(*__ler, &SpinButton::get_value))));
192 
193  __connections.push_back(__lhyp->signal_value_changed().connect(compose(mem_fun(*__rhyp, &SpinButton::set_value), mem_fun(*__lhyp, &SpinButton::get_value))));
194  __connections.push_back(__lhr->signal_value_changed().connect(compose(mem_fun(*__rhr, &SpinButton::set_value), mem_fun(*__lhr, &SpinButton::get_value))));
195  __connections.push_back(__lhp->signal_value_changed().connect(compose(mem_fun(*__rhp, &SpinButton::set_value), mem_fun(*__lhp, &SpinButton::get_value))));
196  __connections.push_back(__lkp->signal_value_changed().connect(compose(mem_fun(*__rkp, &SpinButton::set_value), mem_fun(*__lkp, &SpinButton::get_value))));
197  __connections.push_back(__lar->signal_value_changed().connect(compose(mem_fun(*__rar, &SpinButton::set_value), mem_fun(*__lar, &SpinButton::get_value))));
198  __connections.push_back(__lap->signal_value_changed().connect(compose(mem_fun(*__rap, &SpinButton::set_value), mem_fun(*__lap, &SpinButton::get_value))));
199 
200  __rsp->set_value(__lsp->get_value());
201  __rsr->set_value(__lsr->get_value());
202  __rey->set_value(__ley->get_value());
203  __rer->set_value(__ler->get_value());
204 
205  __rhyp->set_value(__lhyp->get_value());
206  __rhr->set_value(__lhr->get_value());
207  __rhp->set_value(__lhp->get_value());
208  __rkp->set_value(__lkp->get_value());
209  __rar->set_value(__lar->get_value());
210  __rap->set_value(__lap->get_value());
211  }
212  else
213  {
214  while (!__connections.empty())
215  {
216  __connections.back().disconnect();
217  __connections.pop_back();
218  }
219  }
220 }
221 
222 /** Behaviour combobox changed handler. */
224 {
225  __bhv->get_active()->get_value(0, __cur_bhv);
226  __load_vals();
227 }
228 
229 /** Return currently selected behaviour.
230  * @return a string representing the selected behaviour
231  */
233 {
234  return __cur_bhv;
235 }
236 
237 /** Return whether default checkbox is checked.
238  * @return true if default is checked
239  */
241 {
242  return __def->get_active();
243 }
244 
245 /** Set the callback function for loading values in the plugin.
246  * Config is not accessible in the dialog, so it has to be done there.
247  * @param cb the callback
248  */
250 {
251  __load_vals = cb;
252 }
253 
254 
255 /** @class NaoStiffnessConfigPlugin naostiffness_config_plugin.h
256  * Config editor plugin for the Nao joint stiffness values.
257  * @author Tobias Kellner
258  */
259 
260 /** Constructor.
261  * @param ui_path path to the UI file for the plugin's dialog
262  */
264  : ConfigEditorPlugin(STIFFNESS_CFG_PATH, ui_path)
265 {
266 }
267 
268 /** Destructor. */
270 {
271 }
272 
273 /** Load joint stiffness values from config */
274 void
275 NaoStiffnessConfigPlugin::load_vals()
276 {
277  try
278  {
280 
281  string path = string(STIFFNESS_CFG_PATH"/").append(dlg->get_cur_behaviour());
282 
283  __initial_vals.hy = m_config->get_float(string(path).append("/head_yaw").c_str());
284  __initial_vals.hp = m_config->get_float(string(path).append("/head_pitch").c_str());
285 
286  __initial_vals.lsp = m_config->get_float(string(path).append("/l_shoulder_pitch").c_str());
287  __initial_vals.rsp = m_config->get_float(string(path).append("/r_shoulder_pitch").c_str());
288  __initial_vals.lsr = m_config->get_float(string(path).append("/l_shoulder_roll").c_str());
289  __initial_vals.rsr = m_config->get_float(string(path).append("/r_shoulder_roll").c_str());
290  __initial_vals.ley = m_config->get_float(string(path).append("/l_elbow_yaw").c_str());
291  __initial_vals.rey = m_config->get_float(string(path).append("/r_elbow_yaw").c_str());
292  __initial_vals.ler = m_config->get_float(string(path).append("/l_elbow_roll").c_str());
293  __initial_vals.rer = m_config->get_float(string(path).append("/r_elbow_roll").c_str());
294 
295  __initial_vals.lhyp = m_config->get_float(string(path).append("/l_hip_yaw_pitch").c_str());
296  __initial_vals.rhyp = m_config->get_float(string(path).append("/r_hip_yaw_pitch").c_str());
297  __initial_vals.lhr = m_config->get_float(string(path).append("/l_hip_roll").c_str());
298  __initial_vals.rhr = m_config->get_float(string(path).append("/r_hip_roll").c_str());
299  __initial_vals.lhp = m_config->get_float(string(path).append("/l_hip_roll").c_str());
300  __initial_vals.rhp = m_config->get_float(string(path).append("/r_hip_roll").c_str());
301  __initial_vals.lkp = m_config->get_float(string(path).append("/l_knee_pitch").c_str());
302  __initial_vals.rkp = m_config->get_float(string(path).append("/r_knee_pitch").c_str());
303  __initial_vals.lar = m_config->get_float(string(path).append("/l_ankle_roll").c_str());
304  __initial_vals.rar = m_config->get_float(string(path).append("/r_ankle_roll").c_str());
305  __initial_vals.lap = m_config->get_float(string(path).append("/l_ankle_pitch").c_str());
306  __initial_vals.rap = m_config->get_float(string(path).append("/r_ankle_pitch").c_str());
307 
308  dlg->set_stiffnesses(__initial_vals);
309  }
310  catch (Exception &e)
311  {
312  printf("NaoStiffnessConfigPlugin: Could not read required config values.\n");
313 
314  __initial_vals.hy = -1.0;
315  __initial_vals.hp = -1.0;
316 
317  __initial_vals.lsp = -1.0;
318  __initial_vals.rsp = -1.0;
319  __initial_vals.lsr = -1.0;
320  __initial_vals.rsr = -1.0;
321  __initial_vals.ley = -1.0;
322  __initial_vals.rey = -1.0;
323  __initial_vals.ler = -1.0;
324  __initial_vals.rer = -1.0;
325 
326  __initial_vals.lhyp = -1.0;
327  __initial_vals.rhyp = -1.0;
328  __initial_vals.lhr = -1.0;
329  __initial_vals.rhr = -1.0;
330  __initial_vals.lhp = -1.0;
331  __initial_vals.rhp = -1.0;
332  __initial_vals.lkp = -1.0;
333  __initial_vals.rkp = -1.0;
334  __initial_vals.lar = -1.0;
335  __initial_vals.rar = -1.0;
336  __initial_vals.lap = -1.0;
337  __initial_vals.rap = -1.0;
338  }
339 }
340 
341 /** Save joint stiffness values to config */
342 void
343 NaoStiffnessConfigPlugin::save_vals()
344 {
345  try
346  {
349  dlg->get_stiffnesses(vals);
350  string path = string(STIFFNESS_CFG_PATH"/").append(dlg->get_cur_behaviour());
351 
352  void (Configuration::*my_set_float)(const char *, float) = NULL;
353 
354  if (dlg->get_save_default()) my_set_float = &Configuration::set_default_float;
355  else my_set_float = &Configuration::set_float;
356 
357  if (vals.hy != __initial_vals.hy) (m_config->*my_set_float)(string(path).append("/head_yaw").c_str(), vals.hy);
358  if (vals.hp != __initial_vals.hp) (m_config->*my_set_float)(string(path).append("/head_pitch").c_str(), vals.hp);
359 
360  if (vals.lsp != __initial_vals.lsp) (m_config->*my_set_float)(string(path).append("/l_shoulder_pitch").c_str(), vals.lsp);
361  if (vals.rsp != __initial_vals.rsp) (m_config->*my_set_float)(string(path).append("/r_shoulder_pitch").c_str(), vals.rsp);
362  if (vals.lsr != __initial_vals.lsr) (m_config->*my_set_float)(string(path).append("/l_shoulder_roll").c_str(), vals.lsr);
363  if (vals.rsr != __initial_vals.rsr) (m_config->*my_set_float)(string(path).append("/r_shoulder_roll").c_str(), vals.rsr);
364  if (vals.ley != __initial_vals.ley) (m_config->*my_set_float)(string(path).append("/l_elbow_yaw").c_str(), vals.ley);
365  if (vals.rey != __initial_vals.rey) (m_config->*my_set_float)(string(path).append("/r_elbow_yaw").c_str(), vals.rey);
366  if (vals.ler != __initial_vals.ler) (m_config->*my_set_float)(string(path).append("/l_elbow_roll").c_str(), vals.ler);
367  if (vals.rer != __initial_vals.rer) (m_config->*my_set_float)(string(path).append("/r_elbow_roll").c_str(), vals.rer);
368 
369  if (vals.lhyp != __initial_vals.lhyp) (m_config->*my_set_float)(string(path).append("/l_hip_yaw_pitch").c_str(), vals.lhyp);
370  if (vals.rhyp != __initial_vals.rhyp) (m_config->*my_set_float)(string(path).append("/r_hip_yaw_pitch").c_str(), vals.rhyp);
371  if (vals.lhr != __initial_vals.lhr) (m_config->*my_set_float)(string(path).append("/l_hip_roll").c_str(), vals.lhr);
372  if (vals.rhr != __initial_vals.rhr) (m_config->*my_set_float)(string(path).append("/r_hip_roll").c_str(), vals.rhr);
373  if (vals.lhp != __initial_vals.lhp) (m_config->*my_set_float)(string(path).append("/l_hip_roll").c_str(), vals.lhp);
374  if (vals.rhp != __initial_vals.rhp) (m_config->*my_set_float)(string(path).append("/r_hip_roll").c_str(), vals.rhp);
375  if (vals.lkp != __initial_vals.lkp) (m_config->*my_set_float)(string(path).append("/l_knee_pitch").c_str(), vals.lkp);
376  if (vals.rkp != __initial_vals.rkp) (m_config->*my_set_float)(string(path).append("/r_knee_pitch").c_str(), vals.rkp);
377  if (vals.lar != __initial_vals.lar) (m_config->*my_set_float)(string(path).append("/l_ankle_roll").c_str(), vals.lar);
378  if (vals.rar != __initial_vals.rar) (m_config->*my_set_float)(string(path).append("/r_ankle_roll").c_str(), vals.rar);
379  if (vals.lap != __initial_vals.lap) (m_config->*my_set_float)(string(path).append("/l_ankle_pitch").c_str(), vals.lap);
380  if (vals.rap != __initial_vals.rap) (m_config->*my_set_float)(string(path).append("/r_ankle_pitch").c_str(), vals.rap);
381  }
382  catch (Exception &e)
383  {
384  printf("NaoStiffnessConfigPlugin: Could not write required config values.\n");
385  e.print_backtrace();
386  }
387 }
388 
389 void
391 {
392  load_vals();
393 }
394 
395 void
397 {
398  switch (response)
399  {
400  case Gtk::RESPONSE_OK:
401  save_vals();
402  break;
403 
404  case Gtk::RESPONSE_CANCEL: //fall through
405  case Gtk::RESPONSE_DELETE_EVENT:
406  break;
407 
408  default:
409  printf("Nao stiffness config plugin: unknown response\n");
410  break;
411  }
412 }
413 
414 Gtk::Dialog*
416 {
417  NaoStiffnessConfigDialog *dlg = NULL;
418  m_builder->get_widget_derived("PluginDialog", dlg);
419  dlg->set_load_vals(mem_fun(*this, &NaoStiffnessConfigPlugin::load_vals));
420 
421  return dlg;
422 }