def create_robot(request): robot_name = request.config.getoption('robot_name') if request.config.getoption('sim_env') == 'gazebo': return Robot(robot_name, pb=False, use_cam=False) elif request.config.getoption('sim_env') == 'pybullet': return Robot(robot_name, pb=True, use_cam=False, arm_cfg={'render': False}) else: raise ValueError('unknown simulation environment')
def main(): """ This function demonstrates how to setup camera and get rgb/depth images and segmentation mask. """ robot = Robot('ur5e', pb_cfg={'gui': True, 'opengl_render': False}) focus_pt = [0, 0, 1] # ([x, y, z]) robot.cam.setup_camera(focus_pt=focus_pt, dist=3, yaw=90, pitch=0, roll=0) robot.arm.go_home() rgb, depth, seg = robot.cam.get_images(get_rgb=True, get_depth=True, get_seg=True) plt.figure() plt.imshow(rgb) plt.figure() plt.imshow(depth * 25, cmap='gray', vmin=0, vmax=255) log_info('Maximum Depth (m): %f' % np.max(depth)) log_info('Minimum Depth (m): %f' % np.min(depth)) plt.figure() plt.imshow(seg) plt.show()
def main(): """ This function demonstrates how to properly save depth images in python. """ robot = Robot('ur5e', pb_cfg={'gui': False, 'opengl_render': False}) focus_pt = [0, 0, 1] robot.cam.setup_camera(focus_pt=focus_pt, dist=3, yaw=90, pitch=0, roll=0) robot.arm.go_home() rgb, depth, seg = robot.cam.get_images(get_rgb=True, get_depth=True, get_seg=True) log_info(f'Depth image min:{depth.min()} m, max: {depth.max()} m.') # we first convert the depth image into CV_16U format. # The maximum value for CV_16U is 65536. # Since png files only keep the integer values, we need to scale the # depth images to keep enough resolution. scale = 1000.0 sdepth = depth * scale img_name = 'depth.png' # by converting the image data to CV_16U, the resolution of the # depth image data becomes 1 / scale m (0.001m in this case). cv2.imwrite(img_name, sdepth.astype(np.uint16)) re_depth = cv2.imread(img_name, cv2.IMREAD_UNCHANGED) sre_depth = re_depth / scale log_info(f'Saved depth image min:{sre_depth.min()} m, ' f'max: {sre_depth.max()} m.')
def __init__(self, action_repeat=10, gui=True): self._action_repeat = action_repeat self.robot = Robot('ur5e_2f140', pb_cfg={ 'gui': gui, 'realtime': False }) self._ee_pos_scale = 0.02 self._ee_ori_scale = np.pi / 36.0 self.cams = [] pb_client = self.robot.pb_client for i in range(2): self.cams.append( RGBDCameraPybullet(cfgs=self._camera_cfgs(), pb_client=pb_client)) self._setup_cameras() self.reset() self._action_bound = 1.0 self._action_high = np.array([self._action_bound] * 5) self.action_space = spaces.Box(low=-self._action_high, high=self._action_high, dtype=np.float32) ob = self._get_obs() self.observation_space = spaces.Box(low=0, high=255, shape=ob.shape, dtype=np.uint8)
def main(): """ Visualize the point cloud from the RGBD camera. """ robot = Robot('ur5e') robot.arm.go_home() robot.cam.setup_camera(focus_pt=robot.arm.robot_base_pos, dist=3, yaw=55, pitch=-30, roll=0) depth_max = 5.0 vis = open3d.visualization.Visualizer() vis.create_window("Point Cloud") pcd = open3d.geometry.PointCloud() pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=True, depth_max=depth_max) pcd.points = open3d.utility.Vector3dVector(pts) pcd.colors = open3d.utility.Vector3dVector(colors / 255.0) vis.add_geometry(pcd) while True: pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=True, depth_max=depth_max) pcd.points = open3d.utility.Vector3dVector(pts) pcd.colors = open3d.utility.Vector3dVector(colors / 255.0) vis.update_geometry() vis.poll_events() vis.update_renderer() time.sleep(0.1)
def __init__(self, action_repeat=10, render=False): self._action_repeat = action_repeat self.robot = Robot('ur5e_stick', pb=True, pb_cfg={'gui': render, 'realtime':False}) self.ee_ori = [-np.sqrt(2) / 2, np.sqrt(2) / 2, 0, 0] self._action_bound = 1.0 self._ee_pos_scale = 0.02 self._ee_ori_scale = np.pi / 36.0 self._action_high = np.array([self._action_bound] * 2) self.action_space = spaces.Box(low=-self._action_high, high=self._action_high, dtype=np.float32) self.goal = np.array([0.75, -0.3, 1.0]) self.init = np.array([0.5, 0.1, 1.0]) self.robot.arm.reset() ori = euler2quat([0, 0, np.pi / 2]) self.table_id = self.robot.pb_client.load_urdf('table/table.urdf', [.5, 0, 0.4], ori, scaling=0.9) self.marker_id = self.robot.pb_client.load_geom('box', size=0.05, mass=1, base_pos=self.goal.tolist(), rgba=[0, 1, 0, 0.4]) client_id = self.robot.pb_client.get_client_id() p.setCollisionFilterGroupMask(self.marker_id, -1, 0, 0, physicsClientId=client_id) p.setCollisionFilterPair(self.marker_id, self.table_id, -1, -1, 1, physicsClientId=client_id) self.reset() state_low = np.full(len(self._get_obs()), -float('inf')) state_high = np.full(len(self._get_obs()), float('inf')) self.observation_space = spaces.Box(state_low, state_high, dtype=np.float32)
def main(): """ This function demonstrates how the yaw angle ( the yaw angle that is defined in robot.setup_camera) changes the camera view. """ robot = Robot('ur5e') focus_pt = [0, 0, 1] # ([x, y, z]) robot.arm.go_home() img = np.random.rand(480, 640) image = plt.imshow(img, interpolation='none', animated=True, label="cam") ax = plt.gca() while True: for yaw in range(0, 360, 10): robot.cam.setup_camera(focus_pt=focus_pt, dist=3, yaw=yaw, pitch=0, roll=0) rgb, depth = robot.cam.get_images(get_rgb=True, get_depth=True) image.set_data(rgb) ax.plot([0]) plt.pause(0.01)
def main(): """ This function demonstrates how to load different kinds of objects and get state information of objects. """ np.set_printoptions(precision=3, suppress=True) robot = Robot('ur5e_stick') robot.arm.go_home() ori = euler2quat([0, 0, np.pi / 2]) robot.pb_client.load_urdf('table/table.urdf', [1, 0, 0.4], ori, scaling=0.9) sphere_id = robot.pb_client.load_geom('sphere', size=0.05, mass=1, base_pos=[1, 0, 1.0], rgba=[0, 1, 0, 1]) robot.pb_client.load_geom('box', size=0.05, mass=1, base_pos=[1, 0.12, 1.0], rgba=[1, 0, 0, 1]) robot.pb_client.load_geom('box', size=[0.06, 0.02, 0.03], mass=1, base_pos=[1.3, 0.12, 1.0], rgba=[0, 0, 1, 1]) cylinder_id = robot.pb_client.load_geom('cylinder', size=[0.06, 0.08], mass=1, base_pos=[0.8, -0.12, 1.0], rgba=[0, 1, 1, 1]) duck_id = robot.pb_client.load_geom('mesh', mass=1, visualfile='duck.obj', mesh_scale=0.1, base_pos=[0.9, -0.4, 1.0], rgba=[0.5, 0.2, 1, 1]) pos, quat, lin_vel, ang_vel = robot.pb_client.get_body_state(cylinder_id) log_info('Cylinder:') log_info(' position: %s' % np.array2string(pos, precision=2)) log_info(' quaternion: %s' % np.array2string(quat, precision=2)) log_info(' linear vel: %s' % np.array2string(lin_vel, precision=2)) log_info(' angular vel: %s' % np.array2string(ang_vel, precision=2)) log_info('Removing sphere') robot.pb_client.remove_body(sphere_id) time.sleep(2) log_info('Reset duck') robot.pb_client.reset_body(duck_id, base_pos=[0.9, -0.4, 1.0], base_quat=[0, 0, 0, 1], lin_vel=[0, 2, 0], ang_vel=[0, 0, 2]) time.sleep(10)
def main(): """ Move the robot end effector in a straight line. """ robot = Robot('ur5e') robot.arm.go_home() robot.arm.move_ee_xyz([0.1, 0.1, 0.1]) time.sleep(3)
def __init__(self, ifRender=False): # np.set_printoptions(precision=3, suppress=True) # table scaling and table center location self.table_scaling = 0.6 # tabletop x ~ (0.3, 0.9); y ~ (-0.45, 0.45) self.table_x = 0.6 self.table_y = 0 self.table_z = 0.6 self.table_surface_height = 0.975 # get from running robot.cam.get_pix.3dpt self.table_ori = euler2quat([0, 0, np.pi / 2]) # task space x ~ (0.4, 0.8); y ~ (-0.3, 0.3) self.max_arm_reach = 0.91 self.workspace_max_x = 0.75 # 0.8 discouraged, as box can move past max arm reach self.workspace_min_x = 0.4 self.workspace_max_y = 0.3 self.workspace_min_y = -0.3 # robot end-effector self.ee_min_height = 0.99 self.ee_rest_height = 1.1 # stick scale="0.0001 0.0001 0.0007" self.ee_home = [self.table_x, self.table_y, self.ee_rest_height] # initial object location self.box_z = 1 - 0.005 self.box_pos = [self.table_x, self.table_y, self.box_z] self.box_size = 0.02 # distance between center frame and size, box size 0.04 # push config: push_len by default [0.06-0.1] self.push_len_min = 0.06 # 0.06 ensures no contact with box empiracally self.push_len_range = 0.04 # image processing config self.row_min = 40 self.row_max = 360 self.col_min = 0 self.col_max = 640 self.output_row = 100 self.output_col = 200 self.row_scale = (self.row_max - self.row_min) / float(self.output_row) self.col_scale = (self.col_max - self.col_min) / float(self.output_col) assert self.col_scale == self.row_scale # load robot self.robot = Robot('ur5e_stick', pb=True, pb_cfg={'gui': ifRender}) self.robot.arm.go_home() self.ee_origin = self.robot.arm.get_ee_pose() self.go_home() self._home_jpos = self.robot.arm.get_jpos() # load table self.table_id = self.load_table() # load box self.box_id = self.load_box() # initialize camera matrices self.robot.cam.setup_camera(focus_pt=[0.7, 0, 1.], dist=0.5, yaw=90, pitch=-60, roll=0) self.ext_mat = self.robot.cam.get_cam_ext() self.int_mat = self.robot.cam.get_cam_int()
def main(): """ Move the robot end effector in a straight line. """ robot = Robot('yumi_grippers') robot.arm.go_home() robot.arm.right_arm.move_ee_xyz([0.1, 0.1, 0.1]) time.sleep(3) robot.arm.move_ee_xyz([0.1, -0.1, 0.1], arm='left') time.sleep(3)
def main(): """ This function demonstrates how to move the robot arm to the desired joint positions. """ robot = Robot('ur5e') robot.arm.go_home() robot.arm.set_jpos([-0.8, -1.2, -2.2, -1.5, 2.0, 0]) # sleep statement is not necessary time.sleep(3) robot.arm.set_jpos(0.5, 'shoulder_pan_joint') time.sleep(3)
def main(): """ This function demonstrates how to move the robot arm to the desired joint positions. """ dir_path = os.path.dirname(os.path.realpath(__file__)) texture_path = os.path.join(dir_path, 'textures') robot = Robot('ur5e') robot.arm.go_home() modder = TextureModder(pb_client_id=robot.pb_client.get_client_id()) ori = euler2quat([0, 0, np.pi / 2]) table_id = robot.pb_client.load_urdf('table/table.urdf', [1, 0, 0.4], ori, scaling=0.9) sphere_id = robot.pb_client.load_geom('sphere', size=0.05, mass=1, base_pos=[1, 0, 1.0], rgba=[0, 1, 0, 1]) box_id = robot.pb_client.load_geom('box', size=0.05, mass=1, base_pos=[1, 0.12, 1.0], rgba=[1, 0, 0, 1]) duck_id = robot.pb_client.load_geom('mesh', mass=1, visualfile='duck.obj', mesh_scale=0.1, base_pos=[0.9, -0.4, 1.0], rgba=[0.5, 0.2, 1, 1]) modder.set_texture(table_id, -1, os.path.join(texture_path, '1.jpg')) modder.set_texture(sphere_id, -1, os.path.join(texture_path, '2.jpg')) modder.set_texture(box_id, -1, os.path.join(texture_path, '3.jpg')) modder.set_texture(duck_id, -1, os.path.join(texture_path, '4.jpg')) modder.set_texture_path(texture_path) while True: start = time.time() # modder.randomize('rgb') # modder.randomize('gradient') # modder.randomize('noise') # modder.randomize('texture', exclude={robot.arm.robot_id: []}) # modder.randomize('texture', # exclude={robot.arm.robot_id: [3, 4, 5, 6]}) modder.randomize('all') print('Time cost (s): ', time.time() - start) time.sleep(1)
def main(): """ This function shows an example of block stacking. """ np.set_printoptions(precision=4, suppress=True) robot = Robot('franka') success = robot.arm.go_home() if not success: log_warn('Robot go_home failed!!!') ori = euler2quat([0, 0, np.pi / 2]) robot.pb_client.load_urdf('table/table.urdf', [.6, 0, 0.4], ori, scaling=0.9) box_size = 0.03 box_id1 = robot.pb_client.load_geom('box', size=box_size, mass=0.1, base_pos=[.5, 0.12, 1.0], rgba=[1, 0, 0, 1]) box_id2 = robot.pb_client.load_geom('box', size=box_size, mass=0.1, base_pos=[0.3, 0.12, 1.0], rgba=[0, 0, 1, 1]) robot.arm.eetool.open() obj_pos = robot.pb_client.get_body_state(box_id1)[0] move_dir = obj_pos - robot.arm.get_ee_pose()[0] move_dir[2] = 0 eef_step = 0.025 # an example of using IK with nullspace enabled ik_kwargs = dict(ns=True) robot.arm.move_ee_xyz(move_dir, eef_step=eef_step, **dict(ik_kwargs=ik_kwargs)) move_dir = np.zeros(3) move_dir[2] = obj_pos[2] - robot.arm.get_ee_pose()[0][2] robot.arm.move_ee_xyz(move_dir, eef_step=eef_step) robot.arm.eetool.close(wait=False) robot.arm.move_ee_xyz([0, 0, 0.3], eef_step=eef_step) obj_pos = robot.pb_client.get_body_state(box_id2)[0] move_dir = obj_pos - robot.arm.get_ee_pose()[0] move_dir[2] = 0 robot.arm.move_ee_xyz(move_dir, eef_step=eef_step) move_dir = obj_pos - robot.arm.get_ee_pose()[0] move_dir[2] += box_size * 2 robot.arm.move_ee_xyz(move_dir, eef_step=eef_step) robot.arm.eetool.open() move_dir[2] = 0.2 robot.arm.move_ee_xyz(move_dir, eef_step=eef_step) time.sleep(10)
def main(): """ Move all the joints of the robot in a sine-wave fashion. """ robot = Robot('ur5e') robot.arm.go_home() A = 0.2 f = 0.4 start_time = time.time() while True: elapsed_time = time.time() - start_time vels = [sin_wave(elapsed_time, f, A)] * robot.arm.arm_dof robot.arm.set_jvel(vels) time.sleep(0.01)
def main(): """ This function demonstrates how to load a custom end effector to the UR5e robot in pybullet. NOTE: This implementation using PyBullet's createConstraint function is known to have issues with fixed joints (fixed joints are not actually rigidly fixed). """ robot = Robot('ur5e', pb=True, use_eetool=False, arm_cfg={'self_collision': False}) robot.arm.go_home() # custom EE tool can either be added by making a new separate URDF # or using the load_geom capbility in pb_util file # uncomment below to load an example custom URDF root_path = os.path.dirname(os.path.realpath(__file__)) urdf_path = '../../../src/airobot/urdfs/custom_ee_tools/stick_ee_tool.urdf' stick_id = robot.pb_client.load_urdf( os.path.join(root_path, urdf_path), [0.3, 0.0, 0.0], [0.0, 0.0, 0.0, 1.0], ) # uncomment below to load a basic geometry with load_geom # from airobot.utils.pb_util import load_geom # stick_id = load_geom('cylinder', size=[0.015, 0.1524], mass=0.01, # base_pos=[0.3, 0.0, 0.0], rgba=[0, 1, 1, 1]) robot.pb_client.createConstraint(robot.arm.robot_id, robot.arm.ee_link_id, stick_id, -1, p.JOINT_FIXED, jointAxis=[1, 1, 1], parentFramePosition=[0, 0, 0], childFramePosition=[0, 0, -0.077], childFrameOrientation=euler2quat( [0, 0, 0])) time.sleep(5)
def main(): """ Move the robot end effector in a straight line. The pb=False flag is set because we are using the real robot (pb -- pybullet). First movement is executed with MoveIt!, and second movement is executed using urscript commands. """ robot = Robot('ur5e_2f140', pb=False, use_cam=False) robot.arm.go_home() robot.arm.move_ee_xyz([0.2, 0.0, 0.0], wait=True) time.sleep(1.0) robot.arm.set_comm_mode(use_urscript=True) robot.arm.move_ee_xyz([-0.2, 0.0, 0.0], wait=True)
def __init__(self, action_repeat=10, gui=True): self._action_repeat = action_repeat self.robot = Robot('ur5e_2f140', pb_cfg={'gui': gui, 'realtime': False}) self.ee_ori = [-np.sqrt(2) / 2, np.sqrt(2) / 2, 0, 0] self._action_bound = 1.0 self._ee_pos_scale = 0.02 self._ee_ori_scale = np.pi / 36.0 self._action_high = np.array([self._action_bound] * 5) self.action_space = spaces.Box(low=-self._action_high, high=self._action_high, dtype=np.float32) state_low = np.full(len(self._get_obs()), -float('inf')) state_high = np.full(len(self._get_obs()), float('inf')) self.observation_space = spaces.Box(state_low, state_high, dtype=np.float32) self.reset()
def main(): """ Rotate the end effector about a single axis, one at a time """ robot = Robot('ur5e_2f140') robot.arm.go_home() robot.arm.rot_ee_xyz(np.pi / 4, axis='x') time.sleep(1) robot.arm.rot_ee_xyz(-np.pi / 4, axis='x') time.sleep(3) robot.arm.rot_ee_xyz(np.pi / 4, axis='y') time.sleep(1) robot.arm.rot_ee_xyz(-np.pi / 4, axis='y') time.sleep(3) robot.arm.rot_ee_xyz(np.pi / 4, axis='z') time.sleep(1) robot.arm.rot_ee_xyz(-np.pi / 4, axis='z') time.sleep(3)
def main(): """ 3D Point cloud visualization of the current scene. This is useful to see how does the point cloud that the pushing script works on look like """ parser = argparse.ArgumentParser(description='Argument Parser') parser.add_argument('--z_min', type=float, default=-0.15, help='minimium acceptable z value') args = parser.parse_args() np.set_printoptions(precision=4, suppress=True) robot = Robot('ur5e_2f140', pb=False, use_cam=True) pre_jnts = [1.57, -1.66, -1.92, -1.12, 1.57, 0] robot.arm.set_jpos(pre_jnts) cam_pos, cam_ori = read_cam_ext('ur5e') robot.cam.set_cam_ext(cam_pos, cam_ori) vis = open3d.visualization.Visualizer() vis.create_window("Point Cloud") pcd = open3d.geometry.PointCloud() pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=True) pts, colors = filter_points(pts, colors, z_lowest=args.z_min) pcd.points = open3d.utility.Vector3dVector(pts) pcd.colors = open3d.utility.Vector3dVector(colors / 255.0) coord = open3d.geometry.TriangleMesh.create_coordinate_frame(1, [0, 0, 0]) vis.add_geometry(coord) vis.add_geometry(pcd) while True: pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=True) pts, colors = filter_points(pts, colors, z_lowest=args.z_min) pcd.points = open3d.utility.Vector3dVector(pts) pcd.colors = open3d.utility.Vector3dVector(colors / 255.0) vis.update_geometry() vis.poll_events() vis.update_renderer() time.sleep(0.1)
def main(): """ Move the robot end effector to the desired pose. """ robot = Robot('ur5e') robot.arm.go_home() tgt_pos = [0.45, 0.2, 1.3] robot.arm.set_ee_pose(tgt_pos) # sleep statement is not necessary time.sleep(1) tgt_pos = [0.6, -0.15, 1.2] tgt_euler = [1.57, 0.0, 0.0] robot.arm.set_ee_pose(tgt_pos, tgt_euler) time.sleep(1) pos, quat, rot, euler = robot.arm.get_ee_pose() log_info('End effector pose:') log_info('Position:') log_info(pos) log_info('Euler angles:') log_info(euler)
def main(): """ Push the objects on the table randomly. The robot will first go to a reset position so that the robot arm does not block the camera's view. And a clustering alogrithm is used to cluster the 3D point cloud of the objects on the table. Then the gripper pushs a randomly selected object on the table. And the gripper goes back to a reset position. """ parser = argparse.ArgumentParser(description='Argument Parser') parser.add_argument('--z_min', type=float, default=-0.15, help='minimium acceptable z value') args = parser.parse_args() np.set_printoptions(precision=4, suppress=True) bot = Robot('ur5e_2f140', pb=False, use_cam=True) cam_pos, cam_ori = read_cam_ext('ur5e') bot.cam.set_cam_ext(cam_pos, cam_ori) bot.arm.go_home() bot.arm.eetool.activate() bot.arm.eetool.close() pre_jnts = [1.57, -1.66, -1.92, -1.12, 1.57, 0] bot.arm.set_jpos(pre_jnts) time.sleep(1) ee_pose = bot.arm.get_ee_pose() reset_pos = ee_pose[0] print('Reset pos:', reset_pos) while not rospy.is_shutdown(): try: push(bot, reset_pos, z_lowest=args.z_min) except Exception as e: print(e) time.sleep(1)
def main(): """ Visualize the point cloud from the RGBD camera. """ robot = Robot('ur5e_2f140', pb=False, use_cam=True, use_arm=False) cam_pos, cam_ori = read_cam_ext('ur5e') robot.cam.set_cam_ext(cam_pos, cam_ori) vis = open3d.visualization.Visualizer() vis.create_window("Point Cloud") pcd = open3d.geometry.PointCloud() pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=False) pcd.points = open3d.utility.Vector3dVector(pts) pcd.colors = open3d.utility.Vector3dVector(colors / 255.0) vis.add_geometry(pcd) while True: pts, colors = robot.cam.get_pcd(in_world=True, filter_depth=False) pcd.points = open3d.utility.Vector3dVector(pts) pcd.colors = open3d.utility.Vector3dVector(colors / 255.0) vis.update_geometry() vis.poll_events() vis.update_renderer() time.sleep(0.1)
def main(): """ This function demonstrates how to get joint information such as joint positions/velocities/torques. """ robot = Robot('ur5e_2f140', pb_cfg={'gui': True}) robot.arm.go_home() log_info('\nJoint positions for all actuated joints:') jpos = robot.arm.get_jpos() log_info(np.round(jpos, decimals=3)) joint = 'shoulder_pan_joint' log_info('Joint [%s] position: %.3f' % (joint, robot.arm.get_jpos('shoulder_pan_joint'))) log_info('Joint velocities:') jvel = robot.arm.get_jvel() log_info(np.round(jvel, decimals=3)) log_info('Joint torques:') jtorq = robot.arm.get_jtorq() log_info(np.round(jtorq, decimals=3)) robot.arm.eetool.close() log_info('Gripper position (close): %.3f' % robot.arm.eetool.get_pos()) robot.arm.eetool.open() log_info('Gripper position (open): %.3f' % robot.arm.eetool.get_pos())
def main(args): cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml" cfg = get_cfg_defaults() cfg.merge_from_file(cfg_file) cfg.freeze() rospy.init_node('replay') with open(os.join(args.data_dir, args.data_file), 'rb') as data_f: data = pickle.load(data_f) yumi_ar = Robot('yumi', pb=True, arm_cfg={ 'render': True, 'self_collision': False }) if args.object: box_id = pb_util.load_urdf( args.config_package_path + 'descriptions/urdf/' + args.object_name + '.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:])
def worker_yumi(child_conn): while True: # print("here!") try: if not child_conn.poll(0.0001): continue msg = child_conn.recv() except (EOFError, KeyboardInterrupt): break if msg == "RESET": yumi = Robot('yumi', pb=True, arm_cfg={'render': True, 'self_collision': False}) # client_id = p.connect(p.DIRECT) print("\n\nfinished worker construction\n\n") continue if msg == "HOME": yumi.arm.go_home() continue if msg == "END": break print("before sleep!") time.sleep(0.01) print("breaking") child_conn.close()
def main(args): cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml" cfg = get_cfg_defaults() cfg.merge_from_file(cfg_file) cfg.freeze() rospy.init_node('MacroActions') data = {} data['saved_data'] = [] data['metadata'] = {} # parent1, child1 = Pipe() # parent2, child2 = Pipe() # work_queue = Queue() # result_queue = Queue() # p1 = Process(target=worker_yumi, args=(child1, work_queue, result_queue, cfg, args)) # p2 = Process(target=worker_yumi, args=(child2, work_queue, result_queue, cfg, args)) # p1.start() # p2.start() # parent1.send("RESET") # parent2.send("RESET") # print("started workers") # time.sleep(15.0) # embed() # # setup yumi yumi_ar = Robot('yumi', pb=True, arm_cfg={ 'render': args.visualize, 'self_collision': False, 'rt_simulation': False }) yumi_ar.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT) gel_id = 12 alpha = 0.01 K = 500 restitution = 0.99 dynamics_info = {} dynamics_info['contactDamping'] = alpha * K dynamics_info['contactStiffness'] = K dynamics_info['rollingFriction'] = args.rolling dynamics_info['restitution'] = restitution p.changeDynamics(yumi_ar.arm.robot_id, gel_id, restitution=restitution, contactStiffness=K, contactDamping=alpha * K, rollingFriction=args.rolling) # setup yumi_gs # yumi_gs = YumiGelslimPybulet(yumi_ar, cfg, exec_thread=args.execute_thread) yumi_gs = YumiCamsGS(yumi_ar, cfg, exec_thread=args.execute_thread) if args.object: box_id = pb_util.load_urdf( args.config_package_path + 'descriptions/urdf/' + args.object_name + '.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:]) # trans_box_id = pb_util.load_urdf( # args.config_package_path + # 'descriptions/urdf/'+args.object_name+'_trans.urdf', # cfg.OBJECT_POSE_3[0:3], # cfg.OBJECT_POSE_3[3:] # ) # setup macro_planner action_planner = ClosedLoopMacroActions(cfg, yumi_gs, box_id, pb_util.PB_CLIENT, args.config_package_path, replan=args.replan) manipulated_object = None object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT) object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL) palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT) palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT) example_args = {} example_args['object_pose1_world'] = object_pose1_world example_args['object_pose2_world'] = object_pose2_world example_args['palm_pose_l_object'] = palm_pose_l_object example_args['palm_pose_r_object'] = palm_pose_r_object example_args['object'] = manipulated_object # example_args['N'] = 60 # 60 example_args['N'] = calc_n(object_pose1_world, object_pose2_world) # 60 print("N: " + str(example_args['N'])) example_args['init'] = True example_args['table_face'] = 0 primitive_name = args.primitive mesh_file = args.config_package_path + 'descriptions/meshes/objects/' + args.object_name + '_experiments.stl' exp_single = SingleArmPrimitives(cfg, box_id, mesh_file) exp_double = DualArmPrimitives(cfg, box_id, mesh_file) # trans_box_lock = threading.RLock() # goal_viz = GoalVisual( # trans_box_lock, # trans_box_id, # action_planner.pb_client, # cfg.OBJECT_POSE_3) # visualize_goal_thread = threading.Thread( # target=goal_viz.visualize_goal_state) # visualize_goal_thread.daemon = True # visualize_goal_thread.start() data['metadata']['mesh_file'] = mesh_file data['metadata']['cfg'] = cfg data['metadata']['dynamics'] = dynamics_info data['metadata']['cam_cfg'] = yumi_gs.cam_cfg if args.debug: init_id = exp.get_rand_init(ind=2)[-1] obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id]) point, normal, face = exp.sample_contact(primitive_name) # embed() world_pose = exp.get_palm_pose_world_frame( point, normal, primitive_name=primitive_name) obj_pos_world = list( p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[0]) obj_ori_world = list( p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[1]) obj_pose_world = util.list2pose_stamped(obj_pos_world + obj_ori_world) contact_obj_frame = util.convert_reference_frame( world_pose, obj_pose_world, util.unit_pose()) example_args['palm_pose_r_object'] = contact_obj_frame example_args['object_pose1_world'] = obj_pose_world obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id]) obj_pose_final.pose.position.z = obj_pose_world.pose.position.z / 1.175 print("init: ") print(util.pose_stamped2list(object_pose1_world)) print("final: ") print(util.pose_stamped2list(obj_pose_final)) example_args['object_pose2_world'] = obj_pose_final example_args['table_face'] = init_id plan = action_planner.get_primitive_plan(primitive_name, example_args, 'right') embed() import simulation for i in range(10): simulation.visualize_object( object_pose1_world, filepath= "package://config/descriptions/meshes/objects/realsense_box_experiments.stl", name="/object_initial", color=(1., 0., 0., 1.), frame_id="/yumi_body", scale=(1., 1., 1.)) simulation.visualize_object( object_pose2_world, filepath= "package://config/descriptions/meshes/objects/realsense_box_experiments.stl", name="/object_final", color=(0., 0., 1., 1.), frame_id="/yumi_body", scale=(1., 1., 1.)) rospy.sleep(.1) simulation.simulate(plan) else: global_start = time.time() for trial in range(20): k = 0 while True: # sample a random stable pose, and get the corresponding # stable orientation index k += 1 # init_id = exp.get_rand_init()[-1] init_id = exp.get_rand_init(ind=0)[-1] # sample a point on the object that is valid # for the primitive action being executed point, normal, face = exp.sample_contact( primitive_name=primitive_name) if point is not None: break if k >= 10: print("FAILED") return # get the full 6D pose palm in world, at contact location palm_pose_world = exp.get_palm_pose_world_frame( point, normal, primitive_name=primitive_name) # get the object pose in the world frame # if trial == 0: # parent1.send("OBJECT_POSE") # elif trial == 1: # parent2.send("OBJECT_POSE") obj_pos_world = list( p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[0]) obj_ori_world = list( p.getBasePositionAndOrientation(box_id, pb_util.PB_CLIENT)[1]) obj_pose_world = util.list2pose_stamped(obj_pos_world + obj_ori_world) # obj_pose_world = work_queue.get(block=True) # transform the palm pose from the world frame to the object frame contact_obj_frame = util.convert_reference_frame( palm_pose_world, obj_pose_world, util.unit_pose()) # set up inputs to the primitive planner, based on task # including sampled initial object pose and contacts, # and final object pose example_args['palm_pose_r_object'] = contact_obj_frame example_args['object_pose1_world'] = obj_pose_world obj_pose_final = util.list2pose_stamped(exp.init_poses[init_id]) obj_pose_final.pose.position.z /= 1.18 print("init: ") print(util.pose_stamped2list(object_pose1_world)) print("final: ") print(util.pose_stamped2list(obj_pose_final)) example_args['object_pose2_world'] = obj_pose_final example_args['table_face'] = init_id example_args['primitive_name'] = primitive_name example_args['N'] = calc_n(obj_pose_world, obj_pose_final) print("N: " + str(example_args['N'])) # if trial == 0: # goal_viz.update_goal_state(exp.init_poses[init_id]) try: # get observation (images/point cloud) obs = yumi_gs.get_observation(obj_id=box_id) # get start/goal (obj_pose_world, obj_pose_final) start = util.pose_stamped2list(obj_pose_world) goal = util.pose_stamped2list(obj_pose_final) # get corners (from exp? that has mesh) keypoints_start = np.array(exp.mesh_world.vertices.tolist()) keypoints_start_homog = np.hstack( (keypoints_start, np.ones((keypoints_start.shape[0], 1)))) goal_start_frame = util.convert_reference_frame( pose_source=obj_pose_final, pose_frame_target=obj_pose_world, pose_frame_source=util.unit_pose()) goal_start_frame_mat = util.matrix_from_pose(goal_start_frame) keypoints_goal = np.matmul(goal_start_frame_mat, keypoints_start_homog.T).T # get contact (palm pose object frame) contact_obj_frame = util.pose_stamped2list(contact_obj_frame) contact_world_frame = util.pose_stamped2list(palm_pose_world) contact_pos = open3d.utility.DoubleVector( np.array(contact_world_frame[:3])) kdtree = open3d.geometry.KDTreeFlann(obs['pcd_full']) nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0] nearest_pt_world = np.asarray( obs['pcd_full'].points)[nearest_pt_ind] # embed() result = action_planner.execute(primitive_name, example_args) sample = {} sample['obs'] = obs sample['start'] = start sample['goal'] = goal sample['keypoints_start'] = keypoints_start sample['keypoints_goal'] = keypoints_goal sample['transformation'] = util.pose_stamped2list( goal_start_frame) sample['contact_obj_frame'] = contact_obj_frame sample['contact_world_frame'] = contact_world_frame sample['contact_pcd'] = nearest_pt_world sample['result'] = result sample['planner_args'] = example_args data['saved_data'].append(sample) # if trial == 0: # parent1.send("SAMPLE") # elif trial == 1: # parent2.send("SAMPLE") # result = work_queue.get(block=True) # if trial == 0: # parent1.send("SAMPLE") # elif trial == 1: # parent2.send("SAMPLE") # parent1.send("SAMPLE") # parent2.send("SAMPLE") # start = time.time() # done = False # result_list = [] # while (time.time() - start) < cfg.TIMEOUT and not done: # try: # result = result_queue.get(block=True) # result_list.append(result) # if len(result_list) == 2: # done = True # except result_queue.Empty: # continue # time.sleep(0.001) print("reached final: " + str(result[0])) except ValueError: print("moveit failed!") # time.sleep(0.1) # yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT) j_pos = cfg.RIGHT_INIT + cfg.LEFT_INIT for ind, jnt_id in enumerate(yumi_ar.arm.arm_jnt_ids): p.resetJointState(yumi_ar.arm.robot_id, jnt_id, targetValue=j_pos[ind]) # p.resetJointStatesMultiDof( # yumi_ar.arm.robot_id, # yumi_ar.arm.arm_jnt_ids, # targetValues=j_pos) # parent1.send("HOME") # parent2.send("HOME") # time.sleep(1.0) # embed() # embed() print("TOTAL TIME: " + str(time.time() - global_start)) with open('data/sample_data_right_hand_pull.pkl', 'wb') as data_f: pickle.dump(data, data_f)
def main(args): cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml" cfg = get_cfg_defaults() cfg.merge_from_file(cfg_file) cfg.freeze() rospy.init_node('MacroActions') signal.signal(signal.SIGINT, signal_handler) data = {} data['saved_data'] = [] data['metadata'] = {} # parent1, child1 = Pipe() # parent2, child2 = Pipe() # work_queue = Queue() # result_queue = Queue() # p1 = Process(target=worker_yumi, args=(child1, work_queue, result_queue, cfg, args)) # p2 = Process(target=worker_yumi, args=(child2, work_queue, result_queue, cfg, args)) # p1.start() # p2.start() # parent1.send("RESET") # parent2.send("RESET") # print("started workers") # time.sleep(15.0) # embed() # # setup yumi # data_seed = 1 data_seed = args.np_seed np.random.seed(data_seed) yumi_ar = Robot('yumi_palms', pb=True, pb_cfg={'gui': args.visualize}, arm_cfg={'self_collision': False, 'seed': data_seed}) r_gel_id = cfg.RIGHT_GEL_ID l_gel_id = cfg.LEFT_GEL_ID alpha = cfg.ALPHA K = cfg.GEL_CONTACT_STIFFNESS restitution = cfg.GEL_RESTITUION p.changeDynamics( yumi_ar.arm.robot_id, r_gel_id, restitution=restitution, contactStiffness=K, contactDamping=alpha*K, rollingFriction=args.rolling ) p.changeDynamics( yumi_ar.arm.robot_id, l_gel_id, restitution=restitution, contactStiffness=K, contactDamping=alpha*K, rollingFriction=args.rolling ) dynamics_info = {} dynamics_info['contactDamping'] = alpha*K dynamics_info['contactStiffness'] = K dynamics_info['rollingFriction'] = args.rolling dynamics_info['restitution'] = restitution yumi_gs = YumiCamsGS( yumi_ar, cfg, exec_thread=False, sim_step_repeat=args.step_repeat) for _ in range(10): yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT) if args.object: obj_id = yumi_ar.pb_client.load_urdf( args.config_package_path + 'descriptions/urdf/'+args.object_name+'.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:] ) goal_obj_id = yumi_ar.pb_client.load_urdf( args.config_package_path + 'descriptions/urdf/'+args.object_name+'_trans.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:] ) p.setCollisionFilterPair(yumi_ar.arm.robot_id, goal_obj_id, r_gel_id, -1, enableCollision=False) p.setCollisionFilterPair(obj_id, goal_obj_id, -1, -1, enableCollision=False) p.setCollisionFilterPair(yumi_ar.arm.robot_id, obj_id, r_gel_id, -1, enableCollision=True) p.setCollisionFilterPair(yumi_ar.arm.robot_id, obj_id, 27, -1, enableCollision=True) yumi_ar.pb_client.reset_body( obj_id, cfg.OBJECT_POSE_3[:3], cfg.OBJECT_POSE_3[3:]) yumi_ar.pb_client.reset_body( goal_obj_id, cfg.OBJECT_POSE_3[:3], cfg.OBJECT_POSE_3[3:]) manipulated_object = None object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT) object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL) palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT) palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT) example_args = {} example_args['object_pose1_world'] = object_pose1_world example_args['object_pose2_world'] = object_pose2_world example_args['palm_pose_l_object'] = palm_pose_l_object example_args['palm_pose_r_object'] = palm_pose_r_object example_args['object'] = manipulated_object example_args['N'] = 60 example_args['init'] = True example_args['table_face'] = 0 primitive_name = args.primitive mesh_file = args.config_package_path + 'descriptions/meshes/objects/' + args.object_name + '_experiments.stl' exp_single = SingleArmPrimitives( cfg, yumi_ar.pb_client.get_client_id(), obj_id, mesh_file) if primitive_name == 'grasp' or primitive_name == 'pivot': exp_double = DualArmPrimitives( cfg, yumi_ar.pb_client.get_client_id(), obj_id, mesh_file) exp_running = exp_double else: exp_running = exp_single # setup macro_planner action_planner = ClosedLoopMacroActions( cfg, yumi_gs, obj_id, yumi_ar.pb_client.get_client_id(), args.config_package_path, replan=args.replan, object_mesh_file=mesh_file ) data['metadata']['mesh_file'] = mesh_file data['metadata']['cfg'] = cfg data['metadata']['dynamics'] = dynamics_info data['metadata']['cam_cfg'] = yumi_gs.cam_setup_cfg data['metadata']['step_repeat'] = args.step_repeat delta_z_height = 0.95 with open(args.config_package_path+'descriptions/urdf/'+args.object_name+'.urdf', 'rb') as f: urdf_txt = f.read() data['metadata']['object_urdf'] = urdf_txt data['metadata']['delta_z_height'] = delta_z_height data['metadata']['step_repeat'] = args.step_repeat data['metadata']['seed'] = data_seed metadata = data['metadata'] if args.multi: cuboid_sampler = CuboidSampler( '/root/catkin_ws/src/primitives/objects/cuboids/nominal_cuboid.stl', pb_client=yumi_ar.pb_client) cuboid_fname_template = '/root/catkin_ws/src/primitives/objects/cuboids/' cuboid_manager = MultiBlockManager( cuboid_fname_template, cuboid_sampler, robot_id=yumi_ar.arm.robot_id, table_id=27, r_gel_id=r_gel_id, l_gel_id=l_gel_id) yumi_ar.pb_client.remove_body(obj_id) yumi_ar.pb_client.remove_body(goal_obj_id) cuboid_fname = cuboid_manager.get_cuboid_fname() obj_id, sphere_ids, mesh, goal_obj_id = \ cuboid_sampler.sample_cuboid_pybullet( cuboid_fname, goal=True, keypoints=False) cuboid_manager.filter_collisions(obj_id, goal_obj_id) action_planner.update_object(obj_id, mesh_file) trans_box_lock = threading.RLock() goal_viz = GoalVisual( trans_box_lock, goal_obj_id, action_planner.pb_client, cfg.OBJECT_POSE_3) pickle_path = os.path.join( args.data_dir, primitive_name, args.experiment_name ) if not os.path.exists(pickle_path): os.makedirs(pickle_path) data_manager = DataManager(pickle_path) if args.save_data: with open(os.path.join(pickle_path, 'metadata.pkl'), 'wb') as mdata_f: pickle.dump(metadata, mdata_f) if args.debug: if args.multi: cuboid_sampler.delete_cuboid(obj_id, goal_obj_id, sphere_ids) cuboid_fname = cuboid_manager.get_cuboid_fname() obj_id, sphere_ids, mesh, goal_obj_id = cuboid_sampler.sample_cuboid_pybullet( cuboid_fname, goal=True, keypoints=False) cuboid_manager.filter_collisions(obj_id, goal_obj_id) goal_viz.update_goal_obj(goal_obj_id) p.changeDynamics( obj_id, -1, lateralFriction=0.4 ) action_planner.update_object(obj_id, mesh_file) exp_running.initialize_object(obj_id, cuboid_fname) print('Reset multi block!') else: cuboid_fname = '/root/catkin_ws/src/config/descriptions/meshes/objects/cuboids/realsense_box_experiments.stl' for _ in range(args.num_obj_samples): if primitive_name == 'pull': init_id = exp_running.get_rand_init(ind=2)[-1] obj_pose_final = util.list2pose_stamped(exp_running.init_poses[init_id]) point, normal, face = exp_running.sample_contact(primitive_name) world_pose = exp_running.get_palm_poses_world_frame( point, normal, primitive_name=primitive_name) obj_pos_world = list(p.getBasePositionAndOrientation( obj_id, yumi_ar.pb_client.get_client_id())[0]) obj_ori_world = list(p.getBasePositionAndOrientation( obj_id, yumi_ar.pb_client.get_client_id())[1]) obj_pose_world = util.list2pose_stamped(obj_pos_world + obj_ori_world) contact_obj_frame = util.convert_reference_frame( world_pose, obj_pose_world, util.unit_pose()) example_args['palm_pose_r_object'] = contact_obj_frame example_args['object_pose1_world'] = obj_pose_world obj_pose_final = util.list2pose_stamped(exp_running.init_poses[init_id]) obj_pose_final.pose.position.z = obj_pose_world.pose.position.z/1.175 print("init: ") print(util.pose_stamped2list(object_pose1_world)) print("final: ") print(util.pose_stamped2list(obj_pose_final)) example_args['object_pose2_world'] = obj_pose_final example_args['table_face'] = init_id elif primitive_name == 'grasp': k = 0 have_contact = False contact_face = None while True: x, y, dq, q, init_id = exp_running.get_rand_init() obj_pose_world_nom = exp_running.get_obj_pose()[0] palm_poses_world = exp_running.get_palm_poses_world_frame( init_id, obj_pose_world_nom, [x, y, dq]) # get_palm_poses_world_frame may adjust the # initial object pose, so need to check it again obj_pose_world = exp_running.get_obj_pose()[0] if palm_poses_world is not None: have_contact = True break k += 1 if k >= 10: print("FAILED") break if have_contact: obj_pose_final = exp_running.goal_pose_world_frame_mod palm_poses_obj_frame = {} for key in palm_poses_world.keys(): palm_poses_obj_frame[key] = util.convert_reference_frame( palm_poses_world[key], obj_pose_world, util.unit_pose()) example_args['palm_pose_r_object'] = palm_poses_obj_frame['right'] example_args['palm_pose_l_object'] = palm_poses_obj_frame['left'] example_args['object_pose1_world'] = obj_pose_world example_args['object_pose2_world'] = obj_pose_final example_args['table_face'] = init_id plan = action_planner.get_primitive_plan(primitive_name, example_args, 'right') embed() import simulation for i in range(10): simulation.visualize_object( object_pose1_world, filepath="package://config/descriptions/meshes/objects/cuboids/" + cuboid_fname.split('objects/cuboids')[1], name="/object_initial", color=(1., 0., 0., 1.), frame_id="/yumi_body", scale=(1., 1., 1.)) simulation.visualize_object( object_pose2_world, filepath="package://config/descriptions/meshes/objects/cuboids/" + cuboid_fname.split('objects/cuboids')[1], name="/object_final", color=(0., 0., 1., 1.), frame_id="/yumi_body", scale=(1., 1., 1.)) rospy.sleep(.1) simulation.simulate(plan, cuboid_fname.split('objects/cuboids')[1]) else: global_start = time.time() face = 0 # exp_double.reset_graph(face) start_time = time.time() success = 0 for trial in range(args.num_trials): k = 0 if args.multi: cuboid_sampler.delete_cuboid(obj_id, goal_obj_id, sphere_ids) cuboid_fname = cuboid_manager.get_cuboid_fname() obj_id, sphere_ids, mesh, goal_obj_id = cuboid_sampler.sample_cuboid_pybullet( cuboid_fname, goal=True, keypoints=False) cuboid_manager.filter_collisions(obj_id, goal_obj_id) goal_viz.update_goal_obj(goal_obj_id) p.changeDynamics( obj_id, -1, lateralFriction=0.4 ) action_planner.update_object(obj_id, mesh_file) exp_running.initialize_object(obj_id, cuboid_fname) print('Reset multi block!') for _ in range(args.num_obj_samples): while True: have_contact = False # sample a random stable pose, and get the corresponding # stable orientation index k += 1 if primitive_name == 'pull': # init_id = exp_running.get_rand_init()[-1] init_id = exp_running.get_rand_init()[-1] # sample a point on the object that is valid # for the primitive action being executed point, normal, face = exp_running.sample_contact( primitive_name=primitive_name) if point is not None: break elif primitive_name == 'grasp': x, y, dq, q, init_id = exp_double.get_rand_init() obj_pose_world_nom = exp_double.get_obj_pose()[0] palm_poses_world = exp_double.get_palm_poses_world_frame( init_id, obj_pose_world_nom, [x, y, dq]) obj_pose_world = exp_double.get_obj_pose()[0] if palm_poses_world is not None: have_contact = True break if k >= 10: print("FAILED") return # for _ in range(10): # yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT) if primitive_name == 'pull': # get the full 6D pose palm in world, at contact location palm_pose_world = exp_running.get_palm_poses_world_frame( point, normal, primitive_name=primitive_name) # get the object pose in the world frame # if trial == 0: # parent1.send("OBJECT_POSE") # elif trial == 1: # parent2.send("OBJECT_POSE") obj_pos_world = list(p.getBasePositionAndOrientation( obj_id, yumi_ar.pb_client.get_client_id())[0]) obj_ori_world = list(p.getBasePositionAndOrientation( obj_id, yumi_ar.pb_client.get_client_id())[1]) obj_pose_world = util.list2pose_stamped( obj_pos_world + obj_ori_world) # obj_pose_world = work_queue.get(block=True) # transform the palm pose from the world frame to the object frame contact_obj_frame = util.convert_reference_frame( palm_pose_world, obj_pose_world, util.unit_pose()) # set up inputs to the primitive planner, based on task # including sampled initial object pose and contacts, # and final object pose example_args['palm_pose_r_object'] = contact_obj_frame example_args['object_pose1_world'] = obj_pose_world # obj_pose_final = util.list2pose_stamped(exp_running.init_poses[init_id]) x, y, q, _ = exp_running.get_rand_init(execute=False, ind=init_id) final_nominal = exp_running.init_poses[init_id] final_nominal[0] = x final_nominal[1] = y final_nominal[3:] = q obj_pose_final = util.list2pose_stamped(final_nominal) goal_viz.update_goal_state(final_nominal) obj_pose_final.pose.position.z += cfg.TABLE_HEIGHT example_args['object_pose2_world'] = obj_pose_final example_args['table_face'] = init_id example_args['primitive_name'] = primitive_name example_args['N'] = exp_running.calc_n( obj_pose_world, obj_pose_final) elif primitive_name == 'grasp': if have_contact: obj_pose_final = exp_double.goal_pose_world_frame_mod palm_poses_obj_frame = {} for key in palm_poses_world.keys(): palm_poses_obj_frame[key] = util.convert_reference_frame( palm_poses_world[key], obj_pose_world, util.unit_pose() ) example_args['palm_pose_r_object'] = palm_poses_obj_frame['right'] example_args['palm_pose_l_object'] = palm_poses_obj_frame['left'] example_args['object_pose1_world'] = obj_pose_world example_args['object_pose2_world'] = obj_pose_final example_args['table_face'] = init_id else: continue try: obs, pcd = yumi_gs.get_observation(obj_id=obj_id) start = util.pose_stamped2list(obj_pose_world) goal = util.pose_stamped2list(obj_pose_final) keypoints_start = np.array(exp_running.mesh_world.vertices.tolist()) keypoints_start_homog = np.hstack( (keypoints_start, np.ones((keypoints_start.shape[0], 1))) ) start_mat = util.matrix_from_pose(obj_pose_world) goal_mat = util.matrix_from_pose(obj_pose_final) T_mat = np.matmul(goal_mat, np.linalg.inv(start_mat)) keypoints_goal = np.matmul(T_mat, keypoints_start_homog.T).T[:, :3] contact_obj_frame_dict = {} contact_world_frame_dict = {} nearest_pt_world_dict = {} if primitive_name == 'pull': active_arm, inactive_arm = action_planner.get_active_arm( util.pose_stamped2list(obj_pose_world) ) # get contact (palm pose object frame) contact_obj_frame_dict[active_arm] = util.pose_stamped2list(contact_obj_frame) contact_world_frame_dict[active_arm] = util.pose_stamped2list(palm_pose_world) contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[active_arm][:3])) kdtree = open3d.geometry.KDTreeFlann(pcd) # nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0] # nearest_pt_world_dict[active_arm] = np.asarray(pcd.points)[nearest_pt_ind] contact_obj_frame_dict[inactive_arm] = None contact_world_frame_dict[inactive_arm] = None nearest_pt_world_dict[inactive_arm] = None elif primitive_name == 'grasp': for key in palm_poses_obj_frame.keys(): contact_obj_frame_dict[key] = util.pose_stamped2list(palm_poses_obj_frame[key]) contact_world_frame_dict[key] = util.pose_stamped2list(palm_poses_world[key]) contact_pos = open3d.utility.DoubleVector(np.array(contact_world_frame_dict[key][:3])) kdtree = open3d.geometry.KDTreeFlann(pcd) # nearest_pt_ind = kdtree.search_knn_vector_3d(contact_pos, 1)[1][0] # nearest_pt_world_dict[key] = np.asarray(pcd.points)[nearest_pt_ind] result = action_planner.execute(primitive_name, example_args) if result is not None: print('Trial number: ' + str(trial) + ', reached final: ' + str(result[0])) print('Time so far: ' + str(time.time() - start_time)) if result[0]: success += 1 sample = {} sample['obs'] = obs sample['start'] = start sample['goal'] = goal sample['keypoints_start'] = keypoints_start sample['keypoints_goal'] = keypoints_goal sample['transformation'] = util.pose_from_matrix(T_mat) sample['contact_obj_frame'] = contact_obj_frame_dict sample['contact_world_frame'] = contact_world_frame_dict # sample['contact_pcd'] = nearest_pt_world_dict sample['result'] = result if primitive_name == 'grasp': sample['goal_face'] = exp_double.goal_face if args.save_data: data_manager.save_observation(sample, str(trial)) print("Success: " + str(success)) else: continue # data['saved_data'].append(sample) # if trial == 0: # parent1.send("SAMPLE") # elif trial == 1: # parent2.send("SAMPLE") # result = work_queue.get(block=True) # if trial == 0: # parent1.send("SAMPLE") # elif trial == 1: # parent2.send("SAMPLE") # parent1.send("SAMPLE") # parent2.send("SAMPLE") # start = time.time() # done = False # result_list = [] # while (time.time() - start) < cfg.TIMEOUT and not done: # try: # result = result_queue.get(block=True) # result_list.append(result) # if len(result_list) == 2: # done = True # except result_queue.Empty: # continue # time.sleep(0.001) except ValueError as e: print("Value error: ") print(e) # time.sleep(1.0) # pose = util.pose_stamped2list(yumi_gs.compute_fk(yumi_gs.get_jpos(arm='right'))) # pos, ori = pose[:3], pose[3:] # # pose = yumi_gs.get_ee_pose() # # pos, ori = pose[0], pose[1] # # pos[2] -= 0.0714 # pos[2] += 0.001 # r_jnts = yumi_gs.compute_ik(pos, ori, yumi_gs.get_jpos(arm='right')) # l_jnts = yumi_gs.get_jpos(arm='left') # if r_jnts is not None: # for _ in range(10): # pos[2] += 0.001 # r_jnts = yumi_gs.compute_ik(pos, ori, yumi_gs.get_jpos(arm='right')) # l_jnts = yumi_gs.get_jpos(arm='left') # if r_jnts is not None: # yumi_gs.update_joints(list(r_jnts) + l_jnts) # time.sleep(0.1) time.sleep(0.1) for _ in range(10): yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT) # for _ in range(10): # j_pos = cfg.RIGHT_INIT + cfg.LEFT_INIT # for ind, jnt_id in enumerate(yumi_ar.arm.arm_jnt_ids): # p.resetJointState( # yumi_ar.arm.robot_id, # jnt_id, # targetValue=j_pos[ind] # ) # yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT) # p.resetJointStatesMultiDof( # yumi_ar.arm.robot_id, # yumi_ar.arm.arm_jnt_ids, # targetValues=j_pos) # parent1.send("HOME") # parent2.send("HOME") # time.sleep(1.0) print("TOTAL TIME: " + str(time.time() - global_start))
def worker_yumi(child_conn, work_queue, result_queue, cfg, args): while True: # print("here!") try: if not child_conn.poll(0.0001): continue msg = child_conn.recv() except (EOFError, KeyboardInterrupt): break if msg == "RESET": # yumi = Robot('yumi', pb=True, arm_cfg={'render': True, 'self_collision': False}) # client_id = p.connect(p.DIRECT) # print("\n\nfinished worker construction\n\n") yumi_ar = Robot('yumi', pb=True, arm_cfg={'render': True, 'self_collision': False}) yumi_ar.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT) gel_id = 12 alpha = 0.01 K = 500 p.changeDynamics( yumi_ar.arm.robot_id, gel_id, restitution=0.99, contactStiffness=K, contactDamping=alpha*K, rollingFriction=args.rolling ) # setup yumi_gs yumi_gs = YumiGelslimPybulet(yumi_ar, cfg, exec_thread=args.execute_thread, sim_step_repeat=args.step_repeat) obj_id = yumi_ar.pb_client.load_urdf( args.config_package_path + 'descriptions/urdf/'+args.object_name+'.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:] ) trans_box_id = yumi_ar.pb_client.load_urdf( args.config_package_path + 'descriptions/urdf/'+args.object_name+'_trans.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:] ) # setup macro_planner action_planner = ClosedLoopMacroActions( cfg, yumi_gs, obj_id, yumi_ar.pb_client.get_client_id(), args.config_package_path, replan=args.replan ) continue if msg == "HOME": yumi_gs.update_joints(cfg.RIGHT_INIT + cfg.LEFT_INIT) continue if msg == "OBJECT_POSE": obj_pos_world = list(p.getBasePositionAndOrientation( obj_id, yumi_ar.pb_client.get_client_id())[0]) obj_ori_world = list(p.getBasePositionAndOrientation( obj_id, yumi_ar.pb_client.get_client_id())[1]) obj_pose_world = util.list2pose_stamped( obj_pos_world + obj_ori_world) work_queue.put(obj_pose_world) continue if msg == "SAMPLE": # try: # example_args = work_queue.get(block=True) # primitive_name = example_args['primitive_name'] # result = action_planner.execute(primitive_name, example_args) # work_queue.put(result) # except work_queue.Empty: # continue manipulated_object = None object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT) object_pose2_world = util.list2pose_stamped(cfg.OBJECT_FINAL) palm_pose_l_object = util.list2pose_stamped(cfg.PALM_LEFT) palm_pose_r_object = util.list2pose_stamped(cfg.PALM_RIGHT) example_args = {} example_args['object_pose1_world'] = object_pose1_world example_args['object_pose2_world'] = object_pose2_world example_args['palm_pose_l_object'] = palm_pose_l_object example_args['palm_pose_r_object'] = palm_pose_r_object example_args['object'] = manipulated_object example_args['N'] = 60 # 60 example_args['init'] = True example_args['table_face'] = 0 primitive_name = args.primitive mesh_file = args.config_package_path + 'descriptions/meshes/objects/' + args.object_name + '_experiments.stl' exp_running = SingleArmPrimitives(cfg, obj_id, mesh_file) k = 0 while True: # sample a random stable pose, and get the corresponding # stable orientation index k += 1 # init_id = exp_running.get_rand_init()[-1] init_id = exp_running.get_rand_init(ind=0)[-1] # sample a point on the object that is valid # for the primitive action being executed point, normal, face = exp_running.sample_contact( primitive_name=primitive_name) if point is not None: break if k >= 10: print("FAILED") continue # get the full 6D pose palm in world, at contact location palm_pose_world = exp_running.get_palm_poses_world_frame( point, normal, primitive_name=primitive_name) obj_pos_world = list(p.getBasePositionAndOrientation( obj_id, yumi_ar.pb_client.get_client_id())[0]) obj_ori_world = list(p.getBasePositionAndOrientation( obj_id, yumi_ar.pb_client.get_client_id())[1]) obj_pose_world = util.list2pose_stamped( obj_pos_world + obj_ori_world) contact_obj_frame = util.convert_reference_frame( palm_pose_world, obj_pose_world, util.unit_pose()) # set up inputs to the primitive planner, based on task # including sampled initial object pose and contacts, # and final object pose example_args['palm_pose_r_object'] = contact_obj_frame example_args['object_pose1_world'] = obj_pose_world obj_pose_final = util.list2pose_stamped(exp_running.init_poses[init_id]) obj_pose_final.pose.position.z /= 1.155 print("init: ") print(util.pose_stamped2list(object_pose1_world)) print("final: ") print(util.pose_stamped2list(obj_pose_final)) example_args['object_pose2_world'] = obj_pose_final example_args['table_face'] = init_id example_args['primitive_name'] = primitive_name # if trial == 0: # goal_viz.update_goal_state(exp_running.init_poses[init_id]) result = None try: result = action_planner.execute(primitive_name, example_args) # result = work_queue.get(block=True) print("reached final: " + str(result[0])) except ValueError: print("moveit failed!") result_queue.put(result) continue if msg == "END": break print("before sleep!") time.sleep(0.01) print("breaking") child_conn.close()
def main(): """ This function shows an example of block stacking. """ np.set_printoptions(precision=4, suppress=True) robot = Robot('ur5e_2f140', pb=True, pb_cfg={'gui': False}) success = robot.arm.go_home() if not success: ar.log_warn('Robot go_home failed!!!') #setup cam robot_base = robot.arm.robot_base_pos robot.cam.setup_camera(focus_pt=robot_base, dist=3, yaw=55, pitch=-30, roll=0) out = cv2.VideoWriter('pickandplace.mp4', cv2.VideoWriter_fourcc(*'mp4v'), 30, (640, 480)) img = robot.cam.get_images(get_rgb=True, get_depth=False)[0] out.write(np.array(img)) ori = euler2quat([0, 0, np.pi / 2]) robot.pb_client.load_urdf('table/table.urdf', [.5, 0, 0.4], ori, scaling=0.9) box_size = 0.05 box_id1 = robot.pb_client.load_geom('box', size=box_size, mass=1, base_pos=[.5, 0.12, 1.0], rgba=[1, 0, 0, 1]) robot.arm.eetool.open() record(robot, out) obj_pos = robot.pb_client.get_body_state(box_id1)[0] move_dir = obj_pos - robot.arm.get_ee_pose()[0] move_dir[2] = 0 eef_step = 0.025 for i in range(3): robot.arm.move_ee_xyz(move_dir / 3, eef_step=eef_step) record(robot, out) move_dir = np.zeros(3) move_dir[2] = obj_pos[2] - robot.arm.get_ee_pose()[0][2] for i in range(4): robot.arm.move_ee_xyz(move_dir / 4, eef_step=eef_step) record(robot, out) robot.arm.eetool.close(wait=False) record(robot, out) for i in range(10): robot.arm.move_ee_xyz([0, 0, 0.03], eef_step=eef_step) record(robot, out) for i in range(10): robot.arm.move_ee_xyz([0.025, 0, 0.0], eef_step=eef_step) record(robot, out) for i in range(10): robot.arm.move_ee_xyz([0, 0, -0.03], eef_step=eef_step) record(robot, out) robot.arm.eetool.open() record(robot, out) time.sleep(1) record(robot, out) for i in range(10): robot.arm.move_ee_xyz([0, 0, 0.03], eef_step=eef_step) record(robot, out) time.sleep(3) record(robot, out) out.release()