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' })