def pick_problem(arm='left', grasp_type='side'):
other_arm = get_other_arm(arm)
initial_conf = get_carry_conf(arm, grasp_type)
pr2 = create_pr2()
set_base_values(pr2, (0, -1.2, np.pi / 2))
set_arm_conf(pr2, arm, initial_conf)
open_arm(pr2, arm)
set_arm_conf(pr2, other_arm, arm_conf(other_arm, REST_LEFT_ARM))
close_arm(pr2, other_arm)
plane = create_floor()
table = load_pybullet(TABLE_URDF)
# table = create_table(height=0.8)
# table = load_pybullet("table_square/table_square.urdf")
box = create_box(.07, .05, .25)
set_point(box, (-0.25, -0.3, TABLE_MAX_Z + .25 / 2))
# set_point(box, (0.2, -0.2, 0.8 + .25/2 + 0.1))
return Problem(robot=pr2,
movable=[box],
arms=[arm],
grasp_types=[grasp_type],
surfaces=[table],
goal_conf=get_pose(pr2),
goal_holding=[(arm, box)])
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_base_motion(pr2, base_start, base_goal):
#disabled_collisions = get_disabled_collisions(pr2)
set_base_values(pr2, base_start)
user_input('Plan Base?')
base_limits = ((-2.5, -2.5), (2.5, 2.5))
base_path = plan_base_motion(pr2, base_goal, base_limits)
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)
user_input('Continue?')
def test_base_motion(pr2, base_start, base_goal, obstacles=[]):
#disabled_collisions = get_disabled_collisions(pr2)
set_base_values(pr2, base_start)
wait_for_user('Plan Base?')
base_limits = ((-2.5, -2.5), (2.5, 2.5))
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_for_user('Continue?')
else:
time.sleep(SLEEP)
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 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 create_inverse_reachability(robot, body, table, arm, grasp_type, num_samples=500):
link = get_gripper_link(robot, arm)
movable_joints = get_movable_joints(robot)
default_conf = get_joint_positions(robot, movable_joints)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
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))
set_joint_positions(robot, movable_joints, default_conf)
base_conf = next(uniform_pose_generator(robot, gripper_pose))
set_base_values(robot, base_conf)
if pairwise_collision(robot, table):
continue
conf = inverse_kinematics(robot, link, gripper_pose)
if (conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(invert(get_link_pose(robot, link)), get_pose(robot))
gripper_from_base_list.append(gripper_from_base)
print('{} / {}'.format(len(gripper_from_base_list), num_samples))
filename = IR_FILENAME.format(grasp_type, arm)
path = get_database_file(filename)
data = {
'filename': filename,
'robot': get_body_name(robot),
'grasp_type': grasp_type,
'arg': arm,
'carry_conf': get_carry_conf(arm, grasp_type),
'gripper_link': link,
'gripper_from_base': gripper_from_base_list,
}
write_pickle(path, data)
return path