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)