From 6bb61535cd53488af0941581fadd160b73ac1147 Mon Sep 17 00:00:00 2001 From: Yosuke Matsusaka Date: Thu, 27 Aug 2015 12:18:21 +0900 Subject: [PATCH 1/6] update testcode (detection of root nodes of pr2 model seems to be working OK) ref #27 --- simtrans/urdf.py | 2 +- simtrans/utils.py | 30 +++++++++++++++++++++++++----- testrunner.py | 3 +++ 3 files changed, 29 insertions(+), 6 deletions(-) diff --git a/simtrans/urdf.py b/simtrans/urdf.py index e6efc4c..58fd73f 100644 --- a/simtrans/urdf.py +++ b/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) diff --git a/simtrans/utils.py b/simtrans/utils.py index ecb9616..09abb2a 100644 --- a/simtrans/utils.py +++ b/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 @@ -51,9 +57,12 @@ def findroot(mdata): Currently based on following simple principle: - Link with no parent will be the root. + >>> 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') + >>> m = r.read('/tmp/atlas.urdf') >>> findroot(m)[0] 'pelvis' @@ -63,6 +72,15 @@ def findroot(mdata): >>> findroot(m)[0] '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)[0] + 'base_footprint' + >>> from . import sdf >>> r = sdf.SDFReader() >>> m = r.read('model://pr2/model.sdf') @@ -94,11 +112,13 @@ 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 = [] diff --git a/testrunner.py b/testrunner.py index f68f5d9..9b1835c 100755 --- a/testrunner.py +++ b/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) From a19c9c06fcde7c45e5429b047aa83a997606a766 Mon Sep 17 00:00:00 2001 From: Yosuke Matsusaka Date: Thu, 27 Aug 2015 12:28:08 +0900 Subject: [PATCH 2/6] update testcode to check all the found root joints (turned out the code returns false result) ref #27 --- simtrans/utils.py | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/simtrans/utils.py b/simtrans/utils.py index 09abb2a..b4013a1 100644 --- a/simtrans/utils.py +++ b/simtrans/utils.py @@ -63,14 +63,14 @@ def findroot(mdata): 0 >>> r = urdf.URDFReader() >>> m = r.read('/tmp/atlas.urdf') - >>> findroot(m)[0] - 'pelvis' + >>> 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 @@ -78,14 +78,14 @@ def findroot(mdata): 0 >>> r = urdf.URDFReader() >>> m = r.read('/tmp/pr2.urdf') - >>> findroot(m)[0] - 'base_footprint' + >>> 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 = {} From e71393ac822583414170f086bd3a24dc2eff7d05 Mon Sep 17 00:00:00 2001 From: Yosuke Matsusaka Date: Fri, 28 Aug 2015 07:09:29 +0900 Subject: [PATCH 3/6] add utility function to find parents --- simtrans/utils.py | 19 +++++++++++++++++++ 1 file changed, 19 insertions(+) diff --git a/simtrans/utils.py b/simtrans/utils.py index b4013a1..8c61862 100644 --- a/simtrans/utils.py +++ b/simtrans/utils.py @@ -126,3 +126,22 @@ def findchildren(mdata, linkname): 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 From f2752edf77362e39397ad6dd054de5bc10a3de5d Mon Sep 17 00:00:00 2001 From: Yosuke Matsusaka Date: Fri, 28 Aug 2015 07:09:49 +0900 Subject: [PATCH 4/6] display currently writing model --- simtrans/vrml.py | 1 + 1 file changed, 1 insertion(+) diff --git a/simtrans/vrml.py b/simtrans/vrml.py index d170740..cfdf7e8 100644 --- a/simtrans/vrml.py +++ b/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) From 72d1112bed206b938214d12827a85591fd0e08d7 Mon Sep 17 00:00:00 2001 From: Yosuke Matsusaka Date: Fri, 28 Aug 2015 10:22:21 +0900 Subject: [PATCH 5/6] check for local peaks caused by closed link when detecting root links (ref #27) --- simtrans/utils.py | 26 +++++++++++++++++++++++++- 1 file changed, 25 insertions(+), 1 deletion(-) diff --git a/simtrans/utils.py b/simtrans/utils.py index 8c61862..2522f62 100644 --- a/simtrans/utils.py +++ b/simtrans/utils.py @@ -56,6 +56,7 @@ 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 @@ -101,12 +102,34 @@ 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): ''' @@ -127,6 +150,7 @@ def findchildren(mdata, linkname): children.append(j) return children + def findparent(mdata, linkname): ''' Find parent joints connected to specified link From 2c42cfa1258ee3d76532ffc614023e0ea977575c Mon Sep 17 00:00:00 2001 From: Yosuke Matsusaka Date: Fri, 28 Aug 2015 10:50:21 +0900 Subject: [PATCH 6/6] improve presiceness of use joints and disable use segments seems not required --- simtrans/template/vrml.wrl | 2 ++ simtrans/vrml.py | 3 ++- 2 files changed, 4 insertions(+), 1 deletion(-) diff --git a/simtrans/template/vrml.wrl b/simtrans/template/vrml.wrl index 38ae246..781d316 100644 --- a/simtrans/template/vrml.wrl +++ b/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" } diff --git a/simtrans/vrml.py b/simtrans/vrml.py index cfdf7e8..23ad5c5 100644 --- a/simtrans/vrml.py +++ b/simtrans/vrml.py @@ -463,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)