def simulate_demo_traj(sim_env, new_xyz, seg_info, full_trajs, ignore_infeasibility=True, animate=False, interactive=False):
sim_util.reset_arms_to_side(sim_env)
old_xyz = np.squeeze(seg_info["cloud_xyz"])
old_xyz = clouds.downsample(old_xyz, DS_SIZE)
new_xyz = clouds.downsample(new_xyz, DS_SIZE)
handles = []
if animate:
handles.append(sim_env.env.plot3(old_xyz,5, (1,0,0)))
handles.append(sim_env.env.plot3(new_xyz,5, (0,0,1)))
miniseg_starts, miniseg_ends, _ = sim_util.split_trajectory_by_gripper(seg_info, thresh = GRIPPER_ANGLE_THRESHOLD)
success = True
feasible = True
misgrasp = False
print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends
for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
if i_miniseg >= len(full_trajs): break
full_traj = full_trajs[i_miniseg]
for lr in 'lr':
gripper_open = sim_util.binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start], GRIPPER_ANGLE_THRESHOLD)
prev_gripper_open = sim_util.binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start-1], GRIPPER_ANGLE_THRESHOLD) if i_start != 0 else False
if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open, prev_gripper_open):
redprint("Grab %s failed" % lr)
misgrasp = True
success = False
if not success: break
if len(full_traj[0]) > 0:
if not eval_util.traj_is_safe(sim_env, full_traj, COLLISION_DIST_THRESHOLD):
redprint("Trajectory not feasible")
feasible = False
if feasible or ignore_infeasibility:
success &= sim_util.sim_full_traj_maybesim(sim_env, full_traj, animate=animate, interactive=interactive)
else:
success = False
if not success: break
sim_env.sim.settle(animate=animate)
sim_env.sim.release_rope('l')
sim_env.sim.release_rope('r')
sim_util.reset_arms_to_side(sim_env)
if animate:
sim_env.viewer.Step()
return success, feasible, misgrasp, full_trajs
def simulate_demo_traj(sim_env,
new_xyz,
seg_info,
full_trajs,
ignore_infeasibility=True,
animate=False,
interactive=False):
sim_util.reset_arms_to_side(sim_env)
old_xyz = np.squeeze(seg_info["cloud_xyz"])
old_xyz = clouds.downsample(old_xyz, DS_SIZE)
new_xyz = clouds.downsample(new_xyz, DS_SIZE)
handles = []
if animate:
handles.append(sim_env.env.plot3(old_xyz, 5, (1, 0, 0)))
handles.append(sim_env.env.plot3(new_xyz, 5, (0, 0, 1)))
miniseg_starts, miniseg_ends, _ = sim_util.split_trajectory_by_gripper(
seg_info, thresh=GRIPPER_ANGLE_THRESHOLD)
success = True
feasible = True
misgrasp = False
print colorize.colorize("mini segments:",
"red"), miniseg_starts, miniseg_ends
for (i_miniseg, (i_start,
i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
if i_miniseg >= len(full_trajs): break
full_traj = full_trajs[i_miniseg]
for lr in 'lr':
gripper_open = sim_util.binarize_gripper(
seg_info["%s_gripper_joint" % lr][i_start],
GRIPPER_ANGLE_THRESHOLD)
prev_gripper_open = sim_util.binarize_gripper(
seg_info["%s_gripper_joint" % lr][i_start - 1],
GRIPPER_ANGLE_THRESHOLD) if i_start != 0 else False
if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open,
prev_gripper_open):
redprint("Grab %s failed" % lr)
misgrasp = True
success = False
if not success: break
if len(full_traj[0]) > 0:
if not eval_util.traj_is_safe(sim_env, full_traj,
COLLISION_DIST_THRESHOLD):
redprint("Trajectory not feasible")
feasible = False
if feasible or ignore_infeasibility:
success &= sim_util.sim_full_traj_maybesim(
sim_env,
full_traj,
animate=animate,
interactive=interactive)
else:
success = False
if not success: break
sim_env.sim.settle(animate=animate)
sim_env.sim.release_rope('l')
sim_env.sim.release_rope('r')
sim_util.reset_arms_to_side(sim_env)
if animate:
sim_env.viewer.Step()
return success, feasible, misgrasp, full_trajs
def compute_trans_traj(sim_env,
new_xyz,
seg_info,
ignore_infeasibility=True,
animate=False,
interactive=False):
sim_util.reset_arms_to_side(sim_env)
redprint("Generating end-effector trajectory")
old_xyz = np.squeeze(seg_info["cloud_xyz"])
old_xyz = clouds.downsample(old_xyz, DS_SIZE)
new_xyz = clouds.downsample(new_xyz, DS_SIZE)
link_names = ["%s_gripper_tool_frame" % lr for lr in ('lr')]
hmat_list = [(lr, seg_info[ln]['hmat'])
for lr, ln in zip('lr', link_names)]
if GlobalVars.gripper_weighting:
interest_pts = get_closing_pts(seg_info)
else:
interest_pts = None
lr2eetraj, _, old_xyz_warped = warp_hmats_tfm(old_xyz, new_xyz, hmat_list,
interest_pts)
handles = []
if animate:
handles.append(sim_env.env.plot3(old_xyz, 5, (1, 0, 0)))
handles.append(sim_env.env.plot3(new_xyz, 5, (0, 0, 1)))
handles.append(sim_env.env.plot3(old_xyz_warped, 5, (0, 1, 0)))
miniseg_starts, miniseg_ends, _ = sim_util.split_trajectory_by_gripper(
seg_info, thresh=GRIPPER_ANGLE_THRESHOLD)
success = True
feasible = True
misgrasp = False
print colorize.colorize("mini segments:",
"red"), miniseg_starts, miniseg_ends
full_trajs = []
prev_vals = {lr: None for lr in 'lr'}
for (i_miniseg, (i_start,
i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
################################
redprint("Generating joint trajectory for part %i" % (i_miniseg))
# figure out how we're gonna resample stuff
# Use inverse kinematics to get trajectory for initializing TrajOpt,
# since demonstrations library does not contain joint angle data for
# left and right arms
ee_hmats = {}
for lr in 'lr':
ee_link_name = "%s_gripper_tool_frame" % lr
# TODO: Change # of timesteps for resampling?
ee_hmats[ee_link_name] = resampling.interp_hmats(
np.arange(i_end + 1 - i_start), np.arange(i_end + 1 - i_start),
lr2eetraj[lr][i_start:i_end + 1])
lr2oldtraj = get_old_joint_traj_ik(sim_env, ee_hmats, prev_vals,
i_start, i_end)
#lr2oldtraj = {}
#for lr in 'lr':
# manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
# old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1])
# #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end
# if sim_util.arm_moved(old_joint_traj):
# lr2oldtraj[lr] = old_joint_traj
if len(lr2oldtraj) > 0:
old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
JOINT_LENGTH_PER_STEP = .1
_, timesteps_rs = sim_util.unif_resample(old_total_traj,
JOINT_LENGTH_PER_STEP)
####
### Generate fullbody traj
bodypart2traj = {}
for (lr, old_joint_traj) in lr2oldtraj.items():
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
old_joint_traj_rs = mu.interp2d(timesteps_rs,
np.arange(len(old_joint_traj)),
old_joint_traj)
ee_link_name = "%s_gripper_tool_frame" % lr
new_ee_traj = lr2eetraj[lr][i_start:i_end + 1]
new_ee_traj_rs = resampling.interp_hmats(
timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
print "planning trajectory following"
new_joint_traj, pose_errs = planning.plan_follow_traj(
sim_env.robot, manip_name, sim_env.robot.GetLink(ee_link_name),
new_ee_traj_rs, old_joint_traj_rs)
prev_vals[lr] = new_joint_traj[-1]
part_name = {"l": "larm", "r": "rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
################################
redprint("Executing joint trajectory for part %i using arms '%s'" %
(i_miniseg, bodypart2traj.keys()))
full_traj = sim_util.getFullTraj(sim_env, bodypart2traj)
full_trajs.append(full_traj)
for lr in 'lr':
gripper_open = sim_util.binarize_gripper(
seg_info["%s_gripper_joint" % lr][i_start],
GRIPPER_ANGLE_THRESHOLD)
prev_gripper_open = sim_util.binarize_gripper(
seg_info["%s_gripper_joint" % lr][i_start - 1],
GRIPPER_ANGLE_THRESHOLD) if i_start != 0 else False
if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open,
prev_gripper_open):
redprint("Grab %s failed" % lr)
misgrasp = True
success = False
if not success: break
if len(full_traj[0]) > 0:
if not eval_util.traj_is_safe(sim_env, full_traj,
COLLISION_DIST_THRESHOLD):
redprint("Trajectory not feasible")
feasible = False
if feasible or ignore_infeasibility:
success &= sim_util.sim_full_traj_maybesim(
sim_env,
full_traj,
animate=animate,
interactive=interactive)
else:
success = False
if not success: break
sim_env.sim.settle(animate=animate)
sim_env.sim.release_rope('l')
sim_env.sim.release_rope('r')
sim_util.reset_arms_to_side(sim_env)
if animate:
sim_env.viewer.Step()
return success, feasible, misgrasp, full_trajs
def compute_trans_traj(sim_env, new_xyz, seg_info, ignore_infeasibility=True, animate=False, interactive=False):
sim_util.reset_arms_to_side(sim_env)
redprint("Generating end-effector trajectory")
old_xyz = np.squeeze(seg_info["cloud_xyz"])
old_xyz = clouds.downsample(old_xyz, DS_SIZE)
new_xyz = clouds.downsample(new_xyz, DS_SIZE)
link_names = ["%s_gripper_tool_frame"%lr for lr in ('lr')]
hmat_list = [(lr, seg_info[ln]['hmat']) for lr, ln in zip('lr', link_names)]
if GlobalVars.gripper_weighting:
interest_pts = get_closing_pts(seg_info)
else:
interest_pts = None
lr2eetraj, _, old_xyz_warped = warp_hmats_tfm(old_xyz, new_xyz, hmat_list, interest_pts)
handles = []
if animate:
handles.append(sim_env.env.plot3(old_xyz,5, (1,0,0)))
handles.append(sim_env.env.plot3(new_xyz,5, (0,0,1)))
handles.append(sim_env.env.plot3(old_xyz_warped,5, (0,1,0)))
miniseg_starts, miniseg_ends, _ = sim_util.split_trajectory_by_gripper(seg_info, thresh = GRIPPER_ANGLE_THRESHOLD)
success = True
feasible = True
misgrasp = False
print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends
full_trajs = []
prev_vals = {lr:None for lr in 'lr'}
for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
################################
redprint("Generating joint trajectory for part %i"%(i_miniseg))
# figure out how we're gonna resample stuff
# Use inverse kinematics to get trajectory for initializing TrajOpt,
# since demonstrations library does not contain joint angle data for
# left and right arms
ee_hmats = {}
for lr in 'lr':
ee_link_name = "%s_gripper_tool_frame"%lr
# TODO: Change # of timesteps for resampling?
ee_hmats[ee_link_name] = resampling.interp_hmats(np.arange(i_end+1-i_start), np.arange(i_end+1-i_start), lr2eetraj[lr][i_start:i_end+1])
lr2oldtraj = get_old_joint_traj_ik(sim_env, ee_hmats, prev_vals, i_start, i_end)
#lr2oldtraj = {}
#for lr in 'lr':
# manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
# old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1])
# #print (old_joint_traj[1:] - old_joint_traj[:-1]).ptp(axis=0), i_start, i_end
# if sim_util.arm_moved(old_joint_traj):
# lr2oldtraj[lr] = old_joint_traj
if len(lr2oldtraj) > 0:
old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
JOINT_LENGTH_PER_STEP = .1
_, timesteps_rs = sim_util.unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP)
####
### Generate fullbody traj
bodypart2traj = {}
for (lr,old_joint_traj) in lr2oldtraj.items():
manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj)
ee_link_name = "%s_gripper_tool_frame"%lr
new_ee_traj = lr2eetraj[lr][i_start:i_end+1]
new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
print "planning trajectory following"
new_joint_traj, pose_errs = planning.plan_follow_traj(sim_env.robot, manip_name,
sim_env.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
prev_vals[lr] = new_joint_traj[-1]
part_name = {"l":"larm", "r":"rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
################################
redprint("Executing joint trajectory for part %i using arms '%s'"%(i_miniseg, bodypart2traj.keys()))
full_traj = sim_util.getFullTraj(sim_env, bodypart2traj)
full_trajs.append(full_traj)
for lr in 'lr':
gripper_open = sim_util.binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start], GRIPPER_ANGLE_THRESHOLD)
prev_gripper_open = sim_util.binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start-1], GRIPPER_ANGLE_THRESHOLD) if i_start != 0 else False
if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open, prev_gripper_open):
redprint("Grab %s failed" % lr)
misgrasp = True
success = False
if not success: break
if len(full_traj[0]) > 0:
if not eval_util.traj_is_safe(sim_env, full_traj, COLLISION_DIST_THRESHOLD):
redprint("Trajectory not feasible")
feasible = False
if feasible or ignore_infeasibility:
success &= sim_util.sim_full_traj_maybesim(sim_env, full_traj, animate=animate, interactive=interactive)
else:
success = False
if not success: break
sim_env.sim.settle(animate=animate)
sim_env.sim.release_rope('l')
sim_env.sim.release_rope('r')
sim_util.reset_arms_to_side(sim_env)
if animate:
sim_env.viewer.Step()
return success, feasible, misgrasp, full_trajs
def compute_trans_traj(sim_env, new_xyz, seg_info, ignore_infeasibility=True, animate=False, interactive=False):
redprint("Generating end-effector trajectory")
old_xyz = np.squeeze(seg_info["cloud_xyz"])
old_xyz = clouds.downsample(old_xyz, DS_SIZE)
l1 = len(old_xyz)
new_xyz = clouds.downsample(new_xyz, DS_SIZE)
l2 = len(new_xyz)
print l1, l2
link_names = ["%s_gripper_tool_frame"%lr for lr in ('lr')]
hmat_list = [(lr, seg_info[ln]['hmat']) for lr, ln in zip('lr', link_names)]
if GlobalVars.gripper_weighting:
interest_pts = get_closing_pts(seg_info)
else:
interest_pts = None
lr2eetraj, _, old_xyz_warped, f = warp_hmats_tfm(old_xyz, new_xyz, hmat_list, interest_pts)
handles = []
if animate:
handles.extend(plotting_openrave.draw_grid(sim_env.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04))
handles.append(sim_env.env.plot3(old_xyz,5, (1,0,0))) # red: demonstration point cloud
handles.append(sim_env.env.plot3(new_xyz,5, (0,0,1))) # blue: rope nodes
handles.append(sim_env.env.plot3(old_xyz_warped,5, (0,1,0))) # green: warped point cloud from demonstration
mapped_pts = []
for i in range(len(old_xyz)):
mapped_pts.append(old_xyz[i])
mapped_pts.append(old_xyz_warped[i])
handles.append(sim_env.env.drawlinelist(np.array(mapped_pts), 1, [0.1,0.1,1]))
for lr in 'lr':
handles.append(sim_env.env.drawlinestrip(lr2eetraj[lr][:,:3,3], 2, (0,1,0,1)))
for k, hmats in hmat_list:
hmats_tfm = np.asarray([GlobalVars.init_tfm.dot(h) for h in hmats])
handles.append(sim_env.env.drawlinestrip(hmats_tfm[:,:3,3], 2, (1,0,0,1)))
miniseg_starts, miniseg_ends, lr_open = sim_util.split_trajectory_by_gripper(seg_info, thresh=GRIPPER_ANGLE_THRESHOLD)
success = True
feasible = True
misgrasp = False
print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends
miniseg_trajs = []
prev_vals = {lr:None for lr in 'lr'}
for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
################################
redprint("Generating joint trajectory for part %i"%(i_miniseg))
### adaptive resampling based on xyz in end_effector
end_trans_trajs = np.zeros([i_end+1-i_start, 6])
for lr in 'lr':
for i in xrange(i_start,i_end+1):
if lr == 'l':
end_trans_trajs[i-i_start, :3] = lr2eetraj[lr][i][:3,3]
else:
end_trans_trajs[i-i_start, 3:] = lr2eetraj[lr][i][:3,3]
if True:
adaptive_times, end_trans_trajs = resampling.adaptive_resample2(end_trans_trajs, 0.005)
else:
adaptive_times = range(len(end_trans_trajs))
miniseg_traj = {}
for lr in 'lr':
#ee_hmats = resampling.interp_hmats(np.arange(i_end+1-i_start), np.arange(i_end+1-i_start), lr2eetraj[lr][i_start:i_end+1])
ee_hmats = resampling.interp_hmats(adaptive_times, np.arange(i_end+1-i_start), lr2eetraj[lr][i_start:i_end+1])
# the interpolation above will then the velocity of the trajectory (since there are fewer waypoints). Resampling again to make sure
# the trajectory has the same number of waypoints as before.
ee_hmats = resampling.interp_hmats(np.arange(i_end+1-i_start), adaptive_times, ee_hmats)
# if arm_moved(ee_hmats, floating=True):
if True:
miniseg_traj[lr] = ee_hmats
safe_drop = {'l': True, 'r': True}
for lr in 'lr':
next_gripper_open = lr_open[lr][i_miniseg+1] if i_miniseg < len(miniseg_starts) - 1 else False
gripper_open = lr_open[lr][i_miniseg]
if next_gripper_open and not gripper_open:
tfm = miniseg_traj[lr][-1]
if tfm[2,3] > GlobalVars.table_height + 0.01:
safe_drop[lr] = False
#safe_drop = {'l': True, 'r': True}
if not (safe_drop['l'] and safe_drop['r']):
for lr in 'lr':
if not safe_drop[lr]:
tfm = miniseg_traj[lr][-1]
for i in range(1, 8):
safe_drop_tfm = tfm
safe_drop_tfm[2,3] = tfm[2,3] - i / 10. * (tfm[2,3] - GlobalVars.table_height - 0.01)
miniseg_traj[lr].append(safe_drop_tfm)
else:
for i in range(1, 8):
miniseg_traj[lr].append(miniseg_traj[lr][-1])
miniseg_trajs.append(miniseg_traj)
for lr in 'lr':
hmats = np.asarray(miniseg_traj[lr])
handles.append(sim_env.env.drawlinestrip(hmats[:,:3,3], 2, (0,0,1,1)))
redprint("Executing joint trajectory for part %i using arms '%s'"%(i_miniseg, miniseg_traj.keys()))
for lr in 'lr':
gripper_open = lr_open[lr][i_miniseg]
prev_gripper_open = lr_open[lr][i_miniseg-1] if i_miniseg != 0 else False
if not sim_util.set_gripper_maybesim(sim_env, lr, gripper_open, prev_gripper_open, floating=True):
redprint("Grab %s failed"%lr)
success = False
if not success: break
if len(miniseg_traj) > 0:
success &= sim_util.exec_traj_sim(sim_env, miniseg_traj, animate=animate)
if not success: break
sim_env.sim.settle(animate=animate)
sim_env.sim.release_rope('l')
sim_env.sim.release_rope('r')
if animate:
sim_env.viewer.Step()
return success, feasible, misgrasp, miniseg_trajs
