hppwidgetsplugin.cc 24.1 KB
Newer Older
1
2
3
4
5
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel and Heidy Dallard
//

6
#include "hppwidgetsplugin/hppwidgetsplugin.hh"
7

8
#include <boost/regex.hpp>
9
#include <boost/algorithm/string.hpp>
10

11
#include <QDockWidget>
12
#include <QMessageBox>
13

14
15
16
#include <gepetto/gui/mainwindow.hh>
#include <gepetto/gui/windows-manager.hh>
#include <gepetto/gui/omniorb/url.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
17
#include <gepetto/gui/action-search-bar.hh>
18

Florent Lamiraux's avatar
Florent Lamiraux committed
19
20
#include <omniORB4/CORBA.h>

Joseph Mirabel's avatar
Joseph Mirabel committed
21
22
23
24
25
#include "hppwidgetsplugin/pathplayer.hh"
#include "hppwidgetsplugin/solverwidget.hh"
#include "hppwidgetsplugin/jointtreewidget.hh"
#include "hppwidgetsplugin/configurationlistwidget.hh"
#include "hppwidgetsplugin/joint-tree-item.hh"
26
#include "hppwidgetsplugin/constraintwidget.hh"
27
28
#include "hppwidgetsplugin/twojointsconstraint.hh"
#include "hppwidgetsplugin/listjointconstraint.hh"
29
#include "hppwidgetsplugin/conversions.hh"
Joseph Mirabel's avatar
Joseph Mirabel committed
30
#include "hppwidgetsplugin/joint-action.hh"
31

32
#include "hppwidgetsplugin/roadmap.hh"
33

Florent Lamiraux's avatar
Florent Lamiraux committed
34
35
using CORBA::ULong;

Joseph Mirabel's avatar
Joseph Mirabel committed
36
37
namespace hpp {
  namespace gui {
38
    using gepetto::gui::MainWindow;
39
40
    typedef graphics::WindowsManager::Color_t OsgColor_t;
    typedef graphics::Configuration OsgConfiguration_t;
Joseph Mirabel's avatar
Joseph Mirabel committed
41
    typedef gepetto::gui::ActionSearchBar ActionSearchBar;
42

43
    HppWidgetsPlugin::JointElement::JointElement (
44
45
46
47
        const std::string& n, const std::string& prefix,
        const hpp::Names_t& bns, JointTreeItem* i, bool updateV)
      : name (n), prefix (prefix),
      bodyNames (bns.length()), item (i), updateViewer (bns.length(), updateV)
48
49
    {
      for (std::size_t i = 0; i < bns.length(); ++i)
50
        bodyNames[i] = std::string(bns[(CORBA::ULong)i]);
51
52
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
53
54
55
56
    HppWidgetsPlugin::HppWidgetsPlugin() :
      pathPlayer_ (NULL),
      solverWidget_ (NULL),
      configListWidget_ (NULL),
57
58
      hpp_ (NULL),
      jointTreeWidget_ (NULL)
Joseph Mirabel's avatar
Joseph Mirabel committed
59
    {
60
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
61
62
63

    HppWidgetsPlugin::~HppWidgetsPlugin()
    {
64
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
65
66
67
68
69
      foreach (QDockWidget* dock, dockWidgets_) {
        main->removeDockWidget(dock);
        delete dock;
      }
      closeConnection ();
70
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
71
72
73
74
75

    void HppWidgetsPlugin::init()
    {
      openConnection();

76
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
77
78
79
80
      QDockWidget* dock;

      // Configuration list widget
      dock = new QDockWidget ("&Configuration List", main);
81
      dock->setObjectName ("hppwidgetplugin.configurationlist");
Joseph Mirabel's avatar
Joseph Mirabel committed
82
83
      configListWidget_ = new ConfigurationListWidget (this, dock);
      dock->setWidget(configListWidget_);
84
      main->insertDockWidget (dock, Qt::RightDockWidgetArea, Qt::Vertical);
85
      dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + Qt::Key_C);
Joseph Mirabel's avatar
Joseph Mirabel committed
86
      dockWidgets_.append(dock);
87
      main->registerShortcut("Configuration List", "Toggle view", dock->toggleViewAction());
Joseph Mirabel's avatar
Joseph Mirabel committed
88
89
90

      // Solver widget
      dock = new QDockWidget ("Problem &solver", main);
91
      dock->setObjectName ("hppwidgetplugin.problemsolver");
Joseph Mirabel's avatar
Joseph Mirabel committed
92
93
      solverWidget_ = new SolverWidget (this, dock);
      dock->setWidget(solverWidget_);
Joseph Mirabel's avatar
Joseph Mirabel committed
94
      main->insertDockWidget (dock, Qt::RightDockWidgetArea, Qt::Horizontal);
95
      dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + Qt::Key_S);
Joseph Mirabel's avatar
Joseph Mirabel committed
96
      dockWidgets_.append(dock);
97
      main->registerShortcut("Problem solver", "Toggle view", dock->toggleViewAction());
Joseph Mirabel's avatar
Joseph Mirabel committed
98
99
100

      // Path player widget
      dock = new QDockWidget ("&Path player", main);
101
      dock->setObjectName ("hppwidgetplugin.pathplayer");
Joseph Mirabel's avatar
Joseph Mirabel committed
102
103
104
      pathPlayer_ = new PathPlayer (this, dock);
      dock->setWidget(pathPlayer_);
      main->insertDockWidget (dock, Qt::BottomDockWidgetArea, Qt::Horizontal);
105
      dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + Qt::Key_P);
Joseph Mirabel's avatar
Joseph Mirabel committed
106
      dockWidgets_.append(dock);
107
      main->registerShortcut("PathPlayer", "Toggle view", dock->toggleViewAction());
Joseph Mirabel's avatar
Joseph Mirabel committed
108
109
110

      // Joint tree widget
      dock = new QDockWidget ("&Joint Tree", main);
111
      dock->setObjectName ("hppwidgetplugin.jointtree");
Joseph Mirabel's avatar
Joseph Mirabel committed
112
113
114
115
      jointTreeWidget_ = new JointTreeWidget (this, dock);
      dock->setWidget(jointTreeWidget_);
      jointTreeWidget_->dockWidget (dock);
      main->insertDockWidget (dock, Qt::RightDockWidgetArea, Qt::Vertical);
116
      dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + Qt::Key_J);
Joseph Mirabel's avatar
Joseph Mirabel committed
117
      dockWidgets_.append(dock);
118
      main->registerShortcut("JointTree", "Toggle view", dock->toggleViewAction());
Joseph Mirabel's avatar
Joseph Mirabel committed
119

120
      loadConstraintWidget();
121

Joseph Mirabel's avatar
Joseph Mirabel committed
122
123
      // Connect widgets
      connect (solverWidget_, SIGNAL (problemSolved ()), pathPlayer_, SLOT (update()));
Joseph Mirabel's avatar
Joseph Mirabel committed
124
125

      connect (main, SIGNAL (refresh()), SLOT (update()));
Joseph Mirabel's avatar
Joseph Mirabel committed
126
127

      connect (main, SIGNAL (configurationValidation ()),
128
129
130
131
132
          SLOT (configurationValidation ()));
      main->connect (this, SIGNAL (configurationValidationStatus (bool)),
          SLOT (configurationValidationStatusChanged (bool)));
      main->connect (this, SIGNAL (configurationValidationStatus (QStringList)),
          SLOT (configurationValidationStatusChanged (QStringList)));
Joseph Mirabel's avatar
Joseph Mirabel committed
133
      connect (main, SIGNAL (applyCurrentConfiguration()),
134
          SLOT (applyCurrentConfiguration()));
Joseph Mirabel's avatar
Joseph Mirabel committed
135
      connect (main, SIGNAL (selectJointFromBodyName (QString)),
Joseph Mirabel's avatar
Joseph Mirabel committed
136
          SLOT (selectJointFromBodyName (QString)), Qt::QueuedConnection);
137
138
139
140
      main->connect (this, SIGNAL (logJobFailed(int,QString)),
          SLOT (logJobFailed(int, QString)));
      main->connect (this, SIGNAL (logSuccess(QString)), SLOT (log(QString)));
      main->connect (this, SIGNAL (logFailure(QString)), SLOT (logError(QString)));
Joseph Mirabel's avatar
Joseph Mirabel committed
141
142
143

      main->osg()->createGroup("joints");
      main->osg()->addToGroup("joints", "hpp-gui");
144
      main->osg()->refresh();
145
146

      main->registerSlot("requestCreateJointGroup", this);
Joseph Mirabel's avatar
Joseph Mirabel committed
147
      main->registerSlot("requestCreateComGroup", this);
Joseph Mirabel's avatar
Joseph Mirabel committed
148
149
150
      main->registerSlot("setRobotVelocity", pathPlayer_);
      main->registerSlot("lengthBetweenRefresh", pathPlayer_);
      main->registerSlot("getCurrentPath", pathPlayer_);
151
      main->registerSlot("getHppIIOPurl", this);
Joseph Mirabel's avatar
Joseph Mirabel committed
152
      main->registerSlot("getHppContext", this);
Joseph Mirabel's avatar
Joseph Mirabel committed
153
154
      main->registerSlot("getCurrentConfig", this);
      main->registerSlot("setCurrentConfig", this);
155
      main->registerSlot("getSelectedJoint", jointTreeWidget_);
156
      main->registerSignal(SIGNAL(appliedConfigAtParam(int,double)), pathPlayer_);
157
158
159
160
161
162
      QAction* action = main->findChild<QAction*>("actionFetch_configuration");
      if (action != NULL) connect (action, SIGNAL(triggered()), SLOT(fetchConfiguration()));
      else qDebug () << "Action actionFetch_configuration not found";
      action = main->findChild<QAction*>("actionSend_configuration");
      if (action != NULL) connect (action, SIGNAL(triggered()), SLOT(sendConfiguration()));
      else qDebug () << "Action actionSend_configuration not found";
Joseph Mirabel's avatar
Joseph Mirabel committed
163
164
165
166
167
168
169
170
171
172
173

      ActionSearchBar* asb = main->actionSearchBar();
      JointAction* a;

      a = new JointAction (tr("Add joint &frame"), jointTreeWidget_, this);
      connect (a, SIGNAL (triggered(std::string)), SLOT (addJointFrame(std::string)));
      asb->addAction(a);

      a = new JointAction (tr("Display &roadmap"), jointTreeWidget_, this);
      connect (a, SIGNAL (triggered(std::string)), SLOT (displayRoadmap(std::string)));
      asb->addAction(a);
Joseph Mirabel's avatar
Joseph Mirabel committed
174
175
176
177
178
179
180
    }

    QString HppWidgetsPlugin::name() const
    {
      return QString ("Widgets for hpp-corbaserver");
    }

181
    void HppWidgetsPlugin::loadRobotModel(gepetto::gui::DialogLoadRobot::RobotDefinition rd)
Joseph Mirabel's avatar
Joseph Mirabel committed
182
183
    {
      client()->robot()->loadRobotModel(
Joseph Mirabel's avatar
Joseph Mirabel committed
184
185
186
187
188
189
          to_corba(rd.robotName_    ).in(),
          to_corba(rd.rootJointType_).in(),
          to_corba(rd.package_      ).in(),
          to_corba(rd.modelName_    ).in(),
          to_corba(rd.urdfSuf_      ).in(),
          to_corba(rd.srdfSuf_      ).in());
190
191
      // This is already done in requestRefresh
      // jointTreeWidget_->reload();
192
      gepetto::gui::MainWindow::instance()->requestRefresh();
Joseph Mirabel's avatar
Joseph Mirabel committed
193
      gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
194
      emit logSuccess ("Robot " + rd.name_ + " loaded");
Joseph Mirabel's avatar
Joseph Mirabel committed
195
196
    }

197
    void HppWidgetsPlugin::loadEnvironmentModel(gepetto::gui::DialogLoadEnvironment::EnvironmentDefinition ed)
Joseph Mirabel's avatar
Joseph Mirabel committed
198
199
200
    {
      QString prefix = ed.envName_ + "/";
      client()->obstacle()->loadObstacleModel(
Joseph Mirabel's avatar
Joseph Mirabel committed
201
202
203
          to_corba(ed.package_     ).in(),
          to_corba(ed.urdfFilename_).in(),
          to_corba(prefix          ).in());
Joseph Mirabel's avatar
Joseph Mirabel committed
204
      computeObjectPosition ();
205
      gepetto::gui::MainWindow::instance()->requestRefresh();
206
      emit logSuccess ("Environment " + ed.name_ + " loaded");
207
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
208
209
210
211
212

    std::string HppWidgetsPlugin::getBodyFromJoint(const std::string &jointName) const
    {
      JointMap::const_iterator itj = jointMap_.find(jointName);
      if (itj == jointMap_.constEnd()) return std::string();
213
      return itj->prefix + itj->bodyNames[0];
Joseph Mirabel's avatar
Joseph Mirabel committed
214
215
    }

216
217
218
    void HppWidgetsPlugin::fetchConfiguration ()
    {
      hpp::floatSeq_var c = client()->robot ()->getCurrentConfig ();
219
220
221
222
223
224
      setCurrentConfig (c.in());
    }

    void HppWidgetsPlugin::sendConfiguration ()
    {
      client()->robot ()->setCurrentConfig (config_);
225
226
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
227
228
229
    void HppWidgetsPlugin::setCurrentConfig (const hpp::floatSeq& q)
    {
      config_ = q;
Joseph Mirabel's avatar
Joseph Mirabel committed
230
      MainWindow::instance()->requestApplyCurrentConfiguration();
Joseph Mirabel's avatar
Joseph Mirabel committed
231
232
233
234
235
236
237
    }

    hpp::floatSeq const* HppWidgetsPlugin::getCurrentConfig () const
    {
      return &config_;
    }

238
239
240
241
242
243
244
245
246
247
248
249
250
251
    void HppWidgetsPlugin::setCurrentQtConfig (const QVector<double>& q)
    {
      config_  .length (q.size());
      for (ULong i = 0; i < config_.length(); ++i) config_[i] = q[i];
      MainWindow::instance()->requestApplyCurrentConfiguration();
    }

    QVector<double> HppWidgetsPlugin::getCurrentQtConfig () const
    {
      QVector<double> c (config_.length());
      for (ULong i = 0; i < config_.length(); ++i) c[i] = config_[i];
      return c;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
252
253
254
255
256
257
    bool HppWidgetsPlugin::corbaException(int jobId, const CORBA::Exception &excep) const
    {
      try {
        const hpp::Error& error = dynamic_cast <const hpp::Error&> (excep);
        emit logJobFailed(jobId, QString (error.msg));
        return true;
Joseph Mirabel's avatar
Joseph Mirabel committed
258
259
      } catch (const std::bad_cast&) {
        // dynamic_cast failed.
Joseph Mirabel's avatar
Joseph Mirabel committed
260
261
      }
      return false;
262
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
263

264
    QString HppWidgetsPlugin::getHppIIOPurl () const
Joseph Mirabel's avatar
Joseph Mirabel committed
265
    {
266
      QString host = gepetto::gui::MainWindow::instance ()->settings_->getSetting
Joseph Mirabel's avatar
Joseph Mirabel committed
267
        ("hpp/host", QString ()).toString ();
268
      QString port = gepetto::gui::MainWindow::instance ()->settings_->getSetting
Joseph Mirabel's avatar
Joseph Mirabel committed
269
        ("hpp/port", QString ()).toString ();
270
      return gepetto::gui::omniOrb::IIOPurl (host, port);
Joseph Mirabel's avatar
Joseph Mirabel committed
271
272
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
273
274
275
276
277
278
279
    QString HppWidgetsPlugin::getHppContext () const
    {
      QString context = gepetto::gui::MainWindow::instance ()->settings_->getSetting
        ("hpp/context", QString ()).toString ();
      return context;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
280
281
282
283
    void HppWidgetsPlugin::openConnection ()
    {
      closeConnection ();
      hpp_ = new hpp::corbaServer::Client (0,0);
Joseph Mirabel's avatar
Joseph Mirabel committed
284
285
286
      QByteArray iiop    = getHppIIOPurl ().toLatin1();
      QByteArray context = getHppContext ().toLatin1();
      hpp_->connect (iiop.constData (), context.constData ());
Joseph Mirabel's avatar
Joseph Mirabel committed
287
288
289
290
291
292
293
294
    }

    void HppWidgetsPlugin::closeConnection ()
    {
      if (hpp_) delete hpp_;
      hpp_ = NULL;
    }

295
296
297
298
299
300
301
302
303
    inline char* c_str (const std::string& in)
    {
      char* out = new char[in.length()+1];
      strcpy (out, in.c_str());
      return out;
    }

    void HppWidgetsPlugin::prepareApplyConfiguration()
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
304
      bodyNames_.clear();
305
306
      config_  .length (client()->robot()->getConfigSize());
      velocity_.length (client()->robot()->getNumberDof ());
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
      gepetto::gui::MainWindow * main = gepetto::gui::MainWindow::instance ();
      CORBA::ULong size = 0; const CORBA::ULong sall = 100;
      linkNames_.length(sall);
      for (JointMap::iterator ite = jointMap_.begin ();
          ite != jointMap_.end (); ite++) {
        for (std::size_t i = 0; i < ite->bodyNames.size(); ++i)
        {
          std::string bodyName = ite->prefix + ite->bodyNames[i];
          ite->updateViewer[i] = main->osg()->nodeExists (bodyName);
          if (ite->updateViewer[i]) {
            if (size >= linkNames_.length()) {
              // Allocate
              linkNames_.length(linkNames_.length() + sall);
            }
            linkNames_[size] = c_str(ite->bodyNames[i]);
            ++size;
            bodyNames_.push_back(bodyName);
          }
        }
      }
      linkNames_.length(size);
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
330
331
    void HppWidgetsPlugin::applyCurrentConfiguration()
    {
332
      gepetto::gui::MainWindow * main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
333
      if (jointMap_.isEmpty()) {
Joseph Mirabel's avatar
Joseph Mirabel committed
334
335
336
337
338
339
340
341
342
343
344
          if (QMessageBox::Ok == QMessageBox::question (NULL, "Refresh required",
                                 "The current configuration cannot be applied because the joint map is empty. "
                                 "This is probably because you are using external commands (python "
                                 "interface) and you did not refresh this GUI. "
                                 "Do you want to refresh the joint map now ?"))
            jointTreeWidget_->reload();
          else
            emit logFailure("The current configuration cannot be applied. "
                            "This is probably because you are using external commands (python "
                            "interface) and you did not refresh this GUI. "
                            "Use the refresh button \"Tools\" menu.");
Joseph Mirabel's avatar
Joseph Mirabel committed
345
      }
346
      hpp::TransformSeq_var Ts = client()->robot ()->getLinksPosition (config_, linkNames_);
347
348
349
      fromHPP (Ts, bodyConfs_);
      main->osg()->applyConfigurations (bodyNames_, bodyConfs_);

Joseph Mirabel's avatar
Joseph Mirabel committed
350
351
352
      for (JointMap::iterator ite = jointMap_.begin ();
          ite != jointMap_.end (); ite++) {
        if (!ite->item) continue;
353
        if (ite->item->config().length() > 0) {
354
          ite->item->updateFromRobotConfig (config_);
355
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
356
      }
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
      Ts = client()->robot()->getJointsPosition(config_, jointFrames_);
      fromHPP (Ts, bodyConfs_);
      main->osg()->applyConfigurations (jointGroupNames_, bodyConfs_);

      if (comFrames_.size() > 0) {
        static bool firstTime = true;
        if (firstTime) {
          main->log ("COM frames is not thread safe. Use with care.");
          firstTime = false;
        }
        OsgConfiguration_t T;
        T.quat.set(0,0,0,1);
        client()->robot()->setCurrentConfig (config_);
        for (std::list<std::string>::const_iterator it = comFrames_.begin ();
            it != comFrames_.end (); ++it) {
          std::string n = "com_" + escapeJointName(*it);
          hpp::floatSeq_var t = client()->robot()->getPartialCom(it->c_str());
          fromHPP (t, T.position);
          main->osg()->applyConfiguration (n, T);
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
377
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
378
379
380
381
382
      main->osg()->refresh();
    }

    void HppWidgetsPlugin::configurationValidation()
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
383
      bool valid = false;
Joseph Mirabel's avatar
Joseph Mirabel committed
384
385
      CORBA::String_var report;
      try {
Joseph Mirabel's avatar
Joseph Mirabel committed
386
        client()->robot()->isConfigValid (config_, valid, report);
Joseph Mirabel's avatar
Joseph Mirabel committed
387
      } catch (const hpp::Error& e) {
388
        emit logFailure(QString (e.msg));
Joseph Mirabel's avatar
Joseph Mirabel committed
389
390
        return;
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
391
      static QRegExp collision ("Collision between object (.*) and (.*)");
Joseph Mirabel's avatar
Joseph Mirabel committed
392
      QStringList col;
Joseph Mirabel's avatar
Joseph Mirabel committed
393
      if (!valid) {
Joseph Mirabel's avatar
Joseph Mirabel committed
394
        if (collision.exactMatch(QString::fromLocal8Bit(report))) {
395
396
397
          CORBA::String_var robotName = client ()->robot()->getRobotName();
          size_t pos = strlen(robotName) + 1;
          for (int i = 1; i < 3; ++i) {
Joseph Mirabel's avatar
Joseph Mirabel committed
398
399
400
            std::string c = collision.cap (i).toStdString();
            bool found = false;
            foreach (const JointElement& je, jointMap_) {
Joseph Mirabel's avatar
Joseph Mirabel committed
401
402
              for (std::size_t j = 0; j < je.bodyNames.size(); ++j) {
                if (je.bodyNames[j].length() <= pos)
403
                  continue;
Joseph Mirabel's avatar
Joseph Mirabel committed
404
405
406
                size_t len = je.bodyNames[j].length() - pos;
                if (je.bodyNames[j].compare(pos, len, c, 0, len) == 0) {
                  col.append(QString::fromStdString(je.bodyNames[j]));
407
408
409
                  found = true;
                  break;
                }
Joseph Mirabel's avatar
Joseph Mirabel committed
410
              }
411
              if (found) break;
412
            }
Joseph Mirabel's avatar
Joseph Mirabel committed
413
414
            if (!found) col.append(collision.cap (i));
          }
415
416
417
418
        } else {
          qDebug () << report;
          col.append(QString::fromLocal8Bit(client ()->robot()->getRobotName()));
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
419
420
421
        qDebug () << col;
      }
      emit configurationValidationStatus (col);
422
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
423

Joseph Mirabel's avatar
Joseph Mirabel committed
424
    void HppWidgetsPlugin::selectJointFromBodyName(const QString bodyName)
Joseph Mirabel's avatar
Joseph Mirabel committed
425
426
427
    {
      boost::regex roadmap ("^(roadmap|path[0-9]+)_(.*)/(node|edge)([0-9]+)$");
      boost::cmatch what;
Joseph Mirabel's avatar
Joseph Mirabel committed
428
429
      const std::string bname = bodyName.toStdString();
      if (boost::regex_match (bname.c_str(), what, roadmap)) {
Joseph Mirabel's avatar
Joseph Mirabel committed
430
431
432
        std::string group; group.assign(what[1].first, what[1].second);
        std::string joint; joint.assign(what[2].first, what[2].second);
        std::string type;  type .assign(what[3].first, what[3].second);
433
        CORBA::ULong n = (CORBA::ULong)std::atoi (what[4].first);
Joseph Mirabel's avatar
Joseph Mirabel committed
434
        qDebug () << "Detected the" << group.c_str() << type.c_str() << n << "of joint" << joint.c_str();
435
436
437
438
        if (group == "roadmap") {
          if (type == "node") {
            try {
              hpp::floatSeq_var q = hpp_->problem()->node(n);
Joseph Mirabel's avatar
Joseph Mirabel committed
439
              setCurrentConfig(q.in());
440
441
442
443
444
445
446
447
448
449
            } catch (const hpp::Error& e) {
              emit logFailure(QString::fromLocal8Bit(e.msg));
            }
          } else if (type == "edge") {
            // TODO
          }
        } else if (group[0] == 'p') {
          if (type == "node") {
            int pid = std::atoi(&group[4]);
            // compute pid
450
451
            hpp::floatSeq_var times;
            hpp::floatSeqSeq_var waypoints = hpp_->problem()->getWaypoints((CORBA::UShort)pid, times.out());
452
            if (n < waypoints->length()) {
Joseph Mirabel's avatar
Joseph Mirabel committed
453
              setCurrentConfig(waypoints[n]);
454
            }
455
456
          }
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
457
458
459
        return;
      }
      foreach (const JointElement& je, jointMap_) {
460
461
462
        // je.bodyNames will be of size 1 most of the time
        // so it is fine to use a vector + line search, vs map + binary
        // FIXME A good intermediate is to sort the vector.
463
464
        const std::size_t len = je.prefix.length();
        if (bname.compare(0, len, je.prefix) == 0) {
465
466
          for (std::size_t i = 0; i < je.bodyNames.size(); ++i) {
            if (bname.compare(len, std::string::npos, je.bodyNames[i]) == 0) {
467
468
469
470
              // TODO: use je.item for a faster selection.
              jointTreeWidget_->selectJoint (je.name);
              return;
            }
471
          }
472
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
473
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
474
      qDebug () << "Joint for body" << bodyName << "not found.";
475
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
476

Joseph Mirabel's avatar
Joseph Mirabel committed
477
478
479
480
481
482
483
    void HppWidgetsPlugin::update()
    {
      jointTreeWidget_->reload();
      pathPlayer_->update();
      solverWidget_->update();
      configListWidget_->fetchInitAndGoalConfigs();
      constraintWidget_->reload();
484
      prepareApplyConfiguration();
Joseph Mirabel's avatar
Joseph Mirabel committed
485
486
    }

487
488
489
490
491
    QString HppWidgetsPlugin::requestCreateJointGroup(const QString jn)
    {
      return createJointGroup(jn.toStdString()).c_str();
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
492
493
494
495
496
    QString HppWidgetsPlugin::requestCreateComGroup(const QString com)
    {
      return QString::fromStdString(createComGroup(com.toStdString()));
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
497
498
499
    HppWidgetsPlugin::HppClient *HppWidgetsPlugin::client() const
    {
      return hpp_;
500
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
501
502
503
504

    HppWidgetsPlugin::JointMap &HppWidgetsPlugin::jointMap()
    {
      return jointMap_;
505
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
506

507
508
509
510
511
    PathPlayer* HppWidgetsPlugin::pathPlayer() const
    {
      return pathPlayer_;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
512
513
514
515
516
    JointTreeWidget* HppWidgetsPlugin::jointTreeWidget() const
    {
      return jointTreeWidget_;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
517
518
519
    void HppWidgetsPlugin::updateRobotJoints(const QString robotName)
    {
      hpp::Names_t_var joints = client()->robot()->getAllJointNames ();
hdallard's avatar
hdallard committed
520
      jointMap_.clear();
Joseph Mirabel's avatar
Joseph Mirabel committed
521
522
      for (size_t i = 0; i < joints->length (); ++i) {
        const char* jname = joints[(ULong) i];
523
        hpp::Names_t_var lnames = client()->robot()->getLinkNames (jname);
524
525
        std::string prefix (robotName.toStdString() + "/");
        jointMap_[jname] = JointElement(jname, prefix, lnames, 0, true);
Joseph Mirabel's avatar
Joseph Mirabel committed
526
527
528
      }
    }

529
    std::string HppWidgetsPlugin::getSelectedJoint() const
Joseph Mirabel's avatar
Joseph Mirabel committed
530
531
532
533
534
535
536
    {
      return jointTreeWidget_->selectedJoint();
    }

    Roadmap* HppWidgetsPlugin::createRoadmap(const std::string &jointName)
    {
      Roadmap* r = new Roadmap (this);
537
      r->initRoadmapFromJoint(jointName);
Joseph Mirabel's avatar
Joseph Mirabel committed
538
539
540
541
542
543
544
545
546
547
548
549
      return r;
    }

    void HppWidgetsPlugin::displayRoadmap(const std::string &jointName)
    {
      Roadmap* r = createRoadmap (jointName);
      r->displayRoadmap();
      delete r;
    }

    void HppWidgetsPlugin::addJointFrame (const std::string& jointName)
    {
550
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
551
552
      std::string target = createJointGroup(jointName);
      const std::string n = target + "/XYZ";
553
      const OsgColor_t color(1,0,0,1);
Joseph Mirabel's avatar
Joseph Mirabel committed
554
555

      /// This returns false if the frame already exists
Joseph Mirabel's avatar
Joseph Mirabel committed
556
      if (main->osg()->addXYZaxis (n, color, 0.005f, 0.015f)) {
557
        main->osg()->setVisibility (n, "ALWAYS_ON_TOP");
Joseph Mirabel's avatar
Joseph Mirabel committed
558
559
        return;
      } else {
560
        main->osg()->setVisibility (n, "ALWAYS_ON_TOP");
Joseph Mirabel's avatar
Joseph Mirabel committed
561
562
563
564
565
      }
    }

    void HppWidgetsPlugin::computeObjectPosition()
    {
566
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
567
      hpp::Names_t_var obs = client()->obstacle()->getObstacleNames (true, false);
568
      hpp::Transform__var cfg = hpp::Transform__alloc () ;
569
570
571
      OsgConfiguration_t d;
      for (ULong i = 0; i < obs->length(); ++i) {
        client()->obstacle()->getObstaclePosition (obs[i], cfg.out());
572
        fromHPP(cfg, d);
573
        main->osg ()->applyConfiguration(std::string(obs[i]), d);
Joseph Mirabel's avatar
Joseph Mirabel committed
574
      }
575
      main->osg()->refresh();
Joseph Mirabel's avatar
Joseph Mirabel committed
576
577
    }

578
579
580
581
    void HppWidgetsPlugin::loadConstraintWidget()
    {
      MainWindow* main = MainWindow::instance();
      QDockWidget* dock = new QDockWidget ("&Constraint creator", main);
582
      dock->setObjectName ("hppwidgetplugin.constraintcreator");
583
584
585
586
587
588
589
590
591
      constraintWidget_ = new ConstraintWidget (this, dock);
      dock->setWidget(constraintWidget_);
      main->insertDockWidget (dock, Qt::RightDockWidgetArea, Qt::Vertical);
      dock->toggleViewAction()->setShortcut(gepetto::gui::DockKeyShortcutBase + Qt::Key_V);
      dockWidgets_.append(dock);
      constraintWidget_->addConstraint(new PositionConstraint(this));
      constraintWidget_->addConstraint(new OrientationConstraint(this));
      constraintWidget_->addConstraint(new TransformConstraint(this));
      constraintWidget_->addConstraint(new LockedJointConstraint(this));
592
      main->registerShortcut("Constraint Widget", "Toggle view", dock->toggleViewAction());
593
594
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
595
596
597
598
599
600
601
602
603
    std::string HppWidgetsPlugin::escapeJointName(const std::string jn)
    {
      std::string target = jn;
      boost::replace_all (target, "/", "__");
      return target;
    }

    std::string HppWidgetsPlugin::createJointGroup(const std::string jn)
    {
604
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
605
      std::string target = escapeJointName(jn);
Joseph Mirabel's avatar
Joseph Mirabel committed
606
607
      graphics::GroupNodePtr_t group = main->osg()->getGroup (target.c_str(), false);
      if (group) return target;
Joseph Mirabel's avatar
Joseph Mirabel committed
608
609
610
      if (!main->osg()->getGroup(target)) {
        main->osg()->createGroup(target);
        main->osg()->addToGroup(target, "joints");
Joseph Mirabel's avatar
Joseph Mirabel committed
611

612
613
        hpp::Transform__var t = client()->robot()->getJointPosition (jn.c_str());
        OsgConfiguration_t p;
614
        fromHPP(t, p);
615
616
617
        jointFrames_.length(jointFrames_.length()+1);
        jointFrames_[jointFrames_.length() -1] = jn.c_str();
        jointGroupNames_.push_back(target);
618
        main->osg()->applyConfiguration (target, p);
Joseph Mirabel's avatar
Joseph Mirabel committed
619
620
621
        main->osg()->refresh();
      }
      return target;
622
623
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
    std::string HppWidgetsPlugin::createComGroup(const std::string com)
    {
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
      std::string target = "com_" + escapeJointName(com);
      graphics::GroupNodePtr_t group = main->osg()->getGroup (target.c_str(), false);
      if (group) return target;
      if (!main->osg()->getGroup(target)) {
        main->osg()->createGroup(target);
        main->osg()->addToGroup(target, "joints");

        hpp::floatSeq_var p = client()->robot()->getPartialCom (com.c_str());
        OsgConfiguration_t t;
        t.quat.set(0,0,0,1);
        fromHPP (p, t.position);
        comFrames_.push_back(com);
        main->osg()->applyConfiguration (target, t);
        main->osg()->refresh();
      }
      return target;
    }

645
#if (QT_VERSION < QT_VERSION_CHECK(5,0,0))
Joseph Mirabel's avatar
Joseph Mirabel committed
646
    Q_EXPORT_PLUGIN2 (hppwidgetsplugin, HppWidgetsPlugin)
647
#endif
Joseph Mirabel's avatar
Joseph Mirabel committed
648
649
  } // namespace gui
} // namespace hpp