def __init__(self, robot_s): ''' :param robot_s: ''' self.robot_s = robot_s self.inik_slvr = inik.IncrementalNIK(self.robot_s) self.rrtc_planner = rrtc.RRTConnect(self.robot_s)
def __init__(self, robot_s): """ :param robot_s: author: weiwei, hao date: 20191122, 20210113 """ self.robot_s = robot_s self.inik_slvr = inik.IncrementalNIK(self.robot_s) self.rrtc_planner = rrtc.RRTConnect(self.robot_s)
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)
object_holder.set_rotmat(object_holder_gl_rotmat) 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])],
robot_instance.fk("rgt_arm", path[0][1]) start_rgt = robot_instance.get_gl_tcp("rgt_arm") start_lft = robot_instance.fk("lft_arm", path[0][2]) goal_rgt = robot_instance.fk("rgt_arm", path[-1][1]) goal_lft = robot_instance.fk("lft_arm", path[-1][2]) # keyposes.append(start_rgt*np.pi/180) # robot_instance.fk("rgt_arm", numikrmsmp[4][0][1]*np.pi/180) # start_config_rgt = robot_instance.get_gl_tcp("rgt_arm") # # robot_instance.fk("rgt_arm", numikrmsmp[4][-1][1] * np.pi / 180) # goal_config_rgt = robot_instance.get_gl_tcp("rgt_arm") # robot_instance.gen_meshmodel(rgba=(0,1,1,1),dual = "dual").attach_to(base) rrtc_planner = rrtc.RRTConnect(robot_instance) path_0 = rrtc_planner.plan( start_conf=robot_instance.get_jnt_values("rgt_arm"), goal_conf=numikrmsmp[4][0][1] * np.pi / 180, obstacle_list=[object, phoxicam], ext_dist=.1, max_time=300, component_name=component_name) path_1 = rrtc_planner.plan(start_conf=numikrmsmp[4][0][1] * np.pi / 180, goal_conf=numikrmsmp[4][-1][1] * np.pi / 180, obstacle_list=[object, phoxicam], ext_dist=.1, max_time=300, component_name=component_name) path = [path_0, path_1] old_path = [a[1] * np.pi / 180 for a in numikrmsmp[4]]
base = wd.World(cam_pos=[2, 1, 3], lookat_pos=[0, 0, 1.1]) gm.gen_frame().attach_to(base) # object object = cm.CollisionModel("objects/bunnysim.stl") object.set_pos(np.array([.55, -.3, 1.3])) object.set_rgba([.5, .7, .3, 1]) object.attach_to(base) # robot_s component_name = 'lft_arm' robot_s = ur3d.UR3Dual() # possible right goal np.array([0, -math.pi/4, 0, math.pi/2, math.pi/2, math.pi / 6]) # possible left goal np.array([0, -math.pi / 2, -math.pi/3, -math.pi / 2, math.pi / 6, math.pi / 6]) rrtc_planner = rrtc.RRTConnect(robot_s) path = rrtc_planner.plan(component_name=component_name, start_conf=robot_s.lft_arm.homeconf, goal_conf=np.array([0, -math.pi / 2, -math.pi/3, -math.pi / 2, math.pi / 6, math.pi / 6]), obstacle_list=[object], ext_dist=.2, max_time=300) print(path) for pose in path: print(pose) robot_s.fk(component_name, pose) robot_meshmodel = robot_s.gen_meshmodel() robot_meshmodel.attach_to(base) robot_s.gen_stickmodel().attach_to(base) base.run()
microplate24_1.set_pose(pos=np.array([.3, 0.23, -.03]), rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2)) microplate24_1.attach_to(base) microplate24_2 = microplate24_0.copy() microplate24_2.set_pose(pos=np.array([.15, 0.32, -.03]), rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2)) microplate24_2.attach_to(base) microplate24_3 = microplate24_0.copy() microplate24_3.set_pose(pos=np.array([.3, 0.32, -.03]), rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2)) microplate24_3.attach_to(base) rbt_s = cbtr.CobottaRIPPS() init_joint_values = np.radians(np.asarray([0.0, 0.0, 70.0, 0.0, 90.0, 0.0])) component_name = "arm" rbt_s.fk(component_name=component_name, jnt_values=init_joint_values) rbt_s.gen_meshmodel().attach_to(base) planner = rrtc.RRTConnect(rbt_s) ee_s = cbtg.CobottaPipette() id_x = 0 id_y = 0 tip_pos, tip_rotmat = tip_rack.get_rack_hole_pose(id_x=id_x, id_y=id_y) z_offset = np.array([0, 0, .07]) previous_jnt_values = rbt_s.get_jnt_values(component_name=component_name) goal_joint_values_attachment = utils.search_reachable_configuration(rbt_s=rbt_s, ee_s=ee_s, component_name=component_name, tgt_pos=tip_pos + z_offset, cone_axis=-tip_rotmat[:3, 2], rotation_interval=np.radians(15), obstacle_list=[frame_bottom]) if goal_joint_values_attachment is not None:
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.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],
if component_name != 'all': raise ValueError("Only support hnd_name == 'all'!") return self.jlc.get_jnt_values() def is_collided(self, obstacle_list=[], otherrobot_list=[]): for (obpos, size) in obstacle_list: dist = np.linalg.norm(np.asarray(obpos) - self.get_jntvalues()) if dist <= size / 2.0: return True # collision return False # safe if __name__ == '__main__': # ====Search Path with RRT==== obstacle_list = [((5, 5), 3), ((3, 6), 3), ((3, 8), 3), ((3, 10), 3), ((7, 5), 3), ((9, 5), 3), ((10, 5), 3), ((10, 0), 3), ((10, -2), 3), ((10, -4), 3), ((0, 12), 3), ((-1, 10), 3), ((-2, 8), 3)] # [x,y,size] # Set Initial parameters robot = XYBot() rrtc_instance = rrtc.RRTConnect(robot) path = rrtc_instance.plan(component_name='all', start_conf=np.array([0, 0]), goal_conf=np.array([5, 10]), obstacle_list=obstacle_list, ext_dist=1, max_time=300, animation=True) plt.plot([conf[0] for conf in path], [conf[1] for conf in path], '-k') plt.show()