Commit 0c259829 authored by Alexis Nicolin's avatar Alexis Nicolin

Added a grasp task to the sot

Allows two hands to maintain their relative positions while the first hand moves freely.
parent 10596ad2
......@@ -9,21 +9,46 @@ class TaskFactory(ConstraintFactoryAbstract):
def __init__ (self, graphfactory):
super(TaskFactory, self).__init__ (graphfactory)
def buildGrasp (self, g, h):
def buildGrasp (self, g, h, otherGrasp=None):
gf = self.graphfactory
if h is None:
return { 'gripper_open': EEPosture (gf.sotrobot, gf.gripperFrames [g], [0]) }
gripper_close = EEPosture (gf.sotrobot, gf.gripperFrames [g], [-1])
gripper_close = EEPosture (gf.sotrobot, gf.gripperFrames [g], [-0.2])
pregrasp = Grasp (gf.gripperFrames [g],
gf.handleFrames [h])
gf.handleFrames [h],
otherGrasp,
False)
pregrasp.makeTasks (gf.sotrobot)
grasp = Grasp (gf.gripperFrames [g],
gf.handleFrames [h])
gf.handleFrames [h],
otherGrasp,
True)
grasp.makeTasks (gf.sotrobot)
return { 'grasp': grasp,
'pregrasp': pregrasp,
'gripper_close': gripper_close }
## \name Accessors to the different elementary constraints
# \{
def getGrasp(self, gripper, handle, otherGrasp=None):
if isinstance(gripper, str): ig = self.graphfactory.grippers.index(gripper)
else: ig = gripper
if isinstance(handle, str): ih = self.graphfactory.handles.index(handle)
else: ih = handle
if otherGrasp is not None:
otherIg = self.graphfactory.grippers.index(otherGrasp.gripper.name)
otherIh = self.graphfactory.handles.index(otherGrasp.handle.name)
k = (ig, ih, otherIg, otherIh)
else:
k = (ig, ih)
if not self._grasp.has_key(k):
self._grasp[k] = self.buildGrasp(self.graphfactory.grippers[ig], None if ih is None else self.graphfactory.handles[ih], otherGrasp)
assert isinstance (self._grasp[k], dict)
return self._grasp[k]
def g (self, gripper, handle, what, otherGrasp=None):
return self.getGrasp(gripper, handle, otherGrasp)[what]
def buildPlacement (self, o):
# Nothing to do
......@@ -36,12 +61,14 @@ class Factory(GraphFactoryAbstract):
self.grasps = grasps
self.manifold = Manifold()
objectsAlreadyGrasped = {}
for ig, ih in idx_zip (grasps):
if ih is not None:
# Add task gripper_close
self.manifold += tasks.g (factory.grippers[ig], factory.handles[ih], 'gripper_close')
# TODO If an object is grasped by two grippers, then we should add a task
# of relative position of the two grippers.
otherGrasp = objectsAlreadyGrasped.get(factory.objectFromHandle[ih])
self.manifold += tasks.g (factory.grippers[ig], factory.handles[ih], 'grasp', otherGrasp)
objectsAlreadyGrasped[factory.objectFromHandle[ih]] = tasks.g (factory.grippers[ig], factory.handles[ih], 'grasp', otherGrasp)
else:
# Add task gripper_open
self.manifold += tasks.g (factory.grippers[ig], None, 'gripper_open')
......@@ -93,7 +120,7 @@ class Factory(GraphFactoryAbstract):
return Factory.State(self.tasks, grasps, self)
def makeLoopTransition (self, state):
n = self._loopTransitionName(state.grasps)
n = self._loopTransitionName(state.grasps, True)
sot = SOT ('sot_' + n)
sot.setSize(self.sotrobot.dynamic.getDimension())
......@@ -108,13 +135,14 @@ class Factory(GraphFactoryAbstract):
st = stateTo
names = self._transitionNames(sf, st, ig)
# TODO Add the notion of pre/post actions
# to open and close the gripper
print "names: {}".format(names)
iobj = self.objectFromHandle [st.grasps[ig]]
obj = self.objects[iobj]
noPlace = self._isObjectGrasped (sf.grasps, iobj)
print "iobj, obj, noPlace: {}, {}, {}".format(iobj, obj, noPlace)
# The different cases:
pregrasp = True
intersec = not noPlace
......@@ -138,9 +166,9 @@ class Factory(GraphFactoryAbstract):
s.setSize(self.sotrobot.dynamic.getDimension())
self.hpTasks.pushTo(s)
if pregrasp and i == 1:
#if pregrasp and i == 1:
# Add pregrasp task
self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'pregrasp').pushTo (s)
#self.tasks.g (self.grippers[ig], self.handles[st.grasps[ig]], 'pregrasp').pushTo (s)
if i < M: sf.manifold.pushTo(s)
else: st.manifold.pushTo(s)
......
......@@ -73,6 +73,10 @@ class Supervisor(object):
self.preActions = dict()
self.transitions = transitions
print "Transitions:\n"
print transitions
print "\n"
for t in transitions:
# Create SOT solver
# sot = SOT ('sot_' + str(t['id']) + '-' + t['name'])
......@@ -120,7 +124,7 @@ class Supervisor(object):
sot.setSize(self.sotrobot.dynamic.getDimension())
self.keep_posture = Posture ("posture_keep", self.sotrobot)
self.keep_posture.tp.setWithDerivative (False)
self.keep_posture._signalPositionRef().value = self.sotrobot.dynamic.position.value
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:])
self.keep_posture.pushTo(sot)
self.sots[-1] = sot
......@@ -236,10 +240,13 @@ class Supervisor(object):
# raise Exception ("Sot %d not consistent with sot %d" % (self.currentSot, id))
print "Sot %d not consistent with sot %d" % (self.currentSot, id)
if id == -1:
self.keep_posture._signalPositionRef().value = self.sotrobot.dynamic.position.value
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
sot = self.sots[id]
# Start reading queues
self.readQueue(True)
self.readQueue(10)
plug(sot.control, self.sotrobot.device.control)
print "Current sot:", id
print sot.display()
......
This diff is collapsed.
......@@ -45,9 +45,11 @@ namespace dynamicgraph
callback_t callback = boost::bind
(&BindedSignal_t::template writer<ros_const_ptr_t>, bs, _1);
// Keep 50 messages in queue, but only 20 are sent every 100ms
// -> No message should be lost because of a full buffer
bs->subscriber =
boost::make_shared<ros::Subscriber>
(rosSubscribe.nh ().subscribe (topic, 50, callback)); // Keep 50 messages in queue, but only 20 are sent every 100ms
(rosSubscribe.nh ().subscribe (topic, 50, callback));
RosQueuedSubscribe::bindedSignal_t bindedSignal (bs);
rosSubscribe.bindedSignal ()[signal] = bindedSignal;
......@@ -82,10 +84,10 @@ namespace dynamicgraph
else {
data = queue.front();
queue.pop();
if (queue.size() < 10) {
std::cout << signal->getName() << ": queue size is " << queue.size()
<< " at time " << time << std::endl;
}
//if (queue.size() < 10) {
// std::cout << signal->getName() << ": queue size is " << queue.size()
// << " at time " << time << std::endl;
//}
last = data;
}
qmutex.unlock();
......
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