hppwidgetsplugin.cc 21.6 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
#include <gepetto/gui/meta.hh>
34

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

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

44
    HppWidgetsPlugin::JointElement::JointElement (
45
46
47
48
        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)
49
50
    {
      for (std::size_t i = 0; i < bns.length(); ++i)
51
        bodyNames[i] = std::string(bns[(CORBA::ULong)i]);
52
53
    }

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

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

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

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

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

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

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

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

121
      loadConstraintWidget();
122

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

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

      connect (main, SIGNAL (configurationValidation ()),
129
130
131
132
133
          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
134
      connect (main, SIGNAL (applyCurrentConfiguration()),
135
          SLOT (applyCurrentConfiguration()));
Joseph Mirabel's avatar
Joseph Mirabel committed
136
      connect (main, SIGNAL (selectJointFromBodyName (QString)),
Joseph Mirabel's avatar
Joseph Mirabel committed
137
          SLOT (selectJointFromBodyName (QString)), Qt::QueuedConnection);
138
139
140
141
      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
142
143
144

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

      main->registerSlot("requestCreateJointGroup", this);
Joseph Mirabel's avatar
Joseph Mirabel committed
148
      main->registerSlot("requestCreateComGroup", this);
Joseph Mirabel's avatar
Joseph Mirabel committed
149
150
151
      main->registerSlot("setRobotVelocity", pathPlayer_);
      main->registerSlot("lengthBetweenRefresh", pathPlayer_);
      main->registerSlot("getCurrentPath", pathPlayer_);
152
      main->registerSlot("getHppIIOPurl", this);
153
      main->registerSlot("getSelectedJoint", jointTreeWidget_);
154
      main->registerSignal(SIGNAL(appliedConfigAtParam(int,double)), pathPlayer_);
Joseph Mirabel's avatar
Joseph Mirabel committed
155
156
157
158
159
160
161
162
163
164
165

      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
166
167
168
169
170
171
172
    }

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

173
    void HppWidgetsPlugin::loadRobotModel(gepetto::gui::DialogLoadRobot::RobotDefinition rd)
Joseph Mirabel's avatar
Joseph Mirabel committed
174
175
    {
      client()->robot()->loadRobotModel(
176
177
178
179
180
181
          gepetto::gui::Traits<QString>::to_corba(rd.robotName_    ).in(),
          gepetto::gui::Traits<QString>::to_corba(rd.rootJointType_).in(),
          gepetto::gui::Traits<QString>::to_corba(rd.package_      ).in(),
          gepetto::gui::Traits<QString>::to_corba(rd.modelName_    ).in(),
          gepetto::gui::Traits<QString>::to_corba(rd.urdfSuf_      ).in(),
          gepetto::gui::Traits<QString>::to_corba(rd.srdfSuf_      ).in());
182
183
      // This is already done in requestRefresh
      // jointTreeWidget_->reload();
184
      gepetto::gui::MainWindow::instance()->requestRefresh();
Joseph Mirabel's avatar
Joseph Mirabel committed
185
      gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
186
      emit logSuccess ("Robot " + rd.name_ + " loaded");
Joseph Mirabel's avatar
Joseph Mirabel committed
187
188
    }

189
    void HppWidgetsPlugin::loadEnvironmentModel(gepetto::gui::DialogLoadEnvironment::EnvironmentDefinition ed)
Joseph Mirabel's avatar
Joseph Mirabel committed
190
191
192
    {
      QString prefix = ed.envName_ + "/";
      client()->obstacle()->loadObstacleModel(
193
194
195
          gepetto::gui::Traits<QString>::to_corba(ed.package_     ).in(),
          gepetto::gui::Traits<QString>::to_corba(ed.urdfFilename_).in(),
          gepetto::gui::Traits<QString>::to_corba(prefix          ).in());
Joseph Mirabel's avatar
Joseph Mirabel committed
196
      computeObjectPosition ();
197
      gepetto::gui::MainWindow::instance()->requestRefresh();
198
      emit logSuccess ("Environment " + ed.name_ + " loaded");
199
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
200
201
202
203
204

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

    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;
      } catch (const std::exception& exp) {
        qDebug () << exp.what();
      }
      return false;
218
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
219

220
    QString HppWidgetsPlugin::getHppIIOPurl () const
Joseph Mirabel's avatar
Joseph Mirabel committed
221
    {
222
      QString host = gepetto::gui::MainWindow::instance ()->settings_->getSetting
Joseph Mirabel's avatar
Joseph Mirabel committed
223
        ("hpp/host", QString ()).toString ();
224
      QString port = gepetto::gui::MainWindow::instance ()->settings_->getSetting
Joseph Mirabel's avatar
Joseph Mirabel committed
225
        ("hpp/port", QString ()).toString ();
226
      return gepetto::gui::omniOrb::IIOPurl (host, port);
Joseph Mirabel's avatar
Joseph Mirabel committed
227
228
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
229
230
231
232
    void HppWidgetsPlugin::openConnection ()
    {
      closeConnection ();
      hpp_ = new hpp::corbaServer::Client (0,0);
233
      QByteArray iiop = getHppIIOPurl ().toLatin1();
234
      hpp_->connect (iiop.constData ());
Joseph Mirabel's avatar
Joseph Mirabel committed
235
236
237
238
239
240
241
242
243
244
    }

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

    void HppWidgetsPlugin::applyCurrentConfiguration()
    {
245
      gepetto::gui::MainWindow * main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
246
      if (jointMap_.isEmpty()) {
Joseph Mirabel's avatar
Joseph Mirabel committed
247
248
249
250
251
252
253
254
255
256
257
          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
258
      }
259
260
261
      // Something smarter could be done here.
      // For instance, the joint tree item could know the NodePtr_t of their bodies.
      OsgConfiguration_t T;
Joseph Mirabel's avatar
Joseph Mirabel committed
262
263
      for (JointMap::iterator ite = jointMap_.begin ();
          ite != jointMap_.end (); ite++) {
264
265
266
        for (std::size_t i = 0; i < ite->bodyNames.size(); ++i)
        {
          hpp::Transform__var t = client()->robot()->getLinkPosition(ite->bodyNames[i].c_str());
267
268
269
          fromHPP(t, T);
          if (ite->updateViewer[i]) {
            std::string n = ite->prefix + ite->bodyNames[i];
270
            ite->updateViewer[i] = main->osg()->applyConfiguration(n, T);
271
          }
272
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
273
        if (!ite->item) continue;
274
        if (ite->item->config().length() > 0) {
275
          ite->item->updateFromRobotConfig (c.in());
276
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
277
278
279
280
281
      }
      for (std::list<std::string>::const_iterator it = jointFrames_.begin ();
          it != jointFrames_.end (); ++it) {
        std::string n = escapeJointName(*it);
        hpp::Transform__var t = client()->robot()->getJointPosition(it->c_str());
282
        fromHPP(t, T);
283
        main->osg()->applyConfiguration (n, T);
Joseph Mirabel's avatar
Joseph Mirabel committed
284
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
285
286
287
288
289
290
291
292
      T.quat.set(0,0,0,1);
      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
293
294
295
296
297
298
299
300
301
302
303
304
      main->osg()->refresh();
    }

    void HppWidgetsPlugin::configurationValidation()
    {
      hpp::floatSeq_var q = client()->robot()->getCurrentConfig ();
      bool bb = false;
      CORBA::Boolean_out b = bb;
      CORBA::String_var report;
      try {
        client()->robot()->isConfigValid (q.in(), b, report);
      } catch (const hpp::Error& e) {
305
        emit logFailure(QString (e.msg));
Joseph Mirabel's avatar
Joseph Mirabel committed
306
307
        return;
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
308
      static QRegExp collision ("Collision between object (.*) and (.*)");
Joseph Mirabel's avatar
Joseph Mirabel committed
309
310
311
      QStringList col;
      if (!bb) {
        if (collision.exactMatch(QString::fromLocal8Bit(report))) {
312
313
314
          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
315
316
317
            std::string c = collision.cap (i).toStdString();
            bool found = false;
            foreach (const JointElement& je, jointMap_) {
Joseph Mirabel's avatar
Joseph Mirabel committed
318
319
              for (std::size_t j = 0; j < je.bodyNames.size(); ++j) {
                if (je.bodyNames[j].length() <= pos)
320
                  continue;
Joseph Mirabel's avatar
Joseph Mirabel committed
321
322
323
                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]));
324
325
326
                  found = true;
                  break;
                }
Joseph Mirabel's avatar
Joseph Mirabel committed
327
              }
328
              if (found) break;
329
            }
Joseph Mirabel's avatar
Joseph Mirabel committed
330
331
            if (!found) col.append(collision.cap (i));
          }
332
333
334
335
        } else {
          qDebug () << report;
          col.append(QString::fromLocal8Bit(client ()->robot()->getRobotName()));
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
336
337
338
        qDebug () << col;
      }
      emit configurationValidationStatus (col);
339
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
340

Joseph Mirabel's avatar
Joseph Mirabel committed
341
    void HppWidgetsPlugin::selectJointFromBodyName(const QString bodyName)
Joseph Mirabel's avatar
Joseph Mirabel committed
342
343
344
    {
      boost::regex roadmap ("^(roadmap|path[0-9]+)_(.*)/(node|edge)([0-9]+)$");
      boost::cmatch what;
Joseph Mirabel's avatar
Joseph Mirabel committed
345
346
      const std::string bname = bodyName.toStdString();
      if (boost::regex_match (bname.c_str(), what, roadmap)) {
Joseph Mirabel's avatar
Joseph Mirabel committed
347
348
349
        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);
350
        CORBA::ULong n = (CORBA::ULong)std::atoi (what[4].first);
Joseph Mirabel's avatar
Joseph Mirabel committed
351
        qDebug () << "Detected the" << group.c_str() << type.c_str() << n << "of joint" << joint.c_str();
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
        if (group == "roadmap") {
          if (type == "node") {
            try {
              hpp::floatSeq_var q = hpp_->problem()->node(n);
              hpp_->robot()->setCurrentConfig(q.in());
              gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
            } 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
368
369
            hpp::floatSeq_var times;
            hpp::floatSeqSeq_var waypoints = hpp_->problem()->getWaypoints((CORBA::UShort)pid, times.out());
370
371
372
373
            if (n < waypoints->length()) {
              hpp_->robot()->setCurrentConfig(waypoints[n]);
              MainWindow::instance()->requestApplyCurrentConfiguration();
            }
374
375
          }
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
376
377
378
        return;
      }
      foreach (const JointElement& je, jointMap_) {
379
380
381
        // 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.
382
383
        const std::size_t len = je.prefix.length();
        if (bname.compare(0, len, je.prefix) == 0) {
384
385
          for (std::size_t i = 0; i < je.bodyNames.size(); ++i) {
            if (bname.compare(len, std::string::npos, je.bodyNames[i]) == 0) {
386
387
388
389
              // TODO: use je.item for a faster selection.
              jointTreeWidget_->selectJoint (je.name);
              return;
            }
390
          }
391
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
392
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
393
      qDebug () << "Joint for body" << bodyName << "not found.";
394
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
395

Joseph Mirabel's avatar
Joseph Mirabel committed
396
397
398
399
400
401
402
403
404
    void HppWidgetsPlugin::update()
    {
      jointTreeWidget_->reload();
      pathPlayer_->update();
      solverWidget_->update();
      configListWidget_->fetchInitAndGoalConfigs();
      constraintWidget_->reload();
    }

405
406
407
408
409
    QString HppWidgetsPlugin::requestCreateJointGroup(const QString jn)
    {
      return createJointGroup(jn.toStdString()).c_str();
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
410
411
412
413
414
    QString HppWidgetsPlugin::requestCreateComGroup(const QString com)
    {
      return QString::fromStdString(createComGroup(com.toStdString()));
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
415
416
417
    HppWidgetsPlugin::HppClient *HppWidgetsPlugin::client() const
    {
      return hpp_;
418
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
419
420
421
422

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

425
426
427
428
429
    PathPlayer* HppWidgetsPlugin::pathPlayer() const
    {
      return pathPlayer_;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
430
431
432
433
434
    JointTreeWidget* HppWidgetsPlugin::jointTreeWidget() const
    {
      return jointTreeWidget_;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
435
436
437
    void HppWidgetsPlugin::updateRobotJoints(const QString robotName)
    {
      hpp::Names_t_var joints = client()->robot()->getAllJointNames ();
hdallard's avatar
hdallard committed
438
      jointMap_.clear();
Joseph Mirabel's avatar
Joseph Mirabel committed
439
440
      for (size_t i = 0; i < joints->length (); ++i) {
        const char* jname = joints[(ULong) i];
441
        hpp::Names_t_var lnames = client()->robot()->getLinkNames (jname);
442
443
        std::string prefix (robotName.toStdString() + "/");
        jointMap_[jname] = JointElement(jname, prefix, lnames, 0, true);
Joseph Mirabel's avatar
Joseph Mirabel committed
444
445
446
      }
    }

447
    std::string HppWidgetsPlugin::getSelectedJoint() const
Joseph Mirabel's avatar
Joseph Mirabel committed
448
449
450
451
452
453
454
    {
      return jointTreeWidget_->selectedJoint();
    }

    Roadmap* HppWidgetsPlugin::createRoadmap(const std::string &jointName)
    {
      Roadmap* r = new Roadmap (this);
455
      r->initRoadmapFromJoint(jointName);
Joseph Mirabel's avatar
Joseph Mirabel committed
456
457
458
459
460
461
462
463
464
465
466
467
      return r;
    }

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

    void HppWidgetsPlugin::addJointFrame (const std::string& jointName)
    {
468
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
469
470
      std::string target = createJointGroup(jointName);
      const std::string n = target + "/XYZ";
471
      const OsgColor_t color(1,0,0,1);
Joseph Mirabel's avatar
Joseph Mirabel committed
472
473

      /// This returns false if the frame already exists
Joseph Mirabel's avatar
Joseph Mirabel committed
474
      if (main->osg()->addXYZaxis (n, color, 0.005f, 0.015f)) {
475
        main->osg()->setVisibility (n, "ALWAYS_ON_TOP");
Joseph Mirabel's avatar
Joseph Mirabel committed
476
477
        return;
      } else {
478
        main->osg()->setVisibility (n, "ALWAYS_ON_TOP");
Joseph Mirabel's avatar
Joseph Mirabel committed
479
480
481
482
483
      }
    }

    void HppWidgetsPlugin::computeObjectPosition()
    {
484
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
485
      hpp::Names_t_var obs = client()->obstacle()->getObstacleNames (true, false);
486
      hpp::Transform__var cfg = hpp::Transform__alloc () ;
487
488
489
      OsgConfiguration_t d;
      for (ULong i = 0; i < obs->length(); ++i) {
        client()->obstacle()->getObstaclePosition (obs[i], cfg.out());
490
        fromHPP(cfg, d);
491
        main->osg ()->applyConfiguration(std::string(obs[i]), d);
Joseph Mirabel's avatar
Joseph Mirabel committed
492
      }
493
      main->osg()->refresh();
Joseph Mirabel's avatar
Joseph Mirabel committed
494
495
    }

496
497
498
499
    void HppWidgetsPlugin::loadConstraintWidget()
    {
      MainWindow* main = MainWindow::instance();
      QDockWidget* dock = new QDockWidget ("&Constraint creator", main);
500
      dock->setObjectName ("hppwidgetplugin.constraintcreator");
501
502
503
504
505
506
507
508
509
      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));
510
      main->registerShortcut("Constraint Widget", "Toggle view", dock->toggleViewAction());
511
512
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
513
514
515
516
517
518
519
520
521
    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)
    {
522
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
Joseph Mirabel's avatar
Joseph Mirabel committed
523
      std::string target = escapeJointName(jn);
Joseph Mirabel's avatar
Joseph Mirabel committed
524
525
      graphics::GroupNodePtr_t group = main->osg()->getGroup (target.c_str(), false);
      if (group) return target;
Joseph Mirabel's avatar
Joseph Mirabel committed
526
527
528
      if (!main->osg()->getGroup(target)) {
        main->osg()->createGroup(target);
        main->osg()->addToGroup(target, "joints");
Joseph Mirabel's avatar
Joseph Mirabel committed
529

530
531
        hpp::Transform__var t = client()->robot()->getJointPosition (jn.c_str());
        OsgConfiguration_t p;
532
        fromHPP(t, p);
Joseph Mirabel's avatar
Joseph Mirabel committed
533
        jointFrames_.push_back(jn);
534
        main->osg()->applyConfiguration (target, p);
Joseph Mirabel's avatar
Joseph Mirabel committed
535
536
537
        main->osg()->refresh();
      }
      return target;
538
539
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
    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;
    }

561
#if (QT_VERSION < QT_VERSION_CHECK(5,0,0))
Joseph Mirabel's avatar
Joseph Mirabel committed
562
    Q_EXPORT_PLUGIN2 (hppwidgetsplugin, HppWidgetsPlugin)
563
#endif
Joseph Mirabel's avatar
Joseph Mirabel committed
564
565
  } // namespace gui
} // namespace hpp