naostiffness_config_plugin.cpp

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 
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 /** @class NaoStiffnessConfigDialog naostiffness_config_plugin.h
00039  * Config dialog of the config editor plugin for the nao joint stiffnesses.
00040  * @author Tobias Kellner
00041  */
00042 
00043 /** Constructor.
00044  * Allows to construct a dialog by means of get_widget_derived(...).
00045  * @param cobject base object pointer
00046  * @param ref_xml Glade XML object representing the Glade input file
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 /** Destructor. */
00090 NaoStiffnessConfigDialog::~NaoStiffnessConfigDialog()
00091 {
00092 }
00093 
00094 /** Set joint stiffness values in the dialog
00095  * @param vals structure containing the stiffness values
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 /** Get joint stiffness values from the dialog
00139  * @param vals structure the stiffness values get written to
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 /** Lock checkbox toggled handler. */
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 /** Behaviour combobox changed handler. */
00223 void NaoStiffnessConfigDialog::on_combobox_behaviour_changed()
00224 {
00225   __bhv->get_active()->get_value(0, __cur_bhv);
00226   __load_vals();
00227 }
00228 
00229 /** Return currently selected behaviour.
00230  * @return a string representing the selected behaviour
00231  */
00232 string NaoStiffnessConfigDialog::get_cur_behaviour()
00233 {
00234   return __cur_bhv;
00235 }
00236 
00237 /** Return whether default checkbox is checked.
00238  * @return true if default is checked
00239  */
00240 bool NaoStiffnessConfigDialog::get_save_default()
00241 {
00242   return __def->get_active();
00243 }
00244 
00245 /** Set the callback function for loading values in the plugin.
00246  * Config is not accessible in the dialog, so it has to be done there.
00247  * @param cb the callback
00248  */
00249 void NaoStiffnessConfigDialog::set_load_vals(sigc::slot<void> cb)
00250 {
00251   __load_vals = cb;
00252 }
00253 
00254 
00255 /** @class NaoStiffnessConfigPlugin naostiffness_config_plugin.h
00256  * Config editor plugin for the Nao joint stiffness values.
00257  * @author Tobias Kellner
00258  */
00259 
00260 /** Constructor.
00261  * @param glade_path path to the Glade file for the plugin's dialog
00262  */
00263 NaoStiffnessConfigPlugin::NaoStiffnessConfigPlugin(string glade_path)
00264   : ConfigEditorPlugin(STIFFNESS_CFG_PATH, glade_path)
00265 {
00266 }
00267 
00268 /** Destructor. */
00269 NaoStiffnessConfigPlugin::~NaoStiffnessConfigPlugin()
00270 {
00271 }
00272 
00273 /** Load joint stiffness values from config */
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 /** Save joint stiffness values to config */
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: //fall through
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 }

Generated on Tue Feb 22 13:31:30 2011 for Fawkes API by  doxygen 1.4.7