def main(): # TODO: move to pybullet-planning for now parser = argparse.ArgumentParser() parser.add_argument('-viewer', action='store_true', help='enable the viewer while planning') args = parser.parse_args() print(args) connect(use_gui=True) with LockRenderer(): draw_pose(unit_pose(), length=1) floor = create_floor() with HideOutput(): robot = load_pybullet(get_model_path(ROOMBA_URDF), fixed_base=True, scale=2.0) for link in get_all_links(robot): set_color(robot, link=link, color=ORANGE) robot_z = stable_z(robot, floor) set_point(robot, Point(z=robot_z)) #set_base_conf(robot, rover_confs[i]) data_path = add_data_path() shelf, = load_model(os.path.join(data_path, KIVA_SHELF_SDF), fixed_base=True) # TODO: returns a tuple dump_body(shelf) #draw_aabb(get_aabb(shelf)) wait_for_user() disconnect()
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 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 test_grasps(robot, node_points, elements): element = elements[-1] [element_body] = create_elements(node_points, [element]) phi = 0 link = link_from_name(robot, TOOL_NAME) link_pose = get_link_pose(robot, link) draw_pose(link_pose) #, parent=robot, parent_link=link) wait_for_user() n1, n2 = element p1 = node_points[n1] p2 = node_points[n2] length = np.linalg.norm(p2 - p1) # Bottom of cylinder is p1, top is p2 print(element, p1, p2) for phi in np.linspace(0, np.pi, 10, endpoint=True): theta = np.pi / 4 for t in np.linspace(-length / 2, length / 2, 10): translation = Pose( point=Point(z=-t) ) # Want to be moving backwards because +z is out end effector direction = Pose(euler=Euler(roll=np.pi / 2 + theta, pitch=phi)) angle = Pose(euler=Euler(yaw=1.2)) grasp_pose = multiply(angle, direction, translation) element_pose = multiply(link_pose, grasp_pose) set_pose(element_body, element_pose) wait_for_user()
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 main(display='execute'): # control | execute | step # One of the arm's gantry carriage is fixed when the other is moving. connect(use_gui=True) set_camera(yaw=-90, pitch=-40, distance=10, target_position=(0, 7.5, 0)) draw_pose(unit_pose(), length=1.0) disable_real_time() with HideOutput(): root_directory = os.path.dirname(os.path.abspath(__file__)) robot = load_pybullet(os.path.join(root_directory, ETH_RFL_URDF), fixed_base=True) # floor = load_model('models/short_floor.urdf') block = load_model(BLOCK_URDF, fixed_base=False) #link = link_from_name(robot, 'gantry_base_link') #print(get_aabb(robot, link)) block_x = -0.2 #block_y = 1 if ARM == 'right' else 13.5 #block_x = 10 block_y = 5. # set_pose(floor, Pose(Point(x=floor_x, y=1, z=1.3))) # set_pose(block, Pose(Point(x=floor_x, y=0.6, z=stable_z(block, floor)))) set_pose(block, Pose(Point(x=block_x, y=block_y, z=3.5))) # set_default_camera() dump_world() #print(get_camera()) saved_world = WorldSaver() with LockRenderer(): command = plan(robot, block, fixed=[], teleport=False) # fixed=[floor], if (command is None) or (display is None): print('Unable to find a plan! Quit?') wait_for_interrupt() disconnect() return saved_world.restore() update_state() print('{}?'.format(display)) wait_for_interrupt() if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.002) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_for_interrupt() disconnect()
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 test_retraction(robot, info, tool_link, distance=0.1, **kwargs): ik_joints = get_ik_joints(robot, info, tool_link) start_pose = get_link_pose(robot, tool_link) end_pose = multiply(start_pose, Pose(Point(z=-distance))) handles = [ add_line(point_from_pose(start_pose), point_from_pose(end_pose), color=BLUE) ] #handles.extend(draw_pose(start_pose)) #handles.extend(draw_pose(end_pose)) path = [] pose_path = list( interpolate_poses(start_pose, end_pose, pos_step_size=0.01)) for i, pose in enumerate(pose_path): print('Waypoint: {}/{}'.format(i + 1, len(pose_path))) handles.extend(draw_pose(pose)) conf = next( either_inverse_kinematics(robot, info, tool_link, pose, **kwargs), None) if conf is None: print('Failure!') path = None wait_for_user() break set_joint_positions(robot, ik_joints, conf) path.append(conf) wait_for_user() # for conf in islice(ikfast_inverse_kinematics(robot, info, tool_link, pose, max_attempts=INF, max_distance=0.5), 1): # set_joint_positions(robot, joints[:len(conf)], conf) # wait_for_user() remove_handles(handles) return path
def test_ik(robot, info, tool_link, tool_pose): draw_pose(tool_pose) # TODO: sort by one joint angle # TODO: prune based on proximity ik_joints = get_ik_joints(robot, info, tool_link) for conf in either_inverse_kinematics(robot, info, tool_link, tool_pose, use_pybullet=False, max_distance=INF, max_time=10, max_candidates=INF): # TODO: profile set_joint_positions(robot, ik_joints, conf) wait_for_user()
def fn(body, pose, grasp): obstacles = [body] + fixed gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose) approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose) draw_pose(gripper_pose, length=0.04) draw_pose(approach_pose, length=0.04) for _ in range(num_attempts): if USE_IKFAST: q_approach = sample_tool_ik(robot, approach_pose) if q_approach is not None: set_joint_positions(robot, movable_joints, q_approach) else: set_joint_positions(robot, movable_joints, sample_fn()) # Random seed q_approach = inverse_kinematics(robot, grasp.link, approach_pose) if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles): continue conf = BodyConf(robot, joints=movable_joints) if USE_IKFAST: q_grasp = sample_tool_ik(robot, gripper_pose, closest_only=True) if q_grasp is not None: set_joint_positions(robot, movable_joints, q_grasp) else: q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose) if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles): continue if teleport: path = [q_approach, q_grasp] else: conf.assign() #direction, _ = grasp.approach_pose #path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction, # quat_from_pose(approach_pose)) path = plan_direct_joint_motion(robot, conf.joints, q_grasp, obstacles=obstacles, \ self_collisions=self_collisions) if path is None: if DEBUG_FAILURE: user_input('Approach motion failed') continue command = Command([BodyPath(robot, path, joints=movable_joints), Attach(body, robot, grasp.link), BodyPath(robot, path[::-1], joints=movable_joints, attachments=[grasp])]) return (conf, command) # TODO: holding collisions return None
def draw_path(path2d, z=DRAW_Z, **kwargs): if path2d is None: return [] #return list(flatten(draw_pose(pose_from_pose2d(pose2d, z=z), **kwargs) for pose2d in path2d)) #return list(flatten(draw_pose2d(pose2d, z=z, **kwargs) for pose2d in path2d)) base_z = 1. start = path2d[0] mid_yaw = start[2] #mid_yaw = wrap_interval(mid_yaw) interval = (mid_yaw - PI, mid_yaw + PI) #interval = CIRCULAR_LIMITS draw_pose(pose_from_pose2d(start, z=base_z), length=1, **kwargs) # TODO: draw the current pose # TODO: line between orientations when there is a jump return list(flatten(draw_pose2d(pose2d, z=base_z+rescale_interval( wrap_interval(pose2d[2], interval=interval), old_interval=interval, new_interval=(-0.5, 0.5)), **kwargs) for pose2d in path2d))
def main(): # The URDF loader seems robust to package:// and slightly wrong relative paths? connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") with LockRenderer(): with HideOutput(): robot = load_model(MOVO_URDF, fixed_base=True) for link in get_links(robot): set_color(robot, color=apply_alpha(0.25 * np.ones(3), 1), link=link) 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) dump_body(robot) wait_for_user('Start?') #for arm in ARMS: # test_close_gripper(robot, arm) 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_joints = get_ik_joints(robot, MOVO_INFOS[arm], tool_link) #fixed_joints = [] fixed_joints = ik_joints[:1] #fixed_joints = ik_joints sample_fn = get_sample_fn(robot, joints) handles = [] for i in range(10): print('Iteration:', i) conf = sample_fn() print(conf) set_joint_positions(robot, joints, conf) tool_pose = get_link_pose(robot, tool_link) remove_handles(handles) handles = draw_pose(tool_pose) wait_for_user() #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_for_user() test_retraction(robot, MOVO_INFOS[arm], tool_link, fixed_joints=fixed_joints, max_time=0.1) disconnect()
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 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 add_camera(self, name, pose, camera_matrix, max_depth=KINECT_DEPTH, display=False): cone = get_viewcone(depth=max_depth, camera_matrix=camera_matrix, color=apply_alpha(RED, 0.1), mass=0, collision=False) set_pose(cone, pose) if display: kinect = load_pybullet(KINECT_URDF, fixed_base=True) set_pose(kinect, pose) set_color(kinect, BLACK) self.add(name, kinect) self.cameras[name] = Camera(cone, camera_matrix, max_depth) draw_pose(pose) step_simulation() return name
def main(): connect(use_gui=True) add_data_path() draw_pose(Pose(), length=1.) set_camera_pose(camera_point=[1, -1, 1]) plane = p.loadURDF("plane.urdf") with LockRenderer(): with HideOutput(True): robot = load_pybullet(FRANKA_URDF, fixed_base=True) assign_link_colors(robot, max_colors=3, s=0.5, v=1.) #set_all_color(robot, GREEN) obstacles = [plane] # TODO: collisions with the ground dump_body(robot) print('Start?') wait_for_user() info = PANDA_INFO tool_link = link_from_name(robot, 'panda_hand') draw_pose(Pose(), parent=robot, parent_link=tool_link) joints = get_movable_joints(robot) print('Joints', [get_joint_name(robot, joint) for joint in joints]) check_ik_solver(info) sample_fn = get_sample_fn(robot, joints) for i in range(10): print('Iteration:', i) conf = sample_fn() set_joint_positions(robot, joints, conf) wait_for_user() #test_ik(robot, info, tool_link, get_link_pose(robot, tool_link)) test_retraction(robot, info, tool_link, use_pybullet=False, max_distance=0.1, max_time=0.05, max_candidates=100) disconnect()
def test_grasps(robot, block): for arm in ARMS: gripper_joints = get_gripper_joints(robot, arm) tool_link = link_from_name(robot, TOOL_LINK.format(arm)) tool_pose = get_link_pose(robot, tool_link) #handles = draw_pose(tool_pose) #grasps = get_top_grasps(block, under=True, tool_pose=unit_pose()) grasps = get_side_grasps(block, under=True, tool_pose=unit_pose()) for i, grasp_pose in enumerate(grasps): block_pose = multiply(tool_pose, grasp_pose) set_pose(block, block_pose) close_until_collision(robot, gripper_joints, bodies=[block], open_conf=get_open_positions(robot, arm), closed_conf=get_closed_positions(robot, arm)) handles = draw_pose(block_pose) wait_if_gui('Grasp {}'.format(i)) remove_handles(handles)
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 load_pick_and_place(extrusion_name, scale=MILLIMETER, max_bricks=6): assert extrusion_name == 'choreo_brick_demo' root_directory = os.path.dirname(os.path.abspath(__file__)) bricks_directory = os.path.join(root_directory, PICKNPLACE_DIRECTORY, 'bricks') print('Name: {}'.format(extrusion_name)) with open( os.path.join(bricks_directory, PICKNPLACE_FILENAMES[extrusion_name]), 'r') as f: json_data = json.loads(f.read()) kuka_urdf = '../conrob_pybullet/models/kuka_kr6_r900/urdf/kuka_kr6_r900_mit_suction_gripper.urdf' obj_directory = os.path.join(bricks_directory, 'meshes', 'collision') with HideOutput(): #world = load_pybullet(os.path.join(bricks_directory, 'urdf', 'brick_demo.urdf')) robot = load_pybullet(os.path.join(root_directory, kuka_urdf), fixed_base=True) #set_point(robot, (0.14, 0, 0)) #dump_body(robot) pick_base_point = parse_point(json_data['pick_base_center_point']) draw_pose((pick_base_point, unit_quat())) place_base_point = parse_point(json_data['place_base_center_point']) draw_pose((place_base_point, unit_quat())) # workspace_geo_place_support_object_11 = pick_left_over_bricks_11 obstacle_from_name = {} for filename in json_data['pick_support_surface_file_names']: obstacle_from_name[strip_extension(filename)] = \ create_obj(os.path.join(obj_directory, filename), scale=scale, color=(0.5, 0.5, 0.5, 1)) for filename in json_data['place_support_surface_file_names']: obstacle_from_name[strip_extension(filename)] = \ create_obj(os.path.join(obj_directory, filename), scale=scale, color=(1., 0., 0., 1)) brick_from_index = {} for json_element in json_data['sequenced_elements']: index = json_element['order_id'] pick_body = create_obj(os.path.join( obj_directory, json_element['pick_element_geometry_file_name']), scale=scale, color=(0, 0, 1, 1)) add_body_name(pick_body, index) #place_body = create_obj(os.path.join(obj_directory, json_element['place_element_geometry_file_name']), # scale=scale, color=(0, 1, 0, 1)) #add_body_name(place_body, name) world_from_obj_pick = get_pose(pick_body) # [u'pick_grasp_plane', u'pick_grasp_retreat_plane', u'place_grasp_retreat_plane', # u'pick_grasp_approach_plane', u'place_grasp_approach_plane', u'place_grasp_plane'] ee_grasp_poses = [{ name: pose_from_tform(parse_transform(approach)) for name, approach in json_grasp.items() } for json_grasp in json_element['grasps']] # pick_grasp_plane is at the top of the object with z facing downwards grasp_index = 0 world_from_ee_pick = ee_grasp_poses[grasp_index]['pick_grasp_plane'] world_from_ee_place = ee_grasp_poses[grasp_index]['place_grasp_plane'] #draw_pose(world_from_ee_pick, length=0.04) #draw_pose(world_from_ee_place, length=0.04) ee_from_obj = multiply(invert(world_from_ee_pick), world_from_obj_pick) # Using pick frame world_from_obj_place = multiply(world_from_ee_place, ee_from_obj) #set_pose(pick_body, world_from_obj_place) grasps = [ Grasp( index, num, *[ multiply(invert(world_from_ee_pick[name]), world_from_obj_pick) for name in GRASP_NAMES ]) for num, world_from_ee_pick in enumerate(ee_grasp_poses) ] brick_from_index[index] = Brick( index=index, body=pick_body, initial_pose=WorldPose(index, world_from_obj_pick), goal_pose=WorldPose(index, world_from_obj_place), grasps=grasps, goal_supports=json_element.get('place_contact_ngh_ids', [])) # pick_contact_ngh_ids are movable element contact partial orders # pick_support_surface_file_names are fixed element contact partial orders return robot, brick_from_index, obstacle_from_name
def main(): parser = argparse.ArgumentParser() parser.add_argument('-c', '--cfree', action='store_true', help='When enabled, disables collision checking.') # parser.add_argument('-p', '--problem', default='test_pour', choices=sorted(problem_fn_from_name), # help='The name of the problem to solve.') parser.add_argument('--holonomic', action='store_true', # '-h', help='') parser.add_argument('-l', '--lock', action='store_false', help='') parser.add_argument('-s', '--seed', default=None, type=int, help='The random seed to use.') parser.add_argument('-v', '--viewer', action='store_false', help='') args = parser.parse_args() connect(use_gui=args.viewer) seed = args.seed #seed = 0 #seed = time.time() set_random_seed(seed=seed) # None: 2147483648 = 2**31 set_numpy_seed(seed=seed) print('Random seed:', get_random_seed(), random.random()) print('Numpy seed:', get_numpy_seed(), np.random.random()) ######################### robot, base_limits, goal_conf, obstacles = problem1() draw_base_limits(base_limits) custom_limits = create_custom_base_limits(robot, base_limits) base_joints = joints_from_names(robot, BASE_JOINTS) base_link = link_from_name(robot, BASE_LINK_NAME) if args.cfree: obstacles = [] # for obstacle in obstacles: # draw_aabb(get_aabb(obstacle)) # Updates automatically resolutions = None #resolutions = np.array([0.05, 0.05, math.radians(10)]) set_all_static() # Doesn't seem to affect region_aabb = AABB(lower=-np.ones(3), upper=+np.ones(3)) draw_aabb(region_aabb) step_simulation() # Need to call before get_bodies_in_region #update_scene() # TODO: https://github.com/bulletphysics/bullet3/pull/3331 bodies = get_bodies_in_region(region_aabb) print(len(bodies), bodies) # https://github.com/bulletphysics/bullet3/search?q=broadphase # https://github.com/bulletphysics/bullet3/search?p=1&q=getCachedOverlappingObjects&type=&utf8=%E2%9C%93 # https://andysomogyi.github.io/mechanica/bullet.html # http://www.cs.kent.edu/~ruttan/GameEngines/lectures/Bullet_User_Manual #draw_pose(get_link_pose(robot, base_link), length=0.5) for conf in [get_joint_positions(robot, base_joints), goal_conf]: draw_pose(pose_from_pose2d(conf, z=DRAW_Z), length=DRAW_LENGTH) aabb = get_aabb(robot) #aabb = get_subtree_aabb(robot, base_link) draw_aabb(aabb) for link in [BASE_LINK, base_link]: print(link, get_collision_data(robot, link), pairwise_link_collision(robot, link, robot, link)) ######################### saver = WorldSaver() start_time = time.time() profiler = Profiler(field='tottime', num=50) # tottime | cumtime | None profiler.save() with LockRenderer(lock=args.lock): path = plan_motion(robot, base_joints, goal_conf, holonomic=args.holonomic, obstacles=obstacles, custom_limits=custom_limits, resolutions=resolutions, use_aabb=True, cache=True, max_distance=0., restarts=2, iterations=20, smooth=20) # 20 | None saver.restore() #wait_for_duration(duration=1e-3) profiler.restore() ######################### solved = path is not None length = INF if path is None else len(path) cost = sum(get_distance_fn(robot, base_joints, weights=resolutions)(*pair) for pair in get_pairs(path)) print('Solved: {} | Length: {} | Cost: {:.3f} | Runtime: {:.3f} sec'.format( solved, length, cost, elapsed_time(start_time))) if path is None: disconnect() return iterate_path(robot, base_joints, path) disconnect()
def draw_waypoint(conf, z=DRAW_Z): return draw_pose(pose_from_pose2d(conf, z=z), length=DRAW_LENGTH)
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 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 fn(body, pose, grasp): obstacles = [body] + fixed gripper_pose = end_effector_from_body(pose.pose, grasp.grasp_pose) approach_pose = approach_from_grasp(grasp.approach_pose, gripper_pose) draw_pose(get_tool_pose(robot, ARM), length=0.04) draw_pose(approach_pose, length=0.04) draw_pose(gripper_pose, length=0.04) # print(movable_joints) # print(torso_arm) # wait_for_interrupt() # TODO: gantry_x_joint # TODO: proper inverse reachability base_link = link_from_name(robot, IK_BASE_FRAMES[ARM]) base_pose = get_link_pose(robot, base_link) body_point_in_base = point_from_pose(multiply(invert(base_pose), pose.pose)) y_joint = joint_from_name(robot, '{}_gantry_y_joint'.format(prefix_from_arm(ARM))) initial_y = get_joint_position(robot, y_joint) ik_y = initial_y + SIGN_FROM_ARM[ARM]*body_point_in_base[1] set_joint_position(robot, y_joint, ik_y) for _ in range(num_attempts): if USE_IKFAST: q_approach = sample_tool_ik(robot, ARM, approach_pose) if q_approach is not None: set_joint_positions(robot, torso_arm, q_approach) else: set_joint_positions(robot, torso_arm, sample_fn()) # Random seed q_approach = inverse_kinematics(robot, grasp.link, approach_pose) if (q_approach is None) or any(pairwise_collision(robot, b) for b in obstacles): print('- ik for approaching fails!') continue conf = BodyConf(robot, joints=arm_joints) if USE_IKFAST: q_grasp = sample_tool_ik(robot, ARM, gripper_pose, closest_only=True) if q_grasp is not None: set_joint_positions(robot, torso_arm, q_grasp) else: conf.assign() q_grasp = inverse_kinematics(robot, grasp.link, gripper_pose) if (q_grasp is None) or any(pairwise_collision(robot, b) for b in obstacles): print('- ik for grasp fails!') continue if teleport: path = [q_approach, q_grasp] else: conf.assign() #direction, _ = grasp.approach_pose #path = workspace_trajectory(robot, grasp.link, point_from_pose(approach_pose), -direction, # quat_from_pose(approach_pose)) path = plan_direct_joint_motion(robot, torso_arm, q_grasp, obstacles=obstacles, self_collisions=self_collisions) if path is None: if DEBUG_FAILURE: print('Approach motion failed') continue command = Command([BodyPath(robot, path, joints=torso_arm), Attach(body, robot, grasp.link), BodyPath(robot, path[::-1], joints=torso_arm, attachments=[grasp])]) return (conf, command) # TODO: holding collisions return None
def __init__(self, robot_name=FRANKA_CARTER, use_gui=True, full_kitchen=False): self.task = None self.interface = None self.client = connect(use_gui=use_gui) set_real_time(False) #set_caching(False) # Seems to make things worse disable_gravity() add_data_path() set_camera_pose(camera_point=[2, -1.5, 1]) if DEBUG: draw_pose(Pose(), length=3) #self.kitchen_yaml = load_yaml(KITCHEN_YAML) with HideOutput(enable=True): self.kitchen = load_pybullet(KITCHEN_PATH, fixed_base=True, cylinder=True) self.floor = load_pybullet('plane.urdf', fixed_base=True) z = stable_z(self.kitchen, self.floor) - get_point(self.floor)[2] point = np.array(get_point(self.kitchen)) - np.array([0, 0, z]) set_point(self.floor, point) self.robot_name = robot_name if self.robot_name == FRANKA_CARTER: urdf_path, yaml_path = FRANKA_CARTER_PATH, None #urdf_path, yaml_path = FRANKA_CARTER_PATH, FRANKA_YAML #elif self.robot_name == EVE: # urdf_path, yaml_path = EVE_PATH, None else: raise ValueError(self.robot_name) self.robot_yaml = yaml_path if yaml_path is None else load_yaml( yaml_path) with HideOutput(enable=True): self.robot = load_pybullet(urdf_path) #dump_body(self.robot) #chassis_pose = get_link_pose(self.robot, link_from_name(self.robot, 'chassis_link')) #wheel_pose = get_link_pose(self.robot, link_from_name(self.robot, 'left_wheel_link')) #wait_for_user() set_point(self.robot, Point(z=stable_z(self.robot, self.floor))) self.set_initial_conf() self.gripper = create_gripper(self.robot) self.environment_bodies = {} if full_kitchen: self._initialize_environment() self._initialize_ik(urdf_path) self.initial_saver = WorldSaver() self.body_from_name = {} # self.path_from_name = {} self.names_from_type = {} self.custom_limits = {} self.base_limits_handles = [] self.cameras = {} self.disabled_collisions = set() if self.robot_name == FRANKA_CARTER: self.disabled_collisions.update( tuple(link_from_name(self.robot, link) for link in pair) for pair in DISABLED_FRANKA_COLLISIONS) self.carry_conf = FConf(self.robot, self.arm_joints, self.default_conf) #self.calibrate_conf = Conf(self.robot, self.arm_joints, load_calibrate_conf(side='left')) self.calibrate_conf = FConf( self.robot, self.arm_joints, self.default_conf) # Must differ from carry_conf self.special_confs = [self.carry_conf] #, self.calibrate_conf] self.open_gq = FConf(self.robot, self.gripper_joints, get_max_limits(self.robot, self.gripper_joints)) self.closed_gq = FConf(self.robot, self.gripper_joints, get_min_limits(self.robot, self.gripper_joints)) self.gripper_confs = [self.open_gq, self.closed_gq] self.open_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.open_conf(joint)]) for joint in self.kitchen_joints } self.closed_kitchen_confs = { joint: FConf(self.kitchen, [joint], [self.closed_conf(joint)]) for joint in self.kitchen_joints } self._update_custom_limits() self._update_initial()
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 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 plan_extrusion(args_list, viewer=False, verify=False, verbose=False, watch=False): results = [] if not args_list: return results # TODO: setCollisionFilterGroupMask if not verbose: sys.stdout = open(os.devnull, 'w') problems = {args.problem for args in args_list} assert len(problems) == 1 [problem] = problems # TODO: change dir for pddlstream extrusion_path = get_extrusion_path(problem) if False: extrusion_path = transform_json(problem) element_from_id, node_points, ground_nodes = load_extrusion(extrusion_path, verbose=True) elements = sorted(element_from_id.values()) #node_points = transform_model(problem, elements, node_points, ground_nodes) connect(use_gui=viewer, shadows=SHADOWS, color=BACKGROUND_COLOR) with LockRenderer(lock=True): draw_pose(unit_pose(), length=1.) json_data = read_json(extrusion_path) draw_pose(parse_origin_pose(json_data)) draw_model(elements, node_points, ground_nodes) obstacles, robot = load_world() color = apply_alpha(BLACK, alpha=0) # 0, 1 #color = None element_bodies = dict( zip(elements, create_elements_bodies(node_points, elements, color=color))) set_extrusion_camera(node_points) #if viewer: # label_nodes(node_points) saver = WorldSaver() wait_if_gui() #visualize_stiffness(extrusion_path) #debug_elements(robot, node_points, node_order, elements) initial_position = point_from_pose( get_link_pose(robot, link_from_name(robot, TOOL_LINK))) checker = None plan = None for args in args_list: if args.stiffness and (checker is None): checker = create_stiffness_checker(extrusion_path, verbose=False) saver.restore() #initial_conf = get_joint_positions(robot, get_movable_joints(robot)) with LockRenderer(lock=not viewer): start_time = time.time() plan, data = None, {} with timeout(args.max_time): with Profiler(num=10, cumulative=False): plan, data = solve_extrusion(robot, obstacles, element_from_id, node_points, element_bodies, extrusion_path, ground_nodes, args, checker=checker) runtime = elapsed_time(start_time) sequence = recover_directed_sequence(plan) if verify: max_translation, max_rotation = compute_plan_deformation( extrusion_path, recover_sequence(plan)) valid = check_plan(extrusion_path, sequence) print('Valid:', valid) safe = validate_trajectories(element_bodies, obstacles, plan) print('Safe:', safe) data.update({ 'safe': safe, 'valid': valid, 'max_translation': max_translation, 'max_rotation': max_rotation, }) data.update({ 'runtime': runtime, 'num_elements': len(elements), 'ee_distance': compute_sequence_distance(node_points, sequence, start=initial_position, end=initial_position), 'print_distance': get_print_distance(plan, teleport=True), 'distance': get_print_distance(plan, teleport=False), 'sequence': sequence, 'parameters': get_global_parameters(), 'problem': args.problem, 'algorithm': args.algorithm, 'heuristic': args.bias, 'plan_extrusions': not args.disable, 'use_collisions': not args.cfree, 'use_stiffness': args.stiffness, }) print(data) results.append((args, data)) if True: data.update({ 'assembly': read_json(extrusion_path), 'plan': extract_plan_data(plan), # plan | trajectories }) plan_file = '{}_solution.json'.format(args.problem) plan_path = os.path.join('solutions', plan_file) write_json(plan_path, data) reset_simulation() disconnect() if watch and (plan is not None): args = args_list[-1] animate = not (args.disable or args.ee_only) connect(use_gui=True, shadows=SHADOWS, color=BACKGROUND_COLOR) # TODO: avoid reconnecting obstacles, robot = load_world() display_trajectories( node_points, ground_nodes, plan, #time_step=None, video=True, animate=animate) reset_simulation() disconnect() if not verbose: sys.stdout.close() return results