def drawanySingleSurface(a, vertices, color): ''' draw a surface using a calculated fourth point to creat a hull :param base: :param vertices: :param faces: :param color: :return: ''' # print("faces in plotsurface",faces) surface_vertices = vertices # surface = humath.centerPoftrangle(surface_vertices[0][:3], surface_vertices[1][:3], surface_vertices[2][:3]) surface = trimesh.Trimesh(surface_vertices) surface = surface.convex_hull surface = gm.GeometricModel(surface) if color == "red": rgba = (1, 0, 0, 1) elif color == "green": rgba = (61 / 255, 145 / 255, 64 / 255, 1) elif color == "orange": rgba = (255 / 255, 100 / 255, 24 / 255, 1) elif color == "white": rgba = (1, 1, 1, 1) elif color == "blue": # rgba = (3/255,168/255,158/255,1) rgba = (0 / 255, 0 / 255, 255 / 255, 1) elif color == "yellow": rgba = (1, 1, 0, 1) else: rgba = (0, 0, 0, 1) surface.set_rgba(rgba) surface.objpdnp.reparentTo(a)
def update(pcd_list, task): if len(pcd_list) == 0: return task.again if len(shown_pcd_list) != 0: for pcd in shown_pcd_list: pcd.detach() shown_pcd_list.clear() pcd = gm.GeometricModel(pcd_list[0]) pcd.attach_to(base) shown_pcd_list.append(pcd) pcd_list.pop(0) return task.again
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 update(video1, pcdm, task): if len(pcdm) != 0: for one_pcdm in pcdm: one_pcdm.detach() else: pcdm.append(None) key = cv2.waitKey(1) if int(key) == 113: video1.release() return task.done ret, frame = video1.read() frame = cv2.resize(frame, (int(width), int(height)), interpolation=cv2.INTER_CUBIC) depth, hm = itd_cvter.convert(frame) pcdm[0] = gm.GeometricModel(depth * .001) pcdm[0].attach_to(base) return task.again
def drawSingleFaceSurface(self, base, vertices, faces, color): ''' draw a surface using a calculated fourth point to creat a hull :param base: :param vertices: :param faces: :param color: :return: ''' # print("faces in plotsurface",faces) surface_vertices = np.array([vertices[faces[0]], vertices[faces[1]], vertices[faces[2]]]) surface = humath.centerPoftrangle(surface_vertices[0], surface_vertices[1], surface_vertices[2]) surface = trimesh.Trimesh(surface) surface = surface.convex_hull surface = gm.GeometricModel(surface) surface.set_rgba(color) surface.attach_to(base)
def update(pk_obj, pcd_list, marker_center_list, task): if len(pcd_list) != 0: for pcd in pcd_list: pcd.detach() pcd_list.clear() if len(marker_center_list) != 0: for marker_center in marker_center_list: marker_center.detach() marker_center_list.clear() pk_obj.device_get_capture() color_image_handle = pk_obj.capture_get_color_image() depth_image_handle = pk_obj.capture_get_depth_image() if color_image_handle and depth_image_handle: color_image = pk_obj.image_convert_to_numpy(color_image_handle) point_cloud = pk_obj.transform_depth_image_to_point_cloud( depth_image_handle) parameters = aruco.DetectorParameters_create() color_image = cv2.cvtColor(color_image, cv2.COLOR_BGRA2BGR) corners, ids, rejectedImgPoints = aruco.detectMarkers( color_image, dictionary=aruco.Dictionary_get(aruco.DICT_4X4_250), parameters=parameters) if len(corners) == 0: return task.cont image_xy = np.mean(np.mean(corners[0], axis=0), axis=0).astype(np.int16) pcd_pnt = pk_obj.transform_color_xy_to_pcd_xyz( input_color_image_handle=color_image_handle, input_depth_image_handle=depth_image_handle, color_xy=image_xy) # cv2.circle(color_image, tuple(image_xy), 10, (255, 0, 0), -1) # cv2.imshow("test", color_image) # cv2.waitKey(0) mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) pcd_list.append(mypoint_cloud) marker_center = gm.gen_sphere(pos=pcd_pnt, radius=.1) marker_center.attach_to(base) marker_center_list.append(marker_center) pk_obj.image_release(color_image_handle) pk_obj.image_release(depth_image_handle) pk_obj.capture_release() return task.cont
# select_id = 3 # if height > .1: # select_id = 4 # map_color = map_list[select_id] if height < .25: this_rotmat = wither_rotmat_list[id % len(wither_rotmat_list)] branch = Stem(ndof=1, pos=branch_pos, rotmat=this_rotmat, base_length=.1 / (height)**(1 / 2), base_thickness=.002) branch.gen_meshmodel(rgba=wither_rgba).attach_to(base) # main_stem.fk(jnt_values=[math.pi/36,math.pi/36, 0,-math.pi/36,-math.pi/36,0]) # stem1.gen_meshmodel().attach_to(base) sb_leaf = gm.GeometricModel(initor="objects/soybean_leaf.stl") sb_leaf.set_rgba(rgba=leaf_rgba) sbl = sb_leaf.copy() sbl.set_rgba(rgba=wither_rgba) # sbl.set_scale(np.array([1,1,1])/(int(id/3)%(main_stem.jlc.ndof+1)+1)) sbl.set_scale(np.array([1, 1, 1])) jnt_pos = branch.jlc.jnts[-1]['gl_posq'] sbl.set_pos(jnt_pos) sbl.set_rotmat(this_rotmat) # gm.gen_frame(pos=jnt_pos, rotmat=this_rotmat).attach_to(base) sbl.attach_to(base) else: branch = Stem(ndof=1, pos=branch_pos, rotmat=rotmat, base_length=.1 / (height)**(1 / 2),
# action_rotmat_list = [] # for i in range(10): # action_pos_list.append(pos[i] + pos_start) # action_rotmat_list.append(np.dot(rm.rotmat_from_axangle(rot[i][0], rot[i][1]), # rot_start)) # # print(marker_pos_in_hnd) # pos_arm_tcp, rot_arm_tcp = xss.arm.jlc.lnks[7]["gl_pos"], xss.arm.jlc.lnks[7]["gl_rotmat"] # pos_hnd_tcp, rot_hand_tcp = xss.get_gl_tcp(manipulator_name="arm") # # gm.gen_frame(pos_arm_tcp, rot_arm_tcp).attach_to(base) # # gm.gen_frame(pos_hnd_tcp, rot_hand_tcp).attach_to(base) # marker_pos_in_hnd = np.array([-0.026, 0, 0.069]) - np.array([0, 0, pos_hnd_tcp[2] - pos_arm_tcp[2]]) # # print("marker in hand", marker_pos_in_hnd) # calibration_r = dc.calibrate(component_name="arm", # sensor_marker_handler=sensor_handler, # marker_pos_in_hnd=marker_pos_in_hnd, # action_pos_list=action_pos_list, # action_rotmat_list=action_rotmat_list) # print(calibration_r) # base.run() # validate pcd from vision.depth_camera.calibrator import load_calibration_data affine_matrix, _, _ = load_calibration_data() gm.GeometricModel(initor=rm.homomat_transform_points( affine_matrix, sensor_handler.get_point_cloud())).attach_to(base) # # planner # rrtc_planner = rrtc.RRTConnect(xss) base.run()
def show_pcd(pcd, rgba=(1, 1, 1, 1)): pcd_gm = gm.GeometricModel(initor=pcd) pcd_gm.set_rgba(rgba) pcd_gm.attach_to(base)
# pcd_list = [] # marker_center_list = [] # def update(pk_obj, pcd_list, marker_center_list, task): # if len(pcd_list) != 0: # for pcd in pcd_list: # pcd.detach() # pcd_list.clear() # if len(marker_center_list) != 0: # for marker_center in marker_center_list: # marker_center.detach() # marker_center_list.clear() while True: pk_obj.device_get_capture() color_image_handle = pk_obj.capture_get_color_image() depth_image_handle = pk_obj.capture_get_depth_image() if color_image_handle and depth_image_handle: color_image = pk_obj.image_convert_to_numpy(color_image_handle) # cv2.imshow("test", color_image) # cv2.waitKey(0) point_cloud = pk_obj.transform_depth_image_to_point_cloud(depth_image_handle) point_cloud = rm.homomat_transform_points(rm.homomat_inverse(origin_homomat), point_cloud) point_cloud[point_cloud[:,0]<-1]=point_cloud[point_cloud[:,0]<-1]*0 mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) base.run() pk_obj.image_release(color_image_handle) pk_obj.image_release(depth_image_handle) pk_obj.capture_release() base.run()
base = wd.World(cam_pos=[.5, .2, .3], lookat_pos=[0, 0, 0], auto_cam_rotate=False) # objcm = gm.WireFrameModel(gen_torus()) # objcm.set_rgba([1, 0, 0, 1]) # objcm.attach_to(base) # objcm = gm.StaticGeometricModel(gen_axis()) # objcm.set_rgba([1, 0, 0, 1]) # objcm.attach_to(base) import time tic = time.time() for i in range(100): gen_dumbbell() toc = time.time() print("mine", toc - tic) objcm = gm.GeometricModel(gen_dashstick(lsolid=.005, lspace=.005)) objcm = gm.GeometricModel(gen_dashtorus(portion=1)) objcm.set_rgba([1, 0, 0, 1]) objcm.attach_to(base) objcm = gm.GeometricModel(gen_stick()) objcm.set_rgba([1, 0, 0, 1]) objcm.set_pos(np.array([0, .01, 0])) objcm.attach_to(base) objcm = gm.GeometricModel(gen_dasharrow()) objcm.set_rgba([1, 0, 0, 1]) objcm.set_pos(np.array([0, -.01, 0])) objcm.attach_to(base) base.run()
bowl_model.attach_to(base) # print(model_pcd) bowl_model.attach_to(base) # base.run() import random for point_id in range(3000, 10000, 100): tree = cKDTree(bowl_samples) # point_id = random.randint(3000, 10000) nearby_sample_ids = tree.query_ball_point(bowl_samples[point_id, :], .03) nearby_samples = bowl_samples[nearby_sample_ids] colors = np.tile(np.array([1, 0, 0, 1]), (len(nearby_samples), 1)) print(nearby_samples.shape) print(colors.shape) nearby_samples_withcolor = np.column_stack((nearby_samples, colors)) gm.GeometricModel(nearby_samples_withcolor).attach_to(base) plane_center, plane_normal = rm.fit_plane(nearby_samples) plane_tangential = rm.orthogonal_vector(plane_normal) plane_tmp = np.cross(plane_normal, plane_tangential) plane_rotmat = np.column_stack((plane_tangential, plane_tmp, plane_normal)) nearby_samples_on_xy = plane_rotmat.T.dot( (nearby_samples - plane_center).T).T surface = gs.MixedGaussianSurface(nearby_samples_on_xy[:, :2], nearby_samples_on_xy[:, 2], n_mix=1) # t_npt_on_xy = plane_rotmat.T.dot(t_npt - plane_center) # projected_t_npt_z_on_xy = surface.get_zdata(np.array([t_npt_on_xy[:2]])) # projected_t_npt_on_xy = np.array([t_npt_on_xy[0], t_npt_on_xy[1], projected_t_npt_z_on_xy[0]]) # projected_point = plane_rotmat.dot(projected_t_npt_on_xy) + plane_center surface_gm = surface.get_gometricmodel([[-.05, .05], [-.05, .05]], rgba=[.5, .7, 1, 1])
rotmat.dot(tmp_direction) * .05 ]] gm.gen_linesegs(new_line_segs).attach_to(base) # gm.gen_arrow(spos=new_line_segs[0][0], epos=new_line_segs[0][1], thickness=0.004).attach_to(base) t_cpt = cpt last_normal = cnrml direction = rotmat.dot(pt_direction) n = 10 for tick in range(1, n + 1): t_npt = cpt + direction * .05 / n gm.gen_arrow(spos=t_npt, epos=t_npt + last_normal * .015, thickness=0.001).attach_to(base) nearby_sample_ids = tree.query_ball_point(t_npt, .03) nearby_samples = bowl_samples[nearby_sample_ids] gm.GeometricModel(nearby_samples).attach_to(base) plane_center, plane_normal = rm.fit_plane(nearby_samples) plane_tangential = rm.orthogonal_vector(plane_normal) plane_tmp = np.cross(plane_normal, plane_tangential) plane_rotmat = np.column_stack((plane_tangential, plane_tmp, plane_normal)) nearby_samples_on_xy = plane_rotmat.T.dot( (nearby_samples - plane_center).T).T surface = qs.QuadraticSurface(nearby_samples_on_xy[:, :2], nearby_samples_on_xy[:, 2]) # surface = gs.MixedGaussianSurface(nearby_samples_on_xy[:, :2], nearby_samples_on_xy[:,2],n_mix=1) t_npt_on_xy = plane_rotmat.T.dot(t_npt - plane_center) projected_t_npt_z_on_xy = surface.get_zdata(np.array([t_npt_on_xy[:2]])) projected_t_npt_on_xy = np.array( [t_npt_on_xy[0], t_npt_on_xy[1], projected_t_npt_z_on_xy[0]]) projected_point = plane_rotmat.dot(projected_t_npt_on_xy) + plane_center surface_gm = surface.get_gometricmodel([[-.05, .05], [-.05, .05]],
def update(pkx, rbtx, background_image, pcd_list, ball_center_list, counter, task): if len(pcd_list) != 0: for pcd in pcd_list: pcd.detach() pcd_list.clear() while True: pkx.device_get_capture() color_image_handle = pkx.capture_get_color_image() depth_image_handle = pkx.capture_get_depth_image() if color_image_handle and depth_image_handle: break point_cloud = pkx.transform_depth_image_to_point_cloud(depth_image_handle) point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud) ball = [] for id, point_cloud_sub in enumerate(point_cloud): if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[ 1] < .3 and 0.5 < point_cloud_sub[2] < 2.5: ball.append(point_cloud_sub) mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) pcd_list.append(mypoint_cloud) if len(ball) > 20: center = np.mean(np.array(ball), axis=0) ball_center_list.append([center, counter[0]]) ball = gm.gen_sphere(center, radius=.1) ball.attach_to(base) ball_list.append(ball) if len(ball_center_list) > 2: x = [] y = [] z = [] t = [] for cen, f_id in ball_center_list: x.append(cen[0]) y.append(cen[1]) z.append(cen[2]) t.append(f_id) para_x = np.polyfit(t, x, 1) para_y = np.polyfit(t, y, 1) para_z = np.polyfit(t, z, 2) f_x = np.poly1d(para_x) f_y = np.poly1d(para_y) f_z = np.poly1d(para_z) orbit = [] for t in np.linspace(ball_center_list[0][1], ball_center_list[0][1] + 15, 100): point = np.array([f_x(t), f_y(t), f_z(t)]) orbit.append(point) if abs(point[0]) < .7 and abs( point[1]) < .7 and abs(point[2] - 1.1) < .3: # last_point = orbit[-1] jnt_values = rbts.ik(tgt_pos=point, tgt_rotmat=np.array([[-1, 0, 0], [0, -1, 0], [0, 0, 1]])) if jnt_values is None: continue rbts.fk(component_name="arm", jnt_values=jnt_values) rbts.gen_meshmodel().attach_to(base) rbtx.arm_move_jspace_path( [rbtx.get_jnt_values(), jnt_values], max_jntspeed=math.pi * 1.3) break for id in range(len(orbit)): if id > 0: tmp_stick = gm.gen_stick(spos=orbit[id - 1], epos=orbit[id], thickness=.01, type="round") tmp_stick.attach_to(base) para_list.append(tmp_stick) return task.done pkx.image_release(color_image_handle) pkx.image_release(depth_image_handle) pkx.capture_release() counter[0] += 1 return task.cont
def update(pk_obj, pcd_list, ball_center_list, counter, task): if len(pcd_list) != 0: for pcd in pcd_list: pcd.detach() pcd_list.clear() # if len(ball_center_list) != 0: # for ball_center in ball_center_list: # ball_center.detach() # ball_center_list.clear() while True: pk_obj.device_get_capture() color_image_handle = pk_obj.capture_get_color_image() depth_image_handle = pk_obj.capture_get_depth_image() if color_image_handle and depth_image_handle: break point_cloud = pk_obj.transform_depth_image_to_point_cloud( depth_image_handle) point_cloud = rm.homomat_transform_points(affine_matrix, point_cloud) ball = [] for id, point_cloud_sub in enumerate(point_cloud): if 0.3 < point_cloud_sub[0] < 3.3 and -1.3 < point_cloud_sub[ 1] < .3 and 0.5 < point_cloud_sub[2] < 2.5: ball.append(point_cloud_sub) mypoint_cloud = gm.GeometricModel(initor=point_cloud) mypoint_cloud.attach_to(base) pcd_list.append(mypoint_cloud) if len(ball) > 20: center = np.mean(np.array(ball), axis=0) ball_center_list.append([center, counter[0]]) ball = gm.gen_sphere(center, radius=.1) ball.attach_to(base) ball_list.append(ball) if len(ball_center_list) > 2: x = [] y = [] z = [] t = [] for cen, f_id in ball_center_list: x.append(cen[0]) y.append(cen[1]) z.append(cen[2]) t.append(f_id) para_x = np.polyfit(t, x, 1) para_y = np.polyfit(t, y, 1) para_z = np.polyfit(t, z, 2) f_x = np.poly1d(para_x) f_y = np.poly1d(para_y) f_z = np.poly1d(para_z) orbit = [] for t in np.linspace(ball_center_list[0][1], ball_center_list[0][1] + 5, 100): orbit.append(np.array([f_x(t), f_y(t), f_z(t)])) for id in range(len(orbit)): if id > 0: tmp_stick = gm.gen_stick(spos=orbit[id - 1], epos=orbit[id], thickness=.01, type="round") tmp_stick.attach_to(base) para_list.append(tmp_stick) return task.done pk_obj.image_release(color_image_handle) pk_obj.image_release(depth_image_handle) pk_obj.capture_release() counter[0] += 1 return task.cont
# fixture_start_homomat = rm.homomat_from_posrot(fixture_start_pos, fixture_start_rotmat) # fixture_start_rotmat = fixture_start_homomat[:3,:3] # fixture_start.set_pos(fixture_start_pos) # fixture_start.set_rotmat(fixture_start_rotmat) # fixture_start.attach_to(base) fixture_trimesh = fixture.objtrm v = fixture_trimesh.voxelized(0.01) # b = triv.Voxel(fixture_trimesh, 0.01) c= triv.mesh_to_run(fixture_trimesh, 0.01) fixture_voxel = fixture_trimesh.voxelized(0.001) a = gm.GeometricModel(fixture_voxel) a.attach_to(base) # fixture_voxel = voxelizer.creation.local_voxelize base.run() slopename = "tc71.stl" slope_high = slope.Slope(z=-0.005, placement="ping", size=sinfo.Sloperate[slopename], show=False) slopeforcd_high = slope_high.getSlope() for i, item in enumerate(slopeforcd_high): item.set_homomat(fixture_start_homomat.dot(item.get_homomat())) slopeforcd_high[i] = item # slopeforcd_high[0].attach_to(base) # slopeforcd_high[1].attach_to(base) # slopeforcd_high[2].attach_to(base) robot = ur3ed.UR3EDual()
3:] = self._return_value[:, 3:] / 255.0 # regulate to 0-1 self.buffer = bytearray() if __name__ == '__main__': import time client = EtherSenseClient(address="10.2.0.202", port=18360) cv2.namedWindow("window") while True: cv2.imshow("window", client.get_rgb_image()) cv2.waitKey(1) # time.sleep(3) # cv2.imshow("window2", client.get_rgb_image()) # cv2.waitKey(1) # time.sleep(3) import visualization.panda.rpc.rviz_client as rv_client import modeling.geometric_model as gm rvc = rv_client.RVizClient(host="localhost:182001") rvc.reset() last_rmt = None while True: pcd = gm.GeometricModel(initor=client.get_rgb_pointcloud()) current_rmt = rvc.showmodel_to_remote(pcd) if last_rmt is not None: rvc.unshow_model(last_rmt) last_rmt = current_rmt base.run()
if __name__ == '__main__': import numpy as np import visualization.panda.world as wd import modeling.geometric_model as gm base = wd.World(cam_pos=[1, 1, 1], lookat_pos=[0, 0, .1]) pm_s = gm.GeometricModel("./meshes/p1000g.stl") pm_s.attach_to(base) pm_b_s = gm.GeometricModel("./meshes/p1000g_body.stl") pm_b_s.set_scale(scale=[1.03,1.03,1.01]) pm_b_s.set_pos(np.array([0,0, 0.1463])) pm_b_s.set_rgba(rgba=[.3, .4, .6, 1]) pm_b_s.attach_to(base) base.run()
import numpy as np import basis.trimesh_generator as tg import modeling.geometric_model as gm import visualization.panda.world as wd if __name__ == '__main__': base = wd.World(cam_pos=np.array([.3,.3,.3]), lookat_pos=np.zeros(3)) tg_sphere = tg.gen_sphere(pos=np.zeros(3), radius=.05) gm_sphere = gm.GeometricModel(tg_sphere) gm_sphere.attach_to(base) base.run()
bunnycm2.set_rgba([0, 0.7, 0.7, 1.0]) rotmat = rm.rotmat_from_axangle([1, 0, 0], -math.pi / 4.0) bunnycm2.set_pos(np.array([0, .2, 0])) bunnycm2.set_rotmat(rotmat) bunnycm.attach_to(base) bunnycm1.attach_to(base) bunnycm2.attach_to(base) bunnycm.show_cdprimit() bunnycm1.show_cdprimit() bunnycm2.show_cdprimit() bunnycmpoints, _ = bunnycm.sample_surface() bunnycm1points, _ = bunnycm1.sample_surface() bunnycm2points, _ = bunnycm2.sample_surface() bpcm = gm.GeometricModel(bunnycmpoints) bpcm1 = gm.GeometricModel(bunnycm1points) bpcm2 = gm.GeometricModel(bunnycm2points) bpcm.attach_to(base) bpcm1.attach_to(base) bpcm2.attach_to(base) # bunnycm2.show_cdmesh(type='box') # bunnycm.show_cdmesh(type='box') # bunnycm1.show_cdmesh(type='convexhull') # tic = time.time() # bunnycm2.is_mcdwith([bunnycm, bunnycm1]) # toc = time.time() # print("mesh cd cost: ", toc - tic) # tic = time.time() # bunnycm2.is_pcdwith([bunnycm, bunnycm1]) # toc = time.time()
self.earth.set_rgba(rgba=earth_rgba) def set_pos(self, pos): self.cup.set_pos(pos=pos) self.earth.set_pos(pos=pos) def attach_to(self, base): self.cup.attach_to(base) self.earth.attach_to(base) def copy(self): return copy.deepcopy(self) base = wd.World(cam_pos=[4.2, 4.2, 2.5], lookat_pos=[0, 0, .7], auto_cam_rotate=True) frame = gm.GeometricModel(initor="meshes/frame.stl") frame.set_rgba(rgba=aluminium_rgba) frame.attach_to(base) bottom_box = cm.gen_box(extent=[.88, 1.68, .45], rgba=board_rgba) bottom_box.set_pos(pos=np.array([0, 0, .22])) bottom_box.attach_to(base) top_box = cm.gen_box(extent=[.88, 1.68, .3], rgba=board_rgba) top_box.set_pos(pos=np.array([0, 0, 1.65])) top_box.attach_to(base) cup = Cup() cup_pos_x = [0.09 - .44, 0.23 - .44, 0.37 - .44, 0.51 - .44, 0.65 - .44, 0.79 - .44] cup_pos_y = [.09 - .84, .24 - .84, .39 - .84, .54 - .84, .69 - .84, .84 - .84, .99 - .84, 1.14 - .84, 1.29 - .84, 1.44 - .84, 1.59 - .84]
# a_mesh.export('a_mesh_1.stl') # print("a", a) # print("a_mesh", a_mesh) # b_mesh = b.objtrm # b_mesh.export('b_mesh.stl') # a_trimesh = tri.load("a_mesh.stl") # b_trimesh = tri.load("b_mesh.stl") # c_mesh = tb.union([a_trimesh, b_trimesh], engine = "blender") # c = gm.GeometricModel(c_mesh) # print("c", c) # c = cm.CollisionModel(c_mesh) # c = gm.GeometricModel(c_mesh) # c.attach_to(base) base.run() m = [[.050, 0, 0], [.050, .100, 0], [.050, 0, .100], [.150, 0, 0], [.150, .100, .100], [.150, .100, 0], [.150, 0, .100], [.050, .100, .100]] # base.pggen.plotAxis(base.render) # print(Vec3(100, 100, 100), type(Vec3(100, 100, 100))) b.set_rgba((0, 1, 0, 1)) # # b.setPos(Vec3(100, 100, 100)) b.attach_to(base) mesh_b = trimesh.Trimesh(m) body_m = mesh_b.convex_hull bm = cm.CollisionModel(body_m) bm.attach_to(base.render) a = tb.union([mesh, mesh_b], engine="blender") am = gm.GeometricModel(a) am.attach_to(base) # taskMgr.doMethodLater(0.5, update, 'update', extraArgs=[b,counter], appendTask=True) base.run()
import numpy as np import modeling.geometric_model as gm import visualization.panda.world as wd import basis.robot_math as rm import math # import vision.depth_camera.surface.gaussian_surface as gs # import vision.depth_camera.surface.quadrantic_surface as qs import vision.depth_camera.surface.bibspline_surface as bs import vision.depth_camera.surface.plane_surface as ps base = wd.World(cam_pos=np.array([.5,.1,.3]), lookat_pos=np.array([0,0,0.05])) gm.gen_frame().attach_to(base) tube_model = gm.GeometricModel(initor="./objects/bowl.stl") tube_model.set_rgba([.3,.3,.3,.3]) tube_model.attach_to(base) points, points_normals = tube_model.sample_surface(radius=.002, nsample=10000, toggle_option='normals') sampled_points = [] for id, p in enumerate(points.tolist()): if np.dot(np.array([1,0,0]), points_normals[id]) > .3 and p[0]>0: gm.gen_sphere(pos=p, radius=.001).attach_to(base) sampled_points.append(p) # x - v # y - u rotmat_uv = rm.rotmat_from_euler(0, math.pi/2, 0) sampled_points = rotmat_uv.dot(np.array(sampled_points).T).T # surface = rbfs.RBFSurface(sampled_points[:, :2], sampled_points[:,2]) # surface = gs.MixedGaussianSurface(sampled_points[:, :2], sampled_points[:,2], n_mix=1) # surface = qs.QuadraticSurface(sampled_points[:, :2], sampled_points[:,2]) # surface = bs.BiBSpline(sampled_points[:, :2], sampled_points[:,2]) surface = ps.PlaneSurface(sampled_points[:,:2], sampled_points[:,2])