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 add_scales(task, ros_world):
scale_stackings = {}
holding = {grasp.obj_name for grasp in task.init_holding.values()}
with ClientSaver(ros_world.client):
perception = ros_world.perception
items = sorted(set(perception.get_items()) - holding,
key=lambda n: get_point(ros_world.get_body(n))[1],
reverse=False) # Right to left
for i, item in enumerate(items):
if not POUR and (get_type(item) != SCOOP_CUP):
continue
item_body = ros_world.get_body(item)
scale = create_name(SCALE_TYPE, i + 1)
with HideOutput():
scale_body = load_pybullet(get_body_urdf(get_type(scale)),
fixed_base=True)
ros_world.perception.sim_bodies[scale] = scale_body
ros_world.perception.sim_items[scale] = None
item_z = stable_z(item_body, scale_body)
scale_pose_item = Pose(
point=Point(z=-item_z)) # TODO: relies on origin in base
set_pose(scale_body, multiply(get_pose(item_body),
scale_pose_item))
roll, pitch, _ = get_euler(scale_body)
set_euler(scale_body, [roll, pitch, 0])
scale_stackings[scale] = item
#wait_for_user()
return scale_stackings
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 create_world(items=[], **kwargs):
state = {name: unit_pose() for name in items}
with HideOutput():
world = ROSWorld(sim_only=True, state=state, **kwargs) # state=[]
table_body = add_table(world)
create_floor()
return world, table_body
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 __init__(self, task, use_robot=True, visualize=False, **kwargs):
self.task = task
self.real_world = None
self.visualize = visualize
self.client_saver = None
self.client = connect(use_gui=visualize, **kwargs)
print('Planner connected to client {}.'.format(self.client))
self.robot = None
with ClientSaver(self.client):
with HideOutput():
if use_robot:
self.robot = load_pybullet(os.path.join(
get_models_path(), PR2_URDF),
fixed_base=True)
#dump_body(self.robot)
#compute_joint_weights(self.robot)
self.world_name = 'world'
self.world_pose = Pose(unit_pose())
self.bodies = {}
self.fixed = []
self.surfaces = []
self.items = []
#self.holding_liquid = []
self.initial_config = None
self.initial_poses = {}
self.body_mapping = {}
def main():
parser = argparse.ArgumentParser() # Automatically includes help
parser.add_argument('-arm', required=True)
parser.add_argument('-grasp', required=True)
parser.add_argument('-viewer', action='store_true', help='enable viewer.')
args = parser.parse_args()
arm = args.arm
other_arm = get_other_arm(arm)
grasp_type = args.grasp
connect(use_gui=args.viewer)
add_data_path()
with HideOutput():
robot = load_pybullet(DRAKE_PR2_URDF)
set_group_conf(robot, 'torso', [0.2])
set_arm_conf(robot, arm, get_carry_conf(arm, grasp_type))
set_arm_conf(robot, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
#plane = p.loadURDF("plane.urdf")
#table = p.loadURDF("table/table.urdf", 0, 0, 0, 0, 0, 0.707107, 0.707107)
table = create_table()
box = create_box(.07, .07, .14)
#create_inverse_reachability(robot, box, table, arm=arm, grasp_type=grasp_type)
create_inverse_reachability2(robot,
box,
table,
arm=arm,
grasp_type=grasp_type)
disconnect()
def parse_robot(robot_json):
pose = parse_pose(robot_json)
if robot_json['name'] == 'pr2':
with HideOutput(True):
robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True)
set_group_conf(robot_id, 'base', base_values_from_pose(pose))
else:
# TODO: set the z?
#set_pose(robot_id, pose)
raise NotImplementedError(robot_json['name'])
for joint, values in robot_json['conf'].items():
[value] = values
if has_joint(robot_id, joint):
set_joint_position(robot_id, joint_from_name(robot_id, joint),
value)
else:
print('Robot {} lacks joint {}'.format(robot_json['name'], joint))
if robot_json['name'] == 'pr2':
set_group_conf(robot_id, 'torso', [0.2])
set_group_conf(robot_id, 'left_arm', REST_LEFT_ARM)
set_group_conf(robot_id, 'right_arm',
rightarm_from_leftarm(REST_LEFT_ARM))
return robot_id
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():
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 load_robot():
root_directory = os.path.dirname(os.path.abspath(__file__))
kuka_path = os.path.join(root_directory, KUKA_DIR, KUKA_PATH)
with HideOutput():
robot = load_pybullet(kuka_path, fixed_base=True)
#print([get_max_velocity(robot, joint) for joint in get_movable_joints(robot)])
set_static(robot)
set_configuration(robot, INITIAL_CONF)
return robot
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)
user_input('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 _create_robot(self):
with ClientSaver(self.client):
with HideOutput():
pr2_path = os.path.join(get_models_path(), PR2_URDF)
self.pr2 = load_pybullet(pr2_path, fixed_base=True)
# Base link is the origin
base_pose = get_link_pose(self.robot,
link_from_name(self.robot, BASE_FRAME))
set_pose(self.robot, invert(base_pose))
return self.pr2
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 load_body(self, name, pose=None, fixed_base=False):
assert name not in self.sim_bodies
ty = get_type(name)
with ClientSaver(self.client):
with HideOutput():
body = load_cup_bowl(ty)
if body is None:
body = load_pybullet(get_body_urdf(name),
fixed_base=fixed_base)
if pose is not None:
set_pose(body, pose)
self.sim_bodies[name] = body
return body
def main(use_pr2_drake=False):
connect(use_gui=True)
add_data_path()
plane = p.loadURDF("plane.urdf")
#table_path = "table/table.urdf"
# table_path = "models/table_collision/table.urdf"
# table = p.loadURDF(table_path, 0, 0, 0, 0, 0, 0.707107, 0.707107)
# table_square/table_square.urdf, cube.urdf, block.urdf, door.urdf
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)
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)
p.addUserDebugLine(base_start, base_goal,
lineColorRGB=(1, 0, 0)) # addUserDebugText
print(base_start, base_goal)
if use_pr2_drake:
test_drake_base_motion(pr2, base_start, base_goal)
else:
test_base_motion(pr2, base_start, base_goal)
test_arm_motion(pr2, left_joints, arm_goal)
# test_arm_control(pr2, left_joints, arm_start)
user_input('Finish?')
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 load_world(use_floor=True):
obstacles = []
#side, height = 10, 0.01
robot = load_robot()
with HideOutput():
lower, _ = get_aabb(robot)
if use_floor:
#floor = load_model('models/short_floor.urdf', fixed_base=True)
#add_data_path()
#floor = load_pybullet('plane.urdf', fixed_base=True)
#set_color(floor, TAN)
#floor = create_box(w=side, l=side, h=height, color=apply_alpha(GROUND_COLOR))
floor = create_plane(color=apply_alpha(GROUND_COLOR))
obstacles.append(floor)
#set_point(floor, Point(z=lower[2]))
set_point(floor, Point(x=1.2, z=0.023 - 0.025)) # -0.02
else:
floor = None # TODO: make this an empty list of obstacles
#set_all_static()
return obstacles, robot
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 __init__(self,
sim_only=False,
visualize=False,
use_robot=True,
**kwargs):
self.simulation = sim_only
if not self.simulation:
import rospy
rospy.init_node(self.__class__.__name__, anonymous=False)
self.client_saver = None
with HideOutput():
self.client = connect(
visualize
) # TODO: causes assert (len(visual_data) == 1) error?
#self.client = p.connect(p.GUI if visualize else p.DIRECT)
p.setPhysicsEngineParameter(enableFileCaching=0,
physicsClientId=self.client)
p.setRealTimeSimulation(0, physicsClientId=self.client)
p.configureDebugVisualizer(p.COV_ENABLE_GUI,
False,
physicsClientId=self.client)
p.configureDebugVisualizer(p.COV_ENABLE_SHADOWS,
True,
physicsClientId=self.client)
self.pr2 = None
if use_robot:
self._create_robot()
self.initial_beads = {}
if sim_only:
self.perception = PBPerception(self, **kwargs)
self.controller = PBController(self)
self.alternate_controller = None
else:
from perception_tools.retired.ros_perception import ROSPerception
from control_tools.retired.ros_controller import ROSController
self.perception = ROSPerception(self)
self.controller = ROSController(self)
self.alternate_controller = PBController(self)
self.sync_controllers()
def create_stiffness_checker(extrusion_path, verbose=False):
if not USE_CONMECH:
return None
from pyconmech import StiffnessChecker
# TODO: the stiffness checker likely has a memory leak
# https://github.com/yijiangh/conmech/blob/master/src/bindings/pyconmech/pyconmech.cpp
if not os.path.exists(extrusion_path):
raise FileNotFoundError(extrusion_path)
with HideOutput():
#checker = StiffnessChecker(json_file_path=extrusion_path, verbose=verbose)
checker = StiffnessChecker.from_json(json_file_path=extrusion_path,
verbose=verbose)
#checker.set_output_json(True)
#checker.set_output_json_path(file_path=os.getcwd(), file_name="stiffness-results.json")
checker.set_self_weight_load(True)
#checker.set_nodal_displacement_tol(transl_tol=0.005, rot_tol=10 * np.pi / 180)
#checker.set_nodal_displacement_tol(transl_tol=0.003, rot_tol=5 * np.pi / 180)
# checker.set_nodal_displacement_tol(transl_tol=1e-3, rot_tol=3 * (np.pi / 360))
checker.set_nodal_displacement_tol(trans_tol=TRANS_TOL, rot_tol=ROT_TOL)
#checker.set_loads(point_loads=None, include_self_weight=False, uniform_distributed_load={})
return checker
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 main():
# https://github.com/ros-teleop/teleop_twist_keyboard
# http://openrave.org/docs/latest_stable/_modules/openravepy/misc/#SetViewerUserThread
connect(use_gui=True)
add_data_path()
load_pybullet("plane.urdf")
#load_pybullet("models/table_collision/table.urdf")
with HideOutput():
pr2 = load_model(DRAKE_PR2_URDF, fixed_base=True)
enable_gravity()
enable_real_time(
) # TODO: won't work as well on OS X due to simulation thread
#run_simulate(pr2)
run_thread(pr2)
# TODO: keep working on this
#userthread = threading.Thread(target=run_thread, args=[pr2])
#userthread.start()
#userthread.join()
disconnect()
def parse_robot(robot):
name = robot.find('name').text
urdf = robot.find('urdf').text
fixed_base = not parse_boolean(robot.find('movebase'))
print(name, urdf, fixed_base)
pose = parse_pose(robot.find('basepose'))
torso = parse_array(robot.find('torso'))
left_arm = parse_array(robot.find('left_arm'))
right_arm = parse_array(robot.find('right_arm'))
assert (name == 'pr2')
with HideOutput():
robot_id = load_model(DRAKE_PR2_URDF, fixed_base=True)
set_group_conf(robot_id, 'base', base_values_from_pose(pose))
set_group_conf(robot_id, 'torso', torso)
set_group_conf(robot_id, 'left_arm', left_arm)
set_group_conf(robot_id, 'right_arm', right_arm)
#set_point(robot_id, Point(z=point_from_pose(pose)[2]))
# TODO: base pose isn't right
# print(robot.tag)
# print(robot.attrib)
# print(list(robot.iter('basepose')))
return robot_id
def create_table_bodies(world, item_ranges, randomize=True):
perception = world.perception
with HideOutput():
add_data_path()
floor_body = load_pybullet("plane.urdf")
set_pose(floor_body, get_pose(world.robot))
add_table(world)
for name, limits in sorted(item_ranges.items()):
perception.sim_items[name] = None
if randomize:
body = randomize_body(name,
width_range=limits.width_range,
height_range=limits.height_range,
mass_range=limits.mass_range)
else:
body = load_body(name)
perception.sim_bodies[name] = body
# perception.add_item(name, unit_pose())
x, y, yaw = np.random.uniform(*limits.pose2d_range)
surface_body = perception.get_body(limits.surface)
z = stable_z(body, surface_body) + Z_EPSILON
pose = Pose((x, y, z), Euler(yaw=yaw))
perception.set_pose(name, *pose)
def sample_task(skill, **kwargs):
# TODO: extract and just control robot gripper
with HideOutput():
sim_world = ROSWorld(sim_only=True, visualize=kwargs['visualize'])
# Only used by pb_controller, for ros_controller it's assumed always True
sim_world.controller.execute_motor_control = True
collector = SKILL_COLLECTORS[skill]
while True: # TODO: timeout if too many failures in a row
sim_world.controller.set_gravity()
task, feature, evaluation_fn = collector.collect_fn(
sim_world, **kwargs['collect_kwargs'])
# evaluation_fn is the score function for the skill
if task is not None:
task.skill = skill
feature['simulation'] = True
break
sim_world.reset(keep_robot=True)
saver = WorldSaver()
if kwargs['visualize'] and kwargs['visualize'] != 'No_name':
draw_names(sim_world)
draw_forward_reachability(sim_world, task.arms)
return sim_world, collector, task, feature, evaluation_fn, saver
def _load_bodies(self, real_world):
with ClientSaver(self.client):
assert not self.bodies
#for real_body in self.bodies.values():
# remove_body(real_body)
self.bodies = {}
self.initial_poses = {}
with HideOutput():
for name, real_body in real_world.perception.sim_bodies.items(
):
if name in real_world.perception.get_surfaces():
self.add_body(name, fixed=True)
elif name in real_world.perception.get_items():
with real_world:
model_info = get_model_info(real_body)
# self.bodies[item] = clone_body(real_world.perception.get_body(item), client=self.client)
if 'wall' in name:
with real_world:
self.bodies[name] = clone_body(
real_body, client=self.client)
elif 'cylinder' in name:
self.bodies[name] = create_cylinder(
*model_info.path)
elif 'box' in name:
self.bodies[name] = create_box(*model_info.path)
elif model_info is None:
self.add_body(name, fixed=False)
else:
self.bodies[name] = load_model_info(model_info)
else:
raise ValueError(name)
# TODO: the floor might not be added which causes the indices to misalign
self.body_mapping[self.bodies[name]] = real_body
if name not in self.get_held():
self.initial_poses[name] = Pose(
real_world.perception.get_pose(name))
self.set_initial()