def extract_red_alphashape(cloud, robot): """ extract red, get alpha shape, downsample """ raise NotImplementedError # downsample cloud cloud_ds = cloudprocpy.downsampleCloud(cloud, .01) # transform into body frame xyz1_kinect = cloud_ds.to2dArray() xyz1_kinect[:,3] = 1 T_w_k = berkeley_pr2.get_kinect_transform(robot) xyz1_robot = xyz1_kinect.dot(T_w_k.T) # compute 2D alpha shape xyz1_robot_flat = xyz1_robot.copy() xyz1_robot_flat[:,2] = 0 # set z coordinates to zero xyz1_robot_flatalphashape = cloudprocpy.computeAlphaShape(xyz1_robot_flat) # unfortunately pcl alpha shape func throws out the indices, so we have to use nearest neighbor search cloud_robot_flatalphashape = cloudprocpy.CloudXYZ() cloud_robot_flatalphashape.from2dArray(xyz1_robot_flatalphashape) cloud_robot_flat = cloudprocpy.CloudXYZ() cloud_robot_flat.from2dArray(xyz1_robot_flat) alpha_inds = cloudprocpy.getNearestNeighborIndices(xyz1_robot_flatalphashape, xyz1_robot_flat) xyz_robot_alphashape = xyz1_robot_flatalphashape[:,:3] # put back z coordinate xyz_robot_alphashape[:,2] = xyz1_robot[alpha_inds,2] return xyz_robot_alphashape[:,:3]
def get_PC (self): r, d = self.grabber.getRGBD() x = clouds.depth_to_xyz(d, berkeley_pr2.f) if Globals.pr2 is not None: Globals.pr2.update_rave() tfm = berkeley_pr2.get_kinect_transform(Globals.robot) x = x.dot(tfm[:3,:3].T) + tfm[:3,3][None,None,:] return ru.xyzrgb2pc(x, r, 'base_footprint')
def stream_rgbd(): import rospy from rapprentice import berkeley_pr2, PR2 rospy.init_node("tracker", disable_signals=True) pr2 = PR2.PR2() grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() while True: print 'Grabbing cloud...' rgb, depth = grabber.getRGBD() pr2.update_rave() T_w_k = berkeley_pr2.get_kinect_transform(pr2.robot) print 'done' yield rgb, depth, T_w_k
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 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 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)
def main(): import IPython demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 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) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: fake_seg = demofile[args.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6]) hmat[:3,3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) else: #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) print "got new xyz" redprint(new_xyz) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth) np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s"%(seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name) ################################ ### Old end effector forces at r_gripper_tool_frame (eliminating the torques for now) old_eefs = seg_info['end_effector_forces'][:,0:3,:] redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb, plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) old_xyz_transformed = f.transform_points(old_xyz) #handles.append(Globals.env.plot3(old_xyz_transformed,5, np.array([(0,0,1,1) for i in old_xyz_transformed]))) handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04)) link2eetraj = {} for lr in 'r': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj #print old_ee_traj old_ee_pos = old_ee_traj[:,0:3,3] #print old_ee_pos # End effector forces as oppossed to end effector trajectories dfdxs = f.compute_jacobian(old_ee_pos) new_eefs = [] for i in xrange(len(old_eefs)): dfdx = np.matrix(dfdxs[i]) old_eef = np.matrix(old_eefs[i]) new_eefs.append(dfdx * old_eef) old_eefs = asarray(old_eefs)[:,:,0] new_eefs = asarray(new_eefs)[:,:,0] force_data = {} force_data['old_eefs'] = old_eefs force_data['new_eefs'] = new_eefs force_file = open("trial.pickle", 'wa') pickle.dump(force_data, force_file) force_file.close() new_ee_traj_x = new_ee_traj miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends 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 'r': 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 #if arm_moved(old_joint_traj): NOT SURE WHY BUT THIS IS RETURNING FALSE lr2oldtraj[lr] = old_joint_traj if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, lr2oldtraj['r'].shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) # Setting robot arm to joint trajectory r_vals = lr2oldtraj['r'][i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans f_end = np.array(old_eefs[i])/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 # FIRST RESAMPLING HAPPENS HERE: JOINT_LENGTH _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) # Timesteps only, can use to inter eefs for first time #### new_eefs_segment = asarray(new_eefs[i_start:i_end+1,:]) # Extract miniseg, and re-sample if args.no_ds: new_eefs_segment_rs = new_eefs_segment else: new_eefs_segment_rs = mu.interp2d(timesteps_rs, np.arange(len(new_eefs_segment)), new_eefs_segment) ### Generate fullbody traj bodypart2traj = {} for (lr,old_joint_traj) in lr2oldtraj.items(): manip_name = {"l":"leftarm", "r":"rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame"%lr new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1] if args.no_ds: old_joint_traj_rs = old_joint_traj new_ee_traj_rs = new_ee_traj ds_inds = np.arange(0, len(new_ee_traj_rs), args.trajopt_ds) new_ee_traj_rs = new_ee_traj_rs[ds_inds] old_joint_traj_rs = old_joint_traj_rs[ds_inds] 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) new_joint_traj = mu.interp2d(np.arange(len(old_joint_traj)), np.arange(0, len(new_joint_traj) * args.trajopt_ds, args.trajopt_ds), new_joint_traj) else: old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj) new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj) 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) if args.execution: Globals.pr2.update_rave() part_name = {"l":"larm", "r":"rarm"}[lr] bodypart2traj[part_name] = new_joint_traj redprint("Joint trajectory has length: " + str(len(bodypart2traj[part_name]))) redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys())) if args.pid: if not args.fake_data_segment: # If running on PR2, add initial state as waypoint and rough interpolate # Add initial position (arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.arm_joint_names) traj_length = bodypart2traj['rarm'].shape[0] complete_joint_traj = np.zeros((traj_length+1, 7)) # To include initial state as a way point complete_joint_traj[1:,:] = bodypart2traj['rarm'] complete_joint_traj[0,:] = arm_position bodypart2traj['rarm'] = complete_joint_traj # Add initial eff J = np.matrix(np.resize(np.array(robot_states.call_return_jacobian()), (6, 7))) # Jacobian eff = robot_states.compute_end_effector_force(J, arm_effort).T eff = np.array(eff)[0] init_force = eff[:3] complete_force_traj = np.zeros((traj_length+1, 3)) complete_force_traj[1:,:] = new_eefs_segment_rs complete_force_traj[0,:] = init_force else: complete_force_traj = new_eefs_segment_rs # SECOND RESAMPLING HAPPENS HERE: JOINT VELOCITIES if args.no_ds: traj = bodypart2traj['rarm'] stamps = asarray(seg_info['stamps']) times = np.array([stamps[i_end] - stamps[i_start]]) force = complete_force_traj else: times, times_up, traj = pr2_trajectories.return_timed_traj(Globals.pr2, bodypart2traj) # use times and times_up to interpolate the second time force = mu.interp2d(times_up, times, complete_force_traj) #Globals.robot.SetDOFValues(asarray(fake_seg["joint_states"]["position"][0])) if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, traj.shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) 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))) handles.append(Globals.env.drawlinestrip(new_ee_traj_rs[:,:3,3], 2, (0,0,1,1))) # Setting robot arm to joint trajectory r_vals = traj[i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans f_end = np.array(force[i])/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() traj = np.array(np.matrix(traj).T) # 7 x n redprint(traj) traj = np.resize(traj, (1, traj.shape[1]*7)) #resize the data to 1x7n (n being the number of steps) force = np.array(np.matrix(force).T) # 3 x n force = np.resize(force, (1, force.shape[1]*3)) #resize the data to 1x3n #[traj, force, secs] traj_force_secs = np.zeros((1, traj.shape[1] + force.shape[1] + 2)) traj_force_secs[0,0:traj.shape[1]] = traj traj_force_secs[0,traj.shape[1]:traj.shape[1]+force.shape[1]] = force traj_force_secs[0,traj.shape[1]+force.shape[1]] = times[len(times)-1] traj_force_secs[0,traj.shape[1]+force.shape[1]+1] = args.use_force IPython.embed() msg = Float64MultiArray() msg.data = traj_force_secs[0].tolist() pub = rospy.Publisher("/joint_positions_forces_secs", Float64MultiArray) redprint("Press enter to start trajectory") raw_input() time.sleep(1) pub.publish(msg) time.sleep(1) else: #if not success: break if len(bodypart2traj) > 0: exec_traj_maybesim(bodypart2traj)
def main(): demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 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) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: fake_seg = demofile[args.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle( args.fake_data_transform[3:6]) hmat[:3, 3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) else: Globals.pr2.head.set_pan_tilt(0, 1.2) Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') Globals.pr2.join_all() time.sleep(.5) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR, "rgb%i.png" % LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR, "depth%i.png" % LOG_COUNT), depth) np.save(osp.join(LOG_DIR, "xyz%i.npy" % LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s" % (seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR, "neighbor%i.txt" % LOG_COUNT), "w") as fh: fh.write(seg_name) ################################ redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0))) handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1))) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f, _ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb=tpsrpm_plot_cb, plotting=5 if args.animation else 0, rot_reg=np.r_[1e-4, 1e-4, 1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) handles.extend( plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0) - np.r_[0, 0, .1], old_xyz.max(axis=0) + np.r_[0, 0, .1], xres=.1, yres=.1, zres=.04)) link2eetraj = {} for lr in 'lr': link_name = "%s_gripper_tool_frame" % lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = 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))) miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends 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 if arm_moved(old_joint_traj): lr2oldtraj[lr] = old_joint_traj if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) #### ### 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] = new_joint_traj ################################ redprint( "Executing joint trajectory for segment %s, part %i using arms '%s'" % (seg_name, i_miniseg, bodypart2traj.keys())) 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 if not success: break if len(bodypart2traj) > 0: success &= exec_traj_maybesim(bodypart2traj) if not success: break redprint("Segment %s result: %s" % (seg_name, success)) if args.fake_data_segment: break
def main(): demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 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) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: fake_seg = demofile[args.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6]) hmat[:3,3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) else: Globals.pr2.head.set_pan_tilt(0,1.2) Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') Globals.pr2.join_all() time.sleep(.5) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth) np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s"%(seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name) ################################ redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) handles.append(Globals.env.plot3(old_xyz,5, (1,0,0))) handles.append(Globals.env.plot3(new_xyz,5, (0,0,1))) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb, plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04)) link2eetraj = {} for lr in 'lr': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = 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))) miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends 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 if arm_moved(old_joint_traj): lr2oldtraj[lr] = old_joint_traj if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) #### ### 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] = new_joint_traj ################################ redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys())) 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 if not success: break if len(bodypart2traj) > 0: success &= exec_traj_maybesim(bodypart2traj) if not success: break redprint("Segment %s result: %s"%(seg_name, success)) if args.fake_data_segment: break
def main(): parser = argparse.ArgumentParser() parser.add_argument('file', type=str) parser.add_argument('--real_robot', action='store_true') parser.add_argument('--view', action='store_true') args = parser.parse_args() if args.view: print 'Opening file %s' % args.file f = h5py.File(args.file, 'r') rgbs = f['rgb'] depths = f['depth'] T_w_k = np.array(f['T_w_k']) Globals.env = openravepy.Environment() viewer = trajoptpy.GetViewer(Globals.env) num_frames = len(rgbs) for i in range(0, num_frames, 10): print 'Showing frame %d/%d' % (i + 1, num_frames) rgb, depth = rgbs[i], depths[i] # XYZ_k = clouds.depth_to_xyz(depth, berkeley_pr2.f) # XYZ_w = XYZ_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:] # handle = Globals.env.plot3(XYZ_w.reshape(-1,3), 2, rgb.reshape(-1,3)[:,::-1]/255.) xyz = clouds.extract_color(rgb, depth, T_w_k, clouds.yellow_mask) handles = [] handles.append(Globals.env.plot3(xyz, 2, (1, 0, 0))) draw_ax(T_w_k, .1, Globals.env, handles) viewer.Idle() return if args.real_robot: import rospy rospy.init_node("recorder", disable_signals=True) Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot Globals.pr2.update_rave() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) else: Globals.env = openravepy.Environment() T_w_k = np.eye(4) viewer = trajoptpy.GetViewer(Globals.env) #env.LoadData(make_table_xml(translation=[0, 0, -.05], extents=[1, 1, .05])) num_frames = 0 rgbs, depths = [], [] subprocess.call("killall XnSensorServer", shell=True) grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() try: while True: rgb, depth = grabber.getRGBD() rgbs.append(rgb) depths.append(depth) cv2.imshow("rgb", rgb) cv2.imshow("depth", cmap[np.fmin((depth * .064).astype('int'), 255)]) cv2.waitKey(30) num_frames += 1 print 'Captured frame', num_frames viewer.Step() except KeyboardInterrupt: print 'Keyboard interrupt' print 'Writing to %s ...' % args.file out = h5py.File(args.file, 'w') out.create_dataset('rgb', data=rgbs, compression='gzip', chunks=(1, 256, 256, 3)) out.create_dataset('depth', data=depths, compression='gzip', chunks=(1, 256, 256)) out['T_w_k'] = T_w_k out.close()
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
return calculate_depth_error(*vec2args(x)) def vec2args(x): return x[0:3], x[3:6], x[6] def args2vec(xyz, rod, f): out = np.empty(7) out[0:3] = xyz out[3:6] = rod out[6] = f return out T_w_k = berkeley_pr2.get_kinect_transform(robot) #o = T_w_k[:3,3] #x = T_w_k[:3,0] #y = T_w_k[:3,1] #z = T_w_k[:3,2] #handles = [] #handles.append(env.drawarrow(o, o+.2*x, .005,(1,0,0,1))) #handles.append(env.drawarrow(o, o+.2*y, .005,(0,1,0,1))) #handles.append(env.drawarrow(o, o+.2*z, .005,(0,0,1,1))) #viewer.Idle() T_h_k_init = berkeley_pr2.T_h_k xyz_init = T_h_k_init[:3, 3] rod_init = openravepy.axisAngleFromRotationMatrix(T_h_k_init[:3, :3])
#print "press key to continue" cv2.waitKey(10) def calc_error_wrapper(x): return calculate_depth_error(*vec2args(x)) def vec2args(x): return x[0:3], x[3:6], x[6] def args2vec(xyz, rod, f): out = np.empty(7) out[0:3] = xyz out[3:6] = rod out[6] = f return out T_w_k = berkeley_pr2.get_kinect_transform(robot) #o = T_w_k[:3,3] #x = T_w_k[:3,0] #y = T_w_k[:3,1] #z = T_w_k[:3,2] #handles = [] #handles.append(env.drawarrow(o, o+.2*x, .005,(1,0,0,1))) #handles.append(env.drawarrow(o, o+.2*y, .005,(0,1,0,1))) #handles.append(env.drawarrow(o, o+.2*z, .005,(0,0,1,1))) #viewer.Idle() T_h_k_init = berkeley_pr2.T_h_k xyz_init = T_h_k_init[:3,3]
env = openravepy.Environment() viewer = trajoptpy.GetViewer(env) if args.ropefile is None: demo_rope_handle = env.plot3(demo_rope, 10, (0,1,1,1)) perturbed_rope_handle = env.drawlinestrip(perturbed_rope, 10, (1,0,1,1)) else: perturbed_rope_handle = env.plot3(perturbed_rope, 16, (0,0,0,1)) try: grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() while True: rgb, depth = grabber.getRGBD() XYZ_k = clouds.depth_to_xyz(depth, berkeley_pr2.f) Twk = berkeley_pr2.get_kinect_transform(pr2.robot) XYZ_w = XYZ_k.dot(Twk[:3,:3].T) + Twk[:3,3][None,None,:] cloud_handle = env.plot3(XYZ_w.reshape(-1,3), 2, rgb.reshape(-1,3)[:,::-1]/255.) viewer.Step() except: import traceback traceback.print_exc() finally: grabber.stop()
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) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: new_xyz = np.squeeze(demofile[args.fake_data_segment]["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6]) hmat[:3,3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] else: Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') Globals.pr2.join_all() Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) ################################ redprint("Finding closest demonstration") if args.fake_data_segment: seg_name = args.fake_data_segment else: seg_name = find_closest(demofile, new_xyz) seg_info = demofile[seg_name] # redprint("using demo %s, description: %s"%(seg_name, seg_info["description"])) ################################ redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) handles.append(Globals.env.plot3(old_xyz,5, (1,0,0,1))) handles.append(Globals.env.plot3(new_xyz,5, (0,0,1,1))) f = registration.tps_rpm(old_xyz, new_xyz, plot_cb = tpsrpm_plot_cb,plotting=1) #f = registration.ThinPlateSpline() XXX XXX 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)) link2eetraj = {} for lr in 'lr': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = 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))) # TODO plot # plot_warping_and_trajectories(f, old_xyz, new_xyz, old_ee_traj, new_ee_traj) miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends 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)) bodypart2traj = {} arms_used = "" for lr in 'lr': manip_name = {"l":"leftarm", "r":"rightarm"}[lr] old_joint_traj = asarray(seg_info[manip_name][i_start:i_end+1]) if arm_moved(old_joint_traj): ee_link_name = "%s_gripper_tool_frame"%lr new_ee_traj = link2eetraj[ee_link_name] if args.execution: Globals.pr2.update_rave() new_joint_traj = plan_follow_traj(Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj[i_start:i_end+1], old_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 ################################ 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) # TODO measure failure condtions if not success: break redprint("Segment %s result: %s"%(seg_name, success)) if args.fake_data_segment: break
def main(): import IPython demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 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) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: fake_seg = demofile[args.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(args.fake_data_transform[3:6]) hmat[:3,3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) else: #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) print "got new xyz" redprint(new_xyz) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth) np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s"%(seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name) ################################ ### Old end effector forces at r_gripper_tool_frame (including torques) old_forces = seg_info['end_effector_forces'][:,0:3,:] old_torques = seg_info['end_effector_forces'][:,3:6,:] redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb, plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) old_xyz_transformed = f.transform_points(old_xyz) #handles.append(Globals.env.plot3(old_xyz_transformed,5, np.array([(0,0,1,1) for i in old_xyz_transformed]))) handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04)) link2eetraj = {} for lr in 'r': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj #print old_ee_traj old_ee_pos = old_ee_traj[:,0:3,3] #print old_ee_pos # End effector forces as oppossed to end effector trajectories dfdxs = f.compute_jacobian(old_ee_pos) old_eefs = [] new_eefs = [] for i in xrange(len(old_forces)): dfdx = np.matrix(dfdxs[i]) old_force = np.matrix(old_forces[i]) old_torque = np.matrix(old_torques[i]) new_force = (dfdx * old_force) new_torque = (dfdx * old_torque) old_eefs.append(np.vstack((old_force, old_torque))) new_eefs.append(np.vstack((new_force, new_torque))) old_eefs = np.asarray(old_eefs)[:,:,0] new_eefs = np.asarray(new_eefs)[:,:,0] force_data = {} force_data['old_eefs'] = old_eefs force_data['new_eefs'] = new_eefs force_file = open("trial.pickle", 'wa') pickle.dump(force_data, force_file) force_file.close() new_ee_traj_x = new_ee_traj miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends 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 'r': 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 #if arm_moved(old_joint_traj): NOT SURE WHY BUT THIS IS RETURNING FALSE lr2oldtraj[lr] = old_joint_traj if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, lr2oldtraj['r'].shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) # Setting robot arm to joint trajectory r_vals = lr2oldtraj['r'][i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans #IPython.embed() f_end = np.array(old_forces[i].T)[0]/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 # FIRST RESAMPLING HAPPENS HERE: JOINT_LENGTH _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) # Timesteps only, can use to inter eefs for first time #### new_eefs_segment = asarray(new_eefs[i_start:i_end+1,:]) # Extract miniseg, and re-sample if args.no_ds: new_eefs_segment_rs = new_eefs_segment else: new_eefs_segment_rs = mu.interp2d(timesteps_rs, np.arange(len(new_eefs_segment)), new_eefs_segment) ### Generate fullbody traj bodypart2traj = {} for (lr,old_joint_traj) in lr2oldtraj.items(): manip_name = {"l":"leftarm", "r":"rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame"%lr new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1] if args.no_ds: old_joint_traj_rs = old_joint_traj new_ee_traj_rs = new_ee_traj ds_inds = np.arange(0, len(new_ee_traj_rs), args.trajopt_ds) new_ee_traj_rs = new_ee_traj_rs[ds_inds] old_joint_traj_rs = old_joint_traj_rs[ds_inds] 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) new_joint_traj = mu.interp2d(np.arange(len(old_joint_traj)), np.arange(0, len(new_joint_traj) * args.trajopt_ds, args.trajopt_ds), new_joint_traj) else: old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj) new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj) 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) if args.execution: Globals.pr2.update_rave() part_name = {"l":"larm", "r":"rarm"}[lr] bodypart2traj[part_name] = new_joint_traj redprint("Joint trajectory has length: " + str(len(bodypart2traj[part_name]))) redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys())) redprint("Press enter to set gripper") raw_input() #for lr in 'r': # set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start])) if args.pid: if not args.fake_data_segment: # If running on PR2, add initial state as waypoint and rough interpolate # Add initial position (arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.arm_joint_names) traj_length = bodypart2traj['rarm'].shape[0] complete_joint_traj = np.zeros((traj_length+1, 7)) # To include initial state as a way point complete_joint_traj[1:,:] = bodypart2traj['rarm'] complete_joint_traj[0,:] = arm_position bodypart2traj['rarm'] = complete_joint_traj # Add initial eff J = np.matrix(np.resize(np.array(robot_states.call_return_jacobian()), (6, 7))) # Jacobian eff = robot_states.compute_end_effector_force(J, arm_effort).T eff = np.array(eff)[0] init_force = eff[:3] complete_force_traj = np.zeros((traj_length+1, 3)) complete_force_traj[1:,:] = new_eefs_segment_rs complete_force_traj[0,:] = init_force else: complete_force_traj = new_eefs_segment_rs # SECOND RESAMPLING HAPPENS HERE: JOINT VELOCITIES if args.no_ds: traj = bodypart2traj['rarm'] stamps = asarray(seg_info['stamps']) times = np.array([stamps[i_end] - stamps[i_start]]) F = complete_force_traj else: times, times_up, traj = pr2_trajectories.return_timed_traj(Globals.pr2, bodypart2traj) # use times and times_up to interpolate the second time F = mu.interp2d(times_up, times, complete_force_traj) #Globals.robot.SetDOFValues(asarray(fake_seg["joint_states"]["position"][0])) if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, traj.shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) 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))) handles.append(Globals.env.drawlinestrip(new_ee_traj_rs[:,:3,3], 2, (0,0,1,1))) # Setting robot arm to joint trajectory r_vals = traj[i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans f_end = np.array(old_forces[i].T)[0]/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() qs = np.array(np.matrix(traj).T) # 7 x n qs = np.resize(traj, (1, qs.shape[1]*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.resize(F, (1, F.shape[1]*6))[0] #resize the data to 1x3n # Controller code allCs = [] x = np.ones(6) * 1 v = np.ones(6) * 1e-3 a = np.ones(6) * 1e-6 Ct = np.diag(np.hstack((x, v, a))) num_steps = i_end - i_start + 1 #need to figure out the stamps correctly stamps = asarray(seg_info['stamps'][i_start:i_end+1]) for t in range(num_steps-1, -1, -1): allCs.append(Ct) m = np.ones(6) Kps = [] Kvs = [] Ks = [] Qt = None Vs = [] for t in range(num_steps-1, -1, -1): if Qt is None: Qt = allCs[t] else: Ft = np.zeros((12, 18)) for j in range(12): Ft[j][j] = 1.0 deltat = abs(stamps[t+1] - stamps[t]) #print(deltat) for j in range(6): Ft[j][j+6] = deltat for j in range(6): Ft[j+6][j+12] = deltat/m[j] for j in range(6): Ft[j][j+12] = ((deltat)**2)/m[j] Qt = allCs[t] + (np.transpose(Ft).dot(Vs[num_steps-2-t]).dot(Ft)) Qxx = Qt[0:12, 0:12] Quu = Qt[12:18, 12:18] Qxu = Qt[0:12, 12:18] Qux = Qt[12:18, 0:12] Quuinv = np.linalg.inv(Quu) Vt = Qxx - Qxu.dot(Quuinv).dot(Qux) Kt = -1*(Quuinv.dot(Qux)) Ks.append(Kt) Kps.append(Kt[:, 0:6]) Kvs.append(Kt[:, 6:12]) Vs.append(Vt) Ks = Ks[::-1] Kps = Kps[::-1] Kvs = Kvs[::-1] JKpJ = [] JKvJ = [] 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])) JacobianOlds = asarray(seg_info["jacobians"][i_start:i_end+1]) for i in range(i_end- i_start + 1): J = JacobianOlds[i] psuedoJ = np.linalg.inv(np.transpose(J).dot(J)).dot(np.transpose(J)) #JKpJ.append(np.transpose(J).dot(Kps[i]).dot(J) + toAddJkpJ*0.001) #JKvJ.append(np.transpose(J).dot(Kvs[i]).dot(J) + toAddJkvJ*0.001) JKpJ.append(psuedoJ.dot(Kps[i]).dot(J) + toAddJkpJ*0.001) JKvJ.append(psuedoJ.dot(Kvs[i]).dot(J) + toAddJkvJ*0.001) if args.useJK: Kps = [] Kvs = [] for i in range(i_end- i_start + 1): Kps.append(np.zeros((6,6))) Kvs.append(np.zeros((6,6))) else: JKpJ = [] JKvJ = [] for i in range(i_end- i_start + 1): JKpJ.append(np.zeros((7,7))) JKvJ.append(np.zeros((7,7))) Kps = np.asarray(Kps) Kvs = np.asarray(Kvs) JKpJ = np.asarray(JKpJ) JKvJ = np.asarray(JKvJ) IPython.embed() # # Temp Kps and Kvs for testing """ 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])) length = complete_force_traj.shape[0] JKpJ = np.asarray([toAddJkpJ for i in range(length)]) JKpJ = np.resize(JKpJ, (1, 49*length))[0] JKvJ = np.asarray([toAddJkvJ for i in range(length)]) JKvJ = np.resize(JKvJ, (1, 49*length))[0] """ # [traj, Kp, Kv, F, use_force, seconds] Kps = np.resize(Kps, (1, 36 * num_steps))[0] Kvs = np.resize(Kvs, (1, 36 * num_steps))[0] JKpJ = np.resize(JKpJ, (1, 49 * num_steps))[0] JKvJ = np.resize(JKvJ, (1, 49 * num_steps))[0] data = np.zeros((1, len(qs) + len(JKpJ) + len(JKvJ) + len(F) + len(Kps) + len(Kvs) + 2)) data[0][0:len(qs)] = qs data[0][len(qs):len(qs)+len(JKpJ)] = JKpJ data[0][len(qs)+len(JKpJ):len(qs)+len(JKpJ)+len(JKvJ)] = JKvJ data[0][len(qs)+len(JKpJ)+len(JKvJ):len(qs)+len(JKpJ)+len(JKvJ)+len(F)] = F data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)] = Kps data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)+len(Kvs)] = Kvs data[0][-2] = args.use_force 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) else: #if not success: break if len(bodypart2traj) > 0: exec_traj_maybesim(bodypart2traj)
def main(): import IPython demofile = h5py.File(args.h5file, 'r') trajoptpy.SetInteractive(args.interactive) if args.log: LOG_DIR = osp.join(osp.expanduser("~/Data/do_task_logs"), datetime.datetime.now().strftime("%Y%m%d-%H%M%S")) os.mkdir(LOG_DIR) LOG_COUNT = 0 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.eval.fake_data_segment: grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### while True: redprint("Acquire point cloud") if args.eval.fake_data_segment: fake_seg = demofile[args.eval.fake_data_segment] new_xyz = np.squeeze(fake_seg["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle(args.eval.fake_data_transform[3:6]) hmat[:3,3] = args.eval.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3,:3].T) + hmat[:3,3][None,:] r2r = ros2rave.RosToRave(Globals.robot, asarray(fake_seg["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(fake_seg["joint_states"]["position"][0])) #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) else: #Globals.pr2.head.set_pan_tilt(0,1.2) #Globals.pr2.rarm.goto_posture('side') #Globals.pr2.larm.goto_posture('side') #Globals.pr2.join_all() #time.sleep(2) Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) print "got new xyz" redprint(new_xyz) #grab_end(new_xyz) if args.log: LOG_COUNT += 1 import cv2 cv2.imwrite(osp.join(LOG_DIR,"rgb%i.png"%LOG_COUNT), rgb) cv2.imwrite(osp.join(LOG_DIR,"depth%i.png"%LOG_COUNT), depth) np.save(osp.join(LOG_DIR,"xyz%i.npy"%LOG_COUNT), new_xyz) ################################ redprint("Finding closest demonstration") if args.select_manual: seg_name = find_closest_manual(demofile, new_xyz) else: seg_name = find_closest_auto(demofile, new_xyz) seg_info = demofile[seg_name] redprint("closest demo: %s"%(seg_name)) if "done" in seg_name: redprint("DONE!") break if args.log: with open(osp.join(LOG_DIR,"neighbor%i.txt"%LOG_COUNT),"w") as fh: fh.write(seg_name) ################################ ### Old end effector forces at r_gripper_tool_frame (including torques) old_forces = seg_info['end_effector_forces'][:,0:3,:] old_torques = seg_info['end_effector_forces'][:,3:6,:] redprint("Generating end-effector trajectory") handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) scaled_old_xyz, src_params = registration.unit_boxify(old_xyz) scaled_new_xyz, targ_params = registration.unit_boxify(new_xyz) f,_ = registration.tps_rpm_bij(scaled_old_xyz, scaled_new_xyz, plot_cb = tpsrpm_plot_cb, plotting=5 if args.animation else 0,rot_reg=np.r_[1e-4,1e-4,1e-1], n_iter=50, reg_init=10, reg_final=.1) f = registration.unscale_tps(f, src_params, targ_params) old_xyz_transformed = f.transform_points(old_xyz) #handles.append(Globals.env.plot3(old_xyz_transformed,5, np.array([(0,0,1,1) for i in old_xyz_transformed]))) handles.extend(plotting_openrave.draw_grid(Globals.env, f.transform_points, old_xyz.min(axis=0)-np.r_[0,0,.1], old_xyz.max(axis=0)+np.r_[0,0,.1], xres = .1, yres = .1, zres = .04)) link2eetraj = {} for lr in 'r': link_name = "%s_gripper_tool_frame"%lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj #print old_ee_traj old_ee_pos = old_ee_traj[:,0:3,3] #print old_ee_pos # End effector forces as oppossed to end effector trajectories dfdxs = f.compute_jacobian(old_ee_pos) old_eefs = [] new_eefs = [] for i in xrange(len(old_forces)): dfdx = np.matrix(dfdxs[i]) old_force = np.matrix(old_forces[i]) old_torque = np.matrix(old_torques[i]) new_force = (dfdx * old_force) new_torque = (dfdx * old_torque) old_eefs.append(np.vstack((old_force, old_torque))) new_eefs.append(np.vstack((new_force, new_torque))) old_eefs = np.asarray(old_eefs)[:,:,0] new_eefs = np.asarray(new_eefs)[:,:,0] force_data = {} force_data['old_eefs'] = old_eefs force_data['new_eefs'] = new_eefs force_file = open("trial.pickle", 'wa') pickle.dump(force_data, force_file) force_file.close() new_ee_traj_x = new_ee_traj miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends 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 'r': 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 #if arm_moved(old_joint_traj): NOT SURE WHY BUT THIS IS RETURNING FALSE lr2oldtraj[lr] = old_joint_traj if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, lr2oldtraj['r'].shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) handles.append(Globals.env.drawlinestrip(old_ee_traj[:,:3,3], 2, (1,0,0,1))) # Setting robot arm to joint trajectory r_vals = lr2oldtraj['r'][i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans #IPython.embed() f_end = np.array(old_forces[i].T)[0]/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Step() if len(lr2oldtraj) > 0: old_total_traj = np.concatenate(lr2oldtraj.values(), 1) JOINT_LENGTH_PER_STEP = .1 # FIRST RESAMPLING HAPPENS HERE: JOINT_LENGTH _, timesteps_rs = unif_resample(old_total_traj, JOINT_LENGTH_PER_STEP) # Timesteps only, can use to inter eefs for first time #### new_eefs_segment = asarray(new_eefs[i_start:i_end+1,:]) # Extract miniseg, and re-sample if args.no_ds: new_eefs_segment_rs = new_eefs_segment else: new_eefs_segment_rs = mu.interp2d(timesteps_rs, np.arange(len(new_eefs_segment)), new_eefs_segment) ### Generate fullbody traj bodypart2traj = {} for (lr,old_joint_traj) in lr2oldtraj.items(): manip_name = {"l":"leftarm", "r":"rightarm"}[lr] ee_link_name = "%s_gripper_tool_frame"%lr new_ee_traj = link2eetraj[ee_link_name][i_start:i_end+1] if args.no_ds: old_joint_traj_rs = old_joint_traj new_ee_traj_rs = new_ee_traj ds_inds = np.arange(0, len(new_ee_traj_rs), args.trajopt_ds) new_ee_traj_rs = new_ee_traj_rs[ds_inds] old_joint_traj_rs = old_joint_traj_rs[ds_inds] 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) new_joint_traj = mu.interp2d(np.arange(len(old_joint_traj)), np.arange(0, len(new_joint_traj) * args.trajopt_ds, args.trajopt_ds), new_joint_traj) else: old_joint_traj_rs = mu.interp2d(timesteps_rs, np.arange(len(old_joint_traj)), old_joint_traj) new_ee_traj_rs = resampling.interp_hmats(timesteps_rs, np.arange(len(new_ee_traj)), new_ee_traj) 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) if args.execution: Globals.pr2.update_rave() part_name = {"l":"larm", "r":"rarm"}[lr] bodypart2traj[part_name] = new_joint_traj redprint("Joint trajectory has length: " + str(len(bodypart2traj[part_name]))) redprint("Executing joint trajectory for segment %s, part %i using arms '%s'"%(seg_name, i_miniseg, bodypart2traj.keys())) #for lr in 'r': # set_gripper_maybesim(lr, binarize_gripper(seg_info["%s_gripper_joint"%lr][i_start])) if args.pid: # Add trajectory to arrive at to initial state redprint("Press enter to use current position") raw_input() (arm_position, arm_velocity, arm_effort) = robot_states.call_return_joint_states(robot_states.r_arm_joint_names) diff = np.array(arm_position - bodypart2traj['rarm'][0,:]) maximum = max(abs(diff)) speed = (300.0/360.0*2*(np.pi)) time_needed = maximum / speed initial_pos_traj = mu.interp2d(np.arange(0, time_needed, 0.01), np.array([0,time_needed]), np.array([arm_position, bodypart2traj['rarm'][0,:]])) # length of the intial trajectory, use this length for padding PD gains initial_traj_length = initial_pos_traj.shape[0] initial_force_traj = np.array([np.zeros(6) for i in range(initial_traj_length)]) temptraj = bodypart2traj['rarm'] bodypart2traj['rarm'] = np.concatenate((initial_pos_traj, temptraj), axis=0) complete_force_traj = np.concatenate((initial_force_traj, new_eefs), axis=0) # SECOND RESAMPLING HAPPENS HERE: JOINT VELOCITIES if args.no_ds: traj = bodypart2traj['rarm'] stamps = asarray(seg_info['stamps']) times = np.array([stamps[i_end] - stamps[i_start]]) F = complete_force_traj else: times, times_up, traj = pr2_trajectories.return_timed_traj(Globals.pr2, bodypart2traj) # use times and times_up to interpolate the second time F = mu.interp2d(times_up, times, complete_force_traj) #Globals.robot.SetDOFValues(asarray(fake_seg["joint_states"]["position"][0])) if args.visualize: r2r = ros2rave.RosToRave(Globals.robot, asarray(seg_info["joint_states"]["name"])) r2r.set_values(Globals.robot, asarray(seg_info["joint_states"]["position"][0])) for i in range(0, traj.shape[0], 10): handles = [] handles.append(Globals.env.plot3(new_xyz,5,np.array([(0,1,0,1) for x in new_xyz]))) handles.append(Globals.env.plot3(old_xyz,5,np.array([(1,0,0,1) for x in old_xyz]))) 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))) handles.append(Globals.env.drawlinestrip(new_ee_traj_rs[:,:3,3], 2, (0,0,1,1))) # Setting robot arm to joint trajectory r_vals = traj[i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) # Plotting forces from r_gripper_tool_frame hmats = Globals.robot.GetLinkTransformations() trans, rot = conv.hmat_to_trans_rot(hmats[-3]) f_start = np.array([0,0,0]) + trans f_end = np.array(old_forces[i].T)[0]/100 + trans handles.append(Globals.env.drawlinestrip(np.array([f_start, f_end]), 10, (1,0,0,1))) redprint(i) Globals.viewer.Idle() # Controller code in joint space traj_length = traj.shape[0] 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]) m = np.array([3.33, 1.16, 0.1, 0.25, 0.133, 0.0727, 0.0727]) # masses in joint space (feed forward) vel_factor = 1e-3 #new costs covariances, means = analyze_data.run_analyze_data(args) CostsNew = [] counterCovs = 0 endTraj = False covThiss = [] for covMat in covariances: covThis = np.zeros((18,18)) covThis[0:6,0:6] = covMat[0:6,0:6] covThis[12:18,12:18] = covMat[6:12, 6:12] covThis[0:6, 12:18] = covMat[0:6, 6:12] covThis[12:18, 0:6] = covMat[6:12, 0:6] covThis[6:12, 6:12] = np.eye(6)*vel_factor covThis = np.diag(np.diag(covThis)) covThiss.append(covThis) #if len(covThiss) <= 69 and len(covThiss) >= 47: # covThis[12:18,12:18] = np.diag([0.13, 0.06, 0.07, 0.005, 0.01, 0.004]) for j in range(args.eval.downsample_traj): invCov = np.linalg.inv(covThis) CostsNew.append(invCov) counterCovs = counterCovs + 1 if counterCovs >= traj_length: endTraj = True break if endTraj: break allCs = [] x = np.ones(6) * 1 v = np.ones(6) * 1e-3 a = np.ones(6) * 1e-6 Ct = np.diag(np.hstack((x, v, a))) # in end effector space Ms = [] num_steps = i_end - i_start + 1 stamps = asarray(seg_info['stamps'][i_start:i_end+1]) arm = Globals.robot.GetManipulator("rightarm") jacobians = [] for i in range(traj.shape[0]): r_vals = traj[i,:] Globals.robot.SetDOFValues(r_vals, Globals.robot.GetManipulator('rightarm').GetArmIndices()) jacobians.append(np.vstack((arm.CalculateJacobian(), arm.CalculateAngularVelocityJacobian()))) jacobians = asarray(jacobians) #jacobiansx = asarray(seg_info["jacobians"][i_start:i_end+1]) for t in range(num_steps): M = np.zeros((18, 21)) J = jacobians[t] M[0:6,0:7] = J M[6:12,7:14] = J mdiag = np.diag(m) # Convert joint feedforward values into diagonal array mdiag_inv = np.linalg.inv(mdiag) M[12:18,14:21] = np.linalg.inv((J.dot(mdiag_inv).dot(np.transpose(J)))).dot(J).dot(mdiag_inv) Ms.append(M) for t in range(num_steps): topad = np.zeros((21,21)) topad[0:7,0:7] = np.diag(pgains) * 0.1 topad[7:14,7:14] = np.diag(dgains) * 0.1 topad[14:21,14:21] = np.eye(7) * 0.1 #allCs.append(np.transpose(Ms[t]).dot(Ct).dot(Ms[t]) + topad) allCs.append(np.transpose(Ms[t]).dot(CostsNew[t]).dot(Ms[t]) + topad) Kps = [] Kvs = [] Ks = [] Qt = None Vs = [] for t in range(num_steps-1, -1, -1): if Qt is None: Qt = allCs[t] else: Ft = np.zeros((14, 21)) for j in range(14): Ft[j][j] = 1.0 deltat = abs(stamps[t+1] - stamps[t]) #print(deltat) for j in range(7): Ft[j][j+7] = deltat for j in range(7): Ft[j+7][j+14] = deltat/m[j] for j in range(7): Ft[j][j+14] = ((deltat)**2)/m[j] Qt = allCs[t] + (np.transpose(Ft).dot(Vs[num_steps-2-t]).dot(Ft)) Qxx = Qt[0:14, 0:14] Quu = Qt[14:21, 14:21] Qxu = Qt[0:14, 14:21] Qux = Qt[14:21, 0:14] Quuinv = np.linalg.inv(Quu) Vt = Qxx - Qxu.dot(Quuinv).dot(Qux) Vt = 0.5*(Vt + np.transpose(Vt)) Kt = -1*(Quuinv.dot(Qux)) Ks.append(Kt) Kps.append(Kt[:, 0:7]) Kvs.append(Kt[:, 7:14]) Vs.append(Vt) Ks = Ks[::-1] Kps = Kps[::-1] Kvs = Kvs[::-1] JKpJ = np.asarray(Kps) JKvJ = np.asarray(Kvs) total_length = num_steps + initial_traj_length # Pad initial traj with PD gains pgainsdiag = np.diag(np.asarray([-2400.0, -1200.0, -1000.0, -700.0, -300.0, -300.0, -300.0])) dgainsdiag = np.diag(np.asarray([-18.0, -10.0, -6.0, -4.0, -6.0, -4.0, -4.0])) addkp = np.asarray([pgainsdiag for i in range(initial_traj_length)]) addkv = np.asarray([dgainsdiag for i in range(initial_traj_length)]) JKpJ = np.concatenate((addkp, JKpJ), axis=0) JKvJ = np.concatenate((addkv, JKvJ), axis=0) Kps = [] Kvs = [] for i in range(num_steps + initial_traj_length): Kps.append(np.zeros((6,6))) Kvs.append(np.zeros((6,6))) Kps = np.asarray(Kps) Kvs = np.asarray(Kvs) # Gains as JKpJ and JKvJ for testing if args.pdgains: JKpJ = np.asarray([pgainsdiag for i in range(total_length)]) JKvJ = np.asarray([dgainsdiag for i in range(total_length)]) qs = traj IPython.embed() Kps = np.resize(Kps, (1, 36 * total_length))[0] Kvs = np.resize(Kvs, (1, 36 * total_length))[0] JKvJ = np.resize(JKvJ, (1, 49*total_length))[0] JKpJ = np.resize(JKpJ, (1, 49*total_length))[0] # For use with new controller qs = np.resize(qs, (1, qs.shape[0]*7))[0] #resize the data to 1x7n (n being the number of steps) F = np.resize(F, (1, F.shape[0]*6))[0] #resize the data to 1x6n # [traj, Kp, Kv, F, use_force, seconds] data = np.zeros((1, len(qs) + len(JKpJ) + len(JKvJ) + len(F) + len(Kps) + len(Kvs) + 2)) data[0][0:len(qs)] = qs data[0][len(qs):len(qs)+len(JKpJ)] = JKpJ data[0][len(qs)+len(JKpJ):len(qs)+len(JKpJ)+len(JKvJ)] = JKvJ data[0][len(qs)+len(JKpJ)+len(JKvJ):len(qs)+len(JKpJ)+len(JKvJ)+len(F)] = F data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)] = Kps data[0][len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps):len(qs)+len(JKpJ)+len(JKvJ)+len(F)+len(Kps)+len(Kvs)] = Kvs data[0][-2] = args.use_force data[0][-1] = stamps[i_end] - stamps[i_start] + (initial_traj_length * 0.01) # For use with old controller """ qs = np.array(np.matrix(traj).T) # 7 x n qs = np.resize(qs, (1, qs.shape[1]*7))[0] #resize the data to 1x7n (n being the number of steps) F = np.array(np.matrix(F).T) # 6 x n F = F[0:3,:] F = np.resize(F, (1, F.shape[1]*3))[0] #resize the data to 1x3n data = np.zeros((1, len(qs) + len(F) + 2)) data[0][0:len(qs)] = qs data[0][len(qs):len(qs)+len(F)] = F data[0][-1] = args.use_force data[0][-2] = 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) else: #if not success: break if len(bodypart2traj) > 0: exec_traj_maybesim(bodypart2traj)
def main(): parser = argparse.ArgumentParser() parser.add_argument('file', type=str) parser.add_argument('--real_robot', action='store_true') parser.add_argument('--view', action='store_true') args = parser.parse_args() if args.view: print 'Opening file %s' % args.file f = h5py.File(args.file, 'r') rgbs = f['rgb'] depths = f['depth'] T_w_k = np.array(f['T_w_k']) Globals.env = openravepy.Environment() viewer = trajoptpy.GetViewer(Globals.env) num_frames = len(rgbs) for i in range(0, num_frames, 10): print 'Showing frame %d/%d' % (i+1, num_frames) rgb, depth = rgbs[i], depths[i] # XYZ_k = clouds.depth_to_xyz(depth, berkeley_pr2.f) # XYZ_w = XYZ_k.dot(T_w_k[:3,:3].T) + T_w_k[:3,3][None,None,:] # handle = Globals.env.plot3(XYZ_w.reshape(-1,3), 2, rgb.reshape(-1,3)[:,::-1]/255.) xyz = clouds.extract_color(rgb, depth, T_w_k, clouds.yellow_mask) handles = [] handles.append(Globals.env.plot3(xyz, 2, (1,0,0))) draw_ax(T_w_k, .1, Globals.env, handles) viewer.Idle() return if args.real_robot: import rospy rospy.init_node("recorder", disable_signals=True) Globals.pr2 = PR2.PR2() Globals.env = Globals.pr2.env Globals.robot = Globals.pr2.robot Globals.pr2.update_rave() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) else: Globals.env = openravepy.Environment() T_w_k = np.eye(4) viewer = trajoptpy.GetViewer(Globals.env) #env.LoadData(make_table_xml(translation=[0, 0, -.05], extents=[1, 1, .05])) num_frames = 0 rgbs, depths = [], [] subprocess.call("killall XnSensorServer", shell=True) grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() try: while True: rgb, depth = grabber.getRGBD() rgbs.append(rgb) depths.append(depth) cv2.imshow("rgb", rgb) cv2.imshow("depth", cmap[np.fmin((depth*.064).astype('int'), 255)]) cv2.waitKey(30) num_frames += 1 print 'Captured frame', num_frames viewer.Step() except KeyboardInterrupt: print 'Keyboard interrupt' print 'Writing to %s ...' % args.file out = h5py.File(args.file, 'w') out.create_dataset('rgb', data=rgbs, compression='gzip', chunks=(1, 256, 256, 3)) out.create_dataset('depth', data=depths, compression='gzip', chunks=(1, 256, 256)) out['T_w_k'] = T_w_k out.close()
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: subprocess.call("killall XnSensorServer", shell=True) grabber = cloudprocpy.CloudGrabber() grabber.startRGBD() Globals.viewer = trajoptpy.GetViewer(Globals.env) ##################### while True: redprint("Acquire point cloud") if args.fake_data_segment: new_xyz = np.squeeze(demofile[args.fake_data_segment]["cloud_xyz"]) hmat = openravepy.matrixFromAxisAngle( args.fake_data_transform[3:6]) hmat[:3, 3] = args.fake_data_transform[0:3] new_xyz = new_xyz.dot(hmat[:3, :3].T) + hmat[:3, 3][None, :] else: Globals.pr2.rarm.goto_posture('side') Globals.pr2.larm.goto_posture('side') Globals.pr2.join_all() Globals.pr2.update_rave() rgb, depth = grabber.getRGBD() T_w_k = berkeley_pr2.get_kinect_transform(Globals.robot) new_xyz = cloud_proc_func(rgb, depth, T_w_k) ################################ redprint("Finding closest demonstration") if args.fake_data_segment: seg_name = args.fake_data_segment elif args.choice == "costs": f, seg_name = choose_segment(demofile, new_xyz) else: seg_name = choose_segment(demofile, new_xyz) seg_info = demofile[seg_name] # redprint("using demo %s, description: %s"%(seg_name, seg_info["description"])) ################################ redprint("Generating end-effector trajectory") if not args.choice == "costs": handles = [] old_xyz = np.squeeze(seg_info["cloud_xyz"]) handles.append(Globals.env.plot3(old_xyz, 5, (1, 0, 0, 1))) handles.append(Globals.env.plot3(new_xyz, 5, (0, 0, 1, 1))) f = registration.tps_rpm(old_xyz, new_xyz, plot_cb=tpsrpm_plot_cb, plotting=1) 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)) link2eetraj = {} for lr in 'lr': link_name = "%s_gripper_tool_frame" % lr old_ee_traj = asarray(seg_info[link_name]["hmat"]) new_ee_traj = f.transform_hmats(old_ee_traj) link2eetraj[link_name] = new_ee_traj if not args.choice == "costs": 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))) # TODO plot # plot_warping_and_trajectories(f, old_xyz, new_xyz, old_ee_traj, new_ee_traj) miniseg_starts, miniseg_ends = split_trajectory_by_gripper(seg_info) success = True print colorize.colorize("mini segments:", "red"), miniseg_starts, miniseg_ends 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)) bodypart2traj = {} arms_used = "" for lr in 'lr': manip_name = {"l": "leftarm", "r": "rightarm"}[lr] old_joint_traj = asarray(seg_info[manip_name][i_start:i_end + 1]) if arm_moved(old_joint_traj): ee_link_name = "%s_gripper_tool_frame" % lr new_ee_traj = link2eetraj[ee_link_name] if args.execution: Globals.pr2.update_rave() new_joint_traj = plan_follow_traj( Globals.robot, manip_name, Globals.robot.GetLink(ee_link_name), new_ee_traj[i_start:i_end + 1], old_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 ################################ 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) # TODO measure failure condtions if not success: break redprint("Segment %s result: %s" % (seg_name, success)) if args.fake_data_segment: break