def create_annotations(stamps, meanings, bagfile, video_dir):
# initialize with basic information of start, stop, look times and description
seg_infos = bp.joy_to_annotations(stamps, meanings)
frame_stamps = [t["look"] for t in seg_infos]
print frame_stamps
key_rgb_imgs, key_depth_imgs = bp.get_video_frames(video_dir, frame_stamps)
bag = rosbag.Bag(bagfile)
jnames, jstamps, traj = bp.extract_joints(bag)
key_joint_inds = np.searchsorted(jstamps, frame_stamps)
key_joints = [traj[i] for i in key_joint_inds]
for i,t in enumerate(frame_stamps):
print 'Looking for key-points in segment %i.'%i
keypoint_info = {}
Twk = svi.find_Twk(key_joints[i], jnames)
while True:
svi.show_pointclouds([key_rgb_imgs[i]])
kp = raw_input('Which key points are important for this segment?\nChoices are: ' + str(svi.KEYPOINTS) + '.\n Please only enter one key point at a time: ')
kp = kp.strip()
if kp not in svi.KEYPOINTS:
print 'Invalid keypoint: %s'%kp
elif svi.KEYPOINTS_FULL[kp] in keypoint_info.keys():
if yes_or_no('Already have information for %s. Overwrite?'%kp):
kp_loc = svi.find_kp_processing(kp, frame_stamps[i], Twk, video_dir)
if kp_loc is None:
print 'Something went wrong with keypoint selection.'
else:
keypoint_info[svi.KEYPOINTS_FULL[kp]] = kp_loc
print 'Key point %s saved for this segment.'%kp
else:
kp_loc = svi.find_kp_processing(kp, frame_stamps[i], Twk, video_dir)
if kp_loc is None:
print 'Something went wrong with keypoint selection.'
else:
keypoint_info[svi.KEYPOINTS_FULL[kp]] = kp_loc
print 'Key point %s saved for this segment.'%kp
if not yes_or_no('Enter another key point for this segment?'):
break
seg_infos[i]['key_points'] = keypoint_info
seg_infos[i]['ar_marker_poses'] = get_ar_marker_poses(key_rgb_imgs[i], key_depth_imgs[i], Twk)
seg_infos[i]['extra_information'] = []
if yes_or_no('Is the needle-tip the relevant end effector for this segment?'):
if yes_or_no('Is the needle in the left gripper?'):
seg_infos[i]['extra_information'].append("l_grab")
else:
seg_infos[i]['extra_information'].append("r_grab")
if not seg_infos[i]["extra_information"]:
seg_infos[i]["extra_information"].append("nothing")
return seg_infos
def store_in_yaml_kp (name, kpl):
import yaml, rospy
rospy.init_node("store_points", disable_signals = True)
#subprocess.call("killall XnSensorServer", shell=True)
pr2 = PR2.PR2()
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
point_sets = {}
d = 0
while True:
print "Demo %i"%(d+1)
#xyz_tf, rgb = svi.get_kp_clouds(grabber, 1, T_w_k)
while True:
key_points = {}
T_w_k = berkeley_pr2.get_kinect_transform(pr2.robot)
for kp in kpl:
key_points[svi.KEYPOINTS_FULL[kp]] = svi.find_kp_execution(kp, grabber, T_w_k)
if yes_or_no.yes_or_no("Satisfied?"):
break
point_sets[d] = fk.key_points_to_points(key_points, True)
if not yes_or_no.yes_or_no("Do you want to save another situation?"):
break
d += 1
print point_sets
with open(name, 'w') as fl:
yaml.dump(point_sets, fl)
def exec_traj_maybesim(bodypart2traj):
if args.animation:
dof_inds = []
trajs = []
for (part_name, traj) in bodypart2traj.items():
manip_name = {"larm":"leftarm","rarm":"rightarm"}[part_name]
dof_inds.extend(Globals.robot.GetManipulator(manip_name).GetArmIndices())
trajs.append(traj)
full_traj = np.concatenate(trajs, axis=1)
Globals.robot.SetActiveDOFs(dof_inds)
animate_traj.animate_traj(full_traj, Globals.robot, restore=False,pause=True)
if args.execution:
if not args.prompt or yes_or_no("execute?"):
pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj)
else:
return False
return True
def store_normals_in_yaml (name):
import yaml, rospy
#subprocess.call("killall XnSensorServer", shell=True)
rospy.init_node("store_normals", disable_signals = True)
pr2 = PR2.PR2()
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
point_sets = {}
d = 0
dist = 0.1
while True:
print "Demo %i"%(d+1)
point_sets[d] = []
T_w_k = berkeley_pr2.get_kinect_transform(pr2.robot)
for i in [1,2]:
print "Save hole normal %i:"%i
kp_loc = svi.find_kp_execution('hn%i'%i, grabber, T_w_k)
p = kp_loc[0]
n = kp_loc[1]
point_sets[d].append([p[0], p[1], p[2]])
point_sets[d].append([p[0]+dist*n[0]/2, p[1]+dist*n[1]/2, p[2]+dist*n[2]/2])
point_sets[d].append([p[0]+dist*n[0], p[1]+dist*n[1], p[2]+dist*n[2]])
if not yes_or_no.yes_or_no("Do you want to save another situation?"):
break
d += 1
print point_sets
with open(name, 'w') as fl:
yaml.dump(point_sets, fl)
def exec_traj_maybesim(bodypart2traj):
if args.animation:
dof_inds = []
trajs = []
for (part_name, traj) in bodypart2traj.items():
manip_name = {"larm": "leftarm", "rarm": "rightarm"}[part_name]
dof_inds.extend(
Globals.robot.GetManipulator(manip_name).GetArmIndices())
trajs.append(traj)
full_traj = np.concatenate(trajs, axis=1)
Globals.robot.SetActiveDOFs(dof_inds)
animate_traj.animate_traj(full_traj,
Globals.robot,
restore=False,
pause=True)
if args.execution:
if not args.prompt or yes_or_no("execute?"):
pr2_trajectories.follow_body_traj(Globals.pr2, bodypart2traj)
else:
return False
return True
def store_in_yaml (name):
import yaml, rospy
rospy.init_node("store_points", disable_signals = True)
#subprocess.call("killall XnSensorServer", shell=True)
pr2 = PR2.PR2()
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
n = input("Enter the number of points you are saving: ")
point_sets = {}
d = 0
while True:
print "Demo %i"%(d+1)
point_sets[d] = []
T_w_k = berkeley_pr2.get_kinect_transform(pr2.robot)
xyz_tf, rgb = svi.get_kp_clouds(grabber, 1, T_w_k)
for _ in xrange(n):
print "Save point set (make sure you click on them in the same order every situation)."
point = svi.find_kp_single_cloud("nt", xyz_tf, rgb)
point_sets[d].append([float(point[0]), float(point[1]), float(point[2])])
if not yes_or_no.yes_or_no("Do you want to save another situation?"):
break
d += 1
print point_sets
with open(name, 'w') as fl:
yaml.dump(point_sets, fl)
started_video = True
time.sleep(9999)
except KeyboardInterrupt:
print colorize("got control-c", "green")
finally:
if started_bag:
bag_handle.send_signal(signal.SIGINT)
bag_handle.wait()
if started_video:
video_handle.send_signal(signal.SIGINT)
video_handle.wait()
bagfilename = demo_name+".bag"
if yes_or_no("save demo?"):
annfilename = demo_name+".ann.yaml"
call_and_print("generate_annotations.py %s %s"%(bagfilename, annfilename))
with open(args.master_file,"a") as fh:
fh.write("\n"
"- bag_file: %(bagfilename)s\n"
" annotation_file: %(annfilename)s\n"
" video_dir: %(videodir)s"
" demo_name: %(demoname)s"%dict(bagfilename=bagfilename, annfilename=annfilename, videodir=demo_name, demoname=demo_name))
else:
shutil.rmtree(demo_name) #video dir
os.unlink(bagfilename)
from rapprentice.call_and_print import call_and_print
from rapprentice.yes_or_no import yes_or_no
import os.path as osp
import itertools
import yaml
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.master_file))
if not osp.exists(args.master_file):
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.master_file,"w") as fh: fh.write("""
name: %s
h5path: %s
bags:
"""%(basename, basename+".h5"))
else:
print "exiting."
exit(0)
with open(args.master_file, "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"]):
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
import itertools
import yaml
if "localhost" in os.getenv("ROS_MASTER_URI"):
raise Exception("on localhost!")
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.master_file))
if not osp.exists(args.master_file):
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.master_file, "w") as fh:
fh.write("""
name: %s
h5path: %s
bags:
""" % (basename, basename + ".h5"))
else:
print "exiting."
exit(0)
with open(args.master_file, "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())):
parser = argparse.ArgumentParser()
parser.add_argument(
"path", help="script will create a sampledata directory in this location")
parser.add_argument("--use_rsync", action="store_true")
args = parser.parse_args()
import os, urllib2, shutil
from rapprentice.yes_or_no import yes_or_no
from rapprentice.call_and_print import call_and_print
if args.use_rsync:
call_and_print(
"rsync -azvu --delete [email protected]:/var/www/rapprentice/sampledata %s"
% args.path)
else:
os.chdir(args.path)
if os.path.exists("sampledata"):
yn = yes_or_no("delete old directory")
if yn: shutil.rmtree("sampledata")
else: raise IOError
os.mkdir("sampledata")
print "downloading archive file"
urlinfo = urllib2.urlopen(
"http://rll.berkeley.edu/rapprentice/sampledata.tar")
with open("sampledata.tar", "w") as fh:
fh.write(urlinfo.read())
print "unpacking file"
call_and_print("tar xvf sampledata.tar")
os.unlink("sampledata.tar")
import argparse
parser = argparse.ArgumentParser()
parser.add_argument("path", help="script will create a sampledata directory in this location")
parser.add_argument("--use_rsync", action="store_true")
args = parser.parse_args()
import os, urllib2, subprocess, shutil
import os.path as osp
import rapprentice
from rapprentice.yes_or_no import yes_or_no
from rapprentice.call_and_print import call_and_print
if args.use_rsync:
call_and_print("rsync -azvu --delete --exclude='*.tar' [email protected]:/var/www/rapprentice/sampledata %s"%args.path)
else:
os.chdir(args.path)
if os.path.exists("sampledata"):
yn = yes_or_no("delete old directory")
if yn: shutil.rmtree("sampledata")
else: raise IOError
os.mkdir("sampledata")
os.chdir("sampledata")
print "downloading zip file"
urlinfo = urllib2.urlopen("http://rll.berkeley.edu/rapprentice/sampledata/all.tar")
with open("all.tar","w") as fh:
fh.write(urlinfo.read())
print "unpacking file"
call_and_print("tar xvf all.tar")
os.unlink("all.tar")
def main():
demofile = h5py.File(args.h5file, 'r')
trajoptpy.SetInteractive(args.interactive)
rospy.init_node("exec_task",disable_signals=True)
global filename
if args.logging:
name = raw_input ('Enter identifier of this experiment (without spaces): ')
name = name + '.yaml'
import os.path as osp
filename = osp.join('/home/sibi/sandbox/rapprentice/experiments/', name)
if not osp.exists(filename):
with open(filename,'w') as fh:
fh.write("experiment: %s\ninfo: \n"%name)
#thc.start()
if args.execution:
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]
import trajoptpy.make_kinbodies as mk
#Globals.env.Load("/home/sibi/sandbox/rapprentice/objects/table.xml")
addBoxToRave(Globals.env, "table")
Globals.sponge = addBoxToRave(Globals.env, "sponge") # Not sure about this
Globals.needle_tip = mk.create_spheres(Globals.env, [(0,0,0)], radii=0.02, name="needle_tip")
Globals.demo_env = Globals.env.CloneSelf(1)
Globals.demo_env.StopSimulation()
Globals.demo_robot = Globals.demo_env.GetRobot("pr2")
Globals.demo_needle_tip = Globals.demo_env.GetKinBody("needle_tip")
if not args.fake_data_segment:
grabber = cloudprocpy.CloudGrabber()
grabber.startRGBD()
thc.grabber = grabber
#Globals.viewer = trajoptpy.GetViewer(Globals.env)
#####################
#Globals.env.SetViewer('qtcoin')
#threadClass().start()
while True:
redprint("Acquire point cloud")
################################
redprint("Finding closest demonstration")
if args.fake_data_segment:
seg_name = args.fake_data_segment
else:
seg_name = find_closest(demofile)
seg_info = demofile[seg_name]
# redprint("using demo %s, description: %s"%(seg_name, seg_info["description"]))
################################
redprint("Generating end-effector trajectory")
handles = []
if seg_info.get('ar_marker_poses') is None:
use_markers = False
else:
use_markers = True
if use_markers:
old_marker_poses_str = seg_info['ar_marker_poses']
old_marker_poses = {}
for i in old_marker_poses_str:
old_marker_poses[int(i)] = old_marker_poses_str[i]
# TODO: Have a check for only transformations -> rigid transformations
if args.fake_data_segment:
new_keypoints = demofile[args.fake_data_segment]["key_points"]
if use_markers:
new_marker_poses_str = demofile[args.fake_data_segment]["ar_marker_poses"]
new_marker_poses = {}
for i in new_marker_poses_str:
new_marker_poses[int(i)] = new_marker_poses_str[i]
if seg_info['key_points'].keys().sort() != new_keypoints.keys().sort():
print "Keypoints don't match."
exit(1)
else:
if args.execution: Globals.pr2.update_rave()
T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot)
new_keypoints, new_marker_poses = fk.get_keypoints_execution(grabber, seg_info['key_points'].keys(), T_w_k, use_markers)
print "Warping the points for the new situation."
if "l_grab" in seg_info['extra_information'] or "r_grab" in seg_info['extra_information']:
use_ntt_kp = False
else:
use_ntt_kp = True
# Points from keypoints
old_xyz, rigid, kp_mapping = fk.key_points_to_points(seg_info['key_points'], use_ntt_kp)
new_xyz, _, _ = fk.key_points_to_points(new_keypoints, use_ntt_kp)
# Points from markers
if use_markers:
old_common_poses, new_common_poses = find_common_marker_poses(old_marker_poses, new_marker_poses, new_keypoints.keys())
old_m_xyz, rigid_m, _ = fk.key_points_to_points(old_common_poses, False)
new_m_xyz, _, _ = fk.key_points_to_points(new_common_poses, False)
if len(old_m_xyz) > 0 and rigid: rigid = False
elif rigid_m and not old_xyz.any(): rigid = True
# concatenate points
if old_xyz.any() and old_m_xyz.any():
old_xyz = np.r_[old_xyz, old_m_xyz]
elif old_m_xyz.any():
old_xyz = old_m_xyz
if new_xyz.any() and new_m_xyz.any():
new_xyz = np.r_[new_xyz, new_m_xyz]
elif new_m_xyz.any():
new_xyz = new_m_xyz
if new_xyz.any() and new_xyz.shape != (4,4):
#handles.append(Globals.env.plot3(old_xyz,5, (1,0,0,1)))
#handles.append(Globals.env.plot3(old_xyz,5, np.array([(1,0,0) for _ in xrange(old_xyz.shape[0])])))
handles.append(Globals.env.plot3(new_xyz,5, np.array([(0,0,1) for _ in xrange(old_xyz.shape[0])])))
print 'Old points:', old_xyz
print 'New points:', new_xyz
#if not yes_or_no.yes_or_no("Use identity?"):
if rigid:
f = registration.ThinPlateSpline()
rel_tfm = new_xyz.dot(np.linalg.inv(old_xyz))
f.init_rigid_tfm(rel_tfm)
bend_c = 0
rot_c = 0
scale_c = 0
max_error = None
elif len(new_xyz) > 0:
#f.fit(demopoints_m3, newpoints_m3, 10,10)
# TODO - check if this regularization on bending is correct
#if "right_hole_normal" in new_keypoints or "left_hole_normal" in new_keypoints:
# bend_c = 0.1
# rot_c = [1e-5,1e-5,0.1]
#wt = 5
#wt_n = np.ones(len(old_xyz))
#if kp_mapping.get("right_hole_normal"):
# wt_n[kp_mapping["right_hole_normal"][0]] = wt
#if kp_mapping.get("left_hole_normal"):
# wt_n[kp_mapping["left_hole_normal"][0]] = wt
#else:
# bend_c = 0.1
# rot_c = 1e-5#[0,0,1e-5]
# wt_n = None
# f = registration.fit_ThinPlateSpline(old_xyz, new_xyz, bend_coef = bend_c,rot_coef = rot_c)
bend_c = 0.05
rot_c = [1e-3, 1e-3, 1e-3]
scale_c = 0.1
f = registration.fit_ThinPlateSpline_RotReg(old_xyz, new_xyz, bend_c, rot_c, scale_c)
np.set_printoptions(precision=3)
max_error = np.max(np.abs(f.transform_points(old_xyz) - new_xyz))
print "nonlinear part", f.w_ng
print "affine part", f.lin_ag
print "translation part", f.trans_g
print "residual", f.transform_points(old_xyz) - new_xyz
print "max error ", max_error
else:
f = registration.ThinPlateSpline()
bend_c = 0
rot_c = 0
scale_c = 0
max_error = None
# else:
# f = registration.ThinPlateSpline()
# bend_c = 0
# rot_c = 0
#
if old_xyz.any() and old_xyz.shape != (4,4):
tfm_xyz = f.transform_points(old_xyz)
handles.append(Globals.env.plot3(tfm_xyz,5, np.array([(0,1,0) for _ in xrange(tfm_xyz.shape[0])])))
#f = registration.ThinPlateSpline() XXX XXX
if new_xyz.any() and new_xyz.shape != (4,4):
handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0), old_xyz.max(axis=0), xres = .1, yres = .1, zres = .04))
if args.ask:
# if new_xyz.any() and new_xyz.shape != (4,4):
# import visualize
# visualize.plot_mesh_points(f.transform_points, old_xyz, new_xyz)
#lines = plotting_openrave.gen_grid(f.transform_points, np.array([0,-1,0]), np.array([1,1,1]))
#plotting_openrave.plot_lines(lines)
if not yes_or_no.yes_or_no("Continue?"):
continue
miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info)
success = True
print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends
# Assuming only lgrab or rgrab
use_needle = None
for lr in 'lr':
if "%s_grab"%lr in seg_info['extra_information']:
if "needle_tip_transform" in seg_info['key_points']:
demo_ntfm = np.array(seg_info['key_points']['needle_tip_transform'])
ntfm = np.array(new_keypoints['needle_tip_transform'])
use_needle = lr
elif Globals.rel_ntfm is not None:
T_w_g = Globals.robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
T_g_n = Globals.rel_ntfm
ntfm = T_w_g.dot(T_g_n)
T_w_g_demo = Globals.demo_robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
T_g_n_demo = Globals.demo_rel_ntfm
demo_ntfm = T_w_g_demo.dot(T_g_n_demo)
use_needle = lr
if use_needle is not None:
setup_needle_grabs(use_needle, seg_info["joint_states"]["name"], seg_info["joint_states"]["look_position"], demo_ntfm, ntfm)
if args.logging:
with open(filename,'a') as fh:
fh.write("- seg_name: %s\n"%seg_name)
fh.write(" tps_info: \n")
if max_error is not None:
res_cost, bend_cost, tot_cost = f.fitting_cost(new_xyz, bend_c)
fh.write(" res_cost: %f\n bend_cost: %f\n tot_cost: %f\n"%(res_cost, bend_cost, tot_cost))
fh.write(" max_tps_error: %f\n"%max_error)
else:
fh.write(" res_cost: -1\n bend_cost: -1\n tot_cost: -1\n max_tps_error: -1\n")
fh.write(" trajopt_info: \n")
for (i_miniseg, (i_start, i_end)) in enumerate(zip(miniseg_starts, miniseg_ends)):
if args.execution: Globals.pr2.update_rave()
# Changing the trajectory according to the end-effector
full_traj = np.c_[seg_info["leftarm"][i_start:i_end+1], seg_info["rightarm"][i_start:i_end+1]]
full_traj = mu.remove_duplicate_rows(full_traj)
_, ds_traj = resampling.adaptive_resample(full_traj, tol=.01, max_change=.1) # about 2.5 degrees, 10 degrees
Globals.robot.SetActiveDOFs(np.r_[Globals.robot.GetManipulator("leftarm").GetArmIndices(), Globals.robot.GetManipulator("rightarm").GetArmIndices()])
Globals.demo_robot.SetActiveDOFs(np.r_[Globals.robot.GetManipulator("leftarm").GetArmIndices(), Globals.robot.GetManipulator("rightarm").GetArmIndices()])
Globals.demo_robot.SetDOFValues(Globals.robot.GetDOFValues())
################################
redprint("Generating joint trajectory for segment %s, part %i"%(seg_name, i_miniseg))
bodypart2traj = {}
arms_used = ""
if args.logging:
with open(filename, 'a') as fh:
fh.write(" mini_seg%i: \n"%i_miniseg)
for lr in 'lr':
# if "%s_grab"%lr in seg_info['extra_information']:
# if "needle_tip_transform" in seg_info['key_points']:
# demo_ntfm = np.array(seg_info['key_points']['needle_tip_transform'])
# ntfm = np.array(new_keypoints['needle_tip_transform'])
# use_needle = True
# elif Globals.rel_ntfm is not None:
# T_w_g = Globals.robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
# T_g_n = Globals.rel_ntfm
# ntfm = T_w_g.dot(T_g_n)
# T_w_g_demo = Globals.demo_robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
# T_g_n_demo = Globals.demo_rel_ntfm
# demo_ntfm = T_w_g_demo.dot(T_g_n_demo)
# use_needle = True
# else:
# use_needle = False
# else:
# use_needle = False
if use_needle == lr:
Globals.sponge.Enable(False)
arm_traj = {'l':ds_traj[:,:7], 'r':ds_traj[:,7:]}[lr]
old_ee_traj = setup_needle_traj (lr, arm_traj)
link = Globals.needle_tip.GetLinks()[0]
redprint("Using needle for trajectory execution...")
else:
arm = {"l":"leftarm", "r":"rightarm"}[lr]
Globals.demo_robot.SetActiveDOFs(Globals.robot.GetManipulator(arm).GetArmIndices())
if seg_name == "8_knot_tie_next0_seg00":
Globals.sponge.Enable(False)
else:
Globals.sponge.Enable(True)
ee_link_name = "%s_gripper_tool_frame"%lr
link = Globals.robot.GetLink(ee_link_name)
#old_ee_traj = np.asarray(seg_info[ee_link_name]["hmat"])
demo_link = Globals.demo_robot.GetLink(ee_link_name)
old_ee_traj = []
arm_traj = {'l':ds_traj[:,:7], 'r':ds_traj[:,7:]}[lr]
for row in arm_traj:
Globals.demo_robot.SetActiveDOFValues(row)
old_ee_traj.append(demo_link.GetTransform())
############ CAN YOU PLOT THIS OLD_EE_TRAJ?
old_ee_traj = np.asarray(old_ee_traj)
old_joint_traj = {'l':ds_traj[:,:7], 'r':ds_traj[:,7:]}[lr]
if lr == 'l':#arm_moved(old_joint_traj):
manip_name = {"l":"leftarm", "r":"rightarm"}[lr]
new_ee_traj = f.transform_hmats(old_ee_traj)
#new_ee_traj = old_ee_traj
#################### CAN YOU PLOT THIS 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)))
#old_poses = [conv.hmat_to_pose(hmat) for hmat in old_ee_traj]
#print new_ee_traj
#new_poses = [conv.hmat_to_pose(hmat) for hmat in new_ee_traj]
update_markers(new_ee_traj, old_ee_traj)
#thc.run()
if args.execution: Globals.pr2.update_rave()
new_joint_traj, costs, cnts, pos_err = plan_follow_traj(Globals.robot, manip_name,
link, new_ee_traj, old_joint_traj, True)
new_joint_traj = unwrap_joint_traj (lr, new_joint_traj)
# (robot, manip_name, ee_link, new_hmats, old_traj):
part_name = {"l":"larm", "r":"rarm"}[lr]
bodypart2traj[part_name] = new_joint_traj
arms_used += lr
if args.logging:
with open(filename, 'a') as fh:
fh.write(" - %s: \n"%lr)
fh.write(" costs: \n")
tot_cost = 0
for c_name, c_val in costs:
fh.write(" - %s: %f\n"%(c_name, c_val))
tot_cost += c_val
fh.write(" - tot_cost: %f\n"%tot_cost)
fh.write(" constraints: \n")
for cnt in cnts:
fh.write(" - %s\n"%str(cnt))
fh.write(" max_pos_error: %f\n"%pos_err)
################################
redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, arms_used))
for lr in 'lr':
set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start]))
#trajoptpy.GetViewer(Globals.env).Idle()
if len(bodypart2traj) > 0:
exec_traj_maybesim(bodypart2traj, speed_factor=1)
# TODO measure failure condtions
if not success:
break
Globals.robot.ReleaseAllGrabbed()
Globals.demo_robot.ReleaseAllGrabbed()
redprint("Segment %s result: %s"%(seg_name, success))
if args.fake_data_segment: break
