From 936aa281bc20c51aa73cb081ee52fd19af3bacf0 Mon Sep 17 00:00:00 2001 From: Yosuke Matsusaka Date: Mon, 17 Aug 2015 23:29:40 +0900 Subject: [PATCH 1/3] support VRML collision and visual node (close #29) --- simtrans/cli.py | 1 + simtrans/template/vrml.wrl | 111 ++++++++++++++++++++++++------------- simtrans/vrml.py | 48 +++++++++------- 3 files changed, 102 insertions(+), 58 deletions(-) diff --git a/simtrans/cli.py b/simtrans/cli.py index 0f7d93e..dbb8661 100644 --- a/simtrans/cli.py +++ b/simtrans/cli.py @@ -38,6 +38,7 @@ parser.add_argument('-o', '--output', dest='tofile', metavar='FILE', help='convert to FILE') parser.add_argument('-f', '--from', dest='fromformat', metavar='FORMAT', help='convert from FORMAT (optional)') parser.add_argument('-c', '--use-collision', action='store_true', dest='usecollision', default=False, help='use collision shape when converting to VRML') +parser.add_argument('-b', '--use-both', action='store_true', dest='useboth', default=False, help='use both visual and collision shape when converting to VRML (only supported on most recent version of Choreonoid)') parser.add_argument('-t', '--to', dest='toformat', metavar='FORMAT', help='convert to FORMAT (optional)') parser.add_argument('-v', '--verbose', action='store_true', dest='verbose', default=False, help='verbose output') diff --git a/simtrans/template/vrml.wrl b/simtrans/template/vrml.wrl index 8bb10c6..38ae246 100644 --- a/simtrans/template/vrml.wrl +++ b/simtrans/template/vrml.wrl @@ -58,6 +58,24 @@ PROTO Segment [ } } +PROTO Surface [ + field SFVec3f bboxCenter 0 0 0 + field SFVec3f bboxSize -1 -1 -1 + field MFNode visual [ ] + field MFNode collision [ ] + eventIn MFNode addChildren + eventIn MFNode removeChildren +] +{ + Group { + addChildren IS addChildren + bboxCenter IS bboxCenter + bboxSize IS bboxSize + children IS visual + removeChildren IS removeChildren + } +} + PROTO Humanoid [ field SFVec3f bboxCenter 0 0 0 field SFVec3f bboxSize -1 -1 -1 @@ -114,14 +132,47 @@ DEF {{j.name}} ExtraJoint { } {%- endfor %} +{% macro rendershape(v, ShapeModel, shapefilemap) -%} +Transform { + {%- set scale = v.getscale() %} + scale {{scale[0]}} {{scale[1]}} {{scale[2]}} + {%- set trans = v.gettranslation() %} + translation {{trans[0]}} {{trans[1]}} {{trans[2]}} + {%- set angle = v.getangle() %} + rotation {{angle[0][0]}} {{angle[0][1]}} {{angle[0][2]}} {{angle[1]}} + children [ + {%- if v.shapeType == ShapeModel.SP_MESH %} + Inline { + url "{{shapefilemap[v.name]}}" + } + {%- elif v.shapeType == ShapeModel.SP_SPHERE %} + Shape { + geometry Sphere { + radius {{v.data.radius}} + } + } + {%- elif v.shapeType == ShapeModel.SP_BOX %} + Shape { + geometry Box { + size {{v.data.x}} {{v.data.y}} {{v.data.z}} + } + } + {%- elif v.shapeType == ShapeModel.SP_CYLINDER %} + Shape { + geometry Cylinder { + radius {{v.data.radius}} + height {{v.data.height}} + } + } + {%- endif %} + ] +} +{%- endmacro %} + DEF {{model.name}} Humanoid { humanoidBody [ {%- for l in model.children recursive %} {%- if l.link and (l.link.visuals|length or l.link.collisions|length or l.children|length) %} - {%- set shapes = l.link.visuals %} - {%- if options.usecollision %} - {%- set shapes = l.link.collisions %} - {%- endif %} DEF {{l.joint.name}} Joint { jointType "{{l.jointtype}}" {%- if l.joint.axis.axis %} @@ -148,42 +199,28 @@ DEF {{model.name}} Humanoid { centerOfMass {{l.link.centerofmass[0]}} {{l.link.centerofmass[1]}} {{l.link.centerofmass[2]}} momentsOfInertia [{{l.link.inertia[0][0]}} {{l.link.inertia[0][1]}} {{l.link.inertia[0][2]}} {{l.link.inertia[1][0]}} {{l.link.inertia[1][1]}} {{l.link.inertia[1][2]}} {{l.link.inertia[2][0]}} {{l.link.inertia[2][1]}} {{l.link.inertia[2][2]}}] children [ - {%- for v in shapes %} - Transform { - {%- set scale = v.getscale() %} - scale {{scale[0]}} {{scale[1]}} {{scale[2]}} - {%- set trans = v.gettranslation() %} - translation {{trans[0]}} {{trans[1]}} {{trans[2]}} - {%- set angle = v.getangle() %} - rotation {{angle[0][0]}} {{angle[0][1]}} {{angle[0][2]}} {{angle[1]}} - children [ - {%- if v.shapeType == ShapeModel.SP_MESH %} - Inline { - url "{{body.name}}-{{l.link.name|replace('::', '_')}}-{{v.name|replace('::', '_')}}.wrl" - } - {%- elif v.shapeType == ShapeModel.SP_SPHERE %} - Shape { - geometry Sphere { - radius {{v.data.radius}} - } - } - {%- elif v.shapeType == ShapeModel.SP_BOX %} - Shape { - geometry Box { - size {{v.data.x}} {{v.data.y}} {{v.data.z}} - } - } - {%- elif v.shapeType == ShapeModel.SP_CYLINDER %} - Shape { - geometry Cylinder { - radius {{v.data.radius}} - height {{v.data.height}} - } - } - {%- endif %} + {%- if options.usecollision %} + {%- for c in l.link.collisions %} + {{ rendershape(c, ShapeModel, shapefilemap)|indent(loop.depth+11) }} + {%- endfor %} + {%- elif options.useboth %} + Surface { + visual [ + {%- for v in l.link.visuals %} + {{ rendershape(v, ShapeModel, shapefilemap)|indent(loop.depth+15) }} + {%- endfor %} + ] + collision [ + {%- for c in l.link.collisions %} + {{ rendershape(c, ShapeModel, shapefilemap)|indent(loop.depth+15) }} + {%- endfor %} ] } + {%- else %} + {%- for v in l.link.visuals %} + {{ rendershape(v, ShapeModel, shapefilemap)|indent(loop.depth+11) }} {%- endfor %} + {%- endif %} ] } {{ loop(l.children)|indent(4, True) }} diff --git a/simtrans/vrml.py b/simtrans/vrml.py index 89a48d9..d170740 100644 --- a/simtrans/vrml.py +++ b/simtrans/vrml.py @@ -376,28 +376,14 @@ def write(self, mdata, fname, options=None): for m in mdata.links: self._linkmap[m.name] = m - # render main vrml file for each bodies - template = env.get_template('vrml.wrl') - modelfiles = {} - for root in self._roots: - # first convert data structure (VRML uses tree structure) - if root == 'world': - roots = utils.findchildren(mdata, root) - for r in roots: - # print("root joint is world. using %s as root" % root) - mfname = (mdata.name + "-" + r.child + ".wrl").replace('::', '_') - self.renderchildren(mdata, r.child, "fixed", os.path.join(dirname, mfname), template) - modelfiles[mfname] = self._linkmap[r.child] - else: - mfname = (mdata.name + "-" + root + ".wrl").replace('::', '_') - self.renderchildren(mdata, root, "free", os.path.join(dirname, mfname), template) - modelfiles[mfname] = self._linkmap[root] - # render shape vrml file for each links + shapefilemap = {} for l in mdata.links: - shapes = l.visuals + shapes = copy.copy(l.visuals) if options.usecollision: - shapes = l.collisions + shapes = copy.copy(l.collisions) + if options.useboth: + shapes.extend(copy.copy(l.collisions)) for v in shapes: logging.info('writing shape of link: %s, type: %s' % (l.name, v.shapeType)) if v.shapeType == model.ShapeModel.SP_MESH: @@ -406,13 +392,32 @@ def write(self, mdata, fname, options=None): v.data.pretranslate() m = {} m['children'] = [v.data] - with open(os.path.join(dirname, mdata.name + "-" + l.name + "-" + v.name + ".wrl").replace('::', '_'), 'w') as ofile: + shapefname = (mdata.name + "-" + l.name + "-" + v.name + ".wrl").replace('::', '_') + with open(os.path.join(dirname, shapefname), 'w') as ofile: ofile.write(template.render({ 'name': v.name, 'ShapeModel': model.ShapeModel, 'mesh': m })) + shapefilemap[v.name] = shapefname + # render main vrml file for each bodies + template = env.get_template('vrml.wrl') + modelfiles = {} + for root in self._roots: + # first convert data structure (VRML uses tree structure) + if root == 'world': + roots = utils.findchildren(mdata, root) + for r in roots: + # print("root joint is world. using %s as root" % root) + mfname = (mdata.name + "-" + r.child + ".wrl").replace('::', '_') + self.renderchildren(mdata, r.child, "fixed", os.path.join(dirname, mfname), shapefilemap, template) + modelfiles[mfname] = self._linkmap[r.child] + else: + mfname = (mdata.name + "-" + root + ".wrl").replace('::', '_') + self.renderchildren(mdata, root, "free", os.path.join(dirname, mfname), shapefilemap, template) + modelfiles[mfname] = self._linkmap[root] + # render openhrp project template = env.get_template('openhrp-project.xml') with open(fname.replace('.wrl', '-project.xml'), 'w') as ofile: @@ -461,7 +466,7 @@ def convertchildren(self, mdata, pjoint, joints, links): links.append(cjoint.child) return (children, joints, links) - def renderchildren(self, mdata, root, jointtype, fname, template): + def renderchildren(self, mdata, root, jointtype, fname, shapefilemap, template): nmodel = {} rootlink = self._linkmap[root] rootjoint = model.JointModel() @@ -498,6 +503,7 @@ def renderchildren(self, mdata, root, jointtype, fname, template): 'joints': joints, 'jointmap': jointmap, 'ShapeModel': model.ShapeModel, + 'shapefilemap': shapefilemap, 'options': self._options })) From 2f59d93c9e2b4033338faee753fbd9a28537be59 Mon Sep 17 00:00:00 2001 From: Yosuke Matsusaka Date: Mon, 17 Aug 2015 23:54:11 +0900 Subject: [PATCH 2/3] update documentation --- doc/locale/ja/LC_MESSAGES/usage.po | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/doc/locale/ja/LC_MESSAGES/usage.po b/doc/locale/ja/LC_MESSAGES/usage.po index c9cccca..19e9140 100644 --- a/doc/locale/ja/LC_MESSAGES/usage.po +++ b/doc/locale/ja/LC_MESSAGES/usage.po @@ -8,7 +8,7 @@ msgstr "" "Project-Id-Version: simtrans 0.0.1\n" "Report-Msgid-Bugs-To: \n" "POT-Creation-Date: 2014-11-19 17:30+0900\n" -"PO-Revision-Date: 2014-12-15 16:25+0900\n" +"PO-Revision-Date: 2015-08-17 23:52+0900\n" "Last-Translator: Yosuke Matsusaka \n" "Language-Team: none\n" "MIME-Version: 1.0\n" @@ -106,6 +106,18 @@ msgstr "URDFフォーマットからVRMLフォーマットへの変換" msgid "Convert VRML model to SDF format" msgstr "VRMLフォーマットからSDFフォーマットへの変換" +#: ../../:1 +msgid "use collision shape when converting to VRML" +msgstr "VRMLへの変換時にcollision形状を使う" + +#: ../../:1 +msgid "" +"use both visual and collision shape when converting to VRML (only supported " +"on most recent version of Choreonoid)" +msgstr "" +"VRMLへの変換時にcollision形状とvisual形状の両方を使う（この機能は最新の" +"Choreonoidでのみ利用可能です）" + #~ msgid "Convert URDF model to VRML(OpenHRP) format" #~ msgstr "URDF形式のモデルのVRML（OpenHRP）形式への変換" From c88aff2acf0d0064be19c7dc883bf4dfa8c95715 Mon Sep 17 00:00:00 2001 From: Yosuke Matsusaka Date: Tue, 18 Aug 2015 09:24:32 +0900 Subject: [PATCH 3/3] apply rotation of cylinder axis using matrix (fix #20) --- simtrans/sdf.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/simtrans/sdf.py b/simtrans/sdf.py index 78f50e7..1427b8d 100644 --- a/simtrans/sdf.py +++ b/simtrans/sdf.py @@ -290,7 +290,9 @@ def readShape(self, d): m.data = model.CylinderData() m.data.radius = float(g.find('radius').text) m.data.height = float(g.find('length').text) - m.rot = tf.quaternion_multiply(m.getrotation(), tf.quaternion_about_axis(numpy.pi/2, [1,0,0])) + m.matrix = numpy.dot(m.getmatrix(), tf.rotation_matrix(numpy.pi/2, [1,0,0])) + m.trans = None + m.rot = None elif g.tag == 'sphere': m.shapeType = model.ShapeModel.SP_SPHERE m.data = model.SphereData()