def __init__(self, pos=np.zeros(3), rotmat=np.eye(3), use_real=False, create_sim_world=True, lft_robot_ip='10.2.0.50', rgt_robot_ip='10.2.0.51', pc_ip='10.2.0.100', cam_pos=np.array([2, 1, 3]), lookat_pos=np.array([0, 0, 1.1]), auto_cam_rotate=False): self.robot_s = ur3ds.UR3Dual(pos=pos, rotmat=rotmat) self.rrt_planner = rrtc.RRTConnect(self.robot_s) self.inik_solver = inik.IncrementalNIK(self.robot_s) self.pp_planner = ppp.PickPlacePlanner(self.robot_s) if use_real: self.robot_x = ur3dx.UR3DualX(lft_robot_ip=lft_robot_ip, rgt_robot_ip=rgt_robot_ip, pc_ip=pc_ip) if create_sim_world: self.sim_world = wd.World(cam_pos=cam_pos, lookat_pos=lookat_pos, auto_cam_rotate=auto_cam_rotate)
gm.gen_frame().attach_to(object_holder) object_holder_copy = object_holder.copy() object_holder_copy.attach_to(base) # object holder goal object_holder_gl_goal_pos = np.array([.25, -.05, .05]) object_holder_gl_goal_rotmat = rm.rotmat_from_euler(0, 0, -math.pi / 2) obgl_goal_homomat = rm.homomat_from_posrot(object_holder_gl_goal_pos, object_holder_gl_goal_rotmat) object_holder_goal_copy = object_holder.copy() object_holder_goal_copy.set_homomat(obgl_goal_homomat) object_holder_goal_copy.attach_to(base) robot_s = cbt.Cobotta() # robot_s.gen_meshmodel().attach_to(base) rrtc_s = rrtc.RRTConnect(robot_s) ppp_s = ppp.PickPlacePlanner(robot_s) original_grasp_info_list = gpa.load_pickle_file('holder', './', 'cobg_holder_grasps.pickle') manipulator_name = "arm" hand_name = "hnd" start_conf = robot_s.get_jnt_values(manipulator_name) conf_list, jawwidth_list, objpose_list = \ ppp_s.gen_pick_and_place_motion(hnd_name=hand_name, objcm=object_holder, grasp_info_list=original_grasp_info_list, start_conf=start_conf, end_conf=start_conf, goal_homomat_list=[obgl_start_homomat, obgl_goal_homomat], approach_direction_list=[None, np.array([0, 0, -1])], approach_distance_list=[.2] * 2,
object_fixture_corner_rotmat).dot(da.pdmat4_to_npmat4(RotMatnozero[1])) object_fixture_corner.set_homomat(object_fixture_corner_homomat) object_goal = object.copy() object_goal_pos = np.array([0.550, -.050, 0.850]) object_goal_rotmat = rm.rotmat_from_axangle( (0, 0, 1), np.radians(90)).dot(rm.rotmat_from_axangle((0, 1, 0), np.radians(-90))) object_goal_homomat = rm.homomat_from_posrot(object_goal_pos, object_goal_rotmat) object_goal.set_pos(object_goal_pos) object_goal.set_rotmat(object_goal_rotmat) # object_goal.attach_to(base) # base.run() rrtc = rrtc.RRTConnect(robot) ppp = ppp.PickPlacePlanner(robot) hand_name = "rgt_arm" start_conf = robot.get_jnt_values(component_name) # conf_list, jawwidth_list, objpose_list = \ # ppp.gen_pick_and_place_motion(hnd_name=hand_name, # objcm=object, # grasp_info_list=grasp_info_list, # start_conf=start_conf, # end_conf=start_conf, # goal_homomat_list=[object_start_homomat, object_fixture_homomat, # object_start_homomat], # approach_direction_list=[None, np.array([0, 0, -1]), None], # approach_distance_list=[.05] * 3, # depart_direction_list=[np.array([0, 0, 1]), None, None], # depart_distance_list=[.05] * 3)
# robot_instance.gen_meshmodel().attach_to(base) start_tcp_pos, start_tcp_rotmat = robot_instance.get_init_tcp(component_name) print(robot_instance.get_jnt_values(component_name)) goal_tcp_pos = start_tcp_pos + np.array([0,-0.000,-0.005]) goal_tcp_rotmat = start_tcp_rotmat grasp_info_list = gpa.load_pickle_file('tubebig', './', 'graspdata/hnde_tubebig.pickle') # planner = huplanner.PrimaryPlanner(robot_instance) # path = planner.gen_linear_moveto( component_name, # start_tcp_pos, # start_tcp_rotmat, # goal_tcp_pos, # goal_tcp_rotmat, # obstacle_list=[]) ppp = ppp.PickPlacePlanner(robot_instance) goal_homomat_list = [object_start_homomat, object_goal_homomat] for grasp_id in range(len(grasp_info_list)): grasp_info = grasp_info_list[grasp_id] jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info conf_list, jawwidthlist, objpose_list = ppp.gen_holding_moveto(hand_name = component_name, objcm = object, grasp_info = grasp_info, obj_pose_list = goal_homomat_list, approach_direction_list=[None, np.array([0, 0, -1]), None], approach_distance_list=[.05] * 3, depart_direction_list=[np.array([0, 0, 1]), None, None], depart_distance_list=[.05] * 3, ad_granularity=.007, use_rrt=True, obstacle_list=[],