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 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 main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-arm', required=True) parser.add_argument('-grasp', required=True) parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() arm = args.arm other_arm = get_other_arm(arm) grasp_type = args.grasp connect(use_gui=args.viewer) add_data_path() with HideOutput(): robot = load_pybullet(DRAKE_PR2_URDF) set_group_conf(robot, 'torso', [0.2]) set_arm_conf(robot, arm, get_carry_conf(arm, grasp_type)) set_arm_conf(robot, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) #plane = p.loadURDF("plane.urdf") #table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) table = create_table() box = create_box(.07, .07, .14) #create_inverse_reachability(robot, box, table, arm=arm, grasp_type=grasp_type) create_inverse_reachability2(robot, box, table, arm=arm, grasp_type=grasp_type) disconnect()
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 add_box(world, color_name, idx=0, **kwargs): name = name_from_type(color_name, idx) # TODO: geometry type body = create_box(w=0.07, l=0.07, h=0.14, color=COLOR_FROM_NAME[color_name]) world.add(name, body) # pose2d_on_surface(world, name, COUNTERS[0], **kwargs) return name
def create_elements_bodies(node_points, elements, color=apply_alpha(RED, alpha=1), diameter=ELEMENT_DIAMETER, shrink=ELEMENT_SHRINK): # TODO: could scale the whole environment # TODO: create a version without shrinking for transit planning # URDF_USE_IMPLICIT_CYLINDER element_bodies = [] for (n1, n2) in elements: p1, p2 = node_points[n1], node_points[n2] height = max(np.linalg.norm(p2 - p1) - 2 * shrink, 0) #if height == 0: # Cannot keep this here # continue center = (p1 + p2) / 2 # extents = (p2 - p1) / 2 delta = p2 - p1 x, y, z = delta phi = np.math.atan2(y, x) theta = np.math.acos(z / np.linalg.norm(delta)) quat = quat_from_euler(Euler(pitch=theta, yaw=phi)) # p1 is z=-height/2, p2 is z=+height/2 # Much smaller than cylinder # Also faster, C_shape 177.398 vs 400 body = create_box(diameter, diameter, height, color=color, mass=STATIC_MASS) set_color(body, color) set_point(body, center) set_quat(body, quat) #dump_body(body, fixed=True) # Visually, smallest diameter is 2e-3 # The geometries and bounding boxes seem correct though # TODO: create_cylinder takes in a radius not diameter #body = create_cylinder(ELEMENT_DIAMETER, height, color=color, mass=STATIC_MASS) #print('Diameter={:.5f} | Height={:.5f}'.format(ELEMENT_DIAMETER/2., height)) #print(get_aabb_extent(get_aabb(body)).round(6).tolist()) #print(get_visual_data(body)) #print(get_collision_data(body)) #set_point(body, center) #set_quat(body, quat) #draw_aabb(get_aabb(body)) element_bodies.append(body) #wait_for_user() return element_bodies
def create_object(shape, side=0.1, mass=1, color=BLUE): # TODO: simulation parameters # TODO: object dynamics parameters if shape == 'box': obj = create_box(w=side, l=side, h=side, mass=mass, color=color) elif shape == 'sphere': obj = create_sphere(radius=side, mass=mass, color=color) elif shape == 'cylinder': obj = create_cylinder(radius=side, height=side, mass=mass, color=color) elif shape == 'capsule': obj = create_capsule(radius=side, height=side, mass=mass, color=color) else: raise ValueError(shape) return obj
def add_block(world, idx=0, **kwargs): # TODO: automatically produce a unique name color = 'green' #block_type = '{}_block'.format(color) block_type = 'block'.format(color) #block_type = BLOCK_TEMPLATE.format(BLOCK_SIZES[-1], BLOCK_COLORS[0]) #block_type = 'potted_meat_can' name = name_from_type(block_type, idx) #world.add_body(name) #print(get_aabb_extent(get_aabb(world.get_body(name)))) side = BIG_BLOCK_SIDE body = create_box(w=side, l=side, h=side, color=COLOR_FROM_NAME[color]) world.add(name, body) pose2d_on_surface(world, name, COUNTERS[0], **kwargs) return name
def add_walls(ros_world): thickness = 0.01 # 0.005 | 0.01 | 0.02 height = 0.11 # 0.11 | 0.12 [table_name] = ros_world.perception.get_surfaces() #table_body = ros_world.get_body(table_name) table_info = ros_world.perception.info_from_name[table_name] #draw_pose(table_info.pose, length=1) with ros_world: #aabb = aabb_from_points(apply_affine(table_info.pose, table_info.type.vertices)) aabb = aabb_from_points(table_info.type.vertices) draw_aabb(aabb) # pose = get_pose(table_body) # pose = ros_world.get_pose(table_name) # aabb = approximate_as_prism(table_body, pose) x, y, z = get_aabb_center(aabb) l, w, h = get_aabb_extent(aabb) #right_wall = create_box(l, thickness, height) #set_pose(right_wall, multiply(table_info.pose, (Pose(point=[x, y - (w + thickness) / 2., z + (h + height) / 2.])))) #bottom_wall = create_box(thickness, w / 2., height) #set_point(bottom_wall, [x - (l + thickness) / 2., y - w / 4., z + (h + height) / 2.]) top_wall = create_box(thickness, w, height) set_pose( top_wall, multiply(table_info.pose, (Pose( point=[x + (l + thickness) / 2., y, z + (h + height) / 2.])))) walls = [top_wall] #walls = [right_wall, top_wall] # , bottom_wall] for i, body in enumerate(walls): name = create_name('wall', i + 1) ros_world.perception.sim_bodies[name] = body ros_world.perception.sim_items[name] = None #wait_for_user() return walls # Return names?
def _load_bodies(self, real_world): with ClientSaver(self.client): assert not self.bodies #for real_body in self.bodies.values(): # remove_body(real_body) self.bodies = {} self.initial_poses = {} with HideOutput(): for name, real_body in real_world.perception.sim_bodies.items( ): if name in real_world.perception.get_surfaces(): self.add_body(name, fixed=True) elif name in real_world.perception.get_items(): with real_world: model_info = get_model_info(real_body) # self.bodies[item] = clone_body(real_world.perception.get_body(item), client=self.client) if 'wall' in name: with real_world: self.bodies[name] = clone_body( real_body, client=self.client) elif 'cylinder' in name: self.bodies[name] = create_cylinder( *model_info.path) elif 'box' in name: self.bodies[name] = create_box(*model_info.path) elif model_info is None: self.add_body(name, fixed=False) else: self.bodies[name] = load_model_info(model_info) else: raise ValueError(name) # TODO: the floor might not be added which causes the indices to misalign self.body_mapping[self.bodies[name]] = real_body if name not in self.get_held(): self.initial_poses[name] = Pose( real_world.perception.get_pose(name)) self.set_initial()
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5): floor_extent = 5.0 base_limits = (-floor_extent/2.*np.ones(2), +floor_extent/2.*np.ones(2)) floor_height = 0.001 floor = create_box(floor_extent, floor_extent, floor_height, color=TAN) set_point(floor, Point(z=-floor_height/2.)) wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall1, Point(y=floor_extent/2., z=wall_side/2.)) wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall2, Point(y=-floor_extent/2., z=wall_side/2.)) wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall3, Point(x=floor_extent/2., z=wall_side/2.)) wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall4, Point(x=-floor_extent/2., z=wall_side/2.)) walls = [wall1, wall2, wall3, wall4] initial_surfaces = OrderedDict() for _ in range(n_obstacles): body = create_box(obst_width, obst_width, obst_height, color=GREY) initial_surfaces[body] = floor obstacles = walls + list(initial_surfaces) initial_conf = np.array([+floor_extent/3, -floor_extent/3, 3*PI/4]) goal_conf = -initial_conf with HideOutput(): robot = load_model(TURTLEBOT_URDF) base_joints = joints_from_names(robot, BASE_JOINTS) # base_link = child_link_from_joint(base_joints[-1]) base_link = link_from_name(robot, BASE_LINK_NAME) set_all_color(robot, BLUE) dump_body(robot) set_point(robot, Point(z=stable_z(robot, floor))) draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5) set_joint_positions(robot, base_joints, initial_conf) sample_placements(initial_surfaces, obstacles=[robot] + walls, savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)], min_distances=10e-2) return robot, base_limits, goal_conf, obstacles
def main(): parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-arm', required=True) parser.add_argument('-grasp', required=True) parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() arm = args.arm other_arm = get_other_arm(arm) grasp_type = args.grasp connect(use_gui=args.viewer) add_data_path() robot = p.loadURDF("models/pr2_description/pr2_fixed_torso.urdf") set_arm_conf(robot, arm, get_carry_conf(arm, grasp_type)) set_arm_conf(robot, other_arm, arm_conf(other_arm, REST_LEFT_ARM)) #plane = p.loadURDF("plane.urdf") table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) box = create_box(.07, .05, .15) create_inverse_reachability(robot, box, table, arm=arm, grasp_type=grasp_type) disconnect()
def main(num_iterations=10): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") side = 0.05 block = create_box(w=side, l=side, h=side, color=RED) start_time = time.time() with LockRenderer(): with HideOutput(): # TODO: MOVO must be loaded last robot = load_model(MOVO_URDF, fixed_base=True) #set_all_color(robot, color=MOVO_COLOR) assign_link_colors(robot) base_joints = joints_from_names(robot, BASE_JOINTS) draw_base_limits((get_min_limits( robot, base_joints), get_max_limits(robot, base_joints)), z=1e-2) print('Load time: {:.3f}'.format(elapsed_time(start_time))) dump_body(robot) #print(get_colliding(robot)) #for arm in ARMS: # test_close_gripper(robot, arm) #test_grasps(robot, block) arm = RIGHT tool_link = link_from_name(robot, TOOL_LINK.format(arm)) #joint_names = HEAD_JOINTS #joints = joints_from_names(robot, joint_names) joints = base_joints + get_arm_joints(robot, arm) #joints = get_movable_joints(robot) print('Joints:', get_joint_names(robot, joints)) ik_info = MOVO_INFOS[arm] print_ik_warning(ik_info) ik_joints = get_ik_joints(robot, ik_info, tool_link) #fixed_joints = [] fixed_joints = ik_joints[:1] #fixed_joints = ik_joints wait_if_gui('Start?') sample_fn = get_sample_fn(robot, joints) handles = [] for i in range(num_iterations): conf = sample_fn() print('Iteration: {}/{} | Conf: {}'.format(i + 1, num_iterations, np.array(conf))) set_joint_positions(robot, joints, conf) tool_pose = get_link_pose(robot, tool_link) remove_handles(handles) handles = draw_pose(tool_pose) wait_if_gui() #conf = next(ikfast_inverse_kinematics(robot, MOVO_INFOS[arm], tool_link, tool_pose, # fixed_joints=fixed_joints, max_time=0.1), None) #if conf is not None: # set_joint_positions(robot, ik_joints, conf) #wait_if_gui() test_retraction(robot, ik_info, tool_link, fixed_joints=fixed_joints, max_time=0.05, max_candidates=100) disconnect()
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) #ycb_path = os.path.join(DRAKE_PATH, DRAKE_YCB) #ycb_path = os.path.join(YCB_PATH, YCB_TEMPLATE.format('003_cracker_box')) #print(ycb_path) #load_pybullet(ycb_path) with LockRenderer(): draw_pose(unit_pose(), length=1, width=1) floor = create_floor() set_point(floor, Point(z=-EPSILON)) table = create_table(width=TABLE_WIDTH, length=TABLE_WIDTH / 2, height=TABLE_WIDTH / 2, top_color=TAN, cylinder=False) #set_euler(table, Euler(yaw=np.pi/2)) with HideOutput(False): # 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_path = get_file_path(__file__, 'mit_arch_suction_gripper/mit_arch_suction_gripper.urdf') robot = load_pybullet(robot_path, fixed_base=True) #dump_body(robot) #robot = create_cylinder(radius=0.5*BLOCK_SIDE, height=4*BLOCK_SIDE) # vacuum gripper block1 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=RED) block_z = stable_z(block1, table) set_point(block1, Point(z=block_z)) block2 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=GREEN) set_point(block2, Point(x=+0.25, z=block_z)) block3 = create_box(w=BLOCK_SIDE, l=BLOCK_SIDE, h=BLOCK_SIDE, color=BLUE) set_point(block3, Point(x=-0.15, z=block_z)) blocks = [block1, block2, block3] add_line(Point(x=-TABLE_WIDTH / 2, z=block_z - BLOCK_SIDE / 2 + EPSILON), Point(x=+TABLE_WIDTH / 2, z=block_z - BLOCK_SIDE / 2 + EPSILON), color=RED) set_camera_pose(camera_point=Point(y=-1, z=block_z + 1), target_point=Point(z=block_z)) wait_for_user() 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) y_grasp, x_grasp = get_top_grasps(block1, tool_pose=unit_pose(), grasp_length=0.03, under=False) grasp = y_grasp # fingers won't collide gripper_pose = multiply(block_pose, invert(grasp)) set_pose(robot, multiply(gripper_pose, invert(base_from_tool))) wait_for_user('Finish?') disconnect()
def add_obstacles(): ceiling = create_box(w=0.2, l=0.2, h=0.01, color=RED) set_point(ceiling, Point(z=0.2)) pole = create_cylinder(radius=0.025, height=0.1, color=RED) set_point(pole, Point(z=0.15))
def main(floor_width=2.0): # Creates a pybullet world and a visualizer for it connect(use_gui=True) identity_pose = (unit_point(), unit_quat()) origin_handles = draw_pose( identity_pose, length=1.0 ) # Draws the origin coordinate system (x:RED, y:GREEN, z:BLUE) # Bodies are described by an integer index floor = create_box(w=floor_width, l=floor_width, h=0.001, color=TAN) # Creates a tan box object for the floor set_point(floor, [0, 0, -0.001 / 2.]) # Sets the [x,y,z] translation of the floor obstacle = create_box(w=0.5, l=0.5, h=0.1, color=RED) # Creates a red box obstacle set_point( obstacle, [0.5, 0.5, 0.1 / 2.]) # Sets the [x,y,z] position of the obstacle print('Position:', get_point(obstacle)) set_euler(obstacle, [0, 0, np.pi / 4 ]) # Sets the [roll,pitch,yaw] orientation of the obstacle print('Orientation:', get_euler(obstacle)) with LockRenderer( ): # Temporarily prevents the renderer from updating for improved loading efficiency with HideOutput(): # Temporarily suppresses pybullet output robot = load_model(ROOMBA_URDF) # Loads a robot from a *.urdf file robot_z = stable_z( robot, floor ) # Returns the z offset required for robot to be placed on floor set_point(robot, [0, 0, robot_z]) # Sets the z position of the robot dump_body(robot) # Prints joint and link information about robot set_all_static() # Joints are also described by an integer index # The turtlebot has explicit joints representing x, y, theta x_joint = joint_from_name(robot, 'x') # Looks up the robot joint named 'x' y_joint = joint_from_name(robot, 'y') # Looks up the robot joint named 'y' theta_joint = joint_from_name( robot, 'theta') # Looks up the robot joint named 'theta' joints = [x_joint, y_joint, theta_joint] base_link = link_from_name( robot, 'base_link') # Looks up the robot link named 'base_link' world_from_obstacle = get_pose( obstacle ) # Returns the pose of the origin of obstacle wrt the world frame obstacle_aabb = get_subtree_aabb(obstacle) draw_aabb(obstacle_aabb) random.seed(0) # Sets the random number generator state handles = [] for i in range(10): for handle in handles: remove_debug(handle) print('\nIteration: {}'.format(i)) x = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, x_joint, x) # Sets the current value of the x joint y = random.uniform(-floor_width / 2., floor_width / 2.) set_joint_position(robot, y_joint, y) # Sets the current value of the y joint yaw = random.uniform(-np.pi, np.pi) set_joint_position(robot, theta_joint, yaw) # Sets the current value of the theta joint values = get_joint_positions( robot, joints) # Obtains the current values for the specified joints print('Joint values: [x={:.3f}, y={:.3f}, yaw={:.3f}]'.format(*values)) world_from_robot = get_link_pose( robot, base_link) # Returns the pose of base_link wrt the world frame position, quaternion = world_from_robot # Decomposing pose into position and orientation (quaternion) x, y, z = position # Decomposing position into x, y, z print('Base link position: [x={:.3f}, y={:.3f}, z={:.3f}]'.format( x, y, z)) euler = euler_from_quat( quaternion) # Converting from quaternion to euler angles roll, pitch, yaw = euler # Decomposing orientation into roll, pitch, yaw print('Base link orientation: [roll={:.3f}, pitch={:.3f}, yaw={:.3f}]'. format(roll, pitch, yaw)) handles.extend( draw_pose(world_from_robot, length=0.5) ) # # Draws the base coordinate system (x:RED, y:GREEN, z:BLUE) obstacle_from_robot = multiply( invert(world_from_obstacle), world_from_robot) # Relative transformation from robot to obstacle robot_aabb = get_subtree_aabb( robot, base_link) # Computes the robot's axis-aligned bounding box (AABB) lower, upper = robot_aabb # Decomposing the AABB into the lower and upper extrema center = (lower + upper) / 2. # Computing the center of the AABB extent = upper - lower # Computing the dimensions of the AABB handles.extend(draw_aabb(robot_aabb)) collision = pairwise_collision( robot, obstacle ) # Checks whether robot is currently colliding with obstacle print('Collision: {}'.format(collision)) wait_for_duration(1.0) # Like sleep() but also updates the viewer wait_for_user() # Like raw_input() but also updates the viewer # Destroys the pybullet world disconnect()
def eval_task_with_seed(domain, seed, learner, sample_strategy=DIVERSELK, task_lengthscale=None, obstacle=True): if task_lengthscale is not None: learner.task_lengthscale = task_lengthscale print('sample a new task') current_wd, trial_wd = start_task() ### fill in additional object item_ranges = {} ### sim_world, collector, task, feature, evalfunc, saver = sample_task_with_seed( domain.skill, seed=seed, visualize=False, item_ranges=item_ranges) context = domain.context_from_feature(feature) print('context=', *context) # create coffee machine if obstacle: bowl_name = 'bowl' cup_name = 'cup' if domain.skill == 'scoop': cup_name = 'spoon' for key in sim_world.perception.sim_bodies: if bowl_name in key: bowl_name = key if cup_name in key: cup_name = key cylinder_size = (.01, .03) dim_bowl = get_aabb_extent( p.getAABB(sim_world.perception.sim_bodies[bowl_name])) dim_cup = get_aabb_extent( p.getAABB(sim_world.perception.sim_bodies[cup_name])) bowl_point = get_point(sim_world.perception.sim_bodies[bowl_name]) bowl_point = np.array(bowl_point) cylinder_point = bowl_point.copy() cylinder_offset = 1.2 if domain.skill == 'scoop': cylinder_offset = 1.6 cylinder_point[2] += (dim_bowl[2] + dim_cup[2]) * (np.random.random_sample() * .4 + cylinder_offset) \ + cylinder_size[1] / 2. # TODO figure out the task space cylinder_name = create_name('cylinder', 1) obstacle = create_cylinder(*cylinder_size, color=(0, 0, 0, 1)) INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(cylinder_name, cylinder_size, 1, 1) sim_world.perception.sim_bodies[cylinder_name] = obstacle sim_world.perception.sim_items[cylinder_name] = None set_pose(obstacle, (cylinder_point, unit_quat())) box_name = create_name('box', 1) box1_size = (max(.17, dim_bowl[0] / 2 + cylinder_size[0] * 2), 0.01, 0.01) if domain.skill == 'scoop': box1_size = (max(.23, dim_bowl[0] / 2 + cylinder_size[0] * 2 + 0.05), 0.01, 0.01) obstacle = create_box(*box1_size) INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box1_size, 1, 1) sim_world.perception.sim_bodies[box_name] = obstacle sim_world.perception.sim_items[box_name] = None box1_point = cylinder_point.copy() box1_point[2] += cylinder_size[1] / 2 + box1_size[2] / 2 box1_point[0] += box1_size[0] / 2 - cylinder_size[0] * 2 set_pose(obstacle, (box1_point, unit_quat())) box_name = create_name('box', 2) box2_size = (0.01, .01, box1_point[2] - bowl_point[2] + box1_size[2] / 2) obstacle = create_box(*box2_size) INFO_FROM_BODY[(CLIENT, obstacle)] = ModelInfo(box_name, box2_size, 1, 1) sim_world.perception.sim_bodies[box_name] = obstacle sim_world.perception.sim_items[box_name] = None box2_point = bowl_point.copy() box2_point[2] += box2_size[2] / 2 box2_point[0] = box1_point[0] + box1_size[0] / 2 set_pose(obstacle, (box2_point, unit_quat())) result = get_sample_strategy_result(sample_strategy, learner, context, sim_world, collector, task, feature, evalfunc, saver) print('context=', *context) complete_task(sim_world, current_wd, trial_wd) return result
def problem1(n_obstacles=10, wall_side=0.1, obst_width=0.25, obst_height=0.5): floor_extent = 5.0 base_limits = (-floor_extent / 2. * np.ones(2), +floor_extent / 2. * np.ones(2)) floor_height = 0.001 floor = create_box(floor_extent, floor_extent, floor_height, color=TAN) set_point(floor, Point(z=-floor_height / 2.)) wall1 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall1, Point(y=floor_extent / 2., z=wall_side / 2.)) wall2 = create_box(floor_extent + wall_side, wall_side, wall_side, color=GREY) set_point(wall2, Point(y=-floor_extent / 2., z=wall_side / 2.)) wall3 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall3, Point(x=floor_extent / 2., z=wall_side / 2.)) wall4 = create_box(wall_side, floor_extent + wall_side, wall_side, color=GREY) set_point(wall4, Point(x=-floor_extent / 2., z=wall_side / 2.)) wall5 = create_box(obst_width, obst_width, obst_height, color=GREY) set_point(wall5, Point(z=obst_height / 2.)) walls = [wall1, wall2, wall3, wall4, wall5] initial_surfaces = OrderedDict() for _ in range(n_obstacles - 1): body = create_box(obst_width, obst_width, obst_height, color=GREY) initial_surfaces[body] = floor pillars = list(initial_surfaces) obstacles = walls + pillars initial_conf = np.array([+floor_extent / 3, -floor_extent / 3, 3 * PI / 4]) goal_conf = -initial_conf robot = load_pybullet(TURTLEBOT_URDF, fixed_base=True, merge=True, sat=False) base_joints = joints_from_names(robot, BASE_JOINTS) # base_link = child_link_from_joint(base_joints[-1]) base_link = link_from_name(robot, BASE_LINK_NAME) set_all_color(robot, BLUE) dump_body(robot) set_point(robot, Point(z=stable_z(robot, floor))) #set_point(robot, Point(z=base_aligned_z(robot))) draw_pose(Pose(), parent=robot, parent_link=base_link, length=0.5) set_joint_positions(robot, base_joints, initial_conf) sample_placements( initial_surfaces, obstacles=[robot] + walls, savers=[BodySaver(robot, joints=base_joints, positions=goal_conf)], min_distances=10e-2) # The first calls appear to be the slowest # times = [] # for body1, body2 in combinations(pillars, r=2): # start_time = time.time() # colliding = pairwise_collision(body1, body2) # runtime = elapsed_time(start_time) # print(colliding, runtime) # times.append(runtime) # print(times) return robot, base_limits, goal_conf, obstacles
def main(use_turtlebot=True): parser = argparse.ArgumentParser() parser.add_argument('-sim', action='store_true') parser.add_argument('-video', action='store_true') args = parser.parse_args() video = 'video.mp4' if args.video else None connect(use_gui=True, mp4=video) #set_renderer(enable=False) # print(list_pybullet_data()) # print(list_pybullet_robots()) draw_global_system() set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5), target_point=Point(-1.5, +1.5, 0)) plane = load_plane() #door = load_pybullet('models/door.urdf', fixed_base=True) # From drake #set_point(door, Point(z=-.1)) door = create_door() #set_position(door, z=base_aligned_z(door)) set_point(door, base_aligned(door)) #set_collision_margin(door, link=0, margin=0.) set_configuration(door, [math.radians(-5)]) dump_body(door) door_joint = get_movable_joints(door)[0] door_link = child_link_from_joint(door_joint) #draw_pose(get_com_pose(door, door_link), parent=door, parent_link=door_link) draw_pose(Pose(), parent=door, parent_link=door_link) wait_if_gui() ########## start_x = +2 target_x = -start_x if not use_turtlebot: side = 0.25 robot = create_box(w=side, l=side, h=side, mass=5., color=BLUE) set_position(robot, x=start_x) #set_velocity(robot, linear=Point(x=-1)) else: turtlebot_urdf = os.path.abspath(TURTLEBOT_URDF) print(turtlebot_urdf) #print(read(turtlebot_urdf)) robot = load_pybullet(turtlebot_urdf, merge=True, fixed_base=True) robot_joints = get_movable_joints(robot)[:3] set_joint_positions(robot, robot_joints, [start_x, 0, PI]) set_all_color(robot, BLUE) set_position(robot, z=base_aligned_z(robot)) dump_body(robot) ########## set_renderer(enable=True) #test_door(door) if args.sim: test_simulation(robot, target_x, video) else: assert use_turtlebot # TODO: extend to the block test_kinematic(robot, door, target_x) disconnect()
def main(): # TODO: teleporting kuka arm parser = argparse.ArgumentParser() # Automatically includes help parser.add_argument('-viewer', action='store_true', help='enable viewer.') args = parser.parse_args() client = connect(use_gui=args.viewer) add_data_path() #planeId = p.loadURDF("plane.urdf") table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107) box = create_box(.07, .05, .15) # boxId = p.loadURDF("r2d2.urdf",cubeStartPos, cubeStartOrientation) #pr2 = p.loadURDF("pr2_description/pr2.urdf") pr2 = p.loadURDF("pr2_description/pr2_fixed_torso.urdf") #pr2 = p.loadURDF("/Users/caelan/Programs/Installation/pr2_drake/pr2_local2.urdf",) #useFixedBase=0,) #flags=p.URDF_USE_SELF_COLLISION) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_PARENT) #flags=p.URDF_USE_SELF_COLLISION_EXCLUDE_ALL_PARENTS) #pr2 = p.loadURDF("pr2_drake/urdf/pr2_simplified.urdf", useFixedBase=False) initially_colliding = get_colliding_links(pr2) print len(initially_colliding) origin = (0, 0, 0) print p.getNumConstraints() # TODO: no way of controlling the base position by itself # TODO: PR2 seems to collide with itself frequently # real_time = False # p.setRealTimeSimulation(real_time) # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # while True: # control_joints(pr2, left_joints, TOP_HOLDING_LEFT_ARM) # if not real_time: # p.stepSimulation() # A CollisionMap robot allows the user to specify self-collision regions indexed by the values of two joints. # GetRigidlyAttachedLinks print pr2 # for i in range (10000): # p.stepSimulation() # time.sleep(1./240.) #print get_joint_names(pr2) print[get_joint_name(pr2, joint) for joint in get_movable_joints(pr2)] print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #open_gripper(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_limits(pr2, joint_from_name(pr2, LEFT_GRIPPER)) #print get_joint_position(pr2, joint_from_name(pr2, LEFT_GRIPPER)) print self_collision(pr2) """ print p.getNumConstraints() constraint = fixed_constraint(pr2, -1, box, -1) # table p.changeConstraint(constraint) print p.getNumConstraints() print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) p.stepSimulation() raw_input('Continue?') set_point(pr2, (-2, 0, 0)) p.stepSimulation() p.changeConstraint(constraint) print p.getConstraintInfo(constraint) print p.getConstraintState(constraint) raw_input('Continue?') print get_point(pr2) raw_input('Continue?') """ # TODO: would be good if we could set the joint directly print set_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME), 0.2) # Updates automatically print get_joint_position(pr2, joint_from_name(pr2, TORSO_JOINT_NAME)) #return left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] right_joints = [joint_from_name(pr2, name) for name in RIGHT_JOINT_NAMES] print set_joint_positions( pr2, left_joints, TOP_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM print set_joint_positions( pr2, right_joints, REST_RIGHT_ARM) # TOP_HOLDING_RIGHT_ARM | REST_RIGHT_ARM print get_body_name(pr2) print get_body_names() # print p.getBodyUniqueId(pr2) print get_joint_names(pr2) #for joint, value in zip(LEFT_ARM_JOINTS, REST_LEFT_ARM): # set_joint_position(pr2, joint, value) # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # #print name, joint, get_joint_position(pr2, joint), value # print name, get_joint_limits(pr2, joint), get_joint_type(pr2, joint), get_link_name(pr2, joint) # set_joint_position(pr2, joint, value) # #print name, joint, get_joint_position(pr2, joint), value # for name, value in zip(RIGHT_JOINT_NAMES, REST_RIGHT_ARM): # set_joint_position(pr2, joint_from_name(pr2, name), value) print p.getNumJoints(pr2) jointId = 0 print p.getJointInfo(pr2, jointId) print p.getJointState(pr2, jointId) # for i in xrange(10): # #lower, upper = BASE_LIMITS # #q = np.random.rand(len(lower))*(np.array(upper) - np.array(lower)) + lower # q = np.random.uniform(*BASE_LIMITS) # theta = np.random.uniform(*REVOLUTE_LIMITS) # quat = z_rotation(theta) # print q, theta, quat, env_collision(pr2) # #set_point(pr2, q) # set_pose(pr2, q, quat) # #p.getMouseEvents() # #p.getKeyboardEvents() # raw_input('Continue?') # Stalls because waiting for input # # # TODO: self collisions # for i in xrange(10): # for name in LEFT_JOINT_NAMES: # joint = joint_from_name(pr2, name) # value = np.random.uniform(*get_joint_limits(pr2, joint)) # set_joint_position(pr2, joint, value) # raw_input('Continue?') #start = (-2, -2, 0) #set_base_values(pr2, start) # #start = get_base_values(pr2) # goal = (2, 2, 0) # p.addUserDebugLine(start, goal, lineColorRGB=(1, 1, 0)) # addUserDebugText # print start, goal # raw_input('Plan?') # path = plan_base_motion(pr2, goal) # print path # if path is None: # return # print len(path) # for bq in path: # set_base_values(pr2, bq) # raw_input('Continue?') # left_joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] # for joint in left_joints: # print joint, get_joint_name(pr2, joint), get_joint_limits(pr2, joint), \ # is_circular(pr2, joint), get_joint_position(pr2, joint) # # #goal = np.zeros(len(left_joints)) # goal = [] # for name, value in zip(LEFT_JOINT_NAMES, REST_LEFT_ARM): # joint = joint_from_name(pr2, name) # goal.append(wrap_joint(pr2, joint, value)) # # path = plan_joint_motion(pr2, left_joints, goal) # print path # for q in path:s # set_joint_positions(pr2, left_joints, q) # raw_input('Continue?') print p.JOINT_REVOLUTE, p.JOINT_PRISMATIC, p.JOINT_FIXED, p.JOINT_POINT2POINT, p.JOINT_GEAR # 0 1 4 5 6 movable_joints = get_movable_joints(pr2) print len(movable_joints) for joint in xrange(get_num_joints(pr2)): if is_movable(pr2, joint): print joint, get_joint_name(pr2, joint), get_joint_type( pr2, joint), get_joint_limits(pr2, joint) #joints = [joint_from_name(pr2, name) for name in LEFT_JOINT_NAMES] #set_joint_positions(pr2, joints, sample_joints(pr2, joints)) #print get_joint_positions(pr2, joints) # Need to print before the display updates? # set_base_values(pr2, (1, -1, -np.pi/4)) # movable_joints = get_movable_joints(pr2) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # print gripper_pose # print get_joint_positions(pr2, movable_joints) # p.addUserDebugLine(origin, gripper_pose[0], lineColorRGB=(1, 0, 0)) # p.stepSimulation() # raw_input('Pre2 IK') # set_joint_positions(pr2, left_joints, SIDE_HOLDING_LEFT_ARM) # TOP_HOLDING_LEFT_ARM | SIDE_HOLDING_LEFT_ARM # print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Pre IK') # conf = inverse_kinematics(pr2, gripper_pose) # Doesn't automatically set configuraitons # print conf # print get_joint_positions(pr2, movable_joints) # set_joint_positions(pr2, movable_joints, conf) # print get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #print get_joint_positions(pr2, movable_joints) # p.stepSimulation() # raw_input('Post IK') # return # print pose_from_tform(TOOL_TFORM) # gripper_pose = get_link_pose(pr2, link_from_name(pr2, LEFT_ARM_LINK)) # #gripper_pose = multiply(gripper_pose, TOOL_POSE) # #gripper_pose = get_gripper_pose(pr2) # for i, grasp_pose in enumerate(get_top_grasps(box)): # grasp_pose = multiply(TOOL_POSE, grasp_pose) # box_pose = multiply(gripper_pose, grasp_pose) # set_pose(box, *box_pose) # print get_pose(box) # raw_input('Grasp {}'.format(i)) # return torso = joint_from_name(pr2, TORSO_JOINT_NAME) torso_point, torso_quat = get_link_pose(pr2, torso) #torso_constraint = p.createConstraint(pr2, torso, -1, -1, # p.JOINT_FIXED, jointAxis=[0] * 3, # JOINT_FIXED # parentFramePosition=torso_point, # childFramePosition=torso_quat) create_inverse_reachability(pr2, box, table) ir_database = load_inverse_reachability() print len(ir_database) return link = link_from_name(pr2, LEFT_ARM_LINK) point, quat = get_link_pose(pr2, link) print point, quat p.addUserDebugLine(origin, point, lineColorRGB=(1, 1, 0)) # addUserDebugText raw_input('Continue?') current_conf = get_joint_positions(pr2, movable_joints) #ik_conf = p.calculateInverseKinematics(pr2, link, point) #ik_conf = p.calculateInverseKinematics(pr2, link, point, quat) min_limits = [get_joint_limits(pr2, joint)[0] for joint in movable_joints] max_limits = [get_joint_limits(pr2, joint)[1] for joint in movable_joints] max_velocities = [ get_max_velocity(pr2, joint) for joint in movable_joints ] # Range of Jacobian print min_limits print max_limits print max_velocities ik_conf = p.calculateInverseKinematics(pr2, link, point, quat, lowerLimits=min_limits, upperLimits=max_limits, jointRanges=max_velocities, restPoses=current_conf) value_from_joint = dict(zip(movable_joints, ik_conf)) print[value_from_joint[joint] for joint in joints] #print len(ik_conf), ik_conf set_joint_positions(pr2, movable_joints, ik_conf) #print len(movable_joints), get_joint_positions(pr2, movable_joints) print get_joint_positions(pr2, joints) raw_input('Finish?') p.disconnect()