def _appearance(self, e):
fields = {
'metalness': 0.0,
'roughness': 1.0
}
if e.emission and isinstance(e.emission, tuple):
fields['emissiveColor'] = ' '.join(str(a) for a in e.emission[:3])
if e.specular and isinstance(e.specular, tuple):
s = e.specular
roughness = 1.0 - s[3] * (s[0] + s[1] + s[2]) / 3.0
fields['roughness'] = roughness
if e.shininess:
fields['roughness'] *= (1.0 - 0.5 * e.shininess)
if e.diffuse:
if isinstance(e.diffuse, collada.material.Map):
texture_path = Path(e.diffuse.sampler.surface.image.path)
if not texture_path.is_absolute():
texture_path = Path(self.path).parent / texture_path
fields['baseColorMap'] = Node('ImageTexture', {
'url': f'"{texture_path}"'
})
else:
fields['baseColor'] = ' '.join(e.diffuse[:3])
fields['transparency'] = 1.0 - e.diffuse[3]
return Node('PBRAppearance', fields)
def wheel(fr, lr):
return Node(
'Solid', {
'children': [
Node(
'Transform', {
'children': [Collada(str(mesh_path / 'wheel.dae'))],
'scale':
f'{physical[f"{fr}_wheel"]["radius"]} {physical[f"{fr}_wheel"]["width"]} {physical[f"{fr}_wheel"]["radius"]}',
'rotation': f'0.0 0.0 1.0 {0 if lr == "left" else pi}'
})
],
'physics':
Node(
'Physics', {
'centerOfMass': [vector(physical[f'{fr}_wheel']['com'])],
'inertiaMatrix': diagmat(
physical[f'{fr}_wheel']['inertial']),
'density': -1,
'mass': physical[f'{fr}_wheel']['mass']
}),
'boundingObject':
Node(
'Cylinder', {
'radius': physical[f'{fr}_wheel']['radius'],
'height': physical[f'{fr}_wheel']['width'],
'subdivision': 24
}, True),
'name':
f'"{fr}_{lr}_wheel"'
})
def _shape(self):
shapes = []
c = collada.Collada(self.path)
for g in c.geometries:
for p in g.primitives:
if not isinstance(p, collada.lineset.LineSet):
fields = {}
fields['geometry'] = self._indexed_face_set(p)
if p.material:
material = c.materials[p.material]
if material and material.effect:
appearance = self._appearance(material.effect)
fields['appearance'] = appearance
shapes.append(Node('Shape', fields))
if len(shapes) == 1:
return shapes[0]
return Node('Group', {
'children': shapes
})
def piksi(lr):
imu_transformation_fields = {
'translation': vector(model[f'{lr}_piksi_imu']['position']),
'rotation': rotation(model[f'{lr}_piksi_imu']['rotation'])
}
return [
Node(
'GPS', {
'name': f'"{lr}_piksi_gnss"',
'translation': vector(model[f'{lr}_piksi_gnss']['position']),
'accuracy': model[f'{lr}_piksi_gnss']['accuracy']
}),
Node('Solid', {
'name': f'"{lr}_piksi_imu"',
**imu_transformation_fields
}),
Node('Gyro', {
'name': f'"{lr}_piksi_gyro"',
**imu_transformation_fields
}, True),
Node('Accelerometer', {
'name': f'"{lr}_piksi_accelerometer"',
**imu_transformation_fields
}, True)
]
def front_wheel(lr):
return Node(
'Transform', {
'children': [
Node(
'Hinge2Joint', {
'endPoint':
wheel('front', lr),
'jointParameters':
Node('HingeJointParameters', {'axis': '0.0 0.0 1.0'}),
'jointParameters2':
Node('JointParameters', {'axis': '0.0 1.0 0.0'}),
'device': [
Node(
'RotationalMotor', {
'name':
f'"front_{lr}_steering_motor"',
'maxTorque':
physical['max_steering_torque'],
'maxVelocity':
physical['max_steering_velocity'],
'acceleration':
physical['max_steering_acceleration'],
'sound':
'""'
})
]
})
],
'translation':
f'{physical["wheelbase"]} {(1 if lr == "left" else -1) * physical["front_track"] / 2.0} {physical["front_wheel"]["radius"]}'
})
def realsense(fr):
return Node(
'Transform',
{
'children': [
Node(
'Transform',
{
'children': [
Node(
'RangeFinder', {
'name':
f'"{fr}_realsense_aligned_depth_to_color"',
'fieldOfView':
model[f'{fr}_realsense']['color']['fov'],
'width':
model[f'{fr}_realsense']['color']['width'],
'height':
model[f'{fr}_realsense']['color']
['height'],
'maxRange':
model[f'{fr}_realsense']['depth']
['max_range']
}),
Node(
'Camera', {
'name':
f'"{fr}_realsense_color_image_raw"',
'fieldOfView':
model[f'{fr}_realsense']['color']['fov'],
'width':
model[f'{fr}_realsense']['color']['width'],
'height':
model[f'{fr}_realsense']['color']
['height'],
})
],
# Webots camera has -z forward, x right, y up
'rotation':
f'{1.0 / sqrt(3)} {-1.0 / sqrt(3)} {-1.0 / sqrt(3)} {2.0 * pi / 3}'
},
True),
Node('Solid', {'name': f'"camera/{fr}_link"'})
],
'translation':
vector(model[f'{fr}_realsense']['position']),
'rotation':
rotation(model[f'{fr}_realsense']['rotation'])
})
def _indexed_face_set(self, p):
def terminate(a):
"""Flattens a matrix of coordinates terminating each tuple with -1
:param a: A matrix where each row represents a tuple of coordinates
:return: A flat list where each row in the input matrix follows
after the next, but where every row has been terminated with -1
to separate out rows
"""
return np.hstack((a, np.full((a.shape[0], 1), -1))).flatten()
def vec2str(v):
return ' '.join(f'{a:.6}' for a in v)
return Node('IndexedFaceSet', {
'coord': Node('Coordinate', {
'point': [
', '.join(vec2str(v) for v in p.vertex)
]
}),
'normal': Node('Normal', {
'vector': [
', '.join(vec2str(v) for v in p.normal)
]
}),
'texCoord': Node('TextureCoordinate', {
'point': [
', '.join(vec2str(v) for v in p.texcoordset[0])
]
}),
'coordIndex': [
', '.join(str(v) for v in terminate(p.vertex_index))
],
'normalIndex': [
', '.join(str(v) for v in terminate(p.normal_index))
],
'texCoordIndex': [
', '.join(str(v) for v in terminate(p.texcoord_indexset[0]))
],
'convex': 'FALSE'
})
def rear_wheel(lr):
return Node(
'Transform', {
'children': [
Node(
'HingeJoint', {
'endPoint':
wheel('rear', lr),
'jointParameters':
Node('HingeJointParameters', {
'axis': '0.0 1.0 0.0',
}),
'device': [
Node(
'RotationalMotor', {
'name':
f'"rear_{lr}_wheel_motor"',
'maxTorque':
physical['max_drive_torque'],
'acceleration':
physical['max_acceleration'] /
physical['rear_wheel']['radius'],
'sound':
'""'
}),
Node(
'PositionSensor', {
'name': f'"{lr}_wheel_encoder"',
'noise':
model[f'{lr}_wheel_encoder']['noise']
})
]
})
],
'translation':
f'0.0 {(1 if lr == "left" else -1) * physical["rear_track"] / 2.0} {physical["rear_wheel"]["radius"]}'
})
def _urdf(self, trans, parent):
return Node('Mesh', {
'url': Path(self.path).absolute().as_uri()
})._urdf(trans, parent)
def twizy():
return Node(
'Robot',
{
'children': [
Node(
'Solid', {
'children': [
Node(
'Transform', {
'children':
[Collada(str(mesh_path / 'chassis.dae'))],
'scale':
' '.join(
str(physical['chassis'][i])
for i in ['length', 'width', 'height'])
}),
*piksi('left'),
*piksi('right'),
Node(
'Transform', {
'children': [
Node(
'Lidar', {
'name':
'"lidar_vlp16_webots"',
'horizontalResolution':
model['lidar_vlp16']
['horizontal_resolution'],
'fieldOfView':
2 * pi,
'verticalFieldOfView':
model['lidar_vlp16']
['vertical_fov'],
'numberOfLayers':
model['lidar_vlp16']
['number_of_layers'],
'maxRange':
model['lidar_vlp16']
['max_range'],
'type':
'"rotating"',
'defaultFrequency':
model['lidar_vlp16']
['frequency'],
'rotation':
f'1.0 0.0 0.0 {pi / 2}'
}),
Node(
'Solid', {
'name': '"lidar_vlp16"',
'rotation': '0.0 0.0 1.0 4.36'
})
],
'translation':
vector(model['lidar_vlp16']['position']),
'rotation':
rotation(model['lidar_vlp16']['rotation']),
}),
realsense('front'),
realsense('rear'),
],
'physics':
Node(
'Physics', {
'centerOfMass':
[vector(physical['chassis']['com'])],
'inertiaMatrix':
diagmat(physical['chassis']['intertial']),
'density':
-1,
'mass':
physical['chassis']['mass'] - robot_mass
}),
'boundingObject':
Node(
'Transform', {
'children': [
Node(
'Box', {
'size':
' '.join(
str(physical['chassis'][i])
for i in
['length', 'width', 'height'])
}, True),
],
'translation':
f'{physical["front_overhang"] - (physical["chassis"]["length"] - physical["wheelbase"]) * 0.5} 0.0 {physical["chassis"]["height"] * 0.5}'
}),
'translation':
f'{physical["wheelbase"] / 2.0} 0.0 {physical["ground_clearance"]}',
'rotation':
f'0.0 1.0 0.0 {physical["rake"]}'
}),
front_wheel('left'),
front_wheel('right'),
rear_wheel('left'),
rear_wheel('right'),
],
# Dummy physics property. The robot solid is rigedly fixed to the
# chassis solid so in the simulation they will still only be viewed as
# one object. In order for the robot to have a mobile base thoght the
# physics field of the robot node needs to be specified. We could use a
# small dummy mass like how we use a sphere with tiny radius as
# boundingObject below, however webots warns agaist this in console as
# the simulation could in theory become unstable with large mass
# differences between nodes. However in this case that is not relevant
# since the solids are joined into one in the simulation layer anyways.
# To stop warnings beeing printed to the webots console, we split the
# mass between the robot node and the chassis node
'physics':
Node('Physics', {
'density': -1,
'mass': robot_mass
}),
# Dummy bounding object, a bounding object is needed to set the physics
# property but we are only really interested in the boundingObjects of
# chassis and wheels for example and the base robot node does in this
# case not correspond to any physical part with mass and bounds
'boundingObject':
Node('Sphere', {'radius': 0.001}, True),
'synchronization':
'FALSE',
'supervisor':
'TRUE',
'model':
'"Autonomous Twizy"',
'translation':
'IS translation',
'rotation':
'IS rotation',
'controller':
'IS controller',
'controllerArgs':
'IS controllerArgs'
})