object_holder_gl_goal_pos = np.array([.25, -.05, .08]) # object_holder_gl_goal_pos = np.array([-.15, -.3, .1]) 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) # base.run() 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, depart_direction_list=[np.array([0, 0, 1]), None], depart_distance_list=[.2] * 2) robot_attached_list = []
object_goal_rotmat = np.eye(3) 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) # robot_instance = ur3d.UR3Dual() robot = ur3ed.UR3EDual() # robot_instance = sda5.SDA5F() component_name = 'rgt_arm' rrtc = rrtc.RRTConnect(robot) ppp = ppp.PickPlacePlanner(robot) grasp_info_list = gpa.load_pickle_file('tubebig', './', 'graspdata/hnde_tubebig.pickle') # manipulator_name = "arm" 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_goal_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)
object_box_gl_goal_pos = np.array([.3, -.4, .1]) object_box_gl_goal_rotmat = rm.rotmat_from_euler(math.pi / 3, math.pi / 2, math.pi / 2) obgl_goal_homomat = rm.homomat_from_posrot(object_box_gl_goal_pos, object_box_gl_goal_rotmat) object_box_goal_copy = object_box.copy() object_box_goal_copy.set_homomat(obgl_goal_homomat) # object_box_goal_copy.attach_to(base) robot_s = xsm.XArm7YunjiMobile() # robot_s.gen_meshmodel().attach_to(base) # base.run() rrtc_s = rrtc.RRTConnect(robot_s) ppp_s = ppp.PickPlacePlanner(robot_s) original_grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_long_box.pickle') hnd_name = "hnd" start_conf = robot_s.get_jnt_values(hnd_name) conf_list, jawwidth_list, objpose_list = \ ppp_s.gen_pick_and_place_motion(hnd_name=hnd_name, objcm=object_box, 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, depart_direction_list=[np.array([0, 0, 1]), None], depart_distance_list=[.2] * 2) robot_attached_list = [] object_attached_list = []
tool_initial_homomat = rm.homomat_from_posrot(tool_initial_pos, rotmat) tool_final.set_rgba([0, 0, 1, 1]) tool_final_pos = np.array([ table2_center[0], table2_center[1], object_box4.get_pos()[2] + .009 + .11 ]) rotmat = np.dot(rm.rotmat_from_axangle([0, 1, 0], math.radians(90)), rm.rotmat_from_axangle([0, 0, 1], math.radians(90))) tool_final.set_pos(tool_final_pos) tool_final.set_rotmat(rotmat) tool_final_homomat = rm.homomat_from_posrot(tool_final_pos, rotmat) ## rbt_s and grasp list robot_s = xsm.XArm7YunjiMobile() rrtc_s = rrtc.RRTConnect(robot_s) rrtc_planner = rrtdwc.RRTDWConnect(robot_s) grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_long_box.pickle') grasp_info_list_tool = gpa.load_pickle_file('box', './', 'xarm_tool.pickle') jnt_values_initial = robot_s.get_jnt_values(component_name="all") jnt_values = jnt_values_initial.copy() jnt_values[0] = tool_final_pos[0] + .8 jnt_values[1] = tool_final_pos[1] jnt_values[2] = math.pi robot_s.fk(component_name="all", jnt_values=np.array(jnt_values)) print(jnt_values_initial, jnt_values) # robot_s.gen_meshmodel().attach_to(base) ## discretize the location and check if the place is available or not # hnd_name = "hnd" # ppp_s = ppp.PickPlacePlanner(robot_s) # goal_homomat_list = [tool_initial_homomat, tool_final_homomat] # target_obj = object_box4.copy() # c_pos = target_obj.get_pos()
# object_goal = object_fixture object_start = object.copy() object_start_pos = np.array([0.900, -.550, 0.780]) object_start_rotmat = rm.rotmat_from_axangle( (0, 0, 1), np.radians(180)).dot( rm.rotmat_from_axangle((1, 0, 0), np.radians(-90)).dot( rm.rotmat_from_axangle((0, 0, 1), np.radians(180)))) object_start_homomat = rm.homomat_from_posrot(object_start_pos, object_start_rotmat) object_start.set_pos(object_start_pos) object_start.set_rotmat(object_start_rotmat) # object_start.attach_to(base) grasp_info_list = gpa.load_pickle_file('test_long', './', 'PlacementData/rtqhe.pickle') # show_ikfeasible_poses(object_fixture_homomat[:3,:3], object_fixture_pos) # show_ikfeasible_poses(object_fixture_after_homomat[:3, :3], object_fixture_after_pos) # base.run() object_fixture_corner = object.copy() object_fixture_corner_pos = fixture_start_pos object_fixture_corner_rotmat = np.eye(3) object_fixture_corner_homomat = rm.homomat_from_posrot( object_fixture_corner_pos, 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.250, -.050, 0.781])
ground.set_pos(np.array([0, 0, -.5])) ground.attach_to(base) object_box = cm.gen_box(extent=[.02, .06, .2], rgba=[.7, .5, .3, .7]) object_box_gl_pos = np.array([.5, -.3, .1]) object_box_gl_rotmat = np.eye(3) object_box.set_pos(object_box_gl_pos) object_box.set_rotmat(object_box_gl_rotmat) gm.gen_frame().attach_to(object_box) object_box.attach_to(base) robot_s = xsm.XArmShuidi() robot_s.gen_meshmodel().attach_to(base) base.run() grasp_info_list = gpa.load_pickle_file('box', './', 'xarm_box.pickle') component_name = "arm" gripper_s = xag.XArmGripper() for grasp_info in grasp_info_list: jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info gl_jaw_center_pos = object_box_gl_pos + object_box_gl_rotmat.dot( jaw_center_pos) gl_jaw_center_rotmat = object_box_gl_rotmat.dot(jaw_center_rotmat) gl_hnd_pos = object_box_gl_pos + object_box_gl_rotmat.dot(hnd_pos) gl_hnd_rotmat = object_box_gl_rotmat.dot(hnd_rotmat) gripper_s.fix_to(gl_hnd_pos, gl_hnd_rotmat) if not gripper_s.is_mesh_collided([ground]): jnt_values = robot_s.ik(component_name, tgt_pos=gl_jaw_center_pos, tgt_rotmat=gl_jaw_center_rotmat)
base = wd.World(cam_pos=[0.1, 1, 1],w=960, h=540, lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) objname = "tubebig" grippername = "hnde" object_tube = cm.CollisionModel(f"objects/{objname}.stl") object_tube.set_rgba([.9, .75, .35, 1]) object_tube.attach_to(base) # gripper_s = yg.YumiGripper() gripper_s = hnde.RobotiqHE() grasp_info_list = gpa.load_pickle_file('tubebig', '.', f'graspdata/{grippername}_{objname}.pickle') for grasp_info in grasp_info_list: jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info gripper_s.grip_at_with_jcpose(jaw_center_pos, jaw_center_rotmat, jaw_width) gripper_s.gen_meshmodel().attach_to(base) break # for grasp_info in grasp_info_list: # jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp_info # gripper_s.grip_at_with_jcpose(jaw_center_pos, jaw_center_rotmat, jaw_width) # gripper_s.gen_meshmodel(rgba=(1,0,0,0.01)).attach_to(base) def update(textNode, task): if textNode[0] is not None: textNode[0].detachNode()
object_fixture_rotmat = np.eye(3) object_fixture_homomat = rm.homomat_from_posrot(object_fixture_pos, object_fixture_rotmat).dot(da.pdmat4_to_npmat4(RotMatnozero[1])) object_fixture.set_homomat(object_fixture_homomat) # object_fixture.attach_to(base) # object_goal = object_fixture object_start = object.copy() object_start_pos = np.array([0.900, -.350, 0.800]) object_start_rotmat = rm.rotmat_from_axangle((1,0,0), np.radians(-90)).dot(rm.rotmat_from_axangle((0,0,1),np.radians(180))) object_start_homomat = rm.homomat_from_posrot(object_start_pos, object_start_rotmat) object_start.set_pos(object_start_pos) object_start.set_rotmat(object_start_rotmat) # object_start.attach_to(base) grasp_info_list = gpa.load_pickle_file('test_long', './', 'rtqhe.pickle') # show_ikfeasible_poses(object_fixture_homomat[:3,:3], object_fixture_pos) # show_ikfeasible_poses(object_start_homomat[:3, :3], object_start_pos) # base.run() object_fixture_corner = object.copy() object_fixture_corner_pos = fixture_start_pos object_fixture_corner_rotmat = np.eye(3) object_fixture_corner_homomat = rm.homomat_from_posrot(object_fixture_corner_pos, object_fixture_corner_rotmat).dot( da.pdmat4_to_npmat4(RotMatnozero[1])) object_fixture_corner.set_homomat(object_fixture_corner_homomat) object_goal = object.copy()