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 """ ## # \param lpTasks list of low priority tasks. If None, a Posture task will be used. # \param hpTasks list of high priority tasks (like balance) def __init__(self, sotrobot, lpTasks=None, hpTasks=None): self.sotrobot = sotrobot 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 from dynamic_graph.sot.core.switch import SwitchVector self.sot_switch = SwitchVector("sot_supervisor_switch") plug(self.sot_switch.sout, self.sotrobot.device.control) from agimus_sot.events import Events self.done_events = Events("done", sotrobot) self.error_events = Events("error", sotrobot) self.done_events.setupNormOfControl(sotrobot.device.control, 1e-2) self.done_events.setupTime( ) # For signal self. done_events.timeEllapsedSignal self.error_events.setupTime( ) # For signal self.error_events.timeEllapsedSignal def makeInitialSot(self): # Create the initial sot (keep) from .solver import Solver sot = Solver('sot_keep', self.sotrobot.dynamic.getDimension()) self.keep_posture = Posture("posture_keep", self.sotrobot) self.keep_posture._task.setWithDerivative(False) self.keep_posture._signalPositionRef( ).value = self.sotrobot.dynamic.position.value self.keep_posture.pushTo(sot) sot.doneSignal = self.done_events.controlNormSignal sot.errorSignal = False self.addSolver("", sot) ## Set the robot base pose in the world. # \param basePose a list: [x,y,z,r,p,y] or [x,y,z,qx,qy,qz,qw] # \return success True in case of success def setBasePose(self, basePose): if len(basePose) == 7: # Currently, this case never happens from dynamic_graph.sot.tools.quaternion import Quaternion from numpy.linalg import norm q = Quaternion(basePose[6], basePose[3], basePose[4], basePose[5]) if abs(norm(q.array) - 1.) > 1e-2: return False, "Quaternion is not normalized" basePose = basePose[:3] + q.toRPY().tolist() if self.currentSot == "" or len(basePose) != 6: # We are using the SOT to keep the current posture. # The 6 first DoF are not used by the task so we can change them safely. self.sotrobot.device.set( tuple(basePose + list(self.sotrobot.device.state.value[6:]))) self.keep_posture._signalPositionRef( ).value = self.sotrobot.device.state.value return True else: return False ## \name SoT managements ## \{ def addPreAction(self, name, preActionSolver): self.preActions[name] = preActionSolver self._addSignalToSotSwitch(preActionSolver) def addSolver(self, name, solver): self.sots[name] = solver self._addSignalToSotSwitch(solver) def duplicateSolver(self, existingSolver, newSolver): self.sots[newSolver] = self.sots[existingSolver] def addPostActions(self, name, postActionSolvers): self.postActions[name] = postActionSolvers for targetState, pa_sot in postActionSolvers.iteritems(): self._addSignalToSotSwitch(pa_sot) ## This is for internal purpose def _addSignalToSotSwitch(self, solver): n = self.sot_switch.getSignalNumber() self.sot_switch.setSignalNumber(n + 1) self.sots_indexes[solver.name] = n plug(solver.control, self.sot_switch.signal("sin" + str(n))) def _plug(e, events, n, name): assert events.getSignalNumber() == n, "Wrong number of events." events.setSignalNumber(n + 1) events.setConditionString(n, name) if isinstance(e, (bool, int)): events.conditionSignal(n).value = int(e) else: plug(e, events.conditionSignal(n)) _plug(solver.doneSignal, self.done_events, n, solver.name) _plug(solver.errorSignal, self.error_events, n, solver.name) def _selectSolver(self, solver): n = self.sots_indexes[solver.name] self.sot_switch.selection.value = n self.done_events.setSelectedSignal(n) self.error_events.setSelectedSignal(n) ## \} def topics(self): c = self.hpTasks + self.lpTasks for g in self.grasps.values(): c += g for p in self.placements.values(): c += p return c.topics def plugTopicsToRos(self): from dynamic_graph.ros.ros_queued_subscribe import RosQueuedSubscribe self.rosSubscribe = RosQueuedSubscribe('ros_queued_subscribe') from dynamic_graph.ros.ros_tf_listener import RosTfListener self.rosTf = RosTfListener('ros_tf_listener') topics = self.topics() for name, topic_info in topics.items(): topic_handler = _handlers[topic_info.get("handler", "default")] topic_handler(name, topic_info, self.rosSubscribe, self.rosTf) def printQueueSize(self): exec("tmp = " + self.rosSubscribe.list()) for l in tmp: print(l, self.rosSubscribe.queueSize(l)) ## Check consistency between two SoTs. # # This is not used anymore because it must be synchronized with the real-time thread. # \todo Re-enable consistency check between two SoTs. def isSotConsistentWithCurrent(self, transitionName, thr=1e-3): if self.currentSot is None or transitionName == self.currentSot: return True csot = self.sots[self.currentSot] nsot = self.sots[transitionName] t = self.sotrobot.device.control.time # This is not safe since it would be run concurrently with the # real time thread. csot.control.recompute(t) 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: print("Control not consistent:", linalg.norm(error), '\n', error) return False return True def clearQueues(self): self.rosSubscribe.readQueue(-1) exec("tmp = " + self.rosSubscribe.list()) for s in tmp: print('{} queue size: {}'.format(s, self.rosSubscribe.queueSize(s))) self.rosSubscribe.clearQueue(s) ## Wait for the queue to be of a given size. # \param minQueueSize (integer) waits to the queue size of rosSubscribe # to be greater or equal to \c minQueueSize # \param timeout time in seconds after which to return a failure. # \return True on success, False on timeout. def waitForQueue(self, minQueueSize, timeout): ts = self.sotrobot.device.getTimeStep() to = int(timeout / self.sotrobot.device.getTimeStep()) from time import sleep start_it = self.sotrobot.device.control.time exec("queues = " + self.rosSubscribe.list()) for queue in queues: while self.rosSubscribe.queueSize(queue) < minQueueSize: if self.sotrobot.device.control.time > start_it + to: return False, "Queue {} has received {} points.".format( queue, self.rosSubscribe.queueSize(queue)) sleep(ts) return True, "" ## Start reading values received by the RosQueuedSubscribe entity. # \param delay (integer) how many periods to wait before reading. # It allows to give some delay to network connection. # \param minQueueSize (integer) waits to the queue size of rosSubscribe # to be greater or equal to \p minQueueSize # \param duration expected duration (in seconds) of the queue. # \param timeout time in seconds after which to return a failure. # \return success, time boolean, SoT time at which reading starts (invalid if success is False) # # \warning If \p minQueueSize is greater than the number of values to # be received by rosSubscribe, this function does an infinite loop. def readQueue(self, delay, minQueueSize, duration, timeout): from time import sleep print("Current solver {0}".format(self.currentSot)) if delay < 0: print("Delay argument should be >= 0") return False, -1 minSizeReached, msg = self.waitForQueue(minQueueSize, timeout) if not minSizeReached: return False, -1 durationStep = int(duration / self.sotrobot.device.getTimeStep()) t = self.sotrobot.device.control.time + delay self.rosSubscribe.readQueue(t) self.done_events.setFutureTime(t + durationStep) self.error_events.setFutureTime(t + durationStep) return True, t def stopReadingQueue(self): self.rosSubscribe.readQueue(-1) # \return success, time boolean, SoT time at which reading starts (invalid if success is False) def plugSot(self, transitionName, check=False): if check and not self.isSotConsistentWithCurrent(transitionName): # raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id)) print("Sot {0} not consistent with sot {1}".format( self.currentSot, transitionName)) if transitionName == "": self.keep_posture._signalPositionRef( ).value = self.sotrobot.dynamic.position.value solver = self.sots[transitionName] # No done events should be triggered before call # to readQueue. We expect it to happen with 1e6 milli-seconds # from now... devicetime = self.sotrobot.device.control.time self.done_events.setFutureTime(devicetime + 100000) self._selectSolver(solver) print("{0}: Current solver {1}\n{2}".format(devicetime, transitionName, solver.sot.display())) self.currentSot = transitionName if hasattr(self, 'ros_publish_state'): self.ros_publish_state.transition_name.value = transitionName return True, devicetime # \return success, time boolean, SoT time at which reading starts (invalid if success is False) def runPreAction(self, transitionName): if self.preActions.has_key(transitionName): solver = self.preActions[transitionName] t = self.sotrobot.device.control.time + 2 self.done_events.setFutureTime(t) self._selectSolver(solver) print("{0}: Running pre action {1}\n{2}".format( t, transitionName, solver.sot.display())) return True, t - 2 print("No pre action", transitionName) return False, -1 ## Execute a post-action # \return success, time boolean, SoT time at which reading starts (invalid if success is False) def runPostAction(self, targetStateName): if self.postActions.has_key(self.currentSot): d = self.postActions[self.currentSot] if d.has_key(targetStateName): solver = d[targetStateName] devicetime = self.sotrobot.device.control.time self.done_events.setFutureTime(devicetime + 2) self._selectSolver(solver) print("{0}: Running post action {1} --> {2}\n{3}".format( devicetime, self.currentSot, targetStateName, solver.sot.display())) return True, devicetime print("No post action {0} --> {1}".format(self.currentSot, targetStateName)) return False, -1 def getJointList(self, prefix=""): return [prefix + n for n in self.sotrobot.dynamic.model.names[1:]] def publishState(self, subsampling=40): if hasattr(self, "ros_publish_state"): return from dynamic_graph.ros import RosPublish self.ros_publish_state = RosPublish("ros_publish_state") self.ros_publish_state.add("vector", "state", "/agimus/sot/state") self.ros_publish_state.add("vector", "reference_state", "/agimus/sot/reference_state") self.ros_publish_state.add("string", "transition_name", "/agimus/sot/transition_name") self.ros_publish_state.transition_name.value = "" plug(self.sotrobot.device.state, self.ros_publish_state.state) plug(self.rosSubscribe.posture, self.ros_publish_state.reference_state) self.sotrobot.device.after.addDownsampledSignal( "ros_publish_state.trigger", subsampling)
class ControllerSwitch: def __init__ (self,name,controllers,threshold_up,threshold_down): """ Use controller 0 until the condition signal value becomes greater than threshold. Then use controller 1. Manually switch between controller using the latch. OFF means controller 0 and ON means controller 1. Currently support only two controllers. - controllers: output signal of a controller """ self.reverse = (threshold_up < threshold_down) # any(threshold_up < measured_torque) => torque control self._condition_up = CompareVector (name + "_condition_up") self._condition_up.setTrueIfAny(True) self._condition_up.sin1.value = threshold_up self._condition_up.sin2.value = threshold_up # all(measured_torque < threshold_down) => position control self._condition_down = CompareVector (name + "_condition_down") self._condition_down.setTrueIfAny(False) self._condition_down.sin1.value = threshold_down self._condition_down.sin2.value = threshold_down self._event_up = Event (name + "_event_up") self._event_down = Event (name + "_event_down") self._event_up .setOnlyUp(True); self._event_down .setOnlyUp(True); self._latch = Latch(name + "_latch") self._latch.turnOff() self._switch = SwitchVector (name + "_switch") self._switch.setSignalNumber(len(controllers)) plug(self._condition_up .sout, self._event_up .condition) plug(self._condition_down.sout, self._event_down.condition) plug(self._latch.out , self._switch.boolSelection) # This is necessary to initialize the event (the first recompute triggers the event...) self._event_up.check.recompute(0) self._event_down.check.recompute(0) self._event_up .addSignal (name + "_latch.turnOnSout") self._event_down.addSignal (name + "_latch.turnOffSout") for n, sig in enumerate(controllers): plug(sig, self.signalIn(n)) def setMeasurement (self,sig): if self.reverse: plug(sig, self._condition_up .sin1) plug(sig, self._condition_down.sin2) else: plug(sig, self._condition_up .sin2) plug(sig, self._condition_down.sin1) @property def thresholdUp (self): if self.reverse: return self._condition_up .sin2 else: return self._condition_up .sin1 @property def thresholdDown (self): if self.reverse: return self._condition_down.sin1 else: return self._condition_down.sin2 @property def signalOut (self): return self._switch.sout def signalIn (self, n): return self._switch.signal("sin" + str(n)) @property def conditionUp (self): return self._condition_up @property def conditionDown (self): return self._condition_down @property def eventUp (self): return self._event_up @property def eventDown (self): return self._event_down @property def latch (self): return self._latch @property def switch (self): return self._switch