def initActions(self):
self.action = QAction("Switch mode", self)
self.action.connect("triggered()", self.switchMode)
self.action.setShortcut(
QKeySequence(QNamespace.ControlModifier + QNamespace.Key_M))
self.addAction(self.action)
self.mainWindow.registerShortcut(self.plugin.windowTitle, self.action)
def initActions(self):
self.action = QAction("Start pick", self)
self.action.setShortcut(QKeySequence(QNamespace.Key_P))
self.action.connect("triggered()", self.startPlacement)
self.parent().mainWindow.registerShortcut(
self.parent().plugin.windowTitle, self.action)
self.addAction(self.action)
class _DynamicBuilder(QWidget):
def __init__(self, mainWindow, parent):
super(_DynamicBuilder, self).__init__(parent)
self.plugin = parent
self.selected = ""
self.mainWindow = mainWindow
self.mode = 0
# Init the widget view
self.initActions()
self.initWidget()
def initActions(self):
self.action = QAction("Switch mode", self)
self.action.connect("triggered()", self.switchMode)
self.action.setShortcut(
QKeySequence(QNamespace.ControlModifier + QNamespace.Key_M))
self.addAction(self.action)
self.mainWindow.registerShortcut(self.plugin.windowTitle, self.action)
def switchMode(self):
self.mode = not self.mode
if (not self.mode):
self.widgets[1].hide()
self.widgets[0].show()
else:
self.widgets[0].hide()
self.widgets[1].show()
def initWidget(self):
mainBox = QVBoxLayout(self)
mainBox.addWidget(QLabel("Press " + self.action.shortcut.toString() + " to " +\
self.action.text))
self.widgets = [_GraspMode(self), _PlacementMode(self)]
mainBox.addWidget(self.widgets[0])
mainBox.addWidget(self.widgets[1])
self.widgets[1].hide()
def initActions(self):
action = QAction("Choose as gripper", self)
action.connect("triggered()", self.selectGripper)
action.setShortcut(QKeySequence(QNamespace.Key_R))
self.addAction(action)
self.actionsList.append(action)
self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, "Choose as gripper", action)
action = QAction("Choose handle", self)
action.connect("triggered()", self.selectHandle)
action.setShortcut(QKeySequence(QNamespace.Key_H))
self.addAction(action)
self.actionsList.append(action)
self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, "Choose as handle", action)
action = QAction("Grasp from current", self.parentInstance)
action.connect("triggered()", self.graspCurrent)
action.setShortcut(QKeySequence(QNamespace.Key_G))
self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
self.actionsList.append(action)
action = QAction("Grasp from random", self.parentInstance)
action.connect("triggered()", self.graspRandom)
action.setShortcut(QKeySequence(QNamespace.ShiftModifier + QNamespace.Key_G))
self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
self.actionsList.append(action)
action = QAction("Pregrasp from current", self.parentInstance)
action.connect("triggered()", self.pregraspCurrent)
action.setShortcut(QKeySequence(QNamespace.Key_P))
self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
self.actionsList.append(action)
action = QAction("Pregrasp from random", self.parentInstance)
action.connect("triggered()", self.pregraspRandom)
action.setShortcut(QKeySequence(QNamespace.ShiftModifier + QNamespace.Key_P))
self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
self.actionsList.append(action)
action = QAction("Change handle used", self.parentInstance)
action.connect("triggered()", self.changeHandle)
action.setShortcut(QKeySequence(QNamespace.Key_F1))
self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
self.actionsList.append(action)
action = QAction("Change gripper used", self.parentInstance)
action.connect("triggered()", self.changeGripper)
action.setShortcut(QKeySequence(QNamespace.Key_F2))
self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
self.actionsList.append(action)
action = QAction("Lock current object", self.parentInstance)
action.connect("triggered()", self.lock)
action.setShortcut(QKeySequence(QNamespace.Key_L))
self.mainWindow.registerShortcut(self.parentInstance.windowTitle, action)
self.actionsList.append(action)
class _PlacementMode(QWidget):
def __init__(self, parent):
super(_PlacementMode, self).__init__(parent)
self.step = -1
self.surfaceName = ""
self.carryName = ""
self.initActions()
self.createWidget()
def initActions(self):
self.action = QAction("Start pick", self)
self.action.setShortcut(QKeySequence(QNamespace.Key_P))
self.action.connect("triggered()", self.startPlacement)
self.parent().mainWindow.registerShortcut(self.parent().plugin.windowTitle, self.action)
self.addAction(self.action)
def startPlacement(self):
if (self.parent().plugin.osg is not None):
self.step = -1
self.parent().plugin.osg.connect("clicked(QString, QVector3D)", self.selected)
def selected(self, bodyName, position):
selectedName = bodyName[bodyName.rfind("/contact_") + 9:len(bodyName)]
selectedName = selectedName.replace("__", "/")
selectedName = re.sub(r'_[0-9]*_', '', selectedName)
if (self.step == -1):
names = self.parent().plugin.client.manipulation.problem.getRobotContactNames()
for n in names:
if n == selectedName:
self.surfaceName = str(n)
self.step = 0
break
else:
names = self.parent().plugin.client.manipulation.problem.getEnvironmentContactNames()
for n in names:
if n == selectedName:
self.carryName = n
self.endPlacement(position)
break
def endPlacement(self, position):
name = self.surfaceName[0:self.surfaceName.find("/")] + "/root_joint"
names = self.parent().plugin.client.basic.robot.getAllJointNames()
if name in names:
jointType = self.parent().plugin.client.basic.robot.getJointType()
if jointType == "JointModelFreeFlyer":
self.parent().plugin.client.basic.robot.setJointConfig(n, [position.x(), position.y(), position.z(), 0, 0, 0, 1])
elif jointType == "JointModelPlanar":
self.parent().plugin.client.basic.robot.setJointConfig(n, [position.x(), position.y(), 1, 0])
elif jointType == "anchor":
self.parent().plugin.client.basic.robot.setJointPositionInParentFrame(n, [position.x(), position.y(), position.z(), 0, 0, 0, 1])
self.parent().plugin.client.basic.problem.resetConstraints()
self.parent().plugin.osg.disconnect("clicked(QString, QVector3D)", self.selected)
self.parent().plugin.client.manipulation.problem.createPlacementConstraint("placement",
[self.surfaceName], [self.carryName])
self.parent().plugin.client.basic.problem.addNumericalConstraints("numerical", ["placement"], [True])
res = self.parent().plugin.client.basic.problem.applyConstraints(self.parent().plugin.client.basic.robot.getCurrentConfig())
if res[0]:
self.parent().plugin.client.basic.robot.setCurrentConfig(res[1])
self.parent().mainWindow.requestApplyCurrentConfiguration()
def createWidget(self):
box = QVBoxLayout(self)
text = "You are in placement mode.\n" +\
"In this mode you can place an object\non an environment surface.\n" +\
"The object and the environment must\nhave contact surface defined.\n" +\
"Press :\n" +\
" - " + self.action.shortcut.toString() + " to " + self.action.text + "\n\n" +\
"First selected body will be\nconsidered as the object.\n" +\
"Second selected body will be\nconsidered as the environment.\n" +\
"The object will be place where\nyou clicked on the environment."
box.addWidget(QLabel(text, self))