Commit 97a7f173 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Add collision pair widget

parent 97a4511f
......@@ -25,6 +25,7 @@ INSTALL(
${CMAKE_CURRENT_SOURCE_DIR}/hpp/gui/findGrasp.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/gui/dynamicbuild.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/gui/inspector.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/gui/collision_pairs.py
${CMAKE_CURRENT_SOURCE_DIR}/hpp/gui/__init__.py
DESTINATION ${PYTHON_SITELIB}/hpp/gui
)
from PythonQt import QtGui, Qt
import xml.etree.ElementTree as ET
from itertools import izip
import re
def _makeCheckBox(active):
item = QtGui.QCheckBox()
item.checked = active
return item
def _makeTableWidgetItem(lbl, checkable, editable):
item = QtGui.QTableWidgetItem(lbl)
f = Qt.Qt.ItemIsEnabled | Qt.Qt.ItemIsSelectable
if editable:
f = f | Qt.Qt.ItemIsEditable
if checkable:
f = f | Qt.Qt.ItemIsUserCheckable
item.setFlags(f)
return item
def _bodyNameToUrdfLinkName(n):
return re.sub("_[0-9]+$", "", n)
class _Pair:
def __init__(self, l1, l2):
self.l1 = l1
self.l2 = l2
if l1 > l2:
self.key = (l1, l2)
else:
self.key = (l2, l1)
self.hash = hash(self.key)
def __eq__(x,y):
return (x.key == y.key)
def __hash__(self):
return self.hash
class CollisionPairs(QtGui.QWidget):
ACTIVE=0
LINK_1=1
LINK_2=2
REASON=3
CURRENT_CONFIG=4
PERCENTAGE=5
def __init__ (self, parent):
super(CollisionPairs, self).__init__ (parent)
self.plugin = parent
# parent.widgetToRefresh.append(self)
self.orderedPairs = list()
self.pairToRow = dict()
box = QtGui.QVBoxLayout(self)
button = QtGui.QPushButton(QtGui.QIcon.fromTheme("view-refresh"), "Refresh list", self)
button.connect("clicked()", self.refresh)
box.addWidget(button)
# Create table
self.table = QtGui.QTableWidget(0, 6)
self.table.setHorizontalHeaderLabels(["Active", "Link 1", "Link 2", "Reason", "Current configuration", "% of collision"])
self.table.horizontalHeader().setResizeMode(QtGui.QHeaderView.Interactive)
self.table.selectionBehavior = QtGui.QAbstractItemView.SelectRows
self.table.connect("currentItemChanged(QTableWidgetItem*,QTableWidgetItem*)", self.currentItemChanged)
box.addWidget(self.table)
# "Number of random configuration (log scale)"
self.sliderRandomCfg = QtGui.QSlider(Qt.Qt.Horizontal, self)
self.sliderRandomCfg.setRange(20, 60)
self.sliderRandomCfg.setValue(30)
box.addWidget(self.sliderRandomCfg)
button = QtGui.QPushButton("Compute percentage of collision", self)
button.connect("clicked()", self.computePercentageOfCollision)
box.addWidget(button)
button = QtGui.QPushButton("Save to file...", self)
button.connect("clicked()", self.writeToFile)
box.addWidget(button)
obj = self.plugin.main.getFromSlot("getHppIIOPurl")
if obj is not None:
obj.connect("configurationValidationStatus(QStringList)", self.currentBodyInCollisions)
else:
print("Could not find obj")
def showEvent(self, event):
self.plugin.main.connect('configurationValidation()', self.configValidation)
def hideEvent(self, event):
self.plugin.main.disconnect('configurationValidation()', self.configValidation)
def statusChanged(self, row, checked):
row[self.REASON].setText("User defined")
self.plugin.client.robot.setAutoCollision(str(row[self.LINK_1].text()), str(row[self.LINK_2].text()), checked)
def currentItemChanged(self, current, previous):
if current is not None and ( current.column() == 1 or current.column() == 2):
self.plugin.main.bodyTree().selectBodyByName(self.robotName + "/" + current.text())
def configValidation(self):
collide = self.plugin.client.robot.autocollisionCheck()
for p, c in izip(self.orderedPairs, collide):
if c:
self.pairToRow[p][self.CURRENT_CONFIG].setText("Collision")
else:
self.pairToRow[p][self.CURRENT_CONFIG].setText("Free")
def setCollisionPair(self, r, l1, l2, active, reason):
pair = _Pair(l1, l2)
if self.pairToRow.has_key(pair):
self.pairToRow[pair][self.ACTIVE].checked = active
self.pairToRow[pair][self.REASON].setText(reason)
return
row = (_makeCheckBox (active),
_makeTableWidgetItem(l1, True , False),
_makeTableWidgetItem(l2, False, False),
_makeTableWidgetItem(reason, False, True ),
_makeTableWidgetItem("NA" , False, False),
_makeTableWidgetItem("NA" , False, False))
self.table.setCellWidget(r, self.ACTIVE, row[self.ACTIVE])
self.table.setItem(r, self.LINK_1, row[self.LINK_1])
self.table.setItem(r, self.LINK_2, row[self.LINK_2])
self.table.setItem(r, self.REASON, row[self.REASON])
self.table.setItem(r, self.CURRENT_CONFIG, row[self.CURRENT_CONFIG])
self.table.setItem(r, self.PERCENTAGE, row[self.PERCENTAGE])
row[self.ACTIVE].connect("toggled(bool)", lambda c: self.statusChanged(row, c))
self.pairToRow[pair] = row
self.orderedPairs.append(pair)
def currentBodyInCollisions(self, bodies):
self.bodies = bodies
def computePercentageOfCollision(self):
from math import pow
ncfg = int(pow(10, self.sliderRandomCfg.value / 10))
nbCol = [0] * len(self.pairToRow)
r = self.plugin.client.robot
for i in xrange(ncfg):
cfg = r.shootRandomConfig()
r.setCurrentConfig(cfg)
collide = r.autocollisionCheck()
for i in xrange(len(collide)):
if collide[i]:
nbCol[i] += 1
for p, n in izip(self.orderedPairs, nbCol):
self.pairToRow[p][self.PERCENTAGE].setData(Qt.Qt.DisplayRole, n * 100. / ncfg)
def refresh(self):
self.pairToRow.clear()
self.orderedPairs = list()
import time
start_time = time.time()
self.robotName = self.plugin.client.robot.getRobotName()
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):
self.setCollisionPair(r, l1, l2, a, "From SRDF")
self.table.sortingEnabled = True
print(time.time() - start_time)
def writeToFile(self):
_ = _bodyNameToUrdfLinkName
filename = QtGui.QFileDialog.getSaveFileName(self, "SRDF file", "", "SRDF files (*.srdf)")
file = Qt.QFileInfo(filename)
if file.exists():
tree = ET.parse(filename)
else:
tree = ET.ElementTree(ET.Element("robot", { "name": "name" }))
tree.getroot().append(ET.Comment("Generated by hpp-gui Python plugin"))
robot = tree.getroot()
pairsInFile = dict()
for dc in robot.findall("disable_collisions"):
p = _Pair(dc.attrib["link1"], dc.attrib["link2"])
pairsInFile[p] = dc
for p, row in self.pairToRow.iteritems():
if row[self.ACTIVE].checked:
continue
reason = str(row[self.REASON].text())
pp = _Pair(_(p.l1), _(p.l2))
if pairsInFile.has_key(pp):
dc.attrib["reason"] = reason
else:
attrib = dict()
attrib["link1"] = pp.l1
attrib["link2"] = pp.l2
attrib["reason"] = reason
el = ET.Element("disable_collisions", attrib = attrib)
el.text = "\n "
robot.append(el)
tree.write(filename)
from PythonQt import QtGui, Qt
from hpp.corbaserver import Client
from hpp.corbaserver.robot import Robot
from gepetto.corbaserver import Client as GuiClient
from directpath import DirectPathBox
from findGrasp import GraspFinder
from inspector import InspectBodies
from collision_pairs import CollisionPairs
class _PathTab(QtGui.QWidget):
def __init__ (self, parent):
......@@ -18,6 +20,7 @@ class _PathManagement(QtGui.QWidget):
def __init__(self, parent):
super(_PathManagement, self).__init__(parent)
self.plugin = parent
parent.widgetToRefresh.append(self)
box = QtGui.QVBoxLayout(self)
......@@ -157,25 +160,34 @@ class Plugin(QtGui.QDockWidget):
self.main = mainWindow
self.hppPlugin = self.main.getFromSlot("getHppIIOPurl")
self.resetConnection()
self.widgetToRefresh = list()
self.osg = None
# Initialize the widget
self.tabWidget = QtGui.QTabWidget(self)
self.setWidget (self.tabWidget)
self.concatenateWidget = _PathManagement(self)
self.tabWidget.addTab (_PathTab(self), "Path")
self.tabWidget.addTab (_RoadmapTab(self), "Roadmap")
self.tabWidget.addTab (_StepByStepSolverTab(self), "Step by step solver")
self.tabWidget.addTab (GraspFinder(self), "Grasp Finder")
self.tabWidget.addTab (self.concatenateWidget, "Paths management")
self.tabWidget.addTab (_PathManagement(self), "Paths management")
self.tabWidget.addTab (InspectBodies(self), "Inspector")
self.tabWidget.addTab (CollisionPairs(self), "Collision pairs")
def resetConnection(self):
self.client = Client(url= str(self.hppPlugin.getHppIIOPurl()))
self.resetRobot();
self.gui = GuiClient()
def resetRobot(self):
try:
self.robot = Robot(client = self.client)
except:
self.robot = None
def osgWidget(self,osg):
if self.osg is None:
self.osg = osg
def refreshInterface(self):
self.concatenateWidget.refresh()
self.resetRobot()
for w in self.widgetToRefresh: w.refresh()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment