def test_xml_and_urdfdom_robot_only_supported_since_melodic(self):
robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link')
link.add_aggregate(
'visual',
urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
material=urdf.Material(name='mat')))
link.add_aggregate(
'visual',
urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
material=urdf.Material(name='mat2')))
link.add_aggregate(
'collision',
urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1)))
link.add_aggregate(
'collision',
urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5)))
robot.add_link(link)
link = urdf.Link(name='link2')
robot.add_link(link)
#
robot_xml_string = robot.to_xml_string()
robot_xml = minidom.parseString(robot_xml_string)
orig_xml = minidom.parseString(self.xml)
self.assertTrue(xml_matches(robot_xml, orig_xml))
def test_link_multiple_visual(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
<link name="link">
<visual>
<geometry>
<cylinder length="1" radius="1"/>
</geometry>
<material name="mat"/>
</visual>
<visual>
<geometry>
<cylinder length="4" radius="0.5"/>
</geometry>
<material name="mat2"/>
</visual>
</link>
</robot>'''
self.parse_and_compare(xml)
robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link')
link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
material=urdf.Material(name='mat'))
link.visual = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
material=urdf.Material(name='mat2'))
robot.add_link(link)
self.xml_and_compare(robot, xml)
def from_world_body(cls, world_body, *args, **kwargs):
"""
:type world_body: giskard_msgs.msg._WorldBody.WorldBody
:rtype: URDFObject
"""
links = []
joints = []
if world_body.type == world_body.PRIMITIVE_BODY or world_body.type == world_body.MESH_BODY:
if world_body.shape.type == world_body.shape.BOX:
geometry = up.Box(world_body.shape.dimensions)
elif world_body.shape.type == world_body.shape.SPHERE:
geometry = up.Sphere(world_body.shape.dimensions[0])
elif world_body.shape.type == world_body.shape.CYLINDER:
geometry = up.Cylinder(
world_body.shape.dimensions[
world_body.shape.CYLINDER_RADIUS],
world_body.shape.dimensions[
world_body.shape.CYLINDER_HEIGHT])
elif world_body.shape.type == world_body.shape.CONE:
raise TypeError(u'primitive shape cone not supported')
elif world_body.type == world_body.MESH_BODY:
geometry = up.Mesh(world_body.mesh)
else:
raise CorruptShapeException(
u'primitive shape \'{}\' not supported'.format(
world_body.shape.type))
# FIXME test if this works on 16.04
try:
link = up.Link(world_body.name)
link.add_aggregate(
u'visual',
up.Visual(geometry,
material=up.Material(u'green',
color=up.Color(0, 1, 0,
1))))
link.add_aggregate(u'collision', up.Collision(geometry))
except AssertionError:
link = up.Link(world_body.name,
visual=up.Visual(geometry,
material=up.Material(
u'green',
color=up.Color(0, 1, 0,
1))),
collision=up.Collision(geometry))
links.append(link)
elif world_body.type == world_body.URDF_BODY:
o = cls(world_body.urdf, *args, **kwargs)
o.set_name(world_body.name)
return o
else:
raise CorruptShapeException(
u'world body type \'{}\' not supported'.format(
world_body.type))
return cls.from_parts(world_body.name, links, joints, *args, **kwargs)
def default_visual(geometry=None, origin=None):
if geometry is None:
geometry = urdf.Cylinder(radius=0.05, length=n_len)
if origin is None:
origin = urdf.Pose(xyz=[0, 0, n_len / 2], rpy=[0, 0, 0])
return urdf.Visual(geometry=geometry,
origin=origin,
material=urdf.Material("Gazebo/Black"))
def frame_visual(geometry=None, origin=None):
if geometry is None:
geometry = urdf.Cylinder(radius=l_f_rad, length=l_f_len)
if origin is None:
origin = urdf.Pose(xyz=[0, 0, l_f_len / 2], rpy=[0, 0, 0])
return urdf.Visual(geometry=geometry,
origin=origin,
material=urdf.Material(name="Gazebo/Orange"))
def test_xml_and_urdfdom_robot_compatible_with_kinetic(self):
robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link')
link.visual = urdf.Visual(geometry=urdf.Cylinder(length=1, radius=1),
material=urdf.Material(name='mat'))
link.visual = urdf.Visual(geometry=urdf.Cylinder(length=4, radius=0.5),
material=urdf.Material(name='mat2'))
link.collision = urdf.Collision(geometry=urdf.Cylinder(length=1, radius=1))
link.collision = urdf.Collision(geometry=urdf.Cylinder(length=4, radius=0.5))
robot.add_link(link)
link = urdf.Link(name='link2')
robot.add_link(link)
#
robot_xml_string = robot.to_xml_string()
robot_xml = minidom.parseString(robot_xml_string)
orig_xml = minidom.parseString(self.xml)
self.assertTrue(xml_matches(robot_xml, orig_xml))
def test_robot_material(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
<material name="mat">
<color rgba="0.0 0.0 0.0 1.0"/>
</material>
</robot>'''
self.parse_and_compare(xml)
robot = urdf.Robot(name='test', version='1.0')
material = urdf.Material(name='mat', color=urdf.Color([0.0, 0.0, 0.0, 1.0]))
robot.add_aggregate('material', material)
self.xml_and_compare(robot, xml)
def from_object_state(cls, object_state, *args, **kwargs):
"""
:type world_body: knowrob_objects.msg._ObjectState.ObjectState
:rtype: URDFObject
"""
links = []
joints = []
shape = [object_state.size.y,
object_state.size.x,
object_state.size.z]
if object_state.has_visual and object_state.mesh_path == u'':
geometry = up.Box(shape)
elif object_state.has_visual:
geometry = up.Mesh(object_state.mesh_path)
else:
raise CorruptShapeException(u'object state has no visual')
link = up.Link(object_state.object_id,
visual=up.Visual(geometry, material=up.Material(u'green', color=up.Color(0, 1, 0, 1))),
collision=up.Collision(geometry))
links.append(link)
return cls.from_parts(object_state.object_id, links, joints, *args, **kwargs)
def materialFromAffordance(aff):
color = colorFromAffordance(aff)
return urdf.Material(name=stringWithAffordanceId('material_%s', aff),
color=color,
texture=None)
def black():
col_black = urdf.Material(name="black", color=urdf.Color([0, 0, 0, 1.0]))
return col_black
def orange():
col_orange = urdf.Material(
name="orange",
color=urdf.Color([1.0, 0.423529411765, 0.0392156862745, 1.0]))
return col_orange
def white():
col_white = urdf.Material(name="white",
color=urdf.Color([1.0, 1.0, 1.0, 1.0]))
return col_white
def default_visual():
return urdf.Visual(geometry=urdf.Box([0.05, 0.05, 0.05]),
material=urdf.Material(name="red"),
origin=urdf.Pose([-1, 0, 0.5], [0, 0, 0]))
