Merge pull request #32 from yosuke/master

fix conversion problem for model with closed links

simtrans/template/vrml.wrl

@@ -234,11 +234,13 @@ DEF {{model.name}} Humanoid {
     USE {{j}}{%- if loop.last == False %},{%- endif %}
     {%- endfor %}
   ]
+  {#
   segments [
     {%- for l in links %}
     USE {{l}}{%- if loop.last == False %},{%- endif %}
     {%- endfor %}
   ]
+  #}
   name "{{model.name}}"
   version "1.0"
 }

simtrans/urdf.py

@@ -66,7 +66,7 @@ def read(self, fname, assethandler=None, options=None):
         '''
         fd, sdffile = tempfile.mkstemp(suffix='.sdf')
         try:
-            d = subprocess.check_output(['gz', 'sdf', '-p', fname])
+            d = subprocess.check_output(['gz', 'sdf', '-p', utils.resolveFile(fname)])
             os.write(fd, d)
         finally:
             os.close(fd)

simtrans/utils.py

@@ -23,6 +23,7 @@ def resolveFile(f):
     >>> resolveFile('model://PA10/pa10.main.wrl') == os.path.expandvars('$OPENHRP_MODEL_PATH/PA10/pa10.main.wrl')
     True
     '''
+    logger.debug('resolveFile from %s' % f)
     try:
         if f.count('model://') > 0:
             fn = f.replace('model://', '')
@@ -35,13 +36,18 @@ def resolveFile(f):
             for p in paths:
                 ff = os.path.expanduser(os.path.join(p, fn))
                 if os.path.exists(ff):
+                    logger.debug('resolveFile resolved to %s' % ff)
                     return ff
         if f.count('package://') > 0:
             pkgname, pkgfile = f.replace('package://', '').split('/', 1)
             ppath = subprocess.check_output(['rospack', 'find', pkgname]).rstrip()
-            return os.path.join(ppath, pkgfile)
+            logger.debug('resolveFile find package path %s' % ppath)
+            ff = os.path.join(ppath, pkgfile)
+            logger.debug('resolveFile resolved to %s' % ff)
+            return ff
     except Exception, e:
         logger.warn(str(e))
+    logger.debug('resolveFile unresolved (use original)')
     return f
 
 
@@ -50,24 +56,37 @@ def findroot(mdata):
     Find root link from parent to child relationships.
     Currently based on following simple principle:
     - Link with no parent will be the root.
+    - Link should have at least one open connection with the other link
 
+    >>> import subprocess
     >>> from . import urdf
+    >>> subprocess.call('rosrun xacro xacro.py `rospack find atlas_description`/robots/atlas_v3.urdf.xacro > /tmp/atlas.urdf', shell=True)
+    0
     >>> r = urdf.URDFReader()
-    >>> m = r.read('package://atlas_description/urdf/atlas_v3.urdf')
-    >>> findroot(m)[0]
-    'pelvis'
+    >>> m = r.read('/tmp/atlas.urdf')
+    >>> findroot(m)
+    ['pelvis']
 
     >>> from . import urdf
     >>> r = urdf.URDFReader()
     >>> m = r.read('package://ur_description/urdf/ur5_robot.urdf')
-    >>> findroot(m)[0]
-    'world'
+    >>> findroot(m)
+    ['world']
+
+    >>> import subprocess
+    >>> from . import urdf
+    >>> subprocess.call('rosrun xacro xacro.py `rospack find pr2_description`/robots/pr2.urdf.xacro > /tmp/pr2.urdf', shell=True)
+    0
+    >>> r = urdf.URDFReader()
+    >>> m = r.read('/tmp/pr2.urdf')
+    >>> findroot(m)
+    ['base_footprint']
 
     >>> from . import sdf
     >>> r = sdf.SDFReader()
     >>> m = r.read('model://pr2/model.sdf')
-    >>> findroot(m)[0]
-    'base_footprint'
+    >>> findroot(m)
+    ['base_footprint']
     '''
     links = {}
     usedlinks = {}
@@ -83,26 +102,70 @@ def findroot(mdata):
             del links[j.child]
         except KeyError:
             pass
-    ret = [l[0] for l in sorted(links.items(), key=lambda x: x[1], reverse=True)]
+    peaks = [l[0] for l in sorted(links.items(), key=lambda x: x[1], reverse=True)]
+    ret = []
+    for p in peaks:
+        if hasopenlink(mdata, p):
+            ret.append(p)
     for l in mdata.links:
         if not usedlinks.has_key(l.name):
             ret.append(l.name)
     return ret
 
+def hasopenlink(mdata, linkname):
+    '''
+    Check if the link has open connection with neighboring links
+
+    >>> from . import sdf
+    >>> r = sdf.SDFReader()
+    >>> m = r.read('model://pr2/model.sdf')
+    >>> hasopenlink(m, 'base_footprint')
+    True
+    >>> hasopenlink(m, 'l_gripper_l_parallel_link')
+    False
+    '''
+    for c in findchildren(mdata, linkname):
+        parents = [p.parent for p in findparent(mdata, c.child)]
+        if len(set(parents)) == 1:
+            return True
+    return False
+
 
 def findchildren(mdata, linkname):
     '''
     Find child joints connected to specified link
 
+    >>> import subprocess
     >>> from . import urdf
+    >>> subprocess.call('rosrun xacro xacro.py `rospack find atlas_description`/robots/atlas_v3.urdf.xacro > /tmp/atlas.urdf', shell=True)
+    0
     >>> r = urdf.URDFReader()
-    >>> m = r.read('package://atlas_description/urdf/atlas_v3.urdf')
-    >>> w = VRMLWriter()
-    >>> [c.child for c in w.findchildren(m, 'pelvis')]
+    >>> m = r.read('/tmp/atlas.urdf')
+    >>> [c.child for c in findchildren(m, 'pelvis')]
     ['ltorso', 'l_uglut', 'r_uglut']
     '''
     children = []
     for j in mdata.joints:
         if j.parent == linkname:
             children.append(j)
     return children
+
+
+def findparent(mdata, linkname):
+    '''
+    Find parent joints connected to specified link
+
+    >>> import subprocess
+    >>> from . import urdf
+    >>> subprocess.call('rosrun xacro xacro.py `rospack find atlas_description`/robots/atlas_v3.urdf.xacro > /tmp/atlas.urdf', shell=True)
+    0
+    >>> r = urdf.URDFReader()
+    >>> m = r.read('/tmp/atlas.urdf')
+    >>> [p.parent for p in findparent(m, 'ltorso')]
+    ['pelvis']
+    '''
+    parents = []
+    for j in mdata.joints:
+        if j.child == linkname:
+            parents.append(j)
+    return parents

simtrans/vrml.py

@@ -405,6 +405,7 @@ def write(self, mdata, fname, options=None):
         template = env.get_template('vrml.wrl')
         modelfiles = {}
         for root in self._roots:
+            logging.info('writing model for %s' % root)
             # first convert data structure (VRML uses tree structure)
             if root == 'world':
                 roots = utils.findchildren(mdata, root)
@@ -462,7 +463,8 @@ def convertchildren(self, mdata, pjoint, joints, links):
             nmodel['link'] = clink2
             nmodel['children'] = cchildren
             children.append(nmodel)
-            joints.append(cjoint.name)
+            if clink and (clink.visuals or clink.collisions or len(cchildren) > 0):
+                joints.append(cjoint.name)
             links.append(cjoint.child)
         return (children, joints, links)
 

testrunner.py

@@ -9,6 +9,9 @@
 import simtrans.vrml
 import simtrans.graphviz
 
+import logging
+logging.basicConfig(level=logging.DEBUG)
+
 doctest.testmod(simtrans.utils)
 doctest.testmod(simtrans.model)
 doctest.testmod(simtrans.collada)