def are_visible(world): ray_names = [] rays = [] for name in world.movable: for camera, info in world.cameras.items(): camera_pose = get_pose(info.body) camera_point = point_from_pose(camera_pose) point = point_from_pose(get_pose(world.get_body(name))) if is_visible_point(CAMERA_MATRIX, KINECT_DEPTH, point, camera_pose=camera_pose): ray_names.append(name) rays.append(Ray(camera_point, point)) ray_results = batch_ray_collision(rays) visible_indices = [ idx for idx, (name, result) in enumerate(zip(ray_names, ray_results)) if result.objectUniqueId == world.get_body(name) ] visible_names = {ray_names[idx] for idx in visible_indices} print('Detected:', sorted(visible_names)) if has_gui(): handles = [ add_line(rays[idx].start, rays[idx].end, color=BLUE) for idx in visible_indices ] wait_for_duration(1.0) remove_handles(handles) # TODO: the object drop seems to happen around here return visible_names
def approach(self): grasp = np.array([0, 0, 0.2]) target_point_1 = np.array(get_pose(self.pw.pipe))[0] + grasp grasp = np.array([0, 0, 0.13]) target_point_2 = np.array(get_pose(self.pw.pipe))[0] + grasp target_quat = (1, 0.5, 0, 0) #get whatever it is by default target_pose = (target_point_1, target_quat) obstacles = [self.pw.pipe, self.pw.hollow] self.go_to_pose(target_pose, obstacles=obstacles) target_pose = (target_point_2, target_quat) self.go_to_pose(target_pose, obstacles=obstacles) self.pipe_attach = create_attachment(self.pw.robot, self.ee_link, self.pw.pipe) self.squeeze(0, width=self.default_width)
def lower_spoon(world, bowl_name, spoon_name, min_spoon_z, max_spoon_z): assert min_spoon_z <= max_spoon_z bowl_body = world.get_body(bowl_name) bowl_urdf_from_center = get_urdf_from_base( bowl_body) # get_urdf_from_center spoon_body = world.get_body(spoon_name) spoon_quat = lookup_orientation(spoon_name, STIR_QUATS) spoon_urdf_from_center = get_urdf_from_base( spoon_body, reference_quat=spoon_quat) # get_urdf_from_center # Keeping the orientation consistent for generalization purposes # TODO: what happens when the base isn't flat (e.g. bowl) bowl_pose = get_pose(bowl_body) stir_z = None for z in np.arange(max_spoon_z, min_spoon_z, step=-0.005): bowl_from_stirrer = multiply(bowl_urdf_from_center, Pose(Point(z=z)), invert(spoon_urdf_from_center)) set_pose(spoon_body, multiply(bowl_pose, bowl_from_stirrer)) #wait_for_user() if pairwise_collision(bowl_body, spoon_body): # Want to be careful not to make contact with the base break stir_z = z return stir_z
def pick_problem(arm='left', grasp_type='side'): other_arm = get_other_arm(arm) initial_conf = get_carry_conf(arm, grasp_type) pr2 = create_pr2() set_base_values(pr2, (0, -1.2, np.pi / 2)) set_arm_conf(pr2, arm, initial_conf) open_arm(pr2, arm) set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) close_arm(pr2, other_arm) plane = create_floor() table = load_pybullet(TABLE_URDF) # table = create_table(height=0.8) # table = load_pybullet("table_square/table_square.urdf") box = create_box(.07, .05, .25) set_point(box, (-0.25, -0.3, TABLE_MAX_Z + .25 / 2)) # set_point(box, (0.2, -0.2, 0.8 + .25/2 + 0.1)) return Problem(robot=pr2, movable=[box], arms=[arm], grasp_types=[grasp_type], surfaces=[table], goal_conf=get_pose(pr2), goal_holding=[(arm, box)])
def add_scales(task, ros_world): scale_stackings = {} holding = {grasp.obj_name for grasp in task.init_holding.values()} with ClientSaver(ros_world.client): perception = ros_world.perception items = sorted(set(perception.get_items()) - holding, key=lambda n: get_point(ros_world.get_body(n))[1], reverse=False) # Right to left for i, item in enumerate(items): if not POUR and (get_type(item) != SCOOP_CUP): continue item_body = ros_world.get_body(item) scale = create_name(SCALE_TYPE, i + 1) with HideOutput(): scale_body = load_pybullet(get_body_urdf(get_type(scale)), fixed_base=True) ros_world.perception.sim_bodies[scale] = scale_body ros_world.perception.sim_items[scale] = None item_z = stable_z(item_body, scale_body) scale_pose_item = Pose( point=Point(z=-item_z)) # TODO: relies on origin in base set_pose(scale_body, multiply(get_pose(item_body), scale_pose_item)) roll, pitch, _ = get_euler(scale_body) set_euler(scale_body, [roll, pitch, 0]) scale_stackings[scale] = item #wait_for_user() return scale_stackings
def label_elements(element_bodies): # +z points parallel to each element body for element, body in element_bodies.items(): print(element) label_element(element_bodies, element) draw_pose(get_pose(body), length=0.02) wait_for_user()
def __init__(self, end_effector, element_bodies, node_points, ground_nodes, element, reverse, max_directions=INF): # TODO: more directions for ground collisions self.end_effector = end_effector self.element = element self.reverse = reverse self.max_directions = max_directions self.element_pose = get_pose(element_bodies[element]) n1, n2 = element length = get_distance(node_points[n1], node_points[n2]) self.translation_path = np.append( np.arange(-length / 2, length / 2, STEP_SIZE), [length / 2]) if ORTHOGONAL_GROUND and is_ground(element, ground_nodes): # TODO: orthogonal to the ground self.direction_generator = cycle( [Pose(euler=Euler(roll=0, pitch=0))]) else: self.direction_generator = get_direction_generator( self.end_effector, use_halton=False) self.trajectories = []
def test_print(robot, node_points, elements): #elements = [elements[0]] elements = [elements[-1]] [element_body] = create_elements(node_points, elements) wait_for_user() phi = 0 #grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta)) # for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)] #grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi)) # for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)] grasp_rotations = [sample_direction() for _ in range(25)] link = link_from_name(robot, TOOL_NAME) movable_joints = get_movable_joints(robot) sample_fn = get_sample_fn(robot, movable_joints) for grasp_rotation in grasp_rotations: n1, n2 = elements[0] length = np.linalg.norm(node_points[n2] - node_points[n1]) set_joint_positions(robot, movable_joints, sample_fn()) for t in np.linspace(-length / 2, length / 2, 10): #element_translation = Pose(point=Point(z=-t)) #grasp_pose = multiply(grasp_rotation, element_translation) #reverse = Pose(euler=Euler()) reverse = Pose(euler=Euler(roll=np.pi)) grasp_pose = get_grasp_pose(t, grasp_rotation, 0) grasp_pose = multiply(grasp_pose, reverse) element_pose = get_pose(element_body) link_pose = multiply(element_pose, invert(grasp_pose)) conf = inverse_kinematics(robot, link, link_pose) wait_for_user()
def fix_pose(self, name, pose=None, fraction=0.5): body = self.get_body(name) if pose is None: pose = get_pose(body) else: set_pose(body, pose) # TODO: can also drop in simulation x, y, z = point_from_pose(pose) roll, pitch, yaw = euler_from_quat(quat_from_pose(pose)) quat = quat_from_euler(Euler(yaw=yaw)) set_quat(body, quat) surface_name = self.get_supporting(name) if surface_name is None: return None, None if fraction == 0: new_pose = (Point(x, y, z), quat) return new_pose, surface_name surface_aabb = compute_surface_aabb(self, surface_name) new_z = (1 - fraction) * z + fraction * stable_z_on_aabb( body, surface_aabb) point = Point(x, y, new_z) set_point(body, point) print('{} error: roll={:.3f}, pitch={:.3f}, z-delta: {:.3f}'.format( name, roll, pitch, new_z - z)) new_pose = (point, quat) return new_pose, surface_name
def observe_pybullet(world): # TODO: randomize robot's pose # TODO: probabilities based on whether in viewcone or not # TODO: sample from poses on table # world_saver = WorldSaver() visible_entities = are_visible(world) detections = {} assert OBS_P_FP == 0 for name in visible_entities: # TODO: false positives if random.random() < OBS_P_FN: continue body = world.get_body(name) pose = get_pose(body) dx, dy = np.random.multivariate_normal(mean=np.zeros(2), cov=math.pow(OBS_POS_STD, 2) * np.eye(2)) dyaw, = np.random.multivariate_normal(mean=np.zeros(1), cov=math.pow(OBS_ORI_STD, 2) * np.eye(1)) print('{}: dx={:.3f}, dy={:.3f}, dyaw={:.5f}'.format( name, dx, dy, dyaw)) noise_pose = Pose(Point(x=dx, y=dy), Euler(yaw=dyaw)) observed_pose = multiply(pose, noise_pose) #world.get_body_type(name) detections.setdefault(name, []).append( observed_pose) # TODO: use type instead #world_saver.restore() return detections
def create_bounding_mesh(printed_elements, element_bodies=None, node_points=None, buffer=0.): # TODO: use bounding boxes instead of points # TODO: connected components assert printed_elements assert element_bodies or node_points printed_points = [] if node_points is not None: printed_points.extend(node_points[n] for element in printed_elements for n in element) if element_bodies is not None: for element in printed_elements: body = element_bodies[element] printed_points.extend(apply_affine(get_pose(body), vertices_from_link(body, BASE_LINK))) if buffer != 0.: half_extents = buffer*np.ones(3) for point in list(printed_points): printed_points.extend(np.array(point) + np.array(corner) for corner in get_aabb_vertices(AABB(-half_extents, half_extents))) rgb = colorsys.hsv_to_rgb(h=random.random(), s=1, v=1) #rgb = RED try: mesh = convex_hull(printed_points) # handles = draw_mesh(mesh) return create_mesh(mesh, under=True, color=apply_alpha(rgb, 0.5)) except QhullError as e: print(printed_elements) raise e
def mirror_robot(robot1, node_points): # TODO: place robots side by side or diagonal across set_extrusion_camera(node_points, theta=-np.pi / 3) #draw_pose(Pose()) centroid = np.average(node_points, axis=0) centroid_pose = Pose(point=centroid) #draw_pose(Pose(point=centroid)) # print(centroid) scale = 0. # 0.15 vector = get_point(robot1) - centroid set_pose(robot1, Pose(point=Point(*+scale * vector[:2]))) # Inner product of end-effector z with base->centroid or perpendicular to this line # Partition by sides robot2 = load_robot() set_pose( robot2, Pose(point=Point(*-(2 + scale) * vector[:2]), euler=Euler(yaw=np.pi))) # robots = [robot1] robots = [robot1, robot2] for robot in robots: set_configuration(robot, DUAL_CONF) # joint1 = get_movable_joints(robot)[0] # set_joint_position(robot, joint1, np.pi / 8) draw_pose(get_pose(robot), length=0.25) return robots
def is_visible_by_camera(world, point): for camera_name in world.cameras: camera_body, camera_matrix, camera_depth = world.cameras[camera_name] camera_pose = get_pose(camera_body) #camera_point = point_from_pose(camera_pose) if is_visible_point(camera_matrix, camera_depth, point, camera_pose): return True return False
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() connect(use_gui=True) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table1 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) set_point(table1, Point(y=+0.5)) table2 = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH/2, height=TABLE_WIDTH/2, top_color=TAN, cylinder=False) set_point(table2, Point(y=-0.5)) tables = [table1, table2] #set_euler(table1, Euler(yaw=np.pi/2)) with HideOutput(): # data_path = add_data_path() # robot_path = os.path.join(data_path, WSG_GRIPPER) robot_path = get_model_path(WSG_50_URDF) # WSG_50_URDF | PANDA_HAND_URDF robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table1) set_point(block1, Point(y=-0.5, z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=-0.25, y=-0.5, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, y=+0.5, z=block_z)) blocks = [block1, block2, block3] set_camera_pose(camera_point=Point(x=-1, z=block_z+1), target_point=Point(z=block_z)) block_pose = get_pose(block1) open_gripper(robot) tool_link = link_from_name(robot, 'tool_link') base_from_tool = get_relative_pose(robot, tool_link) #draw_pose(unit_pose(), parent=robot, parent_link=tool_link) grasps = get_side_grasps(block1, tool_pose=Pose(euler=Euler(yaw=np.pi/2)), top_offset=0.02, grasp_length=0.03, under=False)[1:2] for grasp in grasps: gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user() wait_for_user('Finish?') disconnect()
def bowl_path_collision(bowl_body, body, body_path_bowl): bowl_pose = get_pose(bowl_body) with BodySaver(body): for cup_pose_bowl in body_path_bowl: cup_pose_world = multiply(bowl_pose, cup_pose_bowl) set_pose(body, cup_pose_world) if body_pair_collision(bowl_body, body): #, collision_buffer=0.0): return True return False
def main(): connect(use_gui=True) with HideOutput(): pr2 = load_model( "models/drake/pr2_description/urdf/pr2_simplified.urdf") set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']), REST_LEFT_ARM) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']), rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']), [0.2]) dump_body(pr2) block = load_model(BLOCK_URDF, fixed_base=False) set_point(block, [2, 0.5, 1]) target_point = point_from_pose(get_pose(block)) # head_link = link_from_name(pr2, HEAD_LINK) head_joints = joints_from_names(pr2, PR2_GROUPS['head']) head_link = head_joints[-1] #max_detect_distance = 2.5 max_register_distance = 1.0 distance_range = (max_register_distance / 2, max_register_distance) base_generator = visible_base_generator(pr2, target_point, distance_range) base_joints = joints_from_names(pr2, PR2_GROUPS['base']) for i in range(5): base_conf = next(base_generator) set_joint_positions(pr2, base_joints, base_conf) p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)), target_point, lineColorRGB=(1, 0, 0)) # addUserDebugText p.addUserDebugLine(point_from_pose( get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))), target_point, lineColorRGB=(0, 0, 1)) # addUserDebugText # head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, ) head_conf = inverse_visibility(pr2, target_point) set_joint_positions(pr2, head_joints, head_conf) print(get_detections(pr2)) # TODO: does this detect the robot sometimes? detect_mesh, z = get_detection_cone(pr2, block) detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5)) set_pose(detect_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25)) set_pose(view_cone, get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))) wait_for_user() remove_body(detect_cone) remove_body(view_cone) disconnect()
def place(self, use_policy=False): grasp = np.array([0, 0, 0.3]) target_point = np.array(get_pose(self.pw.hollow))[0] + grasp self.hollow_pose = get_pose(self.pw.hollow) target_quat = (1, 0.5, 0, 0) target_pose = (target_point, target_quat) lift_point = np.array( p.getLinkState(self.pw.robot, self.finger_joints[0])[0]) + grasp traj1 = self.go_to_pose((lift_point, target_quat), obstacles=[self.pw.hollow], attachments=[self.pipe_attach], cart_traj=True, use_policy=use_policy) traj2 = self.go_to_pose(target_pose, obstacles=[self.pw.hollow], attachments=[self.pipe_attach], cart_traj=True, use_policy=use_policy) return traj1 + traj2
def visualize_cartesian_path(body, pose_path): for i, pose in enumerate(pose_path): set_pose(body, pose) print('{}/{}) continue?'.format(i, len(pose_path))) wait_for_user() handles = draw_pose(get_pose(body)) handles.extend(draw_aabb(get_aabb(body))) print('Finish?') wait_for_user() for h in handles: remove_debug(h)
def get_direction_generator(end_effector, **kwargs): world_from_base = get_pose(end_effector.robot) lower = [-np.pi / 2, -np.pi / 2] upper = [+np.pi / 2, +np.pi / 2] for [roll, pitch] in interval_generator(lower, upper, **kwargs): ##roll = random.uniform(0, np.pi) #roll = np.pi/4 #pitch = random.uniform(0, 2*np.pi) #return Pose(euler=Euler(roll=np.pi / 2 + roll, pitch=pitch)) #roll = random.uniform(-np.pi/2, np.pi/2) #pitch = random.uniform(-np.pi/2, np.pi/2) pose = Pose(euler=Euler(roll=roll, pitch=pitch)) yield pose
def compute_forward_reachability(robot, **kwargs): # TODO: compute wrt torso instead points = list( map(point_from_pose, learned_forward_generator(robot, get_pose(robot), **kwargs))) #for point in points: # draw_point(point) #wait_for_interrupt() points2d = [point[:2] for point in points] #centriod = np.average(points, axis=0) from scipy.spatial import ConvexHull hull = ConvexHull(points2d) return [points2d[i] for i in hull.vertices]
def get_contained_beads(body, beads, height_fraction=1.0, top_threshold=0.0): #aabb = get_aabb(body) center, extent = approximate_as_prism(body, body_pose=get_pose(body)) lower = center - extent / 2. upper = center + extent / 2. _, _, z1 = lower x2, y2, z2 = upper z_upper = z1 + height_fraction * (z2 - z1) + top_threshold new_aabb = AABB(lower, np.array([x2, y2, z_upper])) #handles = draw_aabb(new_aabb) return { bead for bead in beads if aabb_contains_aabb(get_aabb(bead), new_aabb) }
def water_test(q): if attachment is None: return False set_joint_positions(body, joints, q) attachment.assign() attachment_pose = get_pose(attachment.child) pose = multiply(attachment_pose, reference_pose) # TODO: confirm not inverted roll, pitch, _ = euler_from_quat(quat_from_pose(pose)) violation = (MAX_ROTATION < abs(roll)) or (MAX_ROTATION < abs(pitch)) #if violation: # TODO: check whether different confs can be waypoints for each object # print(q, violation) # wait_for_user() return violation
def visualize_learned_pours(learner, bowl_type='blue_bowl', cup_type='red_cup', num_steps=None): learner.query_type = REJECTION # BEST | REJECTION | STRADDLE learner.validity_learner = None world = create_world() #add_obstacles() #environment = get_bodies() world.bodies[bowl_type] = load_cup_bowl(bowl_type) world.bodies[cup_type] = load_cup_bowl(cup_type) feature = get_pour_feature(world, bowl_type, cup_type) draw_pose(get_pose(world.get_body(bowl_type)), length=0.2) # TODO: sample different sizes print('Feature:', feature) for parameter in learner.parameter_generator(world, feature, valid=False): handles = [] print('Parameter:', parameter) valid = is_valid_pour(world, feature, parameter) score = learner.score(feature, parameter) x = learner.func.x_from_feature_parameter(feature, parameter) [prediction] = learner.predict_x(np.array([x]), noise=True) print('Valid: {} | Mean: {:.3f} | Var: {:.3f} | Score: {:.3f}'.format( valid, prediction['mean'], prediction['variance'], score)) #fraction = rescale(clip(prediction['mean'], *DEFAULT_INTERVAL), DEFAULT_INTERVAL, new_interval=(0, 1)) #color = (1 - fraction) * np.array(RED) + fraction * np.array(GREEN) #set_color(world.get_body(cup_type), apply_alpha(color, alpha=0.25)) # TODO: overlay many cups at different poses bowl_from_pivot = get_bowl_from_pivot(world, feature, parameter) #handles.extend(draw_pose(bowl_from_pivot)) handles.extend(draw_point(point_from_pose(bowl_from_pivot), color=BLACK)) # TODO: could label bowl, cup dimensions path, _ = pour_path_from_parameter(world, feature, parameter, cup_yaw=0) for p1, p2 in safe_zip(path[:-1], path[1:]): handles.append(add_line(point_from_pose(p1), point_from_pose(p2), color=RED)) if num_steps is not None: path = path[:num_steps] for i, pose in enumerate(path[::-1]): #step_handles = draw_pose(pose, length=0.1) #step_handles = draw_point(point_from_pose(pose), color=BLACK) set_pose(world.get_body(cup_type), pose) print('Step {}/{}'.format(i+1, len(path))) wait_for_user() #remove_handles(step_handles) remove_handles(handles)
def is_robot_visible(world, links): for link in links: link_point = point_from_pose(get_link_pose(world.robot, link)) visible = False for camera_body, camera_matrix, camera_depth in world.cameras.values(): camera_pose = get_pose(camera_body) #camera_point = point_from_pose(camera_pose) #add_line(link_point, camera_point) if is_visible_point(camera_matrix, camera_depth, link_point, camera_pose): visible = True break if not visible: return False #wait_for_user() return True
def _load_robot(self, real_world): with ClientSaver(self.client): # TODO: set the x,y,theta joints using the base pose pose = get_pose(self.robot) # base_link is origin base_pose = get_link_pose(self.robot, link_from_name(self.robot, BASE_FRAME)) set_pose(self.robot, multiply(invert(base_pose), pose)) # base_pose = real_world.controller.return_cartesian_pose(arm='l') # Because the robot might have an xyz movable_names = real_world.controller.get_joint_names() movable_joints = [ joint_from_name(self.robot, name) for name in movable_names ] movable_positions = real_world.controller.get_joint_positions( movable_names) set_joint_positions(self.robot, movable_joints, movable_positions) self.initial_config = get_configuration(self.robot) self.body_mapping[self.robot] = real_world.robot
def compute_assignments(robots, elements, node_points, initial_confs): # TODO: print direction might influence the assignment assignments = {name: set() for name in initial_confs} for element in elements: point = get_midpoint(node_points, element) # min/max closest_robot, closest_distance = None, INF for i, robot in enumerate(robots): base_pose = get_pose(robot) base_point = point_from_pose(base_pose) point_base = tform_point(invert(base_pose), point) distance = get_yaw(point_base) # which side its on #distance = abs((base_point - point)[0]) # x distance #distance = get_length((base_point - point)[:2]) # xy distance if distance < closest_distance: closest_robot, closest_distance = ROBOT_TEMPLATE.format( i), distance assert closest_robot is not None # TODO: assign to several robots if close to the best distance assignments[closest_robot].add(element) return assignments
def fn(obj_name, pose): # TODO: incorporate probability mass # Ether sample observation (control) or target belief (next state) body = world.get_body(obj_name) open_surface_joints(world, pose.support) for camera_name in world.cameras: camera_body, camera_matrix, camera_depth = world.cameras[ camera_name] camera_pose = get_pose(camera_body) camera_point = point_from_pose(camera_pose) obj_point = point_from_pose(pose.get_world_from_body()) aabb = get_view_aabb(body, camera_pose) center = get_aabb_center(aabb) extent = np.multiply([detect_scale, detect_scale, 1], get_aabb_extent(aabb)) view_aabb = (center - extent / 2, center + extent / 2) # print(is_visible_aabb(view_aabb, camera_matrix=camera_matrix)) obj_points = apply_affine( camera_pose, support_from_aabb(view_aabb)) + [obj_point] # obj_points = [obj_point] if not all( is_visible_point(camera_matrix, camera_depth, point, camera_pose) for point in obj_points): continue rays = [Ray(camera_point, point) for point in obj_points] detect = Detect(world, camera_name, obj_name, pose, rays) if ray_trace: # TODO: how should doors be handled? move_occluding(world) open_surface_joints(world, pose.support) detect.pose.assign() if obstacles & detect.compute_occluding(): continue #detect.draw() #wait_for_user() return (detect, ) return None
def test_simulation(robot, target_x, video=None): use_turtlebot = (get_body_name(robot) == 'turtlebot') if not use_turtlebot: target_point, target_quat = map(list, get_pose(robot)) target_point[0] = target_x add_pose_constraint(robot, pose=(target_point, target_quat), max_force=200) # TODO: velocity constraint? else: # p.changeDynamics(robot, robot_joints[0], # Doesn't work # maxJointVelocity=1, # jointLimitForce=1,) robot_joints = get_movable_joints(robot)[:3] print('Max velocities:', get_max_velocities(robot, robot_joints)) print('Max forces:', get_max_forces(robot, robot_joints)) control_joint(robot, joint=robot_joints[0], position=target_x, velocity=0, position_gain=None, velocity_scale=None, max_velocity=100, max_force=300) # control_joints(robot, robot_joints, positions=[target_x, 0, PI], max_force=300) # velocity_control_joints(robot, robot_joints, velocities=[-2., 0, 0]) #, max_force=300) robot_link = get_first_link(robot) if video is None: wait_if_gui('Begin?') simulate(controller=condition_controller( lambda *args: abs(target_x - point_from_pose( get_link_pose(robot, robot_link))[0]) < 1e-3), sleep=0.01) # TODO: velocity condition # print('Velocities:', get_joint_velocities(robot, robot_joints)) # print('Torques:', get_joint_torques(robot, robot_joints)) if video is None: set_renderer(enable=True) wait_if_gui('Finish?')
def update_dist(self, observation, obstacles=[], verbose=False): # cfree_dist.conditionOnVar(index=1, has_detection=True) if not BAYESIAN and (self.name in observation): # TODO: convert into a Multivariate Gaussian [detected_pose] = observation[self.name] return DeltaDist(detected_pose) if not self.world.cameras: return self.dist.copy() body = self.world.get_body(self.name) all_poses = self.dist.support() cfree_poses = all_poses #cfree_poses = compute_cfree(body, all_poses, obstacles) #cfree_dist = self.cfree_dist cfree_dist = DDist( {pose: self.dist.prob(pose) for pose in cfree_poses}) # TODO: do these updates simultaneously for each object # TODO: check all camera poses [camera] = self.world.cameras.keys() info = self.world.cameras[camera] camera_pose = get_pose(info.body) detectable_poses = compute_detectable(cfree_poses, camera_pose) visible_poses = compute_visible(body, detectable_poses, camera_pose, draw=False) if verbose: print( 'Total: {} | CFree: {} | Detectable: {} | Visible: {}'.format( len(all_poses), len(cfree_poses), len(detectable_poses), len(visible_poses))) assert set(visible_poses) <= set(detectable_poses) # obs_fn = get_observation_fn(surface) #wait_for_user() return self.bayesian_belief_update(cfree_dist, visible_poses, observation, verbose=verbose)
def create_table_bodies(world, item_ranges, randomize=True): perception = world.perception with HideOutput(): add_data_path() floor_body = load_pybullet("plane.urdf") set_pose(floor_body, get_pose(world.robot)) add_table(world) for name, limits in sorted(item_ranges.items()): perception.sim_items[name] = None if randomize: body = randomize_body(name, width_range=limits.width_range, height_range=limits.height_range, mass_range=limits.mass_range) else: body = load_body(name) perception.sim_bodies[name] = body # perception.add_item(name, unit_pose()) x, y, yaw = np.random.uniform(*limits.pose2d_range) surface_body = perception.get_body(limits.surface) z = stable_z(body, surface_body) + Z_EPSILON pose = Pose((x, y, z), Euler(yaw=yaw)) perception.set_pose(name, *pose)