def main(display='execute'): # control | execute | step connect(use_gui=True) disable_real_time() draw_global_system() with HideOutput(): robot = load_model(DRAKE_IIWA_URDF) # KUKA_IIWA_URDF | DRAKE_IIWA_URDF floor = load_model('models/short_floor.urdf') block = load_model(BLOCK_URDF, fixed_base=False) set_pose(block, Pose(Point(y=0.5, z=stable_z(block, floor)))) set_default_camera(distance=2) dump_world() saved_world = WorldSaver() command = plan(robot, block, fixed=[floor], teleport=False) if (command is None) or (display is None): print('Unable to find a plan!') return saved_world.restore() update_state() wait_if_gui('{}?'.format(display)) if display == 'control': enable_gravity() command.control(real_time=False, dt=0) elif display == 'execute': command.refine(num_steps=10).execute(time_step=0.005) elif display == 'step': command.step() else: raise ValueError(display) print('Quit?') wait_if_gui() disconnect()
def simulate_parallel(robots, plan, time_step=0.1, speed_up=10., record=None): # None | video.mp4 # TODO: ensure the step size is appropriate makespan = compute_duration(plan) print('\nMakespan: {:.3f}'.format(makespan)) trajectories = extract_parallel_trajectories(plan) if trajectories is None: return wait_if_gui('Begin?') num_motion = sum(action.name == 'move' for action in plan) with VideoSaver(record): for t in inclusive_range(0, makespan, time_step): # if action.start <= t <= get_end(action): executing = Counter(traj.robot for traj in trajectories if traj.at(t) is not None) print('t={:.3f}/{:.3f} | executing={}'.format( t, makespan, executing)) for robot in robots: num = executing.get(robot, 0) if 2 <= num: raise RuntimeError( 'Robot {} simultaneously executing {} trajectories'. format(robot, num)) if (num_motion == 0) and (num == 0): set_configuration(robot, DUAL_CONF) #step_simulation() wait_for_duration(time_step / speed_up) wait_if_gui('Finish?')
def create_inverse_reachability(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500): tool_link = get_gripper_link(robot, arm) robot_saver = BodySaver(robot) gripper_from_base_list = [] grasps = GET_GRASPS[grasp_type](body) start_time = time.time() while len(gripper_from_base_list) < num_samples: box_pose = sample_placement(body, table) set_pose(body, box_pose) grasp_pose = random.choice(grasps) gripper_pose = multiply(box_pose, invert(grasp_pose)) for attempt in range(max_attempts): robot_saver.restore() base_conf = next(uniform_pose_generator(robot, gripper_pose)) #, reachable_range=(0., 1.))) #set_base_values(robot, base_conf) set_group_conf(robot, 'base', base_conf) if pairwise_collision(robot, table): continue grasp_conf = pr2_inverse_kinematics(robot, arm, gripper_pose) #, nearby_conf=USE_CURRENT) #conf = inverse_kinematics(robot, link, gripper_pose) if (grasp_conf is None) or pairwise_collision(robot, table): continue gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot)) #wait_if_gui() gripper_from_base_list.append(gripper_from_base) print('{} / {} | {} attempts | [{:.3f}]'.format( len(gripper_from_base_list), num_samples, attempt, elapsed_time(start_time))) wait_if_gui() break else: print('Failed to find a kinematic solution after {} attempts'.format(max_attempts)) return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
def test_close_gripper(robot, arm): gripper_joints = get_gripper_joints(robot, arm) extend_fn = get_extend_fn(robot, gripper_joints) for positions in extend_fn(get_open_positions(robot, arm), get_closed_positions(robot, arm)): set_joint_positions(robot, gripper_joints, positions) print(positions) wait_if_gui('Continue?')
def test_lis(world): #model = 'oil' model = 'soup' point_path = os.path.join(LIS_DIRECTORY, 'clouds', CLOUDS[model]) mesh_path = os.path.join(LIS_DIRECTORY, 'meshes', MESHES[model]) # off | ply | wrl #ycb_path = os.path.join(DATA_DIRECTORY, 'ycb', 'plastic_wine_cup', 'meshes', 'tsdf.stl') # stl | ply ycb_path = os.path.join(LIS_DIRECTORY, 'ycb', 'plastic_wine_cup', 'textured_meshes', 'optimized_tsdf_texture_mapped_mesh.obj') # ply print(point_path) print(mesh_path) print(ycb_path) #mesh = read_mesh_off(mesh_path, scale=0.001) #print(mesh) points = read_pcd_file(point_path) #print(points) #print(convex_hull(points)) mesh = mesh_from_points(points) #print(mesh) body = create_mesh(mesh, color=(1, 0, 0, 1)) #print(get_num_links(body)) #print(mesh_from_body(body)) #set_point(body, (1, 1, 1)) wait_if_gui()
def create_inverse_reachability2(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500): tool_link = get_gripper_link(robot, arm) problem = MockProblem(robot, fixed=[table], grasp_types=[grasp_type]) placement_gen_fn = get_stable_gen(problem) grasp_gen_fn = get_grasp_gen(problem, collisions=True) ik_ir_fn = get_ik_ir_gen(problem, max_attempts=max_attempts, learned=False, teleport=True) placement_gen = placement_gen_fn(body, table) grasps = list(grasp_gen_fn(body)) print('Grasps:', len(grasps)) # TODO: sample the torso height # TODO: consider IK with respect to the torso frame start_time = time.time() gripper_from_base_list = [] while len(gripper_from_base_list) < num_samples: [(p,)] = next(placement_gen) (g,) = random.choice(grasps) output = next(ik_ir_fn(arm, body, p, g), None) if output is None: print('Failed to find a solution after {} attempts'.format(max_attempts)) else: (_, ac) = output [at,] = ac.commands at.path[-1].assign() gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot)) gripper_from_base_list.append(gripper_from_base) print('{} / {} [{:.3f}]'.format( len(gripper_from_base_list), num_samples, elapsed_time(start_time))) wait_if_gui() return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
def test_drake_base_motion(pr2, base_start, base_goal, obstacles=[]): # TODO: combine this with test_arm_motion """ Drake's PR2 URDF has explicit base joints """ disabled_collisions = get_disabled_collisions(pr2) base_joints = [joint_from_name(pr2, name) for name in PR2_GROUPS['base']] set_joint_positions(pr2, base_joints, base_start) base_joints = base_joints[:2] base_goal = base_goal[:len(base_joints)] wait_if_gui('Plan Base?') with LockRenderer(lock=False): base_path = plan_joint_motion(pr2, base_joints, base_goal, obstacles=obstacles, disabled_collisions=disabled_collisions) if base_path is None: print('Unable to find a base path') return print(len(base_path)) for bq in base_path: set_joint_positions(pr2, base_joints, bq) if SLEEP is None: wait_if_gui('Continue?') else: wait_for_duration(SLEEP)
def main(): connect(use_gui=True) print(get_json_filenames()) #problem_filenames = sorted(os.listdir(openrave_directory)) #problem_filenames = ['{}.json'.format(name) for name in FFROB] #problem_filenames = ['sink_stove_4_30.json'] # 'dinner.json' | 'simple.json' problem_filenames = ['simple.json'] # 'dinner.json' | 'simple.json' # Mac width/height #width = 2560 #height = 1600 # #640, 480 screenshot = False for problem_filename in problem_filenames: load_json_problem(problem_filename) if screenshot: problem_name = problem_filename.split('.')[0] image_filename = "{}.png".format(problem_name) image_path = os.path.join(SCREENSHOT_DIR, image_filename) wait_for_interrupt(max_time=0.5) os.system("screencapture -R {},{},{},{} {}".format( 225, 200, 600, 500, image_path)) else: wait_if_gui() disconnect()
def test_arm_control(pr2, left_joints, arm_start): wait_if_gui('Control Arm?') real_time = False enable_gravity() p.setRealTimeSimulation(real_time) for _ in joint_controller(pr2, left_joints, arm_start): if not real_time: p.stepSimulation()
def main(): connect(use_gui=True) with HideOutput(): pr2 = load_model(DRAKE_PR2_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) assert head_conf is not None 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_if_gui() remove_body(detect_cone) remove_body(view_cone) disconnect()
def main(): connect(use_gui=True) with HideOutput(): pr2 = load_model("models/pr2_description/pr2.urdf") test_clone_robot(pr2) test_clone_arm(pr2) wait_if_gui('Finish?') disconnect()
def main(use_pr2_drake=True): connect(use_gui=True) add_data_path() plane = p.loadURDF("plane.urdf") table_path = "models/table_collision/table.urdf" table = load_pybullet(table_path, fixed_base=True) set_quat(table, quat_from_euler(Euler(yaw=PI / 2))) # table/table.urdf, table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf obstacles = [plane, table] pr2_urdf = DRAKE_PR2_URDF if use_pr2_drake else PR2_URDF with HideOutput(): pr2 = load_model(pr2_urdf, fixed_base=True) # TODO: suppress warnings? dump_body(pr2) z = base_aligned_z(pr2) print(z) #z = stable_z_on_aabb(pr2, AABB(np.zeros(3), np.zeros(3))) print(z) set_point(pr2, Point(z=z)) print(get_aabb(pr2)) wait_if_gui() base_start = (-2, -2, 0) base_goal = (2, 2, 0) arm_start = SIDE_HOLDING_LEFT_ARM #arm_start = TOP_HOLDING_LEFT_ARM #arm_start = REST_LEFT_ARM arm_goal = TOP_HOLDING_LEFT_ARM #arm_goal = SIDE_HOLDING_LEFT_ARM left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm']) right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm']) torso_joints = joints_from_names(pr2, PR2_GROUPS['torso']) set_joint_positions(pr2, left_joints, arm_start) set_joint_positions(pr2, right_joints, rightarm_from_leftarm(REST_LEFT_ARM)) set_joint_positions(pr2, torso_joints, [0.2]) open_arm(pr2, 'left') # test_ikfast(pr2) add_line(base_start, base_goal, color=RED) print(base_start, base_goal) if use_pr2_drake: test_drake_base_motion(pr2, base_start, base_goal, obstacles=obstacles) else: test_base_motion(pr2, base_start, base_goal, obstacles=obstacles) test_arm_motion(pr2, left_joints, arm_goal) # test_arm_control(pr2, left_joints, arm_start) wait_if_gui('Finish?') disconnect()
def simulate_curve(robot, joints, curve): #set_joint_positions(robot, joints, curve(random.uniform(curve.x[0], curve.x[-1]))) wait_if_gui(message='Begin?') #controller = follow_curve_old(robot, joints, curve) controller = follow_curve(robot, joints, curve) for _ in controller: step_simulation() #wait_if_gui() #wait_for_duration(duration=time_step) #time.sleep(time_step) wait_if_gui(message='Finish?')
def test_clone_arm(pr2): first_joint_name = PR2_GROUPS['left_arm'][0] first_joint = joint_from_name(pr2, first_joint_name) parent_joint = get_link_parent(pr2, first_joint) print(get_link_name(pr2, parent_joint), parent_joint, first_joint_name, first_joint) print(get_link_descendants(pr2, first_joint)) # TODO: least common ancestor links = [first_joint] + get_link_descendants(pr2, first_joint) new_arm = clone_body(pr2, links=links, collision=False) dump_world() set_base_values(pr2, (-2, 0, 0)) wait_if_gui()
def test_arm_motion(pr2, left_joints, arm_goal): disabled_collisions = get_disabled_collisions(pr2) wait_if_gui('Plan Arm?') with LockRenderer(lock=False): arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions) if arm_path is None: print('Unable to find an arm path') return print(len(arm_path)) for q in arm_path: set_joint_positions(pr2, left_joints, q) #wait_if_gui('Continue?') wait_for_duration(0.01)
def test_kinematic(robot, door, target_x): wait_if_gui('Begin?') robot_joints = get_movable_joints(robot)[:3] joint = robot_joints[0] start_x = get_joint_position(robot, joint) num_steps = int(math.ceil(abs(target_x - start_x) / 1e-2)) for x in interpolate(start_x, target_x, num_steps=num_steps): set_joint_position(robot, joint=joint, value=x) #with LockRenderer(): solve_collision_free(door, robot, draw=False) wait_for_duration(duration=1e-2) #wait_if_gui() wait_if_gui('Finish?')
def test_door(door): door_joints = get_movable_joints(door) sample_fn = get_sample_fn(door, door_joints) set_joint_positions(door, door_joints, sample_fn()) while True: positions = sample_fn() set_joint_positions(door, door_joints, positions) wait_if_gui() lower, upper = get_joint_intervals(door, door_joints) control_joints(door, door_joints, positions=lower) velocity_control_joints(door, door_joints, velocities=[PI / 4]) # Able to exceed limits
def test_arm_motion(pr2, left_joints, arm_goal): disabled_collisions = get_disabled_collisions(pr2) wait_if_gui('Plan Arm?') arm_path = plan_joint_motion(pr2, left_joints, arm_goal, disabled_collisions=disabled_collisions) if arm_path is None: print('Unable to find an arm path') return print(len(arm_path)) for q in arm_path: set_joint_positions(pr2, left_joints, q) #wait_if_gui('Continue?') time.sleep(0.01)
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 test_ycb(world): name = 'mustard_bottle' # potted_meat_can | cracker_box | sugar_box | mustard_bottle | bowl path_from_name = get_ycb_objects() print(path_from_name) ycb_directory = os.path.join(YCB_DIRECTORY, path_from_name[name], SENSOR) obj_path = os.path.join(ycb_directory, OBJ_PATH) image_path = os.path.join(ycb_directory, PNG_PATH) print(obj_path) #body = load_pybullet(obj_path) #, fixed_base=True) body = create_obj(obj_path, color=WHITE) set_texture(body, image_path) for link in get_all_links(body): set_color(body, link=link, color=WHITE) wait_if_gui()
def test_base_motion(pr2, base_start, base_goal, obstacles=[]): #disabled_collisions = get_disabled_collisions(pr2) set_base_values(pr2, base_start) wait_if_gui('Plan Base-pr2?') base_limits = ((-2.5, -2.5), (2.5, 2.5)) with LockRenderer(lock=False): base_path = plan_base_motion(pr2, base_goal, base_limits, obstacles=obstacles) if base_path is None: print('Unable to find a base path') return print(len(base_path)) for bq in base_path: set_base_values(pr2, bq) if SLEEP is None: wait_if_gui('Continue?') else: wait_for_duration(SLEEP)
def simulate_parallel(robots, plan, time_step=0.1, speed_up=10., record=None): # None | video.mp4 # TODO: ensure the step size is appropriate makespan = compute_duration(plan) print('\nMakespan: {:.3f}'.format(makespan)) if plan is None: return trajectories = [] for action in plan: command = action.args[-1] if (action.name == 'move') and (command.start_conf is action.args[-2].positions): command = command.reverse() command.retime(start_time=action.start) #print(action) #print(action.start, get_end(action), action.duration) #print(command.start_time, command.end_time, command.duration) #for traj in command.trajectories: # print(traj, traj.start_time, traj.end_time, traj.duration) trajectories.extend(command.trajectories) #print(sum(traj.duration for traj in trajectories)) num_motion = sum(action.name == 'move' for action in plan) wait_if_gui('Begin?') with VideoSaver(record): for t in inclusive_range(0, makespan, time_step): # if action.start <= t <= get_end(action): executing = Counter(traj.robot for traj in trajectories if traj.at(t) is not None) print('t={:.3f}/{:.3f} | executing={}'.format( t, makespan, executing)) for robot in robots: num = executing.get(robot, 0) if 2 <= num: raise RuntimeError( 'Robot {} simultaneously executing {} trajectories'. format(robot, num)) if (num_motion == 0) and (num == 0): set_configuration(robot, DUAL_CONF) #step_simulation() wait_for_duration(time_step / speed_up) wait_if_gui('Finish?')
def test_clone_robot(pr2): # TODO: j toggles frames, p prints timings, w is wire, a is boxes new_pr2 = clone_body(pr2, visual=True, collision=False) #new_pr2 = clone_body_editor(pr2, visual=True, collision=True) dump_world() #print(load_srdf_collisions()) #print(load_dae_collisions()) # TODO: some unimportant quats are off for both URDF and other # TODO: maybe all the frames are actually correct when I load things this way? # TODO: the visual geometries are off but not the collision frames? """ import numpy as np for link in get_links(pr2): if not get_visual_data(new_pr2, link): # pybullet.error: Error receiving visual shape info? continue #print(get_link_name(pr2, link)) data1 = VisualShapeData(*get_visual_data(pr2, link)[0]) data2 = VisualShapeData(*get_visual_data(new_pr2, link)[0]) pose1 = (data1.localVisualFrame_position, data1.localVisualFrame_orientation) pose2 = (data2.localVisualFrame_position, data2.localVisualFrame_orientation) #pose1 = get_link_pose(pr2, link) # Links are fine #pose2 = get_link_pose(new_pr2, link) #pose1 = get_joint_parent_frame(pr2, link) #pose2 = get_joint_parent_frame(new_pr2, link) #pose1 = get_joint_inertial_pose(pr2, link) # Inertia is fine #pose2 = get_joint_inertial_pose(new_pr2, link) if not np.allclose(pose1[0], pose2[0], rtol=0, atol=1e-3): print('Point', get_link_name(pr2, link), link, pose1[0], pose2[0]) #print(data1) #print(data2) #print(get_joint_parent_frame(pr2, link), get_joint_parent_frame(new_pr2, link)) print(get_joint_inertial_pose(pr2, link)) #, get_joint_inertial_pose(new_pr2, link)) print() if not np.allclose(euler_from_quat(pose1[1]), euler_from_quat(pose2[1]), rtol=0, atol=1e-3): print('Quat', get_link_name(pr2, link), link, euler_from_quat(pose1[1]), euler_from_quat(pose2[1])) joint_info1 = get_joint_info(pr2, link) joint_info2 = get_joint_info(new_pr2, link) # TODO: the axis is off for some of these if not np.allclose(joint_info1.jointAxis, joint_info2.jointAxis, rtol=0, atol=1e-3): print('Axis', get_link_name(pr2, link), link, joint_info1.jointAxis, joint_info2.jointAxis) """ set_base_values(new_pr2, (2, 0, 0)) wait_if_gui()
def main(): parser = argparse.ArgumentParser() parser.add_argument('-shape', default='box', choices=['box', 'sphere', 'cylinder', 'capsule']) 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) if video is not None: set_renderer(enable=False) draw_global_system() set_camera_pose(camera_point=Point(+1.5, -1.5, +1.5), target_point=Point(-1.5, +1.5, 0)) add_data_path() plane = load_pybullet('plane.urdf', fixed_base=True) #plane = load_model('plane.urdf') set_point(plane, Point(z=-1e-3)) ramp = create_ramp() dump_body(ramp) obj = create_object(args.shape) set_euler(obj, np.random.uniform(-np.math.radians(1), np.math.radians(1), 3)) set_point(obj, np.random.uniform(-1e-3, +1e-3, 3)) #set_velocity(obj, linear=Point(x=-1)) set_position(obj, z=2.) #set_position(obj, z=base_aligned_z(obj)) dump_body(obj) #add_pose_constraint(obj, pose=(target_point, target_quat), max_force=200) set_renderer(enable=True) if video is None: wait_if_gui('Begin?') simulate(controller=condition_controller(lambda step: (step != 0) and at_rest(obj))) if video is None: wait_if_gui('Finish?') disconnect()
def test_ikfast(pr2): from pybullet_tools.ikfast.pr2.ik import get_tool_pose, get_ik_generator left_joints = joints_from_names(pr2, PR2_GROUPS['left_arm']) #right_joints = joints_from_names(pr2, PR2_GROUPS['right_arm']) torso_joints = joints_from_names(pr2, PR2_GROUPS['torso']) torso_left = torso_joints + left_joints print(get_link_pose(pr2, link_from_name(pr2, 'l_gripper_tool_frame'))) # print(forward_kinematics('left', get_joint_positions(pr2, torso_left))) print(get_tool_pose(pr2, 'left')) arm = 'left' pose = get_tool_pose(pr2, arm) generator = get_ik_generator(pr2, arm, pose, torso_limits=False) for i in range(100): solutions = next(generator) print(i, len(solutions)) for q in solutions: set_joint_positions(pr2, torso_left, q) wait_if_gui()
def main(): benchmark = 'tmp-benchmark-data' problem = 'problem1' # Hanoi #problem = 'problem2' # Blocksworld #problem = 'problem3' # Clutter #problem = 'problem4' # Nonmono root_directory = os.path.dirname(os.path.abspath(__file__)) directory = os.path.join(root_directory, '..', 'problems', benchmark, problem) [mesh_directory] = list( filter(os.path.isdir, (os.path.join(directory, o) for o in os.listdir(directory) if o.endswith('meshes')))) [xml_path] = [ os.path.join(directory, o) for o in os.listdir(directory) if o.endswith('xml') ] if os.path.isdir(xml_path): xml_path = glob.glob(os.path.join(xml_path, '*.xml'))[0] print(mesh_directory) print(xml_path) xml_data = etree.parse(xml_path) connect(use_gui=True) #add_data_path() #load_pybullet("plane.urdf") draw_global_system() set_camera_pose(camera_point=[+1, -1, 1]) #root = xml_data.getroot() #print(root.items()) for obj in xml_data.findall('/objects/obj'): parse_object(obj, mesh_directory) for robot in xml_data.findall('/robots/robot'): parse_robot(robot) wait_if_gui() disconnect()
def simulate_printing(node_points, trajectories, time_step=0.1, speed_up=10.): # TODO: deprecate print_trajectories = [ traj for traj in trajectories if isinstance(traj, PrintTrajectory) ] handles = [] current_time = 0. current_traj = print_trajectories.pop(0) current_curve = current_traj.interpolate_tool(node_points, start_time=current_time) current_position = current_curve.y[0] while True: print('Time: {:.3f} | Remaining: {} | Segments: {}'.format( current_time, len(print_trajectories), len(handles))) end_time = current_curve.x[-1] if end_time < current_time + time_step: handles.append( add_line(current_position, current_curve.y[-1], color=RED)) if not print_trajectories: break current_traj = print_trajectories.pop(0) current_curve = current_traj.interpolate_tool(node_points, start_time=end_time) current_position = current_curve.y[0] print( 'New trajectory | Start time: {:.3f} | End time: {:.3f} | Duration: {:.3f}' .format(current_curve.x[0], current_curve.x[-1], current_curve.x[-1] - current_curve.x[0])) else: current_time += time_step new_position = current_curve(current_time) handles.append(add_line(current_position, new_position, color=RED)) current_position = new_position # TODO: longer wait for recording videos wait_for_duration(time_step / speed_up) # wait_if_gui() wait_if_gui() return handles
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 step_curve(robot, joints, path, step_size=None): wait_if_gui(message='Begin?') for i, conf in enumerate(path): set_joint_positions(robot, joints, conf) if step_size is None: wait_if_gui(message='{}/{} Continue?'.format(i, len(path))) else: wait_for_duration(duration=step_size) wait_if_gui(message='Finish?')
def save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list): # TODO: store value of torso and roll joint for the IK database. Sample the roll joint. # TODO: hash the pr2 urdf as well filename = IR_FILENAME.format(grasp_type, arm) path = get_database_file(filename) data = { 'filename': filename, 'robot': get_body_name(robot), 'grasp_type': grasp_type, 'arm': arm, 'torso': get_group_conf(robot, 'torso'), 'carry_conf': get_carry_conf(arm, grasp_type), 'tool_link': tool_link, 'ikfast': is_ik_compiled(), 'gripper_from_base': gripper_from_base_list, } write_pickle(path, data) if has_gui(): handles = [] for gripper_from_base in gripper_from_base_list: handles.extend(draw_point(point_from_pose(gripper_from_base), color=(1, 0, 0))) wait_if_gui() return path