Unverified Commit 6ee9bbd2 authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub

Merge pull request #5 from jmirabel/master

Remove dependency to HPP except for ConstraintFactoryAbstract
parents a78007e7 f0cf2117
......@@ -93,6 +93,7 @@ SET(FILES
tools.py
ros_interface.py
factory.py
srdf_parser.py
__init__.py)
FOREACH(F ${FILES})
......
......@@ -68,8 +68,8 @@ class TaskFactory(ConstraintFactoryAbstract):
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)
otherIg = self.graphfactory.grippers.index(otherGrasp.gripper.key)
otherIh = self.graphfactory.handles.index(otherGrasp.handle.key)
k = (ig, ih, otherIg, otherIh)
else:
k = (ig, ih)
......@@ -121,22 +121,9 @@ class Factory(GraphFactoryAbstract):
assert isinstance(aff, Affordance)
self.affordances [(aff.gripper, aff.handle)] = aff
def finalize (self, hppclient):
graph, elmts = hppclient.manipulation.graph.getGraph()
ids = { n.name: n.id for n in elmts.edges }
nids = { n.name: n.id for n in elmts.nodes }
self.transitionIds = { n.name: n.id for n in elmts.edges }
# self.supervisor.sots = { ids[n]: sot for n, sot in self.sots.items() if ids.has_key(n) }
# self.supervisor.grasps = { (gh, w): t for gh, ts in self.tasks._grasp.items() for w, t in ts.items() }
# self.supervisor.hpTasks = self.hpTasks
# self.supervisor.lpTasks = self.lpTasks
# self.supervisor.postActions = {
# ids[trans] : {
# nids[state]: sot for state, sot in values.items() if nids.has_key(state)
# } for trans, values in self.postActions.items() if ids.has_key(trans)
# }
# self.supervisor.preActions = { ids[trans] : sot for trans, sot in self.preActions.items() if ids.has_key(trans) }
def generate (self):
super(Factory, self).generate ()
self.supervisor.sots = self.sots
self.supervisor.grasps = { (gh, w): t for gh, ts in self.tasks._grasp.items() for w, t in ts.items() }
self.supervisor.hpTasks = self.hpTasks
......@@ -144,20 +131,14 @@ class Factory(GraphFactoryAbstract):
self.supervisor.postActions = self.postActions
self.supervisor.preActions = self.preActions
def setupFrames (self, hppclient, sotrobot):
def setupFrames (self, srdfGrippers, srdfHandles, sotrobot):
self.sotrobot = sotrobot
self.grippersIdx = { g: i for i,g in enumerate(self.grippers) }
self.handlesIdx = { h: i for i,h in enumerate(self.handles) }
self.gripperFrames = { g: OpFrame(hppclient) for g in self.grippers }
self.handleFrames = { h: OpFrame(hppclient) for h in self.handles }
for g in self.grippers: self.gripperFrames[g].setHppGripper (g)
for h in self.handles : self.handleFrames [h].setHppHandle (h)
for g in self.gripperFrames.values(): g.setSotFrameFromHpp (sotrobot.dynamic.model)
for h in self.handleFrames .values(): h.setSotFrameFromHpp (sotrobot.dynamic.model)
self.gripperFrames = { g: OpFrame(srdfGrippers[g], sotrobot.dynamic.model) for g in self.grippers }
self.handleFrames = { h: OpFrame(srdfHandles [h] ) for h in self.handles }
def makeState (self, grasps, priority):
# Nothing to do here
......
import xml.etree.ElementTree as ET
def _read_name (xml):
return str(xml.attrib["name"])
def _read_clearance (xml):
return float(xml.attrib.get('clearance', 0))
def _read_mask (xml):
masksTag = xml.findall('mask')
if len(masksTag) > 1:
raise ValueError ("Handle needs at most one tag mask")
elif len(masksTag) == 1:
mask = [ bool(v) for v in masksTag[0].text.split() ]
else:
mask = (True, ) * 6
if len(mask) != 6:
raise ValueError ("Tag mask must contain 6 booleans")
return tuple (mask)
def _read_joints (xml):
jointTags = xml.findall('joint')
return tuple ( [ jt.attrib["name"] for jt in jointTags ] )
def _read_position (xml):
positionsTag = xml.findall('position')
if len(positionsTag) != 1:
raise ValueError ("Gripper needs exactly one tag position")
return tuple ([ float(x) for x in positionsTag[0].text.split() ])
def _read_link (xml):
linksTag = xml.findall('link')
if len(linksTag) != 1:
raise ValueError ("Gripper needs exactly one tag link")
return str(linksTag[0].attrib['name'])
def parse_srdf (srdf, packageName = None, prefix = None):
"""
parameters:
- packageName: if provided, the filename is considered relative to this ROS package
- prefix: if provided, the name of the elements will be prepended with
prefix + "/"
"""
import os
if packageName is not None:
from rospkg import RosPack
rospack = RosPack()
path = rospack.get_path(packageName)
srdfFn = os.path.join(path, srdf)
else:
srdfFn = srdf
# tree = ET.fromstring (srdfFn)
tree = ET.parse (srdfFn)
root = tree.getroot()
grippers = {}
for xml in root.iter('gripper'):
n = _read_name (xml)
g = { "robot": prefix,
"name": n,
"clearance": _read_clearance (xml),
"link": _read_link (xml),
"position": _read_position (xml),
"joints": _read_joints (xml),
}
grippers[ prefix + "/" + n if prefix is not None else n] = g
handles = {}
for xml in root.iter('handle'):
n = _read_name (xml)
h = { "robot": prefix,
"name": n,
"clearance": _read_clearance (xml),
"link": _read_link (xml),
"position": _read_position (xml),
"mask": _read_mask (xml),
}
handles[ prefix + "/" + n if prefix is not None else n] = h
return { "grippers": grippers, "handles": handles}
from hpp import Transform
from dynamic_graph.sot.core.meta_tasks_kine import MetaTaskKine6d, MetaTaskKineCom
from dynamic_graph.sot.core.meta_tasks_kine_relative import MetaTaskKine6dRel
from dynamic_graph.sot.core.meta_tasks import setGain
......@@ -8,13 +7,14 @@ def parseHppName (hppjointname):
if hppjointname == "universe": return "", "universe"
return hppjointname.split('/', 1)
def transformToTuple (T):
def transQuatToSE3 (p):
from pinocchio import SE3, Quaternion
from numpy import matrix
return SE3 (Quaternion (p[6],p[3],p[4],p[5]).matrix(), matrix(p[0:3]).transpose())
def se3ToTuple (M):
from dynamic_graph.sot.core.matrix_util import matrixToTuple
from numpy import eye
M = eye (4)
M[:3,:3] = T.quaternion.toRotationMatrix()
M[:3,3] = T.translation
return matrixToTuple(M)
return matrixToTuple (M.homogeneous)
class Manifold(object):
sep = "___"
......@@ -91,31 +91,29 @@ class Posture(Manifold):
def _signalPositionRef (self): return self.tp.featureDes.errorIN
def _signalVelocityRef (self): return self.tp.featureDes.errordotIN
## Represents a gripper or a handle
class OpFrame(object):
def __init__ (self, hppclient):
self.hpp = hppclient
def setHppGripper (self, name):
self.name = name
# Get parent joint and position from HPP
self.hppjoint, self.hpppose = self.hpp.manipulation.robot.getGripperPositionInJoint(name)
self.hpppose = Transform (self.hpppose)
def setHppHandle (self, name):
self.name = name
# Get parent joint and position from HPP
self.hppjoint, self.hpppose = self.hpp.manipulation.robot.getHandlePositionInJoint(name)
self.hpppose = Transform (self.hpppose)
def setSotFrameFromHpp(self, pinrobot):
# The joint should be available in the robot model used by SOT
n = self.hppjoint
self.sotpose = self.hpppose
self.robotname, self.sotjoint = parseHppName (n)
while self.sotjoint not in pinrobot.names:
self.sotpose = Transform(self.hpp.basic.robot.getJointPositionInParentFrame(n)) * self.sotpose
n = self.hpp.basic.robot.getParentJointName(n)
robotname, self.sotjoint = parseHppName (n)
def __init__ (self, srdf, model = None):
self.robotName = srdf["robot"]
self.name = srdf["name"]
self.key = self.robotName + "/" + self.name
self.link = srdf["link"]
pose = transQuatToSE3 (srdf["position"])
if srdf.has_key ("joints"):
assert model is not None
## Only for grippers
self.joints = srdf["joints"]
self._setupParentJoint (self.link, pose, model)
else:
## Only for handles
self.pose = pose
def _setupParentJoint (self, link, pose, model):
frameid = model.getFrameId (link)
frame = model.frames[frameid]
self.pose = frame.placement * pose
self.joint = model.names[frame.parent]
class Grasp (Manifold):
def __init__ (self, gripper, handle, otherGraspOnObject = None, closeGripper = False):
......@@ -124,8 +122,12 @@ class Grasp (Manifold):
self.handle = handle
self.closeGripper = closeGripper
self.relative = otherGraspOnObject is not None \
and otherGraspOnObject.handle.hppjoint == self.handle.hppjoint
# self.relative = otherGraspOnObject is not None \
# and otherGraspOnObject.handle.link == self.handle.link
# TODO: We should make sure that the relative position is constant
# but we have no way to do it (HPP does). We assume that the object is
# not articulated.
self.relative = otherGraspOnObject
if self.relative:
self.otherGripper = otherGraspOnObject.gripper
self.otherHandle = otherGraspOnObject.handle
......@@ -136,19 +138,19 @@ class Grasp (Manifold):
self.graspTask = MetaTaskKine6dRel (
Grasp.sep + self.gripper.name + Grasp.sep + self.otherGripper.name,
sotrobot.dynamic,
self.gripper.sotjoint,
self.otherGripper.sotjoint,
self.gripper.sotjoint,
self.otherGripper.sotjoint)
self.gripper.joint,
self.otherGripper.joint,
self.gripper.joint,
self.otherGripper.joint)
M = transformToTuple(self.gripper.sotpose * self.handle.sotpose.inverse())
M = se3ToTuple(self.gripper.pose * self.handle.pose.inverse())
self.graspTask.opPointModif.activ = True
self.graspTask.opPointModif.setTransformation (M)
# Express the velocities in local frame.
# This is the default.
# self.graspTask.opPointModif.setEndEffector(True)
M = transformToTuple(self.otherGripper.sotpose * self.otherHandle.sotpose.inverse())
M = se3ToTuple(self.otherGripper.pose * self.otherHandle.pose.inverse())
self.graspTask.opPointModifBase.activ = True
self.graspTask.opPointModifBase.setTransformation (M)
# Express the velocities in local frame.
......@@ -179,8 +181,7 @@ class EEPosture (Manifold):
super(EEPosture, self).__init__()
self.gripper = gripper
robotName, gripperName = parseHppName (gripper.name)
self.jointNames = sotrobot.grippers[gripperName]
self.jointNames = gripper.joints
pinmodel = sotrobot.dynamic.model
n = "eeposture" + Posture.sep + gripper.name + Posture.sep + str(list(position))
......
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