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, 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, 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 to_sdf(self, pose=SDF.math.Vector3(0, 0, 0.25), nice_format=None): if type(nice_format) is bool: nice_format = '\t' if nice_format else None return SDF.revolve_bot_to_sdf(self, pose, nice_format, self_collide=self.self_collide)
def to_sdf(self, tree_depth='', parent_link=None, child_link=None): assert (parent_link is not None) name = 'component_{}_{}'.format(tree_depth, self.TYPE) name_sensor = 'sensor_{}_{}'.format(tree_depth, self.TYPE) visual = SDF.Visual(name, self.rgb) geometry = SDF.MeshGeometry(self.VISUAL_MESH) visual.append(geometry) collision = SDF.Collision(name, self.MASS) geometry = SDF.BoxGeometry(self.COLLISION_BOX) # collision.translate(SDF.math.Vector3(0.01175, 0.001, 0)) collision.append(geometry) sensor = SDF.TouchSensor(name_sensor, collision, parent_link, self) parent_link.append(sensor) return visual, collision, sensor
def get_inertial(self): """ Return solid box inertial """ r = self.mass / 12.0 x, y, z = self._box_geometry ixx = r * (y ** 2 + z ** 2) iyy = r * (x ** 2 + z ** 2) izz = r * (x ** 2 + y ** 2) return SDF.Inertial(mass=self.mass, inertia_xx=ixx, inertia_yy=iyy, inertia_zz=izz)
def to_sdf(self, tree_depth='', parent_link=None, child_link=None): """ Transform the module in sdf elements. IMPORTANT: It does not append VISUAL and COLLISION elements to the parent link automatically. It does append automatically the SENSOR element. TODO: make the append automatic for VISUAL AND COLLISION AS WELL. :param tree_depth: current tree depth as string (for naming) :param parent_link: link of the parent (may be needed for certain modules) :param child_link: link of the child (may be needed for certain modules, like hinges) :return: visual SDF element, collision SDF element, sensor SDF element. Sensor SDF element may be None. """ name = 'component_{}_{}__box'.format(tree_depth, self.TYPE) visual = SDF.Visual(name, self.rgb) geometry = SDF.MeshGeometry(self.VISUAL_MESH) visual.append(geometry) collision = SDF.Collision(name, self.MASS) geometry = SDF.BoxGeometry(self.COLLISION_BOX) collision.append(geometry) return visual, collision, None
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, 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 to_sdf(self, tree_depth='', parent_link=None, child_link=None): assert (parent_link is not None) assert (child_link is not None) name_frame = 'component_{}_{}__frame'.format(tree_depth, self.TYPE) name_joint = 'component_{}_{}__joint'.format(tree_depth, self.TYPE) name_servo = 'component_{}_{}__servo'.format(tree_depth, self.TYPE) name_servo2 = 'component_{}_{}__servo2'.format(tree_depth, self.TYPE) visual_frame = SDF.Visual(name_frame, self.rgb) geometry = SDF.MeshGeometry(self.VISUAL_MESH_FRAME) visual_frame.append(geometry) collision_frame = SDF.Collision(name_frame, self.MASS_FRAME) geometry = SDF.BoxGeometry(self.COLLISION_BOX_FRAME) collision_frame.append(geometry) visual_servo = SDF.Visual(name_servo, self.rgb) geometry = SDF.MeshGeometry(self.VISUAL_MESH_SERVO) visual_servo.append(geometry) collision_servo = SDF.Collision(name_servo, self.MASS_SERVO) collision_servo.translate(SDF.math.Vector3(0.002375, 0, 0)) geometry = SDF.BoxGeometry(self.COLLISION_BOX_SERVO) collision_servo.append(geometry) collision_servo_2 = SDF.Collision(name_servo2, 0) collision_servo_2.translate(SDF.math.Vector3(0.01175, 0.001, 0)) geometry = SDF.BoxGeometry(self.COLLISION_BOX_SERVO_2) collision_servo_2.append(geometry) joint = SDF.Joint(self.id, name_joint, parent_link, child_link, axis=SDF.math.Vector3(0, 1, 0), coordinates=self.substrate_coordinates, motorized=True) joint.set_position(SDF.math.Vector3(-0.0085, 0, 0)) return visual_frame, \ [collision_frame], \ visual_servo, \ [collision_servo, collision_servo_2], \ joint
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 _module_to_sdf(module, parent_link, parent_slot: BoxSlot, parent_collision, slot_chain, self_collide): """ Recursive function that takes a module and returns a list of SDF links and joints that that module and his children have generated. :param module: Module to parse :type module: RevolveModule :param parent_link: SDF `Link` of the parent :param parent_slot: Slot of the parent which this module should attach to :param parent_collision: Parent collision box, needed for the alignment. :param slot_chain: Text that names the joints, it encodes the path that was made to arrive to that element. :return: """ links = [] joints = [] sensors = [] collisions = [] my_link = parent_link my_collision = None # ACTIVE HINGE if type(module) is ActiveHingeModule: child_link = SDF.Link('{}_Leg'.format(slot_chain), self_collide=self_collide) visual_frame, collisions_frame, \ visual_servo, collisions_servo, \ joint = module.to_sdf('{}'.format(slot_chain), parent_link, child_link) # parent_slot = parent_module.boxslot(parent_slot) module_slot = module.boxslot_frame(Orientation.SOUTH) _sdf_attach_module(module_slot, module.orientation, visual_frame, collisions_frame[0], parent_slot, parent_collision) parent_slot = module.boxslot_frame(Orientation.NORTH) module_slot = module.boxslot_servo(Orientation.SOUTH) _sdf_attach_module(module_slot, None, visual_servo, collisions_servo[0], parent_slot, collisions_frame[0]) joint.set_rotation(visual_servo.get_rotation()) old_position = joint.get_position() joint.set_position(visual_servo.get_position()) joint.translate(joint.to_parent_direction(old_position)) # Add visuals and collisions for Servo Frame block parent_link.append(visual_frame) for i, collision_frame in enumerate(collisions_frame): parent_link.append(collision_frame) collisions.append(collision_frame) if i != 0: old_pos = collision_frame.get_position() collision_frame.set_rotation(visual_frame.get_rotation()) collision_frame.set_position(visual_frame.get_position()) collision_frame.translate(collision_frame.to_parent_direction(old_pos)) # Add visuals and collisions for Servo block child_link.append(visual_servo) for i, collision_servo in enumerate(collisions_servo): child_link.append(collision_servo) collisions.append(collision_servo) if i != 0: old_pos = collision_servo.get_position() collision_servo.set_position(collisions_servo[0].get_position()) collision_servo.set_rotation(collisions_servo[0].get_rotation()) collision_servo.translate(collision_servo.to_parent_direction(old_pos)) # Add joint child_link.add_joint(joint) links.append(child_link) joints.append(joint) # update my_link and my_collision my_link = child_link my_collision = collisions_servo[0] # OTHERS else: visual, collision, sensor = module.to_sdf(slot_chain, my_link) module_slot = module.boxslot(Orientation.SOUTH) _sdf_attach_module(module_slot, module.orientation, visual, collision, parent_slot, parent_collision) parent_link.append(visual) parent_link.append(collision) collisions.append(collision) my_collision = collision if sensor is not None: sensors.append(sensor) # RECURSION ON CHILDREN for my_slot, child_module in module.iter_children(): if child_module is None: continue my_slot = module.boxslot(Orientation(my_slot)) child_slot_chain = '{}{}'.format(slot_chain, my_slot.orientation.short_repr()) children_links, \ children_joints, \ children_sensors, \ children_collisions = _module_to_sdf(child_module, my_link, my_slot, my_collision, child_slot_chain, self_collide) links.extend(children_links) joints.extend(children_joints) sensors.extend(children_sensors) collisions.extend(children_collisions) return links, joints, sensors, collisions
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 _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 _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, mesh_uri): super().__init__('geometry') mesh = xml.etree.ElementTree.SubElement(self, 'mesh') SDF.sub_element_text(mesh, 'uri', mesh_uri)
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 to_sdf(self, tree_depth='', parent_link=None, child_link=None): imu_sensor = SDF.IMUSensor('core-imu_sensor', parent_link, self) visual, collision, _ = super().to_sdf(tree_depth, parent_link, child_link) parent_link.append(imu_sensor) return visual, collision, imu_sensor
def revolve_bot_to_sdf(robot, robot_pose, nice_format, self_collide=True): from xml.etree import ElementTree from pyrevolve import SDF sdf_root = ElementTree.Element('sdf', {'version': '1.6'}) assert (robot.id is not None) model = ElementTree.SubElement(sdf_root, 'model', { 'name': str(robot.id) }) pose_elem = SDF.Pose(robot_pose) model.append(pose_elem) core_link = SDF.Link('Core', self_collide=self_collide) core_visual, core_collision, imu_core_sensor = robot._body.to_sdf('', core_link) links = [core_link] joints = [] actuators = [] sensors = [imu_core_sensor] collisions = [core_collision] core_link.append(core_visual) core_link.append(core_collision) for core_slot, child_module in robot._body.iter_children(): if child_module is None: continue core_slot = robot._body.boxslot(Orientation(core_slot)) slot_chain = core_slot.orientation.short_repr() children_links, \ children_joints, \ children_sensors, \ children_collisions = _module_to_sdf(child_module, core_link, core_slot, core_collision, slot_chain, self_collide) links.extend(children_links) joints.extend(children_joints) sensors.extend(children_sensors) collisions.extend(children_collisions) for joint in joints: model.append(joint) if joint.is_motorized(): actuators.append(joint) for link in links: link.align_center_of_mass() link.calculate_inertial() model.append(link) # ADD BRAIN plugin_elem = _sdf_brain_plugin_conf(robot._brain, sensors, actuators, robot_genome=None) model.append(plugin_elem) # XML RENDER PHASE # def prettify(rough_string, indent='\t'): """Return a pretty-printed XML string for the Element. """ import xml.dom.minidom reparsed = xml.dom.minidom.parseString(rough_string) return reparsed.toprettyxml(indent=indent) res = xml.etree.ElementTree.tostring(sdf_root, encoding='utf8', method='xml') if nice_format is not None: res = prettify(res, nice_format) else: res = res.decode() return res
def __init__(self, name: str, link, module): super().__init__(name, link, module) SDF.sub_element_text(self, 'always_on', True)
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, 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)