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 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)
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 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/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?")
import h5py
import numpy as np
from utils.yes_or_no import yes_or_no
from utils import conversions
import sys
import openravepy
import rosbag
if exists(args.h5file): filemode = "r+"
else: filemode = "w"
h5file = h5py.File(args.h5file,filemode)
if args.h5path in h5file:
consent = yes_or_no("a group named %s already exists. delete it?"%args.h5path)
if consent: del h5file[args.h5path]
else: raise IOError
traj = h5file.create_group(args.h5path)
#traj["verb"] = "VERB"
#traj["verb_args"] = ["arg0", "arg1", "arg2"]
class RosToRave(object):
def __init__(self):
self.env = openravepy.Environment()
self.env.Load("robots/pr2-beta-static.zae")
self.robot = self.env.GetRobot("pr2")
self.initialized = False
def init(self, joint_msg):
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
