Commits (20)
......@@ -23,18 +23,18 @@ SET(CXX_DISABLE_WERROR TRUE)
INCLUDE(cmake/base.cmake)
INCLUDE(cmake/boost.cmake)
INCLUDE(cmake/python.cmake)
INCLUDE(cmake/hpp.cmake)
INCLUDE(cmake//python.cmake)
# Tells pkg-config to read qtversion and cmake_plugin from pkg config file.
LIST(APPEND PKG_CONFIG_ADDITIONAL_VARIABLES qtversion cmake_plugin)
SET(PROJECT_NAME "hpp-gui")
SET(PROJECT_URL "https://github.com/humanoid-path-planner/hpp-gui")
SET(PROJECT_DESCRIPTION "Graphical interface for HPP ")
SETUP_HPP_PROJECT()
FINDPYTHON()
ADD_REQUIRED_DEPENDENCY("gepetto-viewer-corba >= 1.3")
# Get desired Qt version
......@@ -97,4 +97,14 @@ ELSE ()
DESTINATION ${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui)
ENDIF ()
# If basic.conf not yet installed, install it
IF (EXISTS "${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui/basic.conf")
MESSAGE (STATUS
"File ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui/basic.conf detected.")
ELSE ()
MESSAGE (STATUS "File ${CMAKE_INSTALL_PREFIX}/${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui/basic.conf does not exist. Will install it.")
INSTALL (FILES etc/gepetto-gui/basic.conf
DESTINATION ${CMAKE_INSTALL_SYSCONFDIR}/gepetto-gui)
ENDIF ()
SETUP_HPP_PROJECT_FINALIZE()
Subproject commit ecaf20c28c8ce33de312fbd812f88037cf995b73
Subproject commit c81a37191d764522899b5dcb4468485a826141c2
[plugins]
# Plugin for hpp-manipulation-server
hppmanipulationwidgetsplugin.so=false
hppmonitoringplugin.so=false
# Plugin for hppcorbaserver
hppwidgetsplugin.so=true
......@@ -72,7 +72,7 @@ namespace hpp {
QString name = QInputDialog::getText(NULL, "Node name", "Node name", QLineEdit::Normal, "bvhmodel");
int splitMethod = QInputDialog::getInt(NULL, "Split method type",
"Split method type", 0, 0, 3, 1);
addBV (filename, name, splitMethod);
addBV (name, filename, splitMethod);
}
#if (QT_VERSION < QT_VERSION_CHECK(5,0,0))
......
......@@ -31,8 +31,6 @@ GEPETTO_GUI_PLUGIN(hppmanipulationwidgetsplugin
HEADERS
hppmanipulationwidgetsplugin.hh
linkwidget.hh
manipulationconstraintwidget.hh
manipulationncpicker.hh
HEADERS_NO_MOC
roadmap.hh
......@@ -44,8 +42,6 @@ GEPETTO_GUI_PLUGIN(hppmanipulationwidgetsplugin
hppmanipulationwidgetsplugin.cc
roadmap.cc
linkwidget.cc
manipulationconstraintwidget.cc
manipulationncpicker.cc
LINK_DEPENDENCIES
hppwidgetsplugin
......
......@@ -21,9 +21,6 @@
#include "hppwidgetsplugin/conversions.hh"
#include "hppwidgetsplugin/jointtreewidget.hh"
#include "hppmanipulationwidgetsplugin/linkwidget.hh"
#include "hppmanipulationwidgetsplugin/manipulationconstraintwidget.hh"
#include "hppwidgetsplugin/twojointsconstraint.hh"
#include "hppwidgetsplugin/listjointconstraint.hh"
using CORBA::ULong;
......@@ -83,6 +80,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::loadRobotModel(gepetto::gui::DialogLoadRobot::RobotDefinition rd)
{
if (!hpp_) return;
try {
hpp::floatSeq_var q = client ()->robot ()->getCurrentConfig();
(void)q;
......@@ -104,6 +102,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::loadEnvironmentModel(gepetto::gui::DialogLoadEnvironment::EnvironmentDefinition ed)
{
if (!hpp_) return;
try {
hpp::floatSeq_var q = client ()->robot ()->getCurrentConfig();
(void)q;
......@@ -132,7 +131,24 @@ namespace hpp {
hpp_ = new HppManipClient (0,0);
QByteArray iiop = getHppIIOPurl ().toLatin1();
QByteArray context = getHppContext ().toLatin1();
hpp_->connect (iiop.constData (), context.constData ());
try {
hpp_->connect (iiop.constData (), context.constData ());
hpp::Names_t_var for_memory_deletion = hpp_->problem()->getAvailable("type");
} catch (const CORBA::Exception& e) {
QString error ("Could not find the manipulation server. Is it running ?");
error += "\n";
error += e._name();
error += " : ";
error += e._rep_id();
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
if (main != NULL)
main->logError(error);
else
qDebug () << error;
if (hpp_) delete hpp_;
hpp_ = NULL;
}
}
void HppManipulationWidgetsPlugin::closeConnection()
......@@ -147,17 +163,6 @@ namespace hpp {
return hpp_;
}
void HppManipulationWidgetsPlugin::updateRobotJoints(const QString robotName)
{
Q_UNUSED(robotName)
hpp::Names_t_var joints = client()->robot()->getAllJointNames ();
for (size_t i = 0; i < joints->length (); ++i) {
const char* jname = joints[(ULong) i];
hpp::Names_t_var lnames = client()->robot()->getLinkNames (jname);
jointMap_[jname] = JointElement(jname, "", lnames, 0, true);
}
}
Roadmap *HppManipulationWidgetsPlugin::createRoadmap(const std::string &jointName)
{
ManipulationRoadmap* r = new ManipulationRoadmap(this);
......@@ -197,6 +202,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::drawRobotContacts()
{
if (!hpp_) return;
hpp::Names_t_var rcs = hpp_->problem()->getRobotContactNames();
hpp::floatSeqSeq_var points;
hpp::intSeq_var indexes;
......@@ -220,6 +226,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::drawEnvironmentContacts()
{
if (!hpp_) return;
hpp::Names_t_var rcs = hpp_->problem()->getEnvironmentContactNames();
hpp::floatSeqSeq_var points;
hpp::intSeq_var indexes;
......@@ -240,6 +247,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::drawHandlesFrame()
{
if (!hpp_) return;
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
hpp::Names_t_var rcs = hpp_->problem()->getAvailable("handle");
hpp::Transform__var t (new Transform_);
......@@ -261,6 +269,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::drawGrippersFrame()
{
if (!hpp_) return;
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
hpp::Names_t_var rcs = hpp_->problem()->getAvailable("gripper");
hpp::Transform__var t (new Transform_);
......@@ -335,6 +344,7 @@ namespace hpp {
HppManipulationWidgetsPlugin::MapNames HppManipulationWidgetsPlugin::getObjects()
{
assert (hpp_ != NULL);
HppManipulationWidgetsPlugin::MapNames map;
hpp::Names_t_var handles = manipClient()->problem()->getAvailable("handle");
hpp::Names_t_var surfaces = manipClient()->problem()->getAvailable("robotcontact");
......@@ -406,6 +416,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::buildGraph()
{
if (!hpp_) return;
QListWidget* l = dynamic_cast<QListWidget*>(tw_->widget(0));
HppManipulationWidgetsPlugin::MapNames handlesMap = getObjects();
HppManipulationWidgetsPlugin::MapNames shapesMap = getObjects();
......@@ -444,6 +455,7 @@ namespace hpp {
void HppManipulationWidgetsPlugin::autoBuildGraph()
{
if (!hpp_) return;
if (graphBuilder_ == NULL) {
graphBuilder_ = new QDialog(NULL, Qt::Dialog);
tw_ = new QTabWidget(graphBuilder_);
......@@ -511,22 +523,6 @@ namespace hpp {
graphBuilder_->show();
}
void HppManipulationWidgetsPlugin::loadConstraintWidget()
{
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
QDockWidget* dock = new QDockWidget ("&Constraint creator", main);
dock->setObjectName ("hppmanipulationwidgetsplugin.constraintcreator");
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));
constraintWidget_->addConstraint(new LockedJointConstraint(this));
}
#if (QT_VERSION < QT_VERSION_CHECK(5,0,0))
Q_EXPORT_PLUGIN2 (hppmanipulationwidgetsplugin, HppManipulationWidgetsPlugin)
#endif
......
......@@ -72,10 +72,6 @@ namespace hpp {
/// Get the instance of corba manipulation client.
HppManipClient* manipClient () const;
/// Get the list of joints from corbaserver and update internal joint map.
/// \param robotName name of the robot (unused)
void updateRobotJoints (const QString robotName);
/// Create the roadmap of a given joint.
/// \param jointName name of the joint
virtual Roadmap* createRoadmap (const std::string& jointName);
......@@ -100,9 +96,6 @@ namespace hpp {
/// Construct all the corba vars and create the graph.
void buildGraph();
protected:
virtual void loadConstraintWidget();
private:
// Type used to make one function to build datas needed for autoBuild
typedef std::pair<hpp::Names_t, hpp::corbaserver::manipulation::Namess_t> NamesPair;
......
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel and Heidy Dallard
//
#include "hppmanipulationwidgetsplugin/hppmanipulationwidgetsplugin.hh"
#include "hppwidgetsplugin/ui_constraintwidget.h"
#include "hppmanipulationwidgetsplugin/manipulationncpicker.hh"
#include "manipulationconstraintwidget.hh"
namespace hpp {
namespace gui {
ManipulationConstraintWidget::ManipulationConstraintWidget(HppWidgetsPlugin* plugin, QWidget* parent)
: ConstraintWidget(plugin, parent)
{
}
ManipulationConstraintWidget::~ManipulationConstraintWidget()
{
}
void ManipulationConstraintWidget::applyConstraints()
{
}
void ManipulationConstraintWidget::refresh()
{
HppManipulationWidgetsPlugin* plugin = dynamic_cast<HppManipulationWidgetsPlugin*>(plugin_);
ConstraintWidget::refresh();
hpp::Names_t_var names = plugin->client()->problem()->getAvailable("lockedjoint");
for (unsigned i = 0; i < names->length(); i++) {
ui->nameList->addItem(names[i].in());
}
}
void ManipulationConstraintWidget::reset()
{
HppManipulationWidgetsPlugin* plugin = dynamic_cast<HppManipulationWidgetsPlugin*>(plugin_);
hpp::GraphComp_var graphComp;
hpp::GraphElements_var graphElems;
plugin->manipClient()->graph()->getGraph(graphComp.out(), graphElems.out());
for (unsigned i = 0; i < graphElems->edges.length(); ++i) {
plugin->manipClient()->graph()->resetConstraints(graphElems->edges[i].id);
}
for (unsigned i = 0; i < graphElems->nodes.length(); ++i) {
plugin->manipClient()->graph()->resetConstraints(graphElems->nodes[i].id);
}
}
void ManipulationConstraintWidget::confirmNumerical()
{
HppManipulationWidgetsPlugin* plugin = dynamic_cast<HppManipulationWidgetsPlugin*>(plugin_);
ManipulationNCPicker* ncp = new ManipulationNCPicker(plugin);
ncp->show();
}
}
}
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel and Heidy Dallard
//
#ifndef HPP_GUI_MANIPULATIONCONSTRAINTWIDGET_HH
#define HPP_GUI_MANIPULATIONCONSTRAINTWIDGET_HH
#include "hppwidgetsplugin/constraintwidget.hh"
#include "hppwidgetsplugin/hppwidgetsplugin.hh"
namespace hpp {
namespace gui {
class ManipulationConstraintWidget : public ConstraintWidget
{
Q_OBJECT
public:
ManipulationConstraintWidget(HppWidgetsPlugin* plugin, QWidget* parent = 0);
virtual ~ManipulationConstraintWidget();
public slots:
virtual void refresh();
private slots:
virtual void applyConstraints();
virtual void reset();
virtual void confirmNumerical();
protected:
};
}
}
#endif // HPP_GUI_MANIPULATIONCONSTRAINTWIDGET_HH
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel and Heidy Dallard
//
#include <hppmanipulationwidgetsplugin/hppmanipulationwidgetsplugin.hh>
#include "hppwidgetsplugin/ui_numericalconstraintpicker.h"
#include "manipulationncpicker.hh"
namespace hpp {
namespace gui {
ManipulationNCPicker::ManipulationNCPicker(HppManipulationWidgetsPlugin* plugin,
QWidget* parent)
: NumericalConstraintPicker(plugin, parent)
{
listComp_ = new QListWidget(this);
hpp::GraphComp_var graphComp;
hpp::GraphElements_var graphElems;
plugin->manipClient()->graph()->getGraph(graphComp.out(), graphElems.out());
components_[graphComp->name.in()] = graphComp;
listComp_->addItem(graphComp->name.in());
for (unsigned i = 0; i < graphElems->nodes.length(); ++i) {
if (graphElems->nodes[i].id > graphComp->id) {
components_[graphElems->nodes[i].name.in()] = graphElems->nodes[i];
listComp_->addItem(graphElems->nodes[i].name.in());
}
}
for (unsigned i = 0; i < graphElems->edges.length(); ++i) {
if (graphElems->edges[i].id > graphComp->id) {
components_[graphElems->edges[i].name.in()] = graphElems->edges[i];
listComp_->addItem(graphElems->edges[i].name.in());
}
}
listComp_->setSelectionMode(QAbstractItemView::ExtendedSelection);
QBoxLayout* l = dynamic_cast<QBoxLayout *>(layout());
l->insertWidget(2, new QLabel("Select on which graph components you want to apply constraints.\n"
"The constraints will be automatically applied to all the components under the one you choose."));
l->insertWidget(3, listComp_);
}
ManipulationNCPicker::~ManipulationNCPicker()
{
}
void ManipulationNCPicker::onConfirmClicked()
{
QList<QListWidgetItem*> lj = ui->lockedJointList->selectedItems();
QList<QListWidgetItem*> nc = ui->numericalList->selectedItems();
hpp::Names_t_var constraints = new hpp::Names_t;
hpp::Names_t_var locked = new hpp::Names_t;
hpp::Names_t_var dofs = new hpp::Names_t;
QList<QListWidgetItem*> selectedComp = listComp_->selectedItems();
HppManipulationWidgetsPlugin* plugin = dynamic_cast<HppManipulationWidgetsPlugin*>(plugin_);
constraints->length(nc.count());
locked->length(lj.count());
int i = 0;
foreach (QListWidgetItem* item, lj) {
locked[i] = item->text().toStdString().c_str();
++i;
}
i = 0;
foreach (QListWidgetItem* item, nc) {
constraints[i] = item->text().toStdString().c_str();
++i;
}
foreach (QListWidgetItem* item, selectedComp) {
if (constraints->length())
plugin->manipClient()->graph()->setNumericalConstraints(components_[item->text().toStdString()].id,
constraints.in(), dofs.in());
if (locked->length())
plugin->manipClient()->graph()->setLockedDofConstraints(components_[item->text().toStdString()].id,
locked.in());
}
onCancelClicked();
}
}
}
//
// Copyright (c) CNRS
// Authors: Joseph Mirabel and Heidy Dallard
//
#ifndef MANIPULATIONNCPICKER_HH
#define MANIPULATIONNCPICKER_HH
#include <hpp/corbaserver/manipulation/graph-idl.hh>
#include "hppwidgetsplugin/numericalconstraintpicker.hh"
namespace hpp {
namespace gui {
class HppManipulationWidgetsPlugin;
class ManipulationNCPicker : public NumericalConstraintPicker
{
Q_OBJECT
private slots:
virtual void onConfirmClicked();
public:
ManipulationNCPicker(HppManipulationWidgetsPlugin* plugin,
QWidget* parent = 0);
virtual ~ManipulationNCPicker();
protected:
std::map<std::string, GraphComp> components_;
QListWidget* listComp_;
};
}
}
#endif // MANIPULATIONNCPICKER_HH
......@@ -15,6 +15,7 @@
#include <gepetto/gui/windows-manager.hh>
#include <gepetto/gui/omniorb/url.hh>
#include <gepetto/gui/action-search-bar.hh>
#include <gepetto/gui/safeapplication.hh>
#include <omniORB4/CORBA.h>
......@@ -40,6 +41,22 @@ namespace hpp {
typedef gepetto::viewer::Configuration OsgConfiguration_t;
typedef gepetto::gui::ActionSearchBar ActionSearchBar;
class HppExceptionCatch : public gepetto::gui::SlotExceptionCatch
{
public:
bool safeNotify (QApplication* app, QObject* receiver, QEvent* e)
{
try {
return impl_notify (app, receiver, e);
} catch (const hpp::Error& e) {
qDebug () << e.msg.in();
MainWindow* main = MainWindow::instance();
if (main != NULL) main->logError (e.msg.in());
}
return false;
}
};
HppWidgetsPlugin::JointElement::JointElement (
const std::string& n, const std::string& prefix,
const hpp::Names_t& bns, JointTreeItem* i, bool updateV)
......@@ -71,6 +88,9 @@ namespace hpp {
void HppWidgetsPlugin::init()
{
gepetto::gui::SafeApplication* app = dynamic_cast<gepetto::gui::SafeApplication*>(QApplication::instance());
if (app) app->addAsLeaf(new HppExceptionCatch);
openConnection();
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance ();
......@@ -99,6 +119,7 @@ namespace hpp {
// Path player widget
dock = new QDockWidget ("&Path player", main);
dock->setObjectName ("hppwidgetplugin.pathplayer");
dock->setSizePolicy (QSizePolicy::Minimum, QSizePolicy::Fixed);
pathPlayer_ = new PathPlayer (this, dock);
dock->setWidget(pathPlayer_);
main->insertDockWidget (dock, Qt::BottomDockWidgetArea, Qt::Horizontal);
......@@ -271,7 +292,15 @@ namespace hpp {
hpp_ = new hpp::corbaServer::Client (0,0);
QByteArray iiop = getHppIIOPurl ().toLatin1();
QByteArray context = getHppContext ().toLatin1();
hpp_->connect (iiop.constData (), context.constData ());
try {
hpp_->connect (iiop.constData (), context.constData ());
} catch (const CosNaming::NamingContext::NotFound&) {
const char* msg = "Could not find the HPP server. Is it running ?";
qDebug () << msg;
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
if (main != NULL)
main->logError(msg);
}
}
void HppWidgetsPlugin::closeConnection ()
......
......@@ -20,13 +20,8 @@ namespace hpp {
{
ui->setupUi(this);
hpp::Names_t_var names = plugin->client()->problem()->getAvailable("lockedjoint");
ui->lockedJointList->setSelectionMode(QAbstractItemView::ExtendedSelection);
for (unsigned i = 0; i < names->length(); ++i) {
ui->lockedJointList->addItem(QString(names[i]));
}
names = plugin->client()->problem()->getAvailable("numericalconstraint");
hpp::Names_t_var names =
plugin->client()->problem()->getAvailable("numericalconstraint");
ui->numericalList->setSelectionMode(QAbstractItemView::ExtendedSelection);
for (unsigned i = 0; i < names->length(); ++i) {
ui->numericalList->addItem(QString(names[i]));
......
......@@ -267,15 +267,22 @@ namespace hpp {
void PathPlayer::updateConfiguration ()
{
hpp::floatSeq_var config =
plugin_->client()->problem()->configAtParam ((CORBA::ULong)pathIndex()->value(),currentParam_);
plugin_->currentConfig() = config.in();
if (velocity_) {
config =
plugin_->client()->problem()->derivativeAtParam ((CORBA::ULong)pathIndex()->value(),1,currentParam_);
plugin_->currentVelocity() = config.in();
gepetto::gui::MainWindow* main = gepetto::gui::MainWindow::instance();
hpp::floatSeq_var config, velocity;
try {
config = plugin_->client()->problem()->configAtParam
((CORBA::ULong)pathIndex()->value(),currentParam_);
if (velocity_)
velocity = plugin_->client()->problem()->derivativeAtParam
((CORBA::ULong)pathIndex()->value(),1,currentParam_);
} catch (const hpp::Error& e) {
main->logError(e.msg.in());
return;
}
gepetto::gui::MainWindow::instance()->requestApplyCurrentConfiguration();
plugin_->currentConfig() = config.in();
if (velocity_)
plugin_->currentVelocity() = velocity.in();
main->requestApplyCurrentConfiguration();
emit appliedConfigAtParam (getCurrentPath(), currentParam_);
}
......
......@@ -6,12 +6,12 @@
<rect>
<x>0</x>
<y>0</y>
<width>366</width>
<height>64</height>
<width>562</width>
<height>50</height>
</rect>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Minimum">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
......@@ -22,6 +22,12 @@
<layout class="QHBoxLayout" name="horizontalLayout">
<item>
<widget class="QPushButton" name="refreshButton_path">
<property name="sizePolicy">
<sizepolicy hsizetype="Fixed" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="text">
<string/>
</property>
......@@ -42,6 +48,12 @@
<property name="enabled">
<bool>false</bool>
</property>
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>Path index</string>
</property>
......@@ -126,6 +138,12 @@
</item>
<item>
<widget class="QDoubleSpinBox" name="timeSpinBox">
<property name="sizePolicy">
<sizepolicy hsizetype="Preferred" vsizetype="Fixed">
<horstretch>0</horstretch>
<verstretch>0</verstretch>
</sizepolicy>
</property>
<property name="toolTip">
<string>Total time</string>
</property>
......
......@@ -166,7 +166,7 @@ namespace hpp {
if (!vec3d.isNull()) {
qtQuat = QQuaternion::fromAxisAndAngle(vec3d.normalized(), vec3d.length());
const float theta = vec3d.length();
const float theta = (float) vec3d.length();
qtQuat = QQuaternion(std::cos(theta/2), std::sin(theta/2) * vec3d / theta);
}
......@@ -233,7 +233,7 @@ namespace hpp {
if (!vec3d.isNull()) {
qtQuat = QQuaternion::fromAxisAndAngle(vec3d.normalized(), vec3d.length());
const float theta = vec3d.length();
const float theta = (float) vec3d.length();
qtQuat = QQuaternion(std::cos(theta/2), std::sin(theta/2) * vec3d / theta);
}
......
......@@ -15,8 +15,6 @@
# hpp-corbaserver. If not, see
# <http://www.gnu.org/licenses/>.
FINDPYTHON(2.7 EXACT)
INSTALL(
FILES
${CMAKE_CURRENT_SOURCE_DIR}/hpp/gui/plugin.py
......
from plugin import Plugin
from .plugin import Plugin
......@@ -5,7 +5,7 @@
from PythonQt import QtGui, Qt
import xml.etree.ElementTree as ET
from itertools import izip
import re
def _makeCheckBox(active):
......@@ -113,7 +113,7 @@ class CollisionPairs(QtGui.QWidget):
def configValidation(self):
collide = self.plugin.client.robot.autocollisionCheck()
for p, c in izip(self.orderedPairs, collide):
for p, c in zip(self.orderedPairs, collide):
if c:
self.pairToRow[p][self.CURRENT_CONFIG].setText("Collision")
else:
......@@ -121,7 +121,7 @@ class CollisionPairs(QtGui.QWidget):
def setCollisionPair(self, r, l1, l2, active, reason):
pair = _Pair(l1, l2)
if self.pairToRow.has_key(pair):
if pair in self.pairToRow:
self.pairToRow[pair][self.ACTIVE].checked = active
self.pairToRow[pair][self.REASON].setText(reason)
return
......@@ -149,15 +149,15 @@ class CollisionPairs(QtGui.QWidget):
ncfg = int(pow(10, self.sliderRandomCfg.value / 10))
nbCol = [0] * len(self.pairToRow)
r = self.plugin.client.robot
for i in xrange(ncfg):
for i in range(ncfg):
cfg = r.shootRandomConfig()
r.setCurrentConfig(cfg)
collide = r.autocollisionCheck()
for i in xrange(len(collide)):
for i in range(len(collide)):
if collide[i]:
nbCol[i] += 1
for p, n in izip(self.orderedPairs, nbCol):
for p, n in zip(self.orderedPairs, nbCol):
self.pairToRow[p][self.PERCENTAGE].setData(Qt.Qt.DisplayRole, n * 100. / ncfg)
def refresh(self):
......@@ -169,7 +169,7 @@ class CollisionPairs(QtGui.QWidget):
inner, outer, active = self.plugin.client.robot.autocollisionPairs()
self.table.sortingEnabled = False
self.table.setRowCount(len(inner))
for r, l1, l2, a in izip(xrange(len(inner)), inner, outer, active):
for r, l1, l2, a in zip(range(len(inner)), inner, outer, active):
self.setCollisionPair(r, l1, l2, a, "From SRDF")
self.table.sortingEnabled = True
print(time.time() - start_time)
......@@ -195,12 +195,12 @@ class CollisionPairs(QtGui.QWidget):
p = _Pair(dc.attrib["link1"], dc.attrib["link2"])
pairsInFile[p] = dc
for p, row in self.pairToRow.iteritems():
for p, row in self.pairToRow.items():
if row[self.ACTIVE].checked:
continue
reason = str(row[self.REASON].text())
pp = _Pair(_(p.l1), _(p.l2))
if pairsInFile.has_key(pp):
if pp in pairsInFile:
dc.attrib["reason"] = reason
else:
attrib = dict()
......
......@@ -3,6 +3,7 @@
# Author: Joseph Mirabel
#
from __future__ import print_function
from PythonQt import QtGui, Qt
import numpy as np
from gepetto import Quaternion, Color
......@@ -76,7 +77,7 @@ class GraspFinder(QtGui.QWidget):
def optimize(self):
n = len(self.P)
if not n == len(self.Q):
print "P and Q must be of the same size"
print("P and Q must be of the same size")
return
P = np.array(self.P).transpose()
Q = np.array(self.Q).transpose()
......@@ -91,11 +92,11 @@ class GraspFinder(QtGui.QWidget):
T = [0,0,0,1,0,0,0]
T[0:3] = t.tolist()
T[3:7] = Quaternion(R).toTuple()
print self.names[0], T
print(self.names[0], T)
self.instructions.text = "Current transform of\n" + self.names[0] + "\nis\n" + str(T) + "\n\nSelect a fixed point."
self.plugin.gui.gui.applyConfiguration(self.names[0], T)
self.plugin.gui.gui.refresh()
for i in xrange(n):
for i in range(n):
self.setLineTransform(i)
self.plugin.gui.gui.refresh()
......