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