dynamicbuild.py 18 KB
Newer Older
1 2 3 4 5
#
#  Copyright (c) CNRS
#  Authors: Heidy Dallard, Joseph Mirabel
#

6
from PythonQt.QtGui import QDockWidget, QWidget, QLabel, QPushButton, QVBoxLayout, QFormLayout
7
from PythonQt.Qt import Qt as QNamespace, QAction, QKeySequence
8 9
from hpp.corbaserver import Client as BasicClient
from hpp.corbaserver.manipulation import Client as ManipClient, ConstraintGraph, Rule
Joseph Mirabel's avatar
Joseph Mirabel committed
10
import omniORB
11 12 13
from gepetto.corbaserver import Client as ViewerClient
import re

14 15 16 17 18 19 20 21
def xyzwTowxyz(q):
    return [q[(i+1)%4] for i in range(4)]

def fromHPP(t):
    ret = t[0:3]
    ret.extend(xyzwTowxyz(t[3:7]))
    return ret

22
class _Clients(object):
23 24
    def __init__(self, mainWindow):
        self.hppPlugin = mainWindow.getFromSlot("getHppIIOPurl")
Joseph Mirabel's avatar
Joseph Mirabel committed
25 26
        self.basic = BasicClient(url= str(self.hppPlugin.getHppIIOPurl()),
                postContextId= str(self.hppPlugin.getHppContext()))
Joseph Mirabel's avatar
Joseph Mirabel committed
27
        self.manipulation = ManipClient(url= str(self.hppPlugin.getHppIIOPurl()),
Joseph Mirabel's avatar
Joseph Mirabel committed
28
                postContextId= str(self.hppPlugin.getHppContext()))
29 30
        self.viewer = ViewerClient()

hdallard's avatar
hdallard committed
31 32 33 34 35
class _GraspMode(QWidget):
    def __init__(self, parent):
        super(_GraspMode, self).__init__(parent)
        self.parentInstance = parent
        self.mainWindow = parent.mainWindow
36 37 38 39
        self.selectedHandles = []
        self.selectedGrippers = []
        self.grippers = []
        self.handles = []
40
        self.locked = []
41 42 43
        self.groupName = ""
        self.currentGripper = 0
        self.currentHandle = 0
hdallard's avatar
hdallard committed
44
        self.actionsList = []
45 46 47 48 49

        self.initActions()
        self.createWidget()

    def initActions(self):
hdallard's avatar
hdallard committed
50 51 52 53 54 55 56 57 58 59 60 61 62 63 64
        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)
65 66
        action.connect("triggered()", self.graspCurrent)
        action.setShortcut(QKeySequence(QNamespace.Key_G))
hdallard's avatar
hdallard committed
67 68
        self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
        self.actionsList.append(action)
69

hdallard's avatar
hdallard committed
70
        action = QAction("Grasp from random", self.parentInstance)
71 72
        action.connect("triggered()", self.graspRandom)
        action.setShortcut(QKeySequence(QNamespace.ShiftModifier + QNamespace.Key_G))
hdallard's avatar
hdallard committed
73 74
        self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
        self.actionsList.append(action)
75

hdallard's avatar
hdallard committed
76
        action = QAction("Pregrasp from current", self.parentInstance)
77 78
        action.connect("triggered()", self.pregraspCurrent)
        action.setShortcut(QKeySequence(QNamespace.Key_P))
hdallard's avatar
hdallard committed
79 80
        self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
        self.actionsList.append(action)
81

hdallard's avatar
hdallard committed
82
        action = QAction("Pregrasp from random", self.parentInstance)
83 84
        action.connect("triggered()", self.pregraspRandom)
        action.setShortcut(QKeySequence(QNamespace.ShiftModifier + QNamespace.Key_P))
hdallard's avatar
hdallard committed
85 86
        self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
        self.actionsList.append(action)
87

hdallard's avatar
hdallard committed
88
        action = QAction("Change handle used", self.parentInstance)
Florent Lamiraux's avatar
Florent Lamiraux committed
89
        action.connect("triggered()", self.changeHandle)
90
        action.setShortcut(QKeySequence(QNamespace.Key_F1))
hdallard's avatar
hdallard committed
91 92
        self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
        self.actionsList.append(action)
93

hdallard's avatar
hdallard committed
94
        action = QAction("Change gripper used", self.parentInstance)
95 96
        action.connect("triggered()", self.changeGripper)
        action.setShortcut(QKeySequence(QNamespace.Key_F2))
hdallard's avatar
hdallard committed
97 98
        self.mainWindow.registerShortcut(self.parentInstance.plugin.windowTitle, action)
        self.actionsList.append(action)
99

hdallard's avatar
hdallard committed
100
        action = QAction("Lock current object", self.parentInstance)
101 102
        action.connect("triggered()", self.lock)
        action.setShortcut(QKeySequence(QNamespace.Key_L))
hdallard's avatar
hdallard committed
103 104
        self.mainWindow.registerShortcut(self.parentInstance.windowTitle, action)
        self.actionsList.append(action)
105 106

    def createWidget(self):
hdallard's avatar
hdallard committed
107
        box = QVBoxLayout(self)
108 109 110
        self.text = "You're in grasp/pregrasp mode.\n" +\
        "In this mode, you can select two object.\n" +\
        "To do so, click on it.\n" +\
hdallard's avatar
hdallard committed
111
        "Once you choose one, press :\n"
112

hdallard's avatar
hdallard committed
113 114
        self.addActions(self.actionsList)
        for a in self.actionsList:
115 116
            self.text += "  - " + a.shortcut.toString() + " to " + a.text + ".\n"

hdallard's avatar
hdallard committed
117
        box.addWidget(QLabel(self.text, self))
118
        fBox = QFormLayout()
hdallard's avatar
hdallard committed
119
        self.handleLabel = QLabel("None selected", self)
120
        fBox.addRow("Current handle", self.handleLabel)
hdallard's avatar
hdallard committed
121
        self.gripperLabel = QLabel("None selected", self)
122
        fBox.addRow("Current gripper", self.gripperLabel)
123
        box.addLayout(fBox)
124

hdallard's avatar
hdallard committed
125 126 127 128 129 130 131 132 133 134 135 136
        self.mainWindow.connect("selectJointFromBodyName(QString)", self.changeSelected)

    def changeSelected(self, name):
        self.selected = name.split("/")[0]

    def selectHandle(self):
        if (self.selected != ""):
            self.addHandle(self.selected)

    def selectGripper(self):
        if (self.selected != ""):
            self.addGripper(self.selected)
137 138 139

    def addHandle(self, handleName):
        if (len(self.selectedHandles) == 1):
hdallard's avatar
hdallard committed
140
            self.parentInstance.plugin.client.viewer.gui.deleteNode(self.groupName, True)
141
        self.selectedHandles = [str(handleName)]
142
        self.handles = self.getAvailable(handleName + "/", "handle")
143
        self.currentHandle = 0
144
        if (len(self.handles)):
hdallard's avatar
hdallard committed
145
            config = self.parentInstance.plugin.client.manipulation.robot.getHandlePositionInJoint(self.handles[self.currentHandle])
146 147 148 149
            self.handleLabel.setText(self.handles[self.currentHandle])
            self.drawXYZAxis("handle_"+self.handles[self.currentHandle].replace("/", "_"), config)
        else:
            self.handleLabel.setText("None selected")
150 151 152

    def addGripper(self, gripperName):
        if (len(self.selectedGrippers) == 1):
hdallard's avatar
hdallard committed
153
            self.parentInstance.plugin.client.viewer.gui.deleteNode(self.groupName, True)
154
        self.selectedGrippers = [str(gripperName)]
155
        self.grippers = self.getAvailable(gripperName + "/", "gripper")
156
        self.currentGripper = 0
hdallard's avatar
hdallard committed
157
        self.locked = []
158 159
        if (len(self.grippers) > 0):
            self.gripperLabel.setText(self.grippers[self.currentGripper])
hdallard's avatar
hdallard committed
160
            config = self.parentInstance.plugin.client.manipulation.robot.getGripperPositionInJoint(self.grippers[self.currentGripper])
161 162 163
            self.drawXYZAxis("gripper_"+self.grippers[self.currentGripper].replace("/", "_"), config)
        else:
            self.gripperLabel.setText("None selected")
164 165

    def getAvailable(self, comp, t):
hdallard's avatar
hdallard committed
166
        l = self.parentInstance.plugin.client.manipulation.problem.getAvailable(t)
167 168 169 170 171 172 173 174 175
        ret = []
        for name in l:
            if (name.startswith(comp)):
                ret.append(name)
        return ret
        
    def drawXYZAxis(self, name, config):
        obj = self.mainWindow.getFromSlot("requestCreateJointGroup")
        self.groupName = str(obj.requestCreateJointGroup(config[0]))
Joseph Mirabel's avatar
Joseph Mirabel committed
176
        self.parentInstance.plugin.client.viewer.gui.addXYZaxis(name, [0, 1, 0, 1], 0.005, 0.015)
177
        self.parentInstance.plugin.client.viewer.gui.applyConfiguration(name, fromHPP(config[1])) # XYZW -> WXYZ
hdallard's avatar
hdallard committed
178 179
        self.parentInstance.plugin.client.viewer.gui.addToGroup(name, self.groupName)
        self.parentInstance.plugin.client.viewer.gui.refresh()
180 181 182 183 184 185 186

    def changeHandle(self):
        if (len(self.handles) > 0):
            previous = self.currentHandle
            self.currentHandle += 1
            if (self.currentHandle == len(self.handles)):
                self.currentHandle = 0
hdallard's avatar
hdallard committed
187
            self.handleLabel.setText(self.handles[self.currentHandle])
188
            if (previous != self.currentHandle):
hdallard's avatar
hdallard committed
189 190
                self.parentInstance.plugin.client.viewer.gui.deleteNode(self.groupName, True)
                config = self.parentInstance.plugin.client.manipulation.robot.getHandlePositionInJoint(self.handles[self.currentHandle])
191 192 193 194 195 196 197 198 199 200
                self.drawXYZAxis("handle_"+self.handles[self.currentHandle].replace("/", "_"), config)

    def changeGripper(self):
        if (len(self.grippers) > 0):
            previous = self.currentGripper
            self.currentGripper += 1
            if (self.currentGripper == len(self.grippers)):
                self.currentGripper = 0
            self.gripperLabel.setText(self.grippers[self.currentGripper])
            if (self.currentGripper != previous):
hdallard's avatar
hdallard committed
201 202
                self.parentInstance.plugin.client.viewer.gui.deleteNode(self.groupName, True)
                config = self.parentInstance.plugin.client.manipulation.robot.getGripperPositionInJoint(self.grippers[self.currentGripper])
203 204
                self.drawXYZAxis("gripper_"+self.grippers[self.currentGripper].replace("/", "_"), config)

205 206
    def graspCurrent(self):
        if (len(self.handles) > 0 and len(self.grippers) > 0):
207
            self.grasp(self.parentInstance.plugin.hppPlugin.getCurrentQtConfig())
208

209 210
    def graspRandom(self):
        if (len(self.handles) > 0 and len(self.grippers) > 0):
hdallard's avatar
hdallard committed
211
            self.grasp(self.parentInstance.plugin.client.basic.robot.shootRandomConfig())
212

213
    def grasp(self, config):
hdallard's avatar
hdallard committed
214
        self.parentInstance.plugin.client.basic.problem.resetConstraints()
215
        for j in self.locked:
hdallard's avatar
hdallard committed
216
            self.parentInstance.plugin.client.basic.problem.lockJoint(j, self.parentInstance.plugin.client.basic.robot.getJointConfig(j))
Joseph Mirabel's avatar
Joseph Mirabel committed
217 218 219 220 221
        try:
            self.parentInstance.plugin.client.manipulation.problem.getSelected("constraintgraph")
        except omniORB.CORBA.UserException:
            self.parentInstance.plugin.client.manipulation.graph.createGraph("dummy")
            self.parentInstance.plugin.client.manipulation.graph.createSubGraph("dummy")
222
        name = self.grippers[self.currentGripper] + " grasps " + self.handles[self.currentHandle]
hdallard's avatar
hdallard committed
223
        self.parentInstance.plugin.client.manipulation.problem.createGrasp(name, self.grippers[self.currentGripper], self.handles[self.currentHandle])
Joseph Mirabel's avatar
Joseph Mirabel committed
224
        self.parentInstance.plugin.client.basic.problem.addNumericalConstraints("constraints", [name,], [0,])
hdallard's avatar
hdallard committed
225
        res = self.parentInstance.plugin.client.basic.problem.applyConstraints(config)
226
        if res[0] == True:
227
            self.parentInstance.plugin.hppPlugin.setCurrentQtConfig(res[1])
228 229 230

    def pregraspCurrent(self):
        if (len(self.handles) > 0 and len(self.grippers) > 0):
231
            self.pregrasp(self.parentInstance.plugin.hppPlugin.getCurrentQtConfig())
232 233 234

    def pregraspRandom(self):
        if (len(self.handles) > 0 and len(self.grippers) > 0):
hdallard's avatar
hdallard committed
235
            self.pregrasp(self.parentInstance.plugin.client.basic.robot.shootRandomConfig())
236 237

    def pregrasp(self, config):
hdallard's avatar
hdallard committed
238
        self.parentInstance.plugin.client.basic.problem.resetConstraints()
239
        for j in self.locked:
hdallard's avatar
hdallard committed
240
            self.parentInstance.plugin.client.basic.problem.lockJoint(name, self.parentInstance.plugin.client.basic.robot.getJointConfig())
241
        name = self.grippers[self.currentGripper] + " pregrasps " + self.handles[self.currentHandle]
hdallard's avatar
hdallard committed
242
        self.parentInstance.plugin.client.manipulation.problem.createGrasp(name, self.grippers[self.currentGripper], self.handles[self.currentHandle])
Joseph Mirabel's avatar
Joseph Mirabel committed
243
        self.parentInstance.plugin.client.basic.problem.addNumericalConstraints("constraints", [name], [True])
244
        res = self.parentInstance.plugin.client.basic.problem.applyConstraints(self.parentInstance.plugin.hppPlugin.getCurrentQtConfig())
245
        if (res[0] == True):
246
            self.parentInstance.plugin.hppPlugin.setCurrentQtConfig(res[1])
247

248
    def lock(self):
hdallard's avatar
hdallard committed
249
        if (self.selected != ""):
250
            self.locked = []
hdallard's avatar
hdallard committed
251 252
            joints = self.parentInstance.plugin.client.basic.robot.getAllJointNames()
            name = self.selected + "/"
253 254 255 256
            for j in joints:
                if j.startswith(name):
                    self.locked.append(j)

hdallard's avatar
hdallard committed
257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273 274 275 276
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
hdallard's avatar
hdallard committed
277
            self.parent().plugin.osg.connect("clicked(QString, QVector3D)", self.selected)
hdallard's avatar
hdallard committed
278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298

    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):
Joseph Mirabel's avatar
Joseph Mirabel committed
299
        name = self.surfaceName[0:self.surfaceName.find("/")] + "/root_joint"
hdallard's avatar
hdallard committed
300
        names = self.parent().plugin.client.basic.robot.getAllJointNames()
Joseph Mirabel's avatar
Joseph Mirabel committed
301 302 303 304 305 306 307 308 309
        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()
hdallard's avatar
hdallard committed
310
        self.parent().plugin.osg.disconnect("clicked(QString, QVector3D)", self.selected)
hdallard's avatar
hdallard committed
311 312
        self.parent().plugin.client.manipulation.problem.createPlacementConstraint("placement",
                                                                                   [self.surfaceName], [self.carryName])
Joseph Mirabel's avatar
Joseph Mirabel committed
313 314

        self.parent().plugin.client.basic.problem.addNumericalConstraints("numerical", ["placement"], [True])
315
        res = self.parent().plugin.client.basic.problem.applyConstraints(self.parent().plugin.hppPlugin.getCurrentQtConfig())
hdallard's avatar
hdallard committed
316
        if res[0]:
317
            self.parent().plugin.hppPlugin.setCurrentQtConfig(res[1])
hdallard's avatar
hdallard committed
318 319 320 321 322 323 324 325 326 327 328 329 330 331

    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))

332
class _DynamicBuilder(QWidget):
333
    def __init__(self, mainWindow, parent):
334
        super(_DynamicBuilder, self).__init__(parent)
335
        self.plugin = parent
336 337
        self.selected = ""
        self.mainWindow = mainWindow
hdallard's avatar
hdallard committed
338
        self.mode = 0
339 340

        # Init the widget view
341
        self.initActions()
342 343
        self.initWidget()

344
    def initActions(self):
hdallard's avatar
hdallard committed
345 346
        self.action = QAction("Switch mode", self)
        self.action.connect("triggered()", self.switchMode)
347
        self.action.setShortcut(QKeySequence(QNamespace.ControlModifier + QNamespace.Key_M))
hdallard's avatar
hdallard committed
348 349 350 351 352 353 354 355 356 357 358
        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()
359 360 361 362

    def initWidget(self):
        mainBox = QVBoxLayout(self)

hdallard's avatar
hdallard committed
363 364 365 366 367 368
        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()
369 370 371 372 373 374 375

class Plugin(QDockWidget):
    def __init__(self, mainWindow, flags = None):
        if (flags is not None):
            super(Plugin, self).__init__("Dynamic Builder", flags)
        else:
            super(Plugin, self).__init__("Dynamic Builder")
Joseph Mirabel's avatar
Joseph Mirabel committed
376
        self.setObjectName ("hpp.gui.dynamicbuild")
hdallard's avatar
hdallard committed
377
        self.osg = None
378
        self.mainWindow = mainWindow
379
        self.hppPlugin = mainWindow.getFromSlot("getHppIIOPurl")
Joseph Mirabel's avatar
Joseph Mirabel committed
380
        #self.resetConnection()
381 382
        mainWindow.registerShortcut("Dynamic builder", "Toggle view", self.toggleViewAction())
        self.dynamicBuilder = _DynamicBuilder(mainWindow, self)
383
        self.setWidget(self.dynamicBuilder)
384

hdallard's avatar
hdallard committed
385 386 387 388
    def osgWidget(self, osg):
        if (self.osg is None):
            self.osg = osg

389
    def resetConnection(self):
390
        self.client = _Clients(self.mainWindow)