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

6 7
#include "hppmanipulationwidgetsplugin/hppmanipulationwidgetsplugin.hh"

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

10 11 12 13 14 15 16
#include <QAction>
#include <QDockWidget>
#include <QLayout>
#include <QListWidgetItem>
#include <QPushButton>
#include <QVBoxLayout>

17
#include "hppmanipulationwidgetsplugin/roadmap.hh"
18 19
#include "gepetto/gui/mainwindow.hh"
#include "gepetto/gui/windows-manager.hh"
20

21
#include "hppwidgetsplugin/conversions.hh"
hdallard's avatar
hdallard committed
22
#include "hppwidgetsplugin/jointtreewidget.hh"
23 24 25
#include "hppmanipulationwidgetsplugin/linkwidget.hh"
#include "hppmanipulationwidgetsplugin/manipulationconstraintwidget.hh"
#include "hppwidgetsplugin/twojointsconstraint.hh"
26
#include "hppwidgetsplugin/listjointconstraint.hh"
27 28 29

using CORBA::ULong;

30 31
namespace gv = gepetto::viewer;

Joseph Mirabel's avatar
Joseph Mirabel committed
32 33 34 35 36
namespace hpp {
  namespace gui {
    HppManipulationWidgetsPlugin::HppManipulationWidgetsPlugin() :
      HppWidgetsPlugin (),
      hpp_ (NULL),
Joseph Mirabel's avatar
Joseph Mirabel committed
37
      toolBar_ (NULL),
38 39
      tw_ (NULL),
      graphBuilder_ (NULL)
Joseph Mirabel's avatar
Joseph Mirabel committed
40
    {
41
    }
Joseph Mirabel's avatar
Joseph Mirabel committed
42 43 44

    HppManipulationWidgetsPlugin::~HppManipulationWidgetsPlugin()
    {
45 46 47 48
      if (graphBuilder_) {
        delete graphBuilder_;
        graphBuilder_ = NULL;
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
49 50 51 52 53
    }

    void HppManipulationWidgetsPlugin::init()
    {
      HppWidgetsPlugin::init ();
54
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
Joseph Mirabel's avatar
Joseph Mirabel committed
55

56
      toolBar_ = gepetto::gui::MainWindow::instance()->addToolBar("Manipulation tools");
57
      toolBar_->setObjectName ("hppmanipulationwidgetsplugin.manipulationtools");
58 59 60 61 62 63
      QAction* drawRContact = new QAction ("Draw robot contacts",toolBar_);
      QAction* drawEContact = new QAction ("Draw environment contacts",toolBar_);
      toolBar_->addAction (drawRContact);
      toolBar_->addAction (drawEContact);
      connect (drawRContact, SIGNAL(triggered()), SLOT (drawRobotContacts()));
      connect (drawEContact, SIGNAL(triggered()), SLOT (drawEnvironmentContacts()));
64 65 66 67 68 69
      QAction* drawHFrame = new QAction ("Draw handles frame",toolBar_);
      QAction* drawGFrame = new QAction ("Draw grippers frame",toolBar_);
      toolBar_->addAction (drawHFrame);
      toolBar_->addAction (drawGFrame);
      connect (drawHFrame, SIGNAL(triggered()), SLOT (drawHandlesFrame()));
      connect (drawGFrame, SIGNAL(triggered()), SLOT (drawGrippersFrame()));
hdallard's avatar
hdallard committed
70 71 72
      QAction* autoBuildGraph = new QAction ("Autobuild constraint graph",toolBar_);
      toolBar_->addAction (autoBuildGraph);
      connect(autoBuildGraph, SIGNAL(triggered()), SLOT(autoBuildGraph()));
73

74
      main->registerSlot("autoBuildGraph", this);
75 76
      main->registerSlot("drawRobotContacts", this);
      main->registerSlot("drawEnvironmentsContacts", this);
Joseph Mirabel's avatar
Joseph Mirabel committed
77 78 79 80 81 82 83
    }

    QString HppManipulationWidgetsPlugin::name() const
    {
      return QString ("Widgets for hpp-manipulation-corba");
    }

84
    void HppManipulationWidgetsPlugin::loadRobotModel(gepetto::gui::DialogLoadRobot::RobotDefinition rd)
Joseph Mirabel's avatar
Joseph Mirabel committed
85
    {
86
      try {
Joseph Mirabel's avatar
Joseph Mirabel committed
87 88
        hpp::floatSeq_var q = client ()->robot ()->getCurrentConfig();
        (void)q;
89
      } catch (hpp::Error const& e) {
Joseph Mirabel's avatar
Joseph Mirabel committed
90
	client ()->robot ()->createRobot (to_corba("composite").in());
hdallard's avatar
hdallard committed
91
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
92 93 94 95 96 97
      hpp_->robot ()->insertRobotModel (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());
98 99
      // This is already done in requestRefresh
      // jointTreeWidget_->reload();
100
      gepetto::gui::MainWindow::instance()->requestRefresh();
Joseph Mirabel's avatar
Joseph Mirabel committed
101
      gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
hdallard's avatar
hdallard committed
102
      emit logSuccess ("Robot " + rd.name_ + " loaded");
Joseph Mirabel's avatar
Joseph Mirabel committed
103 104
    }

105
    void HppManipulationWidgetsPlugin::loadEnvironmentModel(gepetto::gui::DialogLoadEnvironment::EnvironmentDefinition ed)
Joseph Mirabel's avatar
Joseph Mirabel committed
106
    {
107
      try {
Joseph Mirabel's avatar
Joseph Mirabel committed
108 109
        hpp::floatSeq_var q = client ()->robot ()->getCurrentConfig();
        (void)q;
110
      } catch (hpp::Error const& e) {
Joseph Mirabel's avatar
Joseph Mirabel committed
111
	client ()->robot ()->createRobot (to_corba("composite").in());
hdallard's avatar
hdallard committed
112
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
113 114 115 116 117
      hpp_->robot ()-> loadEnvironmentModel(to_corba(ed.package_).in(),
					    to_corba(ed.urdfFilename_).in(),
					    to_corba(ed.urdfSuf_).in(),
					    to_corba(ed.srdfSuf_).in(),
					    to_corba(ed.name_ + "/").in());
hdallard's avatar
hdallard committed
118
      HppWidgetsPlugin::computeObjectPosition();
119
      gepetto::gui::MainWindow::instance()->requestRefresh();
hdallard's avatar
hdallard committed
120 121
      emit logSuccess ("Environment " + ed.name_ + " loaded");
   }
Joseph Mirabel's avatar
Joseph Mirabel committed
122 123 124 125 126 127 128 129 130 131 132

    std::string HppManipulationWidgetsPlugin::getBodyFromJoint(const std::string &jointName) const
    {
      /// TODO: fix this
      return HppWidgetsPlugin::getBodyFromJoint (jointName);
    }

    void HppManipulationWidgetsPlugin::openConnection()
    {
      HppWidgetsPlugin::openConnection();
      hpp_ = new HppManipClient (0,0);
Joseph Mirabel's avatar
Joseph Mirabel committed
133 134
      QByteArray iiop    = getHppIIOPurl ().toLatin1();
      QByteArray context = getHppContext ().toLatin1();
135 136 137 138 139 140 141 142 143
      try {
        hpp_->connect (iiop.constData (), context.constData ());
      } catch (const CosNaming::NamingContext::NotFound&) {
        const char* msg = "Could not find the manipulation server. Is it running ?";
        qDebug () << msg;
        gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
        if (main != NULL)
          main->logError(msg);
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160
    }

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

    HppManipulationWidgetsPlugin::HppManipClient *HppManipulationWidgetsPlugin::manipClient() const
    {
      return hpp_;
    }

    Roadmap *HppManipulationWidgetsPlugin::createRoadmap(const std::string &jointName)
    {
      ManipulationRoadmap* r = new ManipulationRoadmap(this);
161
      r->initRoadmapFromJoint(jointName);
Joseph Mirabel's avatar
Joseph Mirabel committed
162 163 164
      return r;
    }

165 166 167 168
    void HppManipulationWidgetsPlugin::drawContactSurface(const std::string& name,
							  hpp::intSeq_var& indexes,
							  hpp::floatSeqSeq_var& points,
							  CORBA::ULong j,
169
							  float epsilon)
Joseph Mirabel's avatar
Joseph Mirabel committed
170
    {
171
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
172 173
      osgVector4 color (0, 1, 0, 1);
      osgVector3 norm(0, 0, 0);
174
      CORBA::Long iPts = (j == 0) ? 0 : indexes[j - 1];
175
      gv::WindowsManager::Vec3ArrayPtr_t ps(new osg::Vec3Array);
176 177 178 179 180 181 182 183 184 185
      ps->resize (indexes[j] - iPts);
      if (ps->size() > 3) {
	osgVector3 a((float)(points[iPts][0] - points[iPts + 1][0]),
		     (float)(points[iPts][1] - points[iPts + 1][1]),
		     (float)(points[iPts][2] - points[iPts + 1][2]));
	osgVector3 b((float)(points[iPts + 1][0] - points[iPts + 2][0]),
		     (float)(points[iPts + 1][1] - points[iPts + 2][1]),
		     (float)(points[iPts + 1][2] - points[iPts + 2][2]));
	osgVector3 c = a ^ b;
	if (c.length() > 0.00001)
186 187 188
	  norm = c / c.length();
      }
      for (CORBA::Long k = iPts; k < indexes[j]; ++k) {
189
        (*ps)[k - iPts] = osgVector3((float)points[k][0],(float)points[k][1],(float)points[k][2]) + norm * epsilon;
190
      }
191 192
      main->osg()->addCurve (name, ps, color);
      main->osg()->setCurveMode (name, GL_POLYGON);
193 194 195 196
    }

    void HppManipulationWidgetsPlugin::drawRobotContacts()
    {
Joseph Mirabel's avatar
Joseph Mirabel committed
197 198 199 200 201
      hpp::Names_t_var rcs = hpp_->problem()->getRobotContactNames();
      hpp::floatSeqSeq_var points;
      hpp::intSeq_var indexes;
      for (CORBA::ULong i = 0; i < rcs->length(); ++i) {
        hpp::Names_t_var cjs = hpp_->problem()->getRobotContact (
202
            rcs[i], indexes.out(), points.out());
Joseph Mirabel's avatar
Joseph Mirabel committed
203
        for (CORBA::ULong j = 0; j < cjs->length(); ++j) {
204
          /// Create group
Joseph Mirabel's avatar
Joseph Mirabel committed
205
          std::string target = createJointGroup(std::string(cjs[j]));
206 207 208 209

          /// Add the contacts
          std::stringstream ssname;
          ssname <<  target << "/contact_"
210
		 << escapeJointName(std::string (rcs[i])) << "_" << j << "_";
211
          std::string name = ssname.str ();
212
	  drawContactSurface(name, indexes, points, j);
213
        }
Joseph Mirabel's avatar
Joseph Mirabel committed
214
      }
215
      gepetto::gui::MainWindow::instance()->requestRefresh();
216 217
    }

218 219 220 221 222 223 224 225 226 227 228 229
    void HppManipulationWidgetsPlugin::drawEnvironmentContacts()
    {
      hpp::Names_t_var rcs = hpp_->problem()->getEnvironmentContactNames();
      hpp::floatSeqSeq_var points;
      hpp::intSeq_var indexes;
      for (CORBA::ULong i = 0; i < rcs->length(); ++i) {
        hpp::Names_t_var cjs = hpp_->problem()->getEnvironmentContact (
            rcs[i], indexes.out(), points.out());
        for (CORBA::ULong j = 0; j < cjs->length(); ++j) {
          /// Add the contacts
          std::stringstream ssname;
          ssname << "hpp-gui/contact_"
230
            << escapeJointName(std::string (rcs[i])) << "_" << j << "_";
231
          std::string name = ssname.str ();
232
	  drawContactSurface(name, indexes, points, j);
233 234
        }
      }
235
      gepetto::gui::MainWindow::instance()->requestRefresh();
236 237
    }

238 239
    void HppManipulationWidgetsPlugin::drawHandlesFrame()
    {
240
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
241 242
      hpp::Names_t_var rcs = hpp_->problem()->getAvailable("handle");
      hpp::Transform__var t (new Transform_);
243 244
      gv::Configuration config;
      const gv::WindowsManager::Color_t color (0, 1, 0, 1);
245 246 247
      for (CORBA::ULong i = 0; i < rcs->length(); ++i) {
        const std::string jn =
          hpp_->robot()->getHandlePositionInJoint (rcs[i],t.out());
248
        std::string groupName = createJointGroup (jn);
Joseph Mirabel's avatar
Joseph Mirabel committed
249
        std::string hn = "handle_" + escapeJointName (std::string(rcs[i]));
250
        fromHPP(t, config);
Joseph Mirabel's avatar
Joseph Mirabel committed
251
        main->osg()->addXYZaxis (hn, color, 0.005f, 0.015f);
252 253
        main->osg()->applyConfiguration (hn, config);
        main->osg()->addToGroup (hn, groupName);
254
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
255
      main->osg()->refresh();
256
      gepetto::gui::MainWindow::instance()->requestRefresh();
257 258 259 260
    }

    void HppManipulationWidgetsPlugin::drawGrippersFrame()
    {
261
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
262 263
      hpp::Names_t_var rcs = hpp_->problem()->getAvailable("gripper");
      hpp::Transform__var t (new Transform_);
264 265
      gv::Configuration config;
      const gv::WindowsManager::Color_t color (0, 1, 0, 1);
266 267 268
      for (CORBA::ULong i = 0; i < rcs->length(); ++i) {
        const std::string jn =
          hpp_->robot()->getGripperPositionInJoint (rcs[i],t.out());
269
        std::string groupName = createJointGroup (jn);
Joseph Mirabel's avatar
Joseph Mirabel committed
270
        std::string hn = "gripper_" + escapeJointName (std::string(rcs[i]));
271
        fromHPP(t, config);
Joseph Mirabel's avatar
Joseph Mirabel committed
272
        main->osg()->addXYZaxis (hn, color, 0.005f, 0.015f);
273 274
        main->osg()->applyConfiguration (hn, config);
        main->osg()->addToGroup (hn, groupName);
275
      }
Joseph Mirabel's avatar
Joseph Mirabel committed
276
      main->osg()->refresh();
277
      gepetto::gui::MainWindow::instance()->requestRefresh();
278 279
    }

hdallard's avatar
hdallard committed
280 281 282 283 284 285 286 287
    HppManipulationWidgetsPlugin::NamesPair
    HppManipulationWidgetsPlugin::convertMap(std::map<std::string,
					     std::list<std::string> >& mapNames)
    {
      HppManipulationWidgetsPlugin::NamesPair names;
      int i = 0;
      int j;

288 289
      names.first.length ((CORBA::ULong) mapNames.size ());
      names.second.length ((CORBA::ULong) mapNames.size ());
hdallard's avatar
hdallard committed
290 291 292
      for (HppManipulationWidgetsPlugin::MapNames::iterator itMap = mapNames.begin();
	   itMap != mapNames.end(); itMap++, i++){
	names.first[i] = (*itMap).first.c_str();
293
	names.second[i].length ((CORBA::ULong) (*itMap).second.size());
hdallard's avatar
hdallard committed
294 295 296 297 298 299 300 301 302
        j = 0;
	for (std::list<std::string>::iterator itList = (*itMap).second.begin();
	     itList != (*itMap).second.end(); itList++, j++) {
	  names.second[i][j] = (*itList).c_str();
	}
      }
      return names;
    }

303 304 305 306 307 308 309 310 311 312
    hpp::Names_t_var HppManipulationWidgetsPlugin::convertToNames(const QList<QListWidgetItem *>& l)
    {
      hpp::Names_t_var cl = new hpp::Names_t;
      cl->length(l.count());
      for (int i = 0; i < l.count(); i++) {
        cl[i]= l[i]->text().toStdString().c_str();
      }
      return cl;
    }

313 314 315 316 317 318 319 320 321 322 323 324 325 326 327 328 329 330 331 332
    void HppManipulationWidgetsPlugin::mergeShapes(MapNames &handles, MapNames &shapes)
    {
      MapNames::iterator itH = handles.begin();
      MapNames::iterator itS = shapes.begin();
      while (itH != handles.end())
      {
        if (itS == shapes.end() || (*itH).first != (*itS).first) {
          itS = shapes.insert(itS, std::make_pair((*itH).first, std::list<std::string>()));
        }
        else
          ++itS;
        itH++;
      }
      while (itS != shapes.end())
      {
        handles.insert(handles.end(), std::make_pair((*itS).first, std::list<std::string>()));
        itS++;
      }
    }

333 334 335 336 337 338 339 340 341 342 343 344 345 346 347 348 349 350 351 352 353 354 355 356 357 358 359 360 361 362 363 364 365 366 367 368 369 370 371 372 373
    HppManipulationWidgetsPlugin::MapNames HppManipulationWidgetsPlugin::getObjects()
    {
      HppManipulationWidgetsPlugin::MapNames map;
      hpp::Names_t_var handles = manipClient()->problem()->getAvailable("handle");
      hpp::Names_t_var surfaces = manipClient()->problem()->getAvailable("robotcontact");

      for (unsigned i = 0; i < handles->length(); ++i) {
        std::string name(handles[i].in());
	size_t pos = name.find_first_of("/");
	std::string object = name.substr(0, pos);

	if (map.find(object) == map.end()) {
	  map[object] = std::list<std::string>();
	}
      }
      for (unsigned i = 0; i < surfaces->length(); ++i) {
        std::string name(surfaces[i].in());
	size_t pos = name.find_first_of("/");
	std::string object = name.substr(0, pos);

	if (map.find(object) == map.end()) {
	  map[object] = std::list<std::string>();
	}
      }
      return map;
    }

    void HppManipulationWidgetsPlugin::fillMap(HppManipulationWidgetsPlugin::MapNames& map,
					       const QList<QListWidgetItem*>& list)
    {
      for (int i = 0; i < list.count(); i++) {
        std::string name(list[i]->text().toStdString());
	size_t pos = name.find_first_of("/");
	std::string object = name.substr(0, pos);

	if (map.find(object) != map.end()) {
	  map[object].push_back(name);
	}
      }
    }

374 375
    void print(const hpp::Names_t& v) {
      std::cout << "[ ";
376 377
      for (std::size_t i = 0; i < v.length(); i++)
        std::cout << '"' << v[(CORBA::ULong) i] << "\", ";
378 379 380 381
      std::cout << "]";
    }
    void print(const hpp::corbaserver::manipulation::Namess_t& v) {
      std::cout << "[ ";
382 383
      for (std::size_t i = 0; (std::size_t) i < v.length(); i++) {
        print(v[(CORBA::ULong) i]);
384 385 386 387 388 389 390 391 392 393 394 395 396
        std::cout << ", ";
      }
      std::cout << "]";
    }
    void print(const hpp::corbaserver::manipulation::Rule& v) {
      std::cout << "Rule(";
      print(v.grippers);
      std::cout << ", ";
      print(v.handles);
      std::cout << ", " << (v.link ? "True" : "False" ) << ')';
    }
    void print(const hpp::corbaserver::manipulation::Rules& v) {
      std::cout << "[ ";
Joseph Mirabel's avatar
Joseph Mirabel committed
397
      for (std::size_t i = 0; i < v.length(); i++) {
398
        print(v[(CORBA::ULong) i]);
399 400 401 402 403
        std::cout << ", ";
      }
      std::cout << "]";
    }

404
    void HppManipulationWidgetsPlugin::buildGraph()
hdallard's avatar
hdallard committed
405
    {
406
      QListWidget* l = dynamic_cast<QListWidget*>(tw_->widget(0));
407 408
      HppManipulationWidgetsPlugin::MapNames handlesMap = getObjects();
      HppManipulationWidgetsPlugin::MapNames shapesMap = getObjects();
409
      hpp::Names_t_var grippers = convertToNames(l->selectedItems());
410 411
      l = dynamic_cast<QListWidget*>(tw_->widget(1)->layout()->itemAt(1)->widget());
      fillMap(handlesMap, l->selectedItems());
412
      l = dynamic_cast<QListWidget*>(tw_->widget(2));
413
      fillMap(shapesMap, l->selectedItems());
414 415
      l = dynamic_cast<QListWidget*>(tw_->widget(3));
      hpp::Names_t_var envNames = convertToNames(l->selectedItems());
416 417
      hpp::corbaserver::manipulation::Rules_var rules = dynamic_cast<LinkWidget *>(tw_->widget(4))->getRules();

418 419
      HppManipulationWidgetsPlugin::NamesPair handles = convertMap(handlesMap);
      HppManipulationWidgetsPlugin::NamesPair shapes = convertMap(shapesMap);
420 421 422 423
      try {
        hpp_->graph ()->deleteGraph("constraints");
      } catch (const hpp::Error&) {
      }
424
      // TODO the return value is never deleted.
425 426 427 428 429 430 431
      std::cout << "graph.buildGenericGraph(robot, 'graph', ";
      print(grippers.in());       std::cout << ", ";
      print(handles.first);       std::cout << ", ";
      print(handles.second);      std::cout << ", ";
      print(shapes.second);       std::cout << ", ";
      print(envNames.in());       std::cout << ", ";
      print(rules.in());          std::cout << ")" << std::endl;
432
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
433 434 435 436 437 438
      hpp_->graph()->autoBuild("constraints", grippers.in(),
          handles.first, handles.second,
          shapes.second,
          envNames.in(), rules.in());
      hpp_->graph()->initialize();
      main->log("Built and initialized graph");
439
      graphBuilder_->close();
440 441 442 443
    }

    void HppManipulationWidgetsPlugin::autoBuildGraph()
    {
444 445 446 447 448 449 450 451 452 453 454 455 456 457 458 459 460 461 462 463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497 498 499 500 501
      if (graphBuilder_ == NULL) {
        graphBuilder_ = new QDialog(NULL, Qt::Dialog);
        tw_ = new QTabWidget(graphBuilder_);
        graphBuilder_->setLayout(new QVBoxLayout(graphBuilder_));
        graphBuilder_->layout()->addWidget(tw_);

        QListWidget* lw;
        QListWidget* grippers;
        QListWidget* handles;
        QPushButton* button = new QPushButton("Confirm", tw_);

        connect(button, SIGNAL(clicked()), SLOT(buildGraph()));
        hpp::Names_t_var n;
        n = hpp_->problem()->getAvailable("Gripper");
        QStringList l;
        for (unsigned i = 0; i < n->length(); i++) {
          l << QString(n[i].in());
        }
        lw = new QListWidget(tw_);
        lw->addItems(l);
        lw->setSelectionMode(QAbstractItemView::ExtendedSelection);
        tw_->addTab(lw, "Grippers");
        grippers = lw;

        n = hpp_->problem()->getAvailable("Handle");
        l.clear();
        for (unsigned i = 0; i < n->length(); i++) {
          l << QString(n[i].in());
        }
        QWidget* widget = new QWidget(tw_);
        QVBoxLayout* box = new QVBoxLayout(widget);
        box->addWidget(new QLabel("The objects not selected will be locked in their current position."));
        lw = new QListWidget(tw_);
        box->addWidget(lw);
        lw->addItems(l);
        lw->setSelectionMode(QAbstractItemView::ExtendedSelection);
        tw_->addTab(widget, "Handles");
        handles = lw;

        n = hpp_->problem()->getRobotContactNames();
        l.clear();
        for (unsigned i = 0; i < n->length(); i++) {
          l << QString(n[i].in());
        }
        lw = new QListWidget(tw_);
        lw->addItems(l);
        lw->setSelectionMode(QAbstractItemView::ExtendedSelection);
        tw_->addTab(lw, "Robot Contacts");

        n = hpp_->problem()->getEnvironmentContactNames();
        l.clear();
        for (unsigned i = 0; i < n->length(); i++) {
          l << QString(n[i].in());
        }
        lw = new QListWidget(tw_);
        lw->addItems(l);
        lw->setSelectionMode(QAbstractItemView::ExtendedSelection);
        tw_->addTab(lw, "Environments Contacts");
502

503 504
        LinkWidget* lWidget = new LinkWidget(grippers, handles, tw_);
        tw_->addTab(lWidget, "Rules");
505

506 507 508
        tw_->setCornerWidget(button, Qt::BottomRightCorner);
      }
      graphBuilder_->show();
hdallard's avatar
hdallard committed
509 510
    }

511 512 513 514
    void HppManipulationWidgetsPlugin::loadConstraintWidget()
    {
      gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
      QDockWidget* dock = new QDockWidget ("&Constraint creator", main);
515
      dock->setObjectName ("hppmanipulationwidgetsplugin.constraintcreator");
516 517 518 519 520 521 522 523
      constraintWidget_ = new ManipulationConstraintWidget (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));
524
      constraintWidget_->addConstraint(new LockedJointConstraint(this));
525 526
    }

527
#if (QT_VERSION < QT_VERSION_CHECK(5,0,0))
Joseph Mirabel's avatar
Joseph Mirabel committed
528
    Q_EXPORT_PLUGIN2 (hppmanipulationwidgetsplugin, HppManipulationWidgetsPlugin)
529
#endif
Joseph Mirabel's avatar
Joseph Mirabel committed
530 531
  } // namespace gui
} // namespace hpp