def rmevo_bot_to_sdf(robot, robot_pose, nice_format, self_collide=True): from xml.etree import ElementTree from pyfactory import SDF from pyfactory.rmevo_module import Orientation, FactoryModule 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 if type(robot.body) == FactoryModule: core_slot = robot.body.SLOT_DATA[core_slot] slot_chain = robot.body.TYPE else: 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 _module_to_sdf(module, parent_link, parent_slot, 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: RMEvoModule :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 if module.is_joint is True: if module.has_children() is False: return links, joints, sensors, collisions child_module = module.children[0] child_link = SDF.Link('{}_{}'.format(child_module.TYPE, module.TYPE), self_collide=self_collide) child_visual, child_collision, imu_core_sensor = child_module.to_sdf( '', child_link) joint = SDF.Joint(module.id, module.TYPE, parent_link, child_link, module.axis) joint_parent_slot = module.SLOT_DATA[0] joint_child_slot = module.SLOT_DATA[1] child_slot = child_module.SLOT_DATA[0] joint.align(joint_parent_slot, parent_slot, parent_collision) _sdf_attach_module(child_slot, child_module.orientation, child_visual, child_collision, parent_slot, parent_collision) # To use when joints have visual sdf data # _sdf_attach_module(joint_parent_slot, module.orientation, # None, None, # parent_slot, parent_collision) # # _sdf_attach_module(child_slot, child_module.orientation, # child_visual, child_collision, # joint_child_slot, None) # 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 new_parent visuals child_link.append(child_visual) child_link.append(child_collision) # Add joint child_link.add_joint(joint) links.append(child_link) joints.append(joint) # update my_link and my_collision my_link = child_link module = child_module my_collision = child_collision else: visual, collision, sensor = module.to_sdf(slot_chain, my_link) module_slot = module.SLOT_DATA[0] _sdf_attach_module(module_slot, module.orientation, visual, collision, parent_slot, parent_collision) visual.set('name', 'Visual_{}'.format(module.id)) collision.set('name', 'Collisions_{}'.format(module.id)) 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 child_slot_chain = '{}{}'.format(slot_chain, my_slot) my_slot = module.SLOT_DATA[my_slot] 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