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 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 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) 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) 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(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) 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 = EvalPrimitives(cfg, box_id, mesh_file) 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") continue # 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) 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( 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.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.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()
yumi.arm.go_home() yumi.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT) gel_id = 12 p.changeDynamics(yumi.arm.robot_id, gel_id, restitution=dynamics_info['restitution'], contactStiffness=dynamics_info['contactStiffness'], contactDamping=dynamics_info['contactDamping'], rollingFriction=dynamics_info['rollingFriction']) yumi_gs = YumiGelslimPybulet(yumi, cfg) box_id = pb_util.load_urdf( '/root/catkin_ws/src/config/descriptions/urdf/realsense_box.urdf', cfg.OBJECT_POSE_3[0:3], cfg.OBJECT_POSE_3[3:]) config_pkg_path = '/root/catkin_ws/src/config/' action_planner = ClosedLoopMacroActions(cfg, yumi_gs, box_id, pb_util.PB_CLIENT, config_pkg_path, object_mesh_file=mesh_file, replan=True) from helper.util import pose_stamped2list, list2pose_stamped planner_args = data['planner_args']
def main(ifRender=False): """ this function loads initial oboject on a table, """ np.set_printoptions(precision=3, suppress=True) # load robot robot = Robot('ur5e', use_eetool=False, arm_cfg={'render': ifRender, 'self_collision': True}) robot.arm.go_home() origin = robot.arm.get_ee_pose() def go_home(): robot.arm.set_ee_pose(home, origin[1]) # robot.arm.eetool.close() # init robot go_home() # load table table_ori = euler2quat([0, 0, np.pi/2.0]) table_pos = [table_x, table_y, table_z] table_id = load_urdf('table/table.urdf', table_pos, table_ori, scaling=table_scaling) # load box box_id = load_geom('box', size=box_size, mass=1, base_pos=box_pos, rgba=[1, 0, 0, 1]) # init camera def get_img(): # screenshot camera images focus_pt = [0.7, 0, 1.] robot.cam.setup_camera(focus_pt=focus_pt, dist=0.5, yaw=90, pitch=-60, roll=0) rgb, depth = robot.cam.get_images(get_rgb=True, get_depth=True) # crop the rgb img = rgb[40:360, 0:640] # dep = depth[40:360, 0:640] # low pass filter : Gaussian blur # blurred_img = cv2.GaussianBlur(img.astype('float32'), (5, 5), 0) small_img = cv2.resize(img.astype('float32'), dsize=(200, 100), interpolation=cv2.INTER_CUBIC) # numpy array dtype numpy int64 by default # small_dep = cv2.resize(dep.astype('float32'), dsize=(200, 100), # interpolation=cv2.INTER_CUBIC) # numpy array dtype numpy int64 by default return small_img, depth # test run def test(): time_to_sleep = 0.5 go_home() pose = robot.arm.get_ee_pose() robot.arm.set_ee_pose([pose[0][0], pose[0][1], min_height], origin[1]) time.sleep(time_to_sleep) get_img() # test boundary robot.arm.set_ee_pose([pose[0][0], pose[0][1]+table_length/2.0, min_height], origin[1]) # robot.arm.eetool.close() time.sleep(time_to_sleep) get_img() robot.arm.move_ee_xyz([0, -table_length, 0]) time.sleep(time_to_sleep) get_img() pose = robot.arm.get_ee_pose() robot.arm.set_ee_pose([pose[0][0], pose[0][1], rest_height], origin[1]) time.sleep(time_to_sleep) get_img() robot.arm.move_ee_xyz([0, table_length, 0]) time.sleep(time_to_sleep) get_img() pose = robot.arm.get_ee_pose() robot.arm.set_ee_pose([pose[0][0], pose[0][1], min_height], origin[1]) time.sleep(time_to_sleep) get_img() # test arc for i in list([np.pi/3.0, np.pi*2/5.0, np.pi*3/7.0, np.pi/2.0, np.pi*4/7.0, np.pi*3/5.0, np.pi*2/3.0]): robot.arm.set_ee_pose([arm_span*np.sin(i), arm_span*np.cos(i), rest_height], origin[1]) time.sleep(time_to_sleep) get_img() robot.arm.set_ee_pose([arm_span*np.sin(i), arm_span*np.cos(i), min_height], origin[1]) time.sleep(time_to_sleep) get_img() robot.arm.set_ee_pose([arm_span*np.sin(i), arm_span*np.cos(i), rest_height], origin[1]) time.sleep(time_to_sleep) get_img() def get_pixel(X, Y, Z=0.975): """return fractional pixels representations Z values comes from running robot.cam.get_pix.3dpt representing the table surface height""" ext_mat = robot.cam.get_cam_ext() int_mat = robot.cam.get_cam_int() XYZ = np.array([X, Y, Z, 1]) xyz = np.linalg.inv(ext_mat).dot(XYZ)[:3] pixel_xyz = int_mat.dot(xyz) pixel_xyz = pixel_xyz / pixel_xyz[2] pixel_col = pixel_xyz[0] / 3.2 # due to image cropping and scaling pixel_row = (pixel_xyz[1] - 40) / 3.2 return pixel_row, pixel_col # # log object # pos, quat, lin_vel, ang_vel = get_body_state(box_id) # ar.log_info('Box:') # ar.log_info(' position: %s' % np.array2string(pos, precision=2)) # ar.log_info(' quaternion: %s' % np.array2string(quat, precision=2)) # ar.log_info(' linear vel: %s' % np.array2string(lin_vel, precision=2)) # ar.log_info(' angular vel: %s' % np.array2string(ang_vel, precision=2)) # plt.figure() # plt.imshow(rgb) # plt.figure() # plt.imshow(depth * 25, cmap='gray', vmin=0, vmax=255) # print('Maximum Depth (m): %f' % np.max(depth)) # print('Minimum Depth (m): %f' % np.min(depth)) # plt.show() def poke(save_dir='data/image'): if not os.path.exists(save_dir): os.makedirs(save_dir) save_depth_dir = save_dir + '_depth' if not os.path.exists(save_depth_dir): os.makedirs(save_depth_dir) index = 0 curr_img, curr_dep = get_img() while True: obj_pos, obj_quat, lin_vel, _ = get_body_state(box_id) obj_ang = quat2euler(obj_quat)[2] # -pi ~ pi obj_x, obj_y, obj_z = obj_pos # check if cube is on table if obj_z < box_z / 2.0 or lin_vel[0] > 1e-3: # important that box is still print(obj_z, lin_vel[0]) reset_body(box_id, box_pos) continue while True: # choose random poke point on the object poke_x = np.random.random()*box_size + obj_x - box_size/2.0 poke_y = np.random.random()*box_size + obj_y - box_size/2.0 # choose poke angle along the z axis poke_ang = np.random.random() * np.pi * 2 - np.pi # choose poke length poke_len = np.random.random() * poke_len_std + poke_len_mean # calc starting poke location and ending poke loaction start_x = poke_x - poke_len * np.cos(poke_ang) start_y = poke_y - poke_len * np.sin(poke_ang) end_x = poke_x + poke_len * np.cos(poke_ang) end_y = poke_y + poke_len * np.sin(poke_ang) if end_x > workspace_min_x and end_x < workspace_max_x \ and end_y > workspace_min_y and end_y < workspace_max_y: break robot.arm.move_ee_xyz([start_x-home[0], start_y-home[1], 0], 0.015) robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015) js1, js2, js3, js4, js5, js6 = robot.arm.get_jpos() robot.arm.move_ee_xyz([end_x-start_x, end_y-start_y, 0], 0.015) je1, je2, je3, je4, je5, je6 = robot.arm.get_jpos() # important that we use move_ee_xyz, as set_ee_pose can throw obj in motion robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015) # move arm away from camera view go_home() # important to have one set_ee_pose every loop to reset accu errors next_img, next_dep = get_img() # calc poke and obj locations in image pixel space start_r, start_c = get_pixel(start_x, start_y) end_r, end_c = get_pixel(end_x, end_y) obj_r, obj_c = get_pixel(obj_x, obj_y) with open(save_dir + '.txt', 'a') as file: file.write('%d %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f %f\n' % \ (index, start_x, start_y, end_x, end_y, obj_x, obj_y, obj_quat[0], obj_quat[1], obj_quat[2], obj_quat[3], js1, js2, js3, js4, js5, js6, je1, je2, je3, je4, je5, je6, start_r, start_c, end_r, end_c, obj_r, obj_c)) cv2.imwrite(save_dir + '/' + str(index) +'.png', cv2.cvtColor(curr_img, cv2.COLOR_RGB2BGR)) # cv2.imwrite(save_dir + '_depth/' + str(index) +'.png', curr_dep) np.save(save_dir + '_depth/' + str(index), curr_dep) curr_img = next_img curr_dep = next_dep if index % 1000 == 0: ar.log_info('number of pokes: %sk' % str(index/1000)) index += 1 def test_data(filename): data = np.loadtxt(filename, unpack=True) idx, stx, sty, edx, edy, obx, oby, qt1, qt2, qt3, qt4, js1, js2, js3, js4, js5, js6, je1, je2, je3, je4, je5, je6 = data go_home() reset_body(box_id, box_pos) _, _ = get_img() for i in range(5): robot.arm.set_ee_pose([stx[i], sty[i], rest_height], origin[1]) robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015) robot.arm.move_ee_xyz([edx[i]-stx[i], edy[i]-sty[i], 0], 0.015) robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015) _, _ = get_img() start_jpos = robot.arm.compute_ik([stx[i], sty[i], min_height]) end_jpos = robot.arm.compute_ik([edx[i], edy[i], min_height]) print('start') print(js1[i], js2[i], js3[i], js4[i], js5[i], js6[i]) print(start_jpos) print('end') print(je1[i], je2[i], je3[i], je4[i], je5[i], je6[i]) print(end_jpos) time.sleep(1) def eval_poke(tag, ground_truth, img_idx, poke): # ground_truth is the entire matrix from 'image_xx.txt' file # img_idx is image index, first column of ground_truth # poke is the predicted 4 vector for world, 8 for pixel and joint # second half of the 8 vector are world coordinates computed from gt pixel and joint values # label index in the poke data array (29, 1) # index 0 is the poke index corresponding to starting image stx, sty, edx, edy = 1, 2, 3, 4 # ee pos of start and end poke obx, oby, qt1, qt2, qt3, qt4 = 5, 6, 7, 8, 9, 10 # obj pose before poke js1, js2, js3, js4, js5, js6 = 11, 12, 13, 14, 15, 16 # jpos before poke je1, je2, je3, je4, je5, je6 = 17, 18, 19, 20, 21, 22 # jpos after poke sr, stc, edr, edc, obr, obc = 23, 24, 25, 26, 27, 28 # row and col locations in image gt = ground_truth[img_idx:img_idx+2] tgt_posi = gt[1, 5:8] tgt_posi[-1] = box_z tgt_quat = gt[1, 7:11] init_posi = gt[0, 5:8] init_posi[-1] = box_z init_quat = gt[0, 7:11] gt_poke = gt[0, 1:5] go_home() reset_body(box_id, init_posi, init_quat) box_id2 = load_geom('box', size=box_size, mass=1, base_pos=tgt_posi, base_ori=tgt_quat, rgba=[0,0,1,0.5]) p.setCollisionFilterPair(box_id, box_id2, -1, -1, enableCollision=0) # to check robot_id and link_id # robot.arm.robot_id # robot.arm.p.getNumJoints(robot_id) # robot.arm.p.getJointInfo(robot_id, 0-max_lnik_id) p.setCollisionFilterPair(box_id2, 1, -1, 11, enableCollision=0) _, _ = get_img() # time.sleep(1) # reset_body(box_id, init_posi, init_quat) # robot.arm.move_ee_xyz([gt_poke[0]-home[0], gt_poke[1]-home[1], 0], 0.015) # robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015) # robot.arm.move_ee_xyz([gt_poke[2]-gt_poke[0], gt_poke[3]-gt_poke[1], 0], 0.015) # robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015) # go_home() # _, _ = get_img() # reset_body(box_id, init_posi, init_quat) # poke = ground_truth[img_idx, 1:5] robot.arm.move_ee_xyz([poke[0]-home[0], poke[1]-home[1], 0], 0.015) robot.arm.move_ee_xyz([0, 0, min_height-rest_height], 0.015) robot.arm.move_ee_xyz([poke[2]-poke[0], poke[3]-poke[1], 0], 0.015) robot.arm.move_ee_xyz([0, 0, rest_height-min_height], 0.015) go_home() _, _ = get_img() curr_posi, _, _, _ = get_body_state(box_id) dist = np.sqrt((tgt_posi[0]-curr_posi[0])**2 + (tgt_posi[1]-curr_posi[1])**2) print(dist) # time.sleep(0.5) remove_body(box_id2) return dist def calc_dist(gtfile, pdfile, tag, test_num=50): # this function generate 50 visualization sequence true = np.loadtxt(gtfile) pred = np.loadtxt(pdfile) accu_dist = 0.0 # query = random.sample(range(0, pred.shape[0]), test_num) total = pred.shape[0] query = range(total)[::total/test_num] for i in query: img_idx = int(pred[i][0]) if tag == 'world': poke = pred[i][1:5] elif tag == 'pixel': rows = np.array([pred[i, 1], pred[i, 3], pred[i, 7], pred[i, 9]]) cols = np.array([pred[i, 2], pred[i, 4], pred[i, 8], pred[i, 10]]) rows = ((rows*3.2 + 40) + 0.5).astype(int) cols = ((cols*3.2) + 0.5).astype(int) depth_file = gtfile[:-4] + '_depth/' + str(img_idx) + '.npy' depth_im = np.load(depth_file) pokes_3d = get_pix_3dpt(depth_im, rows, cols) poke = pokes_3d[:, :2].flatten() else: raise Exception("experiment_tag has to be 'world', 'joint', or 'pixel'") accu_dist += eval_poke(tag, true, img_idx, poke) return accu_dist / len(query) def get_pix_3dpt(depth_im, rs, cs, in_world=True, filter_depth=False, k=1, ktype='median', depth_min=None, depth_max=None): if not isinstance(rs, int) and not isinstance(rs, list) and \ not isinstance(rs, np.ndarray): raise TypeError('rs should be an int, a list or a numpy array') if not isinstance(cs, int) and not isinstance(cs, list) and \ not isinstance(cs, np.ndarray): raise TypeError('cs should be an int, a list or a numpy array') if isinstance(rs, int): rs = [rs] if isinstance(cs, int): cs = [cs] if isinstance(rs, np.ndarray): rs = rs.flatten() if isinstance(cs, np.ndarray): cs = cs.flatten() if not (isinstance(k, int) and (k % 2) == 1): raise TypeError('k should be a positive odd integer.') # _, depth_im = self.get_images(get_rgb=False, get_depth=True) if k == 1: depth_im = depth_im[rs, cs] else: depth_im_list = [] if ktype == 'min': ktype_func = np.min elif ktype == 'max': ktype_func = np.max elif ktype == 'median': ktype_func = np.median elif ktype == 'mean': ktype_func = np.mean else: raise TypeError('Unsupported ktype:[%s]' % ktype) for r, c in zip(rs, cs): s = k // 2 rmin = max(0, r - s) rmax = min(self.img_height, r + s + 1) cmin = max(0, c - s) cmax = min(self.img_width, c + s + 1) depth_im_list.append(ktype_func(depth_im[rmin:rmax, cmin:cmax])) depth_im = np.array(depth_im_list) depth = depth_im.reshape(-1) * 1 #self.depth_scale img_pixs = np.stack((rs, cs)).reshape(2, -1) img_pixs[[0, 1], :] = img_pixs[[1, 0], :] depth_min = depth_min if depth_min else 0.01 #self.depth_min depth_max = depth_max if depth_max else 10 #self.depth_max if filter_depth: valid = depth > depth_min valid = np.logical_and(valid, depth < depth_max) depth = depth[:, valid] img_pixs = img_pixs[:, valid] get_img() # to set up camera matrices cam_int_mat_inv = robot.cam.cam_int_mat_inv cam_ext_mat = robot.cam.cam_ext_mat uv_one = np.concatenate((img_pixs, np.ones((1, img_pixs.shape[1])))) uv_one_in_cam = np.dot(cam_int_mat_inv, uv_one) pts_in_cam = np.multiply(uv_one_in_cam, depth) if in_world: if cam_ext_mat is None: raise ValueError('Please call set_cam_ext() first to set up' ' the camera extrinsic matrix') pts_in_cam = np.concatenate((pts_in_cam, np.ones((1, pts_in_cam.shape[1]))), axis=0) pts_in_world = np.dot(cam_ext_mat, pts_in_cam) pts_in_world = pts_in_world[:3, :].T return pts_in_world else: return pts_in_cam.T from IPython import embed embed()
def main(args): global joint_lock global cfg joint_lock = threading.RLock() print(args) rospy.init_node('test') object_urdf = args.config_package_path + \ 'descriptions/urdf/'+args.object_name+'.urdf' object_mesh = args.config_package_path + \ 'descriptions/meshes/objects'+args.object_name+'.stl' moveit_robot = moveit_commander.RobotCommander() moveit_scene = moveit_commander.PlanningSceneInterface() moveit_planner = 'RRTconnectkConfigDefault' # moveit_planner = 'RRTstarkConfigDefault' mp_left = GroupPlanner('left_arm', moveit_robot, moveit_planner, moveit_scene, max_attempts=50, planning_time=5.0, goal_tol=0.5, eef_delta=0.01, jump_thresh=10.0) mp_right = GroupPlanner('right_arm', moveit_robot, moveit_planner, moveit_scene, max_attempts=50, planning_time=5.0, goal_tol=0.5, eef_delta=0.01, jump_thresh=10.0) cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml" cfg = get_cfg_defaults() cfg.merge_from_file(cfg_file) cfg.freeze() print(cfg) real_object_init = [ 0.3, -0.2, 0.02, -0.007008648232757453, 0.0019098461832132076, 0.5471545719627792, 0.8370000631527658 ] manipulated_object = None object_pose1_world = util.list2pose_stamped(cfg.OBJECT_INIT) # object_pose1_world = util.list2pose_stamped(real_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) active_arm, unactive_arm = get_active_arm(cfg.OBJECT_INIT) planner_args = {} planner_args['object_pose1_world'] = object_pose1_world planner_args['object_pose2_world'] = object_pose2_world planner_args['palm_pose_l_object'] = palm_pose_l_object planner_args['palm_pose_r_object'] = palm_pose_r_object planner_args['object'] = manipulated_object planner_args['N'] = 60 # 60 planner_args['init'] = True global initial_plan initial_plan = get_primitive_plan(args.primitive, planner_args, args.config_package_path, active_arm) box_id = None yumi = Robot('yumi', pb=True, arm_cfg={ 'render': True, 'self_collision': False }) # yumi.arm.go_home() yumi.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT) global both_pos global single_pos global joint_lock global palm_contact joint_lock.acquire() both_pos = cfg.RIGHT_INIT + cfg.LEFT_INIT single_pos = {} single_pos['right'] = cfg.RIGHT_INIT single_pos['left'] = cfg.LEFT_INIT joint_lock.release() if args.object: box_id = pb_util.load_urdf( args.config_package_path + 'descriptions/urdf/' + args.object_name + '.urdf', cfg.OBJECT_INIT[0:3], cfg.OBJECT_INIT[3:]) # box_id = pb_util.load_urdf( # args.config_package_path + # 'descriptions/urdf/'+args.object_name+'.urdf', # real_object_init[0:3], # real_object_init[3:] # ) global trans_box_id global trans_box_id_final trans_box_id = pb_util.load_urdf( args.config_package_path + 'descriptions/urdf/' + args.object_name + '_trans.urdf', cfg.OBJECT_INIT[0:3], cfg.OBJECT_INIT[3:]) trans_box_id_final = pb_util.load_urdf( args.config_package_path + 'descriptions/urdf/' + args.object_name + '_trans.urdf', cfg.OBJECT_INIT[0:3], cfg.OBJECT_INIT[3:]) sleep_t = 0.005 loop_t = 0.125 ik = IKHelper() if args.primitive == 'push' or args.primitive == 'pull': execute_thread = threading.Thread(target=_yumi_execute_arm, args=(yumi, active_arm)) else: execute_thread = threading.Thread(target=_yumi_execute_both, args=(yumi, )) execute_thread.daemon = True execute_thread.start() box_thread = threading.Thread(target=execute_planned_pull, args=(yumi, )) box_thread.daemon = True palm_contact = False box_thread.start() replan = args.replan embed() for plan_number, plan_dict in enumerate(initial_plan): intermediate_object_final = util.pose_stamped2list( plan_dict['object_poses_world'][-1]) if plan_number == len(initial_plan) - 1: intermediate_object_final = cfg.OBJECT_FINAL tip_poses = plan_dict['palm_poses_world'] r_wrist_pos_world = yumi.arm.get_ee_pose(arm='right')[0].tolist() r_wrist_ori_world = yumi.arm.get_ee_pose(arm='right')[1].tolist() l_wrist_pos_world = yumi.arm.get_ee_pose(arm='left')[0].tolist() l_wrist_ori_world = yumi.arm.get_ee_pose(arm='left')[1].tolist() current_wrist_poses = {} current_wrist_poses['right'] = util.list2pose_stamped( r_wrist_pos_world + r_wrist_ori_world) current_wrist_poses['left'] = util.list2pose_stamped( l_wrist_pos_world + l_wrist_ori_world) current_tip_poses = get_wrist_to_tip(current_wrist_poses, cfg) tip_right = [] tip_left = [] tip_right.append(current_tip_poses['right'].pose) tip_left.append(current_tip_poses['left'].pose) for i in range(len(tip_poses)): tip_left.append(tip_poses[i][0].pose) tip_right.append(tip_poses[i][1].pose) l_current = yumi.arm.get_jpos()[7:] r_current = yumi.arm.get_jpos()[:7] if args.primitive == 'pivot' or args.primitive == 'grasp': print("Planning with MoveIt! Plan number: " + str(plan_number)) traj_right = mp_right.plan_waypoints(tip_right, force_start=l_current + r_current, avoid_collisions=False) traj_left = mp_left.plan_waypoints(tip_left, force_start=l_current + r_current, avoid_collisions=False) unified = unify_arm_trajectories(traj_left, traj_right, tip_poses) aligned_left = unified['left']['aligned_joints'] aligned_right = unified['right']['aligned_joints'] if aligned_left.shape != aligned_right.shape: raise ValueError('Could not aligned joint trajectories') reached_final = False for i in range(aligned_right.shape[0]): # r_pos = aligned_right[i, :] # l_pos = aligned_left[i, :] planned_r = aligned_right[i, :] planned_l = aligned_left[i, :] if both_in_contact( yumi, box_id ) and args.primitive == 'grasp' and plan_number > 0: reached_final, pos_err_total, ori_err_total = reach_pose_goal( intermediate_object_final[:3], intermediate_object_final[3:], yumi.arm.p.getBasePositionAndOrientation, box_id, pos_tol=0.025, ori_tol=0.01) print("Reached final: " + str(reached_final)) timeout = 30 start_while = time.time() pos_err = pos_err_total ori_err = ori_err_total timed_out = False not_perturbed = 0 none_count = 0 if replan: while not reached_final and not timed_out and both_in_contact( yumi, box_id): # if i == 40: palm_contact = True pose_ref_r = util.pose_stamped2list( ik_helper.compute_fk( joints=yumi.arm.arm_dict['right'].get_jpos( ), arm='r')) diffs_r = util.pose_difference_np( pose=unified['right']['aligned_fk'][i:], pose_ref=pose_ref_r) pose_ref_l = util.pose_stamped2list( ik_helper.compute_fk( joints=yumi.arm.arm_dict['left'].get_jpos( ), arm='l')) diffs_l = util.pose_difference_np( pose=unified['left']['aligned_fk'][i:], pose_ref=pose_ref_l) seed_ind_r = np.argmin(diffs_r[0]) seed_ind_l = np.argmin(diffs_l[0]) seed = {} seed['right'] = aligned_right[i:, :][seed_ind_r, :] seed['left'] = aligned_left[i:, :][seed_ind_l, :] # yumi.arm.p.resetBasePositionAndOrientation( # box_id, # util.pose_stamped2list(initial_plan[0]['object_poses_world'][-1])[:3], # util.pose_stamped2list( # initial_plan[0]['object_poses_world'][-1])[3:]) # frac_complete = ori_err/ori_err_total # frac_complete = pos_err/pos_err_total frac_complete = (ori_err / ori_err_total + pos_err / pos_err_total) / 2 joints_execute, new_plan = greedy_replan( yumi, active_arm, box_id, args.primitive, object_pose2_world, args.config_package_path, ik, seed, frac_complete=frac_complete, plan_iteration=plan_number) if ori_err / ori_err_total < 0.8 and not_perturbed == 0 and plan_number == 1: perturb_box( yumi, box_id, [0.0, 0.0, 0.0], delta_ori_euler=[np.pi / 8, 0.0, 0.0]) not_perturbed += 1 ############################################# # if plan_number == 2: if False: new_tip_poses = new_plan[plan_number][ 'palm_poses_world'] new_tip_right = [] new_tip_left = [] for k in range(len(new_tip_poses)): new_tip_left.append( new_tip_poses[k][0].pose) new_tip_right.append( new_tip_poses[k][1].pose) l_current = yumi.arm.get_jpos()[7:] r_current = yumi.arm.get_jpos()[:7] new_traj_right = mp_right.plan_waypoints( new_tip_right, force_start=l_current + r_current, avoid_collisions=False) new_traj_left = mp_left.plan_waypoints( new_tip_left, force_start=l_current + r_current, avoid_collisions=False) new_unified = unify_arm_trajectories( new_traj_left, new_traj_right, new_tip_poses) new_aligned_left = new_unified['left'][ 'aligned_joints'] new_aligned_right = new_unified['right'][ 'aligned_joints'] if new_aligned_left.shape != new_aligned_right.shape: raise ValueError( 'Could not aligned joint trajectories') for j in range(new_aligned_right.shape[0]): r_pos = new_aligned_right[j, :].tolist() l_pos = new_aligned_left[j, :].tolist() joint_lock.acquire() both_pos = r_pos + l_pos joint_lock.release() time.sleep(loop_t) ############################################## r_pos = joints_execute['right'] l_pos = joints_execute['left'] if r_pos is not None and l_pos is not None: joint_lock.acquire() both_pos = r_pos + l_pos joint_lock.release() none_count = 0 else: none_count += 1 if none_count >= 10: raise ValueError( 'IK returned none more than 10 times!') pos_tol = 0.05 if plan_number == 1 else 0.01 reached_final, pos_err, ori_err = reach_pose_goal( intermediate_object_final[:3], intermediate_object_final[3:], yumi.arm.p.getBasePositionAndOrientation, box_id, pos_tol=pos_tol, ori_tol=0.001) timed_out = time.time() - start_while > timeout if reached_final: print("REACHED FINAL") break if timed_out: print("TIMED OUT") time.sleep(0.005) r_pos = planned_r.tolist() l_pos = planned_l.tolist() else: r_pos = planned_r.tolist() l_pos = planned_l.tolist() if reached_final: break joint_lock.acquire() both_pos = r_pos + l_pos joint_lock.release() time.sleep(loop_t) else: if active_arm == 'right': traj = mp_right.plan_waypoints(tip_right, force_start=l_current + r_current, avoid_collisions=False) else: traj = mp_left.plan_waypoints(tip_left, force_start=l_current + r_current, avoid_collisions=False) # make numpy array of each arm joint trajectory for each comp joints_np = np.zeros((len(traj.points), 7)) # make numpy array of each arm pose trajectory, based on fk fk_np = np.zeros((len(traj.points), 7)) for i, point in enumerate(traj.points): joints_np[i, :] = point.positions pose = ik_helper.compute_fk(point.positions, arm=active_arm[0]) fk_np[i, :] = util.pose_stamped2list(pose) for i, point in enumerate(traj.points): # j_pos = point.positions planned_pos = point.positions if replan and is_in_contact( yumi, box_id) and (args.primitive == 'pull' or args.primitive == 'push'): palm_contact = True reached_final, pos_err_total, _ = reach_pose_goal( cfg.OBJECT_FINAL[:3], cfg.OBJECT_FINAL[3:], yumi.arm.p.getBasePositionAndOrientation, box_id, pos_tol=0.025, ori_tol=0.01) print("Reached final: " + str(reached_final)) timeout = 30 start_while = time.time() pos_err = pos_err_total timed_out = False not_perturbed = 0 none_count = 0 while not reached_final and not timed_out: # if is_in_contact(yumi, box_id) and (args.primitive == 'pull' or args.primitive == 'push'): if True: palm_contact = True pose_ref = util.pose_stamped2list( ik_helper.compute_fk( joints=yumi.arm.arm_dict[active_arm]. get_jpos(), arm=active_arm[0])) diffs = util.pose_difference_np( pose=fk_np, pose_ref=pose_ref)[0] seed_ind = np.argmin(diffs) seed = {} seed[active_arm] = joints_np[seed_ind, :] seed[unactive_arm] = yumi.arm.arm_dict[ unactive_arm].get_jpos() joints_execute, _ = greedy_replan( yumi, active_arm, box_id, args.primitive, object_pose2_world, args.config_package_path, ik, seed, frac_complete=pos_err / pos_err_total) if joints_execute[active_arm] is not None: joint_lock.acquire() single_pos[active_arm] = joints_execute[ active_arm] joint_lock.release() none_count = 0 else: none_count += 1 if none_count >= 10: raise ValueError( 'IK returned none more than 10 times!') reached_final, pos_err, _ = reach_pose_goal( cfg.OBJECT_FINAL[:3], cfg.OBJECT_FINAL[3:], yumi.arm.p.getBasePositionAndOrientation, box_id, pos_tol=0.003, ori_tol=0.001) # if pos_err/pos_err_total < 0.3 and not_perturbed==1: # perturb_box(yumi, box_id, # [0.0, -0.02, 0.0], # delta_ori_euler=[0.0, 0.0, -np.pi/6]) # not_perturbed += 1 # if pos_err/pos_err_total < 0.8 and not_perturbed==0: # perturb_box(yumi, box_id, # [0.0, -0.05, 0.0], # delta_ori_euler=[0.0, 0.0, np.pi/3]) # not_perturbed += 1 timed_out = time.time() - start_while > timeout if reached_final: print("REACHED FINAL") return if timed_out: print("TIMED OUT") else: pass # joint_lock.acquire() # single_pos[active_arm] = seed[active_arm] # joint_lock.release() time.sleep(0.005) # j_pos = planned_pos else: j_pos = planned_pos joint_lock.acquire() single_pos[active_arm] = j_pos joint_lock.release() time.sleep(loop_t)
def main(args): print(args) yumi = Robot('yumi', pb=True, arm_cfg={ 'render': True, 'self_collision': False }) yumi.arm.go_home() cfg_file = os.path.join(args.example_config_path, args.primitive) + ".yaml" cfg = get_cfg_defaults() cfg.merge_from_file(cfg_file) cfg.freeze() print(cfg) yumi.arm.set_jpos(cfg.RIGHT_INIT + cfg.LEFT_INIT) time.sleep(1.0) 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) if args.primitive == 'push': plan = pushing_planning(object=manipulated_object, object_pose1_world=object_pose1_world, object_pose2_world=object_pose2_world, palm_pose_l_object=palm_pose_l_object, palm_pose_r_object=palm_pose_r_object) elif args.primitive == 'grasp': plan = grasp_planning(object=manipulated_object, object_pose1_world=object_pose1_world, object_pose2_world=object_pose2_world, palm_pose_l_object=palm_pose_l_object, palm_pose_r_object=palm_pose_r_object) elif args.primitive == 'pivot': gripper_name = args.config_package_path + \ 'descriptions/meshes/mpalm/mpalms_all_coarse.stl' table_name = args.config_package_path + \ 'descriptions/meshes/table/table_top.stl' manipulated_object = collisions.CollisionBody( args.config_package_path + 'descriptions/meshes/objects/realsense_box_experiments.stl') plan = levering_planning(object=manipulated_object, object_pose1_world=object_pose1_world, object_pose2_world=object_pose2_world, palm_pose_l_object=palm_pose_l_object, palm_pose_r_object=palm_pose_r_object, gripper_name=gripper_name, table_name=table_name) elif args.primitive == 'pull': plan = pulling_planning(object=manipulated_object, object_pose1_world=object_pose1_world, object_pose2_world=object_pose2_world, palm_pose_l_object=palm_pose_l_object, palm_pose_r_object=palm_pose_r_object, arm='r') else: raise NotImplementedError object_loaded = False for plan_dict in plan: for i, t in enumerate(plan_dict['t']): if i == 0 and not object_loaded and args.object: time.sleep(2.0) pb_util.load_urdf( args.config_package_path + 'descriptions/urdf/realsense_box.urdf', cfg.OBJECT_INIT[0:3], cfg.OBJECT_INIT[3:]) time.sleep(2.0) object_loaded = True tip_poses = plan_dict['palm_poses_world'][i] r_joints, l_joints, wrist_right, wrist_left = get_joint_poses( tip_poses, yumi, cfg, nullspace=False) loop_time = 0.125 sleep_time = 0.005 start = time.time() if args.primitive != 'push': while (time.time() - start < loop_time): yumi.arm.set_jpos(list(r_joints) + list(l_joints), wait=False) time.sleep(sleep_time) else: while (time.time() - start < loop_time): yumi.arm.set_jpos(r_joints, arm='right', wait=False) time.sleep(sleep_time)