def __init__(self, _id: str, name: str, parent_link: SDF.Link, child_link: SDF.Link, axis: SDF.math.Vector3, coordinates=None, motorized=False, position=None, rotation=None ): super().__init__( 'joint', { 'id': _id, 'name': name, 'type': 'revolute' }, position=position, rotation=rotation, ) self._id = _id self._name = name self._motorized = motorized self._coordinates = coordinates SDF.sub_element_text(self, 'parent', parent_link.name) SDF.sub_element_text(self, 'child', child_link.name) self.axis = JointAxis(axis) self.append(self.axis)
def to_robot_config_sdf(self): assert (self.is_motorized()) servomotor = xml.etree.ElementTree.Element('rv:servomotor', { 'type': 'position', 'id': "{}__rotate".format(self._id), 'part_id': self._id, 'part_name': self._name, # 'x': self.x, # 'y': self.y, 'joint': self._name, # noise: 0.1, }) if self._coordinates is not None: servomotor.attrib['coordinates'] = ';'.join(str(i) for i in self._coordinates) pid = xml.etree.ElementTree.SubElement(servomotor, 'rv:pid') SDF.sub_element_text(pid, 'rv:p', 0.9) SDF.sub_element_text(pid, 'rv:i', 0.0) SDF.sub_element_text(pid, 'rv:d', 0.0) SDF.sub_element_text(pid, 'rv:i_max', 0.0) SDF.sub_element_text(pid, 'rv:i_min', 0.0) # SDF.sub_element_text(pid, 'rv:cmd_max', 0.0) # SDF.sub_element_text(pid, 'rv:cmd_min', 0.0) return servomotor
def to_robot_config_sdf(self): assert (self.is_motorized()) servomotor = xml.etree.ElementTree.Element( 'rv:servomotor', { 'type': 'position', 'id': "{}__rotate".format(self._id), 'part_id': self._id, 'part_name': self._name, # 'x': self.x, # 'y': self.y, 'joint': self._name, # noise: 0.1, }) if self._coordinates is not None: servomotor.attrib['coordinates'] = ';'.join( str(i) for i in self._coordinates) pid = xml.etree.ElementTree.SubElement(servomotor, 'rv:pid') SDF.sub_element_text(pid, 'rv:p', 0.9) SDF.sub_element_text(pid, 'rv:i', 0.0) SDF.sub_element_text(pid, 'rv:d', 0.0) SDF.sub_element_text(pid, 'rv:i_max', 0.0) SDF.sub_element_text(pid, 'rv:i_min', 0.0) # SDF.sub_element_text(pid, 'rv:cmd_max', 0.0) # SDF.sub_element_text(pid, 'rv:cmd_min', 0.0) return servomotor
def __init__(self, _id: str, name: str, parent_link: SDF.Link, child_link: SDF.Link, axis: SDF.math.Vector3, coordinates=None, motorized=False, position=None, rotation=None): super().__init__( 'joint', { 'id': _id, 'name': name, 'type': 'revolute' }, position=position, rotation=rotation, ) self._id = _id self._name = name self._motorized = motorized self._coordinates = coordinates SDF.sub_element_text(self, 'parent', parent_link.name) SDF.sub_element_text(self, 'child', child_link.name) self.axis = JointAxis(axis) self.append(self.axis)
def __init__(self, box_size): """ :param box_size: list or tuple of 3 elements with the 3 sizes (x,y,z). """ super().__init__('geometry') self._box = (box_size[0], box_size[1], box_size[2]) mesh = xml.etree.ElementTree.SubElement(self, 'box') SDF.sub_element_text(mesh, 'size', '{:e} {:e} {:e}'.format(box_size[0], box_size[1], box_size[2]))
def __init__(self, name: str, collision_element, link, module): """ Constructor :param name: name of the sensor :param collision_element: name or reference of the collision element :type collision_element: str|SDF.Collision """ super().__init__(name, link, module) collision_element = collision_element if type(collision_element) is str else collision_element.name contact = xml.etree.ElementTree.SubElement(self, 'contact') SDF.sub_element_text(contact, 'collision', collision_element) # SDF.sub_element_text(contact, 'topic', 'topic_{}'.format(collision_element)) # SDF.sub_element_text(self, 'update_rate', 8.0) SDF.sub_element_text(self, 'always_on', True)
def __init__(self, name: str, collision_element, link, module): """ Constructor :param name: name of the sensor :param collision_element: name or reference of the collision element :type collision_element: str|SDF.Collision """ super().__init__(name, link, module) collision_element = collision_element if type( collision_element) is str else collision_element.name contact = xml.etree.ElementTree.SubElement(self, 'contact') SDF.sub_element_text(contact, 'collision', collision_element) # SDF.sub_element_text(contact, 'topic', 'topic_{}'.format(collision_element)) # SDF.sub_element_text(self, 'update_rate', 8.0) SDF.sub_element_text(self, 'always_on', True)
def __init__(self, name: str, self_collide=True, position=None, rotation=None): super().__init__( tag='link', attrib={'name': name}, position=position, rotation=rotation, ) self.name = name self.size = (0, 0, 0, 0, 0, 0) self.inertial = None self.collisions = [] self.joints = [] SDF.sub_element_text(self, 'self_collide', self_collide)
def __init__(self, ambient=(0, 0, 0, 1), diffuse=(0, 0, 0, 1), specular=(0, 0, 0, 1)): super().__init__('material') SDF.sub_element_text(self, 'ambient', '{} {} {} {}'.format(ambient[0], ambient[1], ambient[2], ambient[3])) SDF.sub_element_text(self, 'diffuse', '{} {} {} {}'.format(diffuse[0], diffuse[1], diffuse[2], diffuse[3])) SDF.sub_element_text(self, 'specular', '{} {} {} {}'.format(specular[0], specular[1], specular[2], specular[3]))
def __init__(self, axis: SDF.math.Vector3): super().__init__('axis') self.xyz = SDF.sub_element_text(self, 'xyz', '{:e} {:e} {:e}'.format(axis[0], axis[1], axis[2])) SDF.sub_element_text(self, 'use_parent_model_frame', '0') limit = xml.etree.ElementTree.SubElement(self, 'limit') # TODO calibrate this (load from configuration?) SDF.sub_element_text(limit, 'lower', -7.853982e-01) SDF.sub_element_text(limit, 'upper', 7.853982e-01) SDF.sub_element_text(limit, 'effort', 1.765800e-01) SDF.sub_element_text(limit, 'velocity', 5.235988e+00)
def __init__(self, mass, position=None, rotation=None, inertia_xx=1.0, inertia_xy=0.0, inertia_xz=0.0, inertia_yy=1.0, inertia_yz=0.0, inertia_zz=1.0): super().__init__('inertial', position=position, rotation=rotation) self.ixx, self.ixy, self.ixz = (inertia_xx, inertia_xy, inertia_xz) self.iyy, self.iyz = (inertia_yy, inertia_yz) self.izz = inertia_zz self.mass = mass SDF.sub_element_text(self, 'mass', mass) inertia = xml.etree.ElementTree.SubElement(self, 'inertia') SDF.sub_element_text(inertia, 'ixx', inertia_xx) SDF.sub_element_text(inertia, 'ixy', inertia_xy) SDF.sub_element_text(inertia, 'ixz', inertia_xz) SDF.sub_element_text(inertia, 'iyy', inertia_yy) SDF.sub_element_text(inertia, 'iyz', inertia_yz) SDF.sub_element_text(inertia, 'izz', inertia_zz)
def _sdf_brain_plugin_conf( robot_brain, sensors, actuators, robot_genome=None, update_rate: float = 8.0, controller_plugin: str = 'libRobotControlPlugin.so', ): """ Creates the plugin node with the brain configuration inside :param robot_brain: Brain of the robot to send to the simulator :param battery_level: :param update_rate: Update rate as used by the default controller :param controller_plugin: Name of the shared library of the model plugin :return: The sdf plugin element :rtype: xml.etree.ElementTree.Element """ plugin = xml.etree.ElementTree.Element( 'plugin', attrib={ 'name': 'robot_controller', 'filename': controller_plugin, 'xmlns:rv': 'https://github.com/ci-group/revolve', }) config = xml.etree.ElementTree.SubElement(plugin, 'rv:robot_config') # update rate SDF.sub_element_text(config, 'rv:update_rate', update_rate) # battery # if battery_level is not None: # battery = xml.etree.ElementTree.SubElement(config, 'rv:battery') # SDF.sub_element_text(battery, 'rv:level', battery_level) # brain robot_brain_sdf = xml.etree.ElementTree.SubElement(config, 'rv:brain') robot_learner = robot_brain.learner_sdf() if robot_learner is None: xml.etree.ElementTree.SubElement(robot_brain_sdf, 'rv:learner', {'type': 'None'}) else: robot_brain_sdf.append(robot_learner) robot_controller = robot_brain.controller_sdf() if robot_controller is None: xml.etree.ElementTree.SubElement(robot_brain_sdf, 'rv:controller', {'type': 'None'}) else: robot_brain_sdf.append(robot_controller) # sensors sensors_elem = xml.etree.ElementTree.SubElement(robot_brain_sdf, 'rv:sensors') for sensor in sensors: sensors_elem.append(sensor.to_robot_config_sdf()) # actuators actuators_elem = xml.etree.ElementTree.SubElement(robot_brain_sdf, 'rv:actuators') for actuator in actuators: actuators_elem.append(actuator.to_robot_config_sdf()) # robot genome if robot_genome is not None: SDF.sub_element_text(config, 'rv:genome', str(robot_genome)) return plugin
def __init__(self): super().__init__('surface') contact_tag = xml.etree.ElementTree.SubElement(self, 'contact') friction_tag = xml.etree.ElementTree.SubElement(self, 'friction') contact_ode_tag = xml.etree.ElementTree.SubElement(contact_tag, 'ode') SDF.sub_element_text(contact_ode_tag, 'kd', 10000000.0 / 3) SDF.sub_element_text(contact_ode_tag, 'kp', 90000) friction_ode_tag = xml.etree.ElementTree.SubElement(friction_tag, 'ode') SDF.sub_element_text(friction_ode_tag, 'mu', 1.0) SDF.sub_element_text(friction_ode_tag, 'mu2', 1.0) SDF.sub_element_text(friction_ode_tag, 'slip1', 0.01) SDF.sub_element_text(friction_ode_tag, 'slip2', 0.01) friction_bullet_tag = xml.etree.ElementTree.SubElement(friction_tag, 'bullet') SDF.sub_element_text(friction_bullet_tag, 'friction', 1.0) SDF.sub_element_text(friction_bullet_tag, 'friction2', 1.0)
def __init__(self, mesh_uri): super().__init__('geometry') mesh = xml.etree.ElementTree.SubElement(self, 'mesh') SDF.sub_element_text(mesh, 'uri', mesh_uri)
def __init__(self, name: str, width: int = 320, height: int = 240, horizontal_fov: float = 1.047, clip_near: float = 0.1, clip_far: float = 100.0, position=None, rotation=None): """ Constructor :param name: name of the sensor :param width: pixel width size of the camera :param height: pixel height size of the camera :param horizontal_fov: FOV on the horizontal axis (the vertical one is calculated from this and the canvas proportions) :param clip_near: Clipping near of the camera frustum :param clip_far: Clipping far of the camera frustum :param position: Position of the camera :param rotation: Rotation (orientation) of the camera """ super().__init__( name, 'camera', position, rotation ) camera = xml.etree.ElementTree.SubElement(self, 'camera') SDF.sub_element_text(camera, 'horizontal_fov', horizontal_fov) image = xml.etree.ElementTree.SubElement(camera, 'image') clip = xml.etree.ElementTree.SubElement(camera, 'clip') SDF.sub_element_text(image, 'width', width) SDF.sub_element_text(image, 'width', height) SDF.sub_element_text(clip, 'near', clip_near) SDF.sub_element_text(clip, 'far', clip_far) SDF.sub_element_text(self, 'always_on', True)
def __init__(self, axis: SDF.math.Vector3): super().__init__('axis') self.xyz = SDF.sub_element_text( self, 'xyz', '{:e} {:e} {:e}'.format(axis[0], axis[1], axis[2])) SDF.sub_element_text(self, 'use_parent_model_frame', '0') limit = xml.etree.ElementTree.SubElement(self, 'limit') # TODO calibrate this (load from configuration?) SDF.sub_element_text(limit, 'lower', -7.853982e-01) SDF.sub_element_text(limit, 'upper', 7.853982e-01) SDF.sub_element_text(limit, 'effort', 1.765800e-01) SDF.sub_element_text(limit, 'velocity', 5.235988e+00)
def __init__(self, name: str, width: int = 320, height: int = 240, horizontal_fov: float = 1.047, clip_near: float = 0.1, clip_far: float = 100.0, position=None, rotation=None): """ Constructor :param name: name of the sensor :param width: pixel width size of the camera :param height: pixel height size of the camera :param horizontal_fov: FOV on the horizontal axis (the vertical one is calculated from this and the canvas proportions) :param clip_near: Clipping near of the camera frustum :param clip_far: Clipping far of the camera frustum :param position: Position of the camera :param rotation: Rotation (orientation) of the camera """ super().__init__(name, 'camera', position, rotation) camera = xml.etree.ElementTree.SubElement(self, 'camera') SDF.sub_element_text(camera, 'horizontal_fov', horizontal_fov) image = xml.etree.ElementTree.SubElement(camera, 'image') clip = xml.etree.ElementTree.SubElement(camera, 'clip') SDF.sub_element_text(image, 'width', width) SDF.sub_element_text(image, 'width', height) SDF.sub_element_text(clip, 'near', clip_near) SDF.sub_element_text(clip, 'far', clip_far) SDF.sub_element_text(self, 'always_on', True)
def __init__(self, name: str, link, module): super().__init__(name, link, module) SDF.sub_element_text(self, 'always_on', True)
def _sdf_brain_plugin_conf( robot_brain, sensors, actuators, robot_genome=None, update_rate: float = 8.0, controller_plugin: str = 'libRobotControlPlugin.so', ): """ Creates the plugin node with the brain configuration inside :param robot_brain: Brain of the robot to send to the simulator :param battery_level: :param update_rate: Update rate as used by the default controller :param controller_plugin: Name of the shared library of the model plugin :return: The sdf plugin element :rtype: xml.etree.ElementTree.Element """ plugin = xml.etree.ElementTree.Element( 'plugin', attrib={ 'name': 'robot_controller', 'filename': controller_plugin, }) config = xml.etree.ElementTree.SubElement(plugin, 'rv:robot_config', { 'xmlns:rv': 'https://github.com/ci-group/revolve', }) # update rate SDF.sub_element_text(config, 'rv:update_rate', update_rate) # battery # if battery_level is not None: # battery = xml.etree.ElementTree.SubElement(config, 'rv:battery') # SDF.sub_element_text(battery, 'rv:level', battery_level) # brain robot_brain_sdf = xml.etree.ElementTree.SubElement(config, 'rv:brain') robot_learner = robot_brain.learner_sdf() if robot_learner is None: xml.etree.ElementTree.SubElement(robot_brain_sdf, 'rv:learner', {'type': 'None'}) else: robot_brain_sdf.append(robot_learner) robot_controller = robot_brain.controller_sdf() if robot_controller is None: xml.etree.ElementTree.SubElement(robot_brain_sdf, 'rv:controller', {'type': 'None'}) else: robot_brain_sdf.append(robot_controller) # sensors sensors_elem = xml.etree.ElementTree.SubElement(robot_brain_sdf, 'rv:sensors') for sensor in sensors: sensors_elem.append(sensor.to_robot_config_sdf()) # actuators actuators_elem = xml.etree.ElementTree.SubElement(robot_brain_sdf, 'rv:actuators') for actuator in actuators: actuators_elem.append(actuator.to_robot_config_sdf()) # robot genome if robot_genome is not None: SDF.sub_element_text(config, 'rv:genome', str(robot_genome)) return plugin
def __init__(self, mass, position=None, rotation=None, inertia_xx=1.0, inertia_xy=0.0, inertia_xz=0.0, inertia_yy=1.0, inertia_yz=0.0, inertia_zz=1.0): super().__init__('inertial', position=position, rotation=rotation) self.ixx, self.ixy, self.ixz = (inertia_xx, inertia_xy, inertia_xz) self.iyy, self.iyz = (inertia_yy, inertia_yz) self.izz = inertia_zz self.mass = mass SDF.sub_element_text(self, 'mass', mass) inertia = xml.etree.ElementTree.SubElement(self, 'inertia') SDF.sub_element_text(inertia, 'ixx', inertia_xx) SDF.sub_element_text(inertia, 'ixy', inertia_xy) SDF.sub_element_text(inertia, 'ixz', inertia_xz) SDF.sub_element_text(inertia, 'iyy', inertia_yy) SDF.sub_element_text(inertia, 'iyz', inertia_yz) SDF.sub_element_text(inertia, 'izz', inertia_zz)
def __init__(self, name: str, link, module): super().__init__(name, link, module) SDF.sub_element_text(self, 'always_on', True)