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_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():
connect(use_gui=True)
disable_real_time()
set_default_camera()
problem = pick_problem(arm='left', grasp_type='side')
grasp_gen = get_grasp_gen(problem, collisions=False)
ik_ir_fn = get_ik_ir_gen(problem, max_attempts=100, teleport=False)
pose = Pose(problem.movable[0], support=problem.surfaces[0])
base_conf = Conf(problem.robot, get_group_joints(problem.robot, 'base'))
ik_fn = get_ik_fn(problem)
found = False
saved_world = WorldSaver()
for grasp, in grasp_gen(problem.movable[0]):
print(grasp.value)
# confs_cmds = ik_ir_fn(problem.arms[0], problem.movable[0], pose, grasp)
# for conf, cmds in confs_cmds:
# found = True
cmds = ik_fn(problem.arms[0], problem.movable[0], pose, grasp,
base_conf)
if cmds is not None:
cmds = cmds[0]
found = True
if found:
break
if not found:
raise Exception('No grasp found')
saved_world.restore()
for cmd in cmds.commands:
print(cmd)
control_commands(cmds.commands)
print('Quit?')
wait_for_user()
disconnect()
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 HideOutput():
with LockRenderer():
robot = load_model(MOVO_URDF, fixed_base=True)
dump_body(robot)
print('Start?')
wait_for_user()
#joint_names = HEAD_JOINTS
#joints = joints_from_names(robot, joint_names)
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
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()
disconnect()
def debug_elements(robot, node_points, node_order, elements):
#test_grasps(robot, node_points, elements)
#test_print(robot, node_points, elements)
#return
for element in elements:
color = (0, 0, 1) if doubly_printable(element, node_points) else (1, 0,
0)
draw_element(node_points, element, color=color)
wait_for_user('Continue?')
# TODO: topological sort
node = node_order[40]
node_neighbors = get_node_neighbors(elements)
for element in node_neighbors[node]:
color = (0, 1,
0) if element_supports(element, node, node_points) else (1, 0,
0)
draw_element(node_points, element, color)
element = elements[-1]
draw_element(node_points, element, (0, 1, 0))
incoming_edges, _ = neighbors_from_orders(
get_supported_orders(elements, node_points))
supporters = []
retrace_supporters(element, incoming_edges, supporters)
for e in supporters:
draw_element(node_points, e, (1, 0, 0))
wait_for_user('Continue?')
def test_print(robot, node_points, elements):
#elements = [elements[0]]
elements = [elements[-1]]
[element_body] = create_elements(node_points, elements)
wait_for_user()
phi = 0
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=phi, yaw=theta))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
#grasp_rotations = [Pose(euler=Euler(roll=np.pi/2, pitch=theta, yaw=phi))
# for theta in np.linspace(0, 2*np.pi, 10, endpoint=False)]
grasp_rotations = [sample_direction() for _ in range(25)]
link = link_from_name(robot, TOOL_NAME)
movable_joints = get_movable_joints(robot)
sample_fn = get_sample_fn(robot, movable_joints)
for grasp_rotation in grasp_rotations:
n1, n2 = elements[0]
length = np.linalg.norm(node_points[n2] - node_points[n1])
set_joint_positions(robot, movable_joints, sample_fn())
for t in np.linspace(-length / 2, length / 2, 10):
#element_translation = Pose(point=Point(z=-t))
#grasp_pose = multiply(grasp_rotation, element_translation)
#reverse = Pose(euler=Euler())
reverse = Pose(euler=Euler(roll=np.pi))
grasp_pose = get_grasp_pose(t, grasp_rotation, 0)
grasp_pose = multiply(grasp_pose, reverse)
element_pose = get_pose(element_body)
link_pose = multiply(element_pose, invert(grasp_pose))
conf = inverse_kinematics(robot, link, link_pose)
wait_for_user()
def 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_for_user()
disconnect()
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_for_user()
return path
def main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
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()
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()
user_input('{}?'.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_for_user()
disconnect()
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 main():
world = connect(use_gui=True)
#model = 'oil'
model = 'soup'
point_path = os.path.join(DATA_DIRECTORY, 'clouds', CLOUDS[model])
mesh_path = os.path.join(DATA_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(DATA_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_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 create_meshes(ty, draw=False, visualize=False):
assert not (visualize and draw) # Incompatible?
suffix = SUFFIX_TEMPLATE.format(ty)
models_path = os.path.join(get_models_path(), MODELS_TEMPLATE.format(ty))
ensure_dir(models_path)
for prefix, properties in OBJECT_PROPERTIES[ty].items():
color = normalize_rgb(properties.color)
side = approximate_bowl(properties, d=2) # 1 doesn't seem to really work
name = prefix + suffix
print(name, color)
print(side)
if draw:
draw_curvature(side, name=name)
chunks = make_revolute_chunks(side, n_theta=60, n_chunks=10,
in_off=properties.thickness/4.,
scale=SCALE)
obj_path = os.path.join(models_path, OBJ_TEMPLATE.format(name))
write_obj(chunks, obj_path)
if visualize:
body = create_obj(obj_path, color=color)
_, dims = approximate_as_cylinder(body)
print(dims)
wait_for_user()
remove_body(body)
pcd_path = os.path.join(models_path, PCD_TEMPLATE.format(name))
pcd_from_mesh(obj_path, pcd_path)
def main():
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
with HideOutput():
with LockRenderer():
robot = load_model(FRANKA_URDF, fixed_base=True)
dump_body(robot)
print('Start?')
wait_for_user()
tool_link = link_from_name(robot, 'panda_hand')
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
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_retraction(robot,
PANDA_INFO,
tool_link,
max_distance=0.01,
max_time=0.05)
disconnect()
def iterate_commands(state,
commands,
time_step=DEFAULT_TIME_STEP,
pause=False):
if commands is None:
return False
for i, command in enumerate(commands):
print('\nCommand {:2}/{:2}: {}'.format(i + 1, len(commands), command))
# TODO: skip to end
# TODO: downsample
for j, _ in enumerate(command.iterate(state)):
state.derive()
if j == 0:
continue
if time_step is None:
wait_for_duration(1e-2)
wait_for_user('Command {:2}/{:2} | step {:2} | Next?'.format(
i + 1, len(commands), j))
elif time_step == 0:
pass
else:
wait_for_duration(time_step)
if pause:
wait_for_user('Continue?')
return True
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_for_user('Plan Base?')
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_for_user('Continue?')
else:
time.sleep(SLEEP)
def test_arm_control(pr2, left_joints, arm_start):
wait_for_user('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 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_for_user('Continue?')
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():
# 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 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 test_confs(robot, num_samples=10):
joints = get_movable_joints(robot)
print('Joints', [get_joint_name(robot, joint) for joint in joints])
sample_fn = get_sample_fn(robot, joints)
for i in range(num_samples):
print('Iteration:', i)
conf = sample_fn()
set_joint_positions(robot, joints, conf)
wait_for_user()
def main():
connect(use_gui=True)
with HideOutput():
pr2 = load_model(
"models/drake/pr2_description/urdf/pr2_simplified.urdf")
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['left_arm']),
REST_LEFT_ARM)
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['right_arm']),
rightarm_from_leftarm(REST_LEFT_ARM))
set_joint_positions(pr2, joints_from_names(pr2, PR2_GROUPS['torso']),
[0.2])
dump_body(pr2)
block = load_model(BLOCK_URDF, fixed_base=False)
set_point(block, [2, 0.5, 1])
target_point = point_from_pose(get_pose(block))
# head_link = link_from_name(pr2, HEAD_LINK)
head_joints = joints_from_names(pr2, PR2_GROUPS['head'])
head_link = head_joints[-1]
#max_detect_distance = 2.5
max_register_distance = 1.0
distance_range = (max_register_distance / 2, max_register_distance)
base_generator = visible_base_generator(pr2, target_point, distance_range)
base_joints = joints_from_names(pr2, PR2_GROUPS['base'])
for i in range(5):
base_conf = next(base_generator)
set_joint_positions(pr2, base_joints, base_conf)
p.addUserDebugLine(point_from_pose(get_link_pose(pr2, head_link)),
target_point,
lineColorRGB=(1, 0, 0)) # addUserDebugText
p.addUserDebugLine(point_from_pose(
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME))),
target_point,
lineColorRGB=(0, 0, 1)) # addUserDebugText
# head_conf = sub_inverse_kinematics(pr2, head_joints[0], HEAD_LINK, )
head_conf = inverse_visibility(pr2, target_point)
set_joint_positions(pr2, head_joints, head_conf)
print(get_detections(pr2))
# TODO: does this detect the robot sometimes?
detect_mesh, z = get_detection_cone(pr2, block)
detect_cone = create_mesh(detect_mesh, color=(0, 1, 0, 0.5))
set_pose(detect_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
view_cone = get_viewcone(depth=2.5, color=(1, 0, 0, 0.25))
set_pose(view_cone,
get_link_pose(pr2, link_from_name(pr2, HEAD_LINK_NAME)))
wait_for_user()
remove_body(detect_cone)
remove_body(view_cone)
disconnect()
def main():
connect(use_gui=True)
with HideOutput():
pr2 = load_model("models/pr2_description/pr2.urdf")
test_clone_robot(pr2)
test_clone_arm(pr2)
wait_for_user('Finish?')
disconnect()
def draw_action(node_points, printed, element):
if not has_gui():
return []
with LockRenderer():
remove_all_debug()
handles = [draw_element(node_points, element, color=(1, 0, 0))]
handles.extend(
draw_element(node_points, e, color=(0, 1, 0)) for e in printed)
wait_for_user()
return handles
def main():
parser = argparse.ArgumentParser()
#parser.add_argument('-attempts', default=100, type=int,
# help='The number of attempts')
parser.add_argument('-cfree', action='store_true',
help='When enabled, disables collision checking (for debugging).')
#parser.add_argument('-grasp_type', default=GRASP_TYPES[0],
# help='Specifies the type of grasp.')
#parser.add_argument('-problem', default='test_block',
# help='The name of the problem to solve.')
parser.add_argument('-max_time', default=10*60, type=float,
help='The maximum runtime')
parser.add_argument('-num_samples', default=1000, type=int,
help='The number of samples')
parser.add_argument('-robot', default=FRANKA_CARTER, choices=[FRANKA_CARTER, EVE],
help='The robot to use.')
parser.add_argument('-seed', default=None,
help='The random seed to use.')
parser.add_argument('-teleport', action='store_true',
help='Uses unit costs')
parser.add_argument('-visualize', action='store_true',
help='When enabled, visualizes planning rather than the world (for debugging).')
args = parser.parse_args()
world = World(use_gui=args.visualize, robot_name=args.robot)
#dump_body(world.robot)
for joint in world.kitchen_joints:
world.open_door(joint) # open_door | close_door
world.open_gripper()
# TODO: sample from set of objects?
object_name = '{}_{}_block{}'.format(BLOCK_SIZES[-1], BLOCK_COLORS[0], 0)
world.add_body(object_name)
# TODO: could constrain Eve to be within a torso cone
grasp_colors = {
TOP_GRASP: RED,
SIDE_GRASP: BLUE,
}
#combinations = list(product(OPEN_SURFACES, GRASP_TYPES)) \
# + [(surface_name, TOP_GRASP) for surface_name in DRAWERS] \
# + [(surface_name, SIDE_GRASP) for surface_name in CABINETS] # ENV_SURFACES
combinations = []
for surface_name in ZED_LEFT_SURFACES:
if surface_name in (OPEN_SURFACES + DRAWERS):
combinations.append((surface_name, TOP_GRASP))
if surface_name in (OPEN_SURFACES + CABINETS):
combinations.append((surface_name, SIDE_GRASP))
# TODO: parallelize
print('Combinations:', combinations)
wait_for_user('Start?')
for surface_name, grasp_type in combinations:
#draw_picks(world, object_name, surface_name, grasp_type, color=grasp_colors[grasp_type])
collect_place(world, object_name, surface_name, grasp_type, args)
world.destroy()
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 display_failure(node_points, extruded_elements, element):
client = connect(use_gui=True)
with ClientSaver(client):
obstacles, robot = load_world()
handles = []
for e in extruded_elements:
handles.append(draw_element(node_points, e, color=(0, 1, 0)))
handles.append(draw_element(node_points, element, color=(1, 0, 0)))
print('Failure!')
wait_for_user()
reset_simulation()
disconnect()
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_for_user()
def test_arm_motion(pr2, left_joints, arm_goal):
disabled_collisions = get_disabled_collisions(pr2)
wait_for_user('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)
# raw_input('Continue?')
time.sleep(0.01)
