def make_traj(warped_demo, inds=None, offset=0):
traj = {}
total_feas_inds = 0
all_feas = True
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if "%s_gripper_tool_frame"%lr in warped_demo:
pos = warped_demo["%s_gripper_tool_frame"%lr]["position"]
ori = warped_demo["%s_gripper_tool_frame"%lr]["orientation"]
if inds is not None:
pos, ori = pos[inds], ori[inds]
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
pos + offset,
ori,
pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True)
if len(feas_inds) == 0: assert False and 'failure'
traj["%s_arm"%lr] = arm_traj
traj["%s_arm_feas_inds"%lr] = feas_inds
total_feas_inds += len(feas_inds)
all_feas = all_feas and len(feas_inds) == len(arm_traj)
rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(arm_traj))
return traj, total_feas_inds, all_feas
traj, warped_demo = None, None
if args.traj_file:
with open(args.traj_file, 'r') as f:
traj = pickle.load(f)
print traj
else:
with open(args.warped_demo, 'r') as f:
warped_demo = pickle.load(f)
traj = {}
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
#if best_demo["arms_used"] in [lr, "b"]:
if True:
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
warped_demo["%s_gripper_tool_frame" % lr]["position"],
warped_demo["%s_gripper_tool_frame" % lr]["orientation"],
pr2.robot.GetManipulator("%sarm" % leftright),
"%s_gripper_tool_frame" % lr,
check_collisions=True)
if len(feas_inds) == 0: assert False and 'failure'
traj["%s_arm" % lr] = arm_traj
rospy.loginfo("%s arm: %i of %i points feasible", leftright,
len(feas_inds), len(arm_traj))
def show_ik_solns(part_name, manip, i, warped_demo):
frame = "%s_gripper_tool_frame" % part_name[0]
hmat = conv.trans_rot_to_hmat(warped_demo[frame]['position'][i],
warped_demo[frame]['orientation'][i])
solns = traj_ik_graph_search.ik_for_link(hmat,
manip,
frame,
if args.traj_file:
with open(args.traj_file, 'r') as f:
traj = pickle.load(f)
print traj
else:
with open(args.warped_demo, 'r') as f:
warped_demo = pickle.load(f)
traj = {}
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
#if best_demo["arms_used"] in [lr, "b"]:
if True:
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
warped_demo["%s_gripper_tool_frame"%lr]["position"],
warped_demo["%s_gripper_tool_frame"%lr]["orientation"],
pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True
)
if len(feas_inds) == 0: assert False and 'failure'
traj["%s_arm"%lr] = arm_traj
rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(arm_traj))
def show_ik_solns(part_name, manip, i, warped_demo):
frame = "%s_gripper_tool_frame"%part_name[0]
hmat = conv.trans_rot_to_hmat(warped_demo[frame]['position'][i], warped_demo[frame]['orientation'][i])
solns = traj_ik_graph_search.ik_for_link(hmat, manip, frame, return_all_solns=True)
# newrobots = []
print 'Displaying', len(solns), 'IK solutions for', part_name, 'at time', i
print solns
