objcm = cm.CollisionModel('tubebig.stl')
robot_s = ym.Yumi(enable_cc=True)
manipulator_name = 'rgt_arm'
hand_name = 'rgt_hnd'
start_conf = robot_s.get_jnt_values(manipulator_name)
goal_homomat_list = []
for i in range(6):
goal_pos = np.array([.55, -.1, .3]) - np.array([i * .1, i * .1, 0])
# goal_rotmat = rm.rotmat_from_axangle([0, 1, 0], math.pi / 2)
goal_rotmat = np.eye(3)
goal_homomat_list.append(rm.homomat_from_posrot(goal_pos, goal_rotmat))
tmp_objcm = objcm.copy()
tmp_objcm.set_rgba([1, 0, 0, .3])
tmp_objcm.set_homomat(rm.homomat_from_posrot(goal_pos, goal_rotmat))
tmp_objcm.attach_to(base)
grasp_info_list = gutil.load_pickle_file(objcm_name='tubebig', file_name='yumi_tube_big.pickle')
grasp_info = grasp_info_list[0]
pp_planner = PickPlacePlanner(robot_s=robot_s)
conf_list, jawwidth_list, objpose_list = \
pp_planner.gen_pick_and_place_motion(hnd_name=hand_name,
objcm=objcm,
grasp_info_list=grasp_info_list,
goal_homomat_list=goal_homomat_list,
start_conf=robot_s.get_jnt_values(hand_name),
end_conf=robot_s.get_jnt_values(hand_name),
depart_direction_list=[np.array([0, 0, 1])] * len(goal_homomat_list),
approach_direction_list=[np.array([0, 0, -1])] * len(goal_homomat_list),
# depart_distance_list=[None] * len(goal_homomat_list),
# approach_distance_list=[None] * len(goal_homomat_list),
depart_distance_list=[.2] * len(goal_homomat_list),
approach_distance_list=[.2] * len(goal_homomat_list),
def load_pickle_file(objcm_name, root=None, file_name='preannotated_grasps.pickle'):
if root is None:
root = './'
return gu.load_pickle_file(objcm_name, root=root, file_name=file_name)