supervisor.py 7.5 KB
Newer Older
Joseph Mirabel's avatar
Joseph Mirabel committed
1
from tools import Manifold, Posture
Joseph Mirabel's avatar
Joseph Mirabel committed
2
3
4
from dynamic_graph.sot.core import SOT
from dynamic_graph import plug

5
6
7
8
9
def _hpTasks (sotrobot):
    return Manifold()
def _lpTasks (sotrobot):
    return Posture ("posture", sotrobot)

Joseph Mirabel's avatar
Joseph Mirabel committed
10
11
12
13
14
15
16
17
18
19
class Supervisor(object):
    """
    Steps: P = placement, G = grasp, p = pre-P, g = pre-G
    0. P <-> GP
    1. P <-> gP
    2. gP <-> GP
    3. GP <-> G
    4. GP <-> Gp
    5. Gp <-> G
    """
Joseph Mirabel's avatar
Joseph Mirabel committed
20
    def __init__ (self, sotrobot, lpTasks = None, hpTasks = None):
Joseph Mirabel's avatar
Joseph Mirabel committed
21
        self.sotrobot = sotrobot
Joseph Mirabel's avatar
Joseph Mirabel committed
22
23
24
25
        self.hpTasks = hpTasks if hpTasks is not None else _hpTasks(sotrobot)
        self.lpTasks = lpTasks if lpTasks is not None else _lpTasks(sotrobot)
        self.currentSot = None

Joseph Mirabel's avatar
Joseph Mirabel committed
26
27
28
29
30
31
32
33
34
    def setupEvents (self):
        from dynamic_graph_hpp.sot import Event, CompareDouble
        from dynamic_graph.sot.core.operator import Norm_of_vector
        from dynamic_graph.ros import RosImport
        self.norm = Norm_of_vector ("control_norm")
        plug (self.sotrobot.device.control, self.norm.sin)

        self.norm_comparision = CompareDouble ("control_norm_comparison")
        plug (self.norm.sout, self.norm_comparision.sin1)
Joseph Mirabel's avatar
Joseph Mirabel committed
35
        self.norm_comparision.sin2.value = 1e-2
Joseph Mirabel's avatar
Joseph Mirabel committed
36
37
38
39
40
41
42
43
44
45
46
47

        self.norm_event = Event ("control_norm_event")
        plug (self.norm_comparision.sout, self.norm_event.condition)
        # self.sotrobot.device.after.addSignal (self.norm_event.check.name)
        self.sotrobot.device.after.addSignal ("control_norm_event.check")

        self.norm_ri = RosImport ('ros_import_control_norm')
        self.norm_ri.add ('double', 'event_control_norm', '/sot_hpp/control_norm_changed')
        plug (self.norm.sout, self.norm_ri.event_control_norm)
        # plug (self.norm_event.trigger, self.norm_ri.trigger)
        self.norm_event.addSignal ("ros_import_control_norm.trigger")

Joseph Mirabel's avatar
Joseph Mirabel committed
48
    def makeInitialSot (self):
49
50
51
        # Create the initial sot (keep)
        sot = SOT ('sot_keep')
        sot.setSize(self.sotrobot.dynamic.getDimension())
Joseph Mirabel's avatar
Joseph Mirabel committed
52
        self.keep_posture = Posture ("posture_keep", self.sotrobot)
53
        self.keep_posture.tp.setWithDerivative (False)
54
55
56
57
58
59
60
61
62
63
64
        
        # TODO : I do agree that this is a dirty hack.
        # The COM of the robot in the HPP frame is at those coordinates (approx.).
        # But the keep_posture task is « internally » (there is no actuator able to directly move the COM, 
        # but the controller in the task is computing controls anyway, and integrating them) 
        # moving the computed COM to its reference value which is (0, 0, 0).
        # So, when we send the goal coordinates of the feet from HPP to the SoT, there is an offset of 0,74m
        # between the goal and the current position of the feet. This was the cause of the strange feet
        # movements at the beginning of the demo.
        # Maybe we can get the COM position and orientation from HPP at the beginning of the trajectory
        # to initialize self.sotrobot.dynamic.position.value
Alexis Nicolin's avatar
Alexis Nicolin committed
65
        self.keep_posture._signalPositionRef().value = tuple([-0.74, 0.0, 1.0, 0.0, 0.0, 0.0] + list(self.sotrobot.dynamic.position.value)[6:])
66
        
Joseph Mirabel's avatar
Joseph Mirabel committed
67
        self.keep_posture.pushTo(sot)
68
69
70
        self.sots[-1] = sot

    def topics (self):
Joseph Mirabel's avatar
Joseph Mirabel committed
71
72
73
74
        c = self.hpTasks + self.lpTasks
        for g in self.grasps.values():
            c += g

75
76
77
        return c.topics

    def plugTopics (self, rosexport):
Joseph Mirabel's avatar
Joseph Mirabel committed
78
        self.rosexport = rosexport
79
80
81
        topics = self.topics()

        for n, t in topics.items():
Joseph Mirabel's avatar
Joseph Mirabel committed
82
83
84
85
86
87
88
            if t.has_key('handler'):
                topic = _handlers[t['handler']] (n, t)
            else:
                topic = t["topic"]
            rosexport.add (t["type"], n, topic)
            for s in t['signalGetters']:
                plug (rosexport.signal(n), s())
89
            print topic, "plugged to", n, ', ', len(t['signalGetters']), 'times'
Joseph Mirabel's avatar
Joseph Mirabel committed
90

91
92
    def isSotConsistentWithCurrent(self, transitionName, thr = 1e-3):
        if self.currentSot is None or transitionName == self.currentSot:
93
            return True
Joseph Mirabel's avatar
Joseph Mirabel committed
94
        csot = self.sots[self.currentSot]
95
        nsot = self.sots[transitionName]
Joseph Mirabel's avatar
Joseph Mirabel committed
96
        t = self.sotrobot.device.control.time
97
        csot.control.recompute(t)
Joseph Mirabel's avatar
Joseph Mirabel committed
98
99
100
101
102
        nsot.control.recompute(t)
        from numpy import array, linalg
        error = array(nsot.control.value) - array(csot.control.value)
        n = linalg.norm(error)
        if n > thr:
Joseph Mirabel's avatar
Joseph Mirabel committed
103
104
            print "Control not consistent:", linalg.norm(error)
            print error
Joseph Mirabel's avatar
Joseph Mirabel committed
105
106
107
            return False
        return True

Joseph Mirabel's avatar
Joseph Mirabel committed
108
    def clearQueues(self):
Joseph Mirabel's avatar
Joseph Mirabel committed
109
110
111
        exec ("tmp = " + self.rosexport.list())
        for s in tmp:
            self.rosexport.clearQueue(s)
Joseph Mirabel's avatar
Joseph Mirabel committed
112

113
    def readQueue(self, read):
114
115
116
117
118
119
120
121
        if read < 0:
            print "ReadQueue argument should be >= 0"
            return
        t = self.sotrobot.device.control.time
        self.rosexport.readQueue (t + read)

    def stopReadingQueue(self):
        self.rosexport.readQueue (-1)
122

123
124
    def plugSot(self, transitionName, check = False):
        if check and not self.isSotConsistentWithCurrent (transitionName):
Joseph Mirabel's avatar
Joseph Mirabel committed
125
            # raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id))
126
127
            print("Sot {0} not consistent with sot {1}".format(self.currentSot, transitionName))
        if transitionName == "":
128
            # TODO : Explanation and linked TODO in the function makeInitialSot
Alexis Nicolin's avatar
Alexis Nicolin committed
129
130
131
132
            if self.sotrobot.dynamic.position.value[0] > -0.5:
                self.keep_posture._signalPositionRef().value = tuple([-0.74, 0.0, 1.0, 0.0, 0.0, 0.0] + list(self.sotrobot.dynamic.position.value)[6:])
            else:
                self.keep_posture._signalPositionRef().value = self.sotrobot.dynamic.position.value
133
        sot = self.sots[transitionName]
Joseph Mirabel's avatar
Joseph Mirabel committed
134
        # Start reading queues
Alexis Nicolin's avatar
Alexis Nicolin committed
135
        self.readQueue(10)
Joseph Mirabel's avatar
Joseph Mirabel committed
136
        plug(sot.control, self.sotrobot.device.control)
137
        print "Current sot:", transitionName
Joseph Mirabel's avatar
Joseph Mirabel committed
138
        print sot.display()
139
        self.currentSot = transitionName
Joseph Mirabel's avatar
Joseph Mirabel committed
140

141
142
143
144
    def runPreAction(self, transitionName):
        if self.preActions.has_key(transitionName):
            sot = self.preActions[transitionName]
            print "Running pre action", transitionName
145
146
147
148
149
            print sot.display()
            t = self.sotrobot.device.control.time
            sot.control.recompute(t-1)
            plug(sot.control, self.sotrobot.device.control)
            return
150
        print "No pre action", transitionName
151

152
    def runPostAction(self, targetStateName):
153
154
        if self.postActions.has_key(self.currentSot):
            d = self.postActions[self.currentSot]
155
156
157
            if d.has_key(targetStateName):
                sot = d[targetStateName]
                print "Running post action", self.currentSot, targetStateName
158
159
160
161
162
                print sot.display()
                t = self.sotrobot.device.control.time
                sot.control.recompute(t-1)
                plug(sot.control, self.sotrobot.device.control)
                return
163
        print "No post action", self.currentSot, targetStateName
164

Joseph Mirabel's avatar
Joseph Mirabel committed
165
    def getJointList (self, prefix = ""):
Joseph Mirabel's avatar
Joseph Mirabel committed
166
        return [ prefix + n for n in self.sotrobot.dynamic.model.names[1:] ]
Joseph Mirabel's avatar
Joseph Mirabel committed
167

Joseph Mirabel's avatar
Joseph Mirabel committed
168
169
170
171
def _handleHppJoint (n, t):
    type = t["type"]
    if t["velocity"]: topic = "velocity/op_frame"
    else:             topic = "op_frame"
172
173
174
175
176
177
178
179
180
181
    return "/hpp/target/" + topic + '/' + t['hppjoint']

def _handleHppCom (n, t):
    type = t["type"]
    if t["velocity"]: topic = "velocity/com"
    else:             topic = "com"
    if t['hppcom'] == "":
        return "/hpp/target/" + topic
    else:
        return "/hpp/target/" + topic + '/' + t['hppcom']
Joseph Mirabel's avatar
Joseph Mirabel committed
182
183
184

_handlers = {
        "hppjoint": _handleHppJoint,
185
        "hppcom": _handleHppCom,
Joseph Mirabel's avatar
Joseph Mirabel committed
186
        }