def default_collision(geometry=None, origin=None):
""" Return default collision """
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.Collision(geometry=geometry, origin=origin)
def attach_urdf_object(self, urdf_object, parent_link, pose, round_to=3):
"""
Rigidly attach another object to the robot.
:param urdf_object: Object that shall be attached to the robot.
:type urdf_object: URDFObject
:param parent_link_name: Name of the link to which the object shall be attached.
:type parent_link_name: str
:param pose: Hom. transform between the reference frames of the parent link and the object.
:type pose: Pose
"""
if urdf_object.get_name() in self.get_link_names():
raise DuplicateNameException(
u'\'{}\' already has link with name \'{}\'.'.format(
self.get_name(), urdf_object.get_name()))
if urdf_object.get_name() in self.get_joint_names():
raise DuplicateNameException(
u'\'{}\' already has joint with name \'{}\'.'.format(
self.get_name(), urdf_object.get_name()))
if parent_link not in self.get_link_names():
raise UnknownBodyException(
u'can not attach \'{}\' to non existent parent link \'{}\' of \'{}\''
.format(urdf_object.get_name(), parent_link, self.get_name()))
if len(
set(urdf_object.get_link_names()).intersection(
set(self.get_link_names()))) != 0:
raise DuplicateNameException(
u'can not merge urdfs that share link names')
if len(
set(urdf_object.get_joint_names()).intersection(
set(self.get_joint_names()))) != 0:
raise DuplicateNameException(
u'can not merge urdfs that share joint names')
origin = up.Pose([
np.round(pose.position.x, round_to),
np.round(pose.position.y, round_to),
np.round(pose.position.z, round_to)
],
euler_from_quaternion([
np.round(pose.orientation.x, round_to),
np.round(pose.orientation.y, round_to),
np.round(pose.orientation.z, round_to),
np.round(pose.orientation.w, round_to)
]))
joint = up.Joint(self.robot_name_to_root_joint(urdf_object.get_name()),
parent=parent_link,
child=urdf_object.get_root(),
joint_type=FIXED_JOINT,
origin=origin)
self._urdf_robot.add_joint(joint)
for j in urdf_object._urdf_robot.joints:
self._urdf_robot.add_joint(j)
for l in urdf_object._urdf_robot.links:
self._urdf_robot.add_link(l)
try:
del self._link_to_marker[urdf_object.get_name()]
except:
pass
self.reinitialize()
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 default_collision(geometry=None, origin=None):
""" Return default collision """
if geometry is None:
geometry = urdf.Box([0.05, 0.05, 0.05])
if origin is None:
origin = urdf.Pose([-1, 0, 0.5], [0, 0, 0])
return urdf.Collision(geometry=geometry, origin=origin)
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 frame_collision(scale=1):
""" Return default collision """
# if geometry is None:
_x = (0.033 / 2) * scale
_y = (0.022 / 2) * scale
_z = (0.103 / 2) * scale
geometry = urdf.Cylinder(length=_z, radius=_x)
# if origin is None:
origin = urdf.Pose(xyz=[0, 0, 0], rpy=[0, 0, 0])
return urdf.Collision(geometry=geometry, origin=origin)
def frame_visual(scale=10):
""" Return default collision """
# if geometry is None:
_x = (0.033 / 2) * scale
_y = (0.022 / 2) * scale
_z = (0.103 / 2) * scale
# geometry = urdf.Box(size=[_x, _y,
# _z])
geometry = urdf.Cylinder(length=_z, radius=_x)
# if origin is None:
origin = urdf.Pose(xyz=[0, 0, 0], rpy=[0, 0, 0])
return urdf.Visual(geometry=geometry, origin=origin, material=orange())
def frame_inertial(scale=1):
""" Return default inertial """
_mass = 0.1 * scale
_x = (0.033 / 2)
_y = (0.022 / 2)
_z = (0.103 / 2)
ixx = _mass * _x * _x
iyy = _mass * _y * _y
izz = _mass * _z * _z
return urdf.Inertial(mass=_mass,
inertia=urdf.Inertia(ixx=ixx, iyy=iyy, izz=izz),
origin=urdf.Pose(xyz=[_x, _y, _z], rpy=[0, 0, 0]))
def poseFromAffordance(aff):
t = aff.getChildFrame().transform
position, quat = transformUtils.poseFromTransform(t)
rpy = transformUtils.rollPitchYawFromTransform(t)
return urdf.Pose(position, rpy)
def default_inertial():
""" Return default inertial """
return urdf.Inertial(mass=1,
inertia=urdf.Inertia(ixx=1, iyy=1, izz=1),
origin=urdf.Pose(xyz=[0, 0, n_len / 2], rpy=[0, 0,
0]))
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
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]))
def default_inertial():
""" Return default inertial """
return urdf.Inertial(mass=1e-5,
inertia=urdf.Inertia(ixx=1e-6, iyy=1e-6, izz=1e-6),
origin=urdf.Pose([-1, 0, 0.5], [0, 0, 0]))
