def get_std_convexhull(pcd, origin="center", color=(1, 1, 1), transparency=1, toggledebug=False, toggleransac=True): """ create CollisionModel by pcd, standardized rotation :param pcd: :param origin: "center" or "tip" :param color: :param transparency: :param toggledebug: :param toggleransac: :return: CollisionModel, position of origin """ rot_angle = get_rot_frompcd(pcd, toggledebug=toggledebug, toggleransac=toggleransac) center = get_pcd_center(pcd) origin_pos = np.array(center) pcd = pcd - np.array([center]).repeat(len(pcd), axis=0) pcd = trans_pcd( pcd, rm.homobuild((0, 0, 0), rm.rodrigues((0, 0, 1), -rot_angle))) convexhull = trimesh.Trimesh(vertices=pcd) convexhull = convexhull.convex_hull obj = cm.CollisionModel(initor=convexhull) obj_w, obj_h = get_pcd_w_h(pcd) if origin == "tip": tip = get_pcd_tip(pcd, axis=0) origin_pos = center + \ trans_pcd(np.array([tip]), rm.homobuild((0, 0, 0), rm.rodrigues((0, 0, 1), rot_angle)))[0] pcd = pcd - np.array([tip]).repeat(len(pcd), axis=0) convexhull = trimesh.Trimesh(vertices=pcd) convexhull = convexhull.convex_hull obj = cm.CollisionModel(initor=convexhull) if toggledebug: obj.set_rgba(color[0], color[1], color[2], transparency) obj.reparentTo(base.render) obj.showlocalframe() print("Rotation angle:", rot_angle) print("Origin:", center) base.pggen.plotSphere(base.render, center, radius=2, rgba=(1, 0, 0, 1)) return obj, obj_w, obj_h, origin_pos, rot_angle
def checkgrasp(self): grasp = gpa.load_pickle_file(self.objname, None, "rtqhe.pickle") grasp_info_list = grasp print("number of grasp 1", len(grasp)) hndfa = rtqhe.RobotiqHE(enable_cc=True) rtqHE = copy.deepcopy(hndfa) rtqHE.jaw_to(jawwidth=0.035) slope = self.slopelist_high obj = cm.CollisionModel(initor=self.objpath) obj.set_scale((0.001, 0.001, 0.001)) cfreegrasp = [] for i, rotmat4 in enumerate(self.pRotMat): temcfreegrasp = [] if self.collisionlist[i] == False and self.stablelist[i] == True: obstaclelist = slope obj.set_homomat(da.pdmat4_to_npmat4(rotmat4)) obstaclelist.append(obj) for grasp in grasp_info_list: jaw_width, jaw_center_pos, jaw_center_rotmat, hnd_pos, hnd_rotmat = grasp rotmatgraspnp = np.dot( da.pdmat4_to_npmat4(rotmat4), rm.homomat_from_posrot(hnd_pos, hnd_rotmat)) rotmat4panda = da.npmat4_to_pdmat4(rotmatgraspnp) rtqHE.fix_to(pos=rotmatgraspnp[:3, 3], rotmat=rotmatgraspnp[:3, :3], jawwidth=jaw_width) if rtqHE.is_mesh_collided(obstaclelist) == False: temcfreegrasp.append([rotmat4panda, False]) else: temcfreegrasp.append([rotmat4panda, True]) print("grasp in collision") cfreegrasp.append(temcfreegrasp) print("number of grasp 2", len(temcfreegrasp)) self.cfreegrasppose = cfreegrasp
def get_obj_from_phoxiinfo_withmodel(phxilocator, stl_f_name, load_f_name=None, match_filp=False, bg_f_name="bg_0217.pkl"): objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT + '/obstacles/' + stl_f_name)) grayimg, depthnparray_float32, pcd = load_phxiinfo( phoxi_f_name=load_f_name) workingarea_uint8 = phxilocator.remove_depth_bg(depthnparray_float32, bg_f_name=bg_f_name, toggledebug=False) obj_depth = phxilocator.find_paintingobj(workingarea_uint8) if obj_depth is None: print("painting object not detected!") objpcd = pcdu.trans_pcd( pcdu.remove_pcd_zeros(vu.map_depth2pcd(obj_depth, pcd)), phxilocator.amat) objmat4 = phxilocator.match_pcdncm(objpcd, objcm, match_rotz=match_filp) objcm.sethomomat(objmat4) return item.Item(objcm=objcm, pcd=objpcd, objmat4=objmat4)
def update(textNode, obj_show_node, counter, task): if base.inputmgr.keymap['space'] is True: print("space") # time.sleep(0.5) if counter[0] >= len(freehold.placementRotMat): counter[0] = 0 if obj_show_node[0] is not None: # obj_show_node[0].detach() obj_show_node[1].detachNode() obj_show_node[2].detachNode() obj_show_node[3].detachNode() obj_show_node[0] = cm.CollisionModel(objpath) obj_show_node[0].set_scale((0.001, 0.001, 0.001)) obj_show_node[0].set_rgba((0, 191 / 255, 1, 0.5)) obj_show_node[0].set_homomat(freehold.placementRotMat[counter[0]]) # obj_show_node[0].attach_to(base) obj_show_node[1] = NodePath("com") obj_show_node[2] = NodePath("vertice") obj_show_node[3] = NodePath("normal") if obj_show_node[0].is_mcdwith(fixturecm): obj_show_node[0].set_rgba((1, 0, 0, 0.5)) else: obj_show_node[0].attach_to(base) # gm.gen_sphere(pos = freehold.placementCoM[counter[0]]).attach_to(obj_show_node[1]) obj_show_node[1].reparentTo(base.render) # for vertace in freehold.placementVertices[counter[0]]: # gm.gen_sphere(pos=vertace, radius=0.005).attach_to(obj_show_node[2]) obj_show_node[2].reparentTo(base.render) for i, normal in enumerate(freehold.placementNormals[counter[0]]): print(normal) gm.gen_arrow( spos=freehold.placementfacetpairscenter[counter[0]][i], epos=freehold.placementfacetpairscenter[counter[0]][i] + normal * 0.1).attach_to(obj_show_node[3]) obj_show_node[3].reparentTo(base.render) counter[0] += 1 if textNode[0] is not None: textNode[0].detachNode() textNode[1].detachNode() textNode[2].detachNode() cam_pos = base.cam.getPos() textNode[0] = OnscreenText(text=str(cam_pos[0])[0:5], fg=(1, 0, 0, 1), pos=(1.0, 0.8), align=TextNode.ALeft) textNode[1] = OnscreenText(text=str(cam_pos[1])[0:5], fg=(0, 1, 0, 1), pos=(1.3, 0.8), align=TextNode.ALeft) textNode[2] = OnscreenText(text=str(cam_pos[2])[0:5], fg=(0, 0, 1, 1), pos=(1.6, 0.8), align=TextNode.ALeft) return task.again
def match_pcdncm_ptpt(self, target, src_stl_f_name, dv=10, show_icp=False): obj = cm.CollisionModel( objinit=os.path.join(config.ROOT + '/obstacles/' + src_stl_f_name)) source = np.asarray(ts.sample_surface(obj.trimesh, count=10000)) source = source[source[:, 2] > 5] target = np.asarray(target) rmse, fitness, homomat = o3dh.registration_ptpt( source, target, downsampling_voxelsize=dv, toggledebug=show_icp) return homomat
def checkcollision(self): # checker = cdchecker.MCMchecker(toggledebug=True) temobj = cm.CollisionModel(initor=self.objpath) temobj.set_scale((0.001, 0.001, 0.001)) collisionlist = [] for i in range(len(self.pRotMat)): temobj.set_homomat(da.pdmat4_to_npmat4(self.pRotMat[i])) checkresult = temobj.is_mcdwith(self.slopelist) # checkresult = checker.isMeshMeshListCollided(temobj, self.slopelist) collisionlist.append(checkresult) return collisionlist
def reconstruct_surface(pcd, radii=[.005], toggledebug=False): print("---------------reconstruct surface bp---------------") pcd = np.asarray(pcd) tmmesh = o3d_helper.reconstructsurfaces_bp(pcd, radii=radii, doseparation=False) obj = cm.CollisionModel(initor=tmmesh) if toggledebug: obj.set_rgba(1, 1, 1, 1) obj.reparentTo(base.render) return obj
def update(attached_list, pcd_list, counter, task): if counter[0] >= len(pcd_list): counter[0] = 0 if len(attached_list) != 0: for objcm in attached_list: objcm.detach() attached_list.clear() pcd = pcd_list[counter[0]] attached_list.append(cm.CollisionModel(pcd)) attached_list[-1].attach_to(base) counter[0]+=1 return task.again
def loadObj(f_name, pos=(0, 0, 0), rot=(0, 0, 0), color=(1, 1, 1), transparency=0.5): obj = cm.CollisionModel( objinit=os.path.join(config.ROOT, "obstacles", f_name)) obj.setPos(pos[0], pos[1], pos[2]) obj.setColor(color[0], color[1], color[2], transparency) obj.setRPY(rot[0], rot[1], rot[2]) return obj
def __init__(self, objinit, mass=None, restitution=0, allowdeactivation=False, allowccd=True, friction=.2, dynamic=True, type="convex", name="bdm"): """ :param objinit: GeometricModel (CollisionModel also work) :param mass: :param restitution: :param allowdeactivation: :param allowccd: :param friction: :param dynamic: :param type: "convex", "triangle", "box" :param name: """ if isinstance(objinit, BDModel): self._gm = copy.deepcopy(objinit.gm) self._bdb = objinit.bdb.copy() self._cm = copy.deepcopy(objinit.cm) elif isinstance(objinit, gm.GeometricModel): if mass is None: mass = 0 self._gm = objinit self._bdb = bdb.BDBody(self._gm, type, mass, restitution, allow_deactivation=allowdeactivation, allow_ccd=allowccd, friction=friction, dynamic=dynamic, name=name) else: if mass is None: mass = 0 self._cm = cm.CollisionModel(objinit) self._gm = gm.GeometricModel(objinit) self._bdb = bdb.BDBody(self._gm, type, mass, restitution, allow_deactivation=allowdeactivation, allow_ccd=allowccd, friction=friction, dynamic=dynamic, name=name)
def loadObjitem(f_name, pos=(0, 0, 0), rot=(0, 0, 0), sample_num=10000, type="box"): if f_name[-3:] != 'stl': f_name += '.stl' objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT, "obstacles", f_name), type=type) objmat4 = np.zeros([4, 4]) objmat4[:3, :3] = rm.rotmat_from_euler(rot[0], rot[1], rot[2], axes="sxyz") objmat4[:3, 3] = pos objcm.sethomomat(objmat4) return item.Item(objcm=objcm, objmat4=objmat4, sample_num=sample_num)
def __init__(self,z=-0.015,placement="shu",size=1.0, show=True): self.placement=placement self.size=size if self.placement=="shu": self.slopex = trimesh.primitives.Box(box_extents = (0.150,0.150,0.006)) self.slopex = cm.CollisionModel(self.slopex) self.slopex.set_rgba((255/255,51/255,153/255, 1)) self.slopex.set_pos((0, 0, z)) self.slopex.set_rpy(roll = 0, pitch = -45, yaw = 0) # self.slopex.reparentTo(base.render) self.slopey = trimesh.primitives.Box( box_extents = (0.200,0.200,0.006)) self.slopey = cm.CollisionModel(self.slopey) self.slopey.set_rgba((51/255,255/255,255/255, 1)) self.slopey.set_pos((0, 0, z)) self.slopey.set_rpy(roll = 45, pitch = 45, yaw = 0) # self.slopey.reparentTo(base.render) self.slopez = trimesh.primitives.Box( box_extents = (0.200,0.200,0.006)) self.slopez = cm.CollisionModel(self.slopez) self.slopez.set_rgba((178/255,102/255,255/255, 1)) self.slopez.set_pos(( 0, 0, z)) self.slopez.set_rpy(roll = -45, pitch = 45, yaw = 0) # self.slopez.reparentTo(base.render) if placement == "ping": s = self.size self.slopex = trimesh.primitives.Extrusion( extrude_polygon=Polygon([np.array([0, 0]), np.array([0.0707, -0.0707]) * s, np.array([0.0707, 0.0707]) * s]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.006], [0, 0, 0, 1]]), extrude_height=.006) self.slopex = cm.CollisionModel(self.slopex) self.slopex.set_rgba((51 / 255, 255 / 255, 255 / 255, 1)) self.slopex.set_pos((0, 0, z)) self.slopex.set_rpy(roll=0, pitch=np.radians(-54.74), yaw=0) self.slopey = trimesh.primitives.Extrusion( extrude_polygon=Polygon([np.array([0, 0.100]) * s, np.array([0, 0]), np.array([-0.100, 0]) * s]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.006], [0, 0, 0, 1]]), extrude_height=0.006) self.slopey = cm.CollisionModel(self.slopey) self.slopey.set_rgba((51 / 255, 255 / 255, 255 / 255, 1)) self.slopey.set_pos((0, 0, z)) self.slopey.set_rpy(roll=np.radians(45), pitch=np.radians(90-54.75), yaw=0) self.slopez = trimesh.primitives.Extrusion( extrude_polygon=Polygon([np.array([0, -0.100]) * s, np.array([0, 0]), np.array([-0.100, 0]) * s]), extrude_transform=np.array([[1, 0, 0, 0], [0, 1, 0, 0], [0, 0, 1, -0.006], [0, 0, 0, 1]]), extrude_height=0.006) self.slopez = cm.CollisionModel(self.slopez) self.slopez.set_rgba((51 / 255, 255 / 255, 255 / 255, 1)) self.slopez.set_pos((0, 0, z)) self.slopez.set_rpy(roll=np.radians(-45), pitch=np.radians(90 - 54.75), yaw=0) if show: self.slopex.attach_to(base) self.slopez.attach_to(base) self.slopey.attach_to(base)
def reconstruct_surface_list(pcd, radii=[5], color=(1, 1, 1), transparency=1, toggledebug=False): pcd = np.asarray(pcd) tmmeshlist = o3d_helper.reconstructsurfaces_bp(pcd, radii=radii, doseparation=True) obj_list = [] for tmmesh in tmmeshlist: obj = cm.CollisionModel(initor=tmmesh) obj_list.append(obj) if toggledebug: obj.set_rgba(color[0], color[1], color[2], transparency) obj.reparentTo(base.render) return obj_list
def get_obj_from_phoxiinfo_withmodel_nobgf(phxilocator, stl_f_name, objpcd_list=None, phoxi_f_name=None, load=True, x_range=(200, 800), y_range=(-100, 300), z_range=(790, 1000), match_rotz=False, resolution=1, eps=5, use_rmse=True): if stl_f_name[-3:] != 'stl': stl_f_name += '.stl' objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT + '/obstacles/' + stl_f_name)) if objpcd_list is None: grayimg, depthnparray_float32, pcd = load_phxiinfo( phoxi_f_name=phoxi_f_name, load=load) objpcd_list = phxilocator.find_objpcd_list_by_pos(pcd, x_range=x_range, y_range=y_range, z_range=z_range, toggledebug=False, scan_num=resolution, eps=eps) if len(objpcd_list) == 0: print("No obj is detected!") objpcd = phxilocator.find_closest_objpcd_by_stl(stl_f_name, objpcd_list, use_rmse=use_rmse) objmat4 = phxilocator.match_pcdncm(objpcd, objcm, toggledebug=False, match_rotz=match_rotz) objmat4[:3, :3] = np.eye(3) objcm.sethomomat(objmat4) return item.Item(objcm=objcm, pcd=objpcd, objmat4=objmat4)
def __init__(self, jlobject, cdprimitive_type='box', cdmesh_type='triangles'): """ author: weiwei date: 20200331 """ self.jlobject = jlobject for id in range(self.jlobject.ndof + 1): if self.jlobject.lnks[id][ 'meshfile'] is not None and self.jlobject.lnks[id][ 'collisionmodel'] is None: # in case the collision model is directly set, it allows manually specifying cd primitives # instead of auto initialization. Steps: 1. keep meshmodel to None; 2. directly set cm self.jlobject.lnks[id]['collisionmodel'] = cm.CollisionModel( self.jlobject.lnks[id]['meshfile'], cdprimit_type=cdprimitive_type, cdmesh_type=cdmesh_type) self.jlobject.lnks[id]['collisionmodel'].set_scale( self.jlobject.lnks[id]['scale'])
def loadObjpcd(f_name, pos=(0, 0, 0), rot=(0, 0, 0), sample_num=100000, toggledebug=False): obj = cm.CollisionModel( objinit=os.path.join(config.ROOT, "obstacles", f_name)) rotmat4 = np.zeros([4, 4]) rotmat4[:3, :3] = rm.rotmat_from_euler(rot[0], rot[1], rot[2], axes="sxyz") rotmat4[:3, 3] = pos obj_surface = np.asarray(obj.sample_surface(nsample=sample_num)) # obj_surface = obj_surface[obj_surface[:, 2] > 2] obj_surface_real = __pcd_trans(obj_surface, rotmat4) if toggledebug: # obj_surface = o3d_helper.nparray2o3dpcd(copy.deepcopy(obj_surface)) # obj_surface.paint_uniform_color([1, 0.706, 0]) # o3d.visualization.draw_geometries([obj_surface], window_name='loadObjpcd') # pcddnp = base.pg.genpointcloudnp(obj_surface_real) # pcddnp.reparentTo(base.render) pass return obj_surface_real
def get_obj_inhand_from_phoxiinfo_withmodel_pcd(phxilocator, stl_f_name, tcp_pos, inithomomat=None, phoxi_f_name=None, load=True, showicp=False, showcluster=False): """ find obj pcd directly by clustering :param phxilocator: :param stl_f_name: :param tcp_pos: :param inithomomat: :param phoxi_f_name: :param temp_f_name: :param w: :param h: :return: """ objcm = cm.CollisionModel(objinit=os.path.join(config.ROOT + '/obstacles/' + stl_f_name)) grayimg, depthnparray_float32, pcd = load_phxiinfo( phoxi_f_name=phoxi_f_name, load=load) objpcd = phxilocator.find_objinhand_pcd(tcp_pos, pcd, stl_f_name, toggledebug=showcluster) objmat4 = phxilocator.match_pcdncm(objpcd, objcm, inithomomat, toggledebug=showicp) objcm.sethomomat(objmat4) return item.Item(objcm=objcm, pcd=objpcd, objmat4=objmat4, draw_center=tcp_pos)
def get_org_convexhull(pcd, color=(1, 1, 1), transparency=1, toggledebug=False): """ create CollisionModel by pcd :param pcd: :param color: :param transparency: :return: CollisionModel """ convexhull = trimesh.Trimesh(vertices=pcd) convexhull = convexhull.convex_hull obj = cm.CollisionModel(initor=convexhull, type="ball") if toggledebug: obj.set_rgba(color[0], color[1], color[2], transparency) obj.reparentTo(base.render) obj.showlocalframe() return obj
def find_closest_objpcd_by_stl(self, src_stl_f_name, objpcd_list, inithomomat=None, use_rmse=True): objcm = cm.CollisionModel( objinit=os.path.join(config.ROOT + '/obstacles/' + src_stl_f_name)) min_rmse = 100 max_fitness = 0 result_pcd = None print("---------------find closest objpcd---------------") print("find obj:", src_stl_f_name.split(".stl")[0]) for tgt in objpcd_list: tgt_narray = np.array(tgt) if inithomomat is None: inithomomat = self.__match_pos( np.asarray(ts.sample_surface(objcm.trimesh, count=10000)), tgt_narray) src_narray = pcdu.get_objpcd_partial_bycampos( objcm, objmat4=inithomomat, sample_num=len(tgt_narray), toggledebug=False) rmse, fitness = self.__get_icp_scores(tgt_narray, src_narray, show_icp=False) if use_rmse: print("rmse:", rmse, "pcd length", len(tgt)) if 0.0 < rmse < min_rmse: result_pcd = copy.deepcopy(tgt_narray) min_rmse = rmse else: print("fitness:", fitness, "pcd length", len(tgt)) if fitness != 0.0 and fitness > max_fitness: result_pcd = copy.deepcopy(tgt_narray) max_fitness = fitness return result_pcd
def __init__(self, boundingradius=10.0, betransparent=False): """ load obstacles model separated by category :param base: author: weiwei date: 20181205 """ self.__this_dir, _ = os.path.split(__file__) # table self.__tablepath = os.path.join(self.__this_dir, "../obstacles", "ur3edtable.stl") self.__tablecm = cm.CollisionModel(self.__tablepath, expand_radius=boundingradius, betransparency=betransparent) self.__tablecm.setPos(180, 0, 0) self.__tablecm.setColor(.32, .32, .3, 1.0) self.__battached = False self.__changableobslist = []
import numpy as np import basis.robot_math as rm import robot_sim.robots.cobotta.cobotta_ripps as cbtr import robot_sim.end_effectors.gripper.cobotta_pipette.cobotta_pipette as cbtg import modeling.collision_model as cm import visualization.panda.world as wd import modeling.geometric_model as gm import utils if __name__ == '__main__': base = wd.World(cam_pos=[1.7, -1, .7], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) this_dir, this_filename = os.path.split(__file__) file_frame = os.path.join(this_dir, "meshes", "frame_bottom.stl") frame_bottom = cm.CollisionModel(file_frame) frame_bottom.set_rgba([.55, .55, .55, 1]) frame_bottom.attach_to(base) table_plate = cm.gen_box(extent=[.405, .26, .003]) table_plate.set_pos([0.07 + 0.2025, .055, .0015]) table_plate.set_rgba([.87, .87, .87, 1]) table_plate.attach_to(base) file_tip_rack = os.path.join(this_dir, "objects", "tip_rack.stl") tip_rack = utils.Rack96(file_tip_rack) tip_rack.set_rgba([140 / 255, 110 / 255, 170 / 255, 1]) tip_rack.set_pose(pos=np.array([.25, 0.0, .003]), rotmat=rm.rotmat_from_axangle([0, 0, 1], np.pi / 2)) tip_rack.attach_to(base)
import grasping.planning.antipodal as gpa import math import numpy as np import basis.robot_math as rm import robot_sim.robots.cobotta.cobotta as cbt import manipulation.pick_place_planner as ppp import motion.probabilistic.rrt_connect as rrtc base = wd.World(cam_pos=[1.2, .7, 1], lookat_pos=[.0, 0, .15]) gm.gen_frame().attach_to(base) # ground ground = cm.gen_box(extent=[5, 5, 1], rgba=[.7, .7, .7, .7]) ground.set_pos(np.array([0, 0, -.51])) ground.attach_to(base) # object holder object_holder = cm.CollisionModel("objects/holder.stl") object_holder.set_rgba([.5, .5, .5, 1]) object_holder_gl_pos = np.array([-.15, -.3, .0]) object_holder_gl_rotmat = np.eye(3) obgl_start_homomat = rm.homomat_from_posrot(object_holder_gl_pos, object_holder_gl_rotmat) object_holder.set_pos(object_holder_gl_pos) 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)
import modeling.geometric_model as gm import robot_sim.robots.ur3_dual.ur3_dual as ur3d import robot_sim.robots.ur3e_dual.ur3e_dual as ur3ed import robot_sim.robots.sda5f.sda5f as sda5 import motion.probabilistic.rrt_connect as rrtc import pickle if __name__ == '__main__': base = wd.World(cam_pos=[2, 1, 3], w=960, h=540, lookat_pos=[0, 0, 1.1]) gm.gen_frame().attach_to(base) phoxicam = cm.gen_box(extent=np.array([1.000, .43, .1]), homomat=np.eye(4)) phoxicam.set_pos( np.array([100 + 200, 0, 1360 + 175 + 200 + 90 / 2]) / 1000) phoxicam.attach_to(base) object = cm.CollisionModel("./objects/lshape-30.stl") object.set_scale([0.001, 0.001, 0.001]) object.set_pos(np.array([1.05, -.3, 1.3])) object.set_rgba([.5, .7, .3, 1]) object.attach_to(base) component_name = 'rgt_arm' # robot_instance = ur3d.UR3Dual() robot_instance = ur3ed.UR3EDual() # robot_instance = sda5.SDA5F() # robot_instance.gen_meshmodel().attach_to(base) with open("experiment180-1123.pickle", "rb") as file: objmsmp, numikrmsmp, jawwidthmp, originalpathnidlist = pickle.load( file) keyposes = []
import math import numpy as np import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import robot_sim.robots.ur3_dual.ur3_dual as ur3d import motion.probabilistic.rrt_connect as rrtc 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:
import numpy as np import basis.robot_math as rm import robot_sim.robots.cobotta.cobotta_ripps as cbtr import robot_sim.end_effectors.gripper.cobotta_pipette.cobotta_pipette as cbtg import modeling.collision_model as cm import visualization.panda.world as wd import modeling.geometric_model as gm import utils if __name__ == '__main__': base = wd.World(cam_pos=[1.7, -1, .7], lookat_pos=[0, 0, 0]) gm.gen_frame().attach_to(base) this_dir, this_filename = os.path.split(__file__) file_frame = os.path.join(this_dir, "meshes", "frame_bottom.stl") frame_bottom = cm.CollisionModel(file_frame) frame_bottom.set_rgba([.55, .55, .55, 1]) frame_bottom.attach_to(base) table_plate = cm.gen_box(extent=[.405, .26, .003]) table_plate.set_pos([0.07 + 0.2025, .055, .0015]) table_plate.set_rgba([.87, .87, .87, 1]) table_plate.attach_to(base) file_dispose_box = os.path.join(this_dir, "objects", "tip_rack_cover.stl") dispose_box = cm.CollisionModel(file_dispose_box) dispose_box.set_rgba([140 / 255, 110 / 255, 170 / 255, 1]) dispose_box.set_pos(pos=np.array([.14, 0.07, .003])) dispose_box.attach_to(base) rbt_s = cbtr.CobottaRIPPS()
return None, None, None if __name__ == '__main__': import time import robot_sim.robots.yumi.yumi as ym import visualization.panda.world as wd import modeling.geometric_model as gm import modeling.collision_model as cm import grasping.annotation.utils as gutil import numpy as np import basis.robot_math as rm base = wd.World(cam_pos=[2, 0, 1.5], lookat_pos=[0, 0, .2]) gm.gen_frame().attach_to(base) 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')
capsule_1 = capsule_link_start_end(start=np.array([0, 0, 0]), end=np.array([0, 0, 0.1])) capsule_2 = capsule_link_start_end(start=np.array([0, 0, 0]), end=np.array([0, 0.2, 0])) a_vertices = capsule_1.objtrm.vertices a_face_normals = capsule_1.objtrm.face_normals a_vertex_normals = capsule_1.objtrm.face_normals a_faces = capsule_1.objtrm.faces capsule_1.objtrm.export("capsule_1.stl") capsule_2.objtrm.export("capsule_2.stl") capsule_3 = tb.union([capsule_1.objtrm, capsule_2.objtrm], engine="blender") capsule_3.export("capsule_3.stl") capsule_3_wrsmesh = cm.CollisionModel("capsule_3.stl").attach_to(base) a_sub = trimesh.Trimesh(p).convex_hull a_cm = cm.CollisionModel(a_sub) b_sub = trimesh.Trimesh(m).convex_hull b_cm = cm.CollisionModel(b_sub) # a_gm = gm.GeometricModel(a_sub).attach_to(base) print(a_sub) a_sub.export("a_sub.stl") b_sub.export("b_sub.stl") a_trimesh = tri.load("a_sub.stl") b_trimesh = tri.load("b_sub.stl") c = tb.union([a_trimesh, b_trimesh], engine="blender") c.export("c_sub.stl") # c_wrsmesh = cm.CollisionModel("c_sub.stl").attach_to(base)
import grasping.annotation.utils as gau if __name__ == '__main__': import numpy as np import robot_sim.end_effectors.grippers.robotiq85.robotiq85 as rtq85 import modeling.collision_model as cm import visualization.panda.world as wd base = wd.World(cam_pos=[.5, .5, .3], lookat_pos=[0, 0, 0]) gripper_s = rtq85.Robotiq85(enable_cc=True) objcm = cm.CollisionModel("./objects/bunnysim.stl") objcm.set_pos(np.array([.5, -.3, 1.2])) objcm.attach_to(base) objcm.show_localframe() grasp_info_list = gau.define_grasp_with_rotation( gripper_s, objcm, gl_jaw_center_pos=np.array([0, 0, 0]), gl_jaw_center_z=np.array([1, 0, 0]), gl_hndx=np.array([0, 1, 0]), jaw_width=.04, gl_rotation_ax=np.array([0, 0, 1])) for grasp_info in grasp_info_list: jaw_width, gl_jaw_center, pos, rotmat = grasp_info gic = gripper_s.copy() gic.fix_to(pos, rotmat) gic.jaw_to(jaw_width) print(pos, rotmat) gic.gen_meshmodel().attach_to(base) base.run()
import modeling.collision_model as cm import visualization.panda.world as wd import modeling.geometric_model as gm import utils import motion.probabilistic.rrt_connect as rrtc import motion.trajectory.piecewisepoly_toppra as trajp if __name__ == '__main__': base = wd.World(cam_pos=[1.7, .7, .7], lookat_pos=[0, 0, 0]) traj_gen = trajp.PiecewisePolyTOPPRA() max_vels = [np.pi * .6, np.pi * .4, np.pi, np.pi, np.pi, np.pi * 1.5] gm.gen_frame().attach_to(base) this_dir, this_filename = os.path.split(__file__) file_frame = os.path.join(this_dir, "meshes", "frame_bottom.stl") frame_bottom = cm.CollisionModel(file_frame) frame_bottom.set_rgba([.55, .55, .55, 1]) frame_bottom.attach_to(base) table_plate = cm.gen_box(extent=[.405, .26, .003]) table_plate.set_pos([0.07 + 0.2025, .055, .0015]) table_plate.set_rgba([.87, .87, .87, 1]) table_plate.attach_to(base) file_dispose_box = os.path.join(this_dir, "objects", "tip_rack_cover.stl") dispose_box = cm.CollisionModel(file_dispose_box, expand_radius=.007) dispose_box.set_rgba([140 / 255, 110 / 255, 170 / 255, 1]) dispose_box.set_pos(pos=np.array([.14, 0.07, .003])) dispose_box.attach_to(base) file_tip_rack = os.path.join(this_dir, "objects", "tip_rack.stl")
import numpy as np import modeling.geometric_model as gm import modeling.collision_model as cm import visualization.panda.world as wd import basis.robot_math as rm import math from scipy.spatial import cKDTree import vision.depth_camera.surface.rbf_surface as rbfs base = wd.World(cam_pos=np.array([-.3, -.7, .42]), lookat_pos=np.array([0, 0, 0])) # gm.gen_frame().attach_to(base) bowl_model = cm.CollisionModel(initor="./objects/bowl.stl") bowl_model.set_rgba([.3, .3, .3, .3]) bowl_model.set_rotmat(rm.rotmat_from_euler(math.pi, 0, 0)) # bowl_model.attach_to(base) pn_direction = np.array([0, 0, -1]) bowl_samples, bowl_sample_normals = bowl_model.sample_surface( toggle_option='normals', radius=.002) selection = bowl_sample_normals.dot(-pn_direction) > .1 bowl_samples = bowl_samples[selection] bowl_sample_normals = bowl_sample_normals[selection] tree = cKDTree(bowl_samples) surface = rbfs.RBFSurface(bowl_samples[:, :2], bowl_samples[:, 2]) surface.get_gometricmodel(rgba=[.3, .3, .3, .3]).attach_to(base) pt_direction = rm.orthogonal_vector(pn_direction, toggle_unit=True) tmp_direction = np.cross(pn_direction, pt_direction) plane_rotmat = np.column_stack((pt_direction, tmp_direction, pn_direction))