Skip to content

Commit

Permalink
Merge pull request #31 from yosuke/master
Browse files Browse the repository at this point in the history 
support for choreonoid collision function
  • Loading branch information
@fkanehiro
fkanehiro committed Aug 18, 2015
2 parents d2febf6 + c88aff2 commit 549d682
Show file tree
Hide file tree
Showing 5 changed files with 118 additions and 60 deletions.
14 changes: 13 additions & 1 deletion doc/locale/ja/LC_MESSAGES/usage.po
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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 <[email protected]>\n"
"Language-Team: none\n"
"MIME-Version: 1.0\n"
Expand Down Expand Up @@ -106,6 +106,18 @@ msgstr "URDFフォーマットからVRMLフォーマットへの変換"
msgid "Convert VRML model to SDF format"
msgstr "VRMLフォーマットからSDFフォーマットへの変換"

#: ../../<autoprogram>:1
msgid "use collision shape when converting to VRML"
msgstr "VRMLへの変換時にcollision形状を使う"

#: ../../<autoprogram>: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）形式への変換"

Expand Down
1 change: 1 addition & 0 deletions simtrans/cli.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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')

Expand Down
4 changes: 3 additions & 1 deletion simtrans/sdf.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down
111 changes: 74 additions & 37 deletions simtrans/template/vrml.wrl
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -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 %}
Expand All @@ -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) }}
Expand Down
48 changes: 27 additions & 21 deletions simtrans/vrml.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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:
Expand All @@ -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:
Expand Down Expand Up @@ -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()
Expand Down Expand Up @@ -498,6 +503,7 @@ def renderchildren(self, mdata, root, jointtype, fname, template):
'joints': joints,
'jointmap': jointmap,
'ShapeModel': model.ShapeModel,
'shapefilemap': shapefilemap,
'options': self._options
}))

Expand Down

0 comments on commit 549d682

Please sign in to comment.