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 main(): # define simulation objects table_height = 0.77 sim_objs = [] sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False)) # initialize simulation world and environment sim = DynamicSimulationRobotWorld() sim.add_objects(sim_objs) sim.create_viewer() sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()]) sim.robot.SetDOFValues([1.25], [sim.robot.GetJoint('head_tilt_joint').GetJointIndex()]) # move head down so it can see the rope sim_util.reset_arms_to_side(sim) env = environment.LfdEnvironment(sim, sim, downsample_size=0.025) demo_rope_poss = np.array([[.2, -.2, table_height+0.006], [.8, -.2, table_height+0.006], [.8, .2, table_height+0.006], [.2, .2, table_height+0.006]]) demo = create_rope_demo(env, demo_rope_poss) test_rope_poss = np.array([[.2, -.2, table_height+0.006], [.5, -.4, table_height+0.006], [.8, .0, table_height+0.006], [.8, .2, table_height+0.006], [.6, .0, table_height+0.006], [.4, .2, table_height+0.006], [.2, .2, table_height+0.006]]) test_rope_sim_obj = create_rope(test_rope_poss) sim.add_objects([test_rope_sim_obj]) sim.settle() test_scene_state = env.observe_scene() reg_factory = TpsRpmRegistrationFactory() traj_transferer = FingerTrajectoryTransferer(sim) plot_cb = lambda i, i_em, x_nd, y_md, xtarg_nd, wt_n, f, corr_nm, rad: registration_plot_cb(sim, x_nd, y_md, f) reg_and_traj_transferer = TwoStepRegistrationAndTrajectoryTransferer(reg_factory, traj_transferer) test_aug_traj = reg_and_traj_transferer.transfer(demo, test_scene_state, callback=plot_cb, plotting=True) env.execute_augmented_trajectory(test_aug_traj)
def main(): # define simulation objects table_height = 0.77 cyl_radius = 0.025 cyl_height = 0.3 cyl_pos0 = np.r_[.7, -.15, table_height+cyl_height/2] cyl_pos1 = np.r_[.7, .15, table_height+cyl_height/2] cyl_pos2 = np.r_[.4, -.15, table_height+cyl_height/2] rope_poss = np.array([[.2, -.2, table_height+0.006], [.8, -.2, table_height+0.006], [.8, .2, table_height+0.006], [.2, .2, table_height+0.006]]) sim_objs = [] sim_objs.append(XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append(BoxSimulationObject("table", [1, 0, table_height-.1], [.85, .85, .1], dynamic=False)) cyl_sim_objs = create_cylinder_grid(cyl_pos0, cyl_pos1, cyl_pos2, cyl_radius, cyl_height) sim_objs.extend(cyl_sim_objs) rope_sim_obj = create_rope(rope_poss) sim_objs.append(rope_sim_obj) # initialize simulation world and environment sim = DynamicSimulationRobotWorld() sim.add_objects(sim_objs) sim.create_viewer() sim.robot.SetDOFValues([0.25], [sim.robot.GetJoint('torso_lift_joint').GetJointIndex()]) sim_util.reset_arms_to_side(sim) color_cylinders(cyl_sim_objs) env = environment.LfdEnvironment(sim, sim) # define augmented trajectory pick_pos = rope_poss[0] + .1 * (rope_poss[1] - rope_poss[0]) drop_pos = rope_poss[3] + .1 * (rope_poss[2] - rope_poss[3]) + np.r_[0, .2, 0] pick_R = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]]) drop_R = np.array([[0, 1, 0], [0, 0, -1], [-1, 0, 0]]) move_height = .2 aug_traj = create_augmented_traj(sim.robot, pick_pos, drop_pos, pick_R, drop_R, move_height) env.execute_augmented_trajectory(aug_traj)
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
helix_angs = np.linspace(helix_ang0, helix_ang1, num) helix_heights = np.linspace(helix_height0, helix_height1, num) init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights] rope_params = sim_util.RopeParams() cyl_radius = 0.025 cyl_height = 0.3 cyl_pos0 = np.r_[.6, helix_radius, table_height + .25] cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35] sim_objs = [] sim_objs.append( XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append( BoxSimulationObject("table", [1, 0, table_height - .1], [.85, .85, .1], dynamic=False)) sim_objs.append(RopeSimulationObject("rope", init_rope_nodes, rope_params)) sim_objs.append( CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True)) sim_objs.append( CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height,
def setUp(self): table_height = 0.77 helix_ang0 = 0 helix_ang1 = 4 * np.pi helix_radius = .2 helix_center = np.r_[.6, 0] helix_height0 = table_height + .15 helix_height1 = table_height + .15 + .3 helix_length = np.linalg.norm( np.r_[(helix_ang1 - helix_ang0) * helix_radius, helix_height1 - helix_height0]) num = np.round(helix_length / .02) helix_angs = np.linspace(helix_ang0, helix_ang1, num) helix_heights = np.linspace(helix_height0, helix_height1, num) init_rope_nodes = np.c_[helix_center + helix_radius * np.c_[np.cos(helix_angs), np.sin(helix_angs)], helix_heights] rope_params = sim_util.RopeParams() cyl_radius = 0.025 cyl_height = 0.3 cyl_pos0 = np.r_[.6, helix_radius, table_height + .25] cyl_pos1 = np.r_[.6, -helix_radius, table_height + .35] sim_objs = [] sim_objs.append( XmlSimulationObject("robots/pr2-beta-static.zae", dynamic=False)) sim_objs.append( BoxSimulationObject("table", [1, 0, table_height - .1], [.85, .85, .1], dynamic=False)) sim_objs.append( RopeSimulationObject("rope", init_rope_nodes, rope_params)) sim_objs.append( CylinderSimulationObject("cyl0", cyl_pos0, cyl_radius, cyl_height, dynamic=True)) sim_objs.append( CylinderSimulationObject("cyl1", cyl_pos1, cyl_radius, cyl_height, dynamic=True)) self.sim = DynamicSimulation() self.sim.add_objects(sim_objs) self.sim.robot.SetDOFValues( [0.25], [self.sim.robot.GetJoint('torso_lift_joint').GetJointIndex()]) sim_util.reset_arms_to_side(self.sim) # rotate cylinders by 90 deg for i in range(2): bt_cyl = self.sim.bt_env.GetObjectByName('cyl%d' % i) T = openravepy.matrixFromAxisAngle(np.array([np.pi / 2, 0, 0])) T[:3, 3] = bt_cyl.GetTransform()[:3, 3] bt_cyl.SetTransform( T ) # SetTransform needs to be used in the Bullet object, not the openrave body self.sim.update()