def rotation(value, r):
q0 = quaternions.axangle2quat(
[float(value[0]), float(value[1]),
float(value[2])], float(value[3]))
q1 = quaternions.axangle2quat([r[0], r[1], r[2]], r[3])
qr = quaternions.qmult(q1, q0)
v, theta = quaternions.quat2axangle(qr)
return [
WebotsParser.str(v[0]),
WebotsParser.str(v[1]),
WebotsParser.str(v[2]),
WebotsParser.str(theta)
]
def convert_to_nue(filename):
world = WebotsParser()
world.load(filename)
for node in world.content['root']:
if node['name'] == 'WorldInfo':
for field in node['fields']:
if field['name'] == 'gravity':
gravity = float(field['value'][1])
if gravity != 0:
gravity = -gravity
field['value'] = WebotsParser.str(gravity)
field['type'] = 'SFFloat'
if field[
'name'] == 'coordinateSystem': # world file already updated
return
node['fields'].append({
'name': 'coordinateSystem',
'value': 'NUE',
'type': 'SFString'
})
elif node['name'] in converted_protos:
print('Rotating', node['name'])
rotation_found = False
for field in node['fields']:
if field['name'] in ['rotation']:
rotation_found = True
field['value'] = rotation(field['value'],
converted_protos[node['name']])
if not rotation_found:
node['fields'].append({
'name':
'rotation',
'value':
rotation(['0', '1', '0', '0'],
converted_protos[node['name']]),
'type':
'SFRotation'
})
world.save(filename)