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 create_inverse_reachability(robot, body, table, arm, grasp_type, max_attempts=500, num_samples=500):
tool_link = get_gripper_link(robot, arm)
robot_saver = BodySaver(robot)
gripper_from_base_list = []
grasps = GET_GRASPS[grasp_type](body)
start_time = time.time()
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))
for attempt in range(max_attempts):
robot_saver.restore()
base_conf = next(uniform_pose_generator(robot, gripper_pose)) #, reachable_range=(0., 1.)))
#set_base_values(robot, base_conf)
set_group_conf(robot, 'base', base_conf)
if pairwise_collision(robot, table):
continue
grasp_conf = pr2_inverse_kinematics(robot, arm, gripper_pose) #, nearby_conf=USE_CURRENT)
#conf = inverse_kinematics(robot, link, gripper_pose)
if (grasp_conf is None) or pairwise_collision(robot, table):
continue
gripper_from_base = multiply(invert(get_link_pose(robot, tool_link)), get_base_pose(robot))
#wait_if_gui()
gripper_from_base_list.append(gripper_from_base)
print('{} / {} | {} attempts | [{:.3f}]'.format(
len(gripper_from_base_list), num_samples, attempt, elapsed_time(start_time)))
wait_if_gui()
break
else:
print('Failed to find a kinematic solution after {} attempts'.format(max_attempts))
return save_inverse_reachability(robot, arm, grasp_type, tool_link, gripper_from_base_list)
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 update_world(world, target_body, camera_point=VIEWER_POINT):
pr2 = world.perception.pr2
with ClientSaver(world.perception.client):
open_arm(pr2, LEFT_ARM)
open_arm(pr2, RIGHT_ARM)
set_group_conf(pr2, 'torso', [TORSO_POSITION])
set_group_conf(pr2, arm_from_arm('left'), WIDE_LEFT_ARM)
set_group_conf(pr2, arm_from_arm('right'), rightarm_from_leftarm(WIDE_LEFT_ARM))
target_point = get_point(target_body)
head_conf = inverse_visibility(pr2, target_point, head_name=CAMERA_OPTICAL_FRAME) # Must come after torso
set_group_conf(pr2, 'head', head_conf)
set_camera_pose(camera_point, target_point=target_point)
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
