def do_single(demo_name, stage_num, prev_demo_index, verb_data_accessor, prev_and_cur_pc2):
if stage_num == 0:
do_stage(demo_name, stage_num, None, None, prev_and_cur_pc2[1], verb_data_accessor)
else:
prev_stage_num = stage_num - 1
prev_demo_name = "%s%i" % (demo_base_name, prev_demo_index)
prev_stage_info = verb_data_accessor.get_stage_info(prev_demo_name, prev_stage_num)
call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller")
print colorize("do the stage involving the %s" % (prev_stage_info.item), color="red", bold=True)
yes_or_no("type y to continue")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller")
do_stage(demo_name, stage_num, prev_stage_info, prev_and_cur_pc2[0], prev_and_cur_pc2[1], verb_data_accessor)
def exec_traj_do_work(l_gripper_poses, l_gripper_angles, r_gripper_poses, r_gripper_angles, traj_ik_func, obj_cloud_xyz, obj_name, can_move_lower):
del Globals.handles[1:]
grab_obj_kinbody = setup_obj_rave(obj_cloud_xyz, obj_name) if obj_cloud_xyz is not None else None
body_traj = {}
for (lr, gripper_poses, gripper_angles) in zip("lr", [l_gripper_poses, r_gripper_poses], [l_gripper_angles, r_gripper_angles]):
if len(gripper_poses) == 0: continue
gripper_poses, gripper_angles = np.array(gripper_poses), np.array(gripper_angles)
gripper_angles_grabbing = process_gripper_angles_for_grabbing(lr, gripper_angles)
Globals.pr2.update_rave()
current_pos = Globals.pr2.robot.GetLink("%s_gripper_tool_frame"%lr).GetTransform()
if can_move_lower:
gripper_poses_remove, gripper_angles_remove = gripper_poses, gripper_angles_grabbing
else:
gripper_poses_remove, gripper_angles_remove = remove_lower_poses(current_pos, gripper_poses, gripper_angles_grabbing)
gripper_poses_prepend, gripper_angles_prepend = prepend_path_to_start(current_pos, gripper_poses_remove, gripper_angles_remove, 15)
final_gripper_poses, final_gripper_angles = gripper_poses_prepend, gripper_angles_prepend
#do ik
joint_positions = traj_ik_func(Globals.pr2, lr, final_gripper_poses)
if len(joint_positions) == 0:
return ExecTrajectoryResponse(success=False)
unwrapped_joint_positions = unwrap_angles(joint_positions)
final_joint_positions = unwrapped_joint_positions
body_traj["%s_arm"%lr] = final_joint_positions
body_traj["%s_gripper"%lr] = final_gripper_angles
yn = yes_or_no("continue?")
if yn:
lt.follow_trajectory_with_grabs(Globals.pr2, body_traj)
while not yes_or_no("continue to next stage?"):
lt.follow_trajectory_with_grabs(Globals.pr2, body_traj)
if grab_obj_kinbody is not None:
handle_grab_or_release_obj(grab_obj_kinbody, l_gripper_poses, l_gripper_angles, r_gripper_poses, r_gripper_angles)
raw_input("Press enter when done viewing trajectory")
Globals.pr2.join_all()
return ExecTrajectoryResponse(success=True)
else:
return ExecTrajectoryResponse(success=False)
def do_single(demo_base_name, demo_index, stage_num, prev_demo_index):
if stage_num == 0:
do_stage(demo_base_name, demo_index, stage_num, None, None)
else:
prev_stage_num = stage_num - 1
prev_demo_name = demo_base_name + str(prev_demo_index)
prev_stage_info = verb_data_accessor.get_stage_info(prev_demo_name, prev_stage_num)
prev_exp_pc = do_segmentation(prev_stage_info.item)
call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller")
print colorize("do the stage involving the %s" % (prev_stage_info.item), color="red", bold=True)
yes_or_no("type y to continue")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller")
do_stage(demo_base_name, demo_index, stage_num, prev_stage_info, np.array([prev_exp_pc]))
def execute_dual_arm_trajectory(pr2, lquat, lpos, lgrip, left_used, rquat, rpos, rgrip, right_used, rope):
rviz.draw_curve(lpos, RVIZ_LPOS, width=.005)
rviz.draw_curve(rpos, RVIZ_RPOS, width=.005)
ok = yes_or_no("trajectory ok?")
if not ok:
print "canceling trajectory"
return
for i in xrange(len(lquat)):
if left_used:
brett.larm.set_cart_target(normalize(quatswap(lquat[i])), lpos[i], "/base_footprint")
if lgrip[i] < .03: lgrip[i] = 0
brett.lgrip.set_angle_target(lgrip[i])
if right_used:
brett.rarm.set_cart_target(normalize(quatswap(rquat[i])), rpos[i], "/base_footprint")
if rgrip[i] < .03: rgrip[i] = 0
brett.rgrip.set_angle_target(rgrip[i])
rviz.draw_curve(rope[i], RVIZ_ORIG_ROPE, width=.01, rgba = (1,0,0,1))
sleep(.04)
def erase_all_data(group):
consent = yes_or_no("erase all data in %s?" % group.name)
if consent:
for name in group.keys():
erase_dataset(group, name)
else:
raise IOError
def execute_dual_arm_trajectory(pr2, lquat, lpos, lgrip, left_used, rquat,
rpos, rgrip, right_used, rope):
rviz.draw_curve(lpos, RVIZ_LPOS, width=.005)
rviz.draw_curve(rpos, RVIZ_RPOS, width=.005)
ok = yes_or_no("trajectory ok?")
if not ok:
print "canceling trajectory"
return
for i in xrange(len(lquat)):
if left_used:
brett.larm.set_cart_target(normalize(quatswap(lquat[i])), lpos[i],
"/base_footprint")
if lgrip[i] < .03: lgrip[i] = 0
brett.lgrip.set_angle_target(lgrip[i])
if right_used:
brett.rarm.set_cart_target(normalize(quatswap(rquat[i])), rpos[i],
"/base_footprint")
if rgrip[i] < .03: rgrip[i] = 0
brett.rgrip.set_angle_target(rgrip[i])
rviz.draw_curve(rope[i], RVIZ_ORIG_ROPE, width=.01, rgba=(1, 0, 0, 1))
sleep(.04)
def make_and_execute_verb_traj(verb, arg_clouds):
if "expansion" in verb_info[verb]:
prim_verbs = verb_info[verb]["expansion"]
print "expanding into primitive actions: %s" % " ".join(prim_verbs)
for i in xrange(len(prim_verbs)):
exec_success = make_and_execute_verb_traj(prim_verbs[i],
arg_clouds[i:i + 1])
if exec_success == False:
rospy.logerr("stopping complex verb %s", verb)
return False
else:
if len(F[verb]["demos"]) == 0:
rospy.logerr("verb %s has no demonstrations!", verb)
return False
make_req = MakeTrajectoryRequest()
make_req.object_clouds = [
xyz2pc(cloud, "base_footprint") for cloud in arg_clouds
]
make_req.verb = verb
make_resp = make_svc.call(make_req)
approved = yes_or_no("execute trajectory?")
if approved:
exec_req = ExecTrajectoryRequest(make_resp.traj)
exec_resp = exec_svc.call(exec_req)
if exec_resp.success == False:
rospy.logerr("%s failed :(", verb)
return False
else:
rospy.loginfo("%s succeeded!", verb)
def make_and_execute_verb_traj(verb, arg_clouds):
if "expansion" in verb_info[verb]:
prim_verbs = verb_info[verb]["expansion"]
print "expanding into primitive actions: %s"%" ".join(prim_verbs)
for i in xrange(len(prim_verbs)):
exec_success = make_and_execute_verb_traj(prim_verbs[i], arg_clouds[i:i+1])
if exec_success == False:
rospy.logerr("stopping complex verb %s", verb)
return False
else:
if len(F[verb]["demos"]) == 0:
rospy.logerr("verb %s has no demonstrations!", verb)
return False
make_req = MakeTrajectoryRequest()
make_req.object_clouds = [xyz2pc(cloud, "base_footprint") for cloud in arg_clouds]
make_req.verb = verb
make_resp = make_svc.call(make_req)
approved = yes_or_no("execute trajectory?")
if approved:
exec_req = ExecTrajectoryRequest(make_resp.traj)
exec_resp = exec_svc.call(exec_req)
if exec_resp.success == False:
rospy.logerr("%s failed :(", verb)
return False
else:
rospy.loginfo("%s succeeded!", verb)
def get_all_clouds_pc2(num_objs):
clouds = []
for obj_num in xrange(num_objs):
next_cloud = filter_pc2(do_segmentation("object%i" % obj_num))
while not yes_or_no("Continue?"):
next_cloud = filter_pc2(do_segmentation("object%i" % obj_num))
clouds.append(next_cloud)
return clouds
def do_empty_move(demo_name, exp_name, test_dir_name):
empty_move_init()
verb_data_accessor = multi_item_verbs.VerbDataAccessor(test_info_dir=osp.join("test", TEST_DATA_DIR, test_dir_name))
for stage_num in xrange(verb_data_accessor.get_num_stages(demo_name)):
# info and data for previous stage
if stage_num > 0:
prev_demo_info = verb_data_accessor.get_stage_info(demo_name, stage_num-1)
prev_exp_info = verb_data_accessor.get_stage_info(exp_name, stage_num-1)
prev_exp_data = verb_data_accessor.get_demo_stage_data(prev_exp_info.stage_name)
prev_exp_pc = prev_exp_data["object_cloud"][prev_exp_info.item]["xyz"]
else:
# first stage has no previous items
prev_demo_info, prev_exp_info, prev_exp_data, prev_exp_pc = None, None, None, None
# info and data for current stage
cur_demo_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
cur_exp_info = verb_data_accessor.get_stage_info(exp_name, stage_num)
cur_exp_data = verb_data_accessor.get_demo_stage_data(cur_exp_info.stage_name)
cur_exp_pc = cur_exp_data["object_cloud"][cur_exp_info.item]["xyz"]
if stage_num == 0:
world_to_grip_transform_func = None
else:
world_to_grip_transform_func = multi_item_make_verb_traj.make_world_to_grip_transform_tf("%s_gripper_tool_frame" % cur_exp_info.arms_used)
warped_traj_resp = multi_item_make_verb_traj.make_traj_multi_stage_do_work(demo_name, cur_exp_pc, "base_footprint",
stage_num, prev_demo_info, prev_exp_pc,
verb_data_accessor, world_to_grip_transform_func, transform_type="tps_zrot")
yn = yes_or_no("continue?")
if yn:
can_move_lower = (stage_num == 0)
traj = warped_traj_resp.traj
exec_verb_traj.exec_traj_do_work(traj.l_gripper_poses.poses, traj.l_gripper_angles,
traj.r_gripper_poses.poses, traj.r_gripper_angles,
ik_functions.do_traj_ik_graph_search, cur_exp_pc,
cur_demo_info.item, can_move_lower)
if stage_num < verb_data_accessor.get_num_stages(demo_name)-1:
yn = yes_or_no("continue to next stage?")
if not yn:
break
def mkdir_ask(path,make_path=False):
if os.path.exists(path):
consent = yes_or_no("%s already exists. Delete it?"%path)
if consent:
shutil.rmtree(path)
else:
raise IOError
if make_path: os.makedirs(path)
else: os.mkdir(path)
def mkdir_ask(path, make_path=False):
if os.path.exists(path):
consent = yes_or_no("%s already exists. Delete it?" % path)
if consent:
shutil.rmtree(path)
else:
raise IOError
if make_path: os.makedirs(path)
else: os.mkdir(path)
def record_demo_stage(name_for_motion, item_name, data_dir):
os.chdir(data_dir + "/images")
get_point_cloud(name_for_motion, item_name)
while not yes_or_no("Ready to continue?"):
get_point_cloud(name_for_motion, item_name)
os.chdir(data_dir + "/bags")
record_trajectory(args.dry_run, name_for_motion)
n_tries = 20
if not args.dry_run:
rename_bag_file(name_for_motion, n_tries)
def get_all_clouds_pc2(num_objs, save_file=None):
clouds_pc2 = []
for obj_num in xrange(num_objs):
next_cloud = filter_pc2(do_segmentation("object%i" % obj_num))
while not yes_or_no("Continue?"):
next_cloud = filter_pc2(do_segmentation("object%i" % obj_num))
clouds_pc2.append(next_cloud)
if save_file is not None:
save_pcs(clouds_pc2, save_file)
return clouds_pc2
def record_demonstration_for_motion(name_for_motion, item_name):
# get the point cloud for the first item
os.chdir(data_dir + "/images")
call_and_print("get_point_cloud.py %s" % (name_for_motion))
print colorize("now, select the %s object" % (item_name), color="red", bold=True)
# two point clouds need to be stored, so give first point cloud a suffix of '-1'
call_and_print("manually_segment_point_cloud.py %s.npz --objs=%s" % (name_for_motion, item_name))
yes_or_no("Ready to continue?")
# do the demonstration for the first motion
print colorize("now, human, demonstrate the next action for %s on the robot" % (name_for_motion), color="red", bold=True)
os.chdir(data_dir + "/bags")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager stop r_arm_controller l_arm_controller")
# record the demonstration for the first motion
try:
old_tmp_bags = glob("tmpname*.bag")
if not args.dry_run:
for bag in old_tmp_bags:
os.remove(bag)
call_and_print("rosbag record /tf /joint_states /joy -o tmpname")
except KeyboardInterrupt:
pass
# rename bag file that was just created
n_tries = 20
if not args.dry_run:
success = False
for i in xrange(n_tries):
try:
bagname = glob("tmpname*.bag")[0]
os.rename(bagname, "%s.bag" % (name_for_motion))
success = True
except IndexError:
time.sleep(.1)
if not success: raise Exception("couldn't get bag file")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller")
def execute(self, userdata):
#if not args.test: draw_table()
Globals.pr2.update_rave()
if yes_or_no('about to execute trajectory. save?'):
import pickle
fname = '/tmp/trajectory_' + str(np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(userdata.trajectory, f)
print 'saved to', fname
success = lfd_traj.follow_trajectory_with_grabs(Globals.pr2, userdata.trajectory)
raw_input('done executing segment. press enter to continue')
if success:
if args.count_steps: Globals.stage += 1
return "success"
else: return "failure"
def do_stage(demo_base_name, demo_index, stage_num, prev_stage_info, prev_exp_clouds):
demo_name = demo_base_name + str(demo_index)
stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
pc_sel = do_segmentation(stage_info.item)
make_req = get_trajectory_request(stage_info.verb, pc_sel)
make_resp = make_verb_traj.make_traj_multi_stage(make_req, stage_info, stage_num, prev_stage_info, prev_exp_clouds, verb_data_accessor)
yn = yes_or_no("continue?")
if yn:
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req, traj_ik_func=exec_verb_traj.do_traj_ik_graph_search)
# return stage info and object clouds so they can be saved for use in the next stage if necessary
return (stage_info, make_req.object_clouds)
def execute(self, userdata):
#if not args.test: draw_table()
Globals.pr2.update_rave()
if yes_or_no('about to execute trajectory. save?'):
import pickle
fname = '/tmp/trajectory_' + str(
np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(userdata.trajectory, f)
print 'saved to', fname
success = lfd_traj.follow_trajectory_with_grabs(
Globals.pr2, userdata.trajectory)
raw_input('done executing segment. press enter to continue')
if success:
if args.count_steps: Globals.stage += 1
return "success"
else:
return "failure"
def do_stage(demo_name, stage_num, prev_stage_info, prev_exp_pc2, cur_exp_pc2, verb_data_accessor):
stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
make_req = get_trajectory_request(stage_info.verb, cur_exp_pc2)
make_resp = multi_item_make_verb_traj.make_traj_multi_stage(
make_req, demo_name, stage_num, prev_stage_info, prev_exp_pc2, verb_data_accessor, "tps_zrot"
)
can_move_lower = stage_num == 0
yn = yes_or_no("continue?")
if yn:
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(
exec_req,
traj_ik_func=ik_functions.do_traj_ik_graph_search,
obj_pc=cur_exp_pc2,
obj_name=stage_info.item,
can_move_lower=can_move_lower,
)
def exec_traj(req, traj_ik_func=do_traj_ik_default):
assert isinstance(req, ExecTrajectoryRequest)
del Globals.handles[1:]
traj = req.traj
body_traj = {}
for (lr,gripper_poses, gripper_angles) in zip("lr",[traj.l_gripper_poses.poses,traj.r_gripper_poses.poses], [traj.l_gripper_angles,traj.r_gripper_angles]):
if len(gripper_poses) == 0: continue
gripper_angles = np.array(gripper_angles)
rospy.logwarn("warning! gripper angle hacks")
gripper_angles[gripper_angles < .04] = gripper_angles[gripper_angles < .04] - .02
gripper_angles = get_proper_gripper_angles(lr, gripper_angles)
plot_gripper_xyzs_from_poses(lr, gripper_poses)
#do ik
joint_positions = traj_ik_func(lr, gripper_poses)
if len(joint_positions) == 0:
return ExecTrajectoryResponse(success=False)
joint_positions = ku.smooth_positions(joint_positions, .15)
body_traj["%s_arm"%lr] = joint_positions
body_traj["%s_gripper"%lr] = gripper_angles
pose_array = gm.PoseArray()
pose_array.header.frame_id = "base_footprint"
pose_array.header.stamp = rospy.Time.now()
pose_array.poses = traj.l_gripper_poses.poses if lr == 'l' else traj.r_gripper_poses.poses
Globals.handles.append(Globals.rviz.draw_curve(pose_array, rgba = (1,0,0,1)))
yn = yes_or_no("continue?")
if yn:
lt.go_to_start(Globals.pr2, body_traj)
lt.follow_trajectory_with_grabs(Globals.pr2, body_traj)
Globals.pr2.join_all()
return ExecTrajectoryResponse(success=True)
else:
return ExecTrajectoryResponse(success=False)
def do_stage(demo_name, stage_num, prev_stage_info, prev_exp_pc2, cur_exp_pc2, verb_data_accessor):
stage_info = verb_data_accessor.get_stage_info(demo_name, stage_num)
make_req = get_trajectory_request(stage_info.verb, cur_exp_pc2)
if stage_num == 0:
grip_to_world_transform_func = None
else:
grip_to_world_transform_func = multi_item_make_verb_traj.make_grip_to_world_transform_tf("%s_gripper_tool_frame" %
verb_data_accessor.get_stage_info(demo_name, stage_num).arms_used)
make_resp = multi_item_make_verb_traj.make_traj_multi_stage(make_req, demo_name,
stage_num, prev_stage_info,
prev_exp_pc2, verb_data_accessor,
"tps_zrot")
can_move_lower = (stage_num == 0)
yn = yes_or_no("continue?")
if yn:
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req, traj_ik_func=ik_functions.do_traj_ik_graph_search,
obj_pc=cur_exp_pc2, obj_name=stage_info.item, can_move_lower=can_move_lower)
os.rename(bagname, "%s.bag"%demo_name)
success = True
except IndexError:
time.sleep(.1)
if not success: raise Exception("couldn't get bag file")
call_and_print("rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller")
new_entry_text = """
# AUTOMATICALLY ADDED BY teach_verb.py ON %(datestr)s
%(demo_name)s:
bag_file: bags/%(demo_name)s.bag
seg_file: images/%(demo_name)s.seg.h5
verb: %(verb)s
args: [%(target)s]
arms_used: %(arms_used)s
"""%dict(
datestr = datetime.date.today(),
demo_name = demo_name,
verb = verb,
target = target,
arms_used = arms_used)
yn = yes_or_no("save demonstration?")
if yn:
with open(data_dir + "/verb_demos2.yaml", "a") as demofile:
if not args.dry_run: demofile.write(new_entry_text)
call_and_print("make_verb_library2.py")
else:
print "exiting without saving"
assert args.order is not None
assert args.reset is not None
from comm.vision_pipeline import make_towel_pipeline, make_robot_rope_pipeline2, make_human_rope_pipeline
from comm import pipeline
from comm import comm
import os, subprocess
from jds_utils.yes_or_no import yes_or_no
comm.initComm()
os.chdir(comm.DATA_ROOT)
if args.pipeline == "robot_rope":
if args.reset == "full":
yn = yes_or_no("are you sure you want to delete everything?")
if yn: subprocess.check_call("rm -r *", shell=True)
else: exit(0)
elif args.reset == "partial":
subprocess.check_call(
"rm -rf kinect labels images rope_pts towel_pts rope_init rope_model logs/* joint_states base_pose once/table_corners.txt",
shell=True)
elif args.reset == "no":
pass
PIPELINE = make_robot_rope_pipeline2(
classifier=
"/home/joschu/python/jds_image_proc/pixel_classifiers/rope_sdh_light_afternoon/classifier.pkl",
downsample=5)
if args.pipeline == "human_rope":
def execute(self,userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
#if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds)
ELOG.log('SelectTrajectory', 'xyz_new', xyz_new)
ELOG.log('SelectTrajectory', 'xyz_new_ds', xyz_new_ds)
ELOG.log('SelectTrajectory', 'dists_new', dists_new)
if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage]
else: candidate_demo_names = demos.keys()
global last_selected_segment
print 'Last selected segment:', last_selected_segment
if args.human_select_demo:
best_name = None
while best_name not in demos:
print 'Select demo from', demos.keys()
best_name = raw_input('> ')
else:
from joblib import parallel
costs_names = parallel.Parallel(n_jobs=-2)(parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names)
#costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names]
#costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names]
ELOG.log('SelectTrajectory', 'costs_names', costs_names)
_, best_name = min(costs_names)
ELOG.log('SelectTrajectory', 'best_name', best_name)
best_demo = demos[best_name]
if best_demo["done"]:
rospy.loginfo("best demo was a 'done' state")
return "done"
best_demo = demos[best_name]
rospy.loginfo("best segment name: %s", best_name)
last_selected_segment = best_name
xyz_demo_ds = best_demo["cloud_xyz_ds"]
ELOG.log('SelectTrajectory', 'xyz_demo_ds', xyz_demo_ds)
if args.test: n_iter = 21
else: n_iter = 101
if args.use_rigid:
self.f = registration.Translation2d()
self.f.fit(xyz_demo_ds, xyz_new_ds)
ELOG.log('SelectTrajectory', 'f', self.f)
else:
self.f, info = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False, return_full=True)#, interactive=True)
ELOG.log('SelectTrajectory', 'f', self.f)
ELOG.log('SelectTrajectory', 'f_info', info)
if args.use_nr:
rospy.loginfo('Using nonrigidity costs')
from lfd import tps
import scipy.spatial.distance as ssd
pts_grip = []
for lr in "lr":
if best_demo["arms_used"] in ["b", lr]:
pts_grip.extend(best_demo["%s_gripper_tool_frame"%lr]["position"])
pts_grip = np.array(pts_grip)
dist_to_rope = ssd.cdist(pts_grip, xyz_demo_ds).min(axis=1)
pts_grip_near_rope = pts_grip[dist_to_rope < .04,:]
pts_rigid = voxel_downsample(pts_grip_near_rope, .01)
self.f.lin_ag, self.f.trans_g, self.f.w_ng, self.f.x_na = tps.tps_nr_fit_enhanced(info["x_Nd"], info["targ_Nd"], 0.01, pts_rigid, 0.001, method="newton", plotting=5)
# print 'correspondences', self.f.corr_nm
#################### Generate new trajectory ##################
#### Plot original and warped point clouds #######
# orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint")
# warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint")
# Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
# Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
#### Plot grid ########
mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0)
maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint')
Globals.handles.append(grid_handle)
#### Actually generate the trajectory ###########
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo)
# if yes_or_no('dump warped demo?'):
# import pickle
# fname = '/tmp/warped_demo_' + str(np.random.randint(9999999999)) + '.pkl'
# with open(fname, 'w') as f:
# pickle.dump(warped_demo, f)
# print 'saved to', fname
ELOG.log('SelectTrajectory', 'warped_demo', warped_demo)
def make_traj(warped_demo, inds=None, xyz_offset=0, feas_check_only=False):
traj = {}
total_feas_inds = 0
total_inds = 0
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if args.hard_table:
clipinplace(warped_demo["l_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
clipinplace(warped_demo["r_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
pos = warped_demo["%s_gripper_tool_frame"%lr]["position"] + xyz_offset
ori = warped_demo["%s_gripper_tool_frame"%lr]["orientation"]
if inds is not None:
pos, ori = pos[inds], ori[inds]
if feas_check_only:
feas_inds = lfd_traj.compute_feas_inds(
pos,
ori,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True)
traj["%s_arm_feas_inds"%lr] = feas_inds
else:
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
pos,
ori,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True)
traj["%s_arm"%lr] = arm_traj
traj["%s_arm_feas_inds"%lr] = feas_inds
total_feas_inds += len(feas_inds)
total_inds += len(pos)
rospy.loginfo("%s arm: %i of %i points feasible", leftright, len(feas_inds), len(pos))
return traj, total_feas_inds, total_inds
# Check if we need to move the base for reachability
base_offset = np.array([0, 0, 0])
if args.use_base:
# First figure out how much we need to move the base to maximize feasible points
OFFSET = 0.1
XYZ_OFFSETS = np.array([[0., 0., 0.], [-OFFSET, 0, 0], [OFFSET, 0, 0], [0, -OFFSET, 0], [0, OFFSET, 0]])
inds_to_check = lfd_traj.where_near_rope(best_demo, xyz_demo_ds, add_other_points=30)
need_to_move_base = False
best_feas_inds, best_xyz_offset = -1, None
for xyz_offset in XYZ_OFFSETS:
_, n_feas_inds, n_total_inds = make_traj(warped_demo, inds=inds_to_check, xyz_offset=xyz_offset, feas_check_only=True)
rospy.loginfo('Cloud offset %s has feas inds %d', str(xyz_offset), n_feas_inds)
if n_feas_inds > best_feas_inds:
best_feas_inds, best_xyz_offset = n_feas_inds, xyz_offset
if n_feas_inds >= 0.99*n_total_inds: break
if np.linalg.norm(best_xyz_offset) > 0.01:
need_to_move_base = True
base_offset = -best_xyz_offset
rospy.loginfo('Best base offset: %s, with %d feas inds', str(base_offset), best_feas_inds)
# Move the base
if need_to_move_base:
rospy.loginfo('Will move base.')
userdata.base_offset = base_offset
return 'move_base'
else:
rospy.loginfo('Will not move base.')
Globals.pr2.update_rave()
# calculate joint trajectory using IK
trajectory = make_traj(warped_demo)[0]
# fill in gripper/grab stuff
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if len(trajectory["%s_arm_feas_inds"%lr]) == 0: return "failure"
trajectory["%s_grab"%lr] = best_demo["%s_gripper_joint"%lr] < .07
trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr]
trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0
# smooth any discontinuities in the arm traj
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
trajectory["%s_arm"%lr], discont_times, n_steps = lfd_traj.smooth_disconts(
trajectory["%s_arm"%lr],
Globals.pr2.env,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
ignore_inds=[1] # ignore the 0--1 discontinuity, which is usually just moving from rest to the traj starting pose
)
# after smoothing the arm traj, we need to fill in all other trajectories (in both arms)
other_lr = 'r' if lr == 'l' else 'l'
if best_demo["arms_used"] in [other_lr, "b"]:
trajectory["%s_arm"%other_lr] = lfd_traj.fill_stationary(trajectory["%s_arm"%other_lr], discont_times, n_steps)
for tmp_lr in 'lr':
if best_demo["arms_used"] in [tmp_lr, "b"]:
trajectory["%s_grab"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_grab"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_gripper"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr][trajectory["%s_grab"%tmp_lr]] = 0
# plotting
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
# plot warped trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"], warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (1,0,1,.4), type=Marker.LINE_LIST,
ns='warped_finger_traj'
))
# plot original trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"], best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (0,1,1,.4), type=Marker.LINE_LIST,
ns='demo_finger_traj'
))
ELOG.log('SelectTrajectory', 'trajectory', trajectory)
userdata.trajectory = trajectory
if args.prompt_before_motion:
consent = yes_or_no("trajectory ok?")
else:
consent = True
if consent: return "not_done"
else: return "failure"
from comm.vision_pipeline import make_towel_pipeline, make_robot_rope_pipeline2, make_human_rope_pipeline
from comm import pipeline
from comm import comm
import os, subprocess
from jds_utils.yes_or_no import yes_or_no
comm.initComm()
os.chdir(comm.DATA_ROOT)
if args.pipeline == "robot_rope":
if args.reset == "full":
yn = yes_or_no("are you sure you want to delete everything?")
if yn: subprocess.check_call("rm -r *", shell=True)
else: exit(0)
elif args.reset == "partial":
subprocess.check_call("rm -rf kinect labels images rope_pts towel_pts rope_init rope_model logs/* joint_states base_pose once/table_corners.txt",shell=True)
elif args.reset == "no":
pass
PIPELINE = make_robot_rope_pipeline2(
classifier="/home/joschu/python/jds_image_proc/pixel_classifiers/rope_sdh_light_afternoon/classifier.pkl",
downsample=5)
if args.pipeline == "human_rope":
if args.reset == "full":
yn = yes_or_no("are you sure you want to delete everything?")
success = True
except IndexError:
time.sleep(.1)
if not success: raise Exception("couldn't get bag file")
call_and_print(
"rosrun pr2_controller_manager pr2_controller_manager start r_arm_controller l_arm_controller"
)
new_entry_text = """
# AUTOMATICALLY ADDED BY teach_verb.py ON %(datestr)s
%(demo_name)s:
bag_file: bags/%(demo_name)s.bag
seg_file: images/%(demo_name)s.seg.h5
verb: %(verb)s
args: [%(target)s]
arms_used: %(arms_used)s
""" % dict(datestr=datetime.date.today(),
demo_name=demo_name,
verb=verb,
target=target,
arms_used=arms_used)
yn = yes_or_no("save demonstration?")
if yn:
with open(data_dir + "/verb_demos2.yaml", "a") as demofile:
if not args.dry_run: demofile.write(new_entry_text)
call_and_print("make_verb_library2.py")
else:
print "exiting without saving"
def execute(self, userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
#if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled_old(
xyz_new, xyz_new_ds, ds_inds)
if args.human_select_demo:
raise NotImplementedError
seg_name = trajectory_library.interactive_select_demo(demos)
best_demo = demos[seg_name]
pts0, _ = best_demo["cloud_xyz_ds"]
pts1, _ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0,
pts1,
plotting=4,
reg_init=1,
reg_final=args.reg_final,
n_iter=40)
else:
if args.count_steps:
candidate_demo_names = self.count2segnames[Globals.stage]
else:
candidate_demo_names = demos.keys()
from joblib import parallel
costs_names = parallel.Parallel(n_jobs=-2)(
parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds,
dists_new)
for seg_name in candidate_demo_names)
#costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names]
#costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names]
_, best_name = min(costs_names)
best_demo = demos[best_name]
if best_demo["done"]:
rospy.loginfo("best demo was a 'done' state")
return "done"
best_demo = demos[best_name]
rospy.loginfo("best segment name: %s", best_name)
xyz_demo_ds = best_demo["cloud_xyz_ds"]
if args.test: n_iter = 21
else: n_iter = 101
if args.use_rigid:
self.f = registration.Translation2d()
self.f.fit(xyz_demo_ds, xyz_new_ds)
else:
self.f = registration.tps_rpm(xyz_demo_ds,
xyz_new_ds,
plotting=20,
reg_init=1,
reg_final=.01,
n_iter=n_iter,
verbose=False) #, interactive=True)
np.savez('registration_data',
xyz_demo_ds=xyz_demo_ds,
xyz_new_ds=xyz_new_ds)
# print 'correspondences', self.f.corr_nm
#################### Generate new trajectory ##################
#### Plot original and warped point clouds #######
# orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint")
# warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint")
# Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
# Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
#### Plot grid ########
mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0)
maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points,
mins, maxes, 'base_footprint')
Globals.handles.append(grid_handle)
#### Actually generate the trajectory ###########
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo)
if yes_or_no('dump warped demo?'):
import pickle
fname = '/tmp/warped_demo_' + str(
np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(warped_demo, f)
print 'saved to', fname
Globals.pr2.update_rave()
trajectory = {}
# calculate joint trajectory using IK
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if args.hard_table:
clipinplace(
warped_demo["l_gripper_tool_frame"]["position"][:, 2],
Globals.table_height + .032, np.inf)
clipinplace(
warped_demo["r_gripper_tool_frame"]["position"][:, 2],
Globals.table_height + .032, np.inf)
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
warped_demo["%s_gripper_tool_frame" % lr]["position"],
warped_demo["%s_gripper_tool_frame" % lr]["orientation"],
Globals.pr2.robot.GetManipulator("%sarm" % leftright),
"%s_gripper_tool_frame" % lr,
check_collisions=True)
if len(feas_inds) == 0: return "failure"
trajectory["%s_arm" % lr] = arm_traj
trajectory["%s_grab" %
lr] = best_demo["%s_gripper_joint" % lr] < .07
trajectory["%s_gripper" %
lr] = warped_demo["%s_gripper_joint" % lr]
trajectory["%s_gripper" % lr][trajectory["%s_grab" % lr]] = 0
# smooth any discontinuities in the arm traj
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
trajectory[
"%s_arm" %
lr], discont_times, n_steps = lfd_traj.smooth_disconts(
trajectory["%s_arm" % lr], Globals.pr2.env,
Globals.pr2.robot.GetManipulator("%sarm" % leftright),
"%s_gripper_tool_frame" % lr)
# after smoothing the arm traj, we need to fill in all other trajectories (in both arms)
other_lr = 'r' if lr == 'l' else 'l'
if best_demo["arms_used"] in [other_lr, "b"]:
trajectory["%s_arm" % other_lr] = lfd_traj.fill_stationary(
trajectory["%s_arm" % other_lr], discont_times,
n_steps)
for tmp_lr in 'lr':
if best_demo["arms_used"] in [tmp_lr, "b"]:
trajectory["%s_grab" %
tmp_lr] = lfd_traj.fill_stationary(
trajectory["%s_grab" % tmp_lr],
discont_times, n_steps)
trajectory["%s_gripper" %
tmp_lr] = lfd_traj.fill_stationary(
trajectory["%s_gripper" % tmp_lr],
discont_times, n_steps)
trajectory["%s_gripper" %
tmp_lr][trajectory["%s_grab" % tmp_lr]] = 0
# plotting
for lr in "lr":
leftright = {"l": "left", "r": "right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
# plot warped trajectory
Globals.handles.append(
Globals.rviz.draw_curve(conversions.array_to_pose_array(
alternate(
warped_demo["%s_gripper_l_finger_tip_link" %
lr]["position"],
warped_demo["%s_gripper_r_finger_tip_link" %
lr]["position"]), "base_footprint"),
width=.001,
rgba=(1, 0, 1, .4),
type=Marker.LINE_LIST,
ns='warped_finger_traj'))
# plot original trajectory
Globals.handles.append(
Globals.rviz.draw_curve(conversions.array_to_pose_array(
alternate(
best_demo["%s_gripper_l_finger_tip_link" %
lr]["position"],
best_demo["%s_gripper_r_finger_tip_link" %
lr]["position"]), "base_footprint"),
width=.001,
rgba=(0, 1, 1, .4),
type=Marker.LINE_LIST,
ns='demo_finger_traj'))
userdata.trajectory = trajectory
if args.prompt_before_motion:
consent = yes_or_no("trajectory ok?")
else:
consent = True
if consent: return "not_done"
else: return "failure"
def create_segments(bag, link_names):
"""extract a bunch of dictionaries from bag file, each one corresponding to one segment of trajectory.
each segment corresponds to 'look', 'start', and 'stop' button presses"""
button_presses = get_button_presses(bag)
all_times = read_button_presses(button_presses)
print 'Events in bag file:', actions_to_str(all_times)
if yes_or_no('edit events?'):
s = raw_input(
'new action sequence (must be the same length as the original): ')
#print all_times
all_times = str_to_actions(s, [t for t, _ in all_times])
#print all_times
start_times, stop_times, look_times, l_close_times, l_open_times, r_close_times, r_open_times, done_times = \
correct_bag_errors(all_times)
assert len(start_times) == len(stop_times) == len(look_times)
assert len(done_times) == 1
kinematics_info = extract_kinematics_from_bag(bag, link_names)
N = len(kinematics_info["time"])
l_open = np.zeros(N, bool)
l_close = np.zeros(N, bool)
r_open = np.zeros(N, bool)
r_close = np.zeros(N, bool)
times = kinematics_info["time"]
l_open[np.searchsorted(times, l_open_times)] = True
l_close[np.searchsorted(times, l_close_times)] = True
r_open[np.searchsorted(times, r_open_times)] = True
r_close[np.searchsorted(times, r_close_times)] = True
kinematics_info["l_open"] = l_open
kinematics_info["l_close"] = l_close
kinematics_info["r_open"] = r_open
kinematics_info["r_close"] = r_close
start_inds = np.searchsorted(times, start_times)
stop_inds = np.searchsorted(times, stop_times)
look_clouds = get_transformed_clouds(bag, look_times)
seg_infos = []
for (start_ind, stop_ind, (xyz, bgr)) in zip(start_inds, stop_inds,
look_clouds):
seg_info = extract_segment(kinematics_info, start_ind, stop_ind)
seg_info["arms_used"] = determine_arms_used(seg_info)
seg_info["cloud_xyz"] = xyz
seg_info["cloud_bgr"] = bgr
seg_info["done"] = False
seg_infos.append(seg_info)
done_clouds = get_transformed_clouds(bag, done_times)
for (xyz, bgr) in done_clouds:
seg_info = {}
seg_info["cloud_xyz"] = xyz
seg_info["cloud_bgr"] = bgr
seg_info["done"] = True
seg_infos.append(seg_info)
return seg_infos
def execute(self,userdata):
"""
- lookup closest trajectory from database
- if it's a terminal state, we're done
- warp it based on the current rope
returns: done, not_done, failure
"""
xyz_new = np.squeeze(np.asarray(userdata.points))
#if args.obj == "cloth": xyz_new = voxel_downsample(xyz_new, .025)
xyz_new_ds, ds_inds = downsample(xyz_new)
dists_new = recognition.calc_geodesic_distances_downsampled_old(xyz_new,xyz_new_ds, ds_inds)
if args.human_select_demo:
raise NotImplementedError
seg_name = trajectory_library.interactive_select_demo(demos)
best_demo = demos[seg_name]
pts0,_ = best_demo["cloud_xyz_ds"]
pts1,_ = downsample(xyz_new)
self.f = registration.tps_rpm(pts0, pts1, plotting = 4, reg_init=1,reg_final=args.reg_final,n_iter=40)
else:
if args.count_steps: candidate_demo_names = self.count2segnames[Globals.stage]
else: candidate_demo_names = demos.keys()
from joblib import parallel
costs_names = parallel.Parallel(n_jobs=-2)(parallel.delayed(calc_seg_cost)(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names)
#costs_names = [calc_seg_cost(seg_name, xyz_new_ds, dists_new) for seg_name in candidate_demo_names]
#costs_names = [calc_seg_cost(seg_name) for seg_name in candidate_demo_names]
_, best_name = min(costs_names)
best_demo = demos[best_name]
if best_demo["done"]:
rospy.loginfo("best demo was a 'done' state")
return "done"
best_demo = demos[best_name]
rospy.loginfo("best segment name: %s", best_name)
xyz_demo_ds = best_demo["cloud_xyz_ds"]
if args.test: n_iter = 21
else: n_iter = 101
if args.use_rigid:
self.f = registration.Translation2d()
self.f.fit(xyz_demo_ds, xyz_new_ds)
else:
self.f = registration.tps_rpm(xyz_demo_ds, xyz_new_ds, plotting = 20, reg_init=1,reg_final=.01,n_iter=n_iter,verbose=False)#, interactive=True)
np.savez('registration_data', xyz_demo_ds=xyz_demo_ds, xyz_new_ds=xyz_new_ds)
# print 'correspondences', self.f.corr_nm
#################### Generate new trajectory ##################
#### Plot original and warped point clouds #######
# orig_pose_array = conversions.array_to_pose_array(np.squeeze(best_demo["cloud_xyz_ds"]), "base_footprint")
# warped_pose_array = conversions.array_to_pose_array(self.f.transform_points(np.squeeze(best_demo["cloud_xyz_ds"])), "base_footprint")
# Globals.handles.append(Globals.rviz.draw_curve(orig_pose_array,rgba=(1,0,0,1),id=19024,type=Marker.CUBE_LIST))
# Globals.handles.append(Globals.rviz.draw_curve(warped_pose_array,rgba=(0,1,0,1),id=2983,type=Marker.CUBE_LIST))
#### Plot grid ########
mins = np.squeeze(best_demo["cloud_xyz"]).min(axis=0)
maxes = np.squeeze(best_demo["cloud_xyz"]).max(axis=0)
mins[2] -= .1
maxes[2] += .1
grid_handle = warping.draw_grid(Globals.rviz, self.f.transform_points, mins, maxes, 'base_footprint')
Globals.handles.append(grid_handle)
#### Actually generate the trajectory ###########
warped_demo = warping.transform_demo_with_fingertips(self.f, best_demo)
if yes_or_no('dump warped demo?'):
import pickle
fname = '/tmp/warped_demo_' + str(np.random.randint(9999999999)) + '.pkl'
with open(fname, 'w') as f:
pickle.dump(warped_demo, f)
print 'saved to', fname
Globals.pr2.update_rave()
trajectory = {}
# calculate joint trajectory using IK
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
if args.hard_table:
clipinplace(warped_demo["l_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
clipinplace(warped_demo["r_gripper_tool_frame"]["position"][:,2],Globals.table_height+.032,np.inf)
arm_traj, feas_inds = lfd_traj.make_joint_traj_by_graph_search(
warped_demo["%s_gripper_tool_frame"%lr]["position"],
warped_demo["%s_gripper_tool_frame"%lr]["orientation"],
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr,
check_collisions=True
)
if len(feas_inds) == 0: return "failure"
trajectory["%s_arm"%lr] = arm_traj
trajectory["%s_grab"%lr] = best_demo["%s_gripper_joint"%lr] < .07
trajectory["%s_gripper"%lr] = warped_demo["%s_gripper_joint"%lr]
trajectory["%s_gripper"%lr][trajectory["%s_grab"%lr]] = 0
# smooth any discontinuities in the arm traj
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
trajectory["%s_arm"%lr], discont_times, n_steps = lfd_traj.smooth_disconts(
trajectory["%s_arm"%lr],
Globals.pr2.env,
Globals.pr2.robot.GetManipulator("%sarm"%leftright),
"%s_gripper_tool_frame"%lr
)
# after smoothing the arm traj, we need to fill in all other trajectories (in both arms)
other_lr = 'r' if lr == 'l' else 'l'
if best_demo["arms_used"] in [other_lr, "b"]:
trajectory["%s_arm"%other_lr] = lfd_traj.fill_stationary(trajectory["%s_arm"%other_lr], discont_times, n_steps)
for tmp_lr in 'lr':
if best_demo["arms_used"] in [tmp_lr, "b"]:
trajectory["%s_grab"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_grab"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr] = lfd_traj.fill_stationary(trajectory["%s_gripper"%tmp_lr], discont_times, n_steps)
trajectory["%s_gripper"%tmp_lr][trajectory["%s_grab"%tmp_lr]] = 0
# plotting
for lr in "lr":
leftright = {"l":"left","r":"right"}[lr]
if best_demo["arms_used"] in [lr, "b"]:
# plot warped trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(warped_demo["%s_gripper_l_finger_tip_link"%lr]["position"], warped_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (1,0,1,.4), type=Marker.LINE_LIST,
ns='warped_finger_traj'
))
# plot original trajectory
Globals.handles.append(Globals.rviz.draw_curve(
conversions.array_to_pose_array(
alternate(best_demo["%s_gripper_l_finger_tip_link"%lr]["position"], best_demo["%s_gripper_r_finger_tip_link"%lr]["position"]),
"base_footprint"
),
width=.001, rgba = (0,1,1,.4), type=Marker.LINE_LIST,
ns='demo_finger_traj'
))
userdata.trajectory = trajectory
if args.prompt_before_motion:
consent = yes_or_no("trajectory ok?")
else:
consent = True
if consent: return "not_done"
else: return "failure"
def exec_traj_do_work(l_gripper_poses,
l_gripper_angles,
r_gripper_poses,
r_gripper_angles,
traj_ik_func,
obj_cloud_xyz,
obj_name,
can_move_lower=True):
del Globals.handles[1:]
grab_obj_kinbody = setup_obj_rave(
obj_cloud_xyz, obj_name) if obj_cloud_xyz is not None else None
body_traj = {}
for (lr, gripper_poses,
gripper_angles) in zip("lr", [l_gripper_poses, r_gripper_poses],
[l_gripper_angles, r_gripper_angles]):
if len(gripper_poses) == 0: continue
gripper_poses, gripper_angles = np.array(gripper_poses), np.array(
gripper_angles)
gripper_angles_grabbing = process_gripper_angles_for_grabbing(
lr, gripper_angles)
Globals.pr2.update_rave()
current_pos = Globals.pr2.robot.GetLink("%s_gripper_tool_frame" %
lr).GetTransform()
if can_move_lower:
gripper_poses_remove, gripper_angles_remove = gripper_poses, gripper_angles_grabbing
else:
gripper_poses_remove, gripper_angles_remove = remove_lower_poses(
current_pos, gripper_poses, gripper_angles_grabbing)
gripper_poses_prepend, gripper_angles_prepend = prepend_path_to_start(
current_pos, gripper_poses_remove, gripper_angles_remove, 15)
final_gripper_poses, final_gripper_angles = gripper_poses_prepend, gripper_angles_prepend
#do ik
joint_positions = traj_ik_func(Globals.pr2, lr, final_gripper_poses)
if len(joint_positions) == 0:
return ExecTrajectoryResponse(success=False)
unwrapped_joint_positions = unwrap_angles(joint_positions)
final_joint_positions = unwrapped_joint_positions
body_traj["%s_arm" % lr] = final_joint_positions
body_traj["%s_gripper" % lr] = final_gripper_angles
yn = yes_or_no("continue?")
if yn:
lt.follow_trajectory_with_grabs(Globals.pr2, body_traj)
if grab_obj_kinbody is not None:
handle_grab_or_release_obj(grab_obj_kinbody, l_gripper_poses,
l_gripper_angles, r_gripper_poses,
r_gripper_angles)
raw_input("Press enter when done viewing trajectory")
Globals.pr2.join_all()
return ExecTrajectoryResponse(success=True)
else:
return ExecTrajectoryResponse(success=False)
for obj_name in demo_info["args"]:
print "select the %s" % obj_name
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in=pc_tf)).cloud_out
make_req.object_clouds.append(pc_sel)
else:
object_clouds = [
demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in demo_data["object_clouds"].keys()
]
for i in xrange(len(object_clouds)):
cloud = object_clouds[i].reshape(-1, 3)
#translation = np.random.randn(1,3) * np.array([[.1,.1,0]])
#cloud += translation
center = cloud.mean(axis=0)
from numpy import cos, sin, pi
rotation = np.array([[cos(pi / 4), sin(pi / 4), 0],
[-sin(pi / 4), cos(pi / 4), 0], [0, 0, 1]])
cloud = center[None, :] + np.dot(cloud - center[None, :], rotation.T)
make_req.object_clouds.append(ru.xyz2pc(cloud, 'base_footprint'))
make_resp = make_verb_traj.make_traj(make_req)
yn = yes_or_no("continue?")
if yn:
#exec_verb_traj.Globals.pr2.rarm.vel_limits *= .5
#exec_verb_traj.Globals.pr2.larm.vel_limits *= .5
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req)
import os.path as osp
from jds_utils.yes_or_no import yes_or_no
import os, sys
import yaml
import h5py
data_dir = osp.join(osp.dirname(lfd.__file__), "data")
task_file = osp.join(data_dir, "knot_demos.yaml")
with open(osp.join(data_dir,task_file),"r") as fh:
task_info = yaml.load(fh)
db_file = osp.join(data_dir, task_info[args.task]["db_file"])
print 'Writing to', db_file
if osp.exists(db_file):
if yes_or_no(db_file + ' already exists. Overwrite?'):
os.remove(db_file)
else:
print 'Aborting.'
sys.exit(1)
task_lib = h5py.File(db_file, mode="w")
for (i_demo,bag_file) in enumerate(task_info[args.task]["demo_bags"]):
bag = rosbag.Bag(bag_file)
demo_segs = bag_proc.create_segments(bag, ["r_gripper_tool_frame", "r_gripper_l_finger_tip_link", "r_gripper_r_finger_tip_link",
"l_gripper_tool_frame", "l_gripper_l_finger_tip_link", "l_gripper_r_finger_tip_link"])
for (i_seg,demo_seg) in enumerate(demo_segs):
if demo_seg["done"]:
pc = rospy.wait_for_message("/drop/points", sm.PointCloud2)
pc_tf = ru.transformPointCloud2(pc, exec_verb_traj.Globals.pr2.tf_listener, "base_footprint", pc.header.frame_id)
for obj_name in demo_info["args"]:
print "select the %s"%obj_name
pc_sel = seg_svc.call(ProcessCloudRequest(cloud_in = pc_tf)).cloud_out
make_req.object_clouds.append(pc_sel)
else:
object_clouds = [demo_data["object_clouds"][obj_name]["xyz"]
for obj_name in demo_data["object_clouds"].keys()]
for i in xrange(len(object_clouds)):
cloud = object_clouds[i].reshape(-1,3)
#translation = np.random.randn(1,3) * np.array([[.1,.1,0]])
#cloud += translation
center = cloud.mean(axis=0)
from numpy import cos,sin,pi
rotation = np.array([[cos(pi/4), sin(pi/4), 0], [-sin(pi/4), cos(pi/4), 0], [0, 0, 1]])
cloud = center[None,:] + np.dot(cloud - center[None,:], rotation.T)
make_req.object_clouds.append(ru.xyz2pc(cloud,'base_footprint'))
make_resp = make_verb_traj.make_traj(make_req)
yn = yes_or_no("continue?")
if yn:
#exec_verb_traj.Globals.pr2.rarm.vel_limits *= .5
#exec_verb_traj.Globals.pr2.larm.vel_limits *= .5
exec_req = ExecTrajectoryRequest()
exec_req.traj = make_resp.traj
exec_verb_traj.exec_traj(exec_req)
def create_segments(bag, link_names):
"""extract a bunch of dictionaries from bag file, each one corresponding to one segment of trajectory.
each segment corresponds to 'look', 'start', and 'stop' button presses"""
button_presses = get_button_presses(bag)
all_times = read_button_presses(button_presses)
print 'Events in bag file:', actions_to_str(all_times)
if yes_or_no('edit events?'):
s = raw_input('new action sequence (must be the same length as the original): ')
#print all_times
all_times = str_to_actions(s, [t for t, _ in all_times])
#print all_times
start_times, stop_times, look_times, l_close_times, l_open_times, r_close_times, r_open_times, done_times = \
correct_bag_errors(all_times)
assert len(start_times) == len(stop_times) == len(look_times)
assert len(done_times) == 1
kinematics_info = extract_kinematics_from_bag(bag, link_names)
N = len(kinematics_info["time"])
l_open = np.zeros(N, bool)
l_close = np.zeros(N, bool)
r_open = np.zeros(N, bool)
r_close = np.zeros(N, bool)
times = kinematics_info["time"]
l_open[np.searchsorted(times, l_open_times)] = True
l_close[np.searchsorted(times, l_close_times)] = True
r_open[np.searchsorted(times, r_open_times)] = True
r_close[np.searchsorted(times, r_close_times)] = True
kinematics_info["l_open"] = l_open
kinematics_info["l_close"] = l_close
kinematics_info["r_open"] = r_open
kinematics_info["r_close"] = r_close
start_inds = np.searchsorted(times, start_times)
stop_inds = np.searchsorted(times, stop_times)
look_clouds = get_transformed_clouds(bag, look_times)
seg_infos = []
for (start_ind, stop_ind, (xyz, bgr)) in zip(start_inds, stop_inds, look_clouds):
seg_info = extract_segment(kinematics_info, start_ind, stop_ind)
seg_info["arms_used"] = determine_arms_used(seg_info)
seg_info["cloud_xyz"] = xyz
seg_info["cloud_bgr"] = bgr
seg_info["done"] = False
seg_infos.append(seg_info)
done_clouds = get_transformed_clouds(bag, done_times)
for (xyz, bgr) in done_clouds:
seg_info = {}
seg_info["cloud_xyz"] = xyz
seg_info["cloud_bgr"] = bgr
seg_info["done"] = True
seg_infos.append(seg_info)
return seg_infos
def erase_all_data(group):
consent = yes_or_no("erase all data in %s?"%group.name)
if consent:
for name in group.keys(): erase_dataset(group, name)
else:
raise IOError
