Commit 438420df authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub

Merge pull request #6 from jmirabel/master

Fix some multithreading issues
parents 6ee9bbd2 35742868
......@@ -38,7 +38,8 @@ namespace dynamicgraph {
Event (const std::string& name) :
Entity (name),
checkSOUT ("Event("+name+")::output(bool)::check"),
conditionSIN(NULL,"Event("+name+")::input(bool)::condition")
conditionSIN(NULL,"Event("+name+")::input(bool)::condition"),
lastVal_ (2) // lastVal_ should be different true and false.
{
checkSOUT.setFunction
(boost::bind (&Event::check, this, _1, _2));
......
......@@ -5,7 +5,6 @@
# include <boost/shared_ptr.hpp>
# include <boost/thread/mutex.hpp>
# include <boost/thread/condition_variable.hpp>
# include <dynamic-graph/entity.h>
# include <dynamic-graph/signal-time-dependent.h>
......@@ -106,7 +105,6 @@ namespace dynamicgraph
// synchronize with method reader
rmutex.lock();
frontIdx = backIdx = 0;
fullCondition.notify_all ();
rmutex.unlock();
wmutex.unlock();
}
......@@ -136,7 +134,6 @@ namespace dynamicgraph
size_type backIdx;
buffer_t buffer;
boost::mutex wmutex, rmutex;
boost::condition_variable fullCondition;
T last;
bool init;
......
......@@ -67,10 +67,10 @@ namespace dynamicgraph
{
// synchronize with method clear
boost::mutex::scoped_lock lock(wmutex);
boost::mutex dummy;
boost::unique_lock<boost::mutex> lock_dummy (dummy);
while (full()) {
fullCondition.wait (lock_dummy);
if (full()) {
rmutex.lock();
frontIdx = (frontIdx + 1) % N;
rmutex.unlock();
}
converter (buffer[backIdx], data);
// No need to synchronize with reader here because:
......@@ -99,7 +99,6 @@ namespace dynamicgraph
data = buffer[frontIdx];
frontIdx = (frontIdx + 1) % N;
last = data;
fullCondition.notify_all();
}
}
return data;
......
......@@ -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)
......
......@@ -27,7 +27,7 @@ class Supervisor(object):
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
from dynamic_graph.ros import RosPublish
self.norm = Norm_of_vector ("control_norm")
plug (self.sotrobot.device.control, self.norm.sin)
......@@ -40,11 +40,11 @@ class Supervisor(object):
# 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")
self.ros_publish = RosPublish ('ros_publish_control_norm')
self.ros_publish.add ('double', 'event_control_norm', '/sot_hpp/control_norm_changed')
plug (self.norm.sout, self.ros_publish.event_control_norm)
# plug (self.norm_event.trigger, self.ros_publish.trigger)
self.norm_event.addSignal ("ros_publish_control_norm.trigger")
def makeInitialSot (self):
# Create the initial sot (keep)
......@@ -100,6 +100,8 @@ class Supervisor(object):
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
......@@ -133,9 +135,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
......@@ -145,7 +148,9 @@ class Supervisor(object):
print("Running pre action", transitionName,
"\n", sot.display())
t = self.sotrobot.device.control.time
sot.control.recompute(t-1)
# This is not safe since it would be run concurrently with the
# real time thread.
# sot.control.recompute(t-1)
plug(sot.control, self.sotrobot.device.control)
return
print ("No pre action", transitionName)
......@@ -158,7 +163,9 @@ class Supervisor(object):
print( "Running post action", self.currentSot, targetStateName,
"\n", sot.display())
t = self.sotrobot.device.control.time
sot.control.recompute(t-1)
# This is not safe since it would be run concurrently with the
# real time thread.
# sot.control.recompute(t-1)
plug(sot.control, self.sotrobot.device.control)
return
print ("No post action", self.currentSot, targetStateName)
......@@ -166,6 +173,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,27 +93,30 @@ 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
self.tp = Task ('task' + n)
self.tp.dyn = sotrobot.dynamic
self.tp.feature = FeatureGeneric('feature_'+n)
self.tp.featureDes = FeatureGeneric('feature_des_'+n)
self.tp.gain = GainAdaptive("gain_"+n)
self.tp.feature = FeaturePosture('feature_'+n)
q = list(sotrobot.dynamic.position.value)
self.tp.feature.state.value = q
self.tp.feature.posture.value = q
robotDim = sotrobot.dynamic.getDimension()
self.tp.feature.jacobianIN.value = matrixToTuple( identity(robotDim) )
self.tp.feature.setReference(self.tp.featureDes.name)
for i in range(6, robotDim):
self.tp.feature.selectDof (i, True)
self.tp.gain = GainAdaptive("gain_"+n)
self.tp.add(self.tp.feature.name)
# Connects the dynamics to the current feature of the posture task
plug(sotrobot.dynamic.position, self.tp.feature.errorIN)
plug(sotrobot.dynamic.position, self.tp.feature.state)
self.tp.setWithDerivative (True)
# Set the gain of the posture task
setGain(self.tp.gain,10)
setGain(self.tp.gain,(4.9,0.9,0.01,0.9))
plug(self.tp.gain.gain, self.tp.controlGain)
plug(self.tp.error, self.tp.gain.error)
self.tasks = [ self.tp ]
......@@ -88,8 +131,8 @@ class Posture(Manifold):
"signalGetters": [ self._signalVelocityRef ] },
}
def _signalPositionRef (self): return self.tp.featureDes.errorIN
def _signalVelocityRef (self): return self.tp.featureDes.errordotIN
def _signalPositionRef (self): return self.tp.feature.posture
def _signalVelocityRef (self): return self.tp.feature.postureDot
## Represents a gripper or a handle
class OpFrame(object):
......@@ -133,7 +176,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 +219,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