def delete_demo(demo_type, demo_name): assert demo_type != '', 'DEMO TYPE needs to be valid.' assert demo_name != '', 'DEMO NAME needs to be valid.' demo_type_dir = osp.join(demo_files_dir, demo_type) demo_dir= osp.join(demo_type_dir, demo_name) if not osp.exists(demo_dir): redprint('%s does not exist! Invalid demo.'%demo_dir) return yellowprint('This cannot be undone!') if not yes_or_no('Are you still sure you want to delete %s?'%demo_name): return # If it was the last demo recorded, update the latest_demo.txt file # Otherwise don't bother latest_demo_file = osp.join(demo_type_dir, latest_demo_name) with open(latest_demo_file,'r') as fh: demo_num = int(fh.read()) if demo_name == demo_names.base_name%demo_num: with open(latest_demo_file,'w') as fh: fh.write(str(demo_num-1)) # Delete contents of demo_dir shutil.rmtree(demo_dir) # Update master_file master_file = osp.join(demo_type_dir, master_name) with open(master_file,"r") as fh: master_lines = fh.readlines() fh = open(master_file,"w") for line in master_lines: if demo_name not in line: fh.write(line) fh.close()
def load_data(data_file, lr, freq=30.0): with open(data_file, 'r') as f: dat = cp.load(f) demo_dir = osp.dirname(data_file) cam_types = get_cam_types(demo_dir) T_cam2hbase = dat['T_cam2hbase'] cam_tmin = float('inf') cam_info = {} for kname in dat[lr].keys(): if 'cam' in kname: if kname != 'camera1': continue tfs = [tt[0] for tt in dat[lr][kname]] ts = [tt[1] for tt in dat[lr][kname]] #ctype_name = int(kname[-1]) ## don't append any empty-streams: if len(ts) > 0: cam_strm = streamize(tfs, ts, freq, avg_transform)#, tstart=-1./freq) cam_info[kname] = {'type' : cam_types[kname], 'stream' : cam_strm} cam_tmin = min(ts[0], cam_tmin) ## hydra data: hydra_tfs = [tt[0] for tt in dat[lr]['hydra']] hydra_ts = np.array([tt[1] for tt in dat[lr]['hydra']]) if abs(cam_tmin - hydra_ts[0]) > 1000: hydra_ts += (cam_tmin - hydra_ts[0]) if len(hydra_ts) <= 0: redprint("ERROR : No hydra data found in : %s"%(osp.basename(data_file))) sys.exit(-1) hydra_strm = streamize(hydra_tfs, hydra_ts, freq, avg_transform)#, tstart=-1./freq) ## potentiometer angles: pot_vals = np.array([tt[0] for tt in dat[lr]['pot_angles']]) pot_ts = np.array([tt[1] for tt in dat[lr]['pot_angles']]) print cam_tmin, pot_ts[0] if abs(cam_tmin - pot_ts[0]) > 1000: pot_ts += (cam_tmin - pot_ts[0]) if len(pot_ts) <= 0: redprint("ERROR : No potentiometer data found in : %s"%(osp.basename(data_file))) sys.exit(-1) pot_strm = streamize(pot_vals, pot_ts, freq, np.mean)#, tstart=-1./freq) print hydra_ts[0], pot_ts[0] return (T_cam2hbase, cam_info, hydra_strm, pot_strm)
def align_tf_streams(hydra_strm, cam_strm, wsize=0): """ Calculates the time-offset b/w the camera and hydra transform streams. It uses a hydra-stream because it is full -- it has no missing data-points. =========================== --> If that is not the case, first fit a spline to the hydra stream and then call this function. WSIZE : is the window-size to search in : This function gives the: argmin_{s \in [-wsize, ..,0,..., wsize]} dist(hydra_stream, cam_stream(t+s)) where, dist is the euclidean norm (l2 norm). NOTE : (1) It uses only the position variables for distance calculations. (2) Further assumes that the two streams are on the same time-scale. """ Xs_hy = [] Xs_cam = [] cam_inds = [] idx = 0 ## pre-empt if the stream is small: if len(cam_strm.get_data()[0]) <= wsize: return 0 for tfm in cam_strm: if tfm != None: Xs_cam.append(tfm[0,3]) cam_inds.append(idx) idx += 1 for hydra_tfm in hydra_strm: if hydra_tfm != None: Xs_hy.append(hydra_tfm[0,3]) else: #redprint("None hydra tfm!") if len(Xs_hy) > 0: Xs_hy.append(Xs_hy[-1]) ## chop-off wsized data from the start and end of the camera-data: start_idx, end_idx = 0, len(Xs_cam)-1 while cam_inds[start_idx] < wsize and start_idx < len(Xs_cam) : start_idx += 1 while cam_inds[end_idx] >= len(Xs_hy) - wsize and end_idx >= 0: end_idx -= 1 dists = [] cam_inds = np.array(cam_inds[start_idx:end_idx+1]) Xs_cam = np.array(Xs_cam[start_idx:end_idx+1]) Xs_hy = np.array(Xs_hy) for shift in xrange(-wsize, wsize+1): dists.append(np.linalg.norm(Xs_hy[shift + cam_inds] - Xs_cam)) shift = xrange(-wsize, wsize+1)[np.argmin(dists)] redprint("\t stream time-alignment shift is : %d (= %0.3f seconds)"%(shift,hydra_strm.dt*shift)) return shift
def load_data(data_file, lr, freq=30.0, speed=1.0, hydra_only=False): """ Changed slightly from the one in demo_data_prep to include speed. """ with open(data_file, 'r') as f: dat = cp.load(f) demo_dir = osp.dirname(data_file) with open(osp.join(demo_dir, demo_names.camera_types_name),'r') as fh: cam_types = yaml.load(fh) T_cam2hbase = dat['T_cam2hbase'] if not hydra_only: cam_info = {} for kname in dat[lr].keys(): if 'cam' in kname: tfs = [tt[0] for tt in dat[lr][kname]] ts = [tt[1] for tt in dat[lr][kname]] ctype_name = int(kname[-1]) ## we will add empty stream cam_strm = streamize(tfs, ts, freq, avg_transform, speed=speed)#, tstart=-1./freq) cam_info[ctype_name] = {'type' : cam_types[ctype_name], 'stream' : cam_strm} ## hydra data: hydra_tfs = [tt[0] for tt in dat[lr]['hydra']] hydra_ts = np.array([tt[1] for tt in dat[lr]['hydra']]) if len(hydra_ts) <= 0: redprint("ERROR : No hydra data found in : %s"%(osp.basename(data_file))) sys.exit(-1) hydra_strm = streamize(hydra_tfs, hydra_ts, freq, avg_transform, speed=speed)#, tstart=-1./freq) ## potentiometer angles: pot_vals = np.array([tt[0] for tt in dat[lr]['pot_angles']]) pot_ts = np.array([tt[1] for tt in dat[lr]['pot_angles']]) if len(pot_ts) <= 0: redprint("ERROR : No potentiometer data found in : %s"%(osp.basename(data_file))) sys.exit(-1) pot_strm = streamize(pot_vals, pot_ts, freq, np.mean, speed=speed)#, tstart=-1./freq) if not hydra_only: return (T_cam2hbase, cam_info, hydra_strm, pot_strm) else: return (T_cam2hbase, hydra_strm, pot_strm)
def segment_streams(ann_dat, strms, time_shifts, demo_dir, base_stream='camera1'): """ Segments a list of streams based on the timings in the annotation file and the time-shift data. STRMS : A list of streams TIME_SHIFT : A dictionary of time-shifts applied to the streams wrt to the raw streams DEMO_DIR : The demo-directory : used to get the annotation file BASE_STRM : The stream relative to which the annotation times are specified RETURNS : a list of list of streams. len(list) == number of segments start_times: the shifted segment start times """ if base_stream not in time_shifts.keys(): redprint("Cannot segment streams. Base time shift data is missing for : %s"%base_stream) n_segs = len(ann_dat) start_times = np.array([seg_info['look']+time_shifts[base_stream] for seg_info in ann_dat]) stop_times = np.array([seg_info['stop']+time_shifts[base_stream] for seg_info in ann_dat]) nsteps = map(int, (stop_times - start_times) / strms[0].dt) print start_times strm_segs = [] for strm in strms: strm_segs.append(segment_stream(strm, start_times, stop_times)) out_segs = [] for i in xrange(n_segs): if ann_dat[i]['name'] == 'done': continue si = [] for n in xrange(len(strms)): si.append(strm_segs[n][i]) out_segs.append(si) return out_segs, nsteps, start_times
if args.prompt_delete and yes_or_no('Do you want to delete %s?'%args.demo_name): delete_demo(args.demo_type, args.demo_name) else: first = args.first last = args.last demo_type_dir = osp.join(demo_files_dir, args.demo_type) if args.last == -1: latest_demo_file = osp.join(demo_type_dir, latest_demo_name) if osp.isfile(latest_demo_file): with open(latest_demo_file,'r') as fh: last = int(fh.read()) + 1 else: redprint("No demos!") for i in xrange (first, last+1): demo_name = demo_names.base_name%i demo_dir = osp.join(demo_files_dir, args.demo_type, demo_name) if osp.exists(demo_dir): if args.prompt: raw_input('Hit enter for %s.'%demo_name) yellowprint("Visualizing: %s"%demo_name) if args.use_traj: view_tracking_on_rviz(demo_type=args.demo_type, demo_name=demo_name, tps_model_fname=args.tps_fname, freq=args.freq, speed=args.speed, use_smoother=args.use_smoother, prompt=args.prompt, verbose=args.verbose) else: if args.hydra_only:
def plan_follow_traj( robot, manip_name, ee_link, new_hmats, old_traj, rope_cloud=None, rope_constraint_thresh=0.01, end_pose_constraint=False, ): n_steps = len(new_hmats) assert old_traj.shape[0] == n_steps assert old_traj.shape[1] == 7 arm_inds = robot.GetManipulator(manip_name).GetArmIndices() ee_linkname = ee_link.GetName() init_traj = old_traj.copy() # init_traj[0] = robot.GetDOFValues(arm_inds) end_pose = openravepy.poseFromMatrix(new_hmats[-1]) request = { "basic_info": {"n_steps": n_steps, "manip": manip_name, "start_fixed": False}, "costs": [ {"type": "joint_vel", "params": {"coeffs": [n_steps / 5.0]}}, {"type": "collision", "params": {"coeffs": [1], "dist_pen": [0.01]}}, ], "constraints": [], "init_info": {"type": "given_traj", "data": [x.tolist() for x in init_traj]}, } request["costs"] += [ {"type": "collision", "name": "cont_coll", "params": {"coeffs": [50], "dist_pen": [0.05]}}, {"type": "collision", "name": "col", "params": {"continuous": False, "coeffs": [20], "dist_pen": [0.02]}}, ] # impose that the robot goes to final ee tfm at last ts # the constraint works only when the arm is the 'grasp' arm; otherwise only cost is added # if end_pose_constraint or not is_fake_motion(new_hmats, 0.1): # hack to avoid missing grasp if rope_cloud != None: closest_point = find_closest_point(rope_cloud, end_pose[4:7]) dist = np.linalg.norm(end_pose[4:7] - closest_point) if dist > rope_constraint_thresh and dist < 0.05: end_pose[4:7] = closest_point redprint("grasp hack is active, dist = %f" % dist) else: blueprint("grasp hack is inactive, dist = %f" % dist) # raw_input() request["constraints"] += [ { "type": "pose", "params": { "xyz": end_pose[4:7].tolist(), "wxyz": end_pose[0:4].tolist(), "link": ee_linkname, "pos_coeffs": [100, 100, 100], "rot_coeffs": [10, 10, 10], }, } ] poses = [openravepy.poseFromMatrix(hmat) for hmat in new_hmats] for (i_step, pose) in enumerate(poses): request["costs"].append( { "type": "pose", "params": { "xyz": pose[4:7].tolist(), "wxyz": pose[0:4].tolist(), "link": ee_linkname, "timestep": i_step, "pos_coeffs": [100, 100, 100], "rot_coeffs": [10, 10, 10], }, } ) s = json.dumps(request) prob = trajoptpy.ConstructProblem(s, robot.GetEnv()) # create object that stores optimization problem result = trajoptpy.OptimizeProblem(prob) # do optimization traj = result.GetTraj() saver = openravepy.RobotStateSaver(robot) pos_errs = [] with saver: for i_step in xrange(1, n_steps): row = traj[i_step] robot.SetDOFValues(row, arm_inds) tf = ee_link.GetTransform() pos = tf[:3, 3] pos_err = np.linalg.norm(poses[i_step][4:7] - pos) pos_errs.append(pos_err) pos_errs = np.array(pos_errs) print "planned trajectory for %s. max position error: %.3f. all position errors: %s" % ( manip_name, pos_errs.max(), pos_errs, ) return traj
def plan_fullbody( robot, env, new_hmats_by_bodypart, old_traj_by_bodypart, end_pose_constraints, rope_cloud=None, rope_constraint_thresh=0.01, allow_base=True, init_data=None, ): collision_free = True for part_name in new_hmats_by_bodypart: n_steps = len(new_hmats_by_bodypart[part_name]) break state = np.random.RandomState(None) init_dofs = [] if "base" in old_traj_by_bodypart.keys(): if "rightarm" in old_traj_by_bodypart.keys(): init_dofs += old_traj_by_bodypart["rightarm"].tolist() else: init_dofs += robot.GetDOFValues(robot.GetManipulator("rightarm").GetArmIndices()).tolist() if "leftarm" in old_traj_by_bodypart.keys(): init_dofs += old_traj_by_bodypart["leftarm"].tolist() else: init_dofs += robot.GetDOFValues(robot.GetManipulator("leftarm").GetArmIndices()).tolist() init_dofs += old_traj_by_bodypart["base"] else: arm_dofs = [] if "rightarm" in old_traj_by_bodypart.keys(): arm_dofs = old_traj_by_bodypart["rightarm"] if "leftarm" in old_traj_by_bodypart.keys(): arm_dofs = np.c_[arm_dofs, old_traj_by_bodypart["leftarm"]] init_dofs = arm_dofs if allow_base: x, y, rot = mat_to_base_pose(robot.GetTransform()) base_dofs = np.c_[x, y, rot] init_dofs = np.c_[init_dofs, np.repeat(base_dofs, init_dofs.shape[0], axis=0)] init_dofs = init_dofs.tolist() bodyparts = new_hmats_by_bodypart.keys() if "base" in bodyparts: # problem bodyparts += ["rightarm", "leftarm"] elif allow_base: bodyparts.append("base") costs = [] constraints = [] if "base" in bodyparts: if old_traj_by_bodypart is None: raise Exception("Base motion planning must have init_dofs") n_dofs = len(init_dofs[0]) cost_coeffs = n_dofs * [5] cost_coeffs[-1] = 500 cost_coeffs[-2] = 500 cost_coeffs[-3] = 500 joint_vel_cost = {"type": "joint_vel", "params": {"coeffs": cost_coeffs}} costs.append(joint_vel_cost) else: joint_vel_cost = { "type": "joint_vel", # joint-space velocity cost "params": {"coeffs": [5]}, # a list of length one is automatically expanded to a list of length n_dofs } costs.append(joint_vel_cost) for manip, poses in new_hmats_by_bodypart.items(): if manip == "rightarm": link = "r_gripper_tool_frame" elif manip == "leftarm": link = "l_gripper_tool_frame" elif manip == "base": link = "base_footprint" if manip in ["rightarm", "leftarm"]: if not end_pose_constraints[manip] and is_fake_motion(poses, 0.1): continue if manip in ["rightarm", "leftarm"]: if end_pose_constraints[manip] or not is_fake_motion(poses, 0.1): end_pose = openravepy.poseFromMatrix(poses[-1]) if rope_cloud != None: closest_point = find_closest_point(rope_cloud, end_pose[4:7]) dist = np.linalg.norm(end_pose[4:7] - closest_point) if dist > rope_constraint_thresh and dist < 0.2: end_pose[4:7] = closest_point redprint("grasp hack is active, dist = %f" % dist) constraints.append( { "type": "pose", "params": { "xyz": end_pose[4:7].tolist(), "wxyz": end_pose[0:4].tolist(), "link": link, "pos_coeffs": [10, 10, 10], "rot_coeffs": [10, 10, 10], }, } ) poses = [openravepy.poseFromMatrix(hmat) for hmat in poses] for t, pose in enumerate(poses): costs.append( { "type": "pose", "params": { "xyz": pose[4:].tolist(), "wxyz": pose[:4].tolist(), "link": link, "pos_coeffs": [20, 20, 20], "rot_coeffs": [20, 20, 20], "timestep": t, }, } ) traj, total_cost = generate_traj(robot, env, costs, constraints, n_steps, init_dofs, collision_free) bodypart_traj = traj_to_bodypart_traj(traj, bodyparts) # Do multi init if base planning if "base" in bodypart_traj and not allow_base: waypoint_step = (n_steps - 1) // 2 env_min, env_max = get_environment_limits(env, robot) current_dofs = robot.GetActiveDOFValues().tolist() waypoint_dofs = list(current_dofs) MAX_INITS = 10 n = 0 while True: collisions = get_traj_collisions(bodypart_traj, robot) if collisions == []: break n += 1 if n > MAX_INITS: print "Max base multi init limit hit!" return bodypart_traj, total_cost # return arbitrary traj with collisions print "Base planning failed! Trying random init. Iteration {} of {}".format(n, MAX_INITS) # randomly sample x, y within env limits + some padding to accommodate robot padding = 5.0 env_min_x = env_min[0] - padding env_min_y = env_min[1] - padding env_max_x = env_max[0] + padding env_max_y = env_max[1] + padding waypoint_x = state.uniform(env_min_x, env_max_x) waypoint_y = state.uniform(env_min_y, env_max_y) waypoint_dofs[-3] = waypoint_x waypoint_dofs[-2] = waypoint_y init_data = np.empty((n_steps, robot.GetActiveDOF())) init_data[: waypoint_step + 1] = mu.linspace2d(current_dofs, waypoint_dofs, waypoint_step + 1) init_data[waypoint_step:] = mu.linspace2d(waypoint_dofs, init_dofs, n_steps - waypoint_step) traj, total_cost = generate_traj(robot, env, costs, constraints, n_steps, collision_free, init_dofs, init_data) bodypart_traj = traj_to_bodypart_traj(traj, bodyparts) return bodypart_traj, total_cost