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)