def initialize(load_gazebo=True): openrave_sim = DynamicSimulationRobotWorld() robot_xml = XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False) openrave_sim.add_objects([robot_xml], consider_finger_collisions=True) table = BoxSimulationObject('table', \ [0.66, 0, (TABLE_HEIGHT + 0.03) / 2.0], \ [0.4, 0.75, (TABLE_HEIGHT + 0.03) / 2.0], \ dynamic=False) openrave_sim.add_objects([table], consider_finger_collisions=True) #table_xml = TableSimulationObject("table", \ # [0.66, 0, (TABLE_HEIGHT + 0.03) / 2.0], # [0.4, 0.75, (TABLE_HEIGHT + 0.03) / 2.0], # dynamic=False) #openrave_sim.add_objects([table_xml], consider_finger_collisions=True) cup = XmlSimulationObject(os.path.join(MODELS_DIR, CUP_MESH), dynamic=False) openrave_sim.add_objects([cup], consider_finger_collisions=True) v = trajoptpy.GetViewer( openrave_sim.env) # TODO: not sure if this is necessary # Initialize interface for passing controls to Gazebo. Make sure to pass # in the OpenRave environment we just created, so PR2 isn't referring (and # updating) a separate OpenRave environment. if load_gazebo: pr2 = PR2.PR2(env=openrave_sim.env) time.sleep(0.2) else: pr2 = None return openrave_sim, pr2, v, cup
def create_viewer(self, window_prop=None, camera_matrix=None): trajoptpy.GetViewer(self.env) # creates viewer if window_prop is None: window_prop = settings.WINDOW_PROP self.viewer.SetWindowProp(*window_prop) if camera_matrix is None: camera_matrix = settings.CAMERA_MATRIX self.viewer.SetCameraManipulatorMatrix(np.asarray(camera_matrix))
def animate_traj(traj, robot, pause=True, restore=True): """make sure to set active DOFs beforehand""" if restore: _saver = openravepy.RobotStateSaver(robot) viewer = trajoptpy.GetViewer(robot.GetEnv()) for (i,dofs) in enumerate(traj): print "step %i/%i"%(i+1,len(traj)) robot.SetActiveDOFValues(dofs) if pause: viewer.Idle() else: viewer.Step()
def animate_traj(traj, robot, pause=True, step_viewer=1, restore=True, callback=None, execute_step_cond=None): """make sure to set active DOFs beforehand""" if restore: _saver = openravepy.RobotStateSaver(robot) if step_viewer or pause: viewer = trajoptpy.GetViewer(robot.GetEnv()) for (i,dofs) in enumerate(traj): sys.stdout.write("step %i/%i\r"%(i+1,len(traj))) sys.stdout.flush() if callback is not None: callback(i) if execute_step_cond is not None and not execute_step_cond(i): continue robot.SetActiveDOFValues(dofs) if pause: viewer.Idle() elif step_viewer!=0 and (i%step_viewer==0 or i==len(traj)-1): viewer.Step() sys.stdout.write("\n")
def animate_floating_traj(sim_env, lhmats, rhmats, pause=True, step_viewer=True, callback=None, step=5): assert len(lhmats) == len( rhmats ), "I don't know how to animate trajectory with different lengths" if step_viewer or pause: viewer = trajoptpy.GetViewer(sim_env.sim.env) for i in xrange(len(lhmats)): if callback is not None: callback(i) sim_env.sim.grippers['r'].set_toolframe_transform(rhmats[i]) sim_env.sim.grippers['l'].set_toolframe_transform(lhmats[i]) if pause: viewer.Idle() elif step_viewer and not i % step: viewer.Step()
def main(): import openravepy import trajoptpy env = openravepy.Environment() viewer = trajoptpy.GetViewer(env) env.LoadData(make_table_xml(translation=[0, 0, -.05], extents=[1, 1, .05])) np.random.seed(0) sim_params = trackingpy.SimulationParams() sim_params.dt = .01 sim_params.solver_iters = 10 sim_params.gravity = np.array([0, 0, -1.]) sim_params.damping = 1 sim_params.stretching_stiffness = 1 sim_params.bending_stiffness = .7 cloth = Mesh('/home/jonathan/Desktop/simple_cloth_with_slit.obj', 100, sim_params) # cloth = Cloth(res_x=50, res_y=50, len_x=.5, len_y=1., init_center=np.array([0, 0, .5]), total_mass=100, sim_params=sim_params) # anchors for testing # for i in range(cloth.res_x): # cloth.sys.add_anchor_constraint(i, cloth.init_pos[i]) # add above-table constraints for i in range(cloth.num_nodes): cloth.sys.add_plane_constraint(i, np.array([0, 0, 0]), np.array([0, 0, 1])) # from timeit import Timer # print 'timing' # num_timing_iters = 1000 # t = Timer(lambda: cloth.step()) # result = t.timeit(number=num_timing_iters) # print 'iters/sec =', float(num_timing_iters)/result, 'total time =', result # raw_input('done timing') print 'cloth made' while True: cloth.step() pos = cloth.get_node_positions() handles = [env.plot3(pos, 5)] handles.append( env.drawlinelist(cloth.get_edge_positions().reshape((-1, 3)), 1, (0, 1, 0))) viewer.Idle()
def test_viewer_side_effects(self): """ Check if stepping the viewer has side effects in the simulation """ sim_state0 = self.sim.get_state() self.sim.set_state(sim_state0) self.sim.settle(max_steps=1000, step_viewer=0) sim_state1 = self.sim.get_state() # create viewer viewer = trajoptpy.GetViewer(self.sim.env) self.sim.set_state(sim_state0) self.sim.settle(max_steps=1000, step_viewer=10) sim_state2 = self.sim.get_state() self.assertArrayDictEqual(sim_state1[1], sim_state2[1])
def setup_lfd_environment_sim(args): actions = h5py.File(args.gen_tasks.actionfile, 'r') init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment( actions, args.gen_tasks.fake_data_segment, args.gen_tasks.fake_data_transform) table_height = init_rope_xyz[:, 2].mean() - .02 sim_objs = [] sim_objs.append( BoxSimulationObject("table", [1, 0, table_height + (-.1 + .01)], [.85, .85, .1], dynamic=False)) sim = DynamicSimulation() world = sim sim.add_objects(sim_objs) if args.animation: viewer = trajoptpy.GetViewer(sim.env) if os.path.isfile(args.window_prop_file) and os.path.isfile( args.camera_matrix_file): print "loading window and camera properties" window_prop = np.loadtxt(args.window_prop_file) camera_matrix = np.loadtxt(args.camera_matrix_file) try: viewer.SetWindowProp(*window_prop) viewer.SetCameraManipulatorMatrix(camera_matrix) except: print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." else: print "move viewer to viewpoint that isn't stupid" print "then hit 'p' to continue" viewer.Idle() print "saving window and camera properties" try: window_prop = viewer.GetWindowProp() camera_matrix = viewer.GetCameraManipulatorMatrix() np.savetxt(args.window_prop_file, window_prop, fmt='%d') np.savetxt(args.camera_matrix_file, camera_matrix) except: print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." viewer.Step() return sim
def main(): demofile = h5py.File(args.demos_h5file, 'r') trajoptpy.SetInteractive(False) Globals.env = openravepy.Environment() Globals.env.StopSimulation() Globals.env.Load("robots/pr2-beta-static.zae") Globals.robot = Globals.env.GetRobots()[0] Globals.robot.SetDOFValues(PR2_L_POSTURES["side"], Globals.robot.GetManipulator("leftarm").GetArmIndices()) Globals.robot.SetDOFValues(mirror_arm_joints(PR2_L_POSTURES["side"]), Globals.robot.GetManipulator("rightarm").GetArmIndices()) init_rope_xyz, _ = load_random_start_segment(demofile) init_rope_xyz_robot = apply_tfm(init_rope_xyz[()], args.inittfmfile) table_height = init_rope_xyz_robot[:,2].mean() - .03 table_xml = make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01]) Globals.env.LoadData(table_xml) Globals.sim = ropesim.Simulation(Globals.env, Globals.robot) Globals.viewer = trajoptpy.GetViewer(Globals.env) Globals.viewer.Idle() if not args.same_as_training: for i in range(0, args.dataset_size): print "State ", i, " of ", args.dataset_size rope_nodes, demo_id = sample_rope_state(demofile) save_example_action(rope_nodes, demo_id) else: start_keys = [k for k in demofile.keys() if k.startswith('demo') and k.endswith('00')] for demo_id in start_keys: xyz_camera = demofile[demo_id]['cloud_xyz'][()] xyz_robot = apply_tfm(xyz_camera, args.inittfmfile) rope_nodes = rope_initialization.find_path_through_point_cloud( xyz_robot, perturb_peak_dist = None, num_perturb_points = 0) replace_rope(rope_nodes) Globals.sim.settle() Globals.viewer.Step() new_xyz_robot = Globals.sim.observe_cloud() save_example_action(new_xyz_robot, demo_id) time.sleep(1)
def animate_traj(traj, robot): with robot: viewer = trajoptpy.GetViewer(robot.GetEnv()) for i, traj_mats in enumerate(zip(traj, new_hmats)): dofs = traj_mats[0] mat = traj_mats[1] print mat robot.SetDOFValues( dofs, robot.GetManipulator(manip_name).GetArmIndices()) colors = [(1, 0, 0, 1), (0, 1, 0, 1), (0, 0, 1, 1)] for i in range(3): point = mat[:3, 3] axis = mat[:3, i] / 10 handles.append( Globals.env.drawarrow(p1=point, p2=point + axis, linewidth=0.004, color=colors[i])) viewer.Idle()
def viewer(self): if not self.__viewer_cache: #and trajoptpy.ViewerExists(self.env): self.__viewer_cache = trajoptpy.GetViewer(self.env) return self.__viewer_cache
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()
args = parser.parse_args() import h5py, openravepy, trajoptpy from rapprentice import animate_traj, ros2rave, clouds from numpy import asarray import numpy as np hdf = h5py.File(args.h5file) segnames = [args.seg] if args.seg else hdf.keys() env = openravepy.Environment() env.StopSimulation() env.Load("robots/pr2-beta-static.zae") robot = env.GetRobots()[0] viewer = trajoptpy.GetViewer(env) for segname in segnames: seg_info = hdf[segname] from rapprentice import berkeley_pr2 r2r = ros2rave.RosToRave(robot, seg_info["joint_states"]["name"]) rave_traj = [ r2r.convert(row) for row in asarray(seg_info["joint_states"]["position"]) ] robot.SetActiveDOFs(r2r.rave_inds) robot.SetActiveDOFValues(rave_traj[0])
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
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
import openravepy, sensorsimpy, trajoptpy env = openravepy.Environment() env.StopSimulation() env.Load("robots/pr2-beta-static.zae") viewer = trajoptpy.GetViewer(env) viewer.Idle() sensor = sensorsimpy.CreateFakeKinect(env) sensor.SetPose([0, 0, 2], [0, 0, 1, 0]) sensor.SetIntrinsics(525) sensor.Update() d = sensor.GetDepthImage()
def setup_lfd_environment_sim(args): actions = h5py.File(args.eval.actionfile, 'r') init_rope_xyz, init_joint_names, init_joint_values = sim_util.load_fake_data_segment( actions, args.eval.fake_data_segment, args.eval.fake_data_transform) table_height = init_rope_xyz[:, 2].mean() - .02 sim_objs = [] sim_objs.append( XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append( BoxSimulationObject("table", [1, 0, table_height + (-.1 + .01)], [.85, .85, .1], dynamic=False)) print 'Setting up lfd environment' sim = DynamicRopeSimulationRobotWorld() world = sim sim.add_objects(sim_objs) if args.eval.ground_truth: lfd_env = GroundTruthRopeLfdEnvironment( sim, world, upsample=args.eval.upsample, upsample_rad=args.eval.upsample_rad, downsample_size=args.eval.downsample_size) else: lfd_env = LfdEnvironment(sim, world, downsample_size=args.eval.downsample_size) dof_inds = sim_util.dof_inds_from_name(sim.robot, '+'.join(init_joint_names)) values, dof_inds = zip( *[(value, dof_ind) for value, dof_ind in zip(init_joint_values, dof_inds) if dof_ind != -1]) # this also sets the torso (torso_lift_joint) to the height in the data sim.robot.SetDOFValues(values, dof_inds) sim_util.reset_arms_to_side(sim) if args.animation: viewer = trajoptpy.GetViewer(sim.env) if os.path.isfile(args.window_prop_file) and os.path.isfile( args.camera_matrix_file): print "loading window and camera properties" window_prop = np.loadtxt(args.window_prop_file) camera_matrix = np.loadtxt(args.camera_matrix_file) try: viewer.SetWindowProp(*window_prop) viewer.SetCameraManipulatorMatrix(camera_matrix) except: print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." else: print "move viewer to viewpoint that isn't stupid" print "then hit 'p' to continue" viewer.Idle() print "saving window and camera properties" try: window_prop = viewer.GetWindowProp() camera_matrix = viewer.GetCameraManipulatorMatrix() np.savetxt(args.window_prop_file, window_prop, fmt='%d') np.savetxt(args.camera_matrix_file, camera_matrix) except: print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." viewer.Step() if args.eval.dof_limits_factor != 1.0: assert 0 < args.eval.dof_limits_factor and args.eval.dof_limits_factor <= 1.0 active_dof_indices = sim.robot.GetActiveDOFIndices() active_dof_limits = sim.robot.GetActiveDOFLimits() for lr in 'lr': manip_name = {"l": "leftarm", "r": "rightarm"}[lr] dof_inds = sim.robot.GetManipulator(manip_name).GetArmIndices() limits = np.asarray(sim.robot.GetDOFLimits(dof_inds)) limits_mean = limits.mean(axis=0) limits_width = np.diff(limits, axis=0) new_limits = limits_mean + args.eval.dof_limits_factor * \ np.r_[-limits_width / 2.0, limits_width / 2.0] for i, ind in enumerate(dof_inds): active_dof_limits[0][active_dof_indices.tolist().index( ind)] = new_limits[0, i] active_dof_limits[1][active_dof_indices.tolist().index( ind)] = new_limits[1, i] sim.robot.SetDOFLimits(active_dof_limits[0], active_dof_limits[1]) return lfd_env, sim
def load_simulation(args, sim_env): sim_env.env = openravepy.Environment() sim_env.env.StopSimulation() sim_env.env.Load("robots/pr2-beta-static.zae") sim_env.robot = sim_env.env.GetRobots()[0] actions = h5py.File(args.actionfile, 'r') fakedatafile = h5py.File(args.fakedatafile, 'r') GlobalVars.init_tfm = fakedatafile['init_tfm'][()] init_rope_xyz, _ = sim_util.load_fake_data_segment( sim_env, fakedatafile, args.fake_data_segment, args.fake_data_transform ) # this also sets the torso (torso_lift_joint) to the height in the data # Set table height to correct height of first rope in holdout set holdoutfile = h5py.File(args.holdoutfile, 'r') first_holdout = holdoutfile[holdoutfile.keys()[0]] init_rope_xyz = first_holdout['rope_nodes'][:] if 'frame' not in first_holdout or first_holdout['frame'][()] != 'r': init_rope_xyz = init_rope_xyz.dot( GlobalVars.init_tfm[:3, :3].T) + GlobalVars.init_tfm[:3, 3][None, :] table_height = init_rope_xyz[:, 2].mean() - .02 # Before: .02 table_xml = sim_util.make_table_xml(translation=[1, 0, table_height], extents=[.85, .55, .01]) sim_env.env.LoadData(table_xml) if 'bookshelve' in args.obstacles: sim_env.env.Load("data/bookshelves.env.xml") if 'boxes' in args.obstacles: sim_env.env.LoadData( sim_util.make_box_xml("box0", [.7, .43, table_height + (.01 + .12)], [.12, .12, .12])) sim_env.env.LoadData( sim_util.make_box_xml( "box1", [.74, .47, table_height + (.01 + .12 * 2 + .08)], [.08, .08, .08])) cc = trajoptpy.GetCollisionChecker(sim_env.env) for gripper_link in [ link for link in sim_env.robot.GetLinks() if 'gripper' in link.GetName() ]: cc.ExcludeCollisionPair(gripper_link, sim_env.env.GetKinBody('table').GetLinks()[0]) sim_util.reset_arms_to_side(sim_env) if args.animation: sim_env.viewer = trajoptpy.GetViewer(sim_env.env) if args.animation > 1 and os.path.isfile( args.window_prop_file) and os.path.isfile( args.camera_matrix_file): print "loading window and camera properties" window_prop = np.loadtxt(args.window_prop_file) camera_matrix = np.loadtxt(args.camera_matrix_file) try: sim_env.viewer.SetWindowProp(*window_prop) sim_env.viewer.SetCameraManipulatorMatrix(camera_matrix) except: print "SetWindowProp and SetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt." else: print "move viewer to viewpoint that isn't stupid" print "then hit 'p' to continue" sim_env.viewer.Idle() print "saving window and camera properties" try: window_prop = sim_env.viewer.GetWindowProp() camera_matrix = sim_env.viewer.GetCameraManipulatorMatrix() np.savetxt(args.window_prop_file, window_prop, fmt='%d') np.savetxt(args.camera_matrix_file, camera_matrix) except: print "GetWindowProp and GetCameraManipulatorMatrix are not defined. Pull and recompile Trajopt."
def compute_dt_code(ctl_pts, plotting=False): """Takes rope control points (Nx3 array), closes the loop, and computes the Dowker-Thistlethwaite code for the knot. The z-value for the points are used for determining over/undercrossings. Follows procedure outlined here: http://katlas.math.toronto.edu/wiki/DT_(Dowker-Thistlethwaite)_Codes """ # First, close the loop by introducing extra points under the table and toward the robot (by subtracting z and x values) # first_pt, last_pt = ctl_pts[0], ctl_pts[-1] # flipped = False # if first_pt[1] > last_pt[1]: # first_pt, last_pt = last_pt, first_pt # flipped = True # min_z = ctl_pts[:,2].min() # extra_first_pt, extra_last_pt = first_pt + [-.1, -.1, min_z-1], last_pt + [-.1, .1, min_z-1] # if flipped: # extra_pts = [extra_first_pt, extra_first_pt + [-1, 0, 0], extra_last_pt + [-1, 0, 0], extra_last_pt, last_pt] # else: # extra_pts = [extra_last_pt, extra_last_pt + [-1, 0, 0], extra_first_pt + [-1, 0, 0], extra_first_pt, first_pt] # ctl_pts = np.append(ctl_pts, extra_pts, axis=0) if plotting: import trajoptpy, openravepy env = openravepy.Environment() viewer = trajoptpy.GetViewer(env) handles = [] handles.append(env.plot3(ctl_pts, 5, [0, 0, 1])) viewer.Idle() # Upsampling loop: upsample until every segment has at most one crossing need_upsample_ind = None upsample_iters = 0 max_upsample_iters = 10 while True: counter = 1 crossings = defaultdict(list) # Walk along rope: for each segment, compute intersections with all other segments for i in range(len(ctl_pts) - 1): curr_seg = ctl_pts[i:i + 2, :] intersections, ts, us = intersect_segs(ctl_pts[:, :2], curr_seg[:, :2]) if len(intersections) == 0: continue if len(intersections) != 1: LOG.debug( 'warning: more than one intersection for segment %d, now upsampling', i) need_upsample_ind = i break # for each intersection, determine and record over/undercrossing i_int = intersections[0] if plotting: handles.append( env.drawlinestrip(ctl_pts[i_int:i_int + 2], 5, [1, 0, 0])) int_point_rope = ctl_pts[i_int] + ts[i_int] * (ctl_pts[i_int + 1] - ctl_pts[i_int]) int_point_curr_seg = curr_seg[0] + us[i_int] * (curr_seg[1] - curr_seg[0]) #assert np.allclose(int_point_rope[:2], int_point_curr_seg[:2]) above = int_point_curr_seg[2] > int_point_rope[2] crossings[tuple(sorted( (i, i_int)))].append(-counter if counter % 2 == 0 and above else counter) counter += 1 if plotting: viewer.Idle() # upsample if necessary if need_upsample_ind is not None and upsample_iters < max_upsample_iters: spacing = np.linspace(0, 1, len(ctl_pts)) new_spacing = np.insert( spacing, need_upsample_ind + 1, (spacing[need_upsample_ind] + spacing[need_upsample_ind + 1]) / 2.) ctl_pts = math_utils.interp2d(new_spacing, spacing, ctl_pts) upsample_iters += 1 need_upsample = None continue break # Extract relevant part of crossing data to produce DT code out = [] for pair in crossings.itervalues(): assert len(pair) == 2 odd = [p for p in pair if p % 2 == 1][0] even = [p for p in pair if p % 2 == 0][0] out.append((odd, even)) out.sort() dt_code = [-o[1] for o in out] return dt_code