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