def create_door(width=0.08,
length=1,
height=2,
mass=1,
handle=True,
frame=True,
**kwargs):
# TODO: hinge, cylinder on end, sliding door, knob
# TODO: explicitly disable self collisions (happens automatically)
geometry = get_box_geometry(width, length, height)
hinge = 0 # -width/2
com_point = Point(x=hinge, y=-length / 2., z=height / 2.)
door_collision, door_visual = create_shape(geometry,
pose=Pose(com_point),
**kwargs)
door_link = LinkInfo(
mass=mass,
collision_id=door_collision,
visual_id=door_visual,
#point=com_point,
inertial_point=com_point, # TODO: be careful about the COM
parent=0,
joint_type=p.JOINT_REVOLUTE,
joint_axis=[0, 0, 1])
links = [door_link]
if handle:
links.append(create_handle(width, length, height))
if frame:
links.append(create_frame(width, length, height))
body = create_multi_body(base_link=LinkInfo(), links=links)
#draw_circle(center=unit_point(), diameter=width/2., parent=body, parent_link=0)
#set_joint_limits(body, link=0, lower=-PI, upper=PI)
return body
def create_frame(width, length, height, side=0.1):
# TODO: could be part of the base link instead
shapes = [
Shape(get_box_geometry(width=width,
length=length + 2 * side,
height=side),
pose=Pose(Point(z=height / 2. + side / 2.)),
color=BLACK),
Shape(get_box_geometry(width=width, length=side, height=height),
pose=Pose(Point(y=(length + side) / 2.)),
color=BLACK),
Shape(get_box_geometry(width=width, length=side, height=height),
pose=Pose(Point(y=-(length + side) / 2.)),
color=BLACK),
]
frame_collision, frame_visual = create_shape_array(*unzip(shapes))
return LinkInfo(mass=STATIC_MASS,
collision_id=frame_collision,
visual_id=frame_visual,
point=Point(y=-length / 2., z=height / 2.),
parent=0,
joint_type=p.JOINT_FIXED)
def parse_region(region):
lower = np.min(region['hull'], axis=0)
upper = np.max(region['hull'], axis=0)
x, y = (lower + upper) / 2.
w, h = (upper - lower) # / 2.
geom = get_box_geometry(w, h, 1e-3)
collision_id, visual_id = create_shape(geom,
pose=Pose(Point(x, y)),
color=parse_color(region['color']))
#region_id = create_body(NULL_ID, visual_id)
region_id = create_body(collision_id, visual_id)
set_pose(region_id, parse_pose(region))
return region_id
def parse_geometry(geometry):
# TODO: can also just make fixed links
geom = None
if geometry['type'] == 'box':
geom = get_box_geometry(*2 * np.array(geometry['extents']))
elif geometry['type'] == 'cylinder':
geom = get_cylinder_geometry(geometry['radius'], geometry['height'])
elif geometry['type'] == 'sphere':
# TODO: does sphere not work?
geom = get_sphere_geometry(geometry['radius'])
elif geometry['type'] == 'trimesh':
pass
else:
raise NotImplementedError(geometry['type'])
pose = parse_pose(geometry)
color = parse_color(geometry['color']) # specular=geometry['specular'])
return geom, pose, color
def parse_body(body, important=False):
[link] = body['links']
# for geometry in link['geometries']:
geoms = []
poses = []
colors = []
skipped = False
for geometry in link:
geom, pose, color = parse_geometry(geometry)
if geom == None:
skipped = True
else:
geoms.append(geom)
poses.append(pose)
colors.append(color)
if skipped:
if important:
center = body['aabb']['center']
extents = 2 * np.array(body['aabb']['extents'])
geoms = [get_box_geometry(*extents)]
poses = [Pose(center)]
colors = [(.5, .5, .5, 1)]
else:
return None
if not geoms:
return None
if len(geoms) == 1:
collision_id, visual_id = create_shape(geoms[0],
pose=poses[0],
color=colors[0])
else:
collision_id, visual_id = create_shape_array(geoms, poses, colors)
body_id = create_body(collision_id, visual_id)
set_pose(body_id, parse_pose(body))
return body_id