factory.py 9.48 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
from hpp.corbaserver.manipulation.constraint_graph_factory import ConstraintFactoryAbstract, GraphFactoryAbstract
Joseph Mirabel's avatar
Joseph Mirabel committed
2
from tools import Manifold, Grasp, OpFrame, EEPosture
Joseph Mirabel's avatar
Joseph Mirabel committed
3
4
from dynamic_graph.sot.core import SOT

5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
class Affordance(object):
    def __init__ (self, gripper, handle, **kwargs):
        self.gripper = gripper
        self.handle  = handle
        self.setControl (**kwargs)
    def setControl (self, refOpen, refClose, openType = "position", closeType="position"):
        if openType != "position" or closeType != "position":
            raise NotImplementedError ("Only position control is implemented for gripper opening/closure.")
        self.controlType = {
                "open": openType,
                "close": closeType,
                }
        self.ref = {
                "open": refOpen,
                "close": refClose,
                }

Joseph Mirabel's avatar
Joseph Mirabel committed
22
23
24
25
26
27
28
class TaskFactory(ConstraintFactoryAbstract):
    gfields = ('grasp', 'pregrasp', 'gripper_open', 'gripper_close')
    pfields = ()

    def __init__ (self, graphfactory):
        super(TaskFactory, self).__init__ (graphfactory)

29
30
31
32
33
34
35
36
37
38
39
40
41
    def _buildGripper (self, type, gripper, handle):
        try:
            aff = self.graphfactory.affordances[(gripper, handle)]
        except KeyError:
            # If there are no affordance, do not add a task.
            return Manifold()
        gf = self.graphfactory
        robot = gf.sotrobot
        if aff.controlType[type] == "position":
            return EEPosture (robot, gf.gripperFrames[gripper], aff.ref[type])
        else:
            raise NotImplementedError ("Only position control is implemented for gripper closure.")

Alexis Nicolin's avatar
Alexis Nicolin committed
42
    def buildGrasp (self, g, h, otherGrasp=None):
Joseph Mirabel's avatar
Joseph Mirabel committed
43
44
        gf = self.graphfactory
        if h is None:
45
46
            gripper_open = self._buildGripper ("open", g, h)
            return { 'gripper_open': gripper_open }
Joseph Mirabel's avatar
Joseph Mirabel committed
47

48
        gripper_close  = self._buildGripper ("close", g, h)
Joseph Mirabel's avatar
Joseph Mirabel committed
49
        pregrasp = Grasp (gf.gripperFrames [g],
Alexis Nicolin's avatar
Alexis Nicolin committed
50
51
52
                          gf.handleFrames [h],
                          otherGrasp,
                          False)
Joseph Mirabel's avatar
Joseph Mirabel committed
53
54
        pregrasp.makeTasks (gf.sotrobot)
        grasp = Grasp (gf.gripperFrames [g],
Alexis Nicolin's avatar
Alexis Nicolin committed
55
56
57
                       gf.handleFrames [h],
                       otherGrasp,
                       True)
Joseph Mirabel's avatar
Joseph Mirabel committed
58
59
60
61
        grasp.makeTasks (gf.sotrobot)
        return { 'grasp': grasp,
                 'pregrasp': pregrasp,
                 'gripper_close': gripper_close }
Alexis Nicolin's avatar
Alexis Nicolin committed
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
    
    ## \name Accessors to the different elementary constraints
    # \{
    def getGrasp(self, gripper, handle, otherGrasp=None):
        if isinstance(gripper, str): ig = self.graphfactory.grippers.index(gripper)
        else: ig = gripper
        if isinstance(handle, str): ih = self.graphfactory.handles.index(handle)
        else: ih = handle
        if otherGrasp is not None:
            otherIg = self.graphfactory.grippers.index(otherGrasp.gripper.name)
            otherIh = self.graphfactory.handles.index(otherGrasp.handle.name)
            k = (ig, ih, otherIg, otherIh)
        else:
            k = (ig, ih)
        if not self._grasp.has_key(k):
            self._grasp[k] = self.buildGrasp(self.graphfactory.grippers[ig], None if ih is None else self.graphfactory.handles[ih], otherGrasp)
            assert isinstance (self._grasp[k], dict)
        return self._grasp[k]

    def g (self, gripper, handle, what, otherGrasp=None):
        return self.getGrasp(gripper, handle, otherGrasp)[what]
Joseph Mirabel's avatar
Joseph Mirabel committed
83
84
85
86
87
88
89
90

    def buildPlacement (self, o):
        # Nothing to do
        return dict()

class Factory(GraphFactoryAbstract):
    class State:
        def __init__ (self, tasks, grasps, factory):
Joseph Mirabel's avatar
Joseph Mirabel committed
91
            self.name = factory._stateName (grasps)
Joseph Mirabel's avatar
Joseph Mirabel committed
92
93
94
            self.grasps = grasps
            self.manifold = Manifold()

Alexis Nicolin's avatar
Alexis Nicolin committed
95
96
            objectsAlreadyGrasped = {}
            
Joseph Mirabel's avatar
Joseph Mirabel committed
97
            for ig, ih in enumerate(grasps):
Joseph Mirabel's avatar
Joseph Mirabel committed
98
                if ih is not None:
Joseph Mirabel's avatar
Joseph Mirabel committed
99
                    # Add task gripper_close
100
                    self.manifold += tasks.g (factory.grippers[ig], factory.handles[ih], 'gripper_close')
Alexis Nicolin's avatar
Alexis Nicolin committed
101
102
103
                    otherGrasp = objectsAlreadyGrasped.get(factory.objectFromHandle[ih])
                    self.manifold += tasks.g (factory.grippers[ig], factory.handles[ih], 'grasp', otherGrasp)
                    objectsAlreadyGrasped[factory.objectFromHandle[ih]] = tasks.g (factory.grippers[ig], factory.handles[ih], 'grasp', otherGrasp)
Joseph Mirabel's avatar
Joseph Mirabel committed
104
                else:
Joseph Mirabel's avatar
Joseph Mirabel committed
105
                    # Add task gripper_open
Joseph Mirabel's avatar
Joseph Mirabel committed
106
107
108
109
110
111
112
                    self.manifold += tasks.g (factory.grippers[ig], None, 'gripper_open')

    def __init__ (self, supervisor):
        super(Factory, self).__init__ ()
        self.tasks = TaskFactory (self)
        self.hpTasks = supervisor.hpTasks
        self.lpTasks = supervisor.lpTasks
113
        self.affordances = dict()
Joseph Mirabel's avatar
Joseph Mirabel committed
114
        self.sots = dict()
115
116
        self.postActions = dict()
        self.preActions = dict()
Joseph Mirabel's avatar
Joseph Mirabel committed
117
118
119

        self.supervisor = supervisor

120
121
122
123
    def addAffordance (self, aff):
        assert isinstance(aff, Affordance)
        self.affordances [(aff.gripper, aff.handle)] = aff

Joseph Mirabel's avatar
Joseph Mirabel committed
124
125
126
    def finalize (self, hppclient):
        graph, elmts = hppclient.manipulation.graph.getGraph()
        ids = { n.name: n.id for n in elmts.edges }
127
        nids = { n.name: n.id for n in elmts.nodes }
Joseph Mirabel's avatar
Joseph Mirabel committed
128

129
        self.supervisor.sots = { ids[n]: sot for n, sot in self.sots.items() if ids.has_key(n) }
Joseph Mirabel's avatar
Joseph Mirabel committed
130
131
132
        self.supervisor.grasps = { (gh, w): t for gh, ts in self.tasks._grasp.items() for w, t in ts.items() }
        self.supervisor.hpTasks = self.hpTasks
        self.supervisor.lpTasks = self.lpTasks
133
134
135
        self.supervisor.postActions = {
                ids[trans] : {
                    nids[state]: sot for state, sot in values.items() if nids.has_key(state)
136
                    } for trans, values in self.postActions.items() if ids.has_key(trans)
137
                }
138
        self.supervisor.preActions = { ids[trans] : sot for trans, sot in self.preActions.items() if ids.has_key(trans) }
Joseph Mirabel's avatar
Joseph Mirabel committed
139
140
141
142

    def setupFrames (self, hppclient, sotrobot):
        self.sotrobot = sotrobot

Joseph Mirabel's avatar
Joseph Mirabel committed
143
144
        self.grippersIdx = { g: i for i,g in enumerate(self.grippers) }
        self.handlesIdx  = { h: i for i,h in enumerate(self.handles) }
Joseph Mirabel's avatar
Joseph Mirabel committed
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159

        self.gripperFrames = { g: OpFrame(hppclient) for g in self.grippers }
        self.handleFrames  = { h: OpFrame(hppclient) for h in self.handles  }

        for g in self.grippers: self.gripperFrames[g].setHppGripper (g)
        for h in self.handles : self.handleFrames [h].setHppHandle  (h)

        for g in self.gripperFrames.values(): g.setSotFrameFromHpp (sotrobot.dynamic.model)
        for h in self.handleFrames .values(): h.setSotFrameFromHpp (sotrobot.dynamic.model)

    def makeState (self, grasps, priority):
        # Nothing to do here
        return Factory.State(self.tasks, grasps, self)

    def makeLoopTransition (self, state):
Alexis Nicolin's avatar
Alexis Nicolin committed
160
        n = self._loopTransitionName(state.grasps)
Joseph Mirabel's avatar
Joseph Mirabel committed
161
162
163
164
165
166
167
168
169
170
171
172
173
174
        sot = SOT ('sot_' + n)
        sot.setSize(self.sotrobot.dynamic.getDimension())

        self.hpTasks.pushTo(sot)
        state.manifold.pushTo(sot)
        self.lpTasks.pushTo(sot)

        self.sots[n] = sot

    def makeTransition (self, stateFrom, stateTo, ig):
        sf = stateFrom
        st = stateTo
        names = self._transitionNames(sf, st, ig)

Alexis Nicolin's avatar
Alexis Nicolin committed
175
        print "names: {}".format(names)
Joseph Mirabel's avatar
Joseph Mirabel committed
176
177
178
179
180

        iobj = self.objectFromHandle [st.grasps[ig]]
        obj = self.objects[iobj]
        noPlace = self._isObjectGrasped (sf.grasps, iobj)

Alexis Nicolin's avatar
Alexis Nicolin committed
181
182
        print "iobj, obj, noPlace: {}, {}, {}".format(iobj, obj, noPlace)

Joseph Mirabel's avatar
Joseph Mirabel committed
183
184
185
186
187
188
189
190
191
192
193
194
195
        # The different cases:
        pregrasp = True
        intersec = not noPlace
        preplace = not noPlace

        # Start here
        nWaypoints = pregrasp + intersec + preplace
        nTransitions = 1 + nWaypoints

        # Link waypoints
        transitions = names[:]
        assert nWaypoints > 0
        M = 1 + pregrasp
196
        sots = [ ]
Joseph Mirabel's avatar
Joseph Mirabel committed
197
198
199
200
201
202
203
204
205
        for i in range(nTransitions):
            ns = ("{0}_{1}{2}".format(names[0], i, i+1),
                  "{0}_{2}{1}".format(names[1], i, i+1))

            for n in ns:
                s = SOT ('sot_' + n)
                s.setSize(self.sotrobot.dynamic.getDimension())
                self.hpTasks.pushTo(s)

Alexis Nicolin's avatar
Alexis Nicolin committed
206
                #if pregrasp and i == 1:
Joseph Mirabel's avatar
Joseph Mirabel committed
207
                    # Add pregrasp task
Alexis Nicolin's avatar
Alexis Nicolin committed
208
                    #self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'pregrasp').pushTo (s)
Joseph Mirabel's avatar
Joseph Mirabel committed
209
210
211
212
213
                if i < M: sf.manifold.pushTo(s)
                else:     st.manifold.pushTo(s)

                self.lpTasks.pushTo(s)
                self.sots[n] = s
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
                sots.append (n)

        key = sots[2*(M-1)]
        states = ( st.name, names[0] + "_intersec")

        sot = SOT ("postAction_" + key)
        sot.setSize(self.sotrobot.dynamic.getDimension())
        self.hpTasks.pushTo (sot)
        # self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
        st.manifold.pushTo (sot)
        self.lpTasks.pushTo (sot)
        # print sot

        # TODO one post action is missing
        if not self.postActions.has_key(key):
            self.postActions[ key ] = dict()
        for n in states:
            self.postActions[ key ] [n] = sot

        key = sots[2*(M-1) + 1]
        states = ( st.name, names[0] + "_intersec")

        sot = SOT ("preAction_" + key)
        sot.setSize(self.sotrobot.dynamic.getDimension())
        self.hpTasks.pushTo (sot)
        # self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
        sf.manifold.pushTo (sot)
        self.lpTasks.pushTo (sot)
        # print sot

        self.preActions[ key ] = sot
        self.preActions[ sots[2*(M-1)] ] = sot