Commit 0a9c144f authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel

Add ability to add timers for signal control of the SoTs

parent 2ee79510
......@@ -115,13 +115,38 @@ class Factory(GraphFactoryAbstract):
self.postActions = dict()
self.preActions = dict()
self.timers = {}
self.supervisor = supervisor
self.addTimerToSotControl = False
def _newSoT (self, name):
sot = SOT (name)
sot.setSize(self.sotrobot.dynamic.getDimension())
sot.damping.value = 0.001
if self.addTimerToSotControl:
from .tools import insertTimerOnOutput
self.timers[name] = insertTimerOnOutput (sot.control, "vector")
self.SoTtracer.add (self.timers[name].name + ".timer", str(len(self.timerIds)) + ".timer")
self.timerIds.append(name)
return sot
def addAffordance (self, aff):
assert isinstance(aff, Affordance)
self.affordances [(aff.gripper, aff.handle)] = aff
def generate (self):
if self.addTimerToSotControl:
# init tracer
from dynamic_graph.tracer_real_time import TracerRealTime
self.SoTtracer = TracerRealTime ("tracer_of_timers")
self.timerIds = []
self.SoTtracer.setBufferSize (10 * 1048576) # 10 Mo
self.SoTtracer.open ("/tmp", "sot-control-trace", ".txt")
self.sotrobot.device.after.addSignal("tracer_of_timers.triger")
self.supervisor.SoTtracer = self.SoTtracer
self.supervisor.SoTtimerIds = self.timerIds
super(Factory, self).generate ()
self.supervisor.sots = self.sots
......@@ -130,6 +155,7 @@ class Factory(GraphFactoryAbstract):
self.supervisor.lpTasks = self.lpTasks
self.supervisor.postActions = self.postActions
self.supervisor.preActions = self.preActions
self.supervisor.SoTtimers = self.timers
def setupFrames (self, srdfGrippers, srdfHandles, sotrobot):
self.sotrobot = sotrobot
......@@ -146,9 +172,7 @@ class Factory(GraphFactoryAbstract):
def makeLoopTransition (self, state):
n = self._loopTransitionName(state.grasps)
sot = SOT ('sot_' + n)
sot.setSize(self.sotrobot.dynamic.getDimension())
sot.damping.value = 0.001
sot = self._newSoT ('sot_'+n)
self.hpTasks.pushTo(sot)
state.manifold.pushTo(sot)
......@@ -188,9 +212,7 @@ class Factory(GraphFactoryAbstract):
"{0}_{2}{1}".format(names[1], i, i+1))
for n in ns:
s = SOT ('sot_' + n)
s.setSize(self.sotrobot.dynamic.getDimension())
s.damping.value = 0.001
s = self._newSoT('sot_'+n)
self.hpTasks.pushTo(s)
#if pregrasp and i == 1:
......@@ -206,9 +228,7 @@ class Factory(GraphFactoryAbstract):
key = sots[2*(M-1)]
states = ( st.name, names[0] + "_intersec")
sot = SOT ("postAction_" + key)
sot.setSize(self.sotrobot.dynamic.getDimension())
sot.damping.value = 0.001
sot = self._newSoT ("postAction_" + key)
self.hpTasks.pushTo (sot)
# self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
st.manifold.pushTo (sot)
......@@ -224,9 +244,7 @@ class Factory(GraphFactoryAbstract):
key = sots[2*(M-1) + 1]
states = ( st.name, names[0] + "_intersec")
sot = SOT ("preAction_" + key)
sot.setSize(self.sotrobot.dynamic.getDimension())
sot.damping.value = 0.001
sot = self._newSoT ("preAction_" + key)
self.hpTasks.pushTo (sot)
# self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'gripper_close').pushTo (sot)
sf.manifold.pushTo (sot)
......
......@@ -133,9 +133,10 @@ class Supervisor(object):
# TODO : Explanation and linked TODO in the function makeInitialSot
self.keep_posture._signalPositionRef().value = self.sotrobot.dynamic.position.value
sot = self.sots[transitionName]
control = self._getControlSignal(sot)
# Start reading queues
self.readQueue(10)
plug(sot.control, self.sotrobot.device.control)
plug(control, self.sotrobot.device.control)
print("Current sot:", transitionName, "\n", sot.display())
self.currentSot = transitionName
......@@ -166,6 +167,12 @@ class Supervisor(object):
def getJointList (self, prefix = ""):
return [ prefix + n for n in self.sotrobot.dynamic.model.names[1:] ]
def _getControlSignal (self, sot):
if self.SoTtimers.has_key (sot.name):
return self.SoTtimers[sot.name].sout
else:
return sot.control
def _handleHppJoint (n, t):
type = t["type"]
if t["velocity"]: topic = "velocity/op_frame"
......
......@@ -2,6 +2,46 @@ from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineC
from dynamic_graph.sot.core.meta_tasks_kine_relative import MetaTaskKine6dRel
from dynamic_graph.sot.core.meta_tasks import setGain
from dynamic_graph.sot.core import FeaturePosture
from dynamic_graph import plug
def getTimerType (type):
from dynamic_graph.sot.core.timer import TimerDouble, TimerMatrix, TimerMatrixHomo, TimerVector
if type == "double":
return TimerDouble
elif type == "matrix":
return TimerMatrix
elif type == "matrixhomo":
return TimerMatrixHomo
elif type == "vector":
return TimerVector
else:
raise ValueError ("Unknown type of timer.")
def insertTimerOnOutput (signal, type):
"""
Plug the signal sout of the return entity instead of `signal` to
input signal to enable the timer.
- param signal an output signal.
- return an Timer entity.
"""
Timer = getTimerType (type)
timer = Timer ("timer_of_" + signal.name)
plug(signal, timer.sin)
return timer
def insertTimer (signal, type):
"""
- param signal a plugged input signal.
- return an Timer entity.
"""
assert signal.isPlugged()
from dynamic_graph.sot.core.timer import TimerDouble, TimerMatrix, TimerMatrixHomo, TimerVector
Timer = getTimerType (type)
timer = Timer ("timer_of_" + signal.name)
other = signal.getPlugged()
plug(other, timer.sin)
plug(timer.sout, signal)
return timer
def parseHppName (hppjointname):
if hppjointname == "universe": return "", "universe"
......@@ -53,7 +93,6 @@ class Posture(Manifold):
super(Posture, self).__init__()
from dynamic_graph.sot.core import Task, FeatureGeneric, GainAdaptive
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph import plug
from numpy import identity
n = Posture.sep + name
......@@ -133,7 +172,6 @@ class Grasp (Manifold):
self.otherHandle = otherGraspOnObject.handle
def makeTasks(self, sotrobot):
from dynamic_graph import plug
if self.relative:
self.graspTask = MetaTaskKine6dRel (
Grasp.sep + self.gripper.name + Grasp.sep + self.otherGripper.name,
......@@ -177,7 +215,6 @@ class Grasp (Manifold):
class EEPosture (Manifold):
def __init__ (self, sotrobot, gripper, position):
from dynamic_graph.sot.core import Task, GainAdaptive
from dynamic_graph import plug
super(EEPosture, self).__init__()
self.gripper = gripper
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment