def main(display='control'): # control | execute | step
connect(use_gui=True)
disable_real_time()
# KUKA_IIWA_URDF | DRAKE_IIWA_URDF
robot = load_model(DRAKE_IIWA_URDF, fixed_base=True)
# floor = load_model('models/short_floor.urdf')
floor = p.loadURDF("plane.urdf")
block = load_model(
"models/drake/objects/block_for_pick_and_place_heavy.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(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 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 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 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(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()
