Unverified Commit 46b2e21e authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub

Merge pull request #2 from ksyy/master

Added a grasp task to the sot
parents 10596ad2 3e08271e
......@@ -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,19 @@ 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
# 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
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 +252,14 @@ 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
# TODO : Explanation and linked TODO in the function makeInitialSot
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()
......
......@@ -120,69 +120,69 @@ class OpFrame(object):
robotname, self.sotjoint = parseHppName (n)
class Grasp (Manifold):
def __init__ (self, gripper, handle, otherGraspOnObject = None):
def __init__ (self, gripper, handle, otherGraspOnObject = None, closeGripper = False):
super(Grasp, self).__init__()
self.gripper = gripper
self.handle = handle
self.otherGrasp = otherGraspOnObject
self.closeGripper = closeGripper
self.relative = self.otherGrasp is not None \
and self.otherGrasp.handle.hppjoint == self.handle.hppjoint
if self.relative:
self.topics = dict()
else:
self.topics = {
# self.gripper.name: {
self.gripper.hppjoint: {
"velocity": False,
"type": "matrixHomo",
"handler": "hppjoint",
"hppjoint": self.gripper.hppjoint,
"signalGetters": [ self._signalPositionRef ] },
# "vel_" + self.gripper.name: {
"vel_" + self.gripper.hppjoint: {
"velocity": True,
"type": "vector",
"handler": "hppjoint",
"hppjoint": self.gripper.hppjoint,
"signalGetters": [ self._signalVelocityRef ] },
}
self.otherGripper = otherGrasp.gripper
def makeTasks(self, sotrobot):
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from dynamic_graph.sot.core import Multiply_of_matrixHomo, OpPointModifier
from dynamic_graph.sot.core import Multiply_of_matrix, Multiply_of_matrixHomo, OpPointModifier
from dynamic_graph import plug
if self.relative:
# We define a MetaTaskKine6dRel
self.graspTask = MetaTaskKine6dRel (
Grasp.sep + self.gripper.name + Grasp.sep + self.handle.name +
'(rel_to_' + self.otherGrasp.gripper.name + ')',
sotrobot.dynamic,
self.gripper.sotjoint,
self.gripper.sotjoint,
self.otherGrasp.gripper.sotjoint,
self.otherGrasp.gripper.sotjoint)
M = transformToMatrix(self.gripper.sotpose * self.handle.sotpose.inverse())
self.graspTask.opmodif = matrixToTuple(M)
M = transformToMatrix(self.otherGrasp.handle.sotpose * self.otherGrasp.gripper.sotpose.inverse())
self.graspTask.opmodifBase = matrixToTuple(M)
else:
# We define a MetaTaskKine6d
self.graspTask = MetaTaskKine6d (
Grasp.sep + self.gripper.name + Grasp.sep + self.handle.name,
Grasp.sep + self.gripper.name + Grasp.sep + self.otherGrasp.gripper.name,
sotrobot.dynamic,
self.gripper.sotjoint,
self.gripper.sotjoint)
# TODO At the moment, the reference is the joint frame, not the gripper frame.
# M = transformToMatrix(self.gripper.sotpose)
# self.graspTask.opmodif = matrixToTuple(M)
# self.graspTask.feature.frame("desired")
self.graspTask.task.setWithDerivative (True)
setGain(self.graspTask.gain,(100,0.9,0.01,0.9))
self.graspTask.feature.frame("current")
self.tasks = [ self.graspTask.task ]
def _signalPositionRef (self): return self.graspTask.featureDes.position
def _signalVelocityRef (self): return self.graspTask.featureDes.velocity
# Creates the matrix transforming the goal gripper's position into the desired moving gripper's position:
# on the other handle of the object
#M = ((-1.0, 0.0, 0.0, 0.0), (0.0, 1.0, 0.0, 0.0), (0.0, 0.0, -1.0, -0.55), (0.0, 0.0, 0.0, 1.0))
M = transformToMatrix(self.otherGrasp.gripper.sotpose * self.otherGrasp.handle.sotpose.inverse() * self.handle.sotpose * self.gripper.sotpose.inverse())
self.graspTask.opmodif = matrixToTuple(M)
self.graspTask.feature.frame("current")
setGain(self.graspTask.gain,(4.9,0.9,0.01,0.9))
self.graspTask.task.setWithDerivative (False)
# Sets the position and velocity of the other gripper as the goal of the first gripper pose
if not self.opPointExist(sotrobot.dynamic, self.otherGrasp.gripper.sotjoint):
sotrobot.dynamic.createOpPoint(self.otherGripper.sotjoint, self.otherGripper.sotjoint)
sotrobot.dynamic.createVelocity("vel_"+self.otherGripper.sotjoint, self.otherGrasp.gripper.sotjoint)
plug(sotrobot.dynamic.signal(self.otherGripper.sotjoint), self.graspTask.featureDes.position)
plug(sotrobot.dynamic.signal("vel_"+self.otherGripper.sotjoint), self.graspTask.featureDes.velocity)
#plug(sotrobot.dynamic.signal("J"+self.otherGrasp.gripper.sotjoint), self.graspTask.featureDes.Jq)
# We will need a less barbaric method to do this...
# Zeroes the jacobian for the joints we don't want to use.
# - Create the selection matrix for the desired joints
mat = ()
for i in range(38):
mat += ((0,)*i + (1 if i in range(29, 36) else 0,) + (0,)*(37-i),)
# - Multiplies it with the computed jacobian matrix of the gripper
mult = Multiply_of_matrix('mult')
mult.sin2.value = mat
plug(self.graspTask.opPointModif.jacobian, mult.sin1)
plug(mult.sout, self.graspTask.feature.Jq)
self.tasks = [ self.graspTask.task ]
#self.tasks = []
self += EEPosture (sotrobot, self.gripper, [-0.2 if self.closeGripper else 0])
def opPointExist(self,dyn,opPoint):
sigsP = filter(lambda x: x.getName().split(':')[-1] == opPoint,
dyn.signals())
sigsJ = filter(lambda x: x.getName().split(':')[-1] == 'J'+opPoint,
dyn.signals())
return len(sigsP)==1 & len(sigsJ)==1
class EEPosture (Manifold):
def __init__ (self, sotrobot, gripper, position):
......@@ -212,7 +212,7 @@ class EEPosture (Manifold):
plug(sotrobot.dynamic.position, self.tp.feature.state)
q = list(sotrobot.dynamic.position.value)
q[idx_v:idx_v + 1] = position
q[idx_v:idx_v + joint.nv] = position
self.tp.feature.posture.value = q
self.tp.gain = GainAdaptive("gain_"+n)
......@@ -220,7 +220,8 @@ class EEPosture (Manifold):
# for i in range (6, robotDim):
# self.tp.feature.selectDof (i, False)
# print idx_q, idx_v
self.tp.feature.selectDof (idx_v, True)
for i in range(joint.nv):
self.tp.feature.selectDof (idx_v + i, True)
self.tp.add(self.tp.feature.name)
# Set the gain of the posture task
......@@ -230,11 +231,12 @@ class EEPosture (Manifold):
self.tasks = [ self.tp ]
class Foot (Manifold):
def __init__ (self, footname, sotrobot):
def __init__ (self, footname, sotrobot, selec='111111'):
robotname, sotjoint = parseHppName (footname)
self.taskFoot = MetaTaskKine6d(
Foot.sep + footname,
sotrobot.dynamic,sotjoint,sotjoint)
self.taskFoot.feature.selec.value = selec
super(Foot, self).__init__(
tasks = [ self.taskFoot.task, ],
topics = {
......
......@@ -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,6 @@ 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;
}
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