def _create_skeleton(self, width, height, bb_thickness=0.05):
"""
The busybox skeleton consists of a base and backboard joined by
a fixed Joint. Note all unspecified dimensions are fixed.
:param width: The length of the busybox in the x-dimension.
:param height: The height of the busybox in the z-dimenstion.
"""
base_link = urdf.Link(
'base_link',
urdf.Inertial(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)), urdf.Mass(value=0),
urdf.Inertia(ixx=0.001,
ixy=0,
ixz=0,
iyy=0.001,
iyz=0,
izz=0.001)),
urdf.Collision(urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(urdf.Box(size=(width, 0.3, 0.1)))),
urdf.Visual(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(urdf.Box(size=(width, 0.3, 0.1))),
urdf.Material('brown',
urdf.Color(rgba=(0.82, 0.71, 0.55, 1.0)))))
back_link = urdf.Link(
'back_link',
urdf.Inertial(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Mass(value=0.5),
urdf.Inertia(ixx=0.001,
ixy=0,
ixz=0,
iyy=0.001,
iyz=0,
izz=0.001)),
urdf.Collision(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(urdf.Box(size=(width, bb_thickness, height)))),
urdf.Visual(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(urdf.Box(size=(width, bb_thickness, height))),
urdf.Material('brown',
urdf.Color(rgba=(0.82, 0.71, 0.55, 1.0)))))
fixed_joint = urdf.Joint('fixed_backboard',
urdf.Parent('base_link'),
urdf.Child('back_link'),
urdf.Origin(xyz=(0, 0, height / 2.0 + 0.05),
rpy=(0, 0, 0)),
type='fixed')
self._links.append(base_link)
self._links.append(back_link)
self._joints.append(fixed_joint)
def object_to_urdf(object_name, object):
rgb = np.random.uniform(0, 1, 3)
link_urdf = odio_urdf.Link(object_name,
odio_urdf.Inertial(
odio_urdf.Origin(xyz=object.com, rpy=(0, 0, 0)),
odio_urdf.Mass(value=object.mass),
odio_urdf.Inertia(ixx=0.001,
ixy=0,
ixz=0,
iyy=0.001,
iyz=0,
izz=0.001)
),
odio_urdf.Collision(
odio_urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
odio_urdf.Geometry(
odio_urdf.Box(size=(object.dimensions.width,
object.dimensions.length,
object.dimensions.height))
)
),
odio_urdf.Visual(
odio_urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
odio_urdf.Geometry(
odio_urdf.Box(size=(object.dimensions.width,
object.dimensions.length,
object.dimensions.height))
),
odio_urdf.Material('color',
odio_urdf.Color(rgba=(*object.color, 1.0))
)
))
object_urdf = odio_urdf.Robot(link_urdf)
return object_urdf
def hand_urdf():
rgb = (0, 1, 0)
link_urdf = odio_urdf.Link(
'hand',
odio_urdf.Inertial(
odio_urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
odio_urdf.Mass(value=0.1),
odio_urdf.Inertia(ixx=0.001,
ixy=0,
ixz=0,
iyy=0.001,
iyz=0,
izz=0.001)),
odio_urdf.Collision(odio_urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
odio_urdf.Geometry(odio_urdf.Sphere(radius=0.02))),
odio_urdf.Visual(
odio_urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
odio_urdf.Geometry(odio_urdf.Sphere(radius=0.02), ),
odio_urdf.Material('color', odio_urdf.Color(rgba=(*rgb, 1.0)))))
object_urdf = odio_urdf.Robot(link_urdf)
return object_urdf
def object_to_urdf(object):
rgb = np.random.uniform(0, 1, 3)
I = 0.001
link_urdf = odio_urdf.Link(
object.name,
odio_urdf.Inertial(
odio_urdf.Origin(xyz=tuple(object.com), rpy=(0, 0, 0)),
odio_urdf.Mass(value=object.mass),
odio_urdf.Inertia(ixx=I, ixy=0, ixz=0, iyy=I, iyz=0, izz=I)),
odio_urdf.Collision(
odio_urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
odio_urdf.Geometry(
odio_urdf.Box(size=(object.dimensions.x, object.dimensions.y,
object.dimensions.z)))),
odio_urdf.Visual(
odio_urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
odio_urdf.Geometry(
odio_urdf.Box(size=(object.dimensions.x, object.dimensions.y,
object.dimensions.z))),
odio_urdf.Material('color',
odio_urdf.Color(rgba=(*object.color, 1.0)))))
object_urdf = odio_urdf.Robot(link_urdf, name=object.name)
return object_urdf
def link(self, name):
self.__add(urdf.Link(name))
def __init__(self,
x_offset,
z_offset,
range,
axis,
color,
bb_thickness=0.05):
"""
:param x_offset: The offset in the x-dimension from the busybox backboard.
:param z_offset: The offset in the z-dimension from the busybox backboard.
:param range: The total distance spanned by the prismatic joint.
:param axis: A 2-d unit vector with directions in the x and z directions.
:param color: An 3-tuple of rgb values.
"""
super(Slider, self).__init__('Slider')
name = Slider.n_sliders
Slider.n_sliders += 1
handle_radius = 0.02
slider_handle_name = 'slider_{0}_handle'.format(name)
slider_track_name = 'slider_{0}_track'.format(name)
handle = urdf.Link(
slider_handle_name,
urdf.Inertial(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Mass(value=0.1),
urdf.Inertia(ixx=0.001,
ixy=0,
ixz=0,
iyy=0.001,
iyz=0,
izz=0.001)),
urdf.Collision(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(
urdf.Cylinder(radius=handle_radius,
length=self.handle_length))),
urdf.Visual(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(
urdf.Cylinder(radius=handle_radius,
length=self.handle_length)),
urdf.Material(
'slider_{0}_color'.format(name),
urdf.Color(rgba=(color[0], color[1], color[2], 1.0)))))
track = urdf.Link(
slider_track_name.format(name),
urdf.Inertial(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Mass(value=0.1),
urdf.Inertia(ixx=0.001,
ixy=0,
ixz=0,
iyy=0.001,
iyz=0,
izz=0.001)),
urdf.Collision(urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(urdf.Box(size=(range, 0.005, 0.02)))),
urdf.Visual(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(urdf.Box(size=(range, 0.005, 0.02))),
urdf.Material('slider_track_color'.format(name),
urdf.Color(rgba=(0.6, 0.6, 0.6, 1.0)))))
joint = urdf.Joint('slider_{0}_joint'.format(name),
urdf.Parent('back_link'),
urdf.Child('slider_{0}_handle'.format(name)),
urdf.Axis(xyz=(axis[0], axis[1], 0)),
urdf.Origin(xyz=(x_offset, self.handle_length,
z_offset),
rpy=(1.57, 0, 0)),
urdf.Limit(lower=-range / 2.0, upper=range / 2.0),
urdf.Dynamics(friction=1.0, damping=1.0),
type='prismatic')
angle = -np.arctan2(axis[1], axis[0])
track_joint = urdf.Joint('slider_{0}_track_joint'.format(name),
urdf.Parent('back_link'),
urdf.Child('slider_{0}_track'.format(name)),
urdf.Origin(xyz=(x_offset, bb_thickness / 2,
z_offset),
rpy=(0, angle, 0)),
type='fixed')
self._links.append(handle)
self._joints.append(joint)
self._links.append(track)
self._joints.append(track_joint)
self.handle_name = slider_handle_name
self.track_name = slider_track_name
self.origin = (x_offset, z_offset)
self.range = range
self.handle_radius = handle_radius
self.axis = axis
def __init__(self,
door_offset,
door_size,
handle_offset_z,
flipped,
color,
bb_thickness=0.05):
super(Door, self).__init__('Door')
name = Door.n_doors
Door.n_doors += 1
dir = 1.0
if flipped: dir = -1.0
thickness = 0.01
handle_radius = 0.015
handle_offset_x = 0.005
door_base_name = 'door_{0}_base'.format(name)
door = urdf.Link(
door_base_name,
urdf.Inertial(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Mass(value=0.1),
urdf.Inertia(ixx=0.001,
ixy=0,
ixz=0,
iyy=0.001,
iyz=0,
izz=0.001)),
urdf.Collision(
urdf.Origin(xyz=(-dir * door_size[0] / 2.0, thickness / 2, 0),
rpy=(0, 0, 0)),
urdf.Geometry(
urdf.Box(size=(door_size[0], thickness, door_size[1])))),
urdf.Visual(
urdf.Origin(xyz=(-dir * door_size[0] / 2.0, thickness / 2, 0),
rpy=(0, 0, 0)),
urdf.Geometry(urdf.Box(size=(door_size[0], 0.01,
door_size[1]))),
urdf.Material(
'door_{0}_color'.format(name),
urdf.Color(rgba=(color[0], color[1], color[2], 1.0)))))
door_handle_name = 'door_{0}_handle'.format(name)
door_handle = urdf.Link(
door_handle_name,
urdf.Inertial(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Mass(value=0.1),
urdf.Inertia(ixx=0.001,
ixy=0,
ixz=0,
iyy=0.001,
iyz=0,
izz=0.001)),
urdf.Collision(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(
urdf.Cylinder(radius=handle_radius,
length=self.handle_length))),
urdf.Visual(
urdf.Origin(xyz=(0, 0, 0), rpy=(0, 0, 0)),
urdf.Geometry(
urdf.Cylinder(radius=handle_radius,
length=self.handle_length)),
urdf.Material(
'door_{0}_handle_color'.format(name),
urdf.Color(rgba=(color[0] * 0.5, color[1] * 0.5,
color[2] * 0.5, 1.0)))))
if flipped: limit = urdf.Limit(lower=0, upper=2.355)
else: limit = urdf.Limit(lower=-2.355, upper=0)
door_joint = urdf.Joint('door_{0}_joint'.format(name),
urdf.Child('door_{0}_base'.format(name)),
urdf.Parent('back_link'),
urdf.Axis(xyz=(0, 0, 1)),
urdf.Origin(xyz=(door_offset[0],
bb_thickness / 2,
door_offset[1]),
rpy=(0, 0, 0)),
limit,
type='revolute')
door_handle_joint = urdf.Joint('door_{0}_handle_joint'.format(name),
urdf.Parent('door_{0}_base'.format(name)),
urdf.Child('door_{0}_handle'.format(name)),
urdf.Origin(xyz=(dir*(-door_size[0]+handle_radius+handle_offset_x), \
self.handle_length/2,
handle_offset_z),
rpy=(1.57, 0, 0)),
type='fixed')
self._links.append(door)
self._joints.append(door_joint)
self._links.append(door_handle)
self._joints.append(door_handle_joint)
self._door_base_id = None
self.handle_name = door_handle_name
self.door_base_name = door_base_name
self.origin = door_offset
self.door_size = door_size
self.handle_offset_z = handle_offset_z
self.handle_offset_x = handle_offset_x
self.handle_radius = handle_radius
self.flipped = flipped