pathplayer.cc 10.9 KB
Newer Older
1
2
3
4
5
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel
//

Joseph Mirabel's avatar
Joseph Mirabel committed
6
#include "hppwidgetsplugin/pathplayer.hh"
Joseph Mirabel's avatar
Joseph Mirabel committed
7

Joseph Mirabel's avatar
Joseph Mirabel committed
8
9
#include <sstream>

10
11
#include <QProgressDialog>
#include <QSlider>
Joseph Mirabel's avatar
Joseph Mirabel committed
12
13
14
15
16
#if (QT_VERSION < QT_VERSION_CHECK(5,0,0))
# include <QtCore>
#else
# include <QtConcurrent>
#endif
17

Joseph Mirabel's avatar
Joseph Mirabel committed
18
19
#include <hpp/corbaserver/client.hh>

20
21
22
#include "gepetto/gui/mainwindow.hh"
#include <gepetto/gui/windows-manager.hh>
#include <gepetto/gui/osgwidget.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
23
#include <gepetto/gui/action-search-bar.hh>
Joseph Mirabel's avatar
Joseph Mirabel committed
24

25
#include "hppwidgetsplugin/conversions.hh"
26
#include "hppwidgetsplugin/ui_pathplayerwidget.h"
Joseph Mirabel's avatar
Joseph Mirabel committed
27
#include "hppwidgetsplugin/joint-action.hh"
28
#include <hppwidgetsplugin/hppwidgetsplugin.hh>
29

Joseph Mirabel's avatar
Joseph Mirabel committed
30
31
namespace hpp {
  namespace gui {
Joseph Mirabel's avatar
Joseph Mirabel committed
32
33
34
    using gepetto::gui::MainWindow;
    using gepetto::gui::ActionSearchBar;

Joseph Mirabel's avatar
Joseph Mirabel committed
35
36
37
    PathPlayer::PathPlayer (HppWidgetsPlugin *plugin, QWidget *parent) :
      QWidget (parent)
      , ui_ (new ::Ui::PathPlayerWidget)
38
      , timerId_ (0)
Joseph Mirabel's avatar
Joseph Mirabel committed
39
      , velocity_ (false)
Joseph Mirabel's avatar
Joseph Mirabel committed
40
41
42
43
44
45
46
47
48
49
      , plugin_ (plugin)
    {
      ui_->setupUi (this);
      pathIndex()->setMaximum(0);
      connect (pathSlider(), SIGNAL (sliderMoved (int)), this, SLOT (pathSliderChanged(int)));
      connect (pathIndex(), SIGNAL (valueChanged(int)), this, SLOT (pathIndexChanged(int)));
      connect (playPause(), SIGNAL (toggled (bool)), this, SLOT (playPauseToggled(bool)));
      connect (stop(), SIGNAL (clicked()), this, SLOT (stopClicked()));
      connect (ui_->refreshButton_path, SIGNAL (clicked()), this, SLOT (update()));

Joseph Mirabel's avatar
Joseph Mirabel committed
50
      initSearchActions();
Joseph Mirabel's avatar
Joseph Mirabel committed
51
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
52
53
54
55

    PathPlayer::~PathPlayer()
    {
      delete ui_;
Joseph Mirabel's avatar
Joseph Mirabel committed
56
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
57

Joseph Mirabel's avatar
Joseph Mirabel committed
58
59
60
61
62
63
64
65
66
67
68
69
70
71
    void PathPlayer::initSearchActions()
    {
      ActionSearchBar* asb = MainWindow::instance()->actionSearchBar();
      JointAction* a;

      a = new JointAction (tr("Display &waypoints of selected path"), plugin_->jointTreeWidget(), this);
      connect (a, SIGNAL (triggered(std::string)), SLOT (displayWaypointsOfPath(std::string)));
      asb->addAction(a);

      a = new JointAction (tr("Display selected &path"), plugin_->jointTreeWidget(), this);
      connect (a, SIGNAL (triggered(std::string)), SLOT (displayPath(std::string)));
      asb->addAction(a);
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
72
73
    void PathPlayer::displayWaypointsOfPath(const std::string jointName)
    {
74
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
75
76
77
78
79
80
81
      if (!pathIndex()->isEnabled()) {
        main->logError("There is no path. Did you solve a problem ?");
        return;
      }
      int pid = pathIndex()->value();
      std::stringstream ss; ss << "path" << pid << "_" << jointName;
      std::string pn = ss.str();
82
83
      const osgVector4 colorN (0.f, 0.f, 1.f, 1.f),
                       colorE (1.f, 0.f, 0.f, 1.f);
84
      gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
Joseph Mirabel's avatar
Joseph Mirabel committed
85
      HppWidgetsPlugin::HppClient* hpp = plugin_->client();
86
      hpp::floatSeq_var times;
87
      hpp::floatSeqSeq_var waypoints = hpp->problem()->getWaypoints((CORBA::ULong)pid, times.out());
Joseph Mirabel's avatar
Joseph Mirabel committed
88
89
90
91
      if (!wsm->getGroup(pn, false)) {
        wsm->createGroup (pn);
        wsm->addToGroup(pn, "hpp-gui");
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
92
93
94
95
      // Temporary object to avoid dynamic allocation.
      // Arguments are max, length, storage, take ownership.
      char* tmps[1];
      hpp::Names_t names (1, 1, tmps, false);
96
      gepetto::viewer::Configuration pos;
97
      ::osg::Vec3ArrayRefPtr posSeq = new ::osg::Vec3Array;
Joseph Mirabel's avatar
Joseph Mirabel committed
98
      for (unsigned int i = 0; i < waypoints->length(); ++i) {
99
100
101
102
        // Make name
        ss.clear(); ss.str(std::string()); ss << pn << "/node" << i;
        std::string xyzName = ss.str();
        // Get positions
Joseph Mirabel's avatar
Joseph Mirabel committed
103
104
105
        names[0] = jointName.c_str();
        hpp::TransformSeq_var Ts = hpp->robot()->getJointsPosition(waypoints[i], names);
        fromHPP(Ts[0], pos);
106
        posSeq->push_back(pos.position);
107
        // Create the nodes
Joseph Mirabel's avatar
Joseph Mirabel committed
108
109
        if (wsm->nodeExists(xyzName)) wsm->deleteNode(xyzName, false);
        wsm->addXYZaxis(xyzName, colorN, 0.01f, 0.05f);
110
        wsm->applyConfiguration(xyzName, pos);
Joseph Mirabel's avatar
Joseph Mirabel committed
111
      }
112
      wsm->addCurve(pn + "/curve", posSeq, colorE);
Joseph Mirabel's avatar
Joseph Mirabel committed
113
      wsm->refresh();
114
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
115
116
117
118
119
120
121
122
123
124
125
126

    void PathPlayer::displayPath(const std::string jointName)
    {
      QFutureWatcher <void>* fw = new QFutureWatcher<void>(this);
      QProgressDialog* pd = new QProgressDialog ("Computing curved path. Please wait...", "Cancel", 0, 100, this);
      connect(this, SIGNAL(displayPath_status(int)), pd, SLOT (setValue(int)));
      pd->setCancelButton(0);
      pd->setRange(0, 100);
      pd->show();
      fw->setFuture(QtConcurrent::run (this, &PathPlayer::displayPath_impl, jointName));
      connect (fw, SIGNAL (finished()), pd, SLOT (deleteLater()));
      connect (fw, SIGNAL (finished()), fw, SLOT (deleteLater()));
Joseph Mirabel's avatar
Joseph Mirabel committed
127
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
128
129
130

    void PathPlayer::displayPath_impl(const std::string jointName)
    {
131
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
132
133
134
135
136
      if (!pathIndex()->isEnabled()) {
        main->logError("There is no path. Did you solve a problem ?");
        emit displayPath_status(100);
        return;
      }
137
      CORBA::ULong pid = (CORBA::ULong) pathIndex()->value();
Joseph Mirabel's avatar
Joseph Mirabel committed
138
139
      std::stringstream ss; ss << "curvedpath_" << pid << "_" << jointName;
      std::string pn = ss.str();
140
      gepetto::viewer::WindowsManager::Color_t colorE (1.f, 0.f, 0.f, 1.f);
141
      gepetto::gui::WindowsManagerPtr_t wsm = main->osg();
Joseph Mirabel's avatar
Joseph Mirabel committed
142
143
144
      HppWidgetsPlugin::HppClient* hpp = plugin_->client();
      CORBA::Double length = hpp->problem()->pathLength(pid);
      double dt = lengthBetweenRefresh();
145
      CORBA::ULong nbPos = (CORBA::ULong)(length / dt) + 1;
Joseph Mirabel's avatar
Joseph Mirabel committed
146
      osgVector3 pos;
147
      ::osg::Vec3ArrayRefPtr posSeq = new ::osg::Vec3Array;
Joseph Mirabel's avatar
Joseph Mirabel committed
148
149
150
151
      // Temporary object to avoid dynamic allocation.
      // Arguments are max, length, storage, take ownership.
      char* tmps[1];
      hpp::Names_t names (1, 1, tmps, false);
Joseph Mirabel's avatar
Joseph Mirabel committed
152
153
      float statusStep = 100.f / (float) nbPos;
      emit displayPath_status(0);
154
      for (CORBA::ULong i = 0; i < nbPos; ++i) {
Joseph Mirabel's avatar
Joseph Mirabel committed
155
156
        double t = std::min (i * dt, length);
        hpp::floatSeq_var q = hpp->problem()->configAtParam(pid, t);
Joseph Mirabel's avatar
Joseph Mirabel committed
157
158
159
160
        names[0] = jointName.c_str();
        hpp::TransformSeq_var Ts = hpp->robot()->getJointsPosition(q.in(), names);
        fromHPP(Ts[0], pos);
        posSeq->push_back(pos);
Joseph Mirabel's avatar
Joseph Mirabel committed
161
162
        emit displayPath_status(qFloor ((float)i * statusStep));
      }
163
      if (wsm->nodeExists(pn)) wsm->deleteNode (pn, true);
164
      wsm->addCurve(pn, posSeq, colorE);
Joseph Mirabel's avatar
Joseph Mirabel committed
165
166
167
      wsm->addToGroup(pn.c_str(), "hpp-gui");
      wsm->refresh();
      emit displayPath_status (100);
Joseph Mirabel's avatar
Joseph Mirabel committed
168
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
169
170

    void PathPlayer::update ()
Joseph Mirabel's avatar
Joseph Mirabel committed
171
    {
172
      CORBA::Long nbPath = plugin_->client()->problem ()->numberPaths ();
Joseph Mirabel's avatar
Joseph Mirabel committed
173
174
175
176
177
178
179
      if (nbPath > 0)
      {
        pathIndex()->setEnabled(true);
        pathSlider()->setEnabled(true);
        playPause()->setEnabled(true);
        stop()->setEnabled(true);
        if (pathIndex()->maximum() == 0) {
Joseph Mirabel's avatar
Joseph Mirabel committed
180
181
182
          // If path index value is 0, no signal valueChanged will
          // be emitted. Force a value changed.
          if (pathIndex()->value() == 0)
183
            pathIndexChanged(0);
Joseph Mirabel's avatar
Joseph Mirabel committed
184
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
185
        pathIndex()->setMaximum(nbPath - 1);
186
	pathIndex()->setValue(nbPath - 1);
Joseph Mirabel's avatar
Joseph Mirabel committed
187
188
189
190
191
192
193
194
195
      }
      else {
        pathIndex()->setEnabled(false);
        pathSlider()->setEnabled(false);
        playPause()->setEnabled(false);
        stop()->setEnabled(false);
      }
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
196
197
198
199
200
    void PathPlayer::setRobotVelocity (bool set)
    {
      velocity_ = set;
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
201
202
203
204
205
206
207
208
209
210
211
212
    void PathPlayer::pathIndexChanged(int i)
    {
      assert (i >= 0);
      pathLength_ = plugin_->client()->problem()->pathLength ((short unsigned int)i);
      currentParam_= 0;
    }

    void PathPlayer::pathSliderChanged(int value)
    {
      // The user moved the slider manually.
      currentParam_ = sliderToLength(value);
      updateConfiguration();
Joseph Mirabel's avatar
Joseph Mirabel committed
213
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
214
215
216

    void PathPlayer::playPauseToggled(bool toggled)
    {
217
218
219
220
221
222
      if (toggled) {
        if (timerId_ == 0) timerId_ = startTimer(timeBetweenRefresh());
      } else {
        killTimer(timerId_);
        timerId_ = 0;
      }
223
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240

    void PathPlayer::stopClicked()
    {
      // Pause
      playPause()->setChecked(false);
      // Reset current position.
      currentParam_ = 0;
      pathSlider()->setSliderPosition(0);
      updateConfiguration();
    }

    void PathPlayer::pathPulse()
    {
      if (playPause()->isChecked()) {
        // Stil playing
        currentParam_ += lengthBetweenRefresh();
        if (currentParam_ > pathLength_) {
Joseph Mirabel's avatar
Joseph Mirabel committed
241
          currentParam_ = pathLength_;
Joseph Mirabel's avatar
Joseph Mirabel committed
242
243
          playPause()->setChecked(false);
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
244
245
246
247
248
        pathSlider()->setSliderPosition(lengthToSlider(currentParam_));
        updateConfiguration();
      }
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
249
250
251
252
253
254
255
256
257
258
259
260
    void PathPlayer::setCurrentTime (const double& param)
    {
      if (!pathIndex()->isEnabled()) return;
      if (currentParam_ > pathLength_)
        currentParam_ = pathLength_;
      if (0 < currentParam_)
        currentParam_ = 0;
      currentParam_ = param;
      pathSlider()->setSliderPosition(lengthToSlider(currentParam_));
      updateConfiguration();
    }

Joseph Mirabel's avatar
Joseph Mirabel committed
261
262
263
264
265
266
267
268
269
270
    void PathPlayer::timerEvent(QTimerEvent *event)
    {
      if (timerId_ == event->timerId()) {
        pathPulse();
      }
    }

    void PathPlayer::updateConfiguration ()
    {
      hpp::floatSeq_var config =
271
        plugin_->client()->problem()->configAtParam ((CORBA::ULong)pathIndex()->value(),currentParam_);
272
      plugin_->currentConfig() = config.in();
Joseph Mirabel's avatar
Joseph Mirabel committed
273
274
      if (velocity_) {
        config =
275
          plugin_->client()->problem()->derivativeAtParam ((CORBA::ULong)pathIndex()->value(),1,currentParam_);
276
        plugin_->currentVelocity() = config.in();
Joseph Mirabel's avatar
Joseph Mirabel committed
277
      }
278
      gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
279
      emit appliedConfigAtParam (getCurrentPath(), currentParam_);
Joseph Mirabel's avatar
Joseph Mirabel committed
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
    }

    inline double PathPlayer::sliderToLength(int v) const
    {
      return ((double)v / (double)pathSlider()->maximum()) * pathLength_;
    }

    int PathPlayer::lengthToSlider(double l) const
    {
      return (int) (pathSlider()->maximum() * l / pathLength_);
    }

    inline double PathPlayer::timeToLength(double time) const
    {
      return time * pathLength_ / timeSpinBox()->value();
    }

    inline int PathPlayer::timeBetweenRefresh() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
299
300
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
      return main->settings_->refreshRate;
Joseph Mirabel's avatar
Joseph Mirabel committed
301
302
303
304
    }

    double PathPlayer::lengthBetweenRefresh() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
305
      return pathLength_ * timeBetweenRefresh() / (1000 * timeSpinBox()->value());
Joseph Mirabel's avatar
Joseph Mirabel committed
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
    }

    QDoubleSpinBox *PathPlayer::timeSpinBox() const
    {
      return ui_->timeSpinBox;
    }

    QSpinBox *PathPlayer::pathIndex() const
    {
      return ui_->pathIndexSpin;
    }

    QSlider *PathPlayer::pathSlider() const
    {
      return ui_->pathSlider;
    }

    QPushButton *PathPlayer::playPause() const
    {
      return ui_->playPauseButton;
    }

    QPushButton *PathPlayer::stop() const
    {
      return ui_->stopButton;
    }

333
334
    int	PathPlayer::getCurrentPath() const
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
335
      if (!pathIndex()->isEnabled()) return -1;
336
337
      return pathIndex()->value();
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
338
339
  } // namespace gui
} // namespace hpp