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 parse_object(obj, mesh_directory): name = obj.find('name').text mesh_filename = obj.find('geom').text geom = get_mesh_geometry(os.path.join(mesh_directory, mesh_filename)) pose = parse_pose(obj.find('pose')) movable = parse_boolean(obj.find('moveable')) color = (.75, .75, .75, 1) if 'red' in name: color = (1, 0, 0, 1) elif 'green' in name: color = (0, 1, 0, 1) elif 'blue' in name: color = (0, 0, 1, 1) elif movable: # TODO: assign new color #size = 2 * MAX_INT #size = 255 #n = id(obj) % size #n = hash(obj) % size #h = float(n) / size h = random.random() r, g, b = colorsys.hsv_to_rgb(h, .75, .75) color = (r, g, b, 1) print(name, mesh_filename, movable, color) collision_id, visual_id = create_shape(geom, color=color) body_id = p.createMultiBody(baseMass=STATIC_MASS, baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id, physicsClientId=CLIENT) set_pose(body_id, pose) return body_id
def main(group=SE3): connect(use_gui=True) set_camera_pose(camera_point=SIZE*np.array([1., -1., 1.])) # TODO: can also create all links and fix some joints # TODO: SE(3) motion planner (like my SE(3) one) where some dimensions are fixed obstacle = create_box(w=SIZE, l=SIZE, h=SIZE, color=RED) #robot = create_cylinder(radius=0.025, height=0.1, color=BLUE) obstacles = [obstacle] collision_id, visual_id = create_shape(get_cylinder_geometry(radius=0.025, height=0.1), color=BLUE) robot = create_flying_body(group, collision_id, visual_id) body_link = get_links(robot)[-1] joints = get_movable_joints(robot) joint_from_group = dict(zip(group, joints)) print(joint_from_group) #print(get_aabb(robot, body_link)) dump_body(robot, fixed=False) custom_limits = {joint_from_group[j]: l for j, l in CUSTOM_LIMITS.items()} # sample_fn = get_sample_fn(robot, joints, custom_limits=custom_limits) # for i in range(10): # conf = sample_fn() # set_joint_positions(robot, joints, conf) # wait_for_user('Iteration: {}'.format(i)) # return initial_point = SIZE*np.array([-1., -1., 0]) #initial_point = -1.*np.ones(3) final_point = -initial_point initial_euler = np.zeros(3) add_line(initial_point, final_point, color=GREEN) initial_conf = np.concatenate([initial_point, initial_euler]) final_conf = np.concatenate([final_point, initial_euler]) set_joint_positions(robot, joints, initial_conf) #print(initial_point, get_link_pose(robot, body_link)) #set_pose(robot, Pose(point=-1.*np.ones(3))) path = plan_joint_motion(robot, joints, final_conf, obstacles=obstacles, self_collisions=False, custom_limits=custom_limits) if path is None: disconnect() return for i, conf in enumerate(path): set_joint_positions(robot, joints, conf) point, quat = get_link_pose(robot, body_link) #euler = euler_from_quat(quat) euler = intrinsic_euler_from_quat(quat) print(conf) print(point, euler) wait_for_user('Step: {}/{}'.format(i, len(path))) wait_for_user('Finish?') disconnect()
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_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