def registration(xys, fxys):
if args.to3d:
xys = np.c_[xys, np.zeros((len(xys)))]
fxys = np.c_[fxys, np.zeros((len(fxys)))]
scaled_xys, src_params = reg.unit_boxify(xys)
scaled_fxys, targ_params = reg.unit_boxify(fxys)
downsample = voxel_downsample if args.to3d else pixel_downsample
scaled_ds_xys = downsample(scaled_xys, .01)
scaled_ds_fxys = downsample(scaled_fxys, .01)
scaled_ds_xys = hclust_downsample(scaled_ds_xys, 100)
scaled_ds_fxys = hclust_downsample(scaled_ds_fxys, 101)
print "downsampled to %i and %i pts"%(len(scaled_ds_xys),len(scaled_ds_fxys))
tstart = time()
# fest_scaled = reg.tps_rpm(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
fest_scaled,_ = reg.tps_rpm_bij(scaled_ds_xys, scaled_ds_fxys, n_iter=50, reg_init = 10, reg_final=.01, rad_init=.1, rad_final=.01, plot_cb = plot_cb if args.plotting else None, plotting = 0 if args.plotting else 0)
print "time: %.4f"%(time()-tstart)
fest = reg.unscale_tps(fest_scaled, src_params, targ_params)
fxys_est = fest.transform_points(xys)
if len(fxys_est) == len(fxys): print "error:", np.abs(fxys_est - fxys).mean()
if args.plotting: plot_cb(scaled_ds_xys, scaled_ds_fxys, None,None,None,fest_scaled)
def registration_cost(xyz0, xyz1):
scaled_xyz0, _ = registration.unit_boxify(xyz0)
scaled_xyz1, _ = registration.unit_boxify(xyz1)
#TODO: n_iter was 10, reg_final was 0.01
f, g = registration.tps_rpm_bij(scaled_xyz0, scaled_xyz1, n_iter=10,
reg_init=10, reg_final=cost_reg_final)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return cost
def registration_cost(xyz0, xyz1):
scaled_xyz0, _ = registration.unit_boxify(xyz0)
scaled_xyz1, _ = registration.unit_boxify(xyz1)
f, g = registration.tps_rpm_bij(scaled_xyz0,
scaled_xyz1,
rot_reg=1e-3,
n_iter=30)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return cost
def test_bootstrap_tps(task_params):
"""Do one task.
Arguments:
task_params -- a task_params object
If task_parms.max_steps_before failure is -1, then it loops until the knot is detected.
"""
#Begin: setup local variables from parameters
filename = task_params.log_name
demofile_name = task_params.demofile_name
animate = task_params.animate
max_steps_before_failure = task_params.max_steps_before_failure
choose_segment = task_params.choose_segment
knot = task_params.knot
#End
### Setup ###
set_random_seed(task_params)
setup_log(filename)
demofile = setup_and_return_demofile(demofile_name, 'demo1-seg00', animate=animate)
print "set up viewer"
Globals.viewer.Idle()
new_xyz = Globals.sim.observe_cloud()
old_xyz = rotate_about_median(new_xyz, -np.pi/3.0)
int_xyz = rotate_about_median(new_xyz, -np.pi/6.0)
print 'trying to directly map initial to target'
handles = []
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1)))
handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0)))
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
#TODO: get rid of g
f, g = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb=tpsrpm_plot_cb,
plotting=5 if animate else 0, rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=old_xyz, new_xyz=new_xyz)
redprint('reg_cost:\t'+str(f._cost + g._cost))
print 'trying with an intermediate state'
handles = []
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_int_xyz, int_params = registration.unit_boxify(int_xyz)
handles.append(Globals.env.plot3(int_xyz, 5, (0, 0, 1)))
handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0)))
#TODO: get rid of g
_, _, int_corr = registration.tps_rpm_bij(scaled_old_xyz, scaled_int_xyz, plot_cb=tpsrpm_plot_cb,
plotting=5 if animate else 0, rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=old_xyz, new_xyz=int_xyz, return_corr = True)
print 'initializing with intermediate correspondences'
handles = []
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1)))
handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0)))
f, g, _ = registration.tps_rpm_bootstrap(scaled_old_xyz, scaled_int_xyz, scaled_new_xyz, int_corr, plot_cb=tpsrpm_plot_cb,
plotting=5 if animate else 0, rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=old_xyz, new_xyz=new_xyz)
redprint('reg_cost with intermediate stage:\t'+str(f._cost + g._cost))
def registration(xys, fxys):
if args.to3d:
xys = np.c_[xys, np.zeros((len(xys)))]
fxys = np.c_[fxys, np.zeros((len(fxys)))]
scaled_xys, src_params = reg.unit_boxify(xys)
scaled_fxys, targ_params = reg.unit_boxify(fxys)
downsample = voxel_downsample if args.to3d else pixel_downsample
scaled_ds_xys = downsample(scaled_xys, .03)
scaled_ds_fxys = downsample(scaled_fxys, .03)
print "downsampled to %i and %i pts" % (len(scaled_ds_xys),
len(scaled_ds_fxys))
tstart = time()
# fest_scaled = reg.tps_rpm(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
fest_scaled, _ = reg.tps_rpm_bij(scaled_ds_xys,
scaled_ds_fxys,
n_iter=10,
reg_init=10,
reg_final=.01)
print "time: %.4f" % (time() - tstart)
fest = reg.unscale_tps(fest_scaled, src_params, targ_params)
fxys_est = fest.transform_points(xys)
if len(fxys_est) == len(fxys):
print "error:", np.abs(fxys_est - fxys).mean()
if args.plotting:
import matplotlib.pyplot as plt
plt.clf()
# plt.plot(xys[:,1], xys[:,0],'r.')
# plt.plot(fxys[:,1], fxys[:,0],'b.')
# plt.plot(fxys_est[:,1], fxys_est[:,0],'g.')
scaled_ds_fxys_est = fest_scaled.transform_points(scaled_ds_xys)
plt.plot(scaled_ds_xys[:, 1], scaled_ds_xys[:, 0], 'r.')
plt.plot(scaled_ds_fxys[:, 1], scaled_ds_fxys[:, 0], 'b.')
plt.plot(scaled_ds_fxys_est[:, 1], scaled_ds_fxys_est[:, 0], 'g.')
def to2d(f):
def f2d(x):
return f(np.c_[x, np.zeros((len(x), 1))])[:, :2]
return f2d
transform_func = to2d(fest_scaled.transform_points
) if args.to3d else fest_scaled.transform_points
plot_warped_grid_2d(transform_func, [-.5, -.5], [.5, .5])
plt.draw()
plt.ginput()
def registration_cost(xyz_src, xyz_targ, src_interest_pts=None):
if not src_interest_pts:
weights = None
else:
weights = compute_weights(xyz_src, src_interest_pts)
scaled_xyz_src, src_params = registration.unit_boxify(xyz_src)
scaled_xyz_targ, targ_params = registration.unit_boxify(xyz_targ)
f,g = registration.tps_rpm_bij(scaled_xyz_src, scaled_xyz_targ, plot_cb=None,
plotting=0, rot_reg=np.r_[1e-4, 1e-4, 1e-1],
n_iter=100, reg_init=10, reg_final=.1, outlierfrac=1e-2,
x_weights=weights)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return f, src_params, g, targ_params, cost
def registration_cost(xyz0, xyz1):
scaled_xyz0, _ = registration.unit_boxify(xyz0)
scaled_xyz1, _ = registration.unit_boxify(xyz1)
#import matplotlib.pylab as plt
#plt.scatter(scaled_xyz0[:,0], scaled_xyz0[:,1], c='r' )
#plt.hold(True)
#plt.scatter(scaled_xyz1[:,0], scaled_xyz1[:,1], c='b' )
#plt.show()
#print xyz0.shape, xyz1.shape
f, g = registration.tps_rpm_bij(scaled_xyz0, scaled_xyz1, n_iter=10, rot_reg=1e-3)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return cost
def warp_hmats(xyz_src, xyz_targ, hmat_list):
if not use_rapprentice:
return hmat_list
scaled_xyz_src, src_params = registration.unit_boxify(xyz_src)
scaled_xyz_targ, targ_params = registration.unit_boxify(xyz_targ)
f,g = registration.tps_rpm_bij(scaled_xyz_src, scaled_xyz_targ, plot_cb = None,
plotting=0,rot_reg=np.r_[1e-4,1e-4,1e-1],
n_iter=50, reg_init=10, reg_final=.1)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
f = registration.unscale_tps(f, src_params, targ_params)
trajs = {}
for k, hmats in hmat_list:
trajs[k] = f.transform_hmats(hmats)
return [trajs, cost]
def tpsrpm_plot_cb(x_nd, y_md, targ_Nd, corr_nm, wt_n, f, old_xyz, new_xyz, last_one=False):
_, src_params = registration.unit_boxify(old_xyz)
_, targ_params = registration.unit_boxify(new_xyz)
f = registration.unscale_tps(f, src_params, targ_params)
#ypred_nd = f.transform_points(x_nd)
handles = []
#handles.append(Globals.env.plot3(ypred_nd, 3, (0, 1, 0, 1)))
ypred_nd = f.transform_points(old_xyz)
handles.append(Globals.env.plot3(ypred_nd, 3, (0, 1, 0, 1)))
handles.extend(plotting_openrave.draw_grid(Globals.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))
if Globals.viewer:
Globals.viewer.Step()
time.sleep(0.1)
def registration(xys, fxys):
if args.to3d:
xys = np.c_[xys, np.zeros((len(xys)))]
fxys = np.c_[fxys, np.zeros((len(fxys)))]
scaled_xys, src_params = reg.unit_boxify(xys)
scaled_fxys, targ_params = reg.unit_boxify(fxys)
downsample = voxel_downsample if args.to3d else pixel_downsample
scaled_ds_xys = downsample(scaled_xys, .03)
scaled_ds_fxys = downsample(scaled_fxys, .03)
print "downsampled to %i and %i pts"%(len(scaled_ds_xys),len(scaled_ds_fxys))
tstart = time()
# fest_scaled = reg.tps_rpm(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
fest_scaled,_ = reg.tps_rpm_bij(scaled_ds_xys, scaled_ds_fxys, n_iter=10, reg_init = 10, reg_final=.01)
print "time: %.4f"%(time()-tstart)
fest = reg.unscale_tps(fest_scaled, src_params, targ_params)
fxys_est = fest.transform_points(xys)
if len(fxys_est) == len(fxys): print "error:", np.abs(fxys_est - fxys).mean()
if args.plotting:
import matplotlib.pyplot as plt
plt.clf()
# plt.plot(xys[:,1], xys[:,0],'r.')
# plt.plot(fxys[:,1], fxys[:,0],'b.')
# plt.plot(fxys_est[:,1], fxys_est[:,0],'g.')
scaled_ds_fxys_est = fest_scaled.transform_points(scaled_ds_xys)
plt.plot(scaled_ds_xys[:,1], scaled_ds_xys[:,0],'r.')
plt.plot(scaled_ds_fxys[:,1], scaled_ds_fxys[:,0],'b.')
plt.plot(scaled_ds_fxys_est[:,1], scaled_ds_fxys_est[:,0],'g.')
def to2d(f):
def f2d(x):
return f(np.c_[x, np.zeros((len(x),1))])[:,:2]
return f2d
transform_func = to2d(fest_scaled.transform_points) if args.to3d else fest_scaled.transform_points
plot_warped_grid_2d(transform_func, [-.5,-.5], [.5,.5])
plt.draw()
plt.ginput()
def main():
import IPython
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.log:
LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S"))
os.mkdir(LOG_DIR)
LOG_COUNT = 0
if args.execution:
rospy.init_node("exec_task",disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.fake_data_segment:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
while True:
redprint("Acquire point cloud")
if args.fake_data_segment:
fake_seg = demofile[args.fake_data_segment]
new_xyz = np.squeeze(fake_seg["cloud_xyz"])
hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6])
hmat[:3,3] = args.fake_data_transform[0:3]
new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:]
r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0]))
#Globals.pr2.head.set_pan_tilt(0,1.2)
#Globals.pr2.rarm.goto_posture('side')
#Globals.pr2.larm.goto_posture('side')
#Globals.pr2.join_all()
#time.sleep(2)
else:
#Globals.pr2.head.set_pan_tilt(0,1.2)
#Globals.pr2.rarm.goto_posture('side')
#Globals.pr2.larm.goto_posture('side')
#Globals.pr2.join_all()
#time.sleep(2)
Globals.pr2.update_rave()
rgb, depth = grabber.getRGBD()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_xyz = cloud_proc_func(rgb, depth, T_w_k)
print "got new xyz"
redprint(new_xyz)
#grab_end(new_xyz)
if args.log:
LOG_COUNT += 1
import cv2
cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb)
cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth)
np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz)
################################
redprint("Finding closest demonstration")
if args.select_manual:
seg_name = find_closest_manual(demofile, new_xyz)
else:
seg_name = find_closest_auto(demofile, new_xyz)
seg_info = demofile[seg_name]
redprint("closest demo: %s"%(seg_name))
if "done" in seg_name:
redprint("DONE!")
break
if args.log:
with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name)
################################
### Old end effector forces at r_gripper_tool_frame (including torques)
old_forces = seg_info['end_effector_forces'][:,0:3,:]
old_torques = seg_info['end_effector_forces'][:,3:6,:]
redprint("Generating end-effector trajectory")
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb,
plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1)
f = registration.unscale_tps(f, src_params, targ_params)
old_xyz_transformed = f.transform_points(old_xyz)
#handles.append(Globals.env.plot3(old_xyz_transformed,5, np.array([(0,0,1,1) for i in old_xyz_transformed])))
handles.extend(plotting_openrave.draw_grid(Globals.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))
link2eetraj = {}
for lr in 'r':
link_name = "%s_gripper_tool_frame"%lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
#print old_ee_traj
old_ee_pos = old_ee_traj[:,0:3,3]
#print old_ee_pos
# End effector forces as oppossed to end effector trajectories
dfdxs = f.compute_jacobian(old_ee_pos)
old_eefs = []
new_eefs = []
for i in xrange(len(old_forces)):
dfdx = np.matrix(dfdxs[i])
old_force = np.matrix(old_forces[i])
old_torque = np.matrix(old_torques[i])
new_force = (dfdx * old_force)
new_torque = (dfdx * old_torque)
old_eefs.append(np.vstack((old_force, old_torque)))
new_eefs.append(np.vstack((new_force, new_torque)))
old_eefs = np.asarray(old_eefs)[:,:,0]
new_eefs = np.asarray(new_eefs)[:,:,0]
force_data = {}
force_data['old_eefs'] = old_eefs
force_data['new_eefs'] = new_eefs
force_file = open("trial.pickle", 'wa')
pickle.dump(force_data, force_file)
force_file.close()
new_ee_traj_x = new_ee_traj
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
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 args.execution=="real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg))
# figure out how we're gonna resample stuff
lr2oldtraj = {}
for lr in 'r':
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 arm_moved(old_joint_traj): NOT SURE WHY BUT THIS IS RETURNING FALSE
lr2oldtraj[lr] = old_joint_traj
if args.visualize:
r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0]))
for i in range(0, lr2oldtraj['r'].shape[0], 10):
handles = []
handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz])))
handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz])))
handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1)))
# Setting robot arm to joint trajectory
r_vals = lr2oldtraj['r'][i,:]
Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices())
# Plotting forces from r_gripper_tool_frame
hmats = Globals.robot.GetLinkTransformations()
trans, rot = conv.hmat_to_trans_rot(hmats[-3])
f_start = np.array([0,0,0]) + trans
#IPython.embed()
f_end = np.array(old_forces[i].T)[0]/100 + trans
handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1)))
redprint(i)
Globals.viewer.Step()
if len(lr2oldtraj) > 0:
old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
JOINT_LENGTH_PER_STEP = .1
# FIRST RESAMPLING HAPPENS HERE: JOINT_LENGTH
_, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) # Timesteps only, can use to inter eefs for first time
####
new_eefs_segment = asarray(new_eefs[i_start:i_end+1,:]) # Extract miniseg, and re-sample
if args.no_ds:
new_eefs_segment_rs = new_eefs_segment
else:
new_eefs_segment_rs = mu.interp2d(timesteps_rs, np.arange(len(new_eefs_segment)), new_eefs_segment)
### Generate fullbody traj
bodypart2traj = {}
for (lr,old_joint_traj) in lr2oldtraj.items():
manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
ee_link_name = "%s_gripper_tool_frame"%lr
new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1]
if args.no_ds:
old_joint_traj_rs = old_joint_traj
new_ee_traj_rs = new_ee_traj
ds_inds = np.arange(0, len(new_ee_traj_rs), args.trajopt_ds)
new_ee_traj_rs = new_ee_traj_rs[ds_inds]
old_joint_traj_rs = old_joint_traj_rs[ds_inds]
new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
new_joint_traj = mu.interp2d(np.arange(len(old_joint_traj)), np.arange(0, len(new_joint_traj) * args.trajopt_ds, args.trajopt_ds), new_joint_traj)
else:
old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj)
new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
if args.execution: Globals.pr2.update_rave()
part_name = {"l":"larm", "r":"rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
redprint("Joint trajectory has length: " + str(len(bodypart2traj[part_name])))
redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys()))
redprint("Press enter to set gripper")
raw_input()
#for lr in 'r':
# set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start]))
if args.pid:
if not args.fake_data_segment: # If running on PR2, add initial state as waypoint and rough interpolate
# Add initial position
(arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.arm_joint_names)
traj_length = bodypart2traj['rarm'].shape[0]
complete_joint_traj = np.zeros((traj_length+1, 7)) # To include initial state as a way point
complete_joint_traj[1:,:] = bodypart2traj['rarm']
complete_joint_traj[0,:] = arm_position
bodypart2traj['rarm'] = complete_joint_traj
# Add initial eff
J = np.matrix(np.resize(np.array(robot_states.call_return_jacobian()), (6, 7))) # Jacobian
eff = robot_states.compute_end_effector_force(J, arm_effort).T
eff = np.array(eff)[0]
init_force = eff[:3]
complete_force_traj = np.zeros((traj_length+1, 3))
complete_force_traj[1:,:] = new_eefs_segment_rs
complete_force_traj[0,:] = init_force
else:
complete_force_traj = new_eefs_segment_rs
# SECOND RESAMPLING HAPPENS HERE: JOINT VELOCITIES
if args.no_ds:
traj = bodypart2traj['rarm']
stamps = asarray(seg_info['stamps'])
times = np.array([stamps[i_end] - stamps[i_start]])
F = complete_force_traj
else:
times, times_up, traj = pr2_trajectories.return_timed_traj(Globals.pr2, bodypart2traj) # use times and times_up to interpolate the second time
F = mu.interp2d(times_up, times, complete_force_traj)
#Globals.robot.SetDOFValues(asarray(fake_seg["joint_states"]["position"][0]))
if args.visualize:
r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0]))
for i in range(0, traj.shape[0], 10):
handles = []
handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz])))
handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz])))
handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj_rs[:,:3,3], 2, (0,0,1,1)))
# Setting robot arm to joint trajectory
r_vals = traj[i,:]
Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices())
# Plotting forces from r_gripper_tool_frame
hmats = Globals.robot.GetLinkTransformations()
trans, rot = conv.hmat_to_trans_rot(hmats[-3])
f_start = np.array([0,0,0]) + trans
f_end = np.array(old_forces[i].T)[0]/100 + trans
handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1)))
redprint(i)
Globals.viewer.Step()
qs = np.array(np.matrix(traj).T) # 7 x n
qs = np.resize(traj, (1, qs.shape[1]*7))[0] #resize the data to 1x7n (n being the number of steps)
F = np.array(np.matrix(F).T) # 6 x n
F = np.resize(F, (1, F.shape[1]*6))[0] #resize the data to 1x3n
# Controller code
allCs = []
x = np.ones(6) * 1
v = np.ones(6) * 1e-3
a = np.ones(6) * 1e-6
Ct = np.diag(np.hstack((x, v, a)))
num_steps = i_end - i_start + 1
#need to figure out the stamps correctly
stamps = asarray(seg_info['stamps'][i_start:i_end+1])
for t in range(num_steps-1, -1, -1):
allCs.append(Ct)
m = np.ones(6)
Kps = []
Kvs = []
Ks = []
Qt = None
Vs = []
for t in range(num_steps-1, -1, -1):
if Qt is None:
Qt = allCs[t]
else:
Ft = np.zeros((12, 18))
for j in range(12):
Ft[j][j] = 1.0
deltat = abs(stamps[t+1] - stamps[t])
#print(deltat)
for j in range(6):
Ft[j][j+6] = deltat
for j in range(6):
Ft[j+6][j+12] = deltat/m[j]
for j in range(6):
Ft[j][j+12] = ((deltat)**2)/m[j]
Qt = allCs[t] + (np.transpose(Ft).dot(Vs[num_steps-2-t]).dot(Ft))
Qxx = Qt[0:12, 0:12]
Quu = Qt[12:18, 12:18]
Qxu = Qt[0:12, 12:18]
Qux = Qt[12:18, 0:12]
Quuinv = np.linalg.inv(Quu)
Vt = Qxx - Qxu.dot(Quuinv).dot(Qux)
Kt = -1*(Quuinv.dot(Qux))
Ks.append(Kt)
Kps.append(Kt[:, 0:6])
Kvs.append(Kt[:, 6:12])
Vs.append(Vt)
Ks = Ks[::-1]
Kps = Kps[::-1]
Kvs = Kvs[::-1]
JKpJ = []
JKvJ = []
toAddJkpJ = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0]))
toAddJkvJ = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0]))
JacobianOlds = asarray(seg_info["jacobians"][i_start:i_end+1])
for i in range(i_end- i_start + 1):
J = JacobianOlds[i]
psuedoJ = np.linalg.inv(np.transpose(J).dot(J)).dot(np.transpose(J))
#JKpJ.append(np.transpose(J).dot(Kps[i]).dot(J) + toAddJkpJ*0.001)
#JKvJ.append(np.transpose(J).dot(Kvs[i]).dot(J) + toAddJkvJ*0.001)
JKpJ.append(psuedoJ.dot(Kps[i]).dot(J) + toAddJkpJ*0.001)
JKvJ.append(psuedoJ.dot(Kvs[i]).dot(J) + toAddJkvJ*0.001)
if args.useJK:
Kps = []
Kvs = []
for i in range(i_end- i_start + 1):
Kps.append(np.zeros((6,6)))
Kvs.append(np.zeros((6,6)))
else:
JKpJ = []
JKvJ = []
for i in range(i_end- i_start + 1):
JKpJ.append(np.zeros((7,7)))
JKvJ.append(np.zeros((7,7)))
Kps = np.asarray(Kps)
Kvs = np.asarray(Kvs)
JKpJ = np.asarray(JKpJ)
JKvJ = np.asarray(JKvJ)
IPython.embed()
# # Temp Kps and Kvs for testing
"""
toAddJkpJ = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0]))
toAddJkvJ = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0]))
length = complete_force_traj.shape[0]
JKpJ = np.asarray([toAddJkpJ for i in range(length)])
JKpJ = np.resize(JKpJ, (1, 49*length))[0]
JKvJ = np.asarray([toAddJkvJ for i in range(length)])
JKvJ = np.resize(JKvJ, (1, 49*length))[0]
"""
# [traj, Kp, Kv, F, use_force, seconds]
Kps = np.resize(Kps, (1, 36 * num_steps))[0]
Kvs = np.resize(Kvs, (1, 36 * num_steps))[0]
JKpJ = np.resize(JKpJ, (1, 49 * num_steps))[0]
JKvJ = np.resize(JKvJ, (1, 49 * num_steps))[0]
data = np.zeros((1, len(qs) + len(JKpJ) + len(JKvJ) + len(F) + len(Kps) + len(Kvs) + 2))
data[0][0:len(qs)] = qs
data[0][len(qs):len(qs)+len(JKpJ)] = JKpJ
data[0][len(qs)+len(JKpJ):len(qs)+len(JKpJ)+len(JKvJ)] = JKvJ
data[0][len(qs)+len(JKpJ)+len(JKvJ):len(qs)+len(JKpJ)+len(JKvJ)+len(F)] = F
data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)] = Kps
data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)+len(Kvs)] = Kvs
data[0][-2] = args.use_force
data[0][-1] = stamps[i_end] - stamps[i_start]
msg = Float64MultiArray()
msg.data = data[0].tolist()
pub = rospy.Publisher("/controller_data", Float64MultiArray)
redprint("Press enter to start trajectory")
raw_input()
time.sleep(1)
pub.publish(msg)
time.sleep(1)
else:
#if not success: break
if len(bodypart2traj) > 0:
exec_traj_maybesim(bodypart2traj)
def registration_cost(xyz0, xyz1):
scaled_xyz0, _ = registration.unit_boxify(xyz0)
scaled_xyz1, _ = registration.unit_boxify(xyz1)
f,g = registration.tps_rpm_bij(scaled_xyz0, scaled_xyz1, rot_reg=1e-3, n_iter=30)
cost = registration.tps_reg_cost(f) + registration.tps_reg_cost(g)
return cost
def main():
import IPython
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.log:
LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S"))
os.mkdir(LOG_DIR)
LOG_COUNT = 0
if args.execution:
rospy.init_node("exec_task",disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.fake_data_segment:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
while True:
redprint("Acquire point cloud")
if args.fake_data_segment:
fake_seg = demofile[args.fake_data_segment]
new_xyz = np.squeeze(fake_seg["cloud_xyz"])
hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6])
hmat[:3,3] = args.fake_data_transform[0:3]
new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:]
r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0]))
#Globals.pr2.head.set_pan_tilt(0,1.2)
#Globals.pr2.rarm.goto_posture('side')
#Globals.pr2.larm.goto_posture('side')
#Globals.pr2.join_all()
#time.sleep(2)
else:
#Globals.pr2.head.set_pan_tilt(0,1.2)
#Globals.pr2.rarm.goto_posture('side')
#Globals.pr2.larm.goto_posture('side')
#Globals.pr2.join_all()
#time.sleep(2)
Globals.pr2.update_rave()
rgb, depth = grabber.getRGBD()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_xyz = cloud_proc_func(rgb, depth, T_w_k)
print "got new xyz"
redprint(new_xyz)
#grab_end(new_xyz)
if args.log:
LOG_COUNT += 1
import cv2
cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb)
cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth)
np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz)
################################
redprint("Finding closest demonstration")
if args.select_manual:
seg_name = find_closest_manual(demofile, new_xyz)
else:
seg_name = find_closest_auto(demofile, new_xyz)
seg_info = demofile[seg_name]
redprint("closest demo: %s"%(seg_name))
if "done" in seg_name:
redprint("DONE!")
break
if args.log:
with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name)
################################
### Old end effector forces at r_gripper_tool_frame (eliminating the torques for now)
old_eefs = seg_info['end_effector_forces'][:,0:3,:]
redprint("Generating end-effector trajectory")
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb,
plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1)
f = registration.unscale_tps(f, src_params, targ_params)
old_xyz_transformed = f.transform_points(old_xyz)
#handles.append(Globals.env.plot3(old_xyz_transformed,5, np.array([(0,0,1,1) for i in old_xyz_transformed])))
handles.extend(plotting_openrave.draw_grid(Globals.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))
link2eetraj = {}
for lr in 'r':
link_name = "%s_gripper_tool_frame"%lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
#print old_ee_traj
old_ee_pos = old_ee_traj[:,0:3,3]
#print old_ee_pos
# End effector forces as oppossed to end effector trajectories
dfdxs = f.compute_jacobian(old_ee_pos)
new_eefs = []
for i in xrange(len(old_eefs)):
dfdx = np.matrix(dfdxs[i])
old_eef = np.matrix(old_eefs[i])
new_eefs.append(dfdx * old_eef)
old_eefs = asarray(old_eefs)[:,:,0]
new_eefs = asarray(new_eefs)[:,:,0]
force_data = {}
force_data['old_eefs'] = old_eefs
force_data['new_eefs'] = new_eefs
force_file = open("trial.pickle", 'wa')
pickle.dump(force_data, force_file)
force_file.close()
new_ee_traj_x = new_ee_traj
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
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 args.execution=="real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg))
# figure out how we're gonna resample stuff
lr2oldtraj = {}
for lr in 'r':
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 arm_moved(old_joint_traj): NOT SURE WHY BUT THIS IS RETURNING FALSE
lr2oldtraj[lr] = old_joint_traj
if args.visualize:
r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0]))
for i in range(0, lr2oldtraj['r'].shape[0], 10):
handles = []
handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz])))
handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz])))
handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1)))
# Setting robot arm to joint trajectory
r_vals = lr2oldtraj['r'][i,:]
Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices())
# Plotting forces from r_gripper_tool_frame
hmats = Globals.robot.GetLinkTransformations()
trans, rot = conv.hmat_to_trans_rot(hmats[-3])
f_start = np.array([0,0,0]) + trans
f_end = np.array(old_eefs[i])/100 + trans
handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1)))
redprint(i)
Globals.viewer.Step()
if len(lr2oldtraj) > 0:
old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
JOINT_LENGTH_PER_STEP = .1
# FIRST RESAMPLING HAPPENS HERE: JOINT_LENGTH
_, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) # Timesteps only, can use to inter eefs for first time
####
new_eefs_segment = asarray(new_eefs[i_start:i_end+1,:]) # Extract miniseg, and re-sample
if args.no_ds:
new_eefs_segment_rs = new_eefs_segment
else:
new_eefs_segment_rs = mu.interp2d(timesteps_rs, np.arange(len(new_eefs_segment)), new_eefs_segment)
### Generate fullbody traj
bodypart2traj = {}
for (lr,old_joint_traj) in lr2oldtraj.items():
manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
ee_link_name = "%s_gripper_tool_frame"%lr
new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1]
if args.no_ds:
old_joint_traj_rs = old_joint_traj
new_ee_traj_rs = new_ee_traj
ds_inds = np.arange(0, len(new_ee_traj_rs), args.trajopt_ds)
new_ee_traj_rs = new_ee_traj_rs[ds_inds]
old_joint_traj_rs = old_joint_traj_rs[ds_inds]
new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
new_joint_traj = mu.interp2d(np.arange(len(old_joint_traj)), np.arange(0, len(new_joint_traj) * args.trajopt_ds, args.trajopt_ds), new_joint_traj)
else:
old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj)
new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
if args.execution: Globals.pr2.update_rave()
part_name = {"l":"larm", "r":"rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
redprint("Joint trajectory has length: " + str(len(bodypart2traj[part_name])))
redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys()))
if args.pid:
if not args.fake_data_segment: # If running on PR2, add initial state as waypoint and rough interpolate
# Add initial position
(arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.arm_joint_names)
traj_length = bodypart2traj['rarm'].shape[0]
complete_joint_traj = np.zeros((traj_length+1, 7)) # To include initial state as a way point
complete_joint_traj[1:,:] = bodypart2traj['rarm']
complete_joint_traj[0,:] = arm_position
bodypart2traj['rarm'] = complete_joint_traj
# Add initial eff
J = np.matrix(np.resize(np.array(robot_states.call_return_jacobian()), (6, 7))) # Jacobian
eff = robot_states.compute_end_effector_force(J, arm_effort).T
eff = np.array(eff)[0]
init_force = eff[:3]
complete_force_traj = np.zeros((traj_length+1, 3))
complete_force_traj[1:,:] = new_eefs_segment_rs
complete_force_traj[0,:] = init_force
else:
complete_force_traj = new_eefs_segment_rs
# SECOND RESAMPLING HAPPENS HERE: JOINT VELOCITIES
if args.no_ds:
traj = bodypart2traj['rarm']
stamps = asarray(seg_info['stamps'])
times = np.array([stamps[i_end] - stamps[i_start]])
force = complete_force_traj
else:
times, times_up, traj = pr2_trajectories.return_timed_traj(Globals.pr2, bodypart2traj) # use times and times_up to interpolate the second time
force = mu.interp2d(times_up, times, complete_force_traj)
#Globals.robot.SetDOFValues(asarray(fake_seg["joint_states"]["position"][0]))
if args.visualize:
r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0]))
for i in range(0, traj.shape[0], 10):
handles = []
handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz])))
handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz])))
handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj_rs[:,:3,3], 2, (0,0,1,1)))
# Setting robot arm to joint trajectory
r_vals = traj[i,:]
Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices())
# Plotting forces from r_gripper_tool_frame
hmats = Globals.robot.GetLinkTransformations()
trans, rot = conv.hmat_to_trans_rot(hmats[-3])
f_start = np.array([0,0,0]) + trans
f_end = np.array(force[i])/100 + trans
handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1)))
redprint(i)
Globals.viewer.Step()
traj = np.array(np.matrix(traj).T) # 7 x n
redprint(traj)
traj = np.resize(traj, (1, traj.shape[1]*7)) #resize the data to 1x7n (n being the number of steps)
force = np.array(np.matrix(force).T) # 3 x n
force = np.resize(force, (1, force.shape[1]*3)) #resize the data to 1x3n
#[traj, force, secs]
traj_force_secs = np.zeros((1, traj.shape[1] + force.shape[1] + 2))
traj_force_secs[0,0:traj.shape[1]] = traj
traj_force_secs[0,traj.shape[1]:traj.shape[1]+force.shape[1]] = force
traj_force_secs[0,traj.shape[1]+force.shape[1]] = times[len(times)-1]
traj_force_secs[0,traj.shape[1]+force.shape[1]+1] = args.use_force
IPython.embed()
msg = Float64MultiArray()
msg.data = traj_force_secs[0].tolist()
pub = rospy.Publisher("/joint_positions_forces_secs", Float64MultiArray)
redprint("Press enter to start trajectory")
raw_input()
time.sleep(1)
pub.publish(msg)
time.sleep(1)
else:
#if not success: break
if len(bodypart2traj) > 0:
exec_traj_maybesim(bodypart2traj)
def main():
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.log:
LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"),
datetime.datetime.now().strftime("%Y%m%d-%H%M%S"))
os.mkdir(LOG_DIR)
LOG_COUNT = 0
if args.execution:
rospy.init_node("exec_task", disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.fake_data_segment:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
while True:
redprint("Acquire point cloud")
if args.fake_data_segment:
fake_seg = demofile[args.fake_data_segment]
new_xyz = np.squeeze(fake_seg["cloud_xyz"])
hmat = openravepy.matrixFromAxisAngle(
args.fake_data_transform[3:6])
hmat[:3, 3] = args.fake_data_transform[0:3]
new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :]
r2r = ros2rave.RosToRave(Globals.robot,
asarray(fake_seg["joint_states"]["name"]))
r2r.set_values(Globals.robot,
asarray(fake_seg["joint_states"]["position"][0]))
else:
Globals.pr2.head.set_pan_tilt(0, 1.2)
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.larm.goto_posture('side')
Globals.pr2.join_all()
time.sleep(.5)
Globals.pr2.update_rave()
rgb, depth = grabber.getRGBD()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_xyz = cloud_proc_func(rgb, depth, T_w_k)
#grab_end(new_xyz)
if args.log:
LOG_COUNT += 1
import cv2
cv2.imwrite(osp.join(LOG_DIR, "rgb%i.png" % LOG_COUNT), rgb)
cv2.imwrite(osp.join(LOG_DIR, "depth%i.png" % LOG_COUNT), depth)
np.save(osp.join(LOG_DIR, "xyz%i.npy" % LOG_COUNT), new_xyz)
################################
redprint("Finding closest demonstration")
if args.select_manual:
seg_name = find_closest_manual(demofile, new_xyz)
else:
seg_name = find_closest_auto(demofile, new_xyz)
seg_info = demofile[seg_name]
redprint("closest demo: %s" % (seg_name))
if "done" in seg_name:
redprint("DONE!")
break
if args.log:
with open(osp.join(LOG_DIR, "neighbor%i.txt" % LOG_COUNT),
"w") as fh:
fh.write(seg_name)
################################
redprint("Generating end-effector trajectory")
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0)))
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1)))
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
f, _ = registration.tps_rpm_bij(scaled_old_xyz,
scaled_new_xyz,
plot_cb=tpsrpm_plot_cb,
plotting=5 if args.animation else 0,
rot_reg=np.r_[1e-4, 1e-4, 1e-1],
n_iter=50,
reg_init=10,
reg_final=.1)
f = registration.unscale_tps(f, src_params, targ_params)
handles.extend(
plotting_openrave.draw_grid(Globals.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))
link2eetraj = {}
for lr in 'lr':
link_name = "%s_gripper_tool_frame" % lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
handles.append(
Globals.env.drawlinestrip(old_ee_traj[:, :3, 3], 2,
(1, 0, 0, 1)))
handles.append(
Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2,
(0, 1, 0, 1)))
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
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 args.execution == "real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i" %
(seg_name, i_miniseg))
# figure out how we're gonna resample stuff
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 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 = 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 = link2eetraj[ee_link_name][i_start:i_end + 1]
new_ee_traj_rs = resampling.interp_hmats(
timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
if args.execution: Globals.pr2.update_rave()
new_joint_traj = planning.plan_follow_traj(
Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,
old_joint_traj_rs)
part_name = {"l": "larm", "r": "rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
################################
redprint(
"Executing joint trajectory for segment %s, part %i using arms '%s'"
% (seg_name, i_miniseg, bodypart2traj.keys()))
for lr in 'lr':
success &= set_gripper_maybesim(
lr,
binarize_gripper(seg_info["%s_gripper_joint" %
lr][i_start]))
# Doesn't actually check if grab occurred, unfortunately
if not success: break
if len(bodypart2traj) > 0:
success &= exec_traj_maybesim(bodypart2traj)
if not success: break
redprint("Segment %s result: %s" % (seg_name, success))
if args.fake_data_segment: break
def get_warped_trajectory(seg_info, new_xyz, demofile, warp_root=True, plot=False, no_cmat=False):
"""
@seg_info : segment information from the h5 file for the segment with least tps fit cost.
@new_xyz : point cloud of the rope in the test situation.
@warp_root : warp the root trajectory if True else warp the chosen segment's trajectory.
@returns : the warped trajectory for l/r grippers and the mini-segment information.
"""
print "****WARP ROOT*** : ", warp_root
print "****NO CMAT*** : ", no_cmat
handles = []
seg_xyz = seg_info["cloud_xyz"][:]
scaled_seg_xyz, seg_params = registration.unit_boxify(seg_xyz)
scaled_new_xyz, new_params = registration.unit_boxify(new_xyz)
if plot:
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1)))
handles.append(Globals.env.plot3(seg_xyz, 5, (1, 0, 0)))
root_seg_name = seg_info['root_seg']
root_segment = demofile[root_seg_name.value]
root_xyz = root_segment['cloud_xyz'][:]
seg_root_cmat = seg_info['cmat'][:]
if warp_root:
scaled_root_xyz, root_params = registration.unit_boxify(root_xyz)
if no_cmat:
print "not using cmat for correspondences"
f_root2new, _, corr_new2root = registration.tps_rpm_bij(scaled_root_xyz, scaled_new_xyz,
plotting=5 if plot else 0, plot_cb=tpsrpm_plot_cb,
rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=root_xyz, new_xyz=new_xyz,
return_corr=True)
else:
## !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
## TODO : MAKE SURE THAT THE SCALING IS BEING DONE CORRECTLY HERE:
## !!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!
f_root2new, _, corr_new2root = registration.tps_rpm_bootstrap(scaled_root_xyz, scaled_seg_xyz, scaled_new_xyz, seg_root_cmat,
plotting=5 if plot else 0, plot_cb=tpsrpm_plot_cb,
rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=root_xyz, new_xyz=new_xyz)
f_warping = registration.unscale_tps(f_root2new, root_params, new_params)
old_ee_traj = root_segment['hmats']
rgrip_joints = root_segment['r_gripper_joint'][:]
lgrip_joints = root_segment['l_gripper_joint'][:]
cmat = corr_new2root
else: ## warp to the chosen segment:
f_seg2new, _, corr_new2seg = registration.tps_rpm_bij(scaled_seg_xyz, scaled_new_xyz, plot_cb=tpsrpm_plot_cb,
plotting=5 if plot else 0, rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=seg_xyz, new_xyz=new_xyz,
return_corr=True)
f_warping = registration.unscale_tps(f_seg2new, seg_params, new_params)
old_ee_traj = seg_info['hmats']
rgrip_joints = root_segment['r_gripper_joint'][:]
lgrip_joints = root_segment['l_gripper_joint'][:]
cmat = seg_root_cmat.dot(corr_new2seg)
if compare_bootstrap_correspondences:
scaled_root_xyz, root_params = registration.unit_boxify(root_xyz)
f_root2new, _, corr_new2root = registration.tps_rpm_bootstrap(scaled_root_xyz, scaled_seg_xyz, scaled_new_xyz, seg_root_cmat,
plotting=5 if plot else 0, plot_cb=tpsrpm_plot_cb,
rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=root_xyz, new_xyz=new_xyz)
root_warping = registration.unscale_tps(f_root2new, root_params, new_params)
root_ee_traj = root_segment['hmats']
diff = 0
for lr in 'lr':
no_root_ee_traj = f_warping.transform_hmats(old_ee_traj[lr][:])
warped_root_ee_traj = root_warping.transform_hmats(root_ee_traj[lr][:])
diff += np.linalg.norm(no_root_ee_traj - warped_root_ee_traj)
handles.append(Globals.env.drawlinestrip(old_ee_traj[lr][:, :3, 3], 2, (1, 0, 0, 1)))
handles.append(Globals.env.drawlinestrip(no_root_ee_traj[:, :3, 3], 2, (0, 1, 0, 1)))
handles.append(Globals.env.drawlinestrip(warped_root_ee_traj[:, :3, 3], 2, (0, 1, 0, 1)))
if plot:
print 'traj norm differences:\t', diff
Globals.viewer.Idle()
if plot:
handles.extend(plotting_openrave.draw_grid(Globals.env, f_warping.transform_points, new_xyz.min(axis=0) - np.r_[0, 0, .1],
new_xyz.max(axis=0) + np.r_[0, 0, .02], xres=.01, yres=.01, zres=.04))
warped_ee_traj = {}
#Transform the gripper trajectory here
for lr in 'lr':
new_ee_traj = f_warping.transform_hmats(old_ee_traj[lr][:])
warped_ee_traj[lr] = new_ee_traj
if plot:
handles.append(Globals.env.drawlinestrip(old_ee_traj[lr][:, :3, 3], 2, (1, 0, 0, 1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2, (0, 1, 0, 1)))
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(rgrip_joints, lgrip_joints)
return (cmat, warped_ee_traj, miniseg_starts, miniseg_ends, {'r':rgrip_joints, 'l':lgrip_joints})
def main():
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.log:
LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S"))
os.mkdir(LOG_DIR)
LOG_COUNT = 0
if args.execution:
rospy.init_node("exec_task",disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.fake_data_segment:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
while True:
redprint("Acquire point cloud")
if args.fake_data_segment:
fake_seg = demofile[args.fake_data_segment]
new_xyz = np.squeeze(fake_seg["cloud_xyz"])
hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6])
hmat[:3,3] = args.fake_data_transform[0:3]
new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:]
r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0]))
else:
Globals.pr2.head.set_pan_tilt(0,1.2)
Globals.pr2.rarm.goto_posture('side')
Globals.pr2.larm.goto_posture('side')
Globals.pr2.join_all()
time.sleep(.5)
Globals.pr2.update_rave()
rgb, depth = grabber.getRGBD()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_xyz = cloud_proc_func(rgb, depth, T_w_k)
#grab_end(new_xyz)
if args.log:
LOG_COUNT += 1
import cv2
cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb)
cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth)
np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz)
################################
redprint("Finding closest demonstration")
if args.select_manual:
seg_name = find_closest_manual(demofile, new_xyz)
else:
seg_name = find_closest_auto(demofile, new_xyz)
seg_info = demofile[seg_name]
redprint("closest demo: %s"%(seg_name))
if "done" in seg_name:
redprint("DONE!")
break
if args.log:
with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name)
################################
redprint("Generating end-effector trajectory")
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
handles.append(Globals.env.plot3(old_xyz,5, (1,0,0)))
handles.append(Globals.env.plot3(new_xyz,5, (0,0,1)))
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb,
plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1)
f = registration.unscale_tps(f, src_params, targ_params)
handles.extend(plotting_openrave.draw_grid(Globals.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))
link2eetraj = {}
for lr in 'lr':
link_name = "%s_gripper_tool_frame"%lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1)))
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
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 args.execution=="real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg))
# figure out how we're gonna resample stuff
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 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 = 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 = link2eetraj[ee_link_name][i_start:i_end+1]
new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
if args.execution: Globals.pr2.update_rave()
new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
part_name = {"l":"larm", "r":"rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
################################
redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys()))
for lr in 'lr':
success &= set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start]))
# Doesn't actually check if grab occurred, unfortunately
if not success: break
if len(bodypart2traj) > 0:
success &= exec_traj_maybesim(bodypart2traj)
if not success: break
redprint("Segment %s result: %s"%(seg_name, success))
if args.fake_data_segment: break
return [cached_extract_red(aa(seg["rgb"]), aa(seg["depth"]), aa(seg["T_w_k"])) for (key,seg) in hdf.items()]
if __name__ == "__main__":
hdf = h5py.File("/Users/joschu/Data/knots/master.h5", "r")
clouds = extract_clouds(hdf)
dim = 2
for cloud0 in clouds:
for cloud1 in clouds:
if dim ==2 :
cloud0 = cloud0[:,:2]
cloud1 = cloud1[:,:2]
scaled_cloud0, src_params = reg.unit_boxify(cloud0)
scaled_cloud1, targ_params = reg.unit_boxify(cloud1)
print "downsampled to %i and %i pts"%(len(scaled_cloud0),len(scaled_cloud1))
tstart = time()
fest_scaled,gest_scaled = reg.tps_rpm_bij(scaled_cloud0, scaled_cloud1, n_iter=10, reg_init = 10, reg_final=.01)
print "%.3f elapsed"%(time() - tstart)
cost = reg.tps_reg_cost(fest_scaled) + reg.tps_reg_cost(gest_scaled)
print "cost: %.3f"%cost
import matplotlib.pyplot as plt
plt.clf()
# plt.plot(xys[:,1], xys[:,0],'r.')
# plt.plot(fxys[:,1], fxys[:,0],'b.')
def main():
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.execution:
rospy.init_node("exec_task",disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.fake_data_segment:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
started_bag = False
started_video = False
localtime = time.localtime()
time_string = time.strftime("%Y-%m-%d-%H-%M-%S", localtime)
os.chdir(osp.dirname(args.new_demo))
if not osp.exists(args.new_demo):
yn = yes_or_no("master file does not exist. create?")
basename = raw_input("what is the base name?\n").strip()
if yn:
with open(args.new_demo,'w') as fh: fh.write("""
name: %s
h5path: %s
bags:
"""%(basename, basename+".h5"))
else:
print "exiting."
exit(0)
with open(args.new_demo, "r") as fh: master_info = yaml.load(fh)
if master_info["bags"] is None: master_info["bags"] = []
for suffix in itertools.chain("", (str(i) for i in itertools.count())):
demo_name = args.demo_prefix + suffix
if not any(bag["demo_name"] == demo_name for bag in master_info["bags"]):
break
print demo_name
timestampfile = demo_name+"timestamps.txt"
fht = open(timestampfile,"w")
try:
bag_cmd = "rosbag record /joint_states /joy -O %s"%demo_name
#print colorize(bag_cmd, "green")
bag_handle = subprocess.Popen(bag_cmd, shell=True)
time.sleep(1)
poll_result = bag_handle.poll()
print "poll result", poll_result
if poll_result is not None:
raise Exception("problem starting video recording")
else: started_bag = True
video_cmd = "record_rgbd_video --out=%s --downsample=%i"%(demo_name, args.downsample)
#print colorize(video_cmd, "green")
video_handle = subprocess.Popen(video_cmd, shell=True)
started_video = True
#grab_end(new_xyz)
fht.write('look:' + str(rospy.get_rostime().secs))
################################
redprint("Finding closest demonstration")
seg_name = find_closest_manual(demofile, None)
seg_info = demofile[seg_name]
redprint("Generating end-effector trajectory")
old_xyz = np.squeeze(seg_info["cloud_xyz"])
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
link2eetraj = {}
for lr in 'lr':
link_name = "%s_gripper_tool_frame"%lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
if args.execution=="real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg))
# figure out how we're gonna resample stuff
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
lr2oldtraj[lr] = old_joint_traj
### 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 = link2eetraj[ee_link_name][i_start:i_end+1]
#new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
if args.execution: Globals.pr2.update_rave()
#new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name,
# Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
part_name = {"l":"larm", "r":"rarm"}[lr]
bodypart2traj[part_name] = old_joint_traj
traj = {}
for lr in 'lr':
part_name = {"l":"larm", "r":"rarm"}[lr]
traj[lr] = bodypart2traj[part_name]
if i_miniseg ==0:
redprint("Press enter to use current position as starting point")
raw_input()
arm_positions = {}
(arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.r_arm_joint_names)
arm_positions['r'] = arm_position
diff_r = np.array(arm_position - traj['r'][0,:])
maximum_r = max(abs(diff_r))
(arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.l_arm_joint_names)
arm_positions['l'] = arm_position
diff_l = np.array(arm_position - traj['l'][0,:])
maximum_l = max(abs(diff_l))
maximum = max(maximum_l, maximum_r)
speed = (20.0/360.0*2*(np.pi))
time_needed = maximum / speed
initial_pos_traj = {}
for lr in 'lr':
part_name = {"l":"larm", "r":"rarm"}[lr]
initial_pos_traj[part_name] = mu.interp2d(np.arange(0, time_needed, 0.01), np.array([0,time_needed]), np.array([arm_positions[lr], traj[lr][0,:]]))
#initial_traj_length = initial_pos_traj.shape[0]
#traj[lr] = np.concatenate((initial_pos_traj, traj_part[lr]), axis=0)
#=======================send to controller======================
length_total = initial_pos_traj["larm"].shape[0]
qs_l = np.resize(initial_pos_traj["larm"], (1, initial_pos_traj["larm"].shape[0]*7))[0] #resize the data to 1x7n (n being the number of steps)
qs_r = np.resize(initial_pos_traj["rarm"], (1, initial_pos_traj["rarm"].shape[0]*7))[0]
#F = np.array(np.matrix(F).T) # 6 x n
F = np.zeros((length_total,6))
F = np.resize(F, (1, F.shape[0]*6))[0] #resize the data to 1x3n
gripper = np.zeros((length_total,1))
gripper = np.resize(gripper, (1, gripper.shape[0]*1))[0]
# Controller code in joint space
pgains = np.asarray([2400.0, 1200.0, 1000.0, 700.0, 300.0, 300.0, 300.0])
dgains = np.asarray([18.0, 10.0, 6.0, 4.0, 6.0, 4.0, 4.0])
# Gains as Kps and Kvs for testing
Kps = []
Kvs = []
for i in range(length_total):
Kps.append(np.zeros((6,6)))
Kvs.append(np.zeros((6,6)))
toAddJkpJ = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0]))
toAddJkvJ = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0]))
#toAddJkvJ = np.diag(np.asarray([0, 0, 0, 0, 0, 0, 0]))
#length = complete_force_traj.shape[0]
JKpJ = np.asarray([toAddJkpJ for i in range(length_total)])
JKpJ = np.resize(JKpJ, (1, 49*length_total))[0]
JKvJ = np.asarray([toAddJkvJ for i in range(length_total)])
JKvJ = np.resize(JKvJ, (1, 49*length_total))[0]
# [traj, Kp, Kv, F, use_force, seconds]
Kps = np.resize(Kps, (1, 36 * length_total))[0]
Kvs = np.resize(Kvs, (1, 36 * length_total))[0]
#JKpJ = np.resize(JKpJ, (1, 49 * num_steps))[0]
#JKvJ = np.resize(JKvJ, (1, 49 * num_steps))[0]
stamps = asarray(seg_info['stamps'])
data = np.zeros((1, length_total*(7+49+49+6+36+36+7+49+49+6+36+36+1)+2))
data[0][0:length_total*7] = qs_r
data[0][length_total*7:length_total*(7+49)] = JKpJ
data[0][length_total*(7+49):length_total*(7+49+49)] = JKvJ
data[0][length_total*(7+49+49):length_total*(7+49+49+6)] = F
data[0][length_total*(7+49+49+6):length_total*(7+49+49+6+36)] = Kps
data[0][length_total*(7+49+49+6+36):length_total*(7+49+49+6+36+36)] = Kvs
data[0][length_total*(7+49+49+6+36+36):length_total*(7+49+49+6+36+36+7)] = qs_l
data[0][length_total*(7+49+49+6+36+36+7):length_total*(7+49+49+6+36+36+7+49)] = JKpJ
data[0][length_total*(7+49+49+6+36+36+7+49):length_total*(7+49+49+6+36+36+7+49+49)] = JKvJ
data[0][length_total*(7+49+49+6+36+36+7+49+49):length_total*(7+49+49+6+36+36+7+49+49+6)] = F
data[0][length_total*(7+49+49+6+36+36+7+49+49+6):length_total*(7+49+49+6+36+36+7+49+49+6+36)] = Kps
data[0][length_total*(7+49+49+6+36+36+7+49+49+6+36):length_total*(7+49+49+6+36+36+7+49+49+6+36+36)] = Kvs
data[0][length_total*(7+49+49+6+36+36+7+49+49+6+36+36):length_total*(7+49+49+6+36+36+7+49+49+6+36+36+1)] = gripper
data[0][-2] = 0
data[0][-1] = stamps[i_end] - stamps[i_start]
msg = Float64MultiArray()
msg.data = data[0].tolist()
pub = rospy.Publisher("/controller_data", Float64MultiArray)
redprint("Press enter to start trajectory")
raw_input()
time.sleep(1)
pub.publish(msg)
time.sleep(1)
#===================end send to controller=======================
raw_input("CAME TO START POSITION")
time.sleep(1)
fht.write('\nstart:' + str(rospy.get_rostime().secs))
################################
redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys()))
if not args.useHenry:
for lr in 'lr':
success &= set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start]))
# Doesn't actually check if grab occurred, unfortunately
print('1')
fht.write('\nstart:' + str(rospy.get_rostime().secs))
print('2')
if len(bodypart2traj) > 0:
success &= exec_traj_maybesim(bodypart2traj)
print('3')
fht.write('\nstop:' + str(rospy.get_rostime().secs))
print('4')
else:
for lr in 'lr':
redprint("Press enter to set gripper")
raw_input()
set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start]))
# Doesn't actually check if grab occurred, unfortunately
if bodypart2traj!={}:
length = i_end - i_start + 1
length_total = length
traj_r = traj['r']
qs_r = np.resize(traj_r, (1, traj_r.shape[0]*7))[0] #resize the data to 1x7n (n being the number of steps)
traj_l = traj['l']
qs_l = np.resize(traj_l, (1, traj_l.shape[0]*7))[0] #resize the data to 1x7n (n being the number of steps)
#F = np.array(np.matrix(F).T) # 6 x n
F = np.zeros((length_total,6))
F = np.resize(F, (1, F.shape[0]*6))[0] #resize the data to 1x3n
gripper = np.zeros((length_total,1))
gripper = np.resize(gripper, (1, gripper.shape[0]*1))[0]
# Controller code in joint space
pgains = np.asarray([2400.0, 1200.0, 1000.0, 700.0, 300.0, 300.0, 300.0])
dgains = np.asarray([18.0, 10.0, 6.0, 4.0, 6.0, 4.0, 4.0])
# Gains as Kps and Kvs for testing
Kps = []
Kvs = []
for i in range(length_total):
Kps.append(np.zeros((6,6)))
Kvs.append(np.zeros((6,6)))
toAddJkpJ = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0]))
toAddJkvJ = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0]))
#toAddJkvJ = np.diag(np.asarray([0, 0, 0, 0, 0, 0, 0]))
#length = complete_force_traj.shape[0]
JKpJ = np.asarray([toAddJkpJ for i in range(length_total)])
JKpJ = np.resize(JKpJ, (1, 49*length_total))[0]
JKvJ = np.asarray([toAddJkvJ for i in range(length_total)])
JKvJ = np.resize(JKvJ, (1, 49*length_total))[0]
# [traj, Kp, Kv, F, use_force, seconds]
Kps = np.resize(Kps, (1, 36 * length_total))[0]
Kvs = np.resize(Kvs, (1, 36 * length_total))[0]
#JKpJ = np.resize(JKpJ, (1, 49 * num_steps))[0]
#JKvJ = np.resize(JKvJ, (1, 49 * num_steps))[0]
stamps = asarray(seg_info['stamps'])
data = np.zeros((1, length_total*(7+49+49+6+36+36+7+49+49+6+36+36+1)+2))
data[0][0:length_total*7] = qs_r
data[0][length_total*7:length_total*(7+49)] = JKpJ
data[0][length_total*(7+49):length_total*(7+49+49)] = JKvJ
data[0][length_total*(7+49+49):length_total*(7+49+49+6)] = F
data[0][length_total*(7+49+49+6):length_total*(7+49+49+6+36)] = Kps
data[0][length_total*(7+49+49+6+36):length_total*(7+49+49+6+36+36)] = Kvs
data[0][length_total*(7+49+49+6+36+36):length_total*(7+49+49+6+36+36+7)] = qs_l
data[0][length_total*(7+49+49+6+36+36+7):length_total*(7+49+49+6+36+36+7+49)] = JKpJ
data[0][length_total*(7+49+49+6+36+36+7+49):length_total*(7+49+49+6+36+36+7+49+49)] = JKvJ
data[0][length_total*(7+49+49+6+36+36+7+49+49):length_total*(7+49+49+6+36+36+7+49+49+6)] = F
data[0][length_total*(7+49+49+6+36+36+7+49+49+6):length_total*(7+49+49+6+36+36+7+49+49+6+36)] = Kps
data[0][length_total*(7+49+49+6+36+36+7+49+49+6+36):length_total*(7+49+49+6+36+36+7+49+49+6+36+36)] = Kvs
data[0][length_total*(7+49+49+6+36+36+7+49+49+6+36+36):length_total*(7+49+49+6+36+36+7+49+49+6+36+36+1)] = gripper
data[0][-2] = 0
data[0][-1] = stamps[i_end] - stamps[i_start]
msg = Float64MultiArray()
msg.data = data[0].tolist()
pub = rospy.Publisher("/controller_data", Float64MultiArray)
redprint("Press enter to start trajectory")
raw_input()
time.sleep(1)
pub.publish(msg)
time.sleep(1)
#if not success: break
print("Segment %s result: %s"%(seg_name, success))
print("exit loop")
for i in range(100):
time.sleep(1)
except KeyboardInterrupt:
redprint("=================================DONE================================")
raw_input()
raw_input()
raw_input()
fht.write('\nstop:' + str(rospy.get_rostime().secs))
fht.write('\ndone:' + str(rospy.get_rostime().secs))
fht.close()
time.sleep(3)
if started_bag:
print "stopping bag"
bag_handle.send_signal(signal.SIGINT)
#bag_handle.wait()
started_bag = False
if started_video:
print "stopping video"
video_handle.send_signal(signal.SIGINT)
#video_handle.wait()
started_video = False
bagfilename = demo_name+".bag"
annfilename = demo_name+".ann.yaml"
call_and_print("generate_ann.py %s %s %s"%(bagfilename, annfilename, timestampfile),check=False)
with open(args.new_demo,"a") as fh1:
fh1.write("\n"
"- bag_file: %(bagfilename)s\n"
" annotation_file: %(annfilename)s\n"
" video_dir: %(videodir)s\n"
" demo_name: %(demoname)s"%dict(bagfilename=bagfilename, annfilename=annfilename, videodir=demo_name, demoname=demo_name))
return
def main():
import IPython
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
if args.log:
LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S"))
os.mkdir(LOG_DIR)
LOG_COUNT = 0
if args.execution:
rospy.init_node("exec_task",disable_signals=True)
Globals.pr2 = PR2.PR2()
Globals.env = Globals.pr2.env
Globals.robot = Globals.pr2.robot
else:
Globals.env = openravepy.Environment()
Globals.env.StopSimulation()
Globals.env.Load("robots/pr2-beta-static.zae")
Globals.robot = Globals.env.GetRobots()[0]
if not args.eval.fake_data_segment:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
while True:
redprint("Acquire point cloud")
if args.eval.fake_data_segment:
fake_seg = demofile[args.eval.fake_data_segment]
new_xyz = np.squeeze(fake_seg["cloud_xyz"])
hmat = openravepy.matrixFromAxisAngle(args.eval.fake_data_transform[3:6])
hmat[:3,3] = args.eval.fake_data_transform[0:3]
new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:]
r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0]))
#Globals.pr2.head.set_pan_tilt(0,1.2)
#Globals.pr2.rarm.goto_posture('side')
#Globals.pr2.larm.goto_posture('side')
#Globals.pr2.join_all()
#time.sleep(2)
else:
#Globals.pr2.head.set_pan_tilt(0,1.2)
#Globals.pr2.rarm.goto_posture('side')
#Globals.pr2.larm.goto_posture('side')
#Globals.pr2.join_all()
#time.sleep(2)
Globals.pr2.update_rave()
rgb, depth = grabber.getRGBD()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_xyz = cloud_proc_func(rgb, depth, T_w_k)
print "got new xyz"
redprint(new_xyz)
#grab_end(new_xyz)
if args.log:
LOG_COUNT += 1
import cv2
cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb)
cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth)
np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz)
################################
redprint("Finding closest demonstration")
if args.select_manual:
seg_name = find_closest_manual(demofile, new_xyz)
else:
seg_name = find_closest_auto(demofile, new_xyz)
seg_info = demofile[seg_name]
redprint("closest demo: %s"%(seg_name))
if "done" in seg_name:
redprint("DONE!")
break
if args.log:
with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name)
################################
### Old end effector forces at r_gripper_tool_frame (including torques)
old_forces = seg_info['end_effector_forces'][:,0:3,:]
old_torques = seg_info['end_effector_forces'][:,3:6,:]
redprint("Generating end-effector trajectory")
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb,
plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1)
f = registration.unscale_tps(f, src_params, targ_params)
old_xyz_transformed = f.transform_points(old_xyz)
#handles.append(Globals.env.plot3(old_xyz_transformed,5, np.array([(0,0,1,1) for i in old_xyz_transformed])))
handles.extend(plotting_openrave.draw_grid(Globals.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))
link2eetraj = {}
for lr in 'r':
link_name = "%s_gripper_tool_frame"%lr
old_ee_traj = asarray(seg_info[link_name]["hmat"])
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
#print old_ee_traj
old_ee_pos = old_ee_traj[:,0:3,3]
#print old_ee_pos
# End effector forces as oppossed to end effector trajectories
dfdxs = f.compute_jacobian(old_ee_pos)
old_eefs = []
new_eefs = []
for i in xrange(len(old_forces)):
dfdx = np.matrix(dfdxs[i])
old_force = np.matrix(old_forces[i])
old_torque = np.matrix(old_torques[i])
new_force = (dfdx * old_force)
new_torque = (dfdx * old_torque)
old_eefs.append(np.vstack((old_force, old_torque)))
new_eefs.append(np.vstack((new_force, new_torque)))
old_eefs = np.asarray(old_eefs)[:,:,0]
new_eefs = np.asarray(new_eefs)[:,:,0]
force_data = {}
force_data['old_eefs'] = old_eefs
force_data['new_eefs'] = new_eefs
force_file = open("trial.pickle", 'wa')
pickle.dump(force_data, force_file)
force_file.close()
new_ee_traj_x = new_ee_traj
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
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 args.execution=="real": Globals.pr2.update_rave()
################################
redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg))
# figure out how we're gonna resample stuff
lr2oldtraj = {}
for lr in 'r':
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 arm_moved(old_joint_traj): NOT SURE WHY BUT THIS IS RETURNING FALSE
lr2oldtraj[lr] = old_joint_traj
if args.visualize:
r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0]))
for i in range(0, lr2oldtraj['r'].shape[0], 10):
handles = []
handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz])))
handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz])))
handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1)))
# Setting robot arm to joint trajectory
r_vals = lr2oldtraj['r'][i,:]
Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices())
# Plotting forces from r_gripper_tool_frame
hmats = Globals.robot.GetLinkTransformations()
trans, rot = conv.hmat_to_trans_rot(hmats[-3])
f_start = np.array([0,0,0]) + trans
#IPython.embed()
f_end = np.array(old_forces[i].T)[0]/100 + trans
handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1)))
redprint(i)
Globals.viewer.Step()
if len(lr2oldtraj) > 0:
old_total_traj = np.concatenate(lr2oldtraj.values(), 1)
JOINT_LENGTH_PER_STEP = .1
# FIRST RESAMPLING HAPPENS HERE: JOINT_LENGTH
_, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) # Timesteps only, can use to inter eefs for first time
####
new_eefs_segment = asarray(new_eefs[i_start:i_end+1,:]) # Extract miniseg, and re-sample
if args.no_ds:
new_eefs_segment_rs = new_eefs_segment
else:
new_eefs_segment_rs = mu.interp2d(timesteps_rs, np.arange(len(new_eefs_segment)), new_eefs_segment)
### Generate fullbody traj
bodypart2traj = {}
for (lr,old_joint_traj) in lr2oldtraj.items():
manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
ee_link_name = "%s_gripper_tool_frame"%lr
new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1]
if args.no_ds:
old_joint_traj_rs = old_joint_traj
new_ee_traj_rs = new_ee_traj
ds_inds = np.arange(0, len(new_ee_traj_rs), args.trajopt_ds)
new_ee_traj_rs = new_ee_traj_rs[ds_inds]
old_joint_traj_rs = old_joint_traj_rs[ds_inds]
new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
new_joint_traj = mu.interp2d(np.arange(len(old_joint_traj)), np.arange(0, len(new_joint_traj) * args.trajopt_ds, args.trajopt_ds), new_joint_traj)
else:
old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj)
new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,old_joint_traj_rs)
if args.execution: Globals.pr2.update_rave()
part_name = {"l":"larm", "r":"rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
redprint("Joint trajectory has length: " + str(len(bodypart2traj[part_name])))
redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys()))
#for lr in 'r':
# set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start]))
if args.pid:
# Add trajectory to arrive at to initial state
redprint("Press enter to use current position")
raw_input()
(arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.r_arm_joint_names)
diff = np.array(arm_position - bodypart2traj['rarm'][0,:])
maximum = max(abs(diff))
speed = (300.0/360.0*2*(np.pi))
time_needed = maximum / speed
initial_pos_traj = mu.interp2d(np.arange(0, time_needed, 0.01), np.array([0,time_needed]), np.array([arm_position, bodypart2traj['rarm'][0,:]]))
# length of the intial trajectory, use this length for padding PD gains
initial_traj_length = initial_pos_traj.shape[0]
initial_force_traj = np.array([np.zeros(6) for i in range(initial_traj_length)])
temptraj = bodypart2traj['rarm']
bodypart2traj['rarm'] = np.concatenate((initial_pos_traj, temptraj), axis=0)
complete_force_traj = np.concatenate((initial_force_traj, new_eefs), axis=0)
# SECOND RESAMPLING HAPPENS HERE: JOINT VELOCITIES
if args.no_ds:
traj = bodypart2traj['rarm']
stamps = asarray(seg_info['stamps'])
times = np.array([stamps[i_end] - stamps[i_start]])
F = complete_force_traj
else:
times, times_up, traj = pr2_trajectories.return_timed_traj(Globals.pr2, bodypart2traj) # use times and times_up to interpolate the second time
F = mu.interp2d(times_up, times, complete_force_traj)
#Globals.robot.SetDOFValues(asarray(fake_seg["joint_states"]["position"][0]))
if args.visualize:
r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"]))
r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0]))
for i in range(0, traj.shape[0], 10):
handles = []
handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz])))
handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz])))
handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj[:,:3,3], 2, (0,1,0,1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj_rs[:,:3,3], 2, (0,0,1,1)))
# Setting robot arm to joint trajectory
r_vals = traj[i,:]
Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices())
# Plotting forces from r_gripper_tool_frame
hmats = Globals.robot.GetLinkTransformations()
trans, rot = conv.hmat_to_trans_rot(hmats[-3])
f_start = np.array([0,0,0]) + trans
f_end = np.array(old_forces[i].T)[0]/100 + trans
handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1)))
redprint(i)
Globals.viewer.Idle()
# Controller code in joint space
traj_length = traj.shape[0]
pgains = np.asarray([2400.0, 1200.0, 1000.0, 700.0, 300.0, 300.0, 300.0])
dgains = np.asarray([18.0, 10.0, 6.0, 4.0, 6.0, 4.0, 4.0])
m = np.array([3.33, 1.16, 0.1, 0.25, 0.133, 0.0727, 0.0727]) # masses in joint space (feed forward)
vel_factor = 1e-3
#new costs
covariances, means = analyze_data.run_analyze_data(args)
CostsNew = []
counterCovs = 0
endTraj = False
covThiss = []
for covMat in covariances:
covThis = np.zeros((18,18))
covThis[0:6,0:6] = covMat[0:6,0:6]
covThis[12:18,12:18] = covMat[6:12, 6:12]
covThis[0:6, 12:18] = covMat[0:6, 6:12]
covThis[12:18, 0:6] = covMat[6:12, 0:6]
covThis[6:12, 6:12] = np.eye(6)*vel_factor
covThis = np.diag(np.diag(covThis))
covThiss.append(covThis)
#if len(covThiss) <= 69 and len(covThiss) >= 47:
# covThis[12:18,12:18] = np.diag([0.13, 0.06, 0.07, 0.005, 0.01, 0.004])
for j in range(args.eval.downsample_traj):
invCov = np.linalg.inv(covThis)
CostsNew.append(invCov)
counterCovs = counterCovs + 1
if counterCovs >= traj_length:
endTraj = True
break
if endTraj:
break
allCs = []
x = np.ones(6) * 1
v = np.ones(6) * 1e-3
a = np.ones(6) * 1e-6
Ct = np.diag(np.hstack((x, v, a))) # in end effector space
Ms = []
num_steps = i_end - i_start + 1
stamps = asarray(seg_info['stamps'][i_start:i_end+1])
arm = Globals.robot.GetManipulator("rightarm")
jacobians = []
for i in range(traj.shape[0]):
r_vals = traj[i,:]
Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices())
jacobians.append(np.vstack((arm.CalculateJacobian(), arm.CalculateAngularVelocityJacobian())))
jacobians = asarray(jacobians)
#jacobiansx = asarray(seg_info["jacobians"][i_start:i_end+1])
for t in range(num_steps):
M = np.zeros((18, 21))
J = jacobians[t]
M[0:6,0:7] = J
M[6:12,7:14] = J
mdiag = np.diag(m) # Convert joint feedforward values into diagonal array
mdiag_inv = np.linalg.inv(mdiag)
M[12:18,14:21] = np.linalg.inv((J.dot(mdiag_inv).dot(np.transpose(J)))).dot(J).dot(mdiag_inv)
Ms.append(M)
for t in range(num_steps):
topad = np.zeros((21,21))
topad[0:7,0:7] = np.diag(pgains) * 0.1
topad[7:14,7:14] = np.diag(dgains) * 0.1
topad[14:21,14:21] = np.eye(7) * 0.1
#allCs.append(np.transpose(Ms[t]).dot(Ct).dot(Ms[t]) + topad)
allCs.append(np.transpose(Ms[t]).dot(CostsNew[t]).dot(Ms[t]) + topad)
Kps = []
Kvs = []
Ks = []
Qt = None
Vs = []
for t in range(num_steps-1, -1, -1):
if Qt is None:
Qt = allCs[t]
else:
Ft = np.zeros((14, 21))
for j in range(14):
Ft[j][j] = 1.0
deltat = abs(stamps[t+1] - stamps[t])
#print(deltat)
for j in range(7):
Ft[j][j+7] = deltat
for j in range(7):
Ft[j+7][j+14] = deltat/m[j]
for j in range(7):
Ft[j][j+14] = ((deltat)**2)/m[j]
Qt = allCs[t] + (np.transpose(Ft).dot(Vs[num_steps-2-t]).dot(Ft))
Qxx = Qt[0:14, 0:14]
Quu = Qt[14:21, 14:21]
Qxu = Qt[0:14, 14:21]
Qux = Qt[14:21, 0:14]
Quuinv = np.linalg.inv(Quu)
Vt = Qxx - Qxu.dot(Quuinv).dot(Qux)
Vt = 0.5*(Vt + np.transpose(Vt))
Kt = -1*(Quuinv.dot(Qux))
Ks.append(Kt)
Kps.append(Kt[:, 0:7])
Kvs.append(Kt[:, 7:14])
Vs.append(Vt)
Ks = Ks[::-1]
Kps = Kps[::-1]
Kvs = Kvs[::-1]
JKpJ = np.asarray(Kps)
JKvJ = np.asarray(Kvs)
total_length = num_steps + initial_traj_length
# Pad initial traj with PD gains
pgainsdiag = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0]))
dgainsdiag = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0]))
addkp = np.asarray([pgainsdiag for i in range(initial_traj_length)])
addkv = np.asarray([dgainsdiag for i in range(initial_traj_length)])
JKpJ = np.concatenate((addkp, JKpJ), axis=0)
JKvJ = np.concatenate((addkv, JKvJ), axis=0)
Kps = []
Kvs = []
for i in range(num_steps + initial_traj_length):
Kps.append(np.zeros((6,6)))
Kvs.append(np.zeros((6,6)))
Kps = np.asarray(Kps)
Kvs = np.asarray(Kvs)
# Gains as JKpJ and JKvJ for testing
if args.pdgains:
JKpJ = np.asarray([pgainsdiag for i in range(total_length)])
JKvJ = np.asarray([dgainsdiag for i in range(total_length)])
qs = traj
IPython.embed()
Kps = np.resize(Kps, (1, 36 * total_length))[0]
Kvs = np.resize(Kvs, (1, 36 * total_length))[0]
JKvJ = np.resize(JKvJ, (1, 49*total_length))[0]
JKpJ = np.resize(JKpJ, (1, 49*total_length))[0]
# For use with new controller
qs = np.resize(qs, (1, qs.shape[0]*7))[0] #resize the data to 1x7n (n being the number of steps)
F = np.resize(F, (1, F.shape[0]*6))[0] #resize the data to 1x6n
# [traj, Kp, Kv, F, use_force, seconds]
data = np.zeros((1, len(qs) + len(JKpJ) + len(JKvJ) + len(F) + len(Kps) + len(Kvs) + 2))
data[0][0:len(qs)] = qs
data[0][len(qs):len(qs)+len(JKpJ)] = JKpJ
data[0][len(qs)+len(JKpJ):len(qs)+len(JKpJ)+len(JKvJ)] = JKvJ
data[0][len(qs)+len(JKpJ)+len(JKvJ):len(qs)+len(JKpJ)+len(JKvJ)+len(F)] = F
data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)] = Kps
data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)+len(Kvs)] = Kvs
data[0][-2] = args.use_force
data[0][-1] = stamps[i_end] - stamps[i_start] + (initial_traj_length * 0.01)
# For use with old controller
"""
qs = np.array(np.matrix(traj).T) # 7 x n
qs = np.resize(qs, (1, qs.shape[1]*7))[0] #resize the data to 1x7n (n being the number of steps)
F = np.array(np.matrix(F).T) # 6 x n
F = F[0:3,:]
F = np.resize(F, (1, F.shape[1]*3))[0] #resize the data to 1x3n
data = np.zeros((1, len(qs) + len(F) + 2))
data[0][0:len(qs)] = qs
data[0][len(qs):len(qs)+len(F)] = F
data[0][-1] = args.use_force
data[0][-2] = stamps[i_end] - stamps[i_start]
"""
msg = Float64MultiArray()
msg.data = data[0].tolist()
pub = rospy.Publisher("/controller_data", Float64MultiArray)
redprint("Press enter to start trajectory")
raw_input()
time.sleep(1)
pub.publish(msg)
time.sleep(1)
else:
#if not success: break
if len(bodypart2traj) > 0:
exec_traj_maybesim(bodypart2traj)
def loop_body(demofile, choose_segment, knot, animate, task_params, curr_step=None):
"""Do the body of the main task execution loop (ie. do a segment).
Arguments:
curr_step is 0 indexed
choose_segment is a function that returns the key in the demofile to the segment
knot is the knot the rope is checked against
new_xyz is the new pointcloud
task_params is used for the only_original_segments argument
return None or {'found_knot': found_knot, 'segment': segment, 'link2eetraj': link2eetraj, 'new_xyz': new_xyz}
"""
#TODO -- Return the new trajectory and state info to be used for bootstrapping (knot_success, new_xyz, link2eetraj,
#TODO segment)
#TODO -- End condition
#TODO -- max_segments logic
redprint("Acquire point cloud")
move_sim_arms_to_side()
#TODO -- Possibly refactor this section to be before the loop
new_xyz = Globals.sim.observe_cloud()
new_xyz_upsampled = Globals.sim.observe_cloud(upsample=120)
print "loop_body only_original_segments", task_params.only_original_segments
segment = choose_segment(demofile, new_xyz, task_params.only_original_segments)
if segment is None:
return None
seg_info = demofile[segment]
handles = []
old_xyz = np.squeeze(seg_info["cloud_xyz"])
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1)))
#TODO: test that old_xyz is now bigger if from a derived segment
#old_xyz = clouds.downsample(old_xyz, DS_SIZE)
if not "derived" in seg_info.keys():
old_xyz = downsample(old_xyz, DS_SIZE)
print "derived, so downsamping"
#new_xyz = clouds.downsample(new_xyz, DS_SIZE)
#new_xyz = downsample_if_big(new_xyz, DS_SIZE)
handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0)))
print "new_xyz cloud size", new_xyz.shape
print "old_xyz cloud size", old_xyz.shape
scaled_old_xyz, src_params = registration.unit_boxify(old_xyz)
scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz)
#TODO: get rid of g
f, g = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb=tpsrpm_plot_cb,
plotting=5 if animate else 0, rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=old_xyz, new_xyz=new_xyz)
f = registration.unscale_tps(f, src_params, targ_params)
g = registration.unscale_tps(g, src_params, targ_params)
#Globals.exec_log(curr_step, "gen_traj.f", f)
#handles.extend(plotting_openrave.draw_grid(Globals.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.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0) - np.r_[0, 0, .1],
old_xyz.max(axis=0) + np.r_[0, 0, .02], xres=.01, yres=.01, zres=.04))
#handles.extend(plotting_openrave.draw_grid(Globals.env, g.transform_points, old_xyz.min(axis=0) - np.r_[0, 0, .1],
# old_xyz.max(axis=0) + np.r_[0, 0, .02], xres=.01, yres=.01, zres=.04))
link2eetraj = {}
#link2eetraj is a hash of gripper fram to new trajectory
#Transform the gripper trajectory here
for lr in 'lr':
link_name = "%s_gripper_tool_frame" % lr
#old_ee_traj is the old gripper trajectory
old_ee_traj = asarray(seg_info[link_name]["hmat"])
#new_ee_traj is the transformed gripper trajectory
new_ee_traj = f.transform_hmats(old_ee_traj)
link2eetraj[link_name] = new_ee_traj
#Draw the old and new gripper trajectories as lines
handles.append(Globals.env.drawlinestrip(old_ee_traj[:, :3, 3], 2, (1, 0, 0, 1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2, (0, 1, 0, 1)))
#Globals.exec_log(curr_step, "gen_traj.link2eetraj", link2eetraj)
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
#print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends
#TODO: modify for no body
for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
################################
redprint("Generating joint trajectory for segment %s, part %i" % (segment, i_miniseg))
# figure out how we're gonna resample stuff
lr2oldtraj = {}
for lr in 'lr':
manip_name = {"l": "leftarm", "r": "rightarm"}[lr]
#old_joint_traj is the old trajectory inside the minisegment
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 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 = 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 = link2eetraj[ee_link_name][i_start: i_end + 1]
new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj)
#Call the planner (eg. trajopt)
with dhm_u.suppress_stdout():
new_joint_traj = planning.plan_follow_traj(Globals.robot, manip_name,
Globals.robot.GetLink(ee_link_name), new_ee_traj_rs,
old_joint_traj_rs)
part_name = {"l": "larm", "r": "rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
### Execute the gripper ###
redprint("Executing joint trajectory for segment %s, part %i using arms '%s'" % (
segment, i_miniseg, bodypart2traj.keys()))
for lr in 'lr':
gripper_open = binarize_gripper(seg_info["%s_gripper_joint" % lr][i_start])
prev_gripper_open = binarize_gripper(
seg_info["%s_gripper_joint" % lr][i_start - 1]) if i_start != 0 else False
if not set_gripper_sim(lr, gripper_open, prev_gripper_open, animate):
redprint("Grab %s failed" % lr)
success = False
if not success:
break
# Execute the robot trajectory
if len(bodypart2traj) > 0:
success &= exec_traj_sim(bodypart2traj, animate)
#Globals.exec_log(curr_step, "execute_traj.miniseg_%d.sim_rope_nodes_after_traj" % i_miniseg, Globals.sim.rope.GetNodes())
if not success:
break
Globals.sim.settle(animate=animate)
if Globals.exec_log:
Globals.exec_log(curr_step, "execute_traj.sim_rope_nodes_after_full_traj", Globals.sim.rope.GetNodes())
from rapprentice import knot_identification
knot_name = knot_identification.identify_knot(Globals.sim.rope.GetControlPoints())
found_knot = False
if knot_name is not None:
if knot_name == knot or knot == "any":
redprint("Identified knot: %s. Success!" % knot_name)
#Globals.exec_log(curr_step, "result", True, description="identified knot %s" % knot_name)
found_knot = True
else:
redprint("Identified knot: %s, but expected %s. Continuing." % (knot_name, knot))
else:
redprint("Not a knot. Continuing.")
redprint("Segment %s result: %s" % (segment, success))
return {'found_knot': found_knot, 'segment': segment, 'link2eetraj': link2eetraj, 'new_xyz': new_xyz_upsampled}
def get_warped_trajectory(seg_info, new_xyz, demofile, warp_root=False, plot=False, no_cmat=False):
"""
@seg_info : segment information from the h5 file for the segment with least tps fit cost.
@new_xyz : point cloud of the rope in the test situation.
@warp_root : warp the root trajectory if True else warp the chosen segment's trajectory.
@plot : plot the warping as we run TPS-RPM
@no_cmat : if True, does not use the correspondence matrix to the root to initialize TPS-RPM
@returns : the warped trajectory for l/r grippers and the mini-segment information.
"""
handles = []
seg_xyz = seg_info["cloud_xyz"][:]
scaled_seg_xyz, seg_params = registration.unit_boxify(seg_xyz)
scaled_new_xyz, new_params = registration.unit_boxify(new_xyz)
if plot:
handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1)))
handles.append(Globals.env.plot3(seg_xyz, 5, (1, 0, 0)))
root_seg_name = seg_info['root_seg']
root_segment = demofile[root_seg_name.value]
root_xyz = root_segment['cloud_xyz'][:]
seg_root_cmat = seg_info['cmat'][:]
if warp_root:
## transfer the trajectory from the root demonstration this demonstration is derived from
scaled_root_xyz, root_params = registration.unit_boxify(root_xyz)
if no_cmat:
## find the correspondences from the root again
## only use the bootstrapping for action selection
print "not using cmat for correspondences"
f_root2new, _, corr_new2root = registration.tps_rpm_bij(scaled_root_xyz, scaled_new_xyz,
plotting=5 if plot else 0, plot_cb=tpsrpm_plot_cb,
rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=root_xyz, new_xyz=new_xyz,
return_corr=True)
else:
## use the information from the bootstrapping example to initialize correspondences
## uses bootstrapping to select demonstration to transfer and uses information from that transfer to improve warp
f_root2new, _, corr_new2root = registration.tps_rpm_bootstrap(scaled_root_xyz, scaled_seg_xyz, scaled_new_xyz, seg_root_cmat,
plotting=5 if plot else 0, plot_cb=tpsrpm_plot_cb,
rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=root_xyz, new_xyz=new_xyz)
f_warping = registration.unscale_tps(f_root2new, root_params, new_params)
old_ee_traj = root_segment['hmats']
rgrip_joints = root_segment['r_gripper_joint'][:]
lgrip_joints = root_segment['l_gripper_joint'][:]
cmat = corr_new2root
else:
## warp to the chosen segment, use an iterated TPS to transfer the root trajectory most use of bootstrapping
f_seg2new, _, corr_new2seg = registration.tps_rpm_bij(scaled_seg_xyz, scaled_new_xyz, plot_cb=tpsrpm_plot_cb,
plotting=5 if plot else 0, rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=seg_xyz, new_xyz=new_xyz,
return_corr=True)
f_warping = registration.unscale_tps(f_seg2new, seg_params, new_params)
old_ee_traj = seg_info['hmats']
rgrip_joints = root_segment['r_gripper_joint'][:]
lgrip_joints = root_segment['l_gripper_joint'][:]
cmat = seg_root_cmat.dot(corr_new2seg)
if compare_bootstrap_correspondences:
## used to compare b/t warping root and warping derived demonstrations
scaled_root_xyz, root_params = registration.unit_boxify(root_xyz)
f_root2new, _, corr_new2root = registration.tps_rpm_bootstrap(scaled_root_xyz, scaled_seg_xyz, scaled_new_xyz, seg_root_cmat,
plotting=5 if plot else 0, plot_cb=tpsrpm_plot_cb,
rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50,
reg_init=10, reg_final=.01, old_xyz=root_xyz, new_xyz=new_xyz)
root_warping = registration.unscale_tps(f_root2new, root_params, new_params)
root_ee_traj = root_segment['hmats']
diff = 0
for lr in 'lr':
no_root_ee_traj = f_warping.transform_hmats(old_ee_traj[lr][:])
warped_root_ee_traj = root_warping.transform_hmats(root_ee_traj[lr][:])
diff += np.linalg.norm(no_root_ee_traj - warped_root_ee_traj)
handles.append(Globals.env.drawlinestrip(old_ee_traj[lr][:, :3, 3], 2, (1, 0, 0, 1)))
handles.append(Globals.env.drawlinestrip(no_root_ee_traj[:, :3, 3], 2, (0, 1, 0, 1)))
handles.append(Globals.env.drawlinestrip(warped_root_ee_traj[:, :3, 3], 2, (0, 1, 0, 1)))
if plot:
print 'traj norm differences:\t', diff
Globals.viewer.Idle()
if plot:
handles.extend(plotting_openrave.draw_grid(Globals.env, f_warping.transform_points, new_xyz.min(axis=0) - np.r_[0, 0, .1],
new_xyz.max(axis=0) + np.r_[0, 0, .02], xres=.01, yres=.01, zres=.04))
warped_ee_traj = {}
#Transform the gripper trajectory here
for lr in 'lr':
new_ee_traj = f_warping.transform_hmats(old_ee_traj[lr][:])
warped_ee_traj[lr] = new_ee_traj
if plot:
handles.append(Globals.env.drawlinestrip(old_ee_traj[lr][:, :3, 3], 2, (1, 0, 0, 1)))
handles.append(Globals.env.drawlinestrip(new_ee_traj[:, :3, 3], 2, (0, 1, 0, 1)))
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(rgrip_joints, lgrip_joints)
return (cmat, warped_ee_traj, miniseg_starts, miniseg_ends, {'r':rgrip_joints, 'l':lgrip_joints})
for (key, seg) in hdf.items()
]
if __name__ == "__main__":
hdf = h5py.File("/Users/joschu/Data/knots/master.h5", "r")
clouds = extract_clouds(hdf)
dim = 2
for cloud0 in clouds:
for cloud1 in clouds:
if dim == 2:
cloud0 = cloud0[:, :2]
cloud1 = cloud1[:, :2]
scaled_cloud0, src_params = reg.unit_boxify(cloud0)
scaled_cloud1, targ_params = reg.unit_boxify(cloud1)
print "downsampled to %i and %i pts" % (len(scaled_cloud0),
len(scaled_cloud1))
tstart = time()
fest_scaled, gest_scaled = reg.tps_rpm_bij(scaled_cloud0,
scaled_cloud1,
n_iter=10,
reg_init=10,
reg_final=.01)
print "%.3f elapsed" % (time() - tstart)
cost = reg.tps_reg_cost(fest_scaled) + reg.tps_reg_cost(
gest_scaled)