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