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 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_interrupt()
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_interrupt()
disconnect()
def step_trajectory(trajectory, attachments={}, time_step=np.inf):
for _ in trajectory.iterate():
for attachment in attachments.values():
attachment.assign()
if time_step == np.inf:
wait_for_interrupt(time_step)
else:
wait_for_duration(time_step)
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_interrupt()
remove_body(detect_cone)
remove_body(view_cone)
disconnect()
def main(execute='apply'):
#parser = argparse.ArgumentParser() # Automatically includes help
#parser.add_argument('-display', action='store_true', help='enable viewer.')
#args = parser.parse_args()
#display = args.display
display = True
#display = False
#filename = 'transporter.json' # simple | simple2 | cook | invert | kitchen | nonmonotonic_4_1
#problem_fn = lambda: load_json_problem(filename)
problem_fn = cooking_problem
# holding_problem | stacking_problem | cleaning_problem | cooking_problem
# cleaning_button_problem | cooking_button_problem
with HideOutput():
sim_world = connect(use_gui=False)
set_client(sim_world)
add_data_path()
problem = problem_fn()
print(problem)
#state_id = save_state()
if display:
with HideOutput():
real_world = connect(use_gui=True)
add_data_path()
with ClientSaver(real_world):
problem_fn() # TODO: way of doing this without reloading?
update_state()
wait_for_duration(0.1)
#wait_for_interrupt()
commands = plan_commands(problem)
if (commands is None) or not display:
with HideOutput():
disconnect()
return
time_step = 0.01
set_client(real_world)
if execute == 'control':
enable_gravity()
control_commands(commands)
elif execute == 'execute':
step_commands(commands, time_step=time_step)
elif execute == 'step':
step_commands(commands)
elif execute == 'apply':
state = State()
apply_commands(state, commands, time_step=time_step)
else:
raise ValueError(execute)
wait_for_interrupt()
with HideOutput():
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_interrupt()
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_for_interrupt()
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_for_interrupt()
def step_plan(plan, **kwargs):
wait_for_interrupt()
attachments = {}
for action, args in plan:
trajectory = args[-1]
if action == 'move':
step_trajectory(trajectory, attachments, **kwargs)
elif action == 'pick':
attachment = trajectory.attachments.pop()
step_trajectory(trajectory, attachments, **kwargs)
attachments[attachment.child] = attachment
step_trajectory(trajectory.reverse(), attachments, **kwargs)
elif action == 'place':
attachment = trajectory.attachments.pop()
step_trajectory(trajectory, attachments, **kwargs)
del attachments[attachment.child]
step_trajectory(trajectory.reverse(), attachments, **kwargs)
else:
raise NotImplementedError(action)
wait_for_interrupt()
def main(display='execute'): # control | execute | step
connect(use_gui=True)
disable_real_time()
with HideOutput():
root_directory = os.path.dirname(os.path.abspath(__file__))
robot = load_pybullet(os.path.join(root_directory, IRB6600_TRACK_URDF), fixed_base=True)
floor = load_model('models/short_floor.urdf')
block = load_model(BLOCK_URDF, fixed_base=False)
floor_x = 2
set_pose(floor, Pose(Point(x=floor_x, z=0.5)))
set_pose(block, Pose(Point(x=floor_x, y=0, z=stable_z(block, floor))))
# set_default_camera()
dump_world()
saved_world = WorldSaver()
with LockRenderer():
command = plan(robot, block, fixed=[floor], teleport=False)
if (command is None) or (display is None):
print('Unable to find a plan!')
print('Quit?')
wait_for_interrupt()
disconnect()
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.002)
elif display == 'step':
command.step()
else:
raise ValueError(display)
print('Quit?')
wait_for_interrupt()
disconnect()
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")
#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_for_interrupt()
disconnect()
def simulate_plan(plan, time_step=0.0, real_time=False): #time_step=np.inf
wait_for_interrupt()
enable_gravity()
if real_time:
enable_real_time()
for action, args in plan:
trajectory = args[-1]
if action == 'move':
simulate_trajectory(trajectory, time_step)
elif action == 'pick':
attachment = trajectory.attachments.pop()
simulate_trajectory(trajectory, time_step)
add_fixed_constraint(attachment.child, attachment.parent,
attachment.parent_link)
simulate_trajectory(trajectory.reverse(), time_step)
elif action == 'place':
ttachment = trajectory.attachments.pop()
simulate_trajectory(trajectory, time_step)
remove_fixed_constraint(attachment.child, attachment.parent,
attachment.parent_link)
simulate_trajectory(trajectory.reverse(), time_step)
else:
raise NotImplementedError(action)
wait_for_interrupt()
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(False):
robot = load_model(MOVO_URDF, fixed_base=True)
dump_body(robot)
print('Start?')
wait_for_interrupt()
#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_interrupt()
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 main():
connect(use_gui=True)
add_data_path()
set_camera(0, -30, 1)
plane = load_pybullet('plane.urdf', fixed_base=True)
#plane = load_model('plane.urdf')
cup = load_model('models/cup.urdf', fixed_base=True)
#set_point(cup, Point(z=stable_z(cup, plane)))
set_point(cup, Point(z=.2))
set_color(cup, (1, 0, 0, .4))
#wait_for_interrupt()
num_droplets = 100
#radius = 0.025
#radius = 0.005
radius = 0.0025
# TODO: more efficient ways to make all of these
droplets = [create_sphere(radius, mass=0.01)
for _ in range(num_droplets)] # kg
cup_thickness = 0.001
lower, upper = get_lower_upper(cup)
print(lower, upper)
buffer = cup_thickness + radius
lower = np.array(lower) + buffer * np.ones(len(lower))
upper = np.array(upper) - buffer * np.ones(len(upper))
limits = zip(lower, upper)
x_range, y_range = limits[:2]
z = upper[2] + 0.1
#x_range = [-1, 1]
#y_range = [-1, 1]
#z = 1
for droplet in droplets:
x = np.random.uniform(*x_range)
y = np.random.uniform(*y_range)
set_point(droplet, Point(x, y, z))
for i, droplet in enumerate(droplets):
x, y = np.random.normal(0, 1e-3, 2)
set_point(droplet, Point(x, y, z + i * (2 * radius + 1e-3)))
#dump_world()
wait_for_interrupt()
#input('Start?')
#dt = 1. / 240
dt = 0.01
#dt = 0
print('dt:', dt)
enable_gravity()
simulate_for_duration(5.0, dt=dt)
# enable_real_time()
# try:
# while True:
# enable_gravity() # enable_real_time requires a command
# #time.sleep(dt)
# except KeyboardInterrupt:
# pass
# print()
#time.sleep(1.0)
#wait_for_interrupt()
input('Finish?')
disconnect()
def main(execute='execute'):
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
#parser.add_argument('-display', action='store_true', help='enable viewer.')
args = parser.parse_args()
#display = args.display
display = True
connect(use_gui=args.viewer)
robot, block = load_world()
#robot2 = clone_body(robot)
#block2 = clone_body(block)
#dump_world()
saved_world = WorldSaver()
dump_world()
ss_problem = ss_from_problem(robot,
movable=[block],
teleport=False,
movable_collisions=True)
#ss_problem = ss_problem.debug_problem()
#print(ss_problem)
t0 = time.time()
plan, evaluations = dual_focused(ss_problem, verbose=True)
# plan, evaluations = incremental(ss_problem, verbose=True)
print_plan(plan, evaluations)
print(time.time() - t0)
if (not display) or (plan is None):
disconnect()
return
paths = []
for action, params in plan:
if action.name == 'place':
paths += params[-1].reverse().body_paths
elif action.name in ['move_free', 'move_holding', 'pick']:
paths += params[-1].body_paths
print(paths)
command = Command(paths)
if not args.viewer: # TODO: how to reenable the viewer
disconnect()
connect(use_gui=True)
load_world()
saved_world.restore()
user_input('Execute?')
if execute == 'control':
command.control()
elif execute == 'execute':
command.refine(num_steps=10).execute(time_step=0.001)
elif execute == 'step':
command.step()
else:
raise ValueError(execute)
#dt = 1. / 240 # Bullet default
#p.setTimeStep(dt)
wait_for_interrupt()
disconnect()