00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023 #include "naostiffness_config_plugin.h"
00024
00025 #include <config/config.h>
00026
00027 using namespace fawkes;
00028 using std::string;
00029 using sigc::mem_fun;
00030 using sigc::compose;
00031 using Gtk::SpinButton;
00032 using Gtk::CheckButton;
00033 using Gtk::ComboBox;
00034
00035
00036 #define STIFFNESS_CFG_PATH "/naomotion/stiffness"
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048 NaoStiffnessConfigDialog::NaoStiffnessConfigDialog(BaseObjectType *cobject,
00049 const Glib::RefPtr<Gnome::Glade::Xml> &ref_xml)
00050 : Gtk::Dialog(cobject)
00051 {
00052 ref_xml->get_widget("hy", __hy);
00053 ref_xml->get_widget("hp", __hp);
00054
00055 ref_xml->get_widget("lsp", __lsp);
00056 ref_xml->get_widget("rsp", __rsp);
00057 ref_xml->get_widget("lsr", __lsr);
00058 ref_xml->get_widget("rsr", __rsr);
00059 ref_xml->get_widget("ley", __ley);
00060 ref_xml->get_widget("rey", __rey);
00061 ref_xml->get_widget("ler", __ler);
00062 ref_xml->get_widget("rer", __rer);
00063
00064 ref_xml->get_widget("lhyp", __lhyp);
00065 ref_xml->get_widget("rhyp", __rhyp);
00066 ref_xml->get_widget("lhr", __lhr);
00067 ref_xml->get_widget("rhr", __rhr);
00068 ref_xml->get_widget("lhp", __lhp);
00069 ref_xml->get_widget("rhp", __rhp);
00070 ref_xml->get_widget("lkp", __lkp);
00071 ref_xml->get_widget("rkp", __rkp);
00072 ref_xml->get_widget("lar", __lar);
00073 ref_xml->get_widget("rar", __rar);
00074 ref_xml->get_widget("lap", __lap);
00075 ref_xml->get_widget("rap", __rap);
00076
00077 ref_xml->get_widget("checkbutton_default", __def);
00078 ref_xml->get_widget("checkbutton_lock", __lck);
00079
00080 __lck->signal_toggled().connect(mem_fun(*this, &NaoStiffnessConfigDialog::on_checkbutton_lock_toggled));
00081 on_checkbutton_lock_toggled();
00082
00083 ref_xml->get_widget("combobox_behaviour", __bhv);
00084 __bhv->set_active(0);
00085 __bhv->get_active()->get_value(0, __cur_bhv);
00086 __bhv->signal_changed().connect(mem_fun(*this, &NaoStiffnessConfigDialog::on_combobox_behaviour_changed));
00087 }
00088
00089
00090 NaoStiffnessConfigDialog::~NaoStiffnessConfigDialog()
00091 {
00092 }
00093
00094
00095
00096
00097 void NaoStiffnessConfigDialog::set_stiffnesses(const nao_stiffnesses &vals)
00098 {
00099 __hy->set_value(vals.hy);
00100 __hp->set_value(vals.hp);
00101
00102 __lsp->set_value(vals.lsp);
00103 __rsp->set_value(vals.rsp);
00104 __lsr->set_value(vals.lsr);
00105 __rsr->set_value(vals.rsr);
00106 __ley->set_value(vals.ley);
00107 __rey->set_value(vals.rey);
00108 __ler->set_value(vals.ler);
00109 __rer->set_value(vals.rer);
00110
00111 __lhyp->set_value(vals.lhyp);
00112 __rhyp->set_value(vals.rhyp);
00113 __lhr->set_value(vals.lhr);
00114 __rhr->set_value(vals.rhr);
00115 __lhp->set_value(vals.lhp);
00116 __rhp->set_value(vals.rhp);
00117 __lkp->set_value(vals.lkp);
00118 __rkp->set_value(vals.rkp);
00119 __lar->set_value(vals.lar);
00120 __rar->set_value(vals.rar);
00121 __lap->set_value(vals.lap);
00122 __rap->set_value(vals.rap);
00123
00124 __lck->set_active(
00125 __lsp->get_value() == __rsp->get_value() &&
00126 __lsr->get_value() == __rsr->get_value() &&
00127 __ley->get_value() == __rey->get_value() &&
00128 __ler->get_value() == __rer->get_value() &&
00129 __lhyp->get_value() == __rhyp->get_value() &&
00130 __lhr->get_value() == __rhr->get_value() &&
00131 __lhp->get_value() == __rhp->get_value() &&
00132 __lkp->get_value() == __rkp->get_value() &&
00133 __lar->get_value() == __rar->get_value() &&
00134 __lap->get_value() == __rap->get_value()
00135 );
00136 }
00137
00138
00139
00140
00141 void NaoStiffnessConfigDialog::get_stiffnesses(nao_stiffnesses &vals)
00142 {
00143 vals.hy = __hy ->get_value();
00144 vals.hp = __hp->get_value();
00145
00146 vals.lsp = __lsp->get_value();
00147 vals.rsp = __rsp->get_value();
00148 vals.lsr = __lsr->get_value();
00149 vals.rsr = __rsr->get_value();
00150 vals.ley = __ley->get_value();
00151 vals.rey = __rey->get_value();
00152 vals.ler = __ler->get_value();
00153 vals.rer = __rer->get_value();
00154
00155 vals.lhyp = __lhyp->get_value();
00156 vals.rhyp = __rhyp->get_value();
00157 vals.lhr = __lhr->get_value();
00158 vals.rhr = __rhr->get_value();
00159 vals.lhp = __lhp->get_value();
00160 vals.rhp = __rhp->get_value();
00161 vals.lkp = __lkp->get_value();
00162 vals.rkp = __rkp->get_value();
00163 vals.lar = __lar->get_value();
00164 vals.rar = __rar->get_value();
00165 vals.lap = __lap->get_value();
00166 vals.rap = __rap->get_value();
00167 }
00168
00169
00170 void NaoStiffnessConfigDialog::on_checkbutton_lock_toggled()
00171 {
00172 bool locked = __lck->get_active();
00173
00174 __rsp->set_sensitive(!locked);
00175 __rsr->set_sensitive(!locked);
00176 __rey->set_sensitive(!locked);
00177 __rer->set_sensitive(!locked);
00178
00179 __rhyp->set_sensitive(!locked);
00180 __rhr->set_sensitive(!locked);
00181 __rhp->set_sensitive(!locked);
00182 __rkp->set_sensitive(!locked);
00183 __rar->set_sensitive(!locked);
00184 __rap->set_sensitive(!locked);
00185
00186 if (locked)
00187 {
00188 __connections.push_back(__lsp->signal_value_changed().connect(compose(mem_fun(*__rsp, &SpinButton::set_value), mem_fun(*__lsp, &SpinButton::get_value))));
00189 __connections.push_back(__lsr->signal_value_changed().connect(compose(mem_fun(*__rsr, &SpinButton::set_value), mem_fun(*__lsr, &SpinButton::get_value))));
00190 __connections.push_back(__ley->signal_value_changed().connect(compose(mem_fun(*__rey, &SpinButton::set_value), mem_fun(*__ley, &SpinButton::get_value))));
00191 __connections.push_back(__ler->signal_value_changed().connect(compose(mem_fun(*__rer, &SpinButton::set_value), mem_fun(*__ler, &SpinButton::get_value))));
00192
00193 __connections.push_back(__lhyp->signal_value_changed().connect(compose(mem_fun(*__rhyp, &SpinButton::set_value), mem_fun(*__lhyp, &SpinButton::get_value))));
00194 __connections.push_back(__lhr->signal_value_changed().connect(compose(mem_fun(*__rhr, &SpinButton::set_value), mem_fun(*__lhr, &SpinButton::get_value))));
00195 __connections.push_back(__lhp->signal_value_changed().connect(compose(mem_fun(*__rhp, &SpinButton::set_value), mem_fun(*__lhp, &SpinButton::get_value))));
00196 __connections.push_back(__lkp->signal_value_changed().connect(compose(mem_fun(*__rkp, &SpinButton::set_value), mem_fun(*__lkp, &SpinButton::get_value))));
00197 __connections.push_back(__lar->signal_value_changed().connect(compose(mem_fun(*__rar, &SpinButton::set_value), mem_fun(*__lar, &SpinButton::get_value))));
00198 __connections.push_back(__lap->signal_value_changed().connect(compose(mem_fun(*__rap, &SpinButton::set_value), mem_fun(*__lap, &SpinButton::get_value))));
00199
00200 __rsp->set_value(__lsp->get_value());
00201 __rsr->set_value(__lsr->get_value());
00202 __rey->set_value(__ley->get_value());
00203 __rer->set_value(__ler->get_value());
00204
00205 __rhyp->set_value(__lhyp->get_value());
00206 __rhr->set_value(__lhr->get_value());
00207 __rhp->set_value(__lhp->get_value());
00208 __rkp->set_value(__lkp->get_value());
00209 __rar->set_value(__lar->get_value());
00210 __rap->set_value(__lap->get_value());
00211 }
00212 else
00213 {
00214 while (!__connections.empty())
00215 {
00216 __connections.back().disconnect();
00217 __connections.pop_back();
00218 }
00219 }
00220 }
00221
00222
00223 void NaoStiffnessConfigDialog::on_combobox_behaviour_changed()
00224 {
00225 __bhv->get_active()->get_value(0, __cur_bhv);
00226 __load_vals();
00227 }
00228
00229
00230
00231
00232 string NaoStiffnessConfigDialog::get_cur_behaviour()
00233 {
00234 return __cur_bhv;
00235 }
00236
00237
00238
00239
00240 bool NaoStiffnessConfigDialog::get_save_default()
00241 {
00242 return __def->get_active();
00243 }
00244
00245
00246
00247
00248
00249 void NaoStiffnessConfigDialog::set_load_vals(sigc::slot<void> cb)
00250 {
00251 __load_vals = cb;
00252 }
00253
00254
00255
00256
00257
00258
00259
00260
00261
00262
00263 NaoStiffnessConfigPlugin::NaoStiffnessConfigPlugin(string glade_path)
00264 : ConfigEditorPlugin(STIFFNESS_CFG_PATH, glade_path)
00265 {
00266 }
00267
00268
00269 NaoStiffnessConfigPlugin::~NaoStiffnessConfigPlugin()
00270 {
00271 }
00272
00273
00274 void
00275 NaoStiffnessConfigPlugin::load_vals()
00276 {
00277 try
00278 {
00279 NaoStiffnessConfigDialog* dlg = dynamic_cast<NaoStiffnessConfigDialog *>(m_dialog);
00280
00281 string path = string(STIFFNESS_CFG_PATH"/").append(dlg->get_cur_behaviour());
00282
00283 __initial_vals.hy = m_config->get_float(string(path).append("/head_yaw").c_str());
00284 __initial_vals.hp = m_config->get_float(string(path).append("/head_pitch").c_str());
00285
00286 __initial_vals.lsp = m_config->get_float(string(path).append("/l_shoulder_pitch").c_str());
00287 __initial_vals.rsp = m_config->get_float(string(path).append("/r_shoulder_pitch").c_str());
00288 __initial_vals.lsr = m_config->get_float(string(path).append("/l_shoulder_roll").c_str());
00289 __initial_vals.rsr = m_config->get_float(string(path).append("/r_shoulder_roll").c_str());
00290 __initial_vals.ley = m_config->get_float(string(path).append("/l_elbow_yaw").c_str());
00291 __initial_vals.rey = m_config->get_float(string(path).append("/r_elbow_yaw").c_str());
00292 __initial_vals.ler = m_config->get_float(string(path).append("/l_elbow_roll").c_str());
00293 __initial_vals.rer = m_config->get_float(string(path).append("/r_elbow_roll").c_str());
00294
00295 __initial_vals.lhyp = m_config->get_float(string(path).append("/l_hip_yaw_pitch").c_str());
00296 __initial_vals.rhyp = m_config->get_float(string(path).append("/r_hip_yaw_pitch").c_str());
00297 __initial_vals.lhr = m_config->get_float(string(path).append("/l_hip_roll").c_str());
00298 __initial_vals.rhr = m_config->get_float(string(path).append("/r_hip_roll").c_str());
00299 __initial_vals.lhp = m_config->get_float(string(path).append("/l_hip_roll").c_str());
00300 __initial_vals.rhp = m_config->get_float(string(path).append("/r_hip_roll").c_str());
00301 __initial_vals.lkp = m_config->get_float(string(path).append("/l_knee_pitch").c_str());
00302 __initial_vals.rkp = m_config->get_float(string(path).append("/r_knee_pitch").c_str());
00303 __initial_vals.lar = m_config->get_float(string(path).append("/l_ankle_roll").c_str());
00304 __initial_vals.rar = m_config->get_float(string(path).append("/r_ankle_roll").c_str());
00305 __initial_vals.lap = m_config->get_float(string(path).append("/l_ankle_pitch").c_str());
00306 __initial_vals.rap = m_config->get_float(string(path).append("/r_ankle_pitch").c_str());
00307
00308 dlg->set_stiffnesses(__initial_vals);
00309 }
00310 catch (Exception &e)
00311 {
00312 printf("NaoStiffnessConfigPlugin: Could not read required config values.\n");
00313
00314 __initial_vals.hy = -1.0;
00315 __initial_vals.hp = -1.0;
00316
00317 __initial_vals.lsp = -1.0;
00318 __initial_vals.rsp = -1.0;
00319 __initial_vals.lsr = -1.0;
00320 __initial_vals.rsr = -1.0;
00321 __initial_vals.ley = -1.0;
00322 __initial_vals.rey = -1.0;
00323 __initial_vals.ler = -1.0;
00324 __initial_vals.rer = -1.0;
00325
00326 __initial_vals.lhyp = -1.0;
00327 __initial_vals.rhyp = -1.0;
00328 __initial_vals.lhr = -1.0;
00329 __initial_vals.rhr = -1.0;
00330 __initial_vals.lhp = -1.0;
00331 __initial_vals.rhp = -1.0;
00332 __initial_vals.lkp = -1.0;
00333 __initial_vals.rkp = -1.0;
00334 __initial_vals.lar = -1.0;
00335 __initial_vals.rar = -1.0;
00336 __initial_vals.lap = -1.0;
00337 __initial_vals.rap = -1.0;
00338 }
00339 }
00340
00341
00342 void
00343 NaoStiffnessConfigPlugin::save_vals()
00344 {
00345 try
00346 {
00347 NaoStiffnessConfigDialog::nao_stiffnesses vals;
00348 NaoStiffnessConfigDialog* dlg = dynamic_cast<NaoStiffnessConfigDialog *>(m_dialog);
00349 dlg->get_stiffnesses(vals);
00350 string path = string(STIFFNESS_CFG_PATH"/").append(dlg->get_cur_behaviour());
00351
00352 void (Configuration::*my_set_float)(const char *, float) = NULL;
00353
00354 if (dlg->get_save_default()) my_set_float = &Configuration::set_default_float;
00355 else my_set_float = &Configuration::set_float;
00356
00357 if (vals.hy != __initial_vals.hy) (m_config->*my_set_float)(string(path).append("/head_yaw").c_str(), vals.hy);
00358 if (vals.hp != __initial_vals.hp) (m_config->*my_set_float)(string(path).append("/head_pitch").c_str(), vals.hp);
00359
00360 if (vals.lsp != __initial_vals.lsp) (m_config->*my_set_float)(string(path).append("/l_shoulder_pitch").c_str(), vals.lsp);
00361 if (vals.rsp != __initial_vals.rsp) (m_config->*my_set_float)(string(path).append("/r_shoulder_pitch").c_str(), vals.rsp);
00362 if (vals.lsr != __initial_vals.lsr) (m_config->*my_set_float)(string(path).append("/l_shoulder_roll").c_str(), vals.lsr);
00363 if (vals.rsr != __initial_vals.rsr) (m_config->*my_set_float)(string(path).append("/r_shoulder_roll").c_str(), vals.rsr);
00364 if (vals.ley != __initial_vals.ley) (m_config->*my_set_float)(string(path).append("/l_elbow_yaw").c_str(), vals.ley);
00365 if (vals.rey != __initial_vals.rey) (m_config->*my_set_float)(string(path).append("/r_elbow_yaw").c_str(), vals.rey);
00366 if (vals.ler != __initial_vals.ler) (m_config->*my_set_float)(string(path).append("/l_elbow_roll").c_str(), vals.ler);
00367 if (vals.rer != __initial_vals.rer) (m_config->*my_set_float)(string(path).append("/r_elbow_roll").c_str(), vals.rer);
00368
00369 if (vals.lhyp != __initial_vals.lhyp) (m_config->*my_set_float)(string(path).append("/l_hip_yaw_pitch").c_str(), vals.lhyp);
00370 if (vals.rhyp != __initial_vals.rhyp) (m_config->*my_set_float)(string(path).append("/r_hip_yaw_pitch").c_str(), vals.rhyp);
00371 if (vals.lhr != __initial_vals.lhr) (m_config->*my_set_float)(string(path).append("/l_hip_roll").c_str(), vals.lhr);
00372 if (vals.rhr != __initial_vals.rhr) (m_config->*my_set_float)(string(path).append("/r_hip_roll").c_str(), vals.rhr);
00373 if (vals.lhp != __initial_vals.lhp) (m_config->*my_set_float)(string(path).append("/l_hip_roll").c_str(), vals.lhp);
00374 if (vals.rhp != __initial_vals.rhp) (m_config->*my_set_float)(string(path).append("/r_hip_roll").c_str(), vals.rhp);
00375 if (vals.lkp != __initial_vals.lkp) (m_config->*my_set_float)(string(path).append("/l_knee_pitch").c_str(), vals.lkp);
00376 if (vals.rkp != __initial_vals.rkp) (m_config->*my_set_float)(string(path).append("/r_knee_pitch").c_str(), vals.rkp);
00377 if (vals.lar != __initial_vals.lar) (m_config->*my_set_float)(string(path).append("/l_ankle_roll").c_str(), vals.lar);
00378 if (vals.rar != __initial_vals.rar) (m_config->*my_set_float)(string(path).append("/r_ankle_roll").c_str(), vals.rar);
00379 if (vals.lap != __initial_vals.lap) (m_config->*my_set_float)(string(path).append("/l_ankle_pitch").c_str(), vals.lap);
00380 if (vals.rap != __initial_vals.rap) (m_config->*my_set_float)(string(path).append("/r_ankle_pitch").c_str(), vals.rap);
00381 }
00382 catch (Exception &e)
00383 {
00384 printf("NaoStiffnessConfigPlugin: Could not write required config values.\n");
00385 e.print_backtrace();
00386 }
00387 }
00388
00389 void
00390 NaoStiffnessConfigPlugin::pre_run()
00391 {
00392 load_vals();
00393 }
00394
00395 void
00396 NaoStiffnessConfigPlugin::post_run(int response)
00397 {
00398 switch (response)
00399 {
00400 case Gtk::RESPONSE_OK:
00401 save_vals();
00402 break;
00403
00404 case Gtk::RESPONSE_CANCEL:
00405 case Gtk::RESPONSE_DELETE_EVENT:
00406 break;
00407
00408 default:
00409 printf("Nao stiffness config plugin: unknown response\n");
00410 break;
00411 }
00412 }
00413
00414 Gtk::Dialog*
00415 NaoStiffnessConfigPlugin::load_dialog()
00416 {
00417 NaoStiffnessConfigDialog *dlg = NULL;
00418 m_ref_xml->get_widget_derived("PluginDialog", dlg);
00419 dlg->set_load_vals(mem_fun(*this, &NaoStiffnessConfigPlugin::load_vals));
00420
00421 return dlg;
00422 }