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_material_missing_color_and_texture(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>
</link>
</robot>'''
self.parse_and_compare(xml)
robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link',
visual=urdf.Visual(
geometry=urdf.Cylinder(length=1, radius=1),
material=urdf.Material(name='mat')))
robot.add_link(link)
self.xml_and_compare(robot, 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'))
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 add_rgbd_camera(robot, scale=1, parent_link_name="base_plate"):
_x = (0.033 / 2)
_y = (0.022 / 2)
_z = (0.103 / 2)
l_frame = urdf.Link(name=parent_link_name + "_camera",
visual=frame_visual(),
collision=frame_collision(),
inertial=frame_inertial())
l_frame.visual = frame_visual()
l_frame.collision = frame_collision()
robot.add_link(l_frame)
for link in robot.links:
if link.name == parent_link_name:
origin_xyz = link.inertial.origin.xyz
origin_xyz[2] += _z
origin_rpy = link.inertial.origin.rpy
j_world = urdf.Joint(name=l_frame.name + "_joint",
parent=parent_link_name,
child=l_frame.name,
joint_type="fixed",
origin=urdf.Pose(origin_xyz, origin_rpy))
robot.add_joint(j_world)
return (robot, l_frame.name)
def test_link_multiple_collision(self):
xml = '''<?xml version="1.0"?>
<robot name="test" version="1.0">
<link name="link">
<collision>
<geometry>
<cylinder length="1" radius="1"/>
</geometry>
</collision>
<collision>
<geometry>
<cylinder length="4" radius="0.5"/>
</geometry>
</collision>
</link>
</robot>'''
self.parse_and_compare(xml)
robot = urdf.Robot(name='test', version='1.0')
link = urdf.Link(name='link')
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)
self.xml_and_compare(robot, xml)
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 linkFromAffordance(aff):
geometry = geometryFromAffordance(aff)
material = materialFromAffordance(aff)
pose = poseFromAffordance(aff)
visual = urdf.Visual(geometry=geometry, material=material, origin=pose)
collision = urdf.Collision(geometry=geometry, origin=pose)
return urdf.Link(name=stringWithAffordanceId('link_%s', aff),
visual=visual,
collision=collision)
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 generate_robot(namespace):
""" Generate Inverted Pendulum URDF """
# Robot
robot = urdf.Robot(name)
# Base
l_world = urdf.Link("world")
robot.add_link(l_world)
# Frame
frame_geometry = urdf.Cylinder(radius=l_f_rad, length=l_f_len)
l_frame = urdf.Link("l_frame")
l_frame.collision = frame_collision(geometry=frame_geometry)
l_frame.inertial = frame_inertial()
l_frame.visual = frame_visual(geometry=frame_geometry)
robot.add_link(l_frame)
# World Joint
j_world = urdf.Joint(name="fixed",
parent="world",
child="l_frame",
joint_type="fixed",
origin=urdf.Pose([0, 0, 0], [0, 0, 0]))
robot.add_joint(j_world)
# Links
l_link = [urdf.Link("l_{}".format(i)) for i in range(n_links)]
j_link = [urdf.Joint("j_{}".format(i)) for i in range(n_links)]
for i in range(n_links):
l_y = (0.1 if i == 0 else 2 * l_link_rad)
l_z = ((l_f_len - 0.05) if i == 0 else (l_link_len - 0.05))
l_pos = urdf.Pose(xyz=[0, l_y, l_z], rpy=[0, 0, 0])
l_link[i].visual = default_visual()
l_link[i].inertial = default_inertial()
l_link[i].collision = default_collision()
robot.add_link(l_link[i])
j_link[i] = urdf.Joint(
name="j_{}".format(i),
parent=("l_frame" if i == 0 else "l_{}".format(i - 1)),
child="l_{}".format(i),
origin=l_pos,
joint_type="continuous",
axis=[0, 1, 0],
dynamics=default_dynamics())
robot.add_joint(j_link[i])
for joint in j_link:
# Transmission
t = urdf.Transmission(joint.name + "_trans")
t.type = "transmission_interface/SimpleTransmission"
transjoint = urdf.TransmissionJoint(name=joint.name)
transjoint.add_aggregate("hardwareInterface", "EffortJointInterface")
t.add_aggregate("joint", transjoint)
actuator = urdf.Actuator(joint.name + "_motor")
actuator.mechanicalReduction = 1
t.add_aggregate("actuator", actuator)
robot.add_aggregate("transmission", t)
# joint force-torque sensors
if use_ftSensors:
joints = get_continous_joints(robot)
for joint_i, joint in enumerate(joints):
# Provide feedback
a = etree.Element("gazebo", {"reference": joint.name})
b = etree.SubElement(a, "provideFeedback")
b.text = "true"
robot.add_aggregate("gazebo", a)
# Sensor
a = etree.Element("gazebo")
b = etree.SubElement(
a, "plugin", {
"name": "force_torque_plugin_" + joint.name,
"filename": "libgazebo_ros_ft_sensor.so"
})
c = etree.SubElement(b, "alwaysOn")
c.text = "true"
d = etree.SubElement(b, "updateRate")
d.text = "100"
d = etree.SubElement(b, "jointName")
d.text = joint.name
d = etree.SubElement(b, "topicName")
d.text = namespace + "ftSensors/" + joint.name
robot.add_aggregate("gazebo", a)
# Gazebo Color Plugin for all links
for link_i, link in enumerate(get_all_links(robot)):
a = etree.Element("gazebo", {"reference": link.name})
b = etree.SubElement(a, "material")
b.text = "Gazebo/Black"
robot.add_aggregate("gazebo", a)
# Gazebo Color Plugin for Frame
a = etree.Element("gazebo", {"reference": "l_frame"})
b = etree.SubElement(a, "material")
b.text = "Gazebo/Orange"
robot.add_aggregate("gazebo", a)
# Gazebo plugin
a = etree.Element("gazebo")
b = etree.SubElement(a, "plugin", {
"name": "gazebo_ros_control",
"filename": "libgazebo_ros_control.so"
})
c = etree.SubElement(b, "robotNamespace")
c.text = namespace
# d = etree.SubElement(b, "robotParam")
# d.text = "/robot_description"
robot.add_aggregate("gazebo", a)
return robot
def add_gazebo_plugins(robot,
namespace="/",
use_ft_sensors=False,
use_p3d_sensors=False,
fix_base_joint={
"child": "link_1",
"origin": {
"xyz": [0, 0, 0],
"rpy": [0, 0, 0]
}
},
use_rgbd_camera=None):
joints = get_all_joints(robot)
if fix_base_joint is not None:
l_world = urdf.Link("world")
robot.add_link(l_world)
origin = fix_base_joint.get("origin", 0)
if origin is not 0:
origin_xyz = origin.get("xyz")
origin_rpy = origin.get("rpy")
else:
origin_xyz = [0, 0, 0]
origin_rpy = [0, 0, 0]
child_frame = fix_base_joint.get("child", 0)
if child_frame is 0:
child_frame = "base_link"
j_world = urdf.Joint(name="fixed",
parent="world",
child=child_frame,
joint_type="fixed",
origin=urdf.Pose(origin_xyz, origin_rpy))
robot.add_joint(j_world)
for link_i, link in enumerate(get_all_links(robot)):
a = etree.Element("gazebo", {"reference": link.name})
b = etree.SubElement(a, "material")
b.text = "Gazebo/Grey"
robot.add_aggregate("gazebo", a)
if use_ftSensors:
joints = get_continous_joints(robot)
for joint_i, joint in enumerate(joints):
# Provide feedback
a = etree.Element("gazebo", {"reference": joint.name})
b = etree.SubElement(a, "provideFeedback")
b.text = "true"
c = etree.SubElement(a, "implicitSpringDamper")
c.text = "1"
robot.add_aggregate("gazebo", a)
# Sensor
a = etree.Element("gazebo")
b = etree.SubElement(
a, "plugin", {
"name": "force_torque_plugin_" + joint.name,
"filename": "libgazebo_ros_ft_sensor.so"
})
c = etree.SubElement(b, "alwaysOn")
c.text = "true"
d = etree.SubElement(b, "updateRate")
d.text = "100"
d = etree.SubElement(b, "jointName")
d.text = joint.name
d = etree.SubElement(b, "topicName")
d.text = namespace + "ftSensors/" + joint.name
robot.add_aggregate("gazebo", a)
if use_rgbd_camera is not None:
link_name = use_rgbd_camera.get("link")
camera_name = link_name + "_tof"
camera_frame_name = camera_name + "_optical_frame"
camera_link = urdf.Link(camera_frame_name)
robot.add_link(camera_link)
j_camera_frame = urdf.Joint(name=camera_frame_name + "_joint",
parent=link_name,
child=camera_frame_name,
joint_type="fixed",
origin=urdf.Pose(xyz=[0, 0, 0],
rpy=[0, 0, -np.pi / 2
])) #-np.pi / 2
robot.add_joint(j_camera_frame)
# TOF Sensor
a = etree.Element(
"gazebo",
{"reference": camera_frame_name}) # Try and automate this
b = etree.SubElement(a, "sensor", {"name": camera_name, "type": "ray"})
b1 = etree.SubElement(b, "visualize")
b1.text = "true"
c1 = etree.SubElement(b, "always_on")
c1.text = "true"
c = etree.SubElement(b, "update_rate")
c.text = "20.0"
d = etree.SubElement(b, "ray")
e = etree.SubElement(d, "scan")
g = etree.SubElement(e, "horizontal")
h1 = etree.SubElement(g, "samples")
h1.text = "16"
h2 = etree.SubElement(g, "resolution")
h2.text = "1"
h3 = etree.SubElement(g, "min_angle")
h3.text = str(-1.20428 / 2)
h4 = etree.SubElement(g, "max_angle")
h4.text = str(1.20428 / 2)
i = g = etree.SubElement(e, "vertical")
v1 = etree.SubElement(i, "samples")
v1.text = "16"
v2 = etree.SubElement(i, "resolution")
v2.text = "1"
v3 = etree.SubElement(i, "min_angle")
v3.text = str(-0.890118 / 2)
v4 = etree.SubElement(i, "max_angle")
v4.text = str(0.890118 / 2)
j = etree.SubElement(d, "range")
k = etree.SubElement(j, "min")
k.text = "0.2"
l = etree.SubElement(j, "max")
l.text = "5.0"
n = etree.SubElement(b, "plugin", {
"name": camera_name + "_plugin",
"filename": "libsim_tof.so"
})
o = etree.SubElement(n, "alwaysOn")
o.text = "true"
p = etree.SubElement(n, "updateRate")
p.text = "20.0"
q = etree.SubElement(n, "topicName")
q.text = namespace + camera_name + "/depth/points"
r = etree.SubElement(n, "frameName")
r.text = camera_frame_name
s = etree.SubElement(n, "zone")
t1 = etree.SubElement(s, "id")
t1.text = "0"
t2 = etree.SubElement(s, "startX")
t2.text = "0"
t3 = etree.SubElement(s, "startY")
t3.text = "0"
t4 = etree.SubElement(s, "endX")
t4.text = "16"
t5 = etree.SubElement(s, "endY")
t5.text = "16"
# Camera
u = etree.SubElement(a, "sensor", {
"name": camera_frame_name,
"type": "camera"
})
v = etree.SubElement(u, "update_rate")
v.text = "20.0"
w = etree.SubElement(u, "camera") #, {"name": link_name + "_image"})
x = etree.SubElement(w, "horizontal_fov")
x.text = str(1.20428)
y = etree.SubElement(w, "image")
y1 = etree.SubElement(y, "width")
y1.text = "640"
y2 = etree.SubElement(y, "height")
y2.text = "480"
y3 = etree.SubElement(y, "format")
y3.text = "R8G8B8"
z = etree.SubElement(w, "clip")
z1 = etree.SubElement(z, "near")
z1.text = "0.2"
z2 = etree.SubElement(z, "far")
z2.text = "5"
pg = etree.SubElement(
u, "plugin", {
"name": link_name + "_controller",
"filename": "libgazebo_ros_camera.so"
})
pg1 = etree.SubElement(pg, "alwaysOn")
pg1.text = "true"
pg2 = etree.SubElement(pg, "updateRate")
pg2.text = "0.0"
pg3 = etree.SubElement(pg, "cameraName")
pg3.text = link_name
pg4 = etree.SubElement(pg, "imageTopicName")
pg4.text = namespace + link_name + "/image_raw"
pg5 = etree.SubElement(pg, "cameraInfoTopicName")
pg5.text = namespace + link_name + "/camera_info"
pg6 = etree.SubElement(pg, "frameName")
pg6.text = camera_frame_name
pg7 = etree.SubElement(pg, "distortionK1")
pg7.text = "0.00000001"
pg8 = etree.SubElement(pg, "distortionK2")
pg8.text = "0.00000001"
pg9 = etree.SubElement(pg, "distortionK3")
pg9.text = "0.00000001"
pg10 = etree.SubElement(pg, "distortionT1")
pg10.text = "0.00000001"
pg11 = etree.SubElement(pg, "distortionT2")
pg11.text = "0.00000001"
pg12 = etree.SubElement(pg, "hackBaseline")
pg12.text = "0"
robot.add_aggregate("gazebo", a)
# Gazebo plugin
a = etree.Element("gazebo")
b = etree.SubElement(a, "plugin", {
"name": "gazebo_ros_control",
"filename": "libgazebo_ros_control.so"
})
c = etree.SubElement(b, "robotNamespace")
c.text = namespace
# d = etree.SubElement(b, "robotParam")
# d.text = "/robot_description"
robot.add_aggregate("gazebo", a)
return robot
def generate_robot(namespace):
robot = urdf.Robot("Camera")
ns = namespace
l_base = urdf.Link("base_link")
l_base.inertial = default_inertial()
robot.add_link(l_base)
l_camera = urdf.Link("camera_link")
l_camera.inertial = default_inertial()
l_camera.collision = default_collision()
l_camera.visual = default_visual()
robot.add_link(l_camera)
j_camera = urdf.Joint("camera_joint",
parent="base_link",
child="camera_link",
joint_type="fixed",
axis=[0, 1, 0],
origin=urdf.Pose([-1, 0, 0.5]))
robot.add_joint(j_camera)
a = etree.Element("gazebo", {"reference": "camera_link"})
b = etree.SubElement(a, "sensor", {"type": "camera", "name": "camera1"})
c = etree.SubElement(b, "update_rate")
c.text = "30.0"
d = etree.SubElement(b, "camera", {"name": "camera_main"})
r = etree.SubElement(d, "horizontal_fov")
r.text = "1.3962634"
e = etree.SubElement(d, "image")
f = etree.SubElement(e, "width")
f.text = "800"
g = etree.SubElement(e, "height")
g.text = "800"
h = etree.SubElement(e, "format")
h.text = "R8G8B8"
s = etree.SubElement(d, "clip")
t = etree.SubElement(s, "near")
t.text = "0.02"
w = etree.SubElement(s, "far")
w.text = "300"
i = etree.SubElement(b, "plugin", {
"name": "camera_controller",
"filename": "libgazebo_ros_camera.so"
})
j = etree.SubElement(i, "alwaysOn")
j.text = "true"
k = etree.SubElement(i, "updateRate")
k.text = "0.0"
l = etree.SubElement(i, "cameraName")
l.text = "camera1"
m = etree.SubElement(i, "imageTopicName")
m.text = ns + "camera/image_raw"
n = etree.SubElement(i, "cameraInfoTopicName")
n.text = ns + "camera/info"
o = etree.SubElement(i, "frameName")
o.text = "camera_link"
p = etree.SubElement(a, "static")
p.text = "true"
q = etree.SubElement(a, "turnGravityOff")
q.text = "true"
robot.add_aggregate("gazebo", a)
return robot
