Fawkes API Fawkes Development Version
|
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 }