supervisor.py 7.5 KB
 Joseph Mirabel committed Mar 28, 2018 1 ``````from tools import Manifold, Posture `````` Joseph Mirabel committed Oct 19, 2017 2 3 4 ``````from dynamic_graph.sot.core import SOT from dynamic_graph import plug `````` Joseph Mirabel committed Nov 08, 2017 5 6 7 8 9 ``````def _hpTasks (sotrobot): return Manifold() def _lpTasks (sotrobot): return Posture ("posture", sotrobot) `````` Joseph Mirabel committed Oct 19, 2017 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 committed Nov 15, 2017 20 `````` def __init__ (self, sotrobot, lpTasks = None, hpTasks = None): `````` Joseph Mirabel committed Oct 19, 2017 21 `````` self.sotrobot = sotrobot `````` Joseph Mirabel committed Nov 15, 2017 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 committed Nov 13, 2017 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 committed Nov 13, 2017 35 `````` self.norm_comparision.sin2.value = 1e-2 `````` Joseph Mirabel committed Nov 13, 2017 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 committed Nov 15, 2017 48 `````` def makeInitialSot (self): `````` Joseph Mirabel committed Nov 08, 2017 49 50 51 `````` # Create the initial sot (keep) sot = SOT ('sot_keep') sot.setSize(self.sotrobot.dynamic.getDimension()) `````` Joseph Mirabel committed Nov 13, 2017 52 `````` self.keep_posture = Posture ("posture_keep", self.sotrobot) `````` Joseph Mirabel committed Nov 16, 2017 53 `````` self.keep_posture.tp.setWithDerivative (False) `````` Alexis Nicolin committed Mar 09, 2018 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 committed Mar 09, 2018 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:]) `````` Alexis Nicolin committed Mar 09, 2018 66 `````` `````` Joseph Mirabel committed Nov 13, 2017 67 `````` self.keep_posture.pushTo(sot) `````` Joseph Mirabel committed Nov 08, 2017 68 69 70 `````` self.sots[-1] = sot def topics (self): `````` Joseph Mirabel committed Oct 19, 2017 71 72 73 74 `````` c = self.hpTasks + self.lpTasks for g in self.grasps.values(): c += g `````` Joseph Mirabel committed Nov 08, 2017 75 76 77 `````` return c.topics def plugTopics (self, rosexport): `````` Joseph Mirabel committed Nov 20, 2017 78 `````` self.rosexport = rosexport `````` Joseph Mirabel committed Nov 08, 2017 79 80 81 `````` topics = self.topics() for n, t in topics.items(): `````` Joseph Mirabel committed Oct 19, 2017 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()) `````` Joseph Mirabel committed Nov 08, 2017 89 `````` print topic, "plugged to", n, ', ', len(t['signalGetters']), 'times' `````` Joseph Mirabel committed Oct 19, 2017 90 `````` `````` Joseph Mirabel committed Mar 28, 2018 91 92 `````` def isSotConsistentWithCurrent(self, transitionName, thr = 1e-3): if self.currentSot is None or transitionName == self.currentSot: `````` Joseph Mirabel committed Nov 08, 2017 93 `````` return True `````` Joseph Mirabel committed Oct 19, 2017 94 `````` csot = self.sots[self.currentSot] `````` Joseph Mirabel committed Mar 28, 2018 95 `````` nsot = self.sots[transitionName] `````` Joseph Mirabel committed Nov 20, 2017 96 `````` t = self.sotrobot.device.control.time `````` Joseph Mirabel committed Nov 08, 2017 97 `````` csot.control.recompute(t) `````` Joseph Mirabel committed Oct 19, 2017 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 committed Nov 15, 2017 103 104 `````` print "Control not consistent:", linalg.norm(error) print error `````` Joseph Mirabel committed Oct 19, 2017 105 106 107 `````` return False return True `````` Joseph Mirabel committed Nov 20, 2017 108 `````` def clearQueues(self): `````` Joseph Mirabel committed Nov 20, 2017 109 110 111 `````` exec ("tmp = " + self.rosexport.list()) for s in tmp: self.rosexport.clearQueue(s) `````` Joseph Mirabel committed Nov 20, 2017 112 `````` `````` Joseph Mirabel committed Nov 29, 2017 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) `````` Joseph Mirabel committed Nov 29, 2017 122 `````` `````` Joseph Mirabel committed Mar 28, 2018 123 124 `````` def plugSot(self, transitionName, check = False): if check and not self.isSotConsistentWithCurrent (transitionName): `````` Joseph Mirabel committed Nov 13, 2017 125 `````` # raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id)) `````` Joseph Mirabel committed Mar 28, 2018 126 127 `````` print("Sot {0} not consistent with sot {1}".format(self.currentSot, transitionName)) if transitionName == "": `````` Alexis Nicolin committed Mar 09, 2018 128 `````` # TODO : Explanation and linked TODO in the function makeInitialSot `````` Alexis Nicolin committed Mar 09, 2018 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 `````` Joseph Mirabel committed Mar 28, 2018 133 `````` sot = self.sots[transitionName] `````` Joseph Mirabel committed Dec 21, 2017 134 `````` # Start reading queues `````` Alexis Nicolin committed Mar 09, 2018 135 `````` self.readQueue(10) `````` Joseph Mirabel committed Nov 20, 2017 136 `````` plug(sot.control, self.sotrobot.device.control) `````` Joseph Mirabel committed Mar 28, 2018 137 `````` print "Current sot:", transitionName `````` Joseph Mirabel committed Nov 16, 2017 138 `````` print sot.display() `````` Joseph Mirabel committed Mar 28, 2018 139 `````` self.currentSot = transitionName `````` Joseph Mirabel committed Oct 19, 2017 140 `````` `````` Joseph Mirabel committed Mar 28, 2018 141 142 143 144 `````` def runPreAction(self, transitionName): if self.preActions.has_key(transitionName): sot = self.preActions[transitionName] print "Running pre action", transitionName `````` Joseph Mirabel committed Nov 20, 2017 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 `````` Joseph Mirabel committed Mar 28, 2018 150 `````` print "No pre action", transitionName `````` Joseph Mirabel committed Nov 20, 2017 151 `````` `````` Joseph Mirabel committed Mar 28, 2018 152 `````` def runPostAction(self, targetStateName): `````` Joseph Mirabel committed Nov 20, 2017 153 154 `````` if self.postActions.has_key(self.currentSot): d = self.postActions[self.currentSot] `````` Joseph Mirabel committed Mar 28, 2018 155 156 157 `````` if d.has_key(targetStateName): sot = d[targetStateName] print "Running post action", self.currentSot, targetStateName `````` Joseph Mirabel committed Nov 20, 2017 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 `````` Joseph Mirabel committed Mar 28, 2018 163 `````` print "No post action", self.currentSot, targetStateName `````` Joseph Mirabel committed Nov 20, 2017 164 `````` `````` Joseph Mirabel committed Nov 13, 2017 165 `````` def getJointList (self, prefix = ""): `````` Joseph Mirabel committed Nov 28, 2017 166 `````` return [ prefix + n for n in self.sotrobot.dynamic.model.names[1:] ] `````` Joseph Mirabel committed Nov 13, 2017 167 `````` `````` Joseph Mirabel committed Oct 19, 2017 168 169 170 171 ``````def _handleHppJoint (n, t): type = t["type"] if t["velocity"]: topic = "velocity/op_frame" else: topic = "op_frame" `````` Joseph Mirabel committed Nov 08, 2017 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 committed Oct 19, 2017 182 183 184 `````` _handlers = { "hppjoint": _handleHppJoint, `````` Joseph Mirabel committed Nov 08, 2017 185 `````` "hppcom": _handleHppCom, `````` Joseph Mirabel committed Oct 19, 2017 186 `` }``