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